DIRECTORY
G3dBasic, G3dMatrix, G3dQuaternion, G3dVector, Real, RealFns, Rope;

G3dQuaternionImpl: CEDAR PROGRAM
IMPORTS G3dMatrix, G3dVector, Real, RealFns
EXPORTS G3dQuaternion

~ BEGIN
Error:	PUBLIC ERROR [reason: Rope.ROPE] ~ CODE;
Pair:			TYPE ~ G3dBasic.Pair;
Triple:			TYPE ~ G3dBasic.Triple;
Quad:			TYPE ~ G3dBasic.Quad;
Matrix:			TYPE ~ G3dMatrix.Matrix;
MatrixRep:		TYPE ~ G3dMatrix.MatrixRep;
Quaternion:		TYPE ~ G3dQuaternion.Quaternion;
AxisAngle:		TYPE ~ G3dQuaternion.AxisAngle;
QuaternionSequence:	TYPE ~ G3dQuaternion.QuaternionSequence;
QuaternionSequenceRep:	TYPE ~ G3dQuaternion.QuaternionSequenceRep;
origin:		Triple ~ G3dBasic.origin;

qZero: PUBLIC Quaternion ¬ [x: 0, y: 0, z: 0, w: 0];
quW: PUBLIC Quaternion ¬ [x: 0, y: 0, z: 0, w: 1];
quX: PUBLIC Quaternion ¬ [x: 1, y: 0, z: 0, w: 0];
quY: PUBLIC Quaternion ¬ [x: 0, y: 1, z: 0, w: 0];
quZ: PUBLIC Quaternion ¬ [x: 0, y: 0, z: 1, w: 0];
FromComponents: PUBLIC PROC [x, y, z, w: REAL] RETURNS [qq: Quaternion]
~ {
RETURN[[x: x, y: y, z: z, w: w]];
};

FromVectorScalar: PUBLIC PROC [v: Triple, w: REAL ¬ 1.0] RETURNS [qq: Quaternion]
~ {
RETURN[[x: v.x, y: v.y, z: v.z, w: w]];
};

FromVector: PUBLIC PROC [v: Triple] RETURNS [qq: Quaternion]
~ {
RETURN[[x: v.x, y: v.y, z: v.z, w: 0]];
};

FromScalar: PUBLIC PROC [w: REAL ¬ 1.0] RETURNS [qq: Quaternion]
~ {
RETURN[[x: 0, y: 0, z: 0, w: w]];
};

FromAxisAngle: PUBLIC PROC [unitAxis: Triple, theta: REAL] RETURNS [qu: Quaternion]
~ {
halfTheta: REAL ¬ Real.FScale[theta, -1];
cosHalfTheta: REAL ¬ RealFns.Cos[halfTheta];
sinHalfTheta: REAL ¬ RealFns.Sin[halfTheta];
qu ¬ FromVectorScalar[G3dVector.Mul[unitAxis, sinHalfTheta], cosHalfTheta];
};

FromXYZAngles: PUBLIC PROC [forX, forY, forZ: REAL] RETURNS [qu: Quaternion]
~ {
halfX: REAL ¬ Real.FScale[forX, -1];
halfY: REAL ¬ Real.FScale[forY, -1];
halfZ: REAL ¬ Real.FScale[forZ, -1];
cosHalfX: REAL ¬ RealFns.Cos[halfX];
sinHalfX: REAL ¬ RealFns.Sin[halfX];
cosHalfY: REAL ¬ RealFns.Cos[halfY];
sinHalfY: REAL ¬ RealFns.Sin[halfY];
cosHalfZ: REAL ¬ RealFns.Cos[halfZ];
sinHalfZ: REAL ¬ RealFns.Sin[halfZ];
qu.w ¬ cosHalfX*cosHalfY*cosHalfZ + sinHalfX*sinHalfY*sinHalfZ;
qu.x ¬ sinHalfX*cosHalfY*cosHalfZ - cosHalfX*sinHalfY*sinHalfZ;
qu.y ¬ cosHalfX*sinHalfY*cosHalfZ + sinHalfX*cosHalfY*sinHalfZ;
qu.z ¬ cosHalfX*cosHalfY*sinHalfZ - sinHalfX*sinHalfY*cosHalfZ;
};

