145 lines
2.8 KiB
C
145 lines
2.8 KiB
C
#ifndef _QUATERNION_H_
|
||
#define _QUATERNION_H_
|
||
|
||
typedef struct _MagCaliParam
|
||
{
|
||
float xOffset;
|
||
float yOffset;
|
||
float zOffset;
|
||
float xScale;
|
||
float yScale;
|
||
float zScale;
|
||
}MagCaliParam;
|
||
|
||
typedef struct _Quaternion
|
||
{
|
||
float w,x,y,z;
|
||
}Quaternion;
|
||
|
||
|
||
typedef struct _Vector3
|
||
{
|
||
float x,y,z;
|
||
}Vector3;
|
||
|
||
typedef struct _EulerAngles{
|
||
float Yaw;
|
||
float Roll;
|
||
float Pitch;
|
||
}EulerAngles;
|
||
|
||
typedef struct _MVector3
|
||
{
|
||
float v[3];
|
||
}MVector3;
|
||
|
||
|
||
typedef struct _Matrix3
|
||
{
|
||
float m[3][3];
|
||
}Matrix3;
|
||
|
||
|
||
|
||
#ifdef __cplusplus
|
||
extern "C"{
|
||
|
||
/*给定一个绕单位向量（x,y,z）绕此单位向量旋转angle角度，创建四元数*/
|
||
Quaternion CreateQuaternion(float angle,Vector3 v);
|
||
|
||
/*计算两个四元数相乘，返回相乘的结果*/
|
||
Quaternion QuaternionMulti(Quaternion q1,Quaternion q2);
|
||
|
||
/*给定四元数求该四元数的共轭四元数*/
|
||
Quaternion QuaternionConjugate(Quaternion q1);
|
||
|
||
/*给定四元数求该四元数的模*/
|
||
float QuaternionMod(Quaternion q1);
|
||
|
||
/*给定四元数求该四元数的逆*/
|
||
Quaternion QuaternionInversion(Quaternion q1);
|
||
|
||
void DisplayQuaternion(Quaternion q);
|
||
|
||
Quaternion EulerAnglesToQuaternion(EulerAngles e);
|
||
|
||
//EulerAngles QuaternionToEulerAngles(Quaternion q,bool useRefer);
|
||
|
||
void DisplayEulerAngles(EulerAngles e);
|
||
|
||
//void SetEulerAngles(EulerAngles *e,float Yaw,float Pitch,float Roll);
|
||
|
||
EulerAngles SetEulerAngles(float Yaw,float Pitch,float Roll);
|
||
|
||
/*判断两个四元数是否相等，相等返回ture，否则返回false*/
|
||
bool QuaternionEqual(Quaternion q1,Quaternion q2);
|
||
|
||
|
||
Vector3 SetVector3(float x,float y,float z);
|
||
|
||
Quaternion CreateQuaternionByGyro(float gx,float gy,float gz);
|
||
|
||
|
||
Matrix3 QuaternionToMatrix(Quaternion q);
|
||
|
||
|
||
MVector3 ComputeRotate(Matrix3 mMatrix,MVector3 vec);
|
||
|
||
EulerAngles MathQuaternionToEulerAngles(Quaternion q);
|
||
|
||
|
||
|
||
}
|
||
|
||
#else
|
||
|
||
/*给定一个绕单位向量（x,y,z）绕此单位向量旋转angle角度，创建四元数*/
|
||
Quaternion CreateQuaternion(float angle,Vector3 v);
|
||
|
||
/*计算两个四元数相乘，返回相乘的结果*/
|
||
Quaternion QuaternionMulti(Quaternion q1,Quaternion q2);
|
||
|
||
/*给定四元数求该四元数的共轭四元数*/
|
||
Quaternion QuaternionConjugate(Quaternion q1);
|
||
|
||
/*给定四元数求该四元数的模*/
|
||
float QuaternionMod(Quaternion q1);
|
||
|
||
/*给定四元数求该四元数的逆*/
|
||
Quaternion QuaternionInversion(Quaternion q1);
|
||
|
||
void DisplayQuaternion(Quaternion q);
|
||
|
||
Quaternion EulerAnglesToQuaternion(EulerAngles e);
|
||
|
||
//EulerAngles QuaternionToEulerAngles(Quaternion q,bool useRefer);
|
||
|
||
void DisplayEulerAngles(EulerAngles e);
|
||
|
||
void SetEulerAngles(EulerAngles *e,float Yaw,float Pitch,float Roll);
|
||
|
||
//EulerAngles SetEulerAngles(float Yaw,float Pitch,float Roll);
|
||
|
||
/*判断两个四元数是否相等，相等返回ture，否则返回false*/
|
||
//bool QuaternionEqual(Quaternion q1,Quaternion q2);
|
||
|
||
|
||
Vector3 SetVector3(float x,float y,float z);
|
||
|
||
Quaternion CreateQuaternionByGyro(float gx,float gy,float gz);
|
||
|
||
|
||
Matrix3 QuaternionToMatrix(Quaternion q);
|
||
|
||
|
||
MVector3 ComputeRotate(Matrix3 mMatrix,MVector3 vec);
|
||
|
||
EulerAngles MathQuaternionToEulerAngles(Quaternion q);
|
||
|
||
|
||
#endif
|
||
|
||
|
||
|
||
|
||
#endif |