File: SVCoordSysImpl.mesa
Last edited by Bier on August 7, 1987 0:05:19 am PDT
Author: Eric Bier in the summer of 1982
Contents: Allocation and access to a user-specified set of named coordinate systems
DIRECTORY
SVCoordSys, FunctionCache, IO, SVMatrix3d, Rope, SV2d, SV3d, SVCoordSysType, SVModelTypes;
SVCoordSysImpl: CEDAR PROGRAM
IMPORTS FunctionCache, SVMatrix3d, IO, Rope
EXPORTS SVCoordSys, SVModelTypes =
BEGIN
Camera: TYPE = SVModelTypes.Camera;
CoordSystem: TYPE = REF CoordSysObj;
CoordSysObj: PUBLIC TYPE = SVCoordSysType.CoordSysObj;
CoordSysList: TYPE = SVModelTypes.CoordSysList;
Matrix4by4: TYPE = SV3d.Matrix4by4;
Point2d: TYPE = SV2d.Point2d;
Point3d: TYPE = SV3d.Point3d;
Vector3d: TYPE = SV3d.Vector3d;
globalNumberStream: IO.STREAM; -- initialized in Init[].
Building the Tree
CreateRoot: PUBLIC PROC [name: Rope.ROPE] RETURNS [newCS: CoordSystem] = {
newCS ← NEW[CoordSysObj ← [
name: name,
scalarsOnly: FALSE,
scalars: [1,1,1],
mat: SVMatrix3d.Identity[],
worldOK: TRUE,
wrtWorld: SVMatrix3d.Identity[],
inverseOK: TRUE,
worldWRTlocal: SVMatrix3d.Identity[],
cameraCache: FunctionCache.Create[3],
parent: NIL, children: NIL]];
};
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,
worldOK: FALSE,
wrtWorld: SVMatrix3d.Identity[],
inverseOK: FALSE,
worldWRTlocal: SVMatrix3d.Identity[],
cameraCache: FunctionCache.Create[3],
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: Vector3d, 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: SVMatrix3d.Identity[],
worldOK: FALSE,
wrtWorld: SVMatrix3d.Identity[],
inverseOK: FALSE,
worldWRTlocal: SVMatrix3d.Identity[],
cameraCache: FunctionCache.Create[3],
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 CoordSystem. 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,
worldOK: FALSE,
wrtWorld: SVMatrix3d.Identity[],
inverseOK: FALSE,
worldWRTlocal: SVMatrix3d.Identity[],
cameraCache: FunctionCache.Create[3],
parent: parent]];
Add this coordinate system as a child of its parent, if any.
IF parent # NIL THEN {
parent.children ← AppendCoordSysToList[newCS, parent.children];
};
};
PutAInTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [aInTermsOfb: Matrix4by4] = {
aWorld, bWorld: Matrix4by4;
aWorld ← WRTWorld[a];
bWorld ← WRTWorld[b];
aInTermsOfb ← SVMatrix3d.AInTermsOfB[aWorld, bWorld];
a.parent ← b;
a.mat ← aInTermsOfb;
};
DeleteCoordSysAndChildren: PUBLIC PROC [cs: CoordSystem, root: CoordSystem] = {
parent: CoordSystem ← cs.parent;
DeleteCoordSysLocal[cs];
parent.children ← DeleteCoordSysFromList[cs, parent.children];
};
FindCoordSysInTree: PUBLIC PROC [name: Rope.ROPE, root: CoordSystem] RETURNS [cs: CoordSystem] = {
success: BOOL;
[cs, success] ← FindCoordSysInTreeAux[name, root];
IF NOT success THEN SIGNAL CoordSysNotFound;
};
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];
};
};
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;
};
CoordSysNotFound: PUBLIC SIGNAL = CODE;
CoordSysListEmpty: PUBLIC SIGNAL = CODE;
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;
};
Parent: PUBLIC PROC [cs: CoordSystem] RETURNS [parent: CoordSystem] = {
parent ← cs.parent;
};
Name: PUBLIC PROC [cs: CoordSystem] RETURNS [name: Rope.ROPE] = {
name ← cs.name;
};
SetName: PUBLIC PROC [cs: CoordSystem, name: Rope.ROPE] = {
cs.name ← name;
};
GetSceneAssembly: PUBLIC PROC [world: CoordSystem] RETURNS [sa: CoordSystem] = {
sa ← world.children.first;
};
Unique Names
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]]];
};
NameWithSuffix: PUBLIC PROC [oldName: Rope.ROPE, suffix: Rope.ROPE, root: CoordSystem] RETURNS [probablyUnique: Rope.ROPE] = {
Used in place of SVCoordSys.UniqueNameWithSuffix, which takes too long. We just split up oldName into its base and number and splice in the suffix. We assume that if oldName is unique, probablyUnique will be as well.
base: Rope.ROPE;
num: NAT;
[base, num] ← BaseAndNumber[oldName];
probablyUnique ← IO.PutFR["%g%g.%g", [rope[base]], [rope[suffix]], [integer[num]]];
};
Utility Routines
DeleteCoordSysLocal: 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];
};
FindCoordSysInTreeAux: 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;
};
};
FindCoordSysAndNeighbors: 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;
};
Changing the Transformations
SetScalars: PUBLIC PROC [cs: CoordSystem, scalars: Vector3d] = {
IF NOT cs.scalarsOnly THEN ERROR;
cs.scalars ← scalars;
};
GetScalars: PUBLIC PROC [cs: CoordSystem] RETURNS [scalars: Vector3d] = {
IF NOT cs.scalarsOnly THEN ERROR;
scalars ← cs.scalars;
};
CameraIsDirty: PROC [cs: CoordSystem] = {
oldEntry: CameraEntry;
cameraCache: FunctionCache.Cache ← cs.cameraCache;
allCache: LIST OF FunctionCache.CacheEntry ← FunctionCache.GetList[cameraCache];
FOR list: LIST OF FunctionCache.CacheEntry ← allCache, list.rest UNTIL list = NIL DO
oldEntry ← NARROW[list.first.value];
IF oldEntry # NIL THEN oldEntry.ok ← FALSE;
ENDLOOP;
FOR list: LIST OF Camera ← cameraList, list.rest UNTIL list = NIL DO
UpdateMatForCamera[cs, list.first];
ENDLOOP;
};
SetMat: PUBLIC PROC [cs: CoordSystem, mat: Matrix4by4] = {
changes the relationship of cs to its parent.
cs.mat ← mat;
DirtyCoordSys[cs];
};
DirtyCoordSys: PROC [cs: CoordSystem] = {
IF cs.scalarsOnly THEN {
IF cs.children # NIL THEN ERROR; -- scalars-only coordinate systems must be leaves.
cs.worldOK ← FALSE;
cs.inverseOK ← FALSE;
CameraIsDirty[cs];
}
ELSE {
cs.worldOK ← FALSE;
cs.inverseOK ← FALSE;
CameraIsDirty[cs];
IF cs.children # NIL THEN { -- Process the children as well
FOR list: CoordSysList ← cs.children, list.rest UNTIL list = NIL DO
DirtyCoordSys[list.first];
ENDLOOP;
};
};
};
GetMat: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = {
reports the relationship of cs to its parent.
mat ← cs.mat;
};
IsScalarsOnly: PUBLIC PROC [cs: CoordSystem] RETURNS [BOOL] = {
RETURN[cs.scalarsOnly];
};
Transformation Queries
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 SVMatrix3d.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 SVMatrix3d.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] = {
Takes ptcurrentCS and returns ptnewCS.
currentNew: Matrix4by4;
currentNew ← FindAInTermsOfB[currentCS, newCS];
newPt ← SVMatrix3d.Update[pt, currentNew];
};
FromCSToCSMat: PUBLIC PROC [mat: Matrix4by4, currentCS: CoordSystem, newCS: CoordSystem] RETURNS [newMat: Matrix4by4] = {
Takes matcurrentCS and returns matnewCS.
currentNew: Matrix4by4;
currentNew ← FindAInTermsOfB[currentCS, newCS];
newMat ← SVMatrix3d.MatMult[currentNew, mat];
};
WRTWorld: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = {
thisCS, nextCS: CoordSystem;
IF cs.worldOK THEN RETURN[cs.wrtWorld];
thisCS ← cs;
IF cs.scalarsOnly THEN mat ← SVMatrix3d.MakeScaleMat[cs.scalars[1], cs.scalars[2], cs.scalars[3]]
ELSE mat ← cs.mat;
UNTIL thisCS.parent = NIL DO
nextCS ← thisCS.parent;
mat ← SVMatrix3d.MatMult[nextCS.mat, mat];
thisCS ← nextCS;
ENDLOOP;
cs.wrtWorld ← mat;
cs.worldOK ← TRUE;
};
CameraEntry: TYPE = REF CameraEntryObj;
CameraEntryObj: TYPE = RECORD [
wrtCamera: Matrix4by4,
cameraWRTLocal: Matrix4by4,
ok: BOOL ← FALSE
];
WRTCamera: PUBLIC PROC [cs: CoordSystem, cameraCS: CoordSystem] RETURNS [mat: Matrix4by4] = {
CompareCameras2: PROC [argument: FunctionCache.Domain] RETURNS [good: BOOL] = {
thisName: Rope.ROPE ← NARROW[argument];
good ← Rope.Equal[thisName, cameraCS.name, FALSE];
};
cameraCache: FunctionCache.Cache ← cs.cameraCache;
oldEntryAny: REF ANY;
oldEntry: CameraEntry;
ok: BOOL;
BEGIN
[oldEntryAny, ok] ← FunctionCache.Lookup[cameraCache, CompareCameras2];
IF NOT ok THEN GOTO UpdateTheCache -- not in cache
ELSE {
oldEntry ← NARROW[oldEntryAny];
IF NOT oldEntry.ok THEN GOTO UpdateTheCache -- cache out of date
ELSE mat ← oldEntry.wrtCamera;
};
EXITS
UpdateTheCache => {
UpdateMatrixForCamera[cs, cameraCS];
mat ← WRTCamera[cs, cameraCS]; -- recursive call
};
END;
};
FillMat: PROC [entry: CameraEntry, cs: CoordSystem, cameraCS: CoordSystem] = {
csWorld: Matrix4by4;
cameraWorld: Matrix4by4;
csWorld ← WRTWorld[cs];
cameraWorld ← WRTWorld[cameraCS];
entry.wrtCamera ← SVMatrix3d.AInTermsOfB[csWorld, cameraWorld];
entry.cameraWRTLocal ← SVMatrix3d.Inverse[entry.wrtCamera];
};
UpdateMatrixForCamera: PUBLIC PROC [cs: CoordSystem, cameraCS: CoordSystem] = {
A camera has moved, or we are using this camera for the first time. Update the entry for this camera (adding an entry if needed).
CompareCameras: PROC [argument: FunctionCache.Domain] RETURNS [good: BOOL] = {
thisName: Rope.ROPE ← NARROW[argument];
good ← Rope.Equal[thisName, cameraCS.name, FALSE];
};
cameraCache: FunctionCache.Cache ← cs.cameraCache;
oldEntryAny: REF ANY;
oldEntry: CameraEntry;
ok: BOOL ← FALSE;
[oldEntryAny, ok] ← FunctionCache.Lookup[cameraCache, CompareCameras];
IF ok THEN {
oldEntry ← NARROW[oldEntryAny];
FillMat[oldEntry, cs, cameraCS];
oldEntry.ok ← TRUE;
}
ELSE {
newEntry: CameraEntry ← NEW[CameraEntryObj];
FillMat[newEntry, cs, cameraCS];
newEntry.ok ← TRUE;
FunctionCache.Insert[cameraCache, cameraCS.name, newEntry, 0];
};
};
FindWorldInTermsOf: PUBLIC PROC [cs: CoordSystem] RETURNS [mat: Matrix4by4] = {
csWORLD: Matrix4by4;
IF cs.inverseOK THEN RETURN[cs.worldWRTlocal];
csWORLD ← WRTWorld[cs];
mat ← SVMatrix3d.Inverse[csWORLD];
cs.worldWRTlocal ← mat;
cs.inverseOK ← TRUE;
};
FindCameraInTermsOf: PUBLIC PROC [cs: CoordSystem, cameraCS: CoordSystem] RETURNS [mat: Matrix4by4] = {
CompareCameras3: PROC [argument: FunctionCache.Domain] RETURNS [good: BOOL] = {
thisName: Rope.ROPE ← NARROW[argument];
good ← Rope.Equal[thisName, cameraCS.name, FALSE];
};
cameraCache: FunctionCache.Cache ← cs.cameraCache;
oldEntryAny: REF ANY;
oldEntry: CameraEntry;
ok: BOOL;
BEGIN
[oldEntryAny, ok] ← FunctionCache.Lookup[cameraCache, CompareCameras3];
IF NOT ok THEN GOTO UpdateTheCache -- not in cache
ELSE {
oldEntry ← NARROW[oldEntryAny];
IF NOT oldEntry.ok THEN GOTO UpdateTheCache -- cache out of date
ELSE mat ← oldEntry.cameraWRTLocal;
};
EXITS
UpdateTheCache => {
UpdateMatrixForCamera[cs, cameraCS];
mat ← FindCameraInTermsOf[cs, cameraCS]; -- recursive call
};
END;
};
FindAInTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [aInTermsOfb: Matrix4by4] = {
aWorld, bWorld: Matrix4by4;
aWorld ← WRTWorld[a];
bWorld ← WRTWorld[b];
aInTermsOfb ← SVMatrix3d.AInTermsOfB[aWorld,bWorld];
};
FindTranslationOfAinTermsOfB: PUBLIC PROC [a: CoordSystem, b: CoordSystem] RETURNS [displacements: Vector3d] = {
aInTermsOfb: Matrix4by4 ← FindAInTermsOfB[a,b];
displacements ← SVMatrix3d.OriginOfMatrix[aInTermsOfb];
};
Init: PROC = {
globalNumberStream ← IO.RIS["273"];
};
END.