//------------------------------------------------------------------------- void FKCW_3D_Node::rotate3D(const FKCW_3D_Vector4& axis,float cosA,float sinA) { // axis必须是个单元向量 assert(axis.w()==0); assert(getLength2(axis)==1); FKCW_3D_Matrix4 RMat=calculateRotationMatrix(axis, cosA, sinA); m_mat=m_mat*RMat; };
//------------------------------------------------------------------------- void FKCW_3D_Node::setRotation3D(const FKCW_3D_Vector4&axis,float cosA,float sinA) { assert(axis.w()==0); FKCW_3D_Matrix4 RMat=calculateRotationMatrix(axis, cosA, sinA); //replace left up 3x3 sub matrix of m_mat with RMat const int indexs[9]={0,1,2,4,5,6,8,9,10}; for(int i=0;i<9;i++){ int index=indexs[i]; m_mat.setAt(index, RMat.getAt(index)); } }
/** * Projects a point in real world coordinates against the image * Output: image coordinate in pixels */ CvPoint MultipleViewGeomOld::getProjectionOf(float angle, CvPoint3D32f point) { // map<float, CvMat*>::iterator iter = projMatList.find(angle); CvMat *projMat = cvCreateMat(4, 3, CV_32FC1); if (iter == projMatList.end()) { // project matrix does not exist!! // Calculate rotation matrix CvMat* rtMat = calculateRotationMatrix(angle); // Calculate projection matrix projMat = calculateProjectionMatrix(rtMat); projMat = cvCloneMat(projMat); projMatList.insert(pair<float, CvMat*> (angle, projMat)); } else { // otherwise it exists projMat = iter->second; } LOG4CPLUS_DEBUG(myLogger,"Projection matrix for angle: " << radToDegree(angle) << " and points: " << point << endl << printCvMat(projMat)); // [u v 1] = proj * [X Y Z 1] float uvContents[3]; //CvMat* uvMat = cvMat(3, 1, CV_32F, uvContents); CvMat* uvMat = cvCreateMat(3, 1, CV_32F); cvInitMatHeader(uvMat, 3, 1, CV_32F, uvContents); float xyzContents[] = { point.x, point.y, point.z, 1 }; //CvMat* xyzMat = cvMat(4, 1, CV_32F, xyzContents); CvMat* xyzMat = cvCreateMat(4, 1, CV_32F); cvInitMatHeader(xyzMat, 4, 1, CV_32F, xyzContents); cvMatMul (projMat, xyzMat,uvMat); LOG4CPLUS_DEBUG(myLogger, "Result [u v 1] = proj * [X Y Z 1]: " << endl << printCvMat(uvMat)); return cvPoint(cvRound(cvmGet(uvMat, 0, 0)), cvRound(cvmGet(uvMat, 1, 0))); }
void Cc3dTransform::rotate(float nx,float ny,float nz,float cosa,float sina) //n must be normalized vector { Cc3dMatrix4 rotmat=calculateRotationMatrix(Cc3dVector4(nx,ny,nz,0),cosa,sina); //calculateRotationMatrix(nx, ny, nz, cosa, sina); m_RTmat=m_RTmat*rotmat; }
void RigidBody::calculateDerivedData() { calculateRotationMatrix(); }