已知旋转矩阵求角度_法向量、旋转矩阵计算（五十一）

1、由空间三个点确定所在平面的法向量

int Cal_norm(const double Point_sta[], const double Point_mid[], const double Point_end[], Point3f &Point_norm)//计算法向量{
      double s_x;//起始点笛卡尔坐标  double s_y;  double s_z;  double m_x;//中间点笛卡尔坐标  double m_y;  double m_z;  double e_x;//结束点笛卡尔坐标  double e_y;  double e_z;  //计算单位法向量(SE×EM)  double norm;  double vec_x;  double vec_y;  double vec_z;  //定义新坐标系单位法向量  double n_x;  double n_y;  double n_z;//计算圆心坐标  //计算当前节点在基坐标系下  s_x = Point_sta[0];  s_y = Point_sta[1];  s_z = Point_sta[2];  m_x = Point_mid[0];  m_y = Point_mid[1];  m_z = Point_mid[2];  e_x = Point_end[0];  e_y = Point_end[1];  e_z = Point_end[2];  vec_x = (e_y - s_y) * (m_z - e_z) - (m_y - e_y) * (e_z - s_z);  vec_y = (m_x - e_x) * (e_z - s_z) - (e_x - s_x) * (m_z - e_z);  vec_z = (e_x - s_x) * (m_y - e_y) - (m_x - e_x) * (e_y - s_y);  norm = sqrt(vec_x * vec_x + vec_y * vec_y + vec_z * vec_z);  //若三点共线则出错  if (norm < ERR_PRECISION) return -1;  //定义新坐标系单位法向量  n_x = vec_x / norm;  n_y = vec_y / norm;  n_z = vec_z / norm;  Point_norm.x= n_x;  std::cout << Point_norm.x << std::endl;  Point_norm.y = n_y;  cout << Point_norm.y << endl;  Point_norm.z = n_z;  cout << Point_norm.z << endl;  cout << Point_norm << endl;}

2、计算两个向量之间的旋转矩阵：

为了更好地推导，我们需要加入三个轴对齐的单位向量i，j，k。

i，j，k满足以下特点：

i=jxk；j=kxi；k=ixj；

kxj=–i；ixk=–j；jxi=–k；

ixi=jxj=kxk=0；(0是指0向量)

由此可知，i，j，k是三个相互垂直的向量。它们刚好可以构成一个坐标系。

这三个向量的特例就是i=(1，0，0)j=(0，1，0)k=(0，0，1)。

对于处于i，j，k构成的坐标系中的向量u，v我们可以如下表示：

u=Xui+Yuj+Zuk；

v=Xvi+Yvj+Zvk；

uxv=(Xui+Yuj+Zuk)x(Xvi+Yvj+Zvk)

=XuXv(ixi)+XuYv(ixj)+XuZv(ixk)+YuXv(jxi)+Y