FromMatrix: PUBLIC PROC [mat: Matrix] RETURNS [qu: Quaternion]
~ {
X: NAT ~ 0;  Y: NAT ~ 1;  Z: NAT ~ 2;  W: NAT ~ 3;
tr, s: REAL;

tr ¬ mat[X][X] + mat[Y][Y]+ mat[Z][Z];
IF tr >= 0.0
THEN {
s ¬ RealFns.SqRt[tr + 1.0];
qu.w ¬ Real.FScale[s, -1];
s ¬ 0.5 / s;
qu.x ¬ (mat[Y][Z] - mat[Z][Y]) * s;
qu.y ¬ (mat[Z][X] - mat[X][Z]) * s;
qu.z ¬ (mat[X][Y] - mat[Y][X]) * s;
}
ELSE {
nxt: ARRAY[X..Z] OF NAT ~ [Y, Z, X];
qi, qj, qk: REAL;
j, k: NAT;
i: NAT ¬ X;
IF mat[Y][Y] > mat[X][X] THEN i ¬ Y;
IF mat[Z][Z] > mat[i][i] THEN i ¬ Z;
j ¬ nxt[i];  k ¬ nxt[j];
s ¬ RealFns.SqRt[ (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0 ];
qi ¬ Real.FScale[s, -1];
s ¬ 0.5 / s;
qj ¬ (mat[i][j] + mat[j][i]) * s;
qk ¬ (mat[k][i] + mat[i][k]) * s;
qu.w ¬ (mat[j][k] - mat[k][j]) * s;
SELECT i FROM
X => [qu.x, qu.y, qu.z] ¬ Triple[qi, qj, qk];
Y => [qu.x, qu.y, qu.z] ¬ Triple[qk, qi, qj];
Z => [qu.x, qu.y, qu.z] ¬ Triple[qj, qk, qi];
ENDCASE;
};
};

ToMatrix: PUBLIC PROC [q: Quaternion, out: Matrix ¬ NIL] RETURNS [Matrix]
~ {
s, xs, ys, zs: REAL;
wx, wy, wz,  xx, xy, xz,  yy, yz,  zz: REAL;
norm: REAL ¬ Norm[q];
IF norm = 0.0 THEN RETURN[G3dMatrix.Identity[out]];
s ¬ 2.0 / norm;
xs ¬ q.x*s;		ys ¬ q.y*s;		zs ¬ q.z*s;
wx ¬ q.w*xs;		wy ¬ q.w*ys;	wz ¬ q.w*zs;
xx ¬ q.x*xs;		xy ¬ q.x*ys;		xz ¬ q.x*zs;
yy ¬ q.y*ys;		yz ¬ q.y*zs;		zz ¬ q.z*zs;
IF out = NIL THEN out ¬ NEW[MatrixRep];
out­ ¬ [
[1.0 - (yy + zz),	xy + wz,			xz - wy,			0.0],
[xy - wz,			1.0 - (xx + zz),	yz + wx,			0.0],
[xz + wy,			yz - wx,			1.0 - (xx + yy),	0.0],
[0.0,					0.0,				0.0,				1.0]];
RETURN[out];
};

ToAxisAngle: PUBLIC PROC [qu: Quaternion] RETURNS [axisAngle: AxisAngle]
~ {
axisAngle.unitAxis ¬ G3dVector.Unit[Vector[qu]];
axisAngle.theta ¬ Real.FScale[RealFns.ArcCos[qu.w], 1];
};

ToXYZAngles: PUBLIC PROC [qu: Quaternion] RETURNS [xyzAngles: Triple]
~ {
RETURN[G3dMatrix.ExtractRotate[ToMatrix[qu]]];
};
MakeRotateQ: PUBLIC PROC [q: Quaternion, out: Matrix ¬ NIL] RETURNS [Matrix]
~ ToMatrix;

MakeRotateAboutQ: PUBLIC PROC [q: Quaternion, base: Triple ¬ origin, out: Matrix ¬ NIL]
RETURNS [Matrix]
~ {
rot: Matrix ¬ ToMatrix[q, out];
trans: Triple ¬ G3dVector.Sub[base, G3dMatrix.Transform[base, rot]];
rot[3] ¬ [trans.x, trans.y, trans.z, 1.0];
RETURN[rot];
};
Id: PUBLIC PROC RETURNS [qu: Quaternion]
~ {
qu ¬ quW;
};

Zero: PUBLIC PROC RETURNS [qq: Quaternion]
~ {
qq ¬ qZero;
};

Components: PUBLIC PROC [q: Quaternion] RETURNS [Quad]
~ {
RETURN[[q.x, q.y, q.z, q.w]];
};

Vector: PUBLIC PROC [q: Quaternion] RETURNS [Triple]
~ {
RETURN[[q.x, q.y, q.z]];
};

Scalar: PUBLIC PROC [q: Quaternion] RETURNS [REAL]
~ {
RETURN[q.w];
};

PureVector: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
qq ¬ [x: q.x, y: q.y, z: q.z, w: 0];
};

Norm: PUBLIC PROC [q: Quaternion] RETURNS [REAL]
~ {
RETURN[q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w];
};

Neg: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
qq ¬ [x: -q.x, y: -q.y, z: -q.z, w: -q.w];
};

Conjugate: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
qq ¬ [x: -q.x, y: -q.y, z: -q.z, w: q.w];
};

Inverse: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
norminv: REAL ¬ 1.0 / Norm[q];
qq ¬ Scale[Conjugate[q], norminv];
};

Reverse: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
qq ¬ [x: q.x, y: q.y, z: q.z, w: -q.w];
};

Unit: PUBLIC PROC [q: Quaternion] RETURNS [qu: Quaternion]
~ {
norminv: REAL ¬ 1.0 / RealFns.SqRt[Norm[q]];
qu ¬ Scale[q, norminv];
};

