267 lines
6.8 KiB
C++
267 lines
6.8 KiB
C++
#include "quaternion.h"
|
||
#include "math.h"
|
||
#include "stdio.h"
|
||
|
||
#define PI 3.141592f
|
||
|
||
|
||
/*给定一个绕单位向量（x,y,z）绕此单位向量旋转angle角度，创建四元数*/
|
||
Quaternion CreateQuaternion(float angle,Vector3 v)
|
||
{
|
||
Quaternion q;
|
||
/*把angle转换为弧度*/
|
||
angle = PI*angle/(180.0f);
|
||
q.w = cos(angle/2.0f);
|
||
q.x = v.x * sin(angle/2);
|
||
q.y = v.y * sin(angle/2);
|
||
q.z = v.z * sin(angle/2);
|
||
return q;
|
||
}
|
||
/*计算两个四元数相乘，返回相乘的结果*/
|
||
Quaternion QuaternionMulti(Quaternion q1,_Quaternion q2)
|
||
{
|
||
Quaternion q;
|
||
float mod;
|
||
/*按照四元数乘法计算系数*/
|
||
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; //按照3D数学基础：图形与游戏开发 中四元数修改的定义
|
||
q.x = q1.w * q2.x + q1.x * q2.w - q1.y * q2.z + q1.z * q2.y; //按照3D数学基础：图形与游戏开发 中四元数修改的定义
|
||
q.y = q1.w * q2.y + q1.y * q2.w + q1.x * q2.z - q1.z * q2.x; //按照3D数学基础：图形与游戏开发 中四元数修改的定义
|
||
q.z = q1.w * q2.z + q1.z * q2.w + q1.y * q2.x - q1.x * q2.y; //按照3D数学基础：图形与游戏开发 中四元数修改的定义
|
||
|
||
// //按照四元数乘法标准计算法，实际不用
|
||
//q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
|
||
//q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
|
||
//q.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
|
||
//q.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
|
||
|
||
|
||
mod = QuaternionMod(q);
|
||
q.w = q.w/mod; q.x = q.x/mod; q.y = q.y/mod; q.z = q.z/mod;
|
||
return q;
|
||
}
|
||
|
||
/*给定四元数求该四元数的共轭四元数*/
|
||
Quaternion QuaternionConjugate(Quaternion q1)
|
||
{
|
||
Quaternion q;
|
||
/*按照四元数乘法计算系数*/
|
||
q.w = q1.w;
|
||
q.x = -q1.x;
|
||
q.y = -q1.y;
|
||
q.z = -q1.z;
|
||
return q;
|
||
}
|
||
/*给定四元数求该四元数的模*/
|
||
float QuaternionMod(Quaternion q1)
|
||
{
|
||
float mod;
|
||
/*按照四元数乘法计算系数*/
|
||
mod = q1.w * q1.w + q1.x * q1.x +
|
||
q1.y * q1.y + q1.z * q1.z;
|
||
mod = sqrt(mod);
|
||
return mod;
|
||
}
|
||
|
||
/*给定四元数求该四元数的逆*/
|
||
Quaternion QuaternionInversion(Quaternion q1)
|
||
{
|
||
Quaternion q;
|
||
/*计算四元数模的平方*/
|
||
float mod2 = QuaternionMod(q1);
|
||
/* 按照公式： *q^-1 = (q*)/(|q|)^2 */
|
||
q = QuaternionConjugate(q1);
|
||
q.w = q.w / mod2;
|
||
q.x = q.x / mod2;
|
||
q.y = q.y / mod2;
|
||
q.z = q.z / mod2;
|
||
return q;
|
||
}
|
||
|
||
void DisplayQuaternion(Quaternion q)
|
||
{
|
||
float q0,q1,q2,q3;
|
||
q0 = q.w; q1 = q.x; q2 = q.y; q3 = q.z;
|
||
printf("DisplayQuaternion ::w = %f x = %f y = %f z = %f\n",q.w,q.x,q.y,q.z);
|
||
}
|
||
|
||
/*欧拉角转四元数*/
|
||
Quaternion EulerAnglesToQuaternion(EulerAngles e)
|
||
{
|
||
Quaternion q;
|
||
e.Pitch = e.Pitch/(180.00f/3.141592f);
|
||
e.Yaw = e.Yaw/ (180.00f/3.141592f);
|
||
e.Roll = e.Roll/ (180.00f/3.141592f);
|
||
// 按照Qz*Qy*Qx 产生四元数
|
||
/*w*/
|
||
q.w = (float)(cos(0.5*e.Roll)*cos(0.5*e.Pitch)*cos(0.5*e.Yaw) - sin(0.5*e.Roll)*sin(0.5*e.Pitch)*sin(0.5*e.Yaw));
|
||
/*x 绕x轴旋转是pitch*/
|
||
q.x = (float)(cos(0.5*e.Roll)*sin(0.5*e.Pitch)*cos(0.5*e.Yaw) - sin(0.5*e.Roll)*cos(0.5*e.Pitch)*sin(0.5*e.Yaw));
|
||
/*y 绕y轴旋转是roll*/
|
||
q.y = (float)(sin(0.5*e.Roll)*cos(0.5*e.Pitch)*cos(0.5*e.Yaw) + cos(0.5*e.Roll)*sin(0.5*e.Pitch)*sin(0.5*e.Yaw));
|
||
/*z 绕z轴旋转是Yaw*/
|
||
q.z = (float)(cos(0.5*e.Roll)*cos(0.5*e.Pitch)*sin(0.5*e.Yaw) + sin(0.5*e.Roll)*sin(0.5*e.Pitch)*cos(0.5*e.Yaw));
|
||
return q;
|
||
}
|
||
|
||
EulerAngles QuaternionToEulerAngles(Quaternion q,bool useRefer)
|
||
{
|
||
EulerAngles e;
|
||
/*Pitch 在D3D坐标系中被认为是绕x轴旋转，Roll绕z旋转，Yaw是绕y旋转*/
|
||
float w,x,y,z;
|
||
w = q.w; x = q.x; y = q.y; z = q.z;
|
||
|
||
e.Pitch = asin(-2.0f*(y*z-w*x))* (180.0f /PI);
|
||
e.Yaw = atan2(x*z + w*y,0.5f - x*x - y*y)* (180.0f /PI);
|
||
e.Roll = atan2(x*y + w*z,0.5f - x*x - z*z)* (180.0f /PI);
|
||
|
||
return e;
|
||
}
|
||
|
||
|
||
void DisplayEulerAngles(EulerAngles e)
|
||
{
|
||
printf("DisplayEulerAngles::Yaw=%f,Pitch=%f,Roll=%f\n",e.Yaw,e.Pitch,e.Roll);
|
||
}
|
||
|
||
void SetEulerAngles(EulerAngles *e,float Yaw,float Pitch,float Roll)
|
||
{
|
||
(*e).Pitch = Pitch; (*e).Roll = Roll;(*e).Yaw = Yaw;
|
||
}
|
||
|
||
|
||
EulerAngles SetEulerAngles(float Yaw,float Pitch,float Roll)
|
||
{
|
||
EulerAngles e;
|
||
(e).Pitch = Pitch; (e).Roll = Roll;(e).Yaw = Yaw;
|
||
return e;
|
||
}
|
||
|
||
Vector3 SetVector3(float x,float y,float z)
|
||
{
|
||
Vector3 v;
|
||
v.x = x;
|
||
v.y = y;
|
||
v.z = z;
|
||
return v;
|
||
}
|
||
|
||
|
||
bool QuaternionEqual(Quaternion q1,Quaternion q2)
|
||
{
|
||
if(fabsf(q1.w-q2.w)<0.0001f&&
|
||
fabsf(q1.x-q2.x)<0.0001f&&
|
||
fabsf(q1.y-q2.y)<0.0001f&&
|
||
fabsf(q1.z-q2.z)<0.0001f)return true;
|
||
return false;
|
||
}
|
||
|
||
Quaternion CreateQuaternionByGyro(float gx,float gy,float gz)
|
||
{
|
||
float qw,qx,qy,qz;
|
||
static float q0 = 1.0f,q1 = 0.0f,q2 = 0.0f,q3 = 0.0f;
|
||
float halfT = 1.0f;
|
||
|
||
Quaternion q;
|
||
qw = (-q1*gx - q2*gy - q3*gz)*halfT; /*利用一阶龙格库塔法对四元数进行更新*/
|
||
qx = ( q0*gx + q2*gz - q3*gy)*halfT;
|
||
qy = ( q0*gy - q1*gz + q3*gx)*halfT;
|
||
qz = ( q0*gz + q1*gy - q2*gx)*halfT;
|
||
|
||
q0 = q0 + qw;
|
||
q1 = q1 + qx;
|
||
q2 = q2 + qy;
|
||
q3 = q3 + qz;
|
||
|
||
q.w = q0;
|
||
q.x = q1;
|
||
q.y = q2;
|
||
q.z = q3;
|
||
|
||
|
||
float mod2 = QuaternionMod(q); /*求模*/
|
||
|
||
q.w = q.w / mod2;
|
||
q.x = q.x / mod2;
|
||
q.y = q.y / mod2;
|
||
q.z = q.z / mod2;
|
||
|
||
return q;
|
||
}
|
||
|
||
|
||
/*把四元数转换为旋转矩阵*/
|
||
Matrix3 QuaternionToMatrix(Quaternion q)
|
||
{
|
||
Matrix3 mMatrix;
|
||
float w,x,y,z;
|
||
w = q.w; x = q.x; y = q.y; z = q.z;
|
||
|
||
mMatrix.m[0][0] = 1 - 2*y*y -2*z*z;
|
||
mMatrix.m[0][1] = 2*x*y + 2*w*z;
|
||
mMatrix.m[0][2] = 2*x*y - 2*w*y;
|
||
|
||
mMatrix.m[1][0] = 2*x*y - 2*w*z;
|
||
mMatrix.m[1][1] = 1 - 2*x*x -2*z*z;
|
||
mMatrix.m[1][2] = 2*y*z + 2*w*x;
|
||
|
||
mMatrix.m[2][0] = 2*x*z + 2*w*z;
|
||
mMatrix.m[2][1] = 2*y*z - 2*w*x;
|
||
mMatrix.m[2][2] = 1 - 2*x*x -2*y*y;
|
||
return mMatrix;
|
||
}
|
||
|
||
/* 给定旋转矩阵以及向量，返回旋转后的向量*/
|
||
MVector3 ComputeRotate(Matrix3 mMatrix,MVector3 vec)
|
||
{
|
||
MVector3 mResult;
|
||
int j;
|
||
for(j=0;j<3;j++)
|
||
{
|
||
mResult.v[j] = mMatrix.m[j][0]*vec.v[0] +
|
||
mMatrix.m[j][1]*vec.v[1] +
|
||
mMatrix.m[j][2]*vec.v[2];
|
||
}
|
||
return mResult;
|
||
}
|
||
|
||
|
||
|
||
|
||
/*************************************************************************
|
||
|
||
* 描 述 ： 把标准笛卡尔坐标系下的四元数转换到欧拉角显示。
|
||
这里的变换公式为物体-惯性四元数转换到欧拉角
|
||
|
||
**************************************************************************/
|
||
EulerAngles MathQuaternionToEulerAngles(Quaternion q)
|
||
{
|
||
EulerAngles e;
|
||
/*标准笛卡尔坐标系中，我们认为绕z轴为Yaw，绕x轴为Roll，绕y轴为Pitch*/
|
||
float w,x,y,z;
|
||
w = q.w; x = q.x; y = q.y; z = q.z;
|
||
|
||
e.Pitch = asin(-2.0f*(z*x-w*y))* (180.0f /PI);
|
||
e.Yaw = atan2(y*x + w*z,0.5f - y*y - z*z)* (180.0f /PI);
|
||
e.Roll = atan2(y*z + w*x,0.5f - y*y - x*x)* (180.0f /PI);
|
||
|
||
return e;
|
||
}
|
||
/*************************************************************************
|
||
|
||
* 描 述 ： 把D3D坐标系下的四元数转换到欧拉角显示。
|
||
这里的变换公式为物体-惯性四元数转换到欧拉角
|
||
|
||
**************************************************************************/
|
||
EulerAngles D3DQuaternionToEulerAngles(Quaternion q)
|
||
{
|
||
EulerAngles e;
|
||
/*Pitch 在D3D坐标系中被认为是绕x轴旋转，Roll绕z旋转，Yaw是绕y旋转*/
|
||
float w,x,y,z;
|
||
w = q.w; x = q.x; y = q.y; z = q.z;
|
||
|
||
e.Pitch = asin(-2.0f*(y*z-w*x))* (180.0f /PI);
|
||
e.Yaw = atan2(x*z + w*y,0.5f - x*x - y*y)* (180.0f /PI);
|
||
e.Roll = atan2(x*y + w*z,0.5f - x*x - z*z)* (180.0f /PI);
|
||
|
||
return e;
|
||
} |