-- transformation package
--M. Stone September 24, 1980  3:03 PM
-- Last Edited by: Stone, January 27, 1983 3:34 pm

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

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