-- transformation package
--M. Stone September 24, 1980 3:03 PM
-- Last Edited by: Stone, September 19, 1985 12:21:24 pm PDT

DIRECTORY
 XFormDefs: FROM "XFormDefs",
 RealFns: FROM "RealFns",
 Real: FROM "Real",
 Rope USING [ROPE],
 PointDefs: FROM "PointDefs";

XFormFns: CEDAR PROGRAM IMPORTS RealFns,Real EXPORTS XFormDefs=
BEGIN OPEN XFormDefs,PointDefs;
ProblemWithXForms: PUBLIC SIGNAL[string: Rope.ROPE] = CODE;

--Transform matrix. 2 by 2 plus translation.
-- X← matrix[1][X]*X+matrix[2][X]*Y + matrix[3][X]
-- Y← matrix[1][Y]*X+matrix[2][Y]*Y + matrix[3][Y]

InitXForms: PUBLIC PROCEDURE[matrix: XFMDescriptor] =
BEGIN
matrix[1] ← [1,0];
matrix[2] ← [0,1];
matrix[3] ← [0,0];
END;

--theta in radians
Rotate: PUBLIC PROCEDURE[ theta: REAL, axis: XFormDefs.Axis,matrix: XFMDescriptor] =
BEGIN
rot: ARRAY [1..2] OF ObjPt ← [[0,0],[0,0]];
sin: REAL ← RealFns.Sin[theta];
cos: REAL ← RealFns.Cos[theta];
tmatrix: XFormMatrix;
i: INTEGER;
FOR i IN [1..3] DO tmatrix[i] ← matrix[i]; ENDLOOP;
SELECT axis FROM
 x =>BEGIN
  rot[1][X] ← 1;
  rot[2][Y] ← cos;
  END;
 y =>BEGIN
  rot[1][X] ← cos;
  rot[2][Y] ← 1;
  END;
 z =>BEGIN
  rot[1][X] ← cos;
  rot[2][Y] ← cos;
  rot[1][Y] ← sin;
  rot[2][X] ← -sin;
  END;
 ENDCASE;
FOR i IN [X..Y] DO
 matrix[1][i] ← rot[1][X]*tmatrix[1][i]+rot[1][Y]*tmatrix[2][i];
 matrix[2][i] ← rot[2][X]*tmatrix[1][i]+rot[2][Y]*tmatrix[2][i];
 ENDLOOP;
TestSingular[matrix];
END;

Scale: PUBLIC PROCEDURE[ scale: ARRAY[X..Y] OF REAL,matrix: XFMDescriptor] =
BEGIN
i: INTEGER;
FOR i IN [X..Y] DO
 matrix[1][i] ← matrix[1][i]*scale[X];
 matrix[2][i] ← matrix[2][i]*scale[Y];
 ENDLOOP;
TestSingular[matrix];
END;

Translate: PUBLIC PROCEDURE[ dist: ARRAY[X..Y] OF REAL,matrix: XFMDescriptor] =
BEGIN
i: INTEGER;
FOR i IN [X..Y] DO
 matrix[3][i] ← matrix[1][i]*dist[X]+matrix[2][i]*dist[Y]+matrix[3][i];
 ENDLOOP;
END;

--from Draw. To map q0,q1,q2 into p0,p1,p2, solve the equations for a,b,c,d.
-- xnew ← ax+by+distX
-- ynew ← cx+dy+distY

--a, b, c, d stored in matrix[1][X], matrix[2][X], matrix[1][Y], matrix2[Y] respectively
--dist = translation from p0 to q0 is done separately

--For 4 points, a=d and b=-c. Work out the algebra and get formulae below.
--q0,q1, p0,p1 in pts[0] to pts[3], respectively
--x1=qx1-qx0; x2=px1-px0; y similarly

