Code:
namespace Clockwork{
namespace Quaternion{
float const tol = 0.0000000001f;
public value class Quat{
public:
float n;
Vec3 V;
float DegToRad(float deg){
return deg * PI / 180.0f;
}
float RadToDeg(float rad){
return rad * 180.0f / PI;
}
Quat(float n, float vX, float vY, float vZ){
n = n;
V.x = vX;
V.y = vY;
V.z = vZ;
}
Quat(float n, Vec3 V){
n = n;
this->V = V;
}
Quat operator~(){
return Quat(n, -V.x, -V.y, -V.z);
}
Quat operator+(Quat Q){
return Quat(n+Q.n, V+Q.V);
}
Quat operator-(Quat Q){
return Quat(n=Q.n, V-Q.V);
}
Quat operator*(Quat Q){
return Quat(n*Q.n - V.x*Q.V.x - V.y*Q.V.y - V.z*Q.V.z,
n*Q.V.x + V.x*Q.n + V.y*Q.V.z - V.z*Q.V.y,
n*Q.V.y + V.y*Q.n + V.z*Q.V.x - V.x*Q.V.z,
n*Q.V.z + V.z*Q.n + V.x*Q.V.y - V.y*Q.V.x);
}
Quat operator*(Vec3 Vec){
return Quat(-(V.x*Vec.x + V.y*Vec.y + V.z*Vec.z),
n*Vec.x + V.z*Vec.y - V.y*Vec.z,
n*Vec.y + V.x*Vec.z - V.z*Vec.x,
n*Vec.z + V.y*Vec.x - V.x*Vec.y);
}
Quat operator*(float num){
return Quat(n*num, V.x*num, V.y*num, V.z*num);
}
Quat operator/(float num){
return Quat(n/num, V.x/num, V.y/num, V.z/num);
}
float Length(){
return (float)sqrt((n*n) + (V.x*V.x) + (V.y*V.y) + (V.z*V.z));
}
float GetAngle(){
return (float)(2*acos(n));
}
Quat Normalize(){
Quat res(n, V.x, V.y, V.z);
res = res / Length();
return res;
}
Vec3 GetAxis(){
Vec3 Vec = V;
float m = Vec.Length();
if(m<tol){
return Vec3();
}
else{
return Vec/m;
}
}
Vec3 QuatToAxisAngle(Quat Q){
//float scale = (float)sqrt((Q.V.x*Q.V.x) + (Q.V.y*Q.V.y) + (Q.V.z*Q.V.z));
float scale = sin(acos(n));
float aX = Q.V.x/scale;
float aY = Q.V.y/scale;
float aZ = Q.V.z/scale;
return Vec3(aX, aY, aZ);
}
Vec3 QuatToAxisAngle(){
//float scale = (float)sqrt((V.x*V.x) + (V.y*V.y) + (V.z*V.z));
float scale = sin(acos(n));
float aX = V.x/scale;
float aY = V.y/scale;
float aZ = V.z/scale;
return Vec3(RadToDeg(aX), RadToDeg(aY), RadToDeg(aZ));
}
Quat EulerToQuat(Vec3 V){
float vX = DegToRad(V.x);
float vY = DegToRad(V.y);
float vZ = DegToRad(V.z);
Quat Qx(cos(vX/2), Vec3(sin(vX/2),0,0));
Quat Qy(cos(vY/2), Vec3(0,sin(vY/2),0));
Quat Qz(cos(vZ/2), Vec3(0,0,sin(vZ/2)));
Quat retQuat = Qx*Qy*Qz;
return Quat(retQuat.n, retQuat.V.x, retQuat.V.y, retQuat.V.z);
}
Quat Rotate(Quat Q2){
Quat Q1(n, V.x, V.y, V.z);
return Q1*Q2*(~Q1);
}
Quat Rotate(Quat Q1, Quat Q2){
return Q1*Q2*(~Q1);
}
Vec3 Rotate(Vec3 Vec){
Quat Q(n, V.x, V.y, V.z);
Quat tQ = Q*Vec*(~Q);
tQ.n = 0;
return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
}
Vec3 RotateToRad(Vec3 Vec){
Quat Q(n, V.x, V.y, V.z);
Quat tQ = Q*Vec*(~Q);
return Vec3(DegToRad(tQ.V.x), DegToRad(tQ.V.y), DegToRad(tQ.V.z));
}
Vec3 RotateToDeg(Vec3 Vec){
Quat Q(n, V.x, V.y, V.z);
Quat tQ = Q*Vec*(~Q);
return Vec3(RadToDeg(tQ.V.x), RadToDeg(tQ.V.y), RadToDeg(tQ.V.z));
}
Vec3 Rotate(Quat Q, Vec3 Vec){
Quat tQ = Q*Vec*(~Q);
return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
}
};
}
}