<> <> <> <> 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; <> <> <> <<1.0e-7 is too small. Wow, subtraction has real floating point fuzz.>> <<[x, y, z] _ Matrix3d.ScaleFromMatrix[mat];>> < almostZero OR ABS[y-1.0] > almostZero OR ABS[z - 1.0] > almostZero THEN >> <> <<>> 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] = { <> 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]]; <> 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] = { <> 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]]; <> 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] = { <> <> 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]]; <> 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] = { <> 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] = { <> 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] = { <> 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] = { <> 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; }; <> <> <> <> <> <> <> <> < {noNumber _ TRUE; CONTINUE}];>> <> <> <<};>> BaseAndNumber: PUBLIC PROC [name: Rope.ROPE] RETURNS [base: Rope.ROPE, number: NAT] = { <> 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] = { <> 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] = { <> 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]]]; }; <> <> <> <> <> <> <<>> <<[targetBase, targetNum] _ BaseAndNumber[name];>> <> <<[base, num] _ BaseAndNumber[csl.first.name];>> <> <> <> <<};>> <<>> CameraToScreen: PUBLIC PROC [cameraPoint2d: Point2d, screenCoordSys: CoordSystem] RETURNS [screenPoint2d: Point2d] = { <> 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] = { <> 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 { <> currentNew: Matrix4by4; currentNew _ FindAInTermsOfB[currentCS, newCS]; newPt _ Matrix3d.Update[currentNew, pt]; }; FromCSToCSMat: PUBLIC PROC [mat: Matrix4by4, currentCS: CoordSystem, newCS: CoordSystem] RETURNS [newMat: Matrix4by4] = TRUSTED { <> 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"]; }; <> END.