const mat44 cmat44(const mat33& m)
{
return mat44(m(0, 0), m(0, 1), m(0, 2), 0,
m(1, 0), m(1, 1), m(1, 2), 0,
m(2, 0), m(2, 1), m(2, 2), 0,
0 , 0 , 0 , 1);
}
mat44 mat44::flip_y()
{
return mat44(
1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
}
mat44 mat44::flip_x()
{
return mat44(
-1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
}
mat44 mat44::translate(float x, float y, float z)
{
return mat44(
-1, 0, 0, x,
0, 1, 0, y,
0, 0, 1, z,
0, 0, 0, 1
);
}
mat44 mat44::scale(float sx, float sy, float sz)
{
return mat44(
sx, 0, 0, 0,
0, sy, 0, 0,
0, 0, sz, 0,
0, 0, 0, 1
);
}
mat44 mat44::scale(float scale)
{
return mat44(
scale, 0, 0, 0,
0, scale, 0, 0,
0, 0, scale, 0,
0, 0, 0, 1
);
}
mat44 quaternion::to_mat44() const
{
return mat44(
1.0f-2.0f*(y*y + z*z), 2.0f*(x*y-w*z), 2.0f*(w*y+x*z), 0.0f,
2.0f*(x*y+w*z), 1.0f-2.0f*(x*x+z+z), 2.0f*(y*z-w*x), 0.0f,
2.0f*(x*z-w*y), 2.0f*(y*z+w*x), 1.0f-2.0f*(x*x+y*y), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
}
const mat44 mat44_FromColMajor(
const real data0, const real data1, const real data2, const real data3,
const real data4, const real data5, const real data6, const real data7,
const real data8, const real data9, const real data10, const real data11,
const real data12, const real data13, const real data14, const real data15)
{
return mat44(data0, data4, data8, data12,
data1, data5, data9, data13,
data2, data6, data10, data14,
data3, data7, data11, data15);
}
mat44 mat44::rotate_z(float radian)
{
float cosr = cosf(radian);
float sinr = sinf(radian);
return mat44(
cosr, -sinr, 0, 0,
sinr, cosr, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
}
//--------------------------------------------------------------
void ofApp::draw(){
if (path_est.size() > 0)
{
cam.begin();
ofPushStyle();
/// Render points as 3D cubes
for (size_t i = 0; i < point_cloud_est.size(); ++i)
{
cv::Vec3d point = point_cloud_est[i];
cv::Affine3d point_pose(cv::Mat::eye(3, 3, CV_64F), point);
char buffer[50];
sprintf(buffer, "%d", static_cast<int>(i));
ofBoxPrimitive box;
ofSetLineWidth(2.0f);
ofSetColor(ofColor::blue);
box.set(0.1, 0.1, -0.1);
box.setPosition(point[0], point[1], point[2]);
box.drawWireframe();
}
ofPopStyle();
cam.end();
cv::Affine3d cam_pose = path_est[idx];
cv::Matx44d mat44 = cam_pose.matrix;
ofMatrix4x4 m44(mat44(0, 0), mat44(1, 0), mat44(2, 0), mat44(3, 0),
mat44(0, 1), mat44(1, 1), mat44(2, 1), mat44(3, 1),
mat44(0, 2), mat44(1, 2), mat44(2, 2), mat44(3, 2),
mat44(0, 3), mat44(1, 3), mat44(2, 3), mat44(3, 3));
if (camera_pov) {
cam.setPosition(m44.getTranslation());
cam.lookAt(ofVec3f(0,0,1)*m44, ofVec3f(mat44(1,0),mat44(1,1), mat44(1,2)));
}
else
{
std::vector<ofPoint> path;
for (int i = 0; i < path_est.size()-1; i++) {
cv::Vec3d point = path_est[i].translation();
path.push_back(ofPoint(point[0], point[1], point[2]));
}
ofPolyline trajectory(path);
// render complete trajectory
cam.begin();
ofSetColor(ofColor::green);
trajectory.draw();
ofSetColor(ofColor::yellow);
ofPushMatrix();
ofMultMatrix(m44);
ofDrawAxis(0.25);
drawFrustum(f, cx, cy, 0.025, 0.4);
ofPopMatrix();
cam.end();
}
// update trajectory index (spring effect)
forw *= (idx == n-1 || idx == 0) ? -1 : 1; idx += forw;
}
}
void RenderScene() {
std::cout << "entered renderscene" << std::endl;
glClear(GL_COLOR_BUFFER_BIT);
Mesh mesh;
mesh.LoadMesh("/home/feiyu/devTools/ogldev-source/Content/jeep.obj");
//transform
static float Scale = 0.0f;
Matrix44f mat44;
Scale += 0.001f;
mat44(0, 0) = cosf(Scale);
mat44(0, 1) = 0;
mat44(0, 2) = -sinf(Scale);
mat44(0, 3) = 0;
mat44(1, 0) = 0;
mat44(1, 1) = 1;
mat44(1, 2) = 0;
mat44(1, 3) = 0;
mat44(2, 0) = sinf(Scale);
mat44(2, 1) = 0;
mat44(2, 2) = cosf(Scale);
mat44(2, 3) = 0;
mat44(3, 0) = 0;
mat44(3, 1) = 0;
mat44(3, 2) = 0;
mat44(3, 3) = 1;
glUniformMatrix4fv(gWorldLocation, 1, GL_TRUE, &mat44(0, 0));
//end transform
mesh.Render();
glutSwapBuffers();
glutPostRedisplay();
std::cout << "left renderscene" << std::endl;
/*glClear(GL_COLOR_BUFFER_BIT);
static float Scale = 0.0f;
Matrix44f mat44;
Scale += 0.001f;
mat44(0,0)=cosf(Scale);
mat44(0,1)=0;
mat44(0,2)=-sinf(Scale);
mat44(0,3)=0;
mat44(1,0)=0;
mat44(1,1)=1;
mat44(1,2)=0;
mat44(1,3)=0;
mat44(2,0)=sinf(Scale);
mat44(2,1)=0;
mat44(2,2)=cosf(Scale);
mat44(2,3)=0;
mat44(3,0)=0;
mat44(3,1)=0;
mat44(3,2)=0;
mat44(3,3)=1;
glUniformMatrix4fv(gWorldLocation, 1, GL_TRUE, &mat44(0,0));
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, VBO);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, IBO);
glDrawElements(GL_TRIANGLES, 12, GL_UNSIGNED_INT, 0);
glDisableVertexAttribArray(0);
glutSwapBuffers();*/
}