-- File: CoordSysImpl.mesa -- Last edited by Bier on January 12, 1983 3:15 pm -- Author: Eric Bier in the summer of 1982 -- Contents: Allocation and access to a user-specified set of named coordinate systems DIRECTORY CoordSys, Matrix3d, PriorityQueue, Rope, SVVector3d; CoordSysImpl: PROGRAM IMPORTS Matrix3d, PriorityQueue, Rope, SVVector3d EXPORTS CoordSys = { Matrix4by4: TYPE = Matrix3d.Matrix4by4; Point2d: TYPE = Matrix3d.Point2d; CoordSystem: TYPE = REF CoordSysObj; CoordSysObj: TYPE = CoordSys.CoordSysObj; CoordSysList: TYPE = CoordSys.CoordSysList; Point3d: TYPE = Matrix3d.Point3d; Vector: TYPE = SVVector3d.Vector; InitialCoordSysList: PUBLIC PROC RETURNS [csl: CoordSysList] = { worldCoordSys: CoordSystem _ CreateCoordSys["WORLD",Matrix3d.Identity[],NIL]; screenCoordSys: CoordSystem _ CreateCoordSys["SCREEN",Matrix3d.MakeTranslateMat[-200,-200,0], NIL]; csl _ CONS[worldCoordSys,CONS[screenCoordSys,NIL]]; }; CoordSysSortRec: TYPE = REF CoordSysSortRecObj; CoordSysSortRecObj: TYPE = RECORD[ index: NAT, coordSysCell: CoordSysList]; SortProc: SAFE PROC [x, y: PriorityQueue.Item, data: REF _ NIL] RETURNS [BOOL] = TRUSTED { xRec: CoordSysSortRec _ NARROW[x]; yRec: CoordSysSortRec _ NARROW[y]; RETURN[xRec.index > yRec.index]; }; ListLength: PRIVATE PROC [csl: CoordSysList] RETURNS [len: NAT] = { len _ 0; FOR list: CoordSysList _ csl, list.rest UNTIL list = NIL DO len _ len + 1; ENDLOOP; }; Depth: PRIVATE PROC [cs: CoordSystem] RETURNS [index: NAT] = { -- index is distance from WORLD coordsys thisCS: CoordSystem; index _ 0; thisCS _ cs; UNTIL thisCS = NIL DO index _ index + 1; thisCS _ thisCS.withRespectTo; ENDLOOP; }; SortCoordSysListByBreadth: PUBLIC PROC [csl: CoordSysList] RETURNS [sortedCsl: CoordSysList] = { -- walking the coord sys tree is hard since all of the pointers are back pointers. We associate a tree depth with each coordsystem and then sort these depth numbers. q: PriorityQueue.Ref; thisRec, lastRec: CoordSysSortRec; len: NAT _ ListLength[csl]; q _ PriorityQueue.Predict[len, SortProc]; FOR list: CoordSysList _ csl, list.rest UNTIL list = NIL DO thisRec _ NEW[CoordSysSortRecObj _ [Depth[list.first], list]]; PriorityQueue.Insert[q, thisRec]; ENDLOOP; -- we sort deep to shallow so we can put the list together backwards lastRec _ NARROW[PriorityQueue.Remove[q]]; lastRec.coordSysCell.rest _ NIL; FOR i: NAT IN [1..len-1] DO thisRec _ NARROW[PriorityQueue.Remove[q]]; thisRec.coordSysCell.rest _ lastRec.coordSysCell; lastRec _ thisRec; ENDLOOP; sortedCsl _ thisRec.coordSysCell; }; -- end of SortCoordSysListByBreadth CreateCoordSys: PUBLIC PROC [name: Rope.ROPE, mat: Matrix4by4, withRespectTo: CoordSystem] RETURNS [newCS: CoordSystem] = { -- check that mat has no scaling components x, y, z: REAL; almostZero: REAL _ 1.0e-4; -- 1.0e-7 is too small. Wow, subtraction has real floating point fuzz [x, y, z] _ Matrix3d.ScaleFromMatrix[mat]; IF ABS[x - 1.0] > almostZero OR ABS[y-1.0] > almostZero OR ABS[z - 1.0] > almostZero THEN -- SIGNAL AttemptToCreateCoordSysWithScaling-- {}; newCS _ NEW[CoordSysObj _ [name: name, mat: mat, wrtCamera: Matrix3d.Identity[], wrtWorld: Matrix3d.Identity[], cameraWRTlocal: Matrix3d.Identity[], worldWRTlocal: Matrix3d.Identity[], withRespectTo: withRespectTo]]; }; AttemptToCreateCoordSysWithScaling: PUBLIC SIGNAL = CODE; FindCoordSysFromName: PUBLIC PROC [name: Rope.ROPE, csl: CoordSysList] RETURNS [cs: CoordSystem] = { l: CoordSysList _ csl; IF l = NIL THEN ERROR CoordSysListEmpty; UNTIL l = NIL DO IF Rope.Equal[l.first.name, name] THEN BEGIN cs _ l.first; RETURN END; l _ l.rest; ENDLOOP; SIGNAL CoordSysNotFound; }; FindCoordSysAndNeighborsFromName: PUBLIC PROC [name: Rope.ROPE, csl: CoordSysList] RETURNS [beforeCS, cs, afterCS: CoordSysList] = { -- signals CoordSysNotFound if that is the case. lastL: CoordSysList _ NIL; l: CoordSysList _ csl; IF l = NIL THEN ERROR CoordSysNotFound; UNTIL l = NIL DO IF Rope.Equal[l.first.name, name] THEN { beforeCS _ lastL; cs _ l; afterCS _ l.rest; RETURN}; lastL _ l; l _ l.rest; ENDLOOP; SIGNAL CoordSysNotFound; }; CameraToScreen: PUBLIC PROC [cameraPoint2d: Point2d, screenCoordSys: CoordSystem] RETURNS [screenPoint2d: Point2d] = { screenPoint2d[1] _ cameraPoint2d[1] - screenCoordSys.mat[1][4]; -- inverse of screenWRTCamera screenPoint2d[2] _ cameraPoint2d[2] - screenCoordSys.mat[2][4]; }; ScreenToCamera: PUBLIC PROC [screenPoint2d: Point2d, screenCoordSys: CoordSystem] RETURNS [cameraPoint2d: Point2d] = { cameraPoint2d[1] _ screenPoint2d[1] + screenCoordSys.mat[1][4]; -- screenWRTCamera cameraPoint2d[2] _ screenPoint2d[2] + screenCoordSys.mat[2][4]; }; CoordSysNotFound: PUBLIC SIGNAL = CODE; CoordSysListEmpty: PUBLIC SIGNAL = CODE; GetGenerator: PUBLIC PROC [csl: CoordSysList] RETURNS [g: Generator] = { g _ NEW[GeneratorObj _ [csl]]; }; Generator: TYPE = REF GeneratorObj; GeneratorObj: TYPE = CoordSys.GeneratorObj; Next: PUBLIC PROC [g: Generator] RETURNS [cs: CoordSystem] = { IF g.currentPtr = NIL THEN RETURN[NIL]; cs _ g.currentPtr.first; g.currentPtr _ g.currentPtr.rest; }; FindInTermsOfWorld: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = { thisCS, nextCS: CoordSystem; thisCS _ cs; mat _ cs.mat; UNTIL thisCS.withRespectTo = NIL DO nextCS _ thisCS.withRespectTo; mat _ Matrix3d.MatMult[nextCS.mat, mat]; thisCS _ nextCS; ENDLOOP; }; TellAboutParent: PUBLIC PROC [cs: CoordSystem] = { -- updates the wrtWorld, and wrtCamera fields of cs by premultiplying cs.mat by cs.withRespectTo.wrtWorld and cs.withRespectTo.wrtCamera respectively cs.wrtWorld _ Matrix3d.MatMult[cs.withRespectTo.wrtWorld, cs.mat]; cs.wrtCamera _ Matrix3d.MatMult[cs.withRespectTo.wrtCamera, cs.mat]; cs.worldWRTlocal _ Matrix3d.Inverse[cs.wrtWorld]; cs.cameraWRTlocal _ Matrix3d.Inverse[cs.wrtCamera]; }; FindInTermsOfCamera: PUBLIC PROC [cs: CoordSystem, camera: CoordSystem] RETURNS [mat: Matrix4by4] = { inWorld: Matrix4by4 _ FindInTermsOfWorld[cs]; cameraWRTWorld: Matrix4by4 _ FindInTermsOfWorld[camera]; mat _ Matrix3d.WorldToLocal[cameraWRTWorld,inWorld]; }; FindAinTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [aInTermsOfb: Matrix4by4] = { aWorld, bWorld: Matrix4by4; aWorld _ FindInTermsOfWorld[a]; bWorld _ FindInTermsOfWorld[b]; aInTermsOfb _ Matrix3d.WorldToLocal[bWorld,aWorld]; }; FindTranslationOfAinTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [displacements: Vector] = { aInTermsOfb: Matrix4by4 _ FindAinTermsOfB[a,b]; displacements _ Matrix3d.OriginOfMatrix[aInTermsOfb]; }; TPutAinTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [aInTermsOfb: Matrix4by4] = { aWorld, bWorld: Matrix4by4; aWorld _ FindInTermsOfWorld[a]; bWorld _ FindInTermsOfWorld[b]; aInTermsOfb _ Matrix3d.WorldToLocal[bWorld,aWorld]; a.withRespectTo _ b; a.mat _ aInTermsOfb; }; TPlaceAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem _ NIL, aWRTb: Matrix4by4] = { aWorld, bWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF Rope.Equal[a.name, "WORLD", TRUE] THEN ERROR; -- cannot translate WORLD for now IF Rope.Equal[a.name, "SCREEN", TRUE] THEN {a.mat _ aWRTb; RETURN}; IF b = NIL THEN ERROR -- b cannot float since a will now depend on it ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ Matrix3d.MatMult[bWorld,aWRTb]; withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, aWorld]; a.mat _ aInTermsOfWRT; }; TPlaceTranslationAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem _ NIL, origin: Point3d] = { -- places a in WORLD so that the rotation from a to b remains as it is, but the translation is set to origin. Updates a with respect to its immediate reference (a.withRespectTo) accordingly. aWRTb: Matrix4by4 _ FindAinTermsOfB[a, b]; aWorld, bWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF Rope.Equal[a.name, "WORLD", TRUE] THEN ERROR; -- cannot translate WORLD for now IF Rope.Equal[a.name, "SCREEN", TRUE] THEN {a.mat _ aWRTb; RETURN}; IF b = NIL THEN ERROR -- b cannot float since a will now depend on it ELSE bWorld _ FindInTermsOfWorld[b]; aWRTb[1][4] _ origin[1]; aWRTb[2][4] _ origin[2]; aWRTb[3][4] _ origin[3]; aWorld _ Matrix3d.MatMult[bWorld,aWRTb]; withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, aWorld]; a.mat _ aInTermsOfWRT; }; TTranslateAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem, tx, ty, tz: REAL] = { aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.Translate[aInTermsOfb, tx, ty, tz]; -- must be a global translate. newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TXRotateAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem, degrees: REAL] = { aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.RotateAboutXAxis[aInTermsOfb, degrees]; -- must be a global translate. newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TYRotateAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem, degrees: REAL] = { aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.RotateAboutYAxis[aInTermsOfb, degrees]; -- must be a global translate. newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TZRotateAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem, degrees: REAL] = { aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.RotateAboutZAxis[aInTermsOfb, degrees]; -- must be a global translate. newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TScaleAwrtB: PRIVATE PROC [a: CoordSystem, b: CoordSystem, sx, sy, sz: REAL] = { -- because scaling is only allowed at primitive assemblies, we define the arbitrary scaling of an assembly with respect to a coordinate system as follows: If the assembly is a primitive, change its scalar vector aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.Scale[aInTermsOfb, sx, sy, sz]; -- must be a global translate. newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; IndicesOfMax: PRIVATE PROC [v: Vector] RETURNS [indices: ARRAY[1..3] OF NAT, negArray: ARRAY[1..3] OF BOOL] = { -- bubble sort temp: REAL; tempIndex: NAT; IF v[1] < 0 THEN {negArray[1] _ TRUE; v[1] _ -v[1]} ELSE negArray[1] _ FALSE; IF v[2] < 0 THEN {negArray[2] _ TRUE; v[2] _ -v[2]} ELSE negArray[2] _ FALSE; IF v[3] < 0 THEN {negArray[3] _ TRUE; v[3] _ -v[3]} ELSE negArray[3] _ FALSE; indices _ [1,2,3]; IF v[1] < v[2] THEN {temp _ v[1]; v[1] _ v[2]; v[2] _ temp; tempIndex _ indices[1]; indices[1] _ indices[2]; indices[2] _ tempIndex}; IF v[2] < v[3] THEN {temp _ v[2]; v[2] _ v[3]; v[3] _ temp; tempIndex _ indices[2]; indices[2] _ indices[3]; indices[3] _ tempIndex}; IF v[1] < v[2] THEN {temp _ v[1]; v[1] _ v[2]; v[2] _ temp; tempIndex _ indices[1]; indices[1] _ indices[2]; indices[2] _ tempIndex}; }; TAlignAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem _ NIL] = { -- Find AwrtB. Compute the magnitude of its axes. Make AwrtB have aligned axes of the same magnitude. aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; xAxisChoices, zAxisChoices: ARRAY[1..3] OF NAT; xAxis, zAxis, newXAxis, newYAxis, newZAxis: Vector; xNegArray, zNegArray: ARRAY[1..3] OF BOOL; origin: Point3d; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; xAxis _ Matrix3d.XAxisOfMatrix[aInTermsOfb]; zAxis _ Matrix3d.ZAxisOfMatrix[aInTermsOfb]; origin _ Matrix3d.OriginOfMatrix[aInTermsOfb]; [xAxisChoices, xNegArray] _ IndicesOfMax[xAxis]; -- Find largest component -- this direction is now taken. z axis must find another. [zAxisChoices, zNegArray] _ IndicesOfMax[zAxis]; IF zAxisChoices[1] = xAxisChoices[1] THEN zAxisChoices[1] _ zAxisChoices[2]; -- zAxis gets second choice if xAxis has its first choice newXAxis _ [0,0,0]; newXAxis[xAxisChoices[1]] _ SVVector3d.Magnitude[xAxis]; IF xNegArray[xAxisChoices[1]] THEN newXAxis[xAxisChoices[1]] _ -newXAxis[xAxisChoices[1]]; newZAxis _ [0,0,0]; newZAxis[zAxisChoices[1]] _ SVVector3d.Magnitude[zAxis]; IF zNegArray[zAxisChoices[1]] THEN newZAxis[zAxisChoices[1]] _ -newZAxis[zAxisChoices[1]]; newYAxis _ SVVector3d.CrossProduct[newZAxis, newXAxis]; aInTermsOfb _ Matrix3d.MakeMatFromAxes[newXAxis, newYAxis, newZAxis, origin]; newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TAbutAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] = { -- Find AwrtB. Compute the magnitude of its axes. Make AwrtB have aligned axes of the same magnitude. aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb _ Matrix3d.NormalizeNoTranslation[aInTermsOfb]; newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TAbutXAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] = { -- Find AwrtB. Compute the magnitude of its axes. Make AwrtB have aligned axes of the same magnitude. aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb[1][4] _ 0; newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TAbutYAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] = { -- Find AwrtB. Compute the magnitude of its axes. Make AwrtB have aligned axes of the same magnitude. aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb[2][4] _ 0; newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; TAbutZAwrtB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] = { -- Find AwrtB. Compute the magnitude of its axes. Make AwrtB have aligned axes of the same magnitude. aWorld, bWorld, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix4by4; withRespectTo: CoordSystem; IF b = NIL THEN ERROR ELSE bWorld _ FindInTermsOfWorld[b]; aWorld _ FindInTermsOfWorld[a]; aInTermsOfb _ IF a = b THEN Matrix3d.Identity[] ELSE Matrix3d.WorldToLocal[bWorld,aWorld]; aInTermsOfb[3][4] _ 0; newAWorld _ Matrix3d.MatMult[bWorld, aInTermsOfb]; IF a.withRespectTo = NIL THEN ERROR; -- cannot translate WORLD for now withRespectTo _ a.withRespectTo; withRespectToWorld _ FindInTermsOfWorld[withRespectTo]; aInTermsOfWRT _ Matrix3d.WorldToLocal[withRespectToWorld, newAWorld]; a.mat _ aInTermsOfWRT; }; -- for local transforms, let b = a. }.