三維空間中的三角形旋轉

//已知三角形的初始法向量、三個座標點和終止法向量；求三角形保型旋轉，及旋轉後的座標
//初始法向量叉乘終止法向量得三角形的旋轉軸，同時可利用二向量求得旋轉角度
//利用旋轉軸和旋轉角度計算其旋轉向量，旋轉向量的模長大小爲旋轉角度的弧度值

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