/// Quaternion.cpp
#include <math.h>
#include <iostream>
using namespace std;
/// Define Data type
typedef struct
{
double t; // real-component
double x; // x-component
double y; // y-component
double z; // z-component
} quaternion;
Bill 注:Kakezan 在日语里是 “乘法”的意思
quaternion Kakezan(quaternion left, quaternion right)
{
quaternion ans;
double d1, d2, d3, d4;
d1 = left.t * right.t;
d2 = -left.x * right.x;
d3 = -left.y * right.y;
d4 = -left.z * right.z;
ans.t = d1+ d2+ d3+ d4;
d1 = left.t * right.x;
d2 = right.t * left.x;
d3 = left.y * right.z;
d4 = -left.z * right.y;
ans.x = d1+ d2+ d3+ d4;
d1 = left.t * right.y;
d2 = right.t * left.y;
d3 = left.z * right.x;
d4 = -left.x * right.z;
ans.y = d1+ d2+ d3+ d4;
d1 = left.t * right.z;
d2 = right.t * left.z;
d3 = left.x * right.y;
d4 = -left.y * right.x;
ans.z = d1+ d2+ d3+ d4;
return ans;
}
Make Rotational quaternion
quaternion MakeRotationalQuaternion(double radian, double AxisX, double AxisY, double AxisZ)
{
quaternion ans;
double norm;
double ccc, sss;
ans.t = ans.x = ans.y = ans.z = 0.0;
norm = AxisX * AxisX + AxisY * AxisY + AxisZ * AxisZ;
if(norm <= 0.0) return ans;
norm = 1.0 / sqrt(norm);
AxisX *= norm;
AxisY *= norm;
AxisZ *= norm;
ccc = cos(0.5 * radian);
sss = sin(0.5 * radian);
ans.t = ccc;
ans.x = sss * AxisX;
ans.y = sss * AxisY;
ans.z = sss * AxisZ;
return ans;
}
Put XYZ into quaternion
quaternion PutXYZToQuaternion(double PosX, double PosY, double PosZ)
{
quaternion ans;
ans.t = 0.0;
ans.x = PosX;
ans.y = PosY;
ans.z = PosZ;
return ans;
}
typedef struct {
int angle;
int rotate_vector[3];
} rotate_quaternion;
rotate_quaternion RQ[]={ {45,{1,0,0}}, {90,{0,1,0}}, {180,{0,0,1}} };
/ main
int main()
{
double px, py, pz;
double ax, ay, az, th;
quaternion ppp, qqq, rrr;
int size =3;
rotate_quaternion *pRQ=&RQ[0];
cout << "Point Position (x, y, z) " << endl;
cout << " x = ";
cin >> px;
cout << " y = ";
cin >> py;
cout << " z = ";
cin >> pz;
ppp = PutXYZToQuaternion(px, py, pz);
do{
cout << "\nRotation Degree ? (Enter 0 to Quit) " << endl;
cout << " angle = "<<pRQ->angle <<endl;
// cin >> th ;
th = pRQ->angle;
if(th == 0.0) break;
cout << "Rotation Axis Direction ? (x, y, z) " << endl;
cout << " x = " <<pRQ->rotate_vector[0] ;
ax = pRQ->rotate_vector[0];
//cin >> ax;
cout << " y = "<<pRQ->rotate_vector[1];
ay = pRQ->rotate_vector[1];
//cin >> ay;
cout << " z = "<<pRQ->rotate_vector[2]<<endl;;
//cin >> az;
az = pRQ->rotate_vector[2];
th *= 3.1415926535897932384626433832795 / 180.0; /// Degree -> radian;
qqq = MakeRotationalQuaternion(th, ax, ay, az);
rrr = MakeRotationalQuaternion(-th, ax, ay, az);
ppp = Kakezan(rrr, ppp);
ppp = Kakezan(ppp, qqq);
cout << "\nAnser X = " << ppp.x
<< "\n Y = " << ppp.y
<< "\n Z = " << ppp.z << endl;
} while(pRQ++) ;
return 0;
}
#include <math.h>
#include <iostream>
using namespace std;
/// Define Data type
typedef struct
{
double t; // real-component
double x; // x-component
double y; // y-component
double z; // z-component
} quaternion;
Bill 注:Kakezan 在日语里是 “乘法”的意思
quaternion Kakezan(quaternion left, quaternion right)
{
quaternion ans;
double d1, d2, d3, d4;
d1 = left.t * right.t;
d2 = -left.x * right.x;
d3 = -left.y * right.y;
d4 = -left.z * right.z;
ans.t = d1+ d2+ d3+ d4;
d1 = left.t * right.x;
d2 = right.t * left.x;
d3 = left.y * right.z;
d4 = -left.z * right.y;
ans.x = d1+ d2+ d3+ d4;
d1 = left.t * right.y;
d2 = right.t * left.y;
d3 = left.z * right.x;
d4 = -left.x * right.z;
ans.y = d1+ d2+ d3+ d4;
d1 = left.t * right.z;
d2 = right.t * left.z;
d3 = left.x * right.y;
d4 = -left.y * right.x;
ans.z = d1+ d2+ d3+ d4;
return ans;
}
Make Rotational quaternion
quaternion MakeRotationalQuaternion(double radian, double AxisX, double AxisY, double AxisZ)
{
quaternion ans;
double norm;
double ccc, sss;
ans.t = ans.x = ans.y = ans.z = 0.0;
norm = AxisX * AxisX + AxisY * AxisY + AxisZ * AxisZ;
if(norm <= 0.0) return ans;
norm = 1.0 / sqrt(norm);
AxisX *= norm;
AxisY *= norm;
AxisZ *= norm;
ccc = cos(0.5 * radian);
sss = sin(0.5 * radian);
ans.t = ccc;
ans.x = sss * AxisX;
ans.y = sss * AxisY;
ans.z = sss * AxisZ;
return ans;
}
Put XYZ into quaternion
quaternion PutXYZToQuaternion(double PosX, double PosY, double PosZ)
{
quaternion ans;
ans.t = 0.0;
ans.x = PosX;
ans.y = PosY;
ans.z = PosZ;
return ans;
}
typedef struct {
int angle;
int rotate_vector[3];
} rotate_quaternion;
rotate_quaternion RQ[]={ {45,{1,0,0}}, {90,{0,1,0}}, {180,{0,0,1}} };
/ main
int main()
{
double px, py, pz;
double ax, ay, az, th;
quaternion ppp, qqq, rrr;
int size =3;
rotate_quaternion *pRQ=&RQ[0];
cout << "Point Position (x, y, z) " << endl;
cout << " x = ";
cin >> px;
cout << " y = ";
cin >> py;
cout << " z = ";
cin >> pz;
ppp = PutXYZToQuaternion(px, py, pz);
do{
cout << "\nRotation Degree ? (Enter 0 to Quit) " << endl;
cout << " angle = "<<pRQ->angle <<endl;
// cin >> th ;
th = pRQ->angle;
if(th == 0.0) break;
cout << "Rotation Axis Direction ? (x, y, z) " << endl;
cout << " x = " <<pRQ->rotate_vector[0] ;
ax = pRQ->rotate_vector[0];
//cin >> ax;
cout << " y = "<<pRQ->rotate_vector[1];
ay = pRQ->rotate_vector[1];
//cin >> ay;
cout << " z = "<<pRQ->rotate_vector[2]<<endl;;
//cin >> az;
az = pRQ->rotate_vector[2];
th *= 3.1415926535897932384626433832795 / 180.0; /// Degree -> radian;
qqq = MakeRotationalQuaternion(th, ax, ay, az);
rrr = MakeRotationalQuaternion(-th, ax, ay, az);
ppp = Kakezan(rrr, ppp);
ppp = Kakezan(ppp, qqq);
cout << "\nAnser X = " << ppp.x
<< "\n Y = " << ppp.y
<< "\n Z = " << ppp.z << endl;
} while(pRQ++) ;
return 0;
}