//-------------------------------------------------------------------------
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();
}
