三维空间中的三角形旋转

//已知三角形的初始法向量、三个坐标点和终止法向量；求三角形保型旋转，及旋转后的坐标
//初始法向量叉乘终止法向量得三角形的旋转轴，同时可利用二向量求得旋转角度
//利用旋转轴和旋转角度计算其旋转向量，旋转向量的模长大小为旋转角度的弧度值

// ConsoleApplication2.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <cv.h>

//已知三角形的初始法向量、三个坐标点和终止法向量；求三角形保型旋转，及旋转后的坐标
//初始法向量叉乘终止法向量得三角形的旋转轴，同时可利用二向量求得旋转角度
//利用旋转轴和旋转角度计算其旋转向量，旋转向量的模长大小为旋转角度的弧度值
int _tmain(int argc, _TCHAR* argv[])
{
//vector<CTriangle> ScaTrinagle;//存放分散的三角形
int rotatezhou[3];//旋转轴
float rotatevect[3];
//ScaTrinagle.resize(m_nTriangles);
double angle = 3.14 / 2;//旋转角度

//利用旋转轴和旋转角度计算其旋转向量
rotatezhou[0] = 0;
rotatezhou[1] = 1;
rotatezhou[2] = 0;
rotatevect[0] = rotatezhou[0] * angle;
rotatevect[1] = rotatezhou[1] * angle;
rotatevect[2] = rotatezhou[2] * angle;

//从旋转向量求得旋转矩阵，
double R_matrix[9];
CvMat pr_vec = *(cvCreateMat(1, 3, CV_64F));
CvMat pR_matrix = *(cvCreateMat(3, 3, CV_64F));
CvMat pdst = *(cvCreateMat(1, 3, CV_64F));

/*cvInitMatHeader(&pr_vec, 1, 3, CV_64FC1, rotatevect, CV_AUTOSTEP);
cvInitMatHeader(&pR_matrix, 3, 3, CV_64FC1, R_matrix, CV_AUTOSTEP);
cvInitMatHeader(&pdst, 1, 3, CV_64FC1, temp, CV_AUTOSTEP);*/

CV_MAT_ELEM(pr_vec, double, 0, 0) = rotatevect[0];
CV_MAT_ELEM(pr_vec, double, 0, 1) = rotatevect[1];
CV_MAT_ELEM(pr_vec, double, 0, 2) = rotatevect[2];

cvRodrigues2(&pr_vec, &pR_matrix, 0);//从旋转向量求得旋转矩阵

for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
printf("%f ", CV_MAT_ELEM(pR_matrix, double, i, j));
}
printf("\n");
}
printf("----------------------------\n");
//三角形的各顶点乘以旋转矩阵得各点旋转后的坐标
double src[3];
double dst[3];

src[0] = 0;
src[1] = 0;
src[2] = 1;
cvInitMatHeader(&pr_vec, 1, 3, CV_64FC1, src, CV_AUTOSTEP);
cvMatMul(&pr_vec, &pR_matrix, &pdst, 1);

dst[0] = CV_MAT_ELEM(pdst, double, 0, 0);
dst[1] = CV_MAT_ELEM(pdst, double, 0, 1);
dst[2] = CV_MAT_ELEM(pdst, double, 0, 2);
printf("%f %f %f\n", dst[0], dst[1], dst[2]);

src[0] = 0;
src[1] = 2;
src[2] = 0;
cvInitMatHeader(&pr_vec, 1, 3, CV_64FC1, src, CV_AUTOSTEP);
cvMatMul(&pr_vec, &pR_matrix, &pdst, 1);
dst[0] = CV_MAT_ELEM(pdst, double, 0, 0);
dst[1] = CV_MAT_ELEM(pdst, double, 0, 1);
dst[2] = CV_MAT_ELEM(pdst, double, 0, 2);
printf("%f %f %f\n", dst[0], dst[1], dst[2]);

src[0] = 0;
src[1] = 0;
src[2] = 0;
cvInitMatHeader(&pr_vec, 1, 3, CV_64FC1, src, CV_AUTOSTEP);
cvMatMul(&pr_vec, &pR_matrix, &pdst, 1);
dst[0] = CV_MAT_ELEM(pdst, double, 0, 0);
dst[1] = CV_MAT_ELEM(pdst, double, 0, 1);
dst[2] = CV_MAT_ELEM(pdst, double, 0, 2);
printf("%f %f %f\n", dst[0], dst[1], dst[2]);

return 0;
}