<> <> <> DIRECTORY RealFns USING [CosDeg, SinDeg, SqRt], Vector2 USING [VEC], Linear3d USING [Triple, Quad, Xfm3d]; Linear3dImpl: CEDAR PROGRAM IMPORTS RealFns EXPORTS Linear3d ~ BEGIN Basic3dError: PUBLIC SIGNAL [reason: ATOM] = CODE; <> Pair: TYPE ~ Vector2.VEC; -- RECORD [ x, y: REAL]; Triple: TYPE ~ Linear3d.Triple; -- RECORD [ x, y, z: REAL]; Quad: TYPE ~ Linear3d.Quad; -- RECORD [ x, y, z, w: REAL]; Xfm3d: TYPE ~ Linear3d.Xfm3d; -- RECORD [ a, b, c, d: Quad]; IdentityXfm: Xfm3d ~ [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]; <> Sqr: PROCEDURE [number: REAL] RETURNS [REAL] ~ INLINE { RETURN[number * number]; }; <> SumTriple: PUBLIC PROCEDURE [t1, t2: Triple] RETURNS [Triple] ~ { RETURN[ [t1.x + t2.x, t1.y + t2.y, t1.z + t2.z] ]; }; DiffTriple: PUBLIC PROCEDURE [t1, t2: Triple] RETURNS [Triple] ~ { RETURN[ [t1.x - t2.x, t1.y - t2.y, t1.z - t2.z] ]; }; ScaleTriple: PUBLIC PROCEDURE [t: Triple, s: REAL] RETURNS [Triple] ~ { RETURN[ [t.x * s, t.y * s, t.z * s] ]; }; Magnitude: PUBLIC PROC[v: Triple] RETURNS[REAL] ~ { RETURN [ RealFns.SqRt[v.x * v.x + v.y * v.y + v.z * v.z] ]; }; Normalize: PUBLIC PROC[v: Triple] RETURNS[Triple] ~ { mag: REAL _ RealFns.SqRt[v.x * v.x + v.y * v.y + v.z * v.z]; IF mag <= 0. THEN mag _ 1.; RETURN [ [v.x/mag, v.y/mag, v.z/mag] ]; }; DotProd: PUBLIC PROC[v1, v2: Triple] RETURNS [REAL] ~ { RETURN [v1.x * v2.x + v1.y * v2.y + v1.z * v2.z]; }; CrossProd: PUBLIC PROC[v1, v2: Triple] RETURNS [vOut: Triple] ~ { vOut.x _ v1.y*v2.z - v1.z*v2.y; vOut.y _ v1.z*v2.x - v1.x*v2.z; vOut.z _ v1.x*v2.y - v1.y*v2.x; }; EvalPlane: PUBLIC PROC[plane: Quad, point: Triple] RETURNS [distance: REAL]~ { distance _ plane.x*point.x + plane.y*point.y + plane.z*point.z + plane.w; }; Transform3d: PUBLIC PROC[ vtx: Triple, xfm: Xfm3d] RETURNS [t: Triple] ~ { t.x _ xfm.a.x * vtx.x + xfm.b.x * vtx.y + xfm.c.x * vtx.z + xfm.d.x; t.y _ xfm.a.y * vtx.x + xfm.b.y * vtx.y + xfm.c.y * vtx.z + xfm.d.y; t.z _ xfm.a.z * vtx.x + xfm.b.z * vtx.y + xfm.c.z * vtx.z + xfm.d.z; }; TransformVec3d: PUBLIC PROC[ vtx: Triple, xfm: Xfm3d] RETURNS [t: Triple] ~ { t.x _ xfm.a.x * vtx.x + xfm.b.x * vtx.y + xfm.c.x * vtx.z; t.y _ xfm.a.y * vtx.x + xfm.b.y * vtx.y + xfm.c.y * vtx.z; t.z _ xfm.a.z * vtx.x + xfm.b.z * vtx.y + xfm.c.z * vtx.z; }; <> ConcatT: PUBLIC PROC[xfm1, xfm2: Xfm3d] RETURNS[xfmOut: Xfm3d] ~{ <> QuadProd: PROC[q1, q2: Quad] RETURNS [REAL] ~ { RETURN [q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w]; }; col2x, col2y, col2z, col2w: Quad; col2x _ [xfm2.a.x, xfm2.b.x, xfm2.c.x, xfm2.d.x]; col2y _ [xfm2.a.y, xfm2.b.y, xfm2.c.y, xfm2.d.y]; col2z _ [xfm2.a.z, xfm2.b.z, xfm2.c.z, xfm2.d.z]; col2w _ [xfm2.a.w, xfm2.b.w, xfm2.c.w, xfm2.d.w]; xfmOut.a.x _ QuadProd[xfm1.a, col2x]; xfmOut.a.y _ QuadProd[xfm1.a, col2y]; xfmOut.a.z _ QuadProd[xfm1.a, col2z]; xfmOut.a.w _ QuadProd[xfm1.a, col2w]; xfmOut.b.x _ QuadProd[xfm1.b, col2x]; xfmOut.b.y _ QuadProd[xfm1.b, col2y]; xfmOut.b.z _ QuadProd[xfm1.b, col2z]; xfmOut.b.w _ QuadProd[xfm1.b, col2w]; xfmOut.c.x _ QuadProd[xfm1.c, col2x]; xfmOut.c.y _ QuadProd[xfm1.c, col2y]; xfmOut.c.z _ QuadProd[xfm1.c, col2z]; xfmOut.c.w _ QuadProd[xfm1.c, col2w]; xfmOut.d.x _ QuadProd[xfm1.d, col2x]; xfmOut.d.y _ QuadProd[xfm1.d, col2y]; xfmOut.d.z _ QuadProd[xfm1.d, col2z]; xfmOut.d.w _ QuadProd[xfm1.d, col2w]; }; Translate3d: PUBLIC PROC[ delta: Triple] RETURNS [xfm: Xfm3d] ~ { xfm _ IdentityXfm; xfm.d _ [delta.x, delta.y, delta.z, 1.]; }; Rotate3d: PUBLIC PROC[base, axis: Triple, theta: REAL] RETURNS[Xfm3d] ~ { xfm, mtx: Xfm3d _ IdentityXfm; cosTheta: REAL _ RealFns.CosDeg[theta]; sinTheta: REAL _ RealFns.SinDeg[theta]; xfm.d.x _ -base.x; xfm.d.y _ -base.y; xfm.d.z _ -base.z; axis _ Normalize[axis]; mtx.a.x _ Sqr[axis.x] + (1.0 - Sqr[axis.x]) * cosTheta; mtx.b.y _ Sqr[axis.y] + (1.0 - Sqr[axis.y]) * cosTheta; mtx.c.z _ Sqr[axis.z] + (1.0 - Sqr[axis.z]) * cosTheta; mtx.b.x _ axis.x * axis.y * (1.0 - cosTheta) + axis.z * sinTheta; mtx.a.y _ axis.x * axis.y * (1.0 - cosTheta) - axis.z * sinTheta; mtx.c.x _ axis.x * axis.z * (1.0 - cosTheta) - axis.y * sinTheta; mtx.a.z _ axis.x * axis.z * (1.0 - cosTheta) + axis.y * sinTheta; mtx.c.y _ axis.y * axis.z * (1.0 - cosTheta) + axis.x * sinTheta; mtx.b.z _ axis.y * axis.z * (1.0 - cosTheta) - axis.x * sinTheta; xfm _ ConcatT[xfm, mtx]; mtx _ IdentityXfm; mtx.d.x _ base.x; mtx.d.y _ base.y; mtx.d.z _ base.z; xfm _ ConcatT[xfm, mtx]; RETURN[xfm]; }; END.