File: CoordSysImpl.mesa
Last edited by Bier on January 3, 1985 11:37:55 pm PST
Author: Eric Bier in the summer of 1982
Contents: Allocation and access to a user-specified set of named coordinate systems
DIRECTORY
CoordSys,
IO,
Matrix3d,
Rope,
SV2d,
SV3d,
SVModelTypes;
CoordSysImpl: PROGRAM
IMPORTS Matrix3d, IO, Rope
EXPORTS CoordSys =
BEGIN
CoordSystem: TYPE = REF CoordSysObj;
CoordSysObj: TYPE = SVModelTypes.CoordSysObj;
CoordSysList: TYPE = SVModelTypes.CoordSysList;
Matrix4by4: TYPE = SV3d.Matrix4by4;
Point2d: TYPE = SV2d.Point2d;
Point3d: TYPE = SV3d.Point3d;
Vector: TYPE = SV3d.Vector;
Old CreateCoordSys error checking:
Check that mat has no scaling components
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;
globalNumberStream: IO.STREAM; -- initialized in Init[].
CreateRoot: PUBLIC PROC [name: Rope.ROPE] RETURNS [newCS: CoordSystem] = {
newCS ← NEW[CoordSysObj ← [name: name, scalarsOnly: FALSE, scalars: [1,1,1], mat: Matrix3d.Identity[], wrtCamera: Matrix3d.Identity[], wrtWorld: Matrix3d.Identity[], cameraWRTlocal: Matrix3d.Identity[], worldWRTlocal: Matrix3d.Identity[], parent: NIL, children: NIL, dirty: TRUE]];
};
CreateCoordSysInTree: PUBLIC PROC [name: Rope.ROPE, mat: Matrix4by4, parent: CoordSystem, root: CoordSystem] RETURNS [newCS: CoordSystem] = {
First make sure the name is unique.
IF CoordSysNameIsPresent[name, root] THEN SIGNAL NameAlreadyExists;
newCS ← NEW[CoordSysObj ← [name: name, scalarsOnly: FALSE, scalars: [1,1,1], mat: mat, wrtCamera: Matrix3d.Identity[], wrtWorld: Matrix3d.Identity[], cameraWRTlocal: Matrix3d.Identity[], worldWRTlocal: Matrix3d.Identity[], parent: parent]];
Add this coordinate system as a child of its parent, if any.
IF parent # NIL THEN {
parent.children ← AppendCoordSysToList[newCS, parent.children];
};
};
NameAlreadyExists: PUBLIC SIGNAL = CODE;
CreateScalarsOnlyCoordSysInTree: PUBLIC PROC [name: Rope.ROPE, scalars: Vector, parent: CoordSystem, root: CoordSystem] RETURNS [newCS: CoordSystem] = {
First make sure the name is unique.
IF CoordSysNameIsPresent[name, root] THEN SIGNAL NameAlreadyExists;
newCS ← NEW[CoordSysObj ← [name: name, scalarsOnly: TRUE, scalars: scalars, mat: Matrix3d.Identity[], wrtCamera: Matrix3d.Identity[], wrtWorld: Matrix3d.Identity[], cameraWRTlocal: Matrix3d.Identity[], worldWRTlocal: Matrix3d.Identity[], parent: parent]];
Add this coordinate system as a child of its parent, if any.
IF parent # NIL THEN {
parent.children ← AppendCoordSysToList[newCS, parent.children];
};
};
CopyCoordSysFromAnyTree: PUBLIC PROC [source: CoordSystem, newName: Rope.ROPE, parent: CoordSystem, root: CoordSystem] RETURNS [newCS: CoordSystem] = {
Like CreateCoordSysInTree but uses the matrix from the old CoordSys. If source is a scalars-only coordsys, then newCS will be as well.
First make sure the name is unique.
IF CoordSysNameIsPresent[newName, root] THEN SIGNAL NameAlreadyExists;
newCS ← NEW[CoordSysObj ← [name: newName, scalarsOnly: source.scalarsOnly, scalars: source.scalars, mat: source.mat, wrtCamera: Matrix3d.Identity[], wrtWorld: Matrix3d.Identity[], cameraWRTlocal: Matrix3d.Identity[], worldWRTlocal: Matrix3d.Identity[], parent: parent, dirty: TRUE]];
Add this coordinate system as a child of its parent, if any.
IF parent # NIL THEN {
parent.children ← AppendCoordSysToList[newCS, parent.children];
};
};
ResetScalars: PUBLIC PROC [cs: CoordSystem, scalars: Vector] = {
IF NOT cs.scalarsOnly THEN ERROR;
cs.scalars ← scalars;
};
GetScalars: PUBLIC PROC [cs: CoordSystem] RETURNS [scalars: Vector] = {
IF NOT cs.scalarsOnly THEN ERROR;
scalars ← cs.scalars;
};
AttemptToCreateCoordSysWithScaling: PUBLIC SIGNAL = CODE;
DeleteCoordSysAndChildren: PUBLIC PROC [cs: CoordSystem, root: CoordSystem] = {
parent: CoordSystem ← cs.parent;
DeleteCoordSysLocal[cs];
parent.children ← DeleteCoordSysFromList[cs, parent.children];
};
DeleteCoordSysLocal: PRIVATE PROC [cs: CoordSystem] = {
Breakup cs and everything below. Note that this leaves cs's parent pointing to cs.
next: CoordSysList;
IF cs.children # NIL THEN {
FOR children: CoordSysList ← cs.children, next UNTIL children = NIL DO
DeleteCoordSysLocal[children.first];
next ← children.rest;
children.first ← NIL;
children.rest ← NIL;
ENDLOOP;
cs.children ← NIL;
};
cs.parent ← NIL;
};
DeleteCoordSysFromList: PROC [cs: CoordSystem, list: CoordSysList] RETURNS [CoordSysList] = {
before, l, after: CoordSysList;
[before, l, after] ← FindCoordSysAndNeighbors[cs, list];
IF before = NIL THEN RETURN [after]
ELSE {
l.rest ← NIL;
l.first ← NIL;
before.rest ← after;
RETURN[list];
};
};
AppendCoordSysToList: PROC [cs: CoordSystem, list: CoordSysList] RETURNS [CoordSysList] = {
A copy of List.Nconc1 for CoordSysList instead of LIST OF REF ANY
z: CoordSysList ← list;
IF z = NIL THEN RETURN[CONS[cs,NIL]];
UNTIL z.rest = NIL DO z ← z.rest; ENDLOOP;
z.rest ← CONS[cs,NIL];
RETURN[list];
};
MakeListFromTree: PUBLIC PROC [root: CoordSystem] RETURNS [csl: CoordSysList] = {
Makes a list in Breadth-first order.
pos, end: CoordSysList;
i: NAT ← 0;
end ← csl ← CONS[root, NIL];
FOR pos ← csl, pos.rest UNTIL pos = NIL DO
IF pos.first.children # NIL THEN {
FOR children: CoordSysList ← pos.first.children, children.rest UNTIL children = NIL DO
end.rest ← CONS[children.first, NIL];
end ← end.rest;
i ← i+1;
IF i > 2000 THEN ERROR;
ENDLOOP;
};
ENDLOOP;
};
FindCoordSysInTree: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [cs: CoordSystem] = {
success: BOOL;
[cs, success] ← FindCoordSysInTreeAux[name, root];
IF NOT success THEN SIGNAL CoordSysNotFound;
};
FindCoordSysInTreeAux: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [cs: CoordSystem, success: BOOL] = {
IF root.children = NIL THEN
IF Rope.Equal[name, root.name, TRUE] THEN {
cs ← root;
success ← TRUE;
}
ELSE {
cs ← NIL;
success ← FALSE;
}
ELSE {
IF Rope.Equal[name, root.name, TRUE] THEN {
cs ← root;
success ← TRUE;
RETURN;
};
FOR children: CoordSysList ← root.children, children.rest UNTIL children = NIL DO
[cs, success] ← FindCoordSysInTreeAux[name, children.first];
IF success THEN RETURN;
ENDLOOP;
cs ← NIL;
success ← FALSE;
};
};
CoordSysNameIsPresent: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [BOOL] = {
IF root = NIL THEN RETURN [FALSE];
IF root.children = NIL THEN RETURN[Rope.Equal[name, root.name, TRUE]]
ELSE {
IF Rope.Equal[name, root.name, TRUE] THEN RETURN[TRUE];
FOR children: CoordSysList ← root.children, children.rest UNTIL children = NIL DO
IF CoordSysNameIsPresent[name, children.first] THEN RETURN[TRUE];
ENDLOOP;
RETURN[FALSE];
};
};
FindCoordSysInList: 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;
};
CoordSysNotFound: PUBLIC SIGNAL = CODE;
CoordSysListEmpty: PUBLIC SIGNAL = CODE;
FindCoordSysAndNeighbors: PUBLIC PROC [C: CoordSystem, 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 l.first = C THEN {
beforeCS ← lastL; cs ← l; afterCS ← l.rest; RETURN};
lastL ← l;
l ← l.rest;
ENDLOOP;
SIGNAL CoordSysNotFound;
};
BaseAndNumber: PUBLIC PROC [name: Rope.ROPE] RETURNS [base: Rope.ROPE, number: NAT] = {
Currently finds the "number" after the first decimal point. Should find the last decimal point and use only digits after that.
noNumber: BOOL ← FALSE;
nameStream: IO.STREAM;
next: CHAR;
nameStream ← IO.RIS[name];
base ← ReadAlpha[nameStream];
next ← nameStream.GetChar[
!IO.EndOfStream => {noNumber ← TRUE; CONTINUE}];
IF noNumber THEN number ← 0 ELSE
IF next = '. THEN number ← IO.GetInt[nameStream]
ELSE number ← 0;
};
ReadAlpha: PRIVATE PROC [s: IO.STREAM] RETURNS [base: Rope.ROPE] = {
skip: INT;
[base, skip] ← IO.GetTokenRope[s, AlphaBreakProc];
};
AlphaBreakProc: SAFE PROC [char: CHAR] RETURNS [IO.CharClass] = TRUSTED {
RETURN[IF char = '. THEN break ELSE other]
};
BaseAndNumber: PUBLIC PROC [name: Rope.ROPE] RETURNS [base: Rope.ROPE, number: NAT] = {
Currently finds the "number" after the first decimal point. Should find the last decimal point and use only digits after that.
index: INT;
rest: Rope.ROPE;
numStream: IO.STREAM;
index ← Rope.Find[name, "."];
IF index = -1 THEN RETURN[name, 0]; -- if there is no decimal, pretend we have name.0
base ← Rope.Substr[name, 0, index];
rest ← Rope.Substr[name, index+1];
numStream ← IO.RIS[rest, globalNumberStream];
number ← IO.GetInt[numStream];
IO.Reset[numStream];
};
UniqueNameWithSuffix: PUBLIC PROC [oldName: Rope.ROPE, suffix: Rope.ROPE, root: CoordSystem] RETURNS [unique: Rope.ROPE] = {
Takes a solidviews name, like "teapot.73". Adds the suffix to the basename and fixes the number to ensure uniqueness. If the suffix were "$$tool", then UniqueNameWithSuffix might return "teapot$$tool.89".
base: Rope.ROPE;
num: NAT;
[base, num] ← BaseAndNumber[oldName];
unique ← IO.PutFR["%g%g.%g", [rope[base]], [rope[suffix]], [integer[num]]];
unique ← UniqueNameFrom[unique, root];
};
UniqueNameFrom: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [unique: Rope.ROPE] = {
Finds any digits on the end of the name. Looks through the coordSys tree extracting a digit and base name from each name. Finds the maximum number, adds one, reconcatenates and returns.
maxNum: NAT ← 0;
targetBase, base: Rope.ROPE;
g: CoordSysList ← MakeListFromTree[root];
targetNum, num, targetLen: NAT;
len: INT;
c, firstC: CHAR;
[targetBase, targetNum] ← BaseAndNumber[name];
targetLen ← Rope.Length[targetBase];
firstC ← Rope.Fetch[targetBase, 0];
FOR csl: CoordSysList ← g, csl.rest UNTIL csl = NIL DO
len ← Rope.Find[csl.first.name, "."];
IF len = -1 THEN LOOP; -- A name without a decimal point is a zero. Doesn't increment maxNum.
IF len # targetLen THEN LOOP; -- Clearly not the same name.
c ← Rope.Fetch[csl.first.name, 0];
IF c = firstC THEN { -- cheap tests failed. Pay the piper.
[base, num] ← BaseAndNumber[csl.first.name];
IF Rope.Equal[base, targetBase, TRUE] THEN maxNum ← MAX[num, maxNum];
};
ENDLOOP;
unique ← IO.PutFR["%g.%g", [rope[targetBase]], [integer[maxNum+1]]];
};
UniqueNameFrom: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [unique: Rope.ROPE] = {
Finds any digits on the end of the name. Looks through the coordSys tree extracting a digit and base name from each name. Finds the maximum number, adds one, reconcatenates and returns.
maxNum: NAT ← 0;
targetBase, base: Rope.ROPE;
g: CoordSysList ← MakeListFromTree[root];
targetNum, num: NAT;
[targetBase, targetNum] ← BaseAndNumber[name];
FOR csl: CoordSysList ← g, csl.rest UNTIL csl = NIL DO
[base, num] ← BaseAndNumber[csl.first.name];
IF Rope.Equal[base, targetBase, TRUE] THEN maxNum ← MAX[num, maxNum];
ENDLOOP;
unique ← IO.PutFR["%g.%g", [rope[targetBase]], [integer[maxNum+1]]];
};
CameraToScreen: PUBLIC PROC [cameraPoint2d: Point2d, screenCoordSys: CoordSystem] RETURNS [screenPoint2d: Point2d] = {
Currently, the SCREEN coordinate frame may only contain a translation. This operation then is like Matrix3d.Update[CAMERASCREEN, cameraPoint2d] but faster.
screenPoint2d[1] ← cameraPoint2d[1] - screenCoordSys.mat[1][4];
screenPoint2d[2] ← cameraPoint2d[2] - screenCoordSys.mat[2][4];
};
ScreenToCamera: PUBLIC PROC [screenPoint2d: Point2d, screenCoordSys: CoordSystem] RETURNS [cameraPoint2d: Point2d] = {
Currently, the SCREEN coordinate frame may only contain a translation. This operation then is like Matrix3d.Update[SCREENCAMERA, screenPoint2d] but faster.
cameraPoint2d[1] ← screenPoint2d[1] + screenCoordSys.mat[1][4];
cameraPoint2d[2] ← screenPoint2d[2] + screenCoordSys.mat[2][4];
};
FromCSToCS: PUBLIC PROC [pt: Point3d, currentCS: CoordSystem, newCS: CoordSystem] RETURNS [newPt: Point3d] = TRUSTED {
Takes ptcurrentCS and returns ptnewCS.
currentNew: Matrix4by4;
currentNew ← FindAInTermsOfB[currentCS, newCS];
newPt ← Matrix3d.Update[currentNew, pt];
};
FromCSToCSMat: PUBLIC PROC [mat: Matrix4by4, currentCS: CoordSystem, newCS: CoordSystem] RETURNS [newMat: Matrix4by4] = TRUSTED {
Takes matcurrentCS and returns matnewCS.
currentNew: Matrix4by4;
currentNew ← FindAInTermsOfB[currentCS, newCS];
newMat ← Matrix3d.MatMult[currentNew, mat];
};
FindInTermsOfWorld: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = {
thisCS, nextCS: CoordSystem;
thisCS ← cs;
IF cs.scalarsOnly THEN mat ← Matrix3d.MakeScaleMat[cs.scalars[1], cs.scalars[2], cs.scalars[3]]
ELSE mat ← cs.mat;
UNTIL thisCS.parent = NIL DO
nextCS ← thisCS.parent;
mat ← Matrix3d.MatMult[nextCS.mat, mat];
thisCS ← nextCS;
ENDLOOP;
};
FindInTermsOfCamera: PUBLIC PROC [cs: CoordSystem, camera: CoordSystem] RETURNS [mat: Matrix4by4] = {
inWorld: Matrix4by4 ← FindInTermsOfWorld[cs];
cameraWRTWorld: Matrix4by4 ← FindInTermsOfWorld[camera];
mat ← Matrix3d.WorldToLocal[cameraWRTWorld,inWorld];
};
FindWorldInTermsOf: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = {
csWORLD: Matrix4by4 ← FindInTermsOfWorld[cs];
mat ← Matrix3d.Inverse[csWORLD];
};
FindCameraInTermsOf: PUBLIC PROC [cs: CoordSystem, camera: CoordSystem] RETURNS [mat: Matrix4by4] = {
csWorld: Matrix4by4 ← FindInTermsOfWorld[cs];
cameraWorld: Matrix4by4 ← FindInTermsOfWorld[camera];
mat ← Matrix3d.WorldToLocal[csWorld, cameraWorld];
};
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];
};
PutAInTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [aInTermsOfb: Matrix4by4] = {
aWorld, bWorld: Matrix4by4;
aWorld ← FindInTermsOfWorld[a];
bWorld ← FindInTermsOfWorld[b];
aInTermsOfb ← Matrix3d.WorldToLocal[bWorld,aWorld];
a.parent ← b;
a.mat ← aInTermsOfb;
};
Init: PROC = {
globalNumberStream ← IO.RIS["273"];
};
For local transforms, call these procedures with b = a.
END.