bool KinematicsSolver::checkTheta123(double t1, double t2, double t3) {
double x = pos(1);
double y = pos(2);
double z = pos(3);
Matrix T = getT(DH_a0, DH_d1, DH_alfa0, t1) * getT(DH_a1, DH_d2, DH_alfa1,
t2) * getT(DH_a2, DH_d3, DH_alfa2, t3) * getT(DH_a3, DH_d4,
DH_alfa3, 0);
//TODO: 0.27 0.12 0.024 is not being solved!!
//cout << T << endl;
//cout << x << " " << y << " " << z << endl;
double _X = T(1, 4);
double _Y = T(2, 4);
double _Z = T(3, 4) - DH_d1;
//cout << endl << "CHECKING 123 [ " << t1 << ", " << t2 << ", " << t3 << "] for ";
//cout << "Loc: " << "[ " << x << ", " << y << ", " << z << "]..... ";
bool _check = DBL_EQ(_X, x) && DBL_EQ(_Y, y) && DBL_EQ(_Z, z);
//if (_check) {
// cout << "OK.." << _Z << endl;
//} else {
// cout << "wrong!" << endl;
//}
return _check;
}
/* this tries to swap two neighbouring eigenvalues, 'it' and '--it',
* and returns 'itadd'. If the blocks can be swapped, new eigenvalues
* can emerge due to possible 2-2 block splits. 'it' then points to
* the last eigenvalue coming from block pointed by 'it' at the
* begining, and 'itadd' points to the first. On swap failure, 'it' is
* not changed, and 'itadd' points to previous eignevalue (which must
* be moved backwards before). In either case, it is necessary to
* resolve eigenvalues from 'itadd' to 'it', before the 'it' can be
* resolved.
* The success is signaled by returned true.
*/
bool SchurDecompEig::tryToSwap(diag_iter& it, diag_iter& itadd)
{
itadd = it;
--itadd;
lapack_int n = getDim();
lapack_int ifst = (*it).getIndex() + 1;
lapack_int ilst = (*itadd).getIndex() + 1;
double* work = new double[n];
lapack_int info;
dtrexc("V", &n, getT().base(), &n, getQ().base(), &n, &ifst, &ilst, work,
&info);
delete [] work;
if (info < 0) {
throw SYLV_MES_EXCEPTION("Wrong argument to dtrexc.");
}
if (info == 0) {
// swap successful
getT().swapDiagLogically(itadd);
//check for 2-2 block splits
getT().checkDiagConsistency(it);
getT().checkDiagConsistency(itadd);
// and go back by 'it' in NEW eigenvalue set
--it;
return true;
}
return false;
}
double CMesh::getFOSV() {
for (int i=mXSize; i>0; i--) {
double mT = getT(0,i-1,0);
if ( mT > mFOTemp) {
double v1 = getS(0,i-1,0,1);
double v2 = getS(0,i,0,1);
return v1 + (v2-v1) * ((mFOTemp - mT) /(getT(0,i,0) - mT));
}
}
return 0.;
}
double CMesh::getFOSSST() {
for (int i=mXSize; i> 0; i--) {
double mT = getT(0,i-1,0);
if ( mT > mFOTemp) {
double v1 = ( getPixy(0,i-1,0,1,1) + getPixy(0,i-1,0,2,2)) /( getE(0,i-1,0) + getP(0,i-1,0));
double v2 = ( getPixy(0,i,0,1,1) + getPixy(0,i,0,2,2)) /( getE(0,i,0) + getP(0,i,0));
return (1./JHBARC) * (v1 + (v2- v1) * ((mFOTemp - mT) /(getT(0,i,0) - mT)));
}
}
return 0.;
}
double CMesh::getFOSY() {
for (int i=mYSize; i>0; i--) {
double mT = getT(0,0,i-1);
if ( mT > mFOTemp) {
double v1 = getX(0,0,i-1,2);
double v2 = getX(0,0,i,2);
return v1 + (v2-v1) * ((mFOTemp - mT) /(getT(0,0,i) - mT));
}
}
return 0.;
}
void TranslationRotation3D::createGLModelMatrix(float *M_out) const {
double TGL[3], RGL[3];
getT(TGL);
getR(RGL);
TGL[2] = -TGL[2];
RGL[2] = -RGL[2];
TranslationRotation3D TR(TGL, RGL);
double R_matGL[9];
TR.getR_mat(R_matGL);
M_out[0] = R_matGL[0];
M_out[1] = R_matGL[1];
M_out[2] = R_matGL[2];
M_out[3] = 0.0;
M_out[4] = R_matGL[3];
M_out[5] = R_matGL[4];
M_out[6] = R_matGL[5];
M_out[7] = 0.0;
M_out[8] = R_matGL[6];
M_out[9] = R_matGL[7];
M_out[10] = R_matGL[8];
M_out[11] = 0.0;
M_out[12] = TGL[0];
M_out[13] = TGL[1];
M_out[14] = TGL[2];
M_out[15] = 1.0;
}
Light* SkyDome::getLight(int time){
double t = getT(time, 0.1, 0.55);
double a = t*M_PI;
double sina = sin(a);
Colour black(0,0,0);
Colour white(1,1,1);
Light shadow;
shadow.ambient = black;
shadow.diffuse = black;
shadow.specular = black;
Light lit;
lit.ambient = black*(1-sina) + sina*Colour(0.2,0.2,0.2);
lit.diffuse = black*(1-sina) + sina*white;
lit.specular = black*(1-sina) + sina*white;
Light* lights = new Light[2];
lights[0] = shadow;
lights[1] = lit;
return lights;
}
void getHT(X &x)
{
getSubHeader();
if (mCtx.leftSub != sizeof(X))
fail("getHT(): subrecord size mismatch");
getT(x);
}
vector<double> CIEColor::SinglehueSequential(double t, double s, double b, double c, double h)
{
int N=10;
vector<double> Pseg;
vector<double> p0(3,0),p1(3,0), p2(3,0), q0(3,0), q1(3,0), q2(3,0);
p2[0]=100, p2[1]=0, p2[2]=h;
p1=dividingLCHuvforMSC_H(0,100,h);
p0[0]=0, p0[1]=0, p0[2]=h;
q0[0] = (1-s) * p0[0] + s *p1[0];
q0[1] = (1-s) * p0[1] + s *p1[1];
q0[2] = (1-s) * p0[2] + s *p1[2];
q2[0] = (1-s) * p2[0] + s *p1[0];
q2[1] = (1-s) * p2[1] + s *p1[1];
q2[2] = (1-s) * p2[2] + s *p1[2];
q1[0] = (q0[0] + q2[0])/2;
q1[1] = (q0[1] + q2[1])/2;
q1[2] = (q0[2] + q2[2])/2;
vector<double> T = getT(p0, p1, p2, q0, q1, q2, 125-125*pow(0.2,(1-c)*b+t*c));
Pseg = getCseg(p0, p1, p2, q0, q1, q2, T[0]);
Pseg[2]=h;
double r, g, k;
//LCHuvtoRGB(Pseg[0], Pseg[1], Pseg[2], r, g, k);
closestLCHuvtoRGB(Pseg[0], Pseg[1], Pseg[2], r, g, k);
Pseg[0]= r, Pseg[1]=g, Pseg[2]=k;
return Pseg;
}
void drawObject(objectData *o){
static GLfloat tMatrix[16];
printError("start draw object");
glUseProgram(o->shaderProgram);
getT(o,tMatrix);
glUniformMatrix4fv(glGetUniformLocation(o->shaderProgram, "transMatrix"), 1, GL_TRUE, tMatrix);
glUniformMatrix4fv(glGetUniformLocation(o->shaderProgram, "rotMatrix"), 1, GL_TRUE, o->rotationMatrix);
printError("object upload trans matrix");
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D,o->texture0);
glUniform1i(glGetUniformLocation(o->shaderProgram,"tex0"), 0); //Texture unit 0
if(o->texture1 != 0){
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D,o->texture1);
glUniform1i(glGetUniformLocation(o->shaderProgram,"tex1"), 1); //tex unit 1
}else{
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D,o->texture0);
glUniform1i(glGetUniformLocation(o->shaderProgram,"tex1"), 0); //tex unit 1
}
int h = glGetUniformLocation(o->shaderProgram, "rotMatrix");
printError("binding texture to model");
DrawModel(o->m,o->shaderProgram,"inPosition","inNormal","inTexCoord");
printError("object draw model");
}
bool KinematicsSolver::solveTheta56(double t3, double t2, double t1) {
Matrix T = getT(DH_a0, DH_d1, DH_alfa0, t1) * getT(DH_a1, DH_d2, DH_alfa1,
t2) * getT(DH_a2, DH_d3, DH_alfa2, t3);
// cout << "===================" << endl;
// cout << T.i() << endl;
// cout << "===================" << endl;
T = T.i() * target;
// cout << "===================" << endl;
// cout << T << endl;
// cout << "===================" << endl;
double* t5 = new double[2];
t5[0] = -acos(T(2, 3));
if (DBL_EQ(t5[0], 0)) {
t5[0] = 0;
return solveWristSingularity(currentQ4, t3, t2, t1);
}
// We proceed if t5 != 0;
t5[1] = -t5[0];
//cout << t5[0] << endl;
bool solved1 = false;
bool solved2 = false;
if (checkJointValidity(t5[0], JL5)) {
//if(checkTheta5(t5[0])){
solved1 = solveTheta6(t5[0], t3, t2, t1, T);
//}
}
if (!sameAngles(t5)) {
if (checkJointValidity(t5[1], JL5)) {
//if (checkTheta5(t5[1])){
solved2 = solveTheta6(t5[1], t3, t2, t1, T);
//}
}
}
return solved1 || solved2;
}
boost::shared_ptr<Continent> TeamDatabase::getOrCreateContinent(const char* n)
{
boost::shared_ptr<Soccer::Continent> cont = getT(n);
if(!cont) {
cont = boost::shared_ptr<Soccer::Continent>(new Soccer::Continent(n));
addT(cont);
}
return cont;
}
void SchurDecompEig::orderEigen()
{
diag_iter run = getT().diag_begin();
diag_iter runp = run;
++runp;
double last_size = 0.0;
while (runp != getT().diag_end()) {
diag_iter least = getT().findNextLargerBlock(run, getT().diag_end(),
last_size);
last_size = (*least).getSize();
if (run == least)
++run;
else
run = bubbleEigen(least, run);
runp = run;
++runp;
}
}
void getHT(X &x)
{
getSubHeader();
if (mCtx.leftSub != sizeof(X))
{
std::stringstream error;
error << "getHT(): subrecord size mismatch (requested " << sizeof(X) << ", got " << mCtx.leftSub << ")";
fail(error.str());
}
getT(x);
}
boost::shared_ptr<LeagueSystem> TeamDatabase::getOrCreateLeagueSystem(const char* continentName,
const char* countryName)
{
auto cont = getOrCreateContinent(continentName);
auto lsys = cont->getT(countryName);
if(!lsys) {
lsys = boost::shared_ptr<Soccer::LeagueSystem>(new Soccer::LeagueSystem(countryName));
cont->addT(lsys);
}
return lsys;
}
boost::shared_ptr<League> TeamDatabase::getOrCreateLeague(const char* continentName,
const char* countryName, const char* leagueName, unsigned int level)
{
auto lsys = getOrCreateLeagueSystem(continentName, countryName);
auto leag = lsys->getT(leagueName);
if(!leag) {
leag = boost::shared_ptr<Soccer::League>(new Soccer::League(leagueName, level));
lsys->addT(leag);
}
return leag;
}
Point3D* SkyDome::getMoonPos(int time){
double t = getT(time, 0.53, 0.45);
double a = t*M_PI;
if(a > M_PI || a < 0){
a = 3*M_PI/2;
}
double r = m_radius-2000;
Point3D *ret = new Point3D(r*cos(a), r*sin(a), -150);
*ret = rotation(toRadian(40), 'x') * (*ret);
return ret;
}
void parse_feed(hbc::json_pull& parser, int level) {
hbc::json_enumeration* enumerator = parser.object_elements();
while (enumerator->has_more_elements()) {
const char* key = enumerator->next_element();
if (strcmp(key, "id") == 0) {
printf("--id[");
getT(parser);
printf("]\n");
}
else if (strcmp(key, "title") == 0) {
printf("--title[");
getT(parser);
printf("]\n");
}
else if (strcmp(key, "logo") == 0) {
printf("--logo[");
getT(parser);
printf("]\n");
}
else if (strcmp(key, "openSearch$itemsPerPage") == 0) {
printf("--openSearch$itemsPerPage[");
getT(parser);
printf("]\n");
}
else {
printf("--key[%s]\n", key);
}
}
free(enumerator);
}
/** Room control routine.
* roomDescr - descriptor of the room to control
* targetTemp - target temperature for the room
*/
void controlRoom(RoomControlDescriptor* roomDescr, float targetTemp)
{
// -- Return if no room control available
if (!roomDescr->switchAddress || !roomDescr->sensorAddress)
return;
float roomTemp = getT(roomDescr->sensorAddress) + roomDescr->temperatureCorrection;
if (roomTemp > targetTemp + configuration.tempDelta)
changeSwitch(roomDescr->switchAddress, OFF);
else
changeSwitch(roomDescr->switchAddress, ON);
}
int CEquSystem::excludeVariables(VECTOR_ELEMENT_TYPE *pRes, VECTOR_ELEMENT_TYPE varIdx, VECTOR_ELEMENT_TYPE varValueDelta, const VECTOR_ELEMENT_TYPE *pVarMinVal, CVariableMapping *varMapping)
{
if (varValueDelta) {
pRes += varIdx;
// Add the mapping for first variable
const VECTOR_ELEMENT_TYPE varVal = *pRes += varValueDelta;
// Since we adjusted the right parts of extra equations, by pVarMinVal[varIdx],
// we need to extract this value from varVal
varMapping->addMapping(varIdx, varVal - pVarMinVal[varIdx]);
// Add the mapping for second (with the next index) variable
if (getT() > 2) {
*(pRes + 1) -= varValueDelta;
if (!variable(varIdx)) // this variable is not in the system
return 0;
} else {
// NOTE: What should be done with pVarMinVal[varIdx] ???
varMapping->addMapping(varIdx + 1, *(pRes + 1) -= varValueDelta);
}
}
// Exclude both from the system
return excludeVariables(varMapping, 1, true, getT() > 2? 1 : 2);
}
ReturnMatrix KinematicsSolver::getKine(ColumnVector &p) {
p.resize_keep(6);
Matrix T = getT(DH_a0, DH_d1, DH_alfa0, p(1)) * getT(DH_a1, DH_d2,
DH_alfa1, p(2)) * getT(DH_a2, DH_d3, DH_alfa2, p(3)) * getT(DH_a3,
DH_d4, DH_alfa3, p(4)) * getT(DH_a4, DH_d5, DH_alfa4, p(5)) * getT(
DH_a5, DH_d6, DH_alfa5, p(6));
T.Release();
return T;
}
vector<double> CIEColor::SinglehueSequential(vector<vector<double> > MSCs, double t, double s, double b, double c, double h)
{
int N=10;
vector<double> Pseg;
vector<double> p0(3,0),p1(3,0), p2(3,0), q0(3,0), q1(3,0), q2(3,0);
p2[0]=100, p2[1]=0, p2[2]=h;
//p1=dividingLCHuvforMSC_H(0,100,h);
int mid1, mid2;
mid1 = int(h * 100);
float ML, MC, dl;
dl = h*100 - mid1;
mid1 = mid1%3600;
mid2 = (mid1+1)%3600;;
ML = MSCs[mid1][0]*(1-dl) + MSCs[mid2][0]*dl;
MC = MSCs[mid1][1]*(1-dl) + MSCs[mid2][1]*dl;
p1[0]= ML, p1[1]= MC, p1[2]= h;
p0[0]=0, p0[1]=0, p0[2]=h;
q0[0] = (1-s) * p0[0] + s *p1[0];
q0[1] = (1-s) * p0[1] + s *p1[1];
q0[2] = (1-s) * p0[2] + s *p1[2];
q2[0] = (1-s) * p2[0] + s *p1[0];
q2[1] = (1-s) * p2[1] + s *p1[1];
q2[2] = (1-s) * p2[2] + s *p1[2];
q1[0] = (q0[0] + q2[0])/2;
q1[1] = (q0[1] + q2[1])/2;
q1[2] = (q0[2] + q2[2])/2;
vector<double> T = getT(p0, p1, p2, q0, q1, q2, 125-125*pow(0.2,(1-c)*b+t*c));
Pseg = getCseg(p0, p1, p2, q0, q1, q2, T[0]);
Pseg[2]=h;
double r, g, k;
//LCHuvtoRGB(Pseg[0], Pseg[1], Pseg[2], r, g, k);
closestLCHuvtoRGB(Pseg[0], Pseg[1], Pseg[2], r, g, k);
Pseg[0]= r, Pseg[1]=g, Pseg[2]=k;
return Pseg;
}
int main(int argc, const char** args)
{
// -- Set confguration.ini & others directories
setDirectories();
// -- Check for previous fatal errors
if (wasOverheated())
{
printf("Previous heater failure detected. Cannot run.\n\r");
return EXIT_FAIL;
}
// -- Load settings from .ini file
loadSettings();
initRoomDescriptors();
// debug stuff
// printf("%s", asctime(&configuration.arrive));
// printf("%s", asctime(&configuration.dep));
// time_t tt = getHeatingStartTime();
//
// printf("%s", ctime(&tt));
// printf("%f\n", getTargetTemp());
//
// printf("%f\n", configuration.tempDelta);
// printf("%f\n", configuration.fluidPumpOffTemp);
// return;
// end of debug stuff
// -- Measure current temperatures and set out the target
float controlTemp = getControlTemperature();
float outgoingFluidTemp = getT(outputSensor);
float ingoingFluidTemp = getT(inputSensor);
float electricHeaterTemp = getT(heaterSensor);
float kitchenTemp = getT(kitchenSensor);
float bathroomTemp = getT(bathRoomSensor);
float sashaBedroomTemp = getT(childrenSmallSensor);
float targetTemp = getTargetTemp();
// -- Control heater and pump
int heaterState = controlHeater(controlTemp, electricHeaterTemp, outgoingFluidTemp);
// -- Control sauna floor temp
float saunaFloorTemp = getT(saunaFloorSensor);
float saunaFloorTargetTemp = configuration.saunaFloorTemp;
int saunaFloorHeatingState = controlSaunaFloor(saunaFloorTemp, saunaFloorTargetTemp);
// -- Initizlize temp vector (no paritcular order)
float tv[10];
int tvc = 0;
tv[tvc++] = controlTemp;
tv[tvc++] = electricHeaterTemp;
tv[tvc++] = ingoingFluidTemp;
tv[tvc++] = outgoingFluidTemp;
tv[tvc++] = bathroomTemp;
tv[tvc++] = kitchenTemp;
tv[tvc++] = sashaBedroomTemp;
int pumpState = controlPump(tv, tvc);
// -- Individual rooms control
/* Not tested yet */
int i;
for (i=0; i<ROOMS_COUNT; i++)
controlRoom(&roomControlDescriptors[i], targetTemp);
// -- Dates: now and when to start heating next time by our arrival
char nowStr[TBL], onStr[TBL];
getDateTimeStr(nowStr, TBL, time(NULL));
getDateTimeStr(onStr, TBL, getHeatingStartTime());
printf("%s|%4.2f|%4.2f|%4.2f| %4.2f |%4.2f|%4.2f|%4.2f|%4.2f| %4.2f|%4.2f |%4.2f|%d|%d|%4.2f|%d|%c|%c|%4.1f|%4.1f|%s\r\n",
nowStr,
electricHeaterTemp,
ingoingFluidTemp,
outgoingFluidTemp,
getT(externalSensor),
getT(amSensor),
getT(bedroomSensor),
getT(cabinetSensor),
sashaBedroomTemp,
kitchenTemp,
bathroomTemp,
controlTemp,
heaterState,
pumpState,
saunaFloorTemp,
saunaFloorHeatingState,
(isPresence() ? 'P' : 'S'),
(isSaving() ? 'N' : 'D'),
targetTemp,
saunaFloorTargetTemp,
onStr
);
return EXIT_OK;
}
PV3D* PV3D::calculaN(){
n = getB()->productoVectorial(getT());
return n;
}
void SkyDome::draw(int time){
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, m_gradient);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
float U = (float)time/(CYCLE);
for(int i=0; i<m_nLat; i++){
for(int j=0; j<m_nLong; j++){
int a = i*(m_nLong+1) + j;
int b = a + (m_nLong+1);
float VTop = (float)(i+1)*2/(m_nLat+3);
float VBot = (float)(i+2)*2/(m_nLat+3);
glBegin(GL_TRIANGLES);
glTexCoord2f(U,VTop);
glVertex3d(m_vertices[a][0], m_vertices[a][1], m_vertices[a][2]);
glTexCoord2f(U,VBot);
glVertex3d(m_vertices[b][0], m_vertices[b][1], m_vertices[b][2]);
glTexCoord2f(U,VTop);
glVertex3d(m_vertices[a+1][0], m_vertices[a+1][1], m_vertices[a+1][2]);
glEnd();
glBegin(GL_TRIANGLES);
glTexCoord2f(U,VBot);
glVertex3d(m_vertices[b][0], m_vertices[b][1], m_vertices[b][2]);
glVertex3d(m_vertices[b+1][0], m_vertices[b+1][1], m_vertices[b+1][2]);
glTexCoord2f(U,VTop);
glVertex3d(m_vertices[a+1][0], m_vertices[a+1][1], m_vertices[a+1][2]);
glEnd();
}
}
glDisable(GL_TEXTURE_2D);
// calc sun position
Point3D* sunPos = getSunPos(time);
Point3D* moonPos = getMoonPos(time);
//TODO calc sun color and size
double sina = sin(getT(time, 0.1, 0.55)*M_PI);
int sunSize = 2500 - sina*1200;
Colour sunColor = Colour(1,0.85,0.7)*(1-sina) + Colour(1,1,1)*sina;
// draw sun
GLUquadricObj* qsphere = gluNewQuadric();
gluQuadricDrawStyle(qsphere, GLU_FILL);
gluQuadricOrientation(qsphere, GLU_OUTSIDE);
gluQuadricTexture(qsphere, GL_FALSE);
glColor3f(sunColor.R(),sunColor.G(),sunColor.B());
glPushMatrix();
glTranslatef((*sunPos)[0],(*sunPos)[1],(*sunPos)[2]);
gluSphere(qsphere, sunSize, 20, 20);
glPopMatrix();
// draw moon
GLUquadricObj* qsphere2 = gluNewQuadric();
gluQuadricDrawStyle(qsphere2, GLU_FILL);
gluQuadricOrientation(qsphere2, GLU_OUTSIDE);
gluQuadricTexture(qsphere2, GL_FALSE);
glColor3f(1,0.9,0.6);
glPushMatrix();
glTranslatef((*moonPos)[0],(*moonPos)[1],(*moonPos)[2]);
gluSphere(qsphere2, 1000, 20, 20);
glPopMatrix();
}
/** Returnes combined avergage temperature to control */
float getControlTemperature()
{
return getT(bedroomSensor);
}
void getName(NAME &name) { getT(name); }
int getStarData(STAR* pStar)
{
std::ostringstream ossLog;
unsigned int nPos[] = {6, 32, 50, 53, 56, 62, 69, 76, 89, 96, 102, 110, 117, 120, 0};
char* szBuf = new char[1200*512]; // WARNING!!! THE SIZE!
int nRows = 0;
if(cxFile2BufA("C:\\media\\StarCatalog.prn", szBuf)>0xff)
{
nRows = cxGetLineCount(szBuf); //
}
else
{
delete[] szBuf;
return 0;
}
std::cout<<nRows<<" Rows;"<<std::endl;
// char* szTmp = szBuf;
// std::cout<<szBuf<<std::endl;
// std::cout<<"//////////////////////////////////////////////////////////////////////////////"<<std::endl;
int nCols = sizeof(nPos)/sizeof(int)-1;
// std::cout<<"nRows = "<<nRows<<std::endl;
// std::cout<<"nCols = "<<nCols<<std::endl;
char*** ppszOut = new char**[nRows];
for(unsigned int i = 0; i<nRows; i++)
{
ppszOut[i] = new char*[nCols];
for(unsigned int j = 0; j<nCols; j++)
{
ppszOut[i][j] = new char[nPos[j]-(j>0)*nPos[j-(j>0)]+1]; //
memset(ppszOut[i][j], 0, nPos[j]-(j>0)*nPos[j-(j>0)]+1); //
}
}
getT(ppszOut, szBuf, nPos, nRows, nCols);
// std::cout<<"//////////////////////////////////////////////////////////////////////////////"<<std::endl;
for(unsigned int i = 1; i<nRows; i++) // nRows - 1 stars, because the first row is the header of the table...
{
for(unsigned int j = 0; j<nCols-1; j++)
{
ppszOut[i][j] = trim(ppszOut[i][j]);
}
(pStar+i-1)->nRank = atoi(ppszOut[i][ 0]);
strcpy_s((pStar+i-1)->szBayer_Name, ppszOut[i][ 1]);
strcpy_s((pStar+i-1)->szName, ppszOut[i][ 2]);
(pStar+i-1)->nRA = atoi(ppszOut[i][ 3]);
(pStar+i-1)->nM = atoi(ppszOut[i][ 4]);
(pStar+i-1)->fB = atof(ppszOut[i][ 5]);
(pStar+i-1)->fLGAL = atof(ppszOut[i][ 6]);
(pStar+i-1)->fBGAL = atof(ppszOut[i][ 7]);
strcpy_s((pStar+i-1)->szSpectralType, ppszOut[i][ 8]);
(pStar+i-1)->fMag = atof(ppszOut[i][ 9]);
(pStar+i-1)->fA_Mag = atof(ppszOut[i][10]);
(pStar+i-1)->fPrllx = atof(ppszOut[i][11]);
(pStar+i-1)->fErr = atof(ppszOut[i][12]);
(pStar+i-1)->fDLY = atof(ppszOut[i][13]);
// std::cout<<ppszOut[i][ 1]<<", "; // DEBUG
// std::cout<<ppszOut[i][13]<<", "; // DEBUG
// std::cout<<std::endl;
}
//////////
std::ofstream fib("d:\\tmp\\out_deb.txt");
if(fib.is_open())
{
for(int i=1; i<=nRows; i++)
{
fib<<
(pStar+i)->nRank <<";"<<
(pStar+i)->szBayer_Name <<";"<<
(pStar+i)->szName <<";"<<
(pStar+i)->nRA <<";"<<
(pStar+i)->nM <<";"<<
(pStar+i)->fB <<";"<<
(pStar+i)->fLGAL <<";"<<
(pStar+i)->fBGAL <<";"<<
(pStar+i)->szSpectralType <<";"<<
(pStar+i)->fMag <<";"<<
(pStar+i)->fA_Mag <<";"<<
(pStar+i)->fPrllx <<";"<<
(pStar+i)->fErr <<";"<<
(pStar+i)->fDLY <<";"<<
std::endl;
}
}
else
{
std::cout<<"cannot open test file for parameter output; error: "<<getLastErrorAsString().c_str()<<std::endl;
}
fib.close();
std::cout<<"/////////////////////////////////////////"<<std::endl;
delete[] ppszOut;
// std::cout<<"after delete[] ppszOut"<<std::endl;
// std::cout<<"##DEBUG3889##"<<std::endl;
delete[] szBuf;
// std::cout<<"##DEBUG3323##"<<std::endl;
// std::cout<<ossLog.str().c_str()<<std::endl;
// std::cout << "Hello, world!\n";
return nRows;
}
void getUint(uint32_t &u) { getT(u); }
void getHomographyRtMatrixAndRotationCenter(
std::vector<cv::Point2f> corners, const cv::Size& imageSize,
const cv::Size& boardSize, const double& squareSize,
const int &horizon, const int &deadZone,
const cv::Mat& cameraMatrix,
const cv::Mat& distortionCoefficients,
const double &chessboardHeight, cv::Mat& homography,
cv::Point2d &rotationCenter, cv::Mat &rtMatrix) {
//The size of chessboard on the image
cv::Size imageBoardSize((boardSize.width - 1) * squareSize,
(boardSize.height - 1) * squareSize);
std::vector<cv::Point2f> objPts(4), imgPts(4);
//Describe where the chessboardcorners are on the outcome image
objPts[0].x = imageSize.width / 2 - imageBoardSize.width / 2;
objPts[0].y = imageSize.height - horizon - deadZone
- imageBoardSize.height;
objPts[1].x = imageSize.width / 2 + imageBoardSize.width / 2;
objPts[1].y = imageSize.height - horizon - deadZone
- imageBoardSize.height;
objPts[2].x = imageSize.width / 2 - imageBoardSize.width / 2;
objPts[2].y = imageSize.height - horizon - deadZone;
objPts[3].x = imageSize.width / 2 + imageBoardSize.width / 2;
objPts[3].y = imageSize.height - horizon - deadZone;
//The only four corners used to get homography
imgPts[0] = corners[0];
imgPts[1] = corners[boardSize.width - 1];
imgPts[2] = corners[(boardSize.height - 1) * boardSize.width];
imgPts[3] = corners[(boardSize.height) * boardSize.width - 1];
cv::Mat rvec, tvec;
//Creation of 3D chessboard model
std::vector<cv::Point3d> mCorners;
for (int i = 0; i < boardSize.height; ++i) {
for (int j = 0; j < boardSize.width; ++j) {
mCorners.push_back(
cv::Point3f(i * squareSize, j * squareSize,
chessboardHeight));
}
}
std::vector<cv::Point3d> p3pcorn(4);
p3pcorn[0] = mCorners[0];
p3pcorn[1] = mCorners[boardSize.width - 1];
p3pcorn[2] = mCorners[(boardSize.height - 1) * boardSize.width];
p3pcorn[3] = mCorners[(boardSize.height) * boardSize.width - 1];
//Get rvec and tvec describing where the chessboards first corner lays in cameras coordinate system
cv::solvePnP(p3pcorn, imgPts, cameraMatrix,
distortionCoefficients, rvec, tvec, false, CV_ITERATIVE);
cv::Mat rot;
cv::Rodrigues(rvec, rot);
rtMatrix = invRtTransformMatrix(rot, tvec);
{
cv::Mat invT = getT(rtMatrix);
cv::Mat z = rtMatrix(cv::Range(0, 3), cv::Range(2, 3));
z.at<double>(2) = 0;
cv::Mat zN = z / norm(cv::Vec3d(z));
double gamma = atan2(zN.at<double>(1), zN.at<double>(0))
- atan2(0, 1);
gamma *= 180 / CV_PI;
cv::Mat rMatN = cv::getRotationMatrix2D(objPts[0], -gamma, 1);
cv::Mat rMatP = cv::getRotationMatrix2D(objPts[0], gamma, 1);
cv::Mat homN;
std::vector<cv::Point2f> rPointsN;
cv::transform(objPts, rPointsN, rMatN);
homN = cv::getPerspectiveTransform(rPointsN, imgPts);
// VERY IMPORTANT: in bird's eye view, the x and y axes are swapped!
rotationCenter = rPointsN[0]
+ cv::Point2f(rtMatrix.at<double>(1, 3),
rtMatrix.at<double>(0, 3));
std::vector<cv::Point2f> rcenter, rrcenter;
rcenter.push_back(rotationCenter);
cv::transform(rcenter, rrcenter, rMatN);
rotationCenter = rrcenter.front();
homography = homN.clone();
}
}