Add: PUBLIC PROC [qL, qR: Quaternion] RETURNS [qq: Quaternion]
~ {
qq.w ¬ qL.w + qR.w;
qq.x ¬ qL.x + qR.x;
qq.y ¬ qL.y + qR.y;
qq.z ¬ qL.z + qR.z;
};

Sub: PUBLIC PROC [qL, qR: Quaternion] RETURNS [qq: Quaternion]
~ {
qq.w ¬ qL.w - qR.w;
qq.x ¬ qL.x - qR.x;
qq.y ¬ qL.y - qR.y;
qq.z ¬ qL.z - qR.z;
};

Mul: PUBLIC PROC [qL, qR: Quaternion] RETURNS [qq: Quaternion]
~ {
IF qL.w = 0.0
THEN {
IF qR.w = 0.0
THEN {
qq.w ¬ - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x ¬ qL.y*qR.z - qL.z*qR.y;
qq.y ¬ qL.z*qR.x - qL.x*qR.z;
qq.z ¬ qL.x*qR.y - qL.y*qR.x;
}
ELSE {
qq.w ¬ - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x ¬ qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
qq.y ¬ qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
qq.z ¬ qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
};
}
ELSE {
IF qR.w = 0.0
THEN {
qq.w ¬ - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x ¬ qL.w*qR.x + qL.y*qR.z - qL.z*qR.y;
qq.y ¬ qL.w*qR.y + qL.z*qR.x - qL.x*qR.z;
qq.z ¬ qL.w*qR.z + qL.x*qR.y - qL.y*qR.x;
}
ELSE {
qq.w ¬ qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x ¬ qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
qq.y ¬ qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
qq.z ¬ qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
};
};
};

Scale: PUBLIC PROC [q: Quaternion, w: REAL] RETURNS [qq: Quaternion]
~ {
qq.w ¬ q.w*w;
qq.x ¬ q.x*w;
qq.y ¬ q.y*w;
qq.z ¬ q.z*w;
};

Exp: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
theta: REAL ¬ G3dVector.Length[Vector[q]];
scale: REAL ¬ IF theta > 1.0e-5 THEN RealFns.Sin[theta]/theta ELSE 1.0;
mag: REAL ¬ IF q.w = 0.0 THEN 1.0 ELSE RealFns.Exp[q.w];
qq ¬ FromVectorScalar[G3dVector.Mul[Vector[q], scale*mag], RealFns.Cos[theta]*mag];
};

Ln: PUBLIC PROC [q: Quaternion] RETURNS [qq: Quaternion]
~ {
mag: REAL ¬ RealFns.Ln[RealFns.SqRt[Norm[q]]];
scale: REAL ¬ G3dVector.Length[Vector[q]];
theta: REAL ¬ RealFns.ArcTan[scale, q.w];
IF scale > 0.0 THEN scale ¬ theta/scale;
qq ¬ FromVectorScalar[G3dVector.Mul[Vector[q], scale], mag];
};

Rot: PUBLIC PROC [qu: Quaternion, v: Triple] RETURNS [vr: Triple]
~ {
qv: Triple ¬ Vector[qu];
qvv: Triple ¬ G3dVector.Cross[qv, v];
qvv ¬ G3dVector.Add[qvv, qvv];
vr ¬ G3dVector.Add[v, G3dVector.Mul[qvv, qu.w]];
vr ¬ G3dVector.Add[vr, G3dVector.Cross[qv, qvv]];
};

Dot: PUBLIC PROC [qa, qb: Quaternion] RETURNS [REAL]
~ {
RETURN [qa.x*qb.x + qa.y*qb.y + qa.z*qb.z + qa.w*qb.w];
};

Distance: PUBLIC PROC [qa, qb: Quaternion, p: REAL ¬ 0.0] RETURNS [REAL]
~ {
dx: REAL ¬ ABS[qa.x-qb.x];
dy: REAL ¬ ABS[qa.y-qb.y];
dz: REAL ¬ ABS[qa.z-qb.z];
dw: REAL ¬ ABS[qa.w-qb.w];
SELECT p FROM
0.0 => RETURN[MAX[dx, dy, dz, dw]];
1.0 => RETURN[dx+dy+dz+dw];
0.5 => RETURN[RealFns.SqRt[dx*dx+dy*dy+dz*dz+dw*dw]];
ENDCASE => {
pInv: REAL ~ 1/p;
RETURN[RealFns.Power[
RealFns.Power[dx, pInv]+
RealFns.Power[dy, pInv]+
RealFns.Power[dz, pInv]+
RealFns.Power[dw, pInv], p]]};
};
Slerp: PUBLIC PROC [qu0, qu1: Quaternion, t: REAL] RETURNS [qut: Quaternion]
~ {
omega, cosOmega, sinOmega, qu0Part, qu1Part: REAL;
epsilon: REAL ~ 1.0e-5;
PI: REAL ~ 3.14159265358979;

cosOmega ¬ Dot[qu0, qu1];
IF (1.0 + cosOmega) > epsilon
THEN { -- Usual case
IF (1.0 - cosOmega) > epsilon
THEN { -- Usual case
omega ¬ RealFns.ArcCos[cosOmega];
sinOmega ¬ RealFns.Sin[omega];
qu0Part ¬ RealFns.Sin[(1.0 - t)*omega] / sinOmega;
qu1Part ¬ RealFns.Sin[t*omega] / sinOmega;
}
ELSE { -- Ends very close
qu0Part ¬ 1.0 - t;
qu1Part ¬ t;
};
qut ¬ Add[Scale[qu0, qu0Part], Scale[qu1, qu1Part]];
}
ELSE { -- Ends nearly opposite
qup: Quaternion ¬ [x: -qu0.y, y: qu0.x, z: -qu0.w, w: qu0.z];
qu0Part ¬ RealFns.Sin[(0.5 - t) * PI];
qu1Part ¬ RealFns.Sin[t * PI];
qut ¬ Add[Scale[qu0, qu0Part], Scale[qup, qu1Part]];
};
};