XForm4Pts: PUBLIC PROCEDURE[ pts: PointDefs.ObjPtSequence,matrix: XFMDescriptor] =
BEGIN
tmatrix: XFormMatrix;
i: INTEGER;
x1: REAL ← pts[1][X]-pts[0][X];
x2: REAL ← pts[3][X]-pts[2][X];
y1: REAL ← pts[1][Y]-pts[0][Y];
y2: REAL ← pts[3][Y]-pts[2][Y];
del: REAL ← x1*x1+y1*y1;
tmp: ARRAY [1..3] OF ObjPt ← [[0,0],[0,0],[0,0]];
FOR i IN [1..3] DO tmatrix[i] ← matrix[i]; ENDLOOP;
tmp[1][X] ← (x1*x2+y1*y2)/del; --a
tmp[2][Y] ← tmp[1][X]; --d
tmp[1][Y] ← (x1*y2-y1*x2)/del; --c
tmp[2][X] ← -tmp[1][Y] ; --b
tmp[3] ← [pts[2][X]-pts[0][X],pts[2][Y]-pts[0][Y]]; --distX and distY
FOR i IN [X..Y] DO
 matrix[1][i] ← tmp[1][X]*tmatrix[1][i]+tmp[1][Y]*tmatrix[2][i];
 matrix[2][i] ← tmp[2][X]*tmatrix[1][i]+tmp[2][Y]*tmatrix[2][i];
 matrix[3][i] ← tmp[3][X]*tmatrix[1][i]+tmp[3][Y]*tmatrix[2][i] + tmatrix[3][i];
 ENDLOOP;
TestSingular[matrix];
END;

--For 6 points. Work out the algebra and get formulae below.
--q0,q1,q2, p0,p1,q3 in pts[0] to pts[5], respectively
XForm6Pts: PUBLIC PROCEDURE[ pts: PointDefs.ObjPtSequence,matrix: XFMDescriptor] =
BEGIN
i: INTEGER;
tmatrix: XFormMatrix;
tmp: ARRAY [1..3] OF ObjPt ← [[0,0],[0,0],[0,0]];
dq1: ObjPt ← [pts[1][X]-pts[0][X],pts[1][Y]-pts[0][Y]];
dq2: ObjPt ← [pts[2][X]-pts[0][X],pts[2][Y]-pts[0][Y]];
dp1: ObjPt ← [pts[4][X]-pts[3][X],pts[4][Y]-pts[3][Y]];
dp2: ObjPt ← [pts[5][X]-pts[3][X],pts[5][Y]-pts[3][Y]];
del: REAL ← dq1[X]*dq2[Y]-dq2[X]*dq1[Y];
IF del=0 THEN SIGNAL ProblemWithXForms["Invalid Map. Colinear q points"];
FOR i IN [1..3] DO tmatrix[i] ← matrix[i]; ENDLOOP;
tmp[1][X] ← (dp1[X]*dq2[Y]-dp2[X]*dq1[Y])/del; --a
tmp[2][X] ← (dq1[X]*dp2[X]-dq2[X]*dp1[X])/del; --b
tmp[1][Y] ← (dp1[Y]*dq2[Y]-dp2[Y]*dq1[Y])/del; --c
tmp[2][Y] ← (dq1[X]*dp2[Y]-dq2[X]*dp1[Y])/del; --d
tmp[3] ← [pts[3][X]-pts[0][X],pts[3][Y]-pts[0][Y]]; --distX and distY
FOR i IN [X..Y] DO
 matrix[1][i] ← tmp[1][X]*tmatrix[1][i]+tmp[1][Y]*tmatrix[2][i];
 matrix[2][i] ← tmp[2][X]*tmatrix[1][i]+tmp[2][Y]*tmatrix[2][i];
 matrix[3][i] ← tmp[3][X]*tmatrix[1][i]+tmp[3][Y]*tmatrix[2][i] + tmatrix[3][i];
 ENDLOOP;
TestSingular[matrix];
END;

XFormPt: PUBLIC PROCEDURE [pt: ObjPt,matrix: XFMDescriptor] RETURNS[ObjPt] =
BEGIN
newPt: ObjPt;
i: INTEGER;
FOR i IN [X..Y] DO
 newPt[i] ← matrix[1][i]*pt[X]+matrix[2][i]*pt[Y]+matrix[3][i];
 ENDLOOP;
RETURN[newPt];
END;

TestSingular: PROCEDURE [matrix: XFMDescriptor] = TRUSTED
BEGIN ENABLE Real.RealException =>
 IF flags[divisionByZero] THEN SIGNAL ProblemWithXForms["Singular transform"]
 ELSE SIGNAL ProblemWithXForms["Other problem with transform"];

det: REAL ← matrix[1][X]*matrix[2][Y]-matrix[2][X]*matrix[1][Y];
Nest[det]; --well, there's something funny about the signals in the float package
END;

Nest: PROCEDURE [det: REAL] =
BEGIN
test: REAL ← 1/det; --all for the SIGNAL if det is 0
END;
END.