<<>> <> <> <> <> <> <<>> <> <> <> <<>> 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. ..