// Not implemented
TEST_F(SO3Test, RotationXYZ) {
EXPECT_CLOSE(Rx(0, 0), 1.0);
EXPECT_CLOSE(Rx(0, 1), 0.0);
EXPECT_CLOSE(Rx(0, 2), 0.0);
EXPECT_CLOSE(Rx(1, 0), 0.0);
EXPECT_CLOSE(Rx(1, 1), 0.5);
EXPECT_CLOSE(Rx(1, 2), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 0), 0.0);
EXPECT_CLOSE(Rx(2, 1), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 2), 0.5);
EXPECT_CLOSE(Ry(0, 0), 0.5);
EXPECT_CLOSE(Ry(0, 1), 0.0);
EXPECT_CLOSE(Ry(0, 2), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(1, 0), 0.0);
EXPECT_CLOSE(Ry(1, 1), 1.0);
EXPECT_CLOSE(Ry(1, 2), 0.0);
EXPECT_CLOSE(Ry(2, 0), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(2, 1), 0.0);
EXPECT_CLOSE(Ry(2, 2), 0.5);
EXPECT_CLOSE(Rz(0, 0), 0.5);
EXPECT_CLOSE(Rz(0, 1), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(0, 2), 0.0);
EXPECT_CLOSE(Rz(1, 0), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(1, 1), 0.5);
EXPECT_CLOSE(Rz(1, 2), 0.0);
EXPECT_CLOSE(Rz(2, 0), 0.0);
EXPECT_CLOSE(Rz(2, 1), 0.0);
EXPECT_CLOSE(Rz(2, 2), 1.0);
}
// Rotation around arbitrary axis (rotation only)
mat4 ArbRotate(vec3 axis, GLfloat fi)
{
vec3 x, y, z;
mat4 R, Rt, Raxel, m;
// Check if parallel to Z
if (axis.x < 0.0000001) // Below some small value
if (axis.x > -0.0000001)
if (axis.y < 0.0000001)
if (axis.y > -0.0000001)
{
if (axis.z > 0)
{
m = Rz(fi);
return m;
}
else
{
m = Rz(-fi);
return m;
}
}
x = Normalize(axis);
z = SetVector(0,0,1); // Temp z
y = Normalize(CrossProduct(z, x)); // y' = z^ x x'
z = CrossProduct(x, y); // z' = x x y
if (transposed)
{
R.m[0] = x.x; R.m[4] = x.y; R.m[8] = x.z; R.m[12] = 0.0;
R.m[1] = y.x; R.m[5] = y.y; R.m[9] = y.z; R.m[13] = 0.0;
R.m[2] = z.x; R.m[6] = z.y; R.m[10] = z.z; R.m[14] = 0.0;
R.m[3] = 0.0; R.m[7] = 0.0; R.m[11] = 0.0; R.m[15] = 1.0;
}
else
{
R.m[0] = x.x; R.m[1] = x.y; R.m[2] = x.z; R.m[3] = 0.0;
R.m[4] = y.x; R.m[5] = y.y; R.m[6] = y.z; R.m[7] = 0.0;
R.m[8] = z.x; R.m[9] = z.y; R.m[10] = z.z; R.m[11] = 0.0;
R.m[12] = 0.0; R.m[13] = 0.0; R.m[14] = 0.0; R.m[15] = 1.0;
}
Rt = Transpose(R); // Transpose = Invert -> felet ej i Transpose, och det Šr en ortonormal matris
Raxel = Rx(fi); // Rotate around x axis
// m := Rt * Rx * R
m = Mult(Mult(Rt, Raxel), R);
return m;
}
Matrix & cartControlReachAvoidThread::rotz(const double theta)
{
double t=CTRL_DEG2RAD*theta;
double c=cos(t);
double s=sin(t);
Rz(0,0)=Rz(1,1)=c;
Rz(0,1)=-s;
Rz(1,0)=s;
return Rz;
}
//------------------------------
void MolState::rotate(const double alpha_x, const double alpha_y, const double alpha_z)
{
// three rotation matrices (instead of making one matrix) arouns x, y, z axes
KMatrix R(CARTDIM,CARTDIM), Rx(CARTDIM,CARTDIM), Ry(CARTDIM,CARTDIM), Rz(CARTDIM,CARTDIM);
Rx.Set(0);
Rx.Elem2(0,0)=1;
Rx.Elem2(1,1)=cos(alpha_x);
Rx.Elem2(2,2)=cos(alpha_x);
Rx.Elem2(2,1)=-sin(alpha_x);
Rx.Elem2(1,2)=sin(alpha_x);
Ry.Set(0);
Ry.Elem2(1,1)=1;
Ry.Elem2(0,0)=cos(alpha_y);
Ry.Elem2(2,2)=cos(alpha_y);
Ry.Elem2(2,0)=sin(alpha_y);
Ry.Elem2(0,2)=-sin(alpha_y);
Rz.Set(0);
Rz.Elem2(2,2)=1;
Rz.Elem2(0,0)=cos(alpha_z);
Rz.Elem2(1,1)=cos(alpha_z);
Rz.Elem2(1,0)=-sin(alpha_z);
Rz.Elem2(0,1)=sin(alpha_z);
// overall rotation R=Rx*Ry*Rz
R.SetDiagonal(1);
R*=Rx;
R*=Ry;
R*=Rz;
// and now rotates using matix R:
transformCoordinates(R);
}
Eigen::MatrixXd cameraMatrixKnownRT(const double rx, const double ry, const double rz, const double tx, const double ty, const double tz)
{
// Rotation matrix R = Rx * Ry * Rz(Euler's rotation theorem: any rotation can be broken into 3 angles)
// Rx rotation by rx about x:
Eigen::MatrixXd Rx(3,3), Ry(3,3), Rz(3,3);
Rx << cos(rx), -sin(rx), 0,
sin(rx), cos(rx), 0,
0, 0, 1;
// Ry rotation by ry about y:
Ry << cos(ry), 0, -sin(ry),
0, 1, 0,
sin(ry), 0, cos(ry);
// Rz rotation by rz about z:
Rz << cos(rz), sin(rz), 0,
-sin(rz), cos(rz), 0,
0, 0, 1;
Eigen::MatrixXd R = Rx * Ry * Rz;
Eigen::MatrixXd P(3,4);
P << R(0, 0), R(0, 1), R(0, 2), tx,
R(1, 0), R(1, 1), R(1, 2), ty,
R(2, 0), R(2, 1), R(2, 2), tz;
return P;
}
/////////////////////////////////////////////
// A N I M A T E B O N E S
// Desc: en väldigt enkel animation av skelettet
// vrider ben 1 i en sin(counter)
void animateBones(void)
{
int bone;
// Hur mycket kring varje led? €ndra gŠrna.
float angleScales[10] = { 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f };
float time = glutGet(GLUT_ELAPSED_TIME) / 1000.0;
// Hur mycket skall vi vrida?
float angle = sin(time * 3.f) / 2.0f;
memcpy(&g_bonesRes, &g_bones, kMaxBones*sizeof(Bone));
g_bonesRes[0].rot = Rz(angle * angleScales[0]);
for (bone = 1; bone < kMaxBones; bone++)
g_bonesRes[bone].rot = Rz(angle * angleScales[bone]);
}
//Angle must be in radians
Matrix_2D Sinusoid::get_rot_matrix( float strike )
{
Matrix_2D Rz(2,2);
Rz(1,1) = 1.0;
Rz(2,2) = 1.0;
if ( !GsTL::equals( strike, float(0.) ) ) {
Rz(1,1) = std::cos( strike );
Rz(1,2) = std::sin( strike );
Rz(2,1) = -Rz(1,2);
Rz(2,2) = Rz(1,1);
}
return Rz;
}
void draw_windmill(windmill_t* w, float dt)
{
glUseProgram(programs[WINDMILL_PROGRAM]);
// Send in additional params
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "projectionMatrix"), 1, GL_TRUE, projectionMatrix);
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "camMatrix"), 1, GL_TRUE, camMatrix);
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "lightSourcesColors"), 1, GL_FALSE, lightSourcesColors);
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "lightSourcesDirections"), 1, GL_FALSE, lightSourcesDirections);
float camera_position[3]; camera_position[0] = position.x; camera_position[1] = position.y; camera_position[2] = position.z;
glUniform3fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "camera_position"), 1, camera_position);
w->bladeangle += dt*windspeed/3;
Rz(w->bladeangle, w->bladerotationMatrix);
GLfloat bladeBaseMatrix[16];
Mult(w->windmillMDLMatrix[WINDMILL_BASE], w->bladecenterMatrix, work[0]);
Mult(work[0], w->bladerotationMatrix, bladeBaseMatrix);
{
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, billboards[WOOD_TEXTURE]);
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, billboards[WOOD_TEXTURE]);
glUniform1i(glGetUniformLocation(programs[WINDMILL_PROGRAM], "firstTexUnit"), 0);
glUniform1i(glGetUniformLocation(programs[WINDMILL_PROGRAM], "secondTexUnit"), 1);
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "baseMatrix"), 1, GL_TRUE, bladeBaseMatrix);
int i = 0;
for(i = 0; i < 4; ++i) {
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "mdlMatrix"), 1, GL_TRUE, w->bladeMDLMatrix[i]);
DrawModel(w->blades[i]);
}
}
{
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "baseMatrix"), 1, GL_TRUE, w->windmillMDLMatrix[WINDMILL_BASE]);
int i = 0;
for(i = 1; i < 3; ++i) {
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "mdlMatrix"), 1, GL_TRUE, w->windmillMDLMatrix[i]);
DrawModel(w->windmill[i]);
}
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, billboards[BRICK_TEXTURE]);
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, billboards[BRICK_CRACK_TEXTURE]);
glUniformMatrix4fv(glGetUniformLocation(programs[WINDMILL_PROGRAM], "mdlMatrix"), 1, GL_TRUE, w->windmillMDLMatrix[WALLS]);
DrawModel(w->windmill[WALLS]);
}
//printError("display windmill");
}
void dR_dthetay(Matrix3x3 &out, const Vector3 &theta)
{
Matrix3x3 mRx(Rx(theta.x()));
Matrix3x3 mRz(Rz(theta.z()));
Matrix3x3 mdRy_dtheta(dRy_dtheta(theta.y()));
out = mRz*mdRy_dtheta*mRx;
}
void dR_dthetax(Matrix3x3 &out, const Vector3 &theta)
{
Matrix3x3 mRy(Ry(theta.y()));
Matrix3x3 mRz(Rz(theta.z()));
Matrix3x3 mdRx_dtheta(dRx_dtheta(theta.x()));
out = mRz*mRy*mdRx_dtheta;
}
// Rotation kring godtycklig axel (enbart rotationen)
void ArbRotate(Point3D *axis, GLfloat fi, GLfloat *m)
{
Point3D x, y, z, a;
GLfloat R[16], Rt[16], Raxel[16], RtRx[16];
// Kolla ocksΠom parallell med Z-axel!
if (axis->x < 0.0000001) // Under nŒgon tillrŠckligt liten grŠns
if (axis->x > -0.0000001)
if (axis->y < 0.0000001)
if (axis->y > -0.0000001)
if (axis->z > 0)
{
Rz(fi, m);
return;
}
else
{
Rz(-fi, m);
return;
}
x = *axis;
Normalize(&x); // |x|
SetVector(0,0,1, &z); // Temp z
CrossProduct(&z, &x, &y);
Normalize(&y); // y' = z^ x x'
CrossProduct(&x, &y, &z); // z' = x x y
R[0] = x.x; R[4] = x.y; R[8] = x.z; R[12] = 0.0;
R[1] = y.x; R[5] = y.y; R[9] = y.z; R[13] = 0.0;
R[2] = z.x; R[6] = z.y; R[10] = z.z; R[14] = 0.0;
R[3] = 0.0; R[7] = 0.0; R[11] = 0.0; R[15] = 1.0;
Transpose(&R, &Rt); // Transpose = Invert -> felet ej i Transpose, och det Šr en ortonormal matris
Rx(fi, &Raxel); // Rotate around x axis
// m := Rt * Rx * R
Mult(&Rt, &Raxel, &RtRx);
Mult(&RtRx, &R, m);
}
void dR_dtheta(Matrix3x3 &out, const Vector3 &theta, const Vector3 &theta_dot)
{
Matrix3x3 mRx(Rx(theta.x()));
Matrix3x3 mRy(Ry(theta.y()));
Matrix3x3 mRz(Rz(theta.z()));
Matrix3x3 mdRx_dtheta(dRx_dtheta(theta.x()));
Matrix3x3 mdRy_dtheta(dRx_dtheta(theta.y()));
Matrix3x3 mdRz_dtheta(dRx_dtheta(theta.z()));
out = mdRz_dtheta*mRy*mRx*theta_dot.z();
out += mRz*mdRy_dtheta*mRx*theta_dot.y();
out += mRz*mRy*mdRx_dtheta*theta_dot.x();
}
void draw_boost(Maze* maze){
mat4 total;
int boost = maze->boost;
int boost_x_pos = maze->boost_x_pos;
int boost_z_pos = maze->boost_z_pos;
mat4 boost_pos = T(boost_x_pos, 5, boost_z_pos);
if (boost == 0) {
//Star
glBindTexture(GL_TEXTURE_2D, startex);
glActiveTexture(GL_TEXTURE0);
total = maze->get_total();
total = Mult(total, boost_pos); //T(20,5,4));
total = Mult(total, Rx(M_PI_2));
total = Mult(total, Rz(M_PI_2));
total = Mult(total, S(0.5, 0.5, 0.5));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Star,program,"inPosition","inNormal","inTexCoord");
}
else if (boost == 1) {
//speed
glBindTexture(GL_TEXTURE_2D, speedtex);
glActiveTexture(GL_TEXTURE0);
total = maze->get_total();
total = Mult(total, boost_pos); //T(5,5,8));
//total = Mult(total, S(0.1, 0.1, 0.1));
total = Mult(total, S(0.5, 0.5, 0.5));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(SpeedObj,program,"inPosition","inNormal","inTexCoord");
}
else if ( boost == 2){
//imortal
glBindTexture(GL_TEXTURE_2D, imortaltex);
glActiveTexture(GL_TEXTURE0);
total = maze->get_total();
total = Mult(total, boost_pos); //T(8,4.5,8));
//total = Mult(total, S(0.3, 0.3, 0.3));
total = Mult(total, S(0.5, 0.5, 0.5));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Imortal,program,"inPosition","inNormal","inTexCoord");
}
}
//---------------------------------------------------------
bool CSG_Direct_Georeferencer::Set_Transformation(CSG_Parameters &Parameters, int nCols, int nRows)
{
//-----------------------------------------------------
m_O.Create(2);
m_O[0] = nCols / 2.0;
m_O[1] = nRows / 2.0;
m_f = Parameters("CFL" )->asDouble() / 1000; // [mm] -> [m]
m_s = Parameters("PXSIZE")->asDouble() / 1000000; // [micron] -> [m]
//-----------------------------------------------------
m_T.Create(3);
m_T[0] = Parameters("X")->asDouble();
m_T[1] = Parameters("Y")->asDouble();
m_T[2] = Parameters("Z")->asDouble();
//-----------------------------------------------------
double a;
CSG_Matrix Rx(3, 3), Ry(3, 3), Rz(3, 3);
a = Parameters("OMEGA")->asDouble() * M_DEG_TO_RAD;
Rx[0][0] = 1; Rx[0][1] = 0; Rx[0][2] = 0;
Rx[1][0] = 0; Rx[1][1] = cos(a); Rx[1][2] = -sin(a);
Rx[2][0] = 0; Rx[2][1] = sin(a); Rx[2][2] = cos(a);
a = Parameters("PHI" )->asDouble() * M_DEG_TO_RAD;
Ry[0][0] = cos(a); Ry[0][1] = 0; Ry[0][2] = sin(a);
Ry[1][0] = 0; Ry[1][1] = 1; Ry[1][2] = 0;
Ry[2][0] = -sin(a); Ry[2][1] = 0; Ry[2][2] = cos(a);
a = Parameters("KAPPA")->asDouble() * M_DEG_TO_RAD + Parameters("KAPPA_OFF")->asDouble() * M_DEG_TO_RAD;
Rz[0][0] = cos(a); Rz[0][1] = -sin(a); Rz[0][2] = 0;
Rz[1][0] = sin(a); Rz[1][1] = cos(a); Rz[1][2] = 0;
Rz[2][0] = 0; Rz[2][1] = 0; Rz[2][2] = 1;
switch( Parameters("ORIENTATION")->asInt() )
{
case 0: default: m_R = Rz * Rx * Ry; break; // BLUH
case 1: m_R = Rx * Ry * Rz; break; // PATB
}
m_Rinv = m_R.Get_Inverse();
return( true );
}
void CreateCubeHeightMaps(TextureData* terrainTextures[6], mat4 terrainTransformationMatrix[6], struct PlanetStruct* planet)
{
GLuint i;
for(i = 0; i < 6; i++)
{
terrainTextures[i] = chkmalloc(sizeof(TextureData));
}
// Load terrain data
for (i = 0; i < 6; i++)
if(fuzzy == 0)
LoadTGATextureData("textures/fft-terrain.tga", terrainTextures[i]);
else
LoadTGATextureData("testTGA.tga", terrainTextures[i]);
//Generate terrain model matrix
planet->terrainWidth = (GLfloat)terrainTextures[0]->width;
planet->terrainHeight = (GLfloat)terrainTextures[0]->height;
GLfloat distanceToMiddleX = ((GLfloat)terrainTextures[0]->width*0.5f);
GLfloat distanceToMiddleZ = ((GLfloat)terrainTextures[0]->height*0.5f);
GLfloat distanceToMiddleY = planet->radius;
for (i = 0; i < 4; ++i)
{
terrainTransformationMatrix[i] = T(-distanceToMiddleX, distanceToMiddleY, -distanceToMiddleZ);
terrainTransformationMatrix[i] = Mult( Rz(M_PI*0.5f*(GLfloat)i), terrainTransformationMatrix[i] );
}
//Last two sides
for (i = 0; i < 2; ++i)
{
terrainTransformationMatrix[4+i] = T(-distanceToMiddleX, distanceToMiddleY, -distanceToMiddleZ);
terrainTransformationMatrix[4+i] = Mult( Rx(M_PI*(0.5f+(GLfloat)i)), terrainTransformationMatrix[4+i] );
}
//Weird offset: (Probably size dependent)
terrainTransformationMatrix[1] = Mult(T(0, 1, 0), terrainTransformationMatrix[1]);
terrainTransformationMatrix[2] = Mult(T(-1, 1, 0), terrainTransformationMatrix[2]);
terrainTransformationMatrix[3] = Mult(T(-1, 0, 0), terrainTransformationMatrix[3]);
terrainTransformationMatrix[4] = Mult(T(0, 0, -1), terrainTransformationMatrix[4]);
terrainTransformationMatrix[5] = Mult(T(0, 1, 0), terrainTransformationMatrix[5]);
}
//construct the rotation matrices between ICRF and the local frame
void frame::construct_rotation_matrices(const double& ETepoch)
{
//first, get the current date in TBB
double TDBepoch = convert_ET_to_TDB(ETepoch);
double days_since_reference_epoch = TDBepoch - 2451545.0;
double centuries_since_reference_epoch = days_since_reference_epoch / 36525;
//compute the current values of the angles alpha and delta
double alpha = alpha0 + alphadot * centuries_since_reference_epoch;
double delta = delta0 + deltadot * centuries_since_reference_epoch;
//next, construct the rotation matrix
math::Matrix<double> Rx (3, (math::PIover2 - delta), math::Rxhat);
math::Matrix<double> Rz (3, (math::PIover2 + alpha), math::Rzhat);
R_from_local_to_ICRF = Rz*Rx;
R_from_ICRF_to_local = R_from_local_to_ICRF.transpose();
}
void Body::rotate(char direction, float angle)
{
switch (direction) {
case 'x':
// Rotate around x
rot_mat = Rx(angle) * rot_mat;
break;
case 'y':
// Rotate around y
rot_mat = rot_mat * Ry(angle) * rot_mat;
break;
case 'z':
// Rotate around z
rot_mat = Rz(angle) * rot_mat;
break;
}
update();
}
void DrawSprite(SpritePtr sp)
{
mat4 trans, rot, scale, m;
glUseProgram(program);
// Update matrices
scale = S((float)sp->face->width/gWidth * 2, (float)sp->face->height/gHeight * 2, 1);
// trans = T(sp->position.h/gWidth, sp->position.v/gHeight, 0);
trans = T(sp->position.h/gWidth * 2 - 1, sp->position.v/gHeight * 2 - 1, 0);
rot = Rz(sp->rotation * 3.14 / 180);
m = Mult(trans, Mult(scale, rot));
glUniformMatrix4fv(glGetUniformLocation(program, "m"), 1, GL_TRUE, m.m);
glBindTexture(GL_TEXTURE_2D, sp->face->texID);
// Draw
glBindVertexArray(vertexArrayObjID); // Select VAO
glDrawArrays(GL_TRIANGLES, 0, 6); // draw object
}
void draw_obsticle(Maze* maze){
//gate
//OBSTACLES
int obstacle = maze->obstacle;
int obstacle_x_pos = maze->obstacle_x_pos;
mat4 obstacle_pos = T(obstacle_x_pos, 4, 8);
if ( obstacle == 1){ //BYT ALLT ANDREAS
//gate
glBindTexture(GL_TEXTURE_2D, gatetex);
glActiveTexture(GL_TEXTURE0);
//hö pelare
mat4 total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 6 , 2));
total = Mult(total, T(0,-0.25,2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
//vänstra pelare
total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 6 , 2));
total = Mult(total, T(0,-0.25,-2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
//tak
total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 2 , 6));
total = Mult(total, T(0,0.27,0));
total = Mult(total, Ry(M_PI_2));
total = Mult(total, Rz(M_PI_2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
}
}
void display(void)
{
printError("pre display");
// clear the screen (using chosen color earlier)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(1,1,1,0);
// Set rotation matrix
phi = ( phi < 2*PI ) ? phi+PI/50 : phi-2*PI+PI/50;
vec3 trans = {1,0,-3};
vec3 trans2 = {-1,0,-3};
vec3 lookAtPoint = {0,0,-3};
vec3 cameraLocation = {3.0f*cos(phi),0.0f,-3+3.0f*sin(phi)};
vec3 upVector = {0,1,0};
mat4 translation = T(trans.x,trans.y,trans.z);
mat4 rotations = Mult(Ry(phi),Mult(Rx(phi), Rz(phi)));
mat4 lookAtMatrix = lookAtv(cameraLocation,lookAtPoint,upVector);
transformMatrix = Mult(translation,rotations);
// Send translMatrix to Vertex
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m);
// Send lookAt-vector to vertex shader
glUniformMatrix4fv(glGetUniformLocation(program, "lookAtMatrix"), 1, GL_TRUE, lookAtMatrix.m);
DrawModel(bunny, program, "in_Position", "in_Normal", "inTexCoord");
// Model 2
transformMatrix = T(trans2.x, trans2.y, trans2.z);
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m);
DrawModel(bunny_model2, program, "in_Position", "in_Normal", "inTexCoord");
printError("display");
glutSwapBuffers(); // Swap buffer so that GPU can use the buffer we uploaded to it and we can write to another
}
void display(void)
{
printError("pre display");
// clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
mat4 m1, m2, m;
a += 0.1;
m1 = Rz(Pi/5);
m2 = Ry(a);
m = Mult(m2,m1);
glUniformMatrix4fv(glGetUniformLocation(program, "myMatrix"), 1, GL_TRUE, m.m);
glBindVertexArray(vertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, 36, GL_UNSIGNED_BYTE, NULL); // draw object
printError("display");
glutSwapBuffers();
}
void init_windmill(windmill_t* w, int n)
{
programs[WINDMILL_PROGRAM] = loadShaders("lab3-3.vert", "lab3-3.frag");
LoadTGATextureSimple("wood.tga", &billboards[WOOD_TEXTURE]);
LoadTGATextureSimple("brick.tga", &billboards[BRICK_TEXTURE]);
LoadTGATextureSimple("brick-crack.tga", &billboards[BRICK_CRACK_TEXTURE]);
w->bladeangle = 0;
w->windmill[WALLS] = LoadModelPlus("windmill/windmill-walls.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
w->windmill[BALCONY] = LoadModelPlus("windmill/windmill-balcony.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
w->windmill[ROOF] = LoadModelPlus("windmill/windmill-roof.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
T(0.05,9.15,4.5,work[1]);
Ry(-M_PI_2,work[0]);
Mult(work[0], work[1], w->bladecenterMatrix);
{
int i = 0;
for(i = 0; i < 4; ++i) {
T(0,0,0,w->bladeMDLMatrix[i]);
Ry(100*M_PI/180,work[0]);
Mult(work[0], w->bladeMDLMatrix[i], work[1]);
Rz(M_PI_2*i,work[0]);
Mult(work[0], work[1], w->bladeMDLMatrix[i]);
w->blades[i] = LoadModelPlus("windmill/blade.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
}
}
T((MILL_RADIUS + MILL_RADIUS/(n+1.0))*cos(2*M_PI/(double)NUMBER_OF_WINDMILLS*n),0,(MILL_RADIUS + MILL_RADIUS/(n+1.0))*sin(2*M_PI/(double)NUMBER_OF_WINDMILLS*n),work[0]);
double scale = 10*MILL_SCALE/(n+10.0);
S(scale, scale, scale, work[1]);
Mult(work[0], work[1], w->windmillMDLMatrix[WINDMILL_BASE]);
T(0,0,0,w->windmillMDLMatrix[WALLS]);
T(0,0,0,w->windmillMDLMatrix[BALCONY]);
T(0,0,0,w->windmillMDLMatrix[ROOF]);
printError("init windmill");
}
void display(void)
{
printError("pre display");
// clear the screen (using chosen color earlier)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Set rotation matrix
phi += PI/50;
mat4 translation = T(0,0,-3);
mat4 rotations = Mult(Ry(phi),Mult(Rx(phi), Rz(phi)));
transformMatrix = Mult(translation,rotations);
// Send translMatrix to Vertex
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m); // Variate shader
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers(); // Swap buffer so that GPU can use the buffer we uploaded to it and we can write to another
}
void OnTimer(int value)
{
glutTimerFunc(20, &OnTimer, value);
old_t = t;
t = (GLfloat)glutGet(GLUT_ELAPSED_TIME)/1000.0;
delta_t = (t - old_t);
//printf("%f\n", delta_t);
GLfloat tmpppp[3] = {cow.pos.x, cow.pos.y, cow.pos.z};
glUniform3fv(glGetUniformLocation(g_shader, "cow_pos"), 1, tmpppp);
turn_cow(&cow, -m_angle);
update_cow(&cow, delta_t);
update_fence(&ff, &cow, delta_t);
move_cow(&cow, m_angle);
update_floor(&f, &cow);
update_wall(&wall, &cow, delta_t);
update_ball(&ball, &cow, delta_t);
update_farmer(&farmer);
if(keyIsDown('k'))
update_ragdoll(&ragdoll, delta_t);
if(check_collision_2(&cow.bb, &wall.bb))
{
//printf("yey\n");
glUniform1i(glGetUniformLocation(g_shader, "collision"), 1);
//cow.momentum = SetVector(-cow.momentum.x, cow.momentum.y, -cow.momentum.z);
//wall_collision(wall, cow);
}
else
glUniform1i(glGetUniformLocation(g_shader, "collision"), 0);
//printf("%f, %f\n", delta_t, old_t);
glUniform1f(glGetUniformLocation(g_shader, "time"), t);
mat4 proj_matrix = frustum(-1, 1, -1, 1, 1, 750.0);
mat4 cam_matrix = lookAt(cam_dist*cos(m_angle)+cow.pos.x, 9+cow.pos.y, cam_dist*sin(m_angle)+cow.pos.z, cow.pos.x, 8.5+cow.pos.y, cow.pos.z, 0.0, 1.0, 0.0);
//mat4 cam_matrix = lookAt(200+200*cos(cam_angle), 0, 200, 0, 8, 0, 0.0, 1.0, 0.0);
//g_shader = loadShaders("shader.vert" , "shader.frag");
//glUseProgram(g_shader);
glUniformMatrix4fv(glGetUniformLocation(g_shader, "proj_matrix"), 1, GL_TRUE, proj_matrix.m);
glUniformMatrix4fv(glGetUniformLocation(g_shader, "cam_matrix"), 1, GL_TRUE, cam_matrix.m);
//glutDisplayFunc(DisplayWindow);
//draw_cow(&cow, g_shader);
//draw_bone(&bone, g_shader);
mat4 tmp, testjoint, testjoint2;
testjoint = IdentityMatrix();
joint_s * j = &head_joint[0];
joint_s * jc = j->child[0];
joint_s * jcc = jc->child[0];
//joint_s * jp = j->parent;
//joint_s * jpp = jp->parent;
float Ms[8][4*4];
float legMs[8][4*4];
float currpos[8*3] = {0};
float bonepos[8*3] = {0};
float legcurrpos[8*3] = {0};
float legbonepos[8*3] = {0};
//GLfloat * Mtmp = Ms;
int i=0, ii=0;
//jc->R = ArbRotate(SetVector(0,0,1), cos(4*t/(i+1))/2.5);
//j->R = ArbRotate(SetVector(0,0,1), 0);
j->R = ArbRotate(SetVector(0,0,1), sin(4*t/(3+1))/1.2);
//jcc->R = ArbRotate(SetVector(0,0,1), cos(8*t/(2+1))/4);
mat4 Mpacc, Minvacc, tmptrans, tmppp, invtrans;
tmppp = IdentityMatrix();
float freq = 7;//Norm(cow.speed)/4;
if(Norm(cow.momentum) < .5)
freq = 0;
else if(Norm(cow.momentum) > 5)
freq = 20;
//printf("%f\n", freq);
j = &thigh_joint[0];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/1.5);
jc->R = ArbRotate(SetVector(0,0,1), cos(freq*t)/2.5);
j = &thigh_joint[1];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), -cos(freq*t)/2.5);
jc->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/2.5);
j = &thigh_joint[2];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/1.5);
jc->R = ArbRotate(SetVector(0,0,1), cos(freq*t)/2.5);
j = &thigh_joint[3];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), -cos(freq*t)/2.5);
jc->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/2.5);
j = &legbase_joint[0];
//j->R = ArbRotate(SetVector(1,0,0), -cos(7*t/(i+1)));
//j->R = ArbRotate(SetVector(1,0,0), -M_PI/6);
j = &thigh_joint[0];
//j->R = Mult(ArbRotate(SetVector(1,0,0), cos(7*t/(i+1))), j->R);
//j->R = Mult(ArbRotate(SetVector(1,0,0), M_PI/6), j->R);
i = 0;
tmppp = IdentityMatrix();
//legbase_joint[0].R = ArbRotate(SetVector(0,0,1), cos(3*t)/2);
//legs
calc_bone_transform(&legbase_joint[0], 0);
calc_bone_transform(&legbase_joint[1], 0);
calc_bone_transform(&legbase_joint[2], 0);
calc_bone_transform(&legbase_joint[3], 0);
j = &tail_joint[0];
j->R = ArbRotate(SetVector(0,0,1), -M_PI/2.5);
j = &tail_joint[1];
j->R = ArbRotate(SetVector(0,0,1), -M_PI/8.5);
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), 0);
j = &tail_joint[2];
//j->R = ArbRotate(SetVector(0,0,1), 0);
j = &tail_joint[3];
j->R = ArbRotate(SetVector(0,0,1), 0);
//body
j = &body_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), cos(t*7)/9.5);
j->R = ArbRotate(SetVector(0,1,0), m_angle + cow.angle);
j->R = Mult(j->R, ArbRotate(SetVector(0,0,1), cos(freq*t)/9));
calc_bone_transform(&body_joint[0],0);
//tail
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), -M_PI/2.5);
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//j = &tail_joint[2];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//j = &tail_joint[3];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//calc_bone_transform(&tail_joint[0],0);
//head
j = &head_joint[0];
j->R = ArbRotate(SetVector(0,0,1), sin(7*t)/9.5);
calc_bone_transform(&head_joint[0],0);
j = &farmer.skeleton.joints[2];
//j->R = Rx(cos(4*t));
j->R = Mult(Rx(M_PI/2.2 + sin(5*t)/11), Ry(cos(-5*t)/2));
j = &farmer.skeleton.joints[3];
//j->R = Rx(cos(4*t));
j->R = Mult(Rx(-M_PI/2.2 + sin(5*t)/11), Ry(cos(5*t)/2));
//left shoulder (her right)
//jc = &farmer.skeleton.joints[3];
//jc->R = Rx(-sin(3*t));
jc = &farmer.skeleton.joints[4];
jc->R = Ry(M_PI/3 + cos(5*t)/8);
jc = &farmer.skeleton.joints[5];
jc->R = Ry(-M_PI/3 + cos(5*t)/8);
jc = &farmer.skeleton.joints[10];
//jc->R = Rz(M_PI/3);
jc->R = Rz(.5-cos(5*t));
jc = &farmer.skeleton.joints[12];
jc->R = Rz((-1-cos(5*t))/2);
jc = &farmer.skeleton.joints[11];
//jc->R = Rz(M_PI/3);
jc->R = Rz(.5+cos(5*t));
jc = &farmer.skeleton.joints[13];
jc->R = Rz((-1-cos(5*t))/2);
calc_bone_transform(&farmer.skeleton.joints[0], 0);
calc_bone_transform(&farmer.skeleton.joints[2], 0);
calc_bone_transform(&farmer.skeleton.joints[3], 0);
//calc_bone_transform(&farmer.skeleton.joints[4], 0);
calc_bone_transform(&farmer.skeleton.joints[1], 0);
calc_bone_transform(&farmer.skeleton.joints[10], 0);
calc_bone_transform(&farmer.skeleton.joints[11], 0);
// calc_bone_transform(&farmer.skeleton.joints[8], 0);
// calc_bone_transform(&farmer.skeleton.joints[9], 0);
glutPostRedisplay();
}
// retourne l'objet en World Coordinates
C3DObject* C3DObject::getWorldObject(void) {
C3DObject *o = new C3DObject;
unsigned i,j;
float *farray;
// sinus & cosinus
float cox = cos(orientation.x);
float six = sin(orientation.x);
float coy = cos(orientation.y);
float siy = sin(orientation.y);
float coz = cos(orientation.z);
float siz = sin(orientation.z);
// --- Rotation Matrix ---
SMLXMatrix Rx(4,4);
// initialise une matrice identite
for(j=0;j<4;j++) for(i=0;i<4;i++) Rx[j][i]=(i==j?1.0f:.0f);
Rx[1][1] = cox;
Rx[1][2] =-six;
Rx[2][1] = six;
Rx[2][2] = cox;
SMLXMatrix Ry(4,4);
// initialise une matrice identite
for(j=0;j<4;j++) for(i=0;i<4;i++) Ry[j][i]=(i==j?1.0f:.0f);
Ry[0][0] = coy;
Ry[0][2] = siy;
Ry[2][0] =-siy;
Ry[2][2] = coy;
SMLXMatrix Rz(4,4);
// initialise une matrice identite
for(j=0;j<4;j++) for(i=0;i<4;i++) Rz[j][i]=(i==j?1.0f:.0f);
Rz[0][0] = coz;
Rz[0][1] =-siz;
Rz[1][0] = siz;
Rz[1][1] = coz;
// Matrice tenant compte des 3 rotations
SMLXMatrix R = Rz*Ry*Rx;
// --- Scale Matrix ---
SMLXMatrix S(4,4);
for(j=0;j<4;j++) for(i=0;i<4;i++) S[j][i]=(i==j?1.0f:.0f);
farray = size.data();
for (i=0; i<3; i++) S[i][i] = farray[i];
// --- Translation Matrix ---
SMLXMatrix T(4,4);
for(j=0;j<4;j++) for(i=0;i<4;i++) T[j][i]=(i==j?1.0f:.0f);
farray = origin.data();
for (i=0; i<3; i++) T[i][3] = farray[i];
SMLXMatrix M = T*R*S;
o->set(*this);
// converti les vertices
for (unsigned v=0; v<o->nb_verts; v++) {
SMLXMatrix vec(4,1);
vec[0][0] = o->vertices[v].x;
vec[1][0] = o->vertices[v].y;
vec[2][0] = o->vertices[v].z;
vec[3][0] = 1.0f;
SMLXMatrix nvec = M*vec;
o->vertices[v].x = nvec[0][0];
o->vertices[v].y = nvec[1][0];
o->vertices[v].z = nvec[2][0];
}
// converti les normales (rotation seulement)
for (unsigned f=0; f<o->nb_faces; f++) {
SMLXMatrix vec(4,1);
vec[0][0] = o->faces[f].norm.x;
vec[1][0] = o->faces[f].norm.y;
vec[2][0] = o->faces[f].norm.z;
vec[3][0] = 1.0f;
SMLXMatrix nvec = R*vec;
o->faces[f].norm.x = nvec[0][0];
o->faces[f].norm.y = nvec[1][0];
o->faces[f].norm.z = nvec[2][0];
for (v=0; v<3; v++) {
SMLXMatrix nml(4,1);
nml[0][0] = o->faces[f].pt_norm[v].x;
nml[1][0] = o->faces[f].pt_norm[v].y;
nml[2][0] = o->faces[f].pt_norm[v].z;
nml[3][0] = 1.0f;
SMLXMatrix nnml = R*nml;
o->faces[f].pt_norm[v].x = nnml[0][0];
o->faces[f].pt_norm[v].y = nnml[1][0];
o->faces[f].pt_norm[v].z = nnml[2][0];
}
}
o->compute_bbox();
return o;
}
void display(void)
{
if (glutKeyIsDown('a')) {
x=x-0.3;
}
if (glutKeyIsDown('d')) {
x=x+0.3;
}
if (glutKeyIsDown('w')) {
y=y+0.3;
}
if (glutKeyIsDown('s')) {
y=y-0.3;
}
if (glutKeyIsDown('l')) {
ry=ry-0.3;
}
if (glutKeyIsDown('j')) {
ry=ry+0.3;
}
if (glutKeyIsDown('i')) {
rx=rx-0.3;
}
if (glutKeyIsDown('k')) {
rx=rx+0.3;
}
a += 0.1;
mat4 rot, trans, trans2, transCam, transCam2, rot2, rotTot, totCam, total, bladeRotMat, windMillPos;
//clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
rot = Ry(ry/10);
transCam = T(0, 0, 16);
transCam2 = T(0,0,-16);
totCam = Mult(rot, transCam);
totCam = Mult(transCam2, totCam);
vec3 p = SetVector(x, y+8, 10);
vec4 s = vec3tovec4(p);
vec4 d = MultVec4(totCam, s);
vec3 q = vec4tovec3(d);
vec3 l = SetVector(0,8,-16);
vec3 v = SetVector(0,1,0);
camera = lookAtv(q,l,v);
glUniformMatrix4fv(glGetUniformLocation(program, "camMatrix"), 1, GL_TRUE, camera.m);
// walls
rot = Ry(0);
windMillPos = T(0,0,-16);
total = Mult(windMillPos, rot);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(walls, program,"in_Position" , "in_Normal", "inTexCoord");
// Blade rotation matrix
trans = T(0, -9, 16);
trans2 = T(0, 9, -16);
rot = Rz(a/6);
bladeRotMat = Mult(rot, trans);
bladeRotMat = Mult(trans2, bladeRotMat);
// Bladener
rot = Ry(1.57);
trans = T(0, 9, -11);
total = Mult(trans, rot);
total = Mult(bladeRotMat, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(blade, program,"in_Position" , "in_Normal", "inTexCoord");
// Bladupp
rot = Ry(1.57);
rot2 = Rz(3.14);
rotTot = Mult(rot2, rot);
trans = T(0, 9, -11);
total = Mult(trans, rotTot);
total = Mult(bladeRotMat, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(blade, program,"in_Position" , "in_Normal", "inTexCoord");
// BladeV
rot = Ry(1.57);
rot2 = Rz(1.57);
rotTot = Mult(rot2, rot);
trans = T(0, 9, -11);
total = Mult(trans, rotTot);
total = Mult(bladeRotMat, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(blade, program,"in_Position" , "in_Normal", "inTexCoord");
// BladeH
rot = Ry(1.57);
rot2 = Rz(-1.57);
rotTot = Mult(rot2, rot);
trans = T(0, 9, -11);
total = Mult(trans, rotTot);
total = Mult(bladeRotMat, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(blade, program,"in_Position" , "in_Normal", "inTexCoord");
//roof model
rot = Ry(0/6);
trans = T(0,0,-16);
total = Mult(trans, rot);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(roof, program,"in_Position" , "in_Normal", "inTexCoord");
printError("display");
glutSwapBuffers();
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
/// @brief Loads the lighting system for the supplied arena.
/// @param aid The ArenaID of the arena to load.
void SceneSetup::loadArenaLighting (ArenaID aid)
{
Ogre::Degree sunRotation; // rotation horizontally (yaw) from +x axis
Ogre::Degree sunPitch; // rotation downwards (pitch) from horizontal
float sunBrightness[4]; // RGBA
float sunSpecular[4]; // RGBA
float sunAmbience[4]; // RGBA
std::string skyBoxMap;
float sf; // scaling factor
// Set lighting constants
if (aid == COLOSSEUM_ARENA)
{
sunRotation = -170;
sunPitch = 60;
sunBrightness[0] = 233;
sunBrightness[1] = 225;
sunBrightness[2] = 201;
sunBrightness[3] = 700;
sunSpecular[0] = 233;
sunSpecular[1] = 225;
sunSpecular[2] = 201;
sunSpecular[3] = 400;
sunAmbience[0] = 241;
sunAmbience[1] = 228;
sunAmbience[2] = 190;
sunAmbience[3] = 500;
skyBoxMap = "arena1_skybox";
}
else if (aid == FOREST_ARENA)
{
sunRotation = 43;
sunPitch = 45;
sunBrightness[0] = 255;
sunBrightness[1] = 232;
sunBrightness[2] = 208;
sunBrightness[3] = 650;
sunSpecular[0] = 255;
sunSpecular[1] = 232;
sunSpecular[2] = 208;
sunSpecular[3] = 300;
sunAmbience[0] = 183;
sunAmbience[1] = 201;
sunAmbience[2] = 206;
sunAmbience[3] = 400;
skyBoxMap = "arena2_skybox";
}
else // Quarry
{
sunRotation = -55;
sunPitch = 35;
sunBrightness[0] = 255;
sunBrightness[1] = 255;
sunBrightness[2] = 249;
sunBrightness[3] = 550;
sunSpecular[0] = 255;
sunSpecular[1] = 255;
sunSpecular[2] = 255;
sunSpecular[3] = 250;
sunAmbience[0] = 172;
sunAmbience[1] = 196;
sunAmbience[2] = 204;
sunAmbience[3] = 200;
skyBoxMap = "arena3_skybox";
}
// Setup the lighting colours
sf = (1.0f / 255.0f) * (sunAmbience[3] / 1000.0f);
Ogre::ColourValue sunAmbienceColour = Ogre::ColourValue(sunAmbience[0] * sf, sunAmbience[1] * sf, sunAmbience[2] * sf);
sf = (1.0f / 255.0f) * (sunBrightness[3] / 1000.0f);
Ogre::ColourValue sunBrightnessColour = Ogre::ColourValue(sunBrightness[0] * sf, sunBrightness[1] * sf, sunBrightness[2] * sf);
sf = (1.0f / 255.0f) * (sunSpecular[3] / 1000.0f);
Ogre::ColourValue sunSpecularColour = Ogre::ColourValue(sunSpecular[0] * sf, sunSpecular[1] * sf, sunSpecular[2] * sf);
// Calculate the sun direction (using rotation matrices).
Ogre::Real cos_pitch = Ogre::Math::Cos(sunPitch);
Ogre::Real sin_pitch = Ogre::Math::Sin(sunPitch);
Ogre::Real cos_yaw = Ogre::Math::Cos(sunRotation);
Ogre::Real sin_yaw = Ogre::Math::Sin(sunRotation);
Ogre::Matrix3 Rz(cos_pitch, -sin_pitch, 0,
sin_pitch, cos_pitch, 0,
0, 0, 1);
Ogre::Matrix3 Ry( cos_yaw, 0, sin_yaw,
0, 1, 0,
-sin_yaw, 0, cos_yaw);
Ogre::Vector3 sunDirection = Ry * Rz * Ogre::Vector3(-1, 0, 0);
sunDirection.normalise();
// Set the ambient light.
GameCore::mSceneMgr->setAmbientLight(sunAmbienceColour);
// Add a directional light (for the sun).
mWorldSun->setDiffuseColour(sunBrightnessColour);
mWorldSun->setSpecularColour(sunSpecularColour);
mWorldSun->setDirection(sunDirection);
mWorldSun->setCastShadows(true);
// Create the skybox
GameCore::mSceneMgr->setSkyBox(true, skyBoxMap, 1000);
}
void StereoReconstructor::computeRTRandom(std::string cam1Folder, std::string cam2Folder) {
bool load1 = loadMatrixAndCoe(cam1Folder, camMatrix1, distCoeffs1);
bool load2 = loadMatrixAndCoe(cam2Folder, camMatrix2, distCoeffs2);
if (load1 == false || load2 == false) {
std::cout << "Load matrix and distortion failed!" << std::endl;
return;
}
std::vector<std::string> imgFiles1 = Utilities::folderImagesScan(cam1Folder.c_str());
std::vector<std::string> imgFiles2 = Utilities::folderImagesScan(cam2Folder.c_str());
cv::Mat img = cv::imread(cam1Folder + imgFiles1[0].c_str(), 1);
camImageSize = img.size();
int totalN = imgFiles1.size();
std::ofstream Tx("辅助_T_x.txt"); //x方向位移
std::ofstream Ty("辅助_T_y.txt"); //y方向位移
std::ofstream Tz("辅助_T_z.txt"); //z方向位移
std::ofstream Rx("辅助_R_x.txt"); //旋转轴x方向
std::ofstream Ry("辅助_R_y.txt"); //旋转轴y方向
std::ofstream Rz("辅助_R_z.txt"); //旋转轴z方向
std::ofstream Ra("辅助_R_a.txt"); //旋转角度
std::ofstream All("辅助_All.txt"); //所有数据
All << "Tx " << "Ty " << "Tz " << "T " << "Rx " << "Ry " << "Rz " << "Ra " << std::endl;
for (int k = 1; k <= totalN; k++) {
std::cout << k;
double tbegin = cv::getTickCount();
std::vector<int> nums;
while (nums.size() < k) {
srand(time(NULL));
int random = rand() % totalN;
if (nums.empty() || std::find(nums.begin(), nums.end(), random) == nums.end()) {
nums.push_back(random);
}
}
for (size_t i = 0; i < nums.size(); ++i) {
imgPoints1.push_back(load2DPoints(cam1Folder + imgFiles1[nums[i]].substr(0, imgFiles1[nums[i]].size() - 4) + "_imgCorners.txt"));
imgPoints2.push_back(load2DPoints(cam2Folder + imgFiles2[nums[i]].substr(0, imgFiles2[nums[i]].size() - 4) + "_imgCorners.txt"));
objPoints.push_back(load3DPoints(cam1Folder + "ObjCorners.txt"));
}
cv::Mat E, F;
cv::stereoCalibrate(objPoints, imgPoints1, imgPoints2, camMatrix1, distCoeffs1, camMatrix2, distCoeffs2, camImageSize, R, T, E, F,
cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_INTRINSIC);
//罗德里格斯（Rodrigues）变换
cv::Mat R2T(3, 1, CV_64F);
cv::Rodrigues(R, R2T);
float tx = Utilities::matGet2D(T, 0, 0);
float ty = Utilities::matGet2D(T, 0, 1);
float tz = Utilities::matGet2D(T, 0, 2);
float T = std::sqrt(tx*tx + ty*ty + tz*tz);
Tx << tx << std::endl;
Ty << ty << std::endl;
Tz << tz << std::endl;
double dx = Utilities::matGet2D(R2T, 0, 0);
double dy = Utilities::matGet2D(R2T, 0, 1);
double dz = Utilities::matGet2D(R2T, 0, 2);
double dl = std::sqrt(dx*dx + dy*dy + dz*dz);
float angle = dl * 180 / 3.1415926535897932;
float fdx = dx / dl;
float fdy = dy / dl;
float fdz = dz / dl;
Rx << fdx << std::endl;
Ry << fdy << std::endl;
Rz << fdz << std::endl;
Ra << angle << std::endl;
All << tx << " " << ty << " " << tz << " " << T << " " << fdx << " " << fdy << " " << fdz << " " << angle << std::endl;
nums.clear();
imgPoints1.clear();
imgPoints2.clear();
objPoints.clear();
double tend = cv::getTickCount();
double frequency = cv::getTickFrequency();
int span = (tend - tbegin) / frequency;
std::cout << "副图像,标定时间 " << span << ",基线长度" << T << std::endl;
}
Tx.close();
Ty.close();
Tz.close();
Rx.close();
Ry.close();
Rz.close();
Ra.close();
All.close();
}
//Helper function for rasterize with built-in rotation
location_3d<int> HalfEllipsoid::rotate_loc( location_3d<int> loc,
location_3d<int> cen, float angle )
{
//Set up a 2D rotation matrix about the z axix
Matrix_2D Rz(2,2);
Rz(1,1) = std::cos( angle );
Rz(1,2) = std::sin( angle );
Rz(2,1) = -Rz(1,2);
Rz(2,2) = Rz(1,1);
int ii, jj;
ii = GsTL::round( Rz(1,1)*(loc[0]-cen[0]) + Rz(1,2)*(loc[1]-cen[1]) + cen[0] );
jj = GsTL::round( Rz(2,1)*(loc[0]-cen[0]) + Rz(2,2)*(loc[1]-cen[1]) + cen[1] );
location_3d<int> rot_loc( ii, jj, loc[2] );
return rot_loc;
}