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

}.
Κ––
"Mesa" style˜Iprocš ΟcΠ
œ
Οk	œKžœžœ,žœžœ!žœ"žœžœžœ'žœ#žœžœΟnœžœ
žœ
žœbžœdžœ
žœžœžœžœžœ)žœžœ
žœ!Ÿœžœ
žœ"žœžœžœžœžœžœžœžœ Ÿ
œžœ
žœžœžœžœ%žœžœ
žœžœŸœžœ
žœžœ	žœ)œ1žœ
žœ
žœ8žœŸœžœ
žœžœ §
œAžœ@žœ%žœžœ
žœ
žœWžœEœžœ8žœžœžœ
žœžœ
žœežœ)$œŸœžœ
žœ
žœ/žœ,œ
žœ’
žœ
žœžœ
žœžœ
žœžœž	œ3žœχ
žœ
žœžœŸœžœ
žœ
žœžœ0žœžœ
žœ
žœžœžœ
žœžœ žœ
žœžœ
žœžœžœŸ œžœ
žœ
žœžœ,1œžœžœžœ
žœ
žœžœžœ
žœžœ žœ3žœžœžœŸœžœ
žœ7žœ_œFŸœžœ
žœ7žœ_œXžœ
žœžœžœ
žœžœŸœžœ
žœžœžœ)žœžœžœŸœžœ
žœžœžœžœ
žœ
žœ
žœEŸœžœ
žœžœSžœžœ
žœažœŸœžœ
žœ•
œω
Ÿœžœ
žœ(žœ»
Ÿœžœ
žœ"žœΈ
Ÿœžœ
žœ"žœ‹
Ÿœžœ
žœ"žœε
Ÿœžœ
žœ#žœwžœžœžœ
žœ"œ
žœžœžœžœžœžœ
žœ
žœ
0œžœˆŸœžœ
žœ#žœΐ
œŽ
žœžœžœ
žœ"œ
žœžœžœžœžœžœ
žœ
žœ
0œžœήŸœžœ
žœ.žœ|žœžœ
žœ
žœžœQžœžœžœcœ5žœžœ
žœ
žœ"œΐ
Ÿ
œžœ
žœ+žœ|žœžœ
žœ
žœžœQžœžœžœgœ5žœžœ
žœ
žœ"œΑ
Ÿ
œžœ
žœ+žœ|žœžœ
žœ
žœžœQžœžœžœgœ5žœžœ
žœ
žœ"œΑ
Ÿ
œžœ
žœ+žœ|žœžœ
žœ
žœžœQžœžœžœgœ5žœžœ
žœ
žœ"œΐ
Ÿœžœ
žœ.žœΦ
œvžœžœ
žœ
žœžœQžœžœžœ_œ5žœžœ
žœ
žœ"œΏ
Ÿœžœ
žœ
žœžœžœ
žœžœžœ
žœœžœžœžœ
žœžœžœžœžœ
žœžœžœžœžœ
žœžœžœžœžœ
žœyžœ
žœwžœ
žœ|Ÿœžœ
žœ#žœhœ’
žœžœ
žœNžœžœ
žœžœžœ
žœ
žœžœQžœžœžœκ
œ;œ3žœ#žœ&:œPžœžœ‰
žœžœω
žœžœ
žœ
žœ"œΐ
Ÿ
œžœ
žœ'hœvžœžœ
žœ
žœžœQžœžœžœ‘
žœžœ
žœ
žœ"œΏ
Ÿœžœ
žœ'hœvžœžœ
žœ
žœžœQžœžœžœ|žœžœ
žœ
žœ"œΐ
Ÿœžœ
žœ'hœvžœžœ
žœ
žœžœQžœžœžœ|žœžœ
žœ
žœ"œΐ
Ÿœžœ
žœ'hœvžœžœ
žœ
žœžœQžœžœžœ|žœžœ
žœ
žœ"œΐ
$œ˜ζ“
—…—IθQ„