-- File: SVCoordSys2dImpl.mesa
-- Last edited by Bier on December 18, 1982 1:29 am
-- Author: Eric Bier on November 22, 1982 9:35 pm
-- Contents: Routines for manipulating coordinate systems with respect to each other in 2 dimensions. Unlike Coordsystems which are told about WORLD and CAMERA at draw time, CoordSystem2d's are told about Pad every time they are updated. This only makes sense if the updated coordsys has no sons. The general case will be implemented later.

DIRECTORY
 SV2d,
 SVCoordSys2d,
 SVMatrix2d,
 SVVector2d;
 
SVCoordSys2dImpl: PROGRAM
 IMPORTS SVMatrix2d, SVVector2d
 EXPORTS SVCoordSys2d =
BEGIN

Matrix3by3: TYPE = SVMatrix2d.Matrix3by3;
Point2d: TYPE = SV2d.Point2d;
Vector2d: TYPE = SVVector2d.Vector2d;

CoordSystem2d: TYPE = REF CoordSystem2dObj;
CoordSystem2dObj: TYPE = SVCoordSys2d.CoordSystem2dObj;
 
TellCoordSysAboutPad: PUBLIC PROC [cs: CoordSystem2d] = {
 cs.localWRTPad ← FindInTermsOfPad[cs];
 cs.padWRTLocal ← SVMatrix2d.Inverse[cs.localWRTPad];
 };

CreateCoordSys: PUBLIC PROC [mat: Matrix3by3, withRespectTo: CoordSystem2d] RETURNS [newCS: CoordSystem2d] = {
 newCS ← NEW[CoordSystem2dObj];
 newCS.mat ← mat;
 newCS.localWRTPad ← SVMatrix2d.Identity[];
 newCS.padWRTLocal ← SVMatrix2d.Identity[];
 newCS.withRespectTo ← withRespectTo;
 TellCoordSysAboutPad[newCS];
 };

PadToScreen: PUBLIC PROC [padPoint: Point2d, screen: CoordSystem2d] RETURNS [screenPoint: Point2d] = {
 screenPoint ← SVMatrix2d.Update[screen.localWRTPad, padPoint];
 };
 
ScreenToPad: PUBLIC PROC [screenPoint: Point2d, screen: CoordSystem2d] RETURNS [padPoint: Point2d] = {
 padPoint ← SVMatrix2d.Update[screen.padWRTLocal, padPoint];
 };

FindInTermsOfPad: PUBLIC PROC [cs: CoordSystem2d] RETURNS [mat: Matrix3by3] = {
 thisCS, nextCS: CoordSystem2d;
 thisCS ← cs;
 IF thisCS = NIL THEN RETURN [SVMatrix2d.Identity[]];
 mat ← cs.mat;
 UNTIL thisCS.withRespectTo = NIL DO
  nextCS ← thisCS.withRespectTo;
  mat ← SVMatrix2d.MatMult[nextCS.mat, mat];
  thisCS ← nextCS;
 ENDLOOP;
 };
 
FindAinTermsOfB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d] RETURNS [aInTermsOfb: Matrix3by3] = {
 aPad, bPad: Matrix3by3;
 aPad ← FindInTermsOfPad[a];
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aInTermsOfb ← SVMatrix2d.WorldToLocal[bPad,aPad];
 };

FindTranslationOfAinTermsOfB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d] RETURNS [displacements: Vector2d] = {
 aInTermsOfb: Matrix3by3;
 IF b = NIL THEN aInTermsOfb ← FindInTermsOfPad[a]
 ELSE aInTermsOfb ← FindAinTermsOfB[a, b];
 displacements ← SVMatrix2d.OriginOfMatrix[aInTermsOfb];
 };

PutAinTermsOfB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d] RETURNS [aInTermsOfb: Matrix3by3] = {
 -- finds the matrix and redefines CoordSystem a to be in terms of b from now on.
 aPad, bPad: Matrix3by3;
 aPad ← FindInTermsOfPad[a];
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aInTermsOfb ← SVMatrix2d.WorldToLocal[bPad,aPad];
 a.withRespectTo ← b;
 a.mat ← aInTermsOfb;
 TellCoordSysAboutPad[a];
 };
 

PlaceAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL, aWRTb: Matrix3by3] = {
-- places a in PAD so that the transform from b to a is aWRTb. Updates a with respect to its immediate reference (a.withRespectTo) accordingly.
aPad, bPad, withRespectToPad, aInTermsOfWRT: Matrix3by3;
 withRespectTo: CoordSystem2d;
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aPad ← SVMatrix2d.MatMult[bPad, aWRTb];
 withRespectTo ← a.withRespectTo;
 withRespectToPad ← FindInTermsOfPad[withRespectTo];
 aInTermsOfWRT ← SVMatrix2d.WorldToLocal[withRespectToPad, aPad];
 a.mat ← aInTermsOfWRT;
 TellCoordSysAboutPad[a];
 };
 

PlaceTranslationAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL, origin: Point2d] = {
-- places a in PAD 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: Matrix3by3 ← FindAinTermsOfB[a, b];
aPad, bPad, withRespectToPad, aInTermsOfWRT: Matrix3by3;
withRespectTo: CoordSystem2d;
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
aWRTb[1][3] ← origin[1];
aWRTb[2][3] ← origin[2];
aPad ← SVMatrix2d.MatMult[bPad, aWRTb];
withRespectTo ← a.withRespectTo;
withRespectToPad ← FindInTermsOfPad[withRespectTo];
aInTermsOfWRT ← SVMatrix2d.WorldToLocal[withRespectToPad, aPad];
a.mat ← aInTermsOfWRT;
TellCoordSysAboutPad[a];
};


TranslateAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL, tx, ty: REAL] = {
 aPad, bPad, aInTermsOfb, newAWorld, withRespectToWorld, aInTermsOfWRT: Matrix3by3;
 withRespectTo: CoordSystem2d;
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aPad ← FindInTermsOfPad[a];
 aInTermsOfb ← IF a = b THEN SVMatrix2d.Identity[]
  ELSE SVMatrix2d.WorldToLocal[bPad, aPad];
 aInTermsOfb ← SVMatrix2d.Translate[aInTermsOfb, tx, ty]; -- must be a global translate.
 newAWorld ← SVMatrix2d.MatMult[bPad, aInTermsOfb];
 withRespectTo ← a.withRespectTo;
 withRespectToWorld ← FindInTermsOfPad[withRespectTo];
 aInTermsOfWRT ← SVMatrix2d.WorldToLocal[withRespectToWorld, newAWorld];
 a.mat ← aInTermsOfWRT;
 TellCoordSysAboutPad[a];
 };
 

RotateCCWAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL, degrees: REAL] = {
 aPad, bPad, aInTermsOfb, newAWorld, withRespectToPad, aInTermsOfWRT: Matrix3by3;
 withRespectTo: CoordSystem2d;
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aPad ← FindInTermsOfPad[a];
 aInTermsOfb ← IF a = b THEN SVMatrix2d.Identity[]
  ELSE SVMatrix2d.WorldToLocal[bPad,aPad];
 aInTermsOfb ← SVMatrix2d.RotateCCW[aInTermsOfb, degrees]; -- must be a global translate.
 newAWorld ← SVMatrix2d.MatMult[bPad, aInTermsOfb];
 withRespectTo ← a.withRespectTo;
 withRespectToPad ← FindInTermsOfPad[withRespectTo];
 aInTermsOfWRT ← SVMatrix2d.WorldToLocal[withRespectToPad, newAWorld];
 a.mat ← aInTermsOfWRT;
 TellCoordSysAboutPad[a];
 };
 
IndicesOfMax: PRIVATE PROC [v: Vector2d] RETURNS [indices: ARRAY[1..2] OF NAT, negArray: ARRAY[1..2] 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;
 indices ← [1,2];
 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};
 };


AlignAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL] = {
 -- rotate A as little as possible to align its two axes parallel to two of the axes of B (though not necessarily the same two).
 aPad, bPad, aInTermsOfb, newAPad, withRespectToPad, aInTermsOfWRT: Matrix3by3;
 withRespectTo: CoordSystem2d;
 xAxisChoices: ARRAY[1..2] OF NAT;
 xAxis, newXAxis, yAxis, newYAxis: Vector2d;
 xNegArray: ARRAY[1..2] OF BOOL;
 origin: Point2d;
 angle: REAL;
 IF b = NIL THEN bPad ← SVMatrix2d.Identity[]
 ELSE bPad ← FindInTermsOfPad[b];
 aPad ← FindInTermsOfPad[a];
 aInTermsOfb ← IF a = b THEN SVMatrix2d.Identity[] ELSE
  SVMatrix2d.WorldToLocal[bPad,aPad];
  
 xAxis ← SVMatrix2d.XAxisOfMatrix[aInTermsOfb];
 yAxis ← SVMatrix2d.YAxisOfMatrix[aInTermsOfb];
 origin ← SVMatrix2d.OriginOfMatrix[aInTermsOfb];
 angle ← SVVector2d.SmallestAngleBetweenVectors[xAxis, yAxis];
 [xAxisChoices, xNegArray] ← IndicesOfMax[xAxis]; -- Find largest component
  -- this is the new x axis direction.
 newXAxis ← [0,0];
 newXAxis[xAxisChoices[1]] ← SVVector2d.Magnitude[xAxis];
 IF xNegArray[xAxisChoices[1]]
  THEN newXAxis[xAxisChoices[1]] ← -newXAxis[xAxisChoices[1]];
 newYAxis ← SVVector2d.VectorPlusAngle[newXAxis, angle]; -- preserve angle between x axis and y axis
 newYAxis ← SVVector2d.Scale[newYAxis, SVVector2d.Magnitude[yAxis]];
  -- preserve magnitude of axes
 
 aInTermsOfb ← SVMatrix2d.MakeMatFromAxes[newXAxis, newYAxis, origin];
 
 newAPad ← SVMatrix2d.MatMult[bPad, aInTermsOfb];
 withRespectTo ← a.withRespectTo;
 withRespectToPad ← FindInTermsOfPad[withRespectTo];
 aInTermsOfWRT ← SVMatrix2d.WorldToLocal[withRespectToPad, newAPad];
 a.mat ← aInTermsOfWRT;
 TellCoordSysAboutPad[a];
 };
 

AbutAwrtB: PUBLIC PROC [a: CoordSystem2d, b: CoordSystem2d ← NIL] = {
 -- equivalent to PlaceTranslationAwrtB where origin = [0, 0]
 PlaceTranslationAwrtB[a, b, [0, 0]];
 };


-- these perform the named transform but do not change the coordinate system with respect to which CoordSystem a is defined.
-- when b is NIL, assumes b = PAD.
-- for local transforms, let b = a.


END.