四元数 欧拉角和轴向角

Posted 2020-09-27 banana16314

参考博客：http://www.cnblogs.com/ITcode/p/3910205.html
http://www.cnblogs.com/graphics/archive/2012/08/10/2627458.html

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define CLAMP(x , min , max) ((x) > (max) ? (max) : ((x) < (min) ? (min) : x))

typedef struct quater{
    float x , y , z , w;    
}Q;

typedef struct Euler{
    float m_fYaw , m_fPitch, m_fRoll;
}E;

typedef struct matrix{
    float _11,_12,_13,_14;
    float _21,_22,_23,_24;
    float _31,_32,_33,_34;
    float _41,_42,_43,_44;
}M;

typedef struct aix{
    float theta;
    float x,y,z;
}A;

typedef struct u{
    float i,j,k;
}U;

E getEuler(Q q0) {
    E ea;
    ea.m_fRoll  = atan2(2 * (q0.w * q0.z + q0.x * q0.y) , 1 - 2 * (q0.z * q0.z + q0.x * q0.x));
    ea.m_fPitch = asin(CLAMP(2 * (q0.w * q0.x - q0.y * q0.z) , -1.0f , 1.0f));
    ea.m_fYaw   = atan2(2 * (q0.w * q0.y + q0.z * q0.x) , 1 - 2 * (q0.x * q0.x + q0.y * q0.y));

    return ea;
}

M getMatrix(Q q0){
      M ret;              
      float xx = q0.x*q0.x;
   float yy = q0.y*q0.y;
   float zz = q0.z*q0.z;
   float xy = q0.x*q0.y;
   float wz = -q0.w*q0.z;
   float wy = -q0.w*q0.y;
   float xz = q0.x*q0.z;
   float yz = q0.y*q0.z;
   float wx = -q0.w*q0.x;
 
   ret._31 = 1.0f-2*(yy+zz);
   ret._32 = 2*(xy-wz);
   ret._33 = 2*(wy+xz);
   ret._34 = 0.0f;
 
   ret._11 = 2*(xy+wz);
   ret._12 = 1.0f-2*(xx+zz);
   ret._13 = 2*(yz-wx);
   ret._14 = 0.0f;
 
   ret._21 = 2*(xy-wy);
   ret._22 = 2*(yz+wx);
   ret._23 = 1.0f-2*(xx+yy);
   ret._24 = 0.0f;
 
   ret._41 = 0.0f;
   ret._42 = 0.0f;
   ret._43 = 0.0f;
   ret._44 = 1.0f;
   return ret;
}

A getAix(Q q0){
    A r;
    r.theta=2*acos(q0.w);
    float temp=r.theta/2;
    r.x=q0.x/sin(temp);
    r.y=q0.y/sin(temp);
    r.z=q0.z/sin(temp);
    return r;
}

U getNew(U u0,A a){
    M temp;
    float scale=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
    a.x/=scale;
    a.y/=scale;
    a.z/=scale;
    float c=cos(a.theta*3.1415926/180);
    float s=sin(a.theta*3.1415926/180);
    float t=1-c;
    float q1=a.x;
    float q2=a.y;
    float q3=a.z;
    temp._11=t*q1*q1+c;
    temp._12=t*q1*q2+s*q3;
    temp._13=t*q1*q3-s*q2;
    temp._14=0;
    temp._21=t*q1*q2-s*q3;
    temp._22=t*q2*q2+c;
    temp._23=t*q3*q2+s*q1;
    temp._24=0;
    temp._31=t*q1*q3+s*q2;
    temp._32=t*q2*q3-s*q1;
    temp._33=t*q3*q3+c;
    temp._34=0;
    temp._41=0;
    temp._42=0;
    temp._43=0;
    temp._44=1;
    U u1;
    u1.i=u0.i*temp._11+u0.j*temp._21+u0.k*temp._31+0;
    u1.j=u0.i*temp._12+u0.j*temp._22+u0.k*temp._32+0;
    u1.k=u0.i*temp._13+u0.j*temp._23+u0.k*temp._33+0;
    return u1;
}
int main(){
    Q q0;
    q0.x=0.5;q0.y=0.5;q0.z=0.5;q0.w=0.5;
    E ea=getEuler(q0);
    printf("get Euler Angle:%.2f %.2f %.2f\\n",ea.m_fRoll,ea.m_fPitch,ea.m_fYaw);
    printf("get Matrix:\\n");
    M mr=getMatrix(q0);
    printf("%.2f %.2f %.2f %0.2f\\n",mr._11,mr._12,mr._13,mr._14);
    printf("%.2f %.2f %.2f %0.2f\\n",mr._21,mr._22,mr._23,mr._24);
    printf("%.2f %.2f %.2f %0.2f\\n",mr._31,mr._32,mr._33,mr._34);
    printf("%.2f %.2f %.2f %0.2f\\n",mr._41,mr._42,mr._43,mr._44);
    printf("get Axis:\\n");
    A a=getAix(q0);
    printf("get Aixs Angle:%.2f %.2f %.2f %0.2f\\n",a.theta,a.x,a.y,a.z);
    
    A a1;
    a1.theta=120;a1.x=1;a1.y=1,a1.z=1;
    U u0;
    u0.i=1;u0.j=2;u0.k=3;
    U u1=getNew(u0,a1);
    printf("the second:\\n");
    printf("get new vec:%.2f %.2f %.2f\\n",u1.i,u1.j,u1.k);    
    return 0;
}