三维空间中的三角形旋转

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

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