Bisect: PUBLIC PROC [qu0, qu1: Quaternion] RETURNS [qu: Quaternion]
~ {
qu ¬ Unit[Add[qu0, qu1]
! Real.RealException => {qu ¬ [x: -qu0.y, y: qu0.x, z: -qu0.w, w: qu0.z]; CONTINUE}];
};

Double: PUBLIC PROC [qu0, qu1: Quaternion] RETURNS [qu: Quaternion]
~ {
qu ¬ Sub[Scale[qu1, 2*Dot[qu0, qu1]], qu0];
};

Squad: PUBLIC PROC [qu0, qu1, quq0, quq1: Quaternion, t: REAL] RETURNS [qut: Quaternion]
~ {
qu01: Quaternion ¬ Slerp[qu0, qu1, t];
quq01: Quaternion ¬ Slerp[quq0, quq1, t];
qut ¬ Slerp[qu01, quq01, 2.0*t*(1.0 - t)];
};

CatmullRom: PUBLIC PROC [qu0, qu1, qu2, qu3: Quaternion, t: REAL]
RETURNS [qut: Quaternion]
~ {
qu01: Quaternion ¬ Slerp[qu0, qu1, t+1];
qu12: Quaternion ¬ Slerp[qu1, qu2, t];
qu23: Quaternion ¬ Slerp[qu2, qu3, t-1];
qu02: Quaternion ¬ Slerp[qu01, qu12, (t+1)/2];
qu13: Quaternion ¬ Slerp[qu12, qu23, t/2];
qut ¬ Slerp[qu02, qu13, t];
};

Bezier: PUBLIC PROC [qu0, qua0, qub1, qu1: Quaternion, t: REAL] RETURNS [qut: Quaternion]
~ {
qus0a: Quaternion ¬ Slerp[qu0, qua0, t];
qusab: Quaternion ¬ Slerp[qua0, qub1, t];
qusb1: Quaternion ¬ Slerp[qub1, qu1, t];
qus0ab: Quaternion ¬ Slerp[qus0a, qusab, t];
qusab1: Quaternion ¬ Slerp[qusab, qusb1, t];
qut ¬ Slerp[qus0ab, qusab1, t];
};

BSpline: PUBLIC PROC [qu0, qu1, qu2, qu3: Quaternion, t1, t2, t3, t4, t5, t6, t: REAL]
RETURNS [qut: Quaternion]
~ {
qu01: Quaternion ¬ Slerp[qu0, qu1, (t-t1)/(t4-t1)];
qu12: Quaternion ¬ Slerp[qu1, qu2, (t-t2)/(t5-t2)];
qu23: Quaternion ¬ Slerp[qu2, qu3, (t-t3)/(t6-t3)];
qu02: Quaternion ¬ Slerp[qu01, qu12, (t-t2)/(t4-t2)];
qu13: Quaternion ¬ Slerp[qu12, qu23, (t-t3)/(t5-t3)];
qut ¬ Slerp[qu02, qu13, (t-t3)/(t4-t3)];
};

TangentVector: PUBLIC PROC [quNm1, quN, quNp1: Quaternion] RETURNS [vt: Triple]
~ {
qiN: Quaternion ¬ Conjugate[quN];
bt: Triple ¬ Vector[Ln[Mul[qiN, quNm1]]];
at: Triple ¬ Vector[Ln[Mul[qiN, quNp1]]];
vt ¬ G3dVector.Mul[G3dVector.Sub[at, bt], 0.5];
};

SquadTanFromPoints: PUBLIC PROC [quNm1, quN, quNp1: Quaternion]
RETURNS [quqN: Quaternion]
~ {
qiN: Quaternion ¬ Conjugate[quN];
qDelP: Quaternion ¬ Ln[Mul[qiN, quNp1]];
qDelM: Quaternion ¬ Ln[Mul[qiN, quNm1]];
quqN ¬ Mul[quN, Exp[PureVector[Scale[Add[qDelP, qDelM], -0.25]]]];
};

