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