logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/4.Software/GeekIMU Manager GUI 1.2/GeekIMUDriver 1.2/Src/quaternion.cpp

266 lines
6.8 KiB
C++
Raw Normal View History

上传GeekIMU资料
2024-11-09 21:39:20 +08:00
#include "quaternion.h"
#include "math.h"
#include "stdio.h"
#define PI 3.141592f
/*<2A><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>Ƶ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x,y,z<><7A><EFBFBD>ƴ˵<C6B4>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תangle<6C>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>*/
Quaternion CreateQuaternion(float angle,Vector3 v)
{
Quaternion q;
/*<2A><>angleת<65><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>*/
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;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD><CBA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵Ľ<CBB5><C4BD><EFBFBD>*/
Quaternion QuaternionMulti(Quaternion q1,_Quaternion q2)
{
Quaternion q;
float mod;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>*/
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; //<2F><><EFBFBD><EFBFBD>3D<33><44>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>޸ĵĶ<C4B5><C4B6><EFBFBD>
q.x = q1.w * q2.x + q1.x * q2.w - q1.y * q2.z + q1.z * q2.y; //<2F><><EFBFBD><EFBFBD>3D<33><44>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>޸ĵĶ<C4B5><C4B6><EFBFBD>
q.y = q1.w * q2.y + q1.y * q2.w + q1.x * q2.z - q1.z * q2.x; //<2F><><EFBFBD><EFBFBD>3D<33><44>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>޸ĵĶ<C4B5><C4B6><EFBFBD>
q.z = q1.w * q2.z + q1.z * q2.w + q1.y * q2.x - q1.x * q2.y; //<2F><><EFBFBD><EFBFBD>3D<33><44>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>޸ĵĶ<C4B5><C4B6><EFBFBD>
// //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>˷<EFBFBD><CBB7><EFBFBD>׼<EFBFBD><D7BC><EFBFBD>㷨<EFBFBD><E3B7A8>ʵ<EFBFBD>ʲ<EFBFBD><CAB2><EFBFBD>
//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;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>*/
Quaternion QuaternionConjugate(Quaternion q1)
{
Quaternion q;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>*/
q.w = q1.w;
q.x = -q1.x;
q.y = -q1.y;
q.z = -q1.z;
return q;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ģ*/
float QuaternionMod(Quaternion q1)
{
float mod;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>*/
mod = q1.w * q1.w + q1.x * q1.x +
q1.y * q1.y + q1.z * q1.z;
mod = sqrt(mod);
return mod;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
Quaternion QuaternionInversion(Quaternion q1)
{
Quaternion q;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ģ<EFBFBD><C4A3>ƽ<EFBFBD><C6BD>*/
float mod2 = QuaternionMod(q1);
/* <20><><EFBFBD>չ<EFBFBD>ʽ<EFBFBD><CABD> *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);
}
/*ŷ<><C5B7><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ԫ<EFBFBD><D4AA>*/
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);
// <20><><EFBFBD><EFBFBD>Qz*Qy*Qx <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
/*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 <20><>x<EFBFBD><78><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>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 <20><>y<EFBFBD><79><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>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 <20><>z<EFBFBD><7A><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>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 <20><>D3D<33><44><EFBFBD><EFBFBD>ϵ<EFBFBD>б<EFBFBD><D0B1><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>x<EFBFBD><78><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Roll<6C><6C>z<EFBFBD><7A>ת<EFBFBD><D7AA>Yaw<61><77><EFBFBD><EFBFBD>y<EFBFBD><79>ת*/
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; /*<2A><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD>*/
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); /*<2A><>ģ*/
q.w = q.w / mod2;
q.x = q.x / mod2;
q.y = q.y / mod2;
q.z = q.z / mod2;
return q;
}
/*<2A><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>*/
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;
}
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
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;
}
/*************************************************************************
* <EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD>ѱ<EFBFBD>׼<EFBFBD>ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı任<EFBFBD><EFBFBD>ʽΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
EulerAngles MathQuaternionToEulerAngles(Quaternion q)
{
EulerAngles e;
/*<2A><>׼<EFBFBD>ѿ<EFBFBD><D1BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>z<EFBFBD><7A>ΪYaw<61><77><EFBFBD><EFBFBD>x<EFBFBD><78>ΪRoll<6C><6C><EFBFBD><EFBFBD>y<EFBFBD><79>Ϊ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;
}
/*************************************************************************
* <EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD>D3D<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı任<EFBFBD><EFBFBD>ʽΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
EulerAngles D3DQuaternionToEulerAngles(Quaternion q)
{
EulerAngles e;
/*Pitch <20><>D3D<33><44><EFBFBD><EFBFBD>ϵ<EFBFBD>б<EFBFBD><D0B1><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>x<EFBFBD><78><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Roll<6C><6C>z<EFBFBD><7A>ת<EFBFBD><D7AA>Yaw<61><77><EFBFBD><EFBFBD>y<EFBFBD><79>ת*/
double 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;
}