struct Quaternion
{
var () float n; //scalar
var () vector v; //vector
};
static final function Quaternion Quad(float a, float b, float c, float d){ //quaternion constructor
local Quaternion Temp;
Temp.n=a;
Temp.v.x=b;
Temp.v.y=c;
Temp.v.z=d;
return Temp;
}
static final operator(16) Quaternion * ( Quaternion A, Quaternion B ){ //mult quads
return Quad(
A.n*B.n-A.v.x*B.v.x-A.v.y*B.v.y-A.v.z*B.v.z,
A.n*B.v.x+A.v.x*B.n+A.v.y*B.v.z-A.v.z*B.v.y,
A.n*B.v.y+A.v.y*B.n+A.v.z*B.v.x-A.v.x*B.v.z,
A.n*B.v.z+A.v.z*B.n+A.v.x*B.v.y-A.v.y*B.v.x
);
}
static final operator(16) Quaternion * ( Quaternion A, Vector B ){ //mult quad by vector
return Quad(
-(A.v.x*B.x+A.v.y*B.y+A.v.z*B.z),
A.n*B.x+A.v.y*B.z-A.v.z*B.y,
A.n*B.y+A.v.z*B.x-A.v.x*B.z,
A.n*B.z+A.v.x*B.y-A.v.y*B.x
);
}
static final operator(16) Quaternion * ( Vector B, Quaternion A ){ //mult quad by vector (other order
return A*B;
}
static final operator(16) Quaternion * ( Quaternion A, float B ){ //mult quads by scalar
return Quad(
A.n*B,
A.v.x*B,
A.v.y*B,
A.v.z*B
);
}
static final operator(16) Quaternion * ( float B, Quaternion A ){ //mult quads by scalar (other order)
return A*B;
}
static final operator(20) Quaternion + ( Quaternion A, Quaternion B ){ //add quads
return Quad(
A.n+B.n,
A.v.x+B.V.x,
A.v.y+B.V.y,
A.v.z+B.V.z
);
}
static final operator(20) Quaternion - ( Quaternion A, Quaternion B ){ //subtracts quads
return Quad(
A.n-B.n,
A.v.x-B.V.x,
A.v.y-B.V.y,
A.v.z-B.V.z
);
}
static final operator(34) Quaternion += ( out Quaternion A, Quaternion B ){ //add quads to
A.n-=B.n;
A.v.x-=B.V.x;
A.v.y-=B.V.y;
A.v.z-=B.V.z;
}
static final operator(34) Quaternion -= ( out Quaternion A, Quaternion B ){ //subtracts quads from
A.n-=B.n;
A.v.x-=B.V.x;
A.v.y-=B.V.y;
A.v.z-=B.V.z;
}
static final operator(34) Quaternion *= ( out Quaternion A, float B ){ //mult by scalar
A.n*=B;
A.v.x*=B;
A.v.y*=B;
A.v.z*=B;
}
static final operator(34) Quaternion /= ( out Quaternion A, float B ){ //divide by scalar
A.n/=B;
A.v.x/=B;
A.v.y/=B;
A.v.z/=B;
}
static final preoperator Quaternion ~ ( Quaternion A ){ //conjugate (negative of vector part)
return Quad(A.n,-v.x,-v.y,-v.z);
}
static final function float QSizeSquared(Quaternion A){ //get size (faster)
return Square(A.n)+Square(Q.v.x)+Square(Q.v.y)+Square(Q.v.z);
}
static final function float QSize(Quaternion A){ //get size (magnitude)
return sqrt(QSizeSquared(A));
}
static final function Quaternion QNormal(Quaternion A){ //normalize
return A/=QSize(A);
}
static final function float QGetAngle(Quaternion A){ //get angle about quad vector axis
return 2*acos(A.n);
}
static final function vector QGetAxis(Quaternion A){ //get unit vector along rot.
return Normal(A.v);
}
static final function Quaternion QRotate(Quaternion A, Quaternion B){ //rotate A by B
return A*B*(~A);
}
static final function Vector QVRotate(Quaternion A, Vector B){ //rotate vector B by quaternion B
return (A*B*(~A)).V;
}
static final function Quaternion ROTtoQuat(Rotator A){
local float pitch; //converted to radians.
local float yaw;
local float roll;
local float cyaw, cpitch, croll, syaw, spitch, sroll; //multiplies
local float cyawcpitch, syawspitch, cyawspitch, syawcpitch;
pitch=A.pitch/UURot;
yaw=A.yaw/UURot;
roll=A.roll/UURot;
cpitch=cos(0.5*pitch);
cyaw=cos(0.5*yaw);
croll=cos(0.5*roll);
spitch=sin(0.5*pitch);
syaw=sin(0.5*yaw);
sroll=sin(0.5*roll);
cyawcpitch=cyaw*cpitch;
syawspitch=syaw*spitch;
cyawspitch=cyaw*spitch;
syawcpitch=syaw*cpitch;
return Quad(
cyawcpitch * croll + syawspitch * sroll,
cyawcpitch * sroll - syawspitch * croll,
cyawspitch * croll + syawcpitch * sroll,
syawcpitch * croll - cyawspitch * sroll
);
}
static final function Rotator QuatToRot(Quaternion q){
local float r11, r21, r31, r32, r33, r12, r13;
local float q00, q11, q22, q33;
local float tmp;
q00 = Square (q.n);
q11 = Square (q.v.x);
q22 = Square (q.v.y);
q33 = Square (q.v.z);
r11 = q00 + q11 - q22 -q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 +q33;
tmp = abs(r31);
if (tmp>(999999.0/1000000.0)){ //uscript sux with float consts...
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
return Rot(-(pi/2)*(r31/temp)*uurot, atan2(-r12,-r31*r13)*uurot, 0);
}
return Rot(asin(-r31)*uurot, atan2(r21, r11)*uurot, atan2(r32,r33)*uurot);
}