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