<> <> <> <> 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[]. <> 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] = { <> 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]]; <> 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] = { <> 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]]; <> 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, worldOK: FALSE, wrtWorld: SVMatrix3d.Identity[], inverseOK: FALSE, worldWRTlocal: SVMatrix3d.Identity[], cameraCache: FunctionCache.Create[3], parent: parent]]; <> 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] = { <> 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; }; <> 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]]]; }; NameWithSuffix: PUBLIC PROC [oldName: Rope.ROPE, suffix: Rope.ROPE, root: CoordSystem] RETURNS [probablyUnique: Rope.ROPE] = { <> base: Rope.ROPE; num: NAT; [base, num] _ BaseAndNumber[oldName]; probablyUnique _ IO.PutFR["%g%g.%g", [rope[base]], [rope[suffix]], [integer[num]]]; }; <> DeleteCoordSysLocal: 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]; }; 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] = { <> 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; }; <> 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; <> <> <> }; SetMat: PUBLIC PROC [cs: CoordSystem, mat: Matrix4by4] = { <> 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] = { <> mat _ cs.mat; }; IsScalarsOnly: PUBLIC PROC [cs: CoordSystem] RETURNS [BOOL] = { RETURN[cs.scalarsOnly]; }; <> 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] = { <> currentNew: Matrix4by4; currentNew _ FindAInTermsOfB[currentCS, newCS]; newPt _ SVMatrix3d.Update[pt, currentNew]; }; FromCSToCSMat: PUBLIC PROC [mat: Matrix4by4, currentCS: CoordSystem, newCS: CoordSystem] RETURNS [newMat: Matrix4by4] = { <> 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] = { <> 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.