mirror of
https://gitee.com/Lyon1998/pikapython.git
synced 2025-01-15 17:02:53 +08:00
275 lines
7.4 KiB
C
275 lines
7.4 KiB
C
#include "PikaMath_Quaternion.h"
|
|
#include "PikaStdData_List.h"
|
|
#include "math.h"
|
|
|
|
void PikaMath_Quaternion___init__(PikaObj *self){
|
|
obj_setFloat(self,"x",0.0);
|
|
obj_setFloat(self,"y",0.0);
|
|
obj_setFloat(self,"z",0.0);
|
|
obj_setFloat(self,"w",1.0);
|
|
}
|
|
|
|
void PikaMath_Quaternion_set(PikaObj *self, double w, double x, double y, double z){
|
|
obj_setFloat(self,"x",x);
|
|
obj_setFloat(self,"y",y);
|
|
obj_setFloat(self,"z",z);
|
|
obj_setFloat(self,"w",w);
|
|
}
|
|
|
|
double PikaMath_Quaternion_get(PikaObj *self, int key){
|
|
if(key==0){
|
|
return obj_getFloat(self,"x");
|
|
}
|
|
if(key==1){
|
|
return obj_getFloat(self,"y");
|
|
}
|
|
if(key==2){
|
|
return obj_getFloat(self,"z");
|
|
}
|
|
if(key==3){
|
|
return obj_getFloat(self,"w");
|
|
}
|
|
else{
|
|
obj_setErrorCode(self,1);
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
void PikaMath_Quaternion_add(PikaObj *self, PikaObj* quat){
|
|
float x_ = obj_getFloat(quat,"x");
|
|
float y_ = obj_getFloat(quat,"y");
|
|
float z_ = obj_getFloat(quat,"z");
|
|
float w_ = obj_getFloat(quat,"w");
|
|
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float x_sum = x + x_ ;
|
|
float y_sum = y + y_ ;
|
|
float z_sum = z + z_ ;
|
|
float w_sum = w + w_ ;
|
|
|
|
obj_setFloat(self,"x",x_sum);
|
|
obj_setFloat(self,"y",y_sum);
|
|
obj_setFloat(self,"z",z_sum);
|
|
obj_setFloat(self,"w",w_sum);
|
|
}
|
|
|
|
void PikaMath_Quaternion_sub(PikaObj *self, PikaObj* quat){
|
|
float x_ = obj_getFloat(quat,"x");
|
|
float y_ = obj_getFloat(quat,"y");
|
|
float z_ = obj_getFloat(quat,"z");
|
|
float w_ = obj_getFloat(quat,"w");
|
|
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float x_sub = x - x_ ;
|
|
float y_sub = y - y_ ;
|
|
float z_sub = z - z_ ;
|
|
float w_sub = w - w_ ;
|
|
|
|
obj_setFloat(self,"x",x_sub);
|
|
obj_setFloat(self,"y",y_sub);
|
|
obj_setFloat(self,"z",z_sub);
|
|
obj_setFloat(self,"w",w_sub);
|
|
}
|
|
|
|
void PikaMath_Quaternion_mul(PikaObj *self, PikaObj* quat){
|
|
float x_ = obj_getFloat(quat,"x");
|
|
float y_ = obj_getFloat(quat,"y");
|
|
float z_ = obj_getFloat(quat,"z");
|
|
float w_ = obj_getFloat(quat,"w");
|
|
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float x_mul = w * x_ + x * w_ - y * z_ + z * y_;
|
|
float y_mul = w * y_ + y * w_ + x * z_ - z * x_ ;
|
|
float z_mul = w * z_ + z * w_ - x * y_ + y * x_;
|
|
float w_mul = w * w_ - x * x_ - y * y_ - z * z_;
|
|
|
|
obj_setFloat(self,"x",x_mul);
|
|
obj_setFloat(self,"y",y_mul);
|
|
obj_setFloat(self,"z",z_mul);
|
|
obj_setFloat(self,"w",w_mul);
|
|
}
|
|
double PikaMath_Quaternion_magnituded(PikaObj *self){
|
|
float magnituded = PikaMath_Quaternion_magnitudedsquare(self);
|
|
|
|
return pow(magnituded,0.5);
|
|
}
|
|
double PikaMath_Quaternion_magnitudedsquare(PikaObj *self){
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float magnitudedsquare = pow(x,2) + pow(y,2) + pow(z,2) + pow(w,2);
|
|
|
|
return magnitudedsquare;
|
|
}
|
|
|
|
void PikaMath_Quaternion_reverse(PikaObj *self){
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
obj_setFloat(self,"x",-x);
|
|
obj_setFloat(self,"y",-y);
|
|
obj_setFloat(self,"z",-z);
|
|
obj_setFloat(self,"w",w);
|
|
}
|
|
|
|
void PikaMath_Quaternion_inverse(PikaObj *self){
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float mag = PikaMath_Quaternion_magnitudedsquare(self);
|
|
if(mag>0.0001){
|
|
obj_setFloat(self,"x",-x/mag);
|
|
obj_setFloat(self,"y",-y/mag);
|
|
obj_setFloat(self,"z",-z/mag);
|
|
obj_setFloat(self,"w",w/mag);
|
|
}
|
|
else{
|
|
obj_setFloat(self,"x",0.0);
|
|
obj_setFloat(self,"y",0.0);
|
|
obj_setFloat(self,"z",0.0);
|
|
obj_setFloat(self,"w",1.0);
|
|
}
|
|
}
|
|
|
|
void PikaMath_Quaternion_crossproduct(PikaObj *self, PikaObj* quat){
|
|
float x_ = obj_getFloat(quat,"x");
|
|
float y_ = obj_getFloat(quat,"y");
|
|
float z_ = obj_getFloat(quat,"z");
|
|
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
|
|
float x_cross = y * z_ - z * y_ ;
|
|
float y_cross = z * x_ - x * z_ ;
|
|
float z_cross = x * y_ - y * x_ ;
|
|
|
|
obj_setFloat(self,"x",x_cross);
|
|
obj_setFloat(self,"y",y_cross);
|
|
obj_setFloat(self,"z",z_cross);
|
|
obj_setFloat(self,"w",0.0);
|
|
|
|
}
|
|
|
|
double PikaMath_Quaternion_dot(PikaObj *self, PikaObj* quat){
|
|
float x_ = obj_getFloat(quat,"x");
|
|
float y_ = obj_getFloat(quat,"y");
|
|
float z_ = obj_getFloat(quat,"z");
|
|
float w_ = obj_getFloat(quat,"w");
|
|
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
return ( x_ * x + y_ * y +z_ * z + w_ * w );
|
|
|
|
}
|
|
|
|
void PikaMath_Quaternion_normalize(PikaObj *self){
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float mag = PikaMath_Quaternion_magnituded(self);
|
|
if(mag>0.0001){
|
|
obj_setFloat(self,"x",x/mag);
|
|
obj_setFloat(self,"y",y/mag);
|
|
obj_setFloat(self,"z",z/mag);
|
|
obj_setFloat(self,"w",w/mag);
|
|
}
|
|
else{
|
|
obj_setFloat(self,"x",0.0);
|
|
obj_setFloat(self,"y",0.0);
|
|
obj_setFloat(self,"z",0.0);
|
|
obj_setFloat(self,"w",1.0);
|
|
}
|
|
}
|
|
|
|
int PikaMath_Quaternion_isnormalize(PikaObj *self){
|
|
float mag = PikaMath_Quaternion_magnituded(self);
|
|
|
|
if(1.0==mag){
|
|
return 1;
|
|
}
|
|
else{
|
|
return 0;
|
|
}
|
|
|
|
}
|
|
|
|
#define PI_DIV_180 (0.017453292519943296)
|
|
#define DegToRad(x) ((x)*PI_DIV_180)
|
|
|
|
void PikaMath_Quaternion_fromEuler(PikaObj *self, int mode, double pitch, double roll, double yaw){
|
|
if(mode){
|
|
pitch = DegToRad(pitch);
|
|
roll = DegToRad(roll);
|
|
yaw = DegToRad(yaw);
|
|
}
|
|
|
|
float sinp = sin( pitch / 2 );
|
|
float siny = sin( yaw / 2 );
|
|
float sinr = sin( roll / 2 );
|
|
|
|
float cosp = cos( pitch / 2 );
|
|
float cosy = cos( yaw / 2 );
|
|
float cosr = cos( roll / 2 );
|
|
|
|
float x = sinr*cosp*cosy - cosr*sinp*siny;
|
|
float y = cosr*sinp*cosy + sinr*cosp*siny;
|
|
float z = cosr*cosp*siny - sinr*sinp*cosy;
|
|
float w = cosr*cosp*cosy + sinr*sinp*siny;
|
|
|
|
obj_setFloat(self,"x",x);
|
|
obj_setFloat(self,"y",y);
|
|
obj_setFloat(self,"z",z);
|
|
obj_setFloat(self,"w",w);
|
|
|
|
}
|
|
|
|
PikaObj* PikaMath_Quaternion_toEuler(PikaObj *self){
|
|
float x = obj_getFloat(self,"x");
|
|
float y = obj_getFloat(self,"y");
|
|
float z = obj_getFloat(self,"z");
|
|
float w = obj_getFloat(self,"w");
|
|
|
|
float roll = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
|
|
float pitch = asin( 2 * (w * y - z * x));
|
|
float yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
|
|
|
|
float rpy[3]={roll,pitch,yaw};
|
|
|
|
PikaObj* list = newNormalObj(New_PikaStdData_List);
|
|
PikaStdData_List___init__(list);
|
|
|
|
for(int i= 0 ; i < 3 ; i++){
|
|
Arg* token_arg = arg_newFloat(rpy[i]);
|
|
/* 添加到 list 对象 */
|
|
PikaStdData_List_append(list, token_arg);
|
|
/* 销毁 arg */
|
|
arg_deinit(token_arg);
|
|
}
|
|
|
|
return list;
|
|
|
|
} |