BezierTanFromPoints: PUBLIC PROC [quNm1, quN, quNp1: Quaternion]
RETURNS [quaN: Quaternion]
~ {
vt: Triple ¬ G3dVector.Div[TangentVector[quNm1, quN, quNp1], 3];
quaN ¬ Mul[quN, Exp[FromVector[vt]]];
};

SquadTanFromVector: PUBLIC PROC [quN, quNp1: Quaternion, vt: Triple]
RETURNS [quqN: Quaternion]
~ {
qDel: Quaternion ¬ Ln[Mul[Conjugate[quN], quNp1]];
quqN ¬ Mul[quN, Exp[PureVector[Scale[Sub[FromVector[vt], qDel], 0.5]]]];
};

BezierTanFromVector: PUBLIC PROC [quN: Quaternion, vt: Triple]
RETURNS [quaN: Quaternion]
~ {
quaN ¬ Mul[quN, Exp[FromVector[G3dVector.Div[vt, 3]]]];
};
ConformQuaternionSequence: PUBLIC PROC [qs: QuaternionSequence] ~ {
FOR i: INT IN [0 .. qs.length-1) DO
IF Dot[qs[i], qs[i+1]] < 0.0 THEN qs[i+1] ¬ Neg[qs[i+1]];
ENDLOOP;
};

CopyQuaternionSequence: PUBLIC PROC [qs: QuaternionSequence]
RETURNS [new: QuaternionSequence] ~ {
copy: QuaternionSequence ¬ NIL;
IF qs # NIL THEN {
copy ¬ NEW[QuaternionSequenceRep[qs.length]];
copy.length ¬ qs.length;
FOR n: NAT IN [0..qs.length) DO copy[n] ¬ qs[n]; ENDLOOP;
};
RETURN[copy];
};

AddToQuaternionSequence: PUBLIC PROC [qs: QuaternionSequence, q: Quaternion]
RETURNS [QuaternionSequence] ~ {
IF qs = NIL THEN qs ¬ NEW[QuaternionSequenceRep[1]];
IF qs.length = qs.maxLength THEN qs ¬ LengthenQuaternionSequence[qs];
qs[qs.length] ¬ q;
qs.length ¬ qs.length+1;
RETURN[qs];
};

LengthenQuaternionSequence: PUBLIC PROC [qs: QuaternionSequence, amount: REAL ¬ 1.3]
RETURNS [new: QuaternionSequence] ~ {
newLength: NAT ¬ MAX[Real.Ceiling[amount*qs.maxLength], 3];
new ¬ NEW[QuaternionSequenceRep[newLength]];
FOR i: NAT IN [0..qs.length) DO new[i] ¬ qs[i]; ENDLOOP;
new.length ¬ qs.length;
};

END.
..
º
G3dQuaternionImpl.mesa
Copyright Ó 1988, 1992 by Xerox Corporation.  All rights reserved.
Ken Shoemake, November 23, 1989 2:52:29 am PST
Bloomenthal, July 14, 1992 3:03 pm PDT
Glassner, July 3, 1990 12:07:41 pm PDT

See  ``Animating Rotation with Quaternion Curves'', SIGGRAPH '85 Proceedings, and ``Quaternion Calculus and Fast Animation,'' in SIGGRAPH '87 tutorial #10.
Left-handed eyespace coordinate system (right, up, depth);
right-handed world coordinate system (longitude, latitude, altitude).

Constant Declarations
Type Declarations
Common Values
Conversion Procedures
Construct a (possibly non-unit) quaternion from real components.
Construct a (possibly non-unit) quaternion from vector (for x, y, z) and real scalar (for w).  Note that FromVectorScalar[v] gives a different scalar component than FromVector[v].
Construct a (possibly non-unit) quaternion from vector (for x, y, z).  Scalar will be 0.
Construct a (possibly non-unit) quaternion from scalar (for w).  Vector will be 0.
Construct a unit quaternion for rotation around unit vector unitAxis by theta radians.
Construct a unit quaternion for rotation (in radians) around body z then y then x axis.
Construct a unit quaternion from rotation matrix.  Assumes matrix is used to multiply row vector on the right: vnew _ vold mat.  Works correctly for right-handed coordinate system and right-handed rotations.  Translation and perspective components ignored.
This algorithm avoids near-zero divides by looking for a large component  first w, then x, y, or z.  When the trace is greater than zero, |w| is greater than 1/2, which is as small as a largest component can be.  Otherwise, the largest diagonal entry corresponds to the largest of |x|, |y|, or |z|, one of which must be larger than |w|, and at least 1/2.
Construct rotation matrix from (possibly non-unit) quaternion.  Assumes matrix is used to multiply row vector on the right: vnew _ vold mat.  Works correctly for right-handed coordinate system and right-handed rotations.
Extract rotation axis of unit quaternion qu as a unit vector, along with rotation angle of qu (in radians).  Note that a rotation with an angle of zero will have an undefined axis.  Note also that [v,q] and [v,q] are the same rotation (opposite to [v,q] and [v,q]).
Describe unit quaternion as rotation (in radians) around body z then y then x axis.
Compatibility Procedures
Construct rotation matrix from (possibly non-unit) quaternion.  Assumes matrix is used to multiply row vector on the right: vnew _ vold mat.  Works correctly for right-handed coordinate system and right-handed rotations.  Use out if non-NIL.
Construct translated rotation matrix from (possibly non-unit) quaternion and base point for rotation.  Assumes matrix is used to multiply row vector on the right: vnew _ vold mat.  Works correctly for right-handed coordinate system and right-handed rotations.  Use out if non-NIL.
Algebraic Procedures
Return quaternion multiplicative identity, gives null rotation.
Return quaternion additive identity, does not give a rotation.
Return components of quaternion.
Return vector part of quaternion.
Return scalar part of quaternion.
Return quaternion with scalar component zero.  Can use as cheap cleanup for Ln, since PureVector[Ln[q]] = Ln[Unit[q]].
Return norm of quaternion; that is, the sum of the squares of components.
Return negated quaternion; that is, its additive inverse.
Return conjugate of quaternion.
Return inverse of quaternion; that is, its multiplicative inverse.
Return reverse of quaternion as a rotation; marginally cheaper than conjugate.
Return normalized quaternion; that is, make sum of squares equal 1.
Return quaternion sum qL + qR.
Return quaternion difference qL - qR.
Return quaternion product qL * qR.  Note: order is important!  To combine rotations, use the product Mul[qSecond, qFirst], in contrast to G3dMatrix.Mul[mFirst, mSecond].
Both qL and qR are pure vectors.  Save 7 out of 16 multiplies.
Only qL is a pure vector.  Save 4 out of 16 multiplies.
Only qR is a pure vector.  Save 4 out of 16 multiplies.
Neither qL nor qR are pure vectors.  Do all 16 multiplies.
Return product of quaternion q by scalar w.
Return exponentiated quaternion; that is, S qn/n!.
Return the natural logarithm of quaternion, inverse function of Exp.
Return v rotated by unit quaternion qu.  Faster for single vector than making qu into matrix.
Return the 4-D dot product of quaternions qa and qb.
Return the distance between quaternions qa and qb in the L1/p norm, the p-th power of the sum of the p-th roots of the absolute values of the component differences.  Only cheap if p=0, 1/2, or 1, corresponding to maximum, Euclidean, and taxicab distances.
Curve Procedures
Spherical linear interpolation of unit quaternions.  Takes qu0 to qu1 as t goes from 0 to 1.
Fast Slerp for t = 1/2.
Fast Slerp for t = 2.
Spherical quadrangle curve from qu0 to qu1, for t IN [0..1], with tangents manipulated by quadrangle points quq0 and quq1.  All quaternions are assumed to be of unit norm.
Spherical cubic Catmull-Rom curve for control points qu0 to qu3, for t IN [0..1].  All quaternions are assumed to be of unit norm.
Spherical cubic Bezier curve from qu0 to qu1, for t IN [0..1], with tangents manipulated by Bezier points qua0 and qub1.  All quaternions are assumed to be of unit norm.
Spherical cubic B-Spline curve for control points qu0 to qu3 and knots t1 to t6, t IN [t3..t4].  Must have t1 < t2 < t3 < t4 < t5 < t6.  All quaternions are assumed to be of unit norm.
Given unit quaternions quNm1, quN, and quNp1in that orderestimate the tangent at quN for a smooth curve passing through all three.
Use "average of central differences" idea, averaging tangents of incoming and outgoing arcs.
Given unit quaternions quNm1, quN, and quNp1in that orderestimate the tangent at quN for a smooth curve passing through all three.  Compute quandrangle point quqN for Squad.
Given unit quaternions quNm1, quN, and quNp1in that orderestimate the tangent at quN for a smooth curve passing through all three.  Compute Bezier point quaN for Bezier.
Given unit quaternions quN and quNp1in that orderand a tangent vector vt at quN, compute quandrangle point quqN for Squad.
Given unit quaternion quN, and a tangent vector vt at quN, compute Bezier point quaN for Bezier.
Sequence Procedures
�ʪ–
"cedarcode" style•NewlineDelimiter

™™Jšœ
Ïe
œ7™BJš
œ.™.J™&™&J™—šœ4ÏzœO™›
J™:J™E—J™—š
Ïk	˜	J˜CJ˜—šÐblœŸœ
Ÿ˜ JšŸœ#œ
˜+JšŸœ˜J˜—JšœŸ˜
headš
Ïl™Jš	œŸœ
ŸœŸœŸœ
˜/—š
¡™JšœŸœ˜Jšœ
Ÿœ˜!JšœŸœ˜Jšœ
Ÿœ˜"JšœŸœ˜'Jšœ
Ÿœ˜-JšœŸœ˜+JšœŸœ$˜<JšœŸœ'˜B—š
¡
™
˜"J˜—JšœŸœ'˜4JšœŸœ'˜2JšœŸœ'˜2JšœŸœ'˜2JšœŸœ'˜2—š
¡™š
ÏnœŸœ
ŸœŸœŸœ˜GJ™@J˜JšŸœ˜!J˜J˜—š
¢œŸœ
ŸœŸœŸœ˜QJš
œ³
™³
J˜JšŸœ!˜'J˜J˜—š¢
œŸœ
Ÿœ
Ÿœ˜<Jš
œX™XJ˜JšŸœ!˜'J˜J˜—š
¢
œŸœ
ŸœŸœŸœ˜@Jš
œR™RJ˜JšŸœ˜!J˜J˜—š¢
œŸœ
ŸœŸ
œŸœ
Ÿœ˜SJš
œV™VJ˜JšœŸœ˜)JšœŸœ˜,JšœŸœ˜,J˜KJ˜J˜—š
¢
œŸœ
ŸœŸœ
Ÿœ˜LJš
œW™WJ˜JšœŸœ
Ÿ
œ˜$JšœŸœ
Ÿ
œ˜$JšœŸœ
Ÿ
œ˜$Jšœ
Ÿœ˜$Jšœ
Ÿœ˜$Jšœ
Ÿœ˜$Jšœ
Ÿœ˜$Jšœ
Ÿœ˜$Jšœ
Ÿœ˜$J˜?J˜?J˜?J˜?J˜J˜—š¢
œŸœ
ŸœŸœ˜>Jš	œoÏb
Ïdœ£
¤œ
£œ‚
™€J˜Jš
œã™ãJš	œŸœ
Ÿœ
Ÿœ
Ÿœ˜2JšœŸœ
˜J˜J˜&šŸœ
˜šŸœ˜J˜J˜J˜J˜#J˜#J˜#J˜
—šŸœ˜JšœŸœŸœ
Ÿœ
˜$JšœŸœ
˜JšœŸœ
˜
JšœŸœ˜JšŸœŸœ˜$JšŸœŸœ˜$J˜J˜>J˜J˜J˜!J˜!J˜#šŸœŸ˜
J˜-J˜-J˜-JšŸœ
˜—J˜——J˜J˜—š¢œŸœ
ŸœŸ
œŸœ
Ÿœ	˜IJš	œ|£
¤œ£
¤œ
£œQ™Ü
J˜JšœŸœ
˜Jšœ'Ÿœ
˜,JšœŸœ˜JšŸœŸœ
Ÿœ˜3J˜J–
4 sp tabStops˜%J–
4 sp tabStops˜'J–
4 sp tabStops˜(J–
4 sp tabStops˜(JšŸœŸœ
ŸœŸœ˜'˜J–
4 sp tabStops˜-J–
4 sp tabStops˜-J–
4 sp tabStops˜-J–
4 sp tabStops˜ —JšŸœ˜J˜J˜—š¢œŸœ
ŸœŸœ˜HJš	œÈ
Ïg
ϴ
œ)¥
œ
¥
œ™�J˜J˜0J˜7J˜J˜—š¢œŸœ
ŸœŸœ˜EJš
œS™SJš
œ˜JšŸœ(˜.J˜——š
¡™š¢œŸœ
ŸœŸ
œŸœ
Ÿœ	˜LJš	œ|£
¤œ£
¤œ
£œf™ñ
Jš
œ˜J˜—š¢œŸœ
ŸœŸ
œŸ
œŸœ
˜WJšŸœ	˜Jš	œ£
£
¤œ£
¤œ
£œf™˜J˜J˜J˜DJ˜*JšŸœ˜J˜——š
¡™š¢œŸœ
Ÿœ
Ÿœ˜(J™?J˜J˜	J˜J˜—š¢œŸœ
Ÿœ
Ÿœ˜*J™>J˜J˜J˜J˜—š¢
œŸœ
ŸœŸœ˜6Jš
œ ™ J˜JšŸœ˜J˜J˜—š¢œŸœ
ŸœŸœ	˜4J™!J˜JšŸœ˜J˜J˜—š
¢œŸœ
ŸœŸœŸœ
˜2Jš
œ!™!J˜JšŸœ˜J˜J˜—š¢
œŸœ
ŸœŸœ˜@J™vJ˜J˜$J˜J˜—š
¢œŸœ
ŸœŸœŸœ
˜0J™IJ˜JšŸœ(˜.J˜J˜—š¢œŸœ
ŸœŸœ˜9J™9J˜J˜*J˜J˜—š¢	œŸœ
ŸœŸœ˜?J™J˜J˜)J˜J˜—š¢œŸœ
ŸœŸœ˜=J™BJ˜Jšœ	Ÿœ˜J˜"J˜J˜—š¢œŸœ
ŸœŸœ˜=J™NJ˜J˜'J˜J˜—š¢œŸœ
ŸœŸœ˜:Jš
œC™CJ˜Jšœ	Ÿœ˜,J˜J˜J˜—š¢œŸœ
ŸœŸœ˜>J™J˜J˜J˜J˜J˜J˜J˜—š¢œŸœ
ŸœŸœ˜>J™%J˜J˜J˜J˜J˜J˜J˜—š¢œŸœ
ŸœŸœ˜>J™©
J˜šŸœ˜
šŸœ˜šŸœ˜
šŸœ˜J™>J˜+J˜J˜J˜J˜
—šŸœ˜J™7J˜+J˜)J˜)J˜)J˜——J˜
—šŸœ˜šŸœ˜
šŸœ˜J™7J˜+J˜)J˜)J˜)J˜
—šŸœ˜J™:J˜5J˜5J˜5J˜5J˜——J˜——J˜J˜—š
¢œŸœ
ŸœŸœŸœ˜DJ™+J˜J˜
J˜
J˜
J˜
J˜J˜—š¢œŸœ
ŸœŸœ˜9Jšœ*¥
œÏu
œ™2J˜JšœŸœ˜*Jš	œŸœŸœŸœŸœ˜GJš	œŸœŸœŸœŸœ˜8J˜SJ˜J˜—š¢œŸœ
ŸœŸœ˜8J™DJ˜JšœŸœ%˜.JšœŸœ˜*JšœŸœ˜)JšŸœ
Ÿœ˜(J˜<Jš
œ˜J˜—š¢œŸœ
ŸœŸœ
˜AJ™]J˜J˜J˜%J˜J˜0J˜1J˜J˜—š
¢œŸœ
ŸœŸœŸœ
˜4J™4J˜JšŸœ1˜7J˜J˜—š¢œŸœ
ŸœŸœŸœŸœ
˜HJšœ:Ðdo
¤œÂ
™ÿ
J˜JšœŸœŸœ˜JšœŸœŸœ˜JšœŸœŸœ˜JšœŸœŸœ˜šŸœŸ˜
JšœŸœ
Ÿœ˜#JšœŸœ˜JšœŸœ(˜5šŸœ˜JšœŸœ˜JšŸœy˜——J˜——š
¡™š
¢œŸœ
ŸœŸœ
Ÿœ˜LJ™\J˜Jšœ-Ÿœ
˜2Jšœ	Ÿœ
˜JšŸœŸœ˜J˜J˜šŸœ˜šŸœÏc
˜šŸœ˜šŸœ¨
˜J˜!J˜J˜2J˜*J˜
—šŸœ¨˜J˜J˜J˜——J˜4J˜
—šŸœ¨˜J˜=Jšœ"Ÿœ˜&JšœŸœ˜J˜4J˜——Jš
œ˜J˜—š¢œŸœ
ŸœŸœ˜CJ™J˜˜JšœŸ
œ0Ÿ	œ˜U—J˜J˜—š¢œŸœ
ŸœŸœ˜CJ™J˜J˜+J˜J˜—š
¢œŸœ
Ÿœ'Ÿœ
Ÿœ˜XJšœ2Ÿœw™«
J˜J˜&J˜)J˜*J˜J˜—š¢
œŸœ
Ÿœ%Ÿœ
˜AJšŸœ˜JšœGŸœ9™‚
J˜J˜(J˜&J˜(J˜.J˜*J˜J˜J˜—š
¢œŸœ
Ÿœ'Ÿœ
Ÿœ˜YJšœ4Ÿœs™©
J˜J˜(J˜)J˜(J˜,J˜,J˜J˜J˜—š¢œŸœ
Ÿœ=Ÿœ
˜VJšŸœ˜JšœSŸœÏm
ϩ
œ	©
ϩ
œ5™¸
J˜J˜3J˜3J˜3J˜5J˜5J˜(J˜J˜—š¢
œŸœ
Ÿœ!Ÿœ
˜OJš
œ„
™„
J˜J™\J˜!J˜)J˜)J˜/J˜J˜—š¢œŸœ
Ÿœ ˜?JšŸœ˜Jš
œ¯
™¯
J˜J˜!J˜(J˜(J˜BJ˜J˜—š¢œŸœ
Ÿœ ˜@JšŸœ˜Jš
œ«
™«
J˜J˜@J˜%J˜J˜—š¢œŸœ
Ÿœ%˜DJšŸœ˜Jš
œ|™|J˜J˜2J˜HJ˜J˜—š¢œŸœ
Ÿœ˜>JšŸœ˜Jš
œ`™`J˜J˜7J˜——š
¡™š¢œŸœ
Ÿœ˜CšŸœŸœ
ŸœŸ˜#JšŸœŸœ˜9JšŸœ
˜—J˜J˜—š¢œŸœ˜<JšŸœ˜%JšœŸœ
˜šŸœŸœ
Ÿœ˜JšœŸœ#˜-J˜Jš
ŸœŸœ
ŸœŸœŸœ
˜9J˜—JšŸœ˜
J˜J˜—š¢œŸœ(˜LJšŸœ˜ JšŸœŸœ
ŸœŸœ˜4JšŸœŸœ%˜EJ˜J˜JšŸœ˜J˜J˜—š¢œŸœ"Ÿœ˜TJšŸœ˜%JšœŸœŸœ'˜;JšœŸœ#˜,Jš
ŸœŸœ
ŸœŸœŸœ
˜8J˜J˜——J˜JšŸœ
˜J˜—…—3–\ú