Unit ThreeD;

Interface

Const
    MatrixSize	= 4;
    AlmostZero	= 1.0E-10;

Type
    Vector	= Array [1..MatrixSize] of Real;
    Matrix	= Array [1..MatrixSize, 1..MatrixSize] of Real;

Function  Dot_Product (U, V : Vector; N : Integer) : Real;
Procedure Cross_Product_3 (U, V : Vector; Var W : Vector);
Procedure Matrix_Matrix_Multiply (A, B : Matrix; Var C : Matrix);
Procedure Matrix_Vector_Multiply (A : Matrix; B : Vector; Var C : Vector);
Procedure Vector_Matrix_Multiply (A : Vector; B : Matrix; Var C : Vector);
Procedure Make_Identity_Matrix (Var M : Matrix);
Procedure Translate_Matrix_3 (Tx, Ty, Tz : Real; Var M : Matrix);
Procedure Scale_Matrix_3 (Sx, Sy, Sz : Real; Var M : Matrix);
Procedure Rotate_X_Matrix_3 (A : Real; Var M : Matrix);
Procedure Rotate_Y_Matrix_3 (A : Real; Var M : Matrix);
Procedure Rotate_Z_Matrix_3 (A : Real; Var M : Matrix);
Procedure Set_Translate_Matrix_3 (Tx, Ty, Tz : Real; Var M : Matrix);
Procedure Set_Scale_Matrix_3 (Sx, Sy, Sz : Real; Var M : Matrix);
Procedure Set_Rotate_X_Matrix_3 (CosA, SinA : Real; Var M : Matrix);
Procedure Set_Rotate_Y_Matrix_3 (CosA, SinA : Real; Var M : Matrix);
Procedure Set_Rotate_Z_Matrix_3 (CosA, SinA : Real; Var M : Matrix);
Procedure Set_Coordinates (Ex, Ey, Ez, VRx, VRy, VRz, Vx, Vy, Vz: Real;
				Var M : Matrix);
Procedure Make_Perspective_Matrix (D : Real; Var M : Matrix);
Procedure Deflate (V : Vector; M : Matrix; Var X, Y : Real);

Implementation

(*-------------------------------------------------------------------------*)

Function Dot_Product (U, V : Vector; N : Integer) : Real;

Var
    I	: Integer;
    Sum	: Real;

Begin
    Sum := 0.0;
    For I := 1 to N do
	Sum := Sum + U[I] * V[I];
    Dot_Product := Sum;
End; {Dot_Product}

(*-------------------------------------------------------------------------*)

Procedure Cross_Product_3 (U, V : Vector; Var W : Vector);

Begin
    W[1] := U[2]*V[3] - U[3]*V[2];
    W[2] := U[3]*V[1] - U[1]*V[3];
    W[3] := U[1]*V[2] - U[2]*V[1];
    W[4] := 1.0;
End; {Cross_Product_3}

(*-------------------------------------------------------------------------*)

Procedure Matrix_Matrix_Multiply (A, B : Matrix; Var C : Matrix);

(*
 * Forms the matrix product C = A B.
 *)

Var
    I, J, K	: Integer;

Begin
    For I := 1 to MatrixSize do
	For J := 1 to MatrixSize do Begin
	    C[I,J] := 0.0;
	    For K := 1 to MatrixSize do
		C[I,J] := C[I,J] + A[I,K] * B[K,J];
	End; {For}
End; {Matrix_Matrix_Multiply}

(*-------------------------------------------------------------------------*)

Procedure Matrix_Vector_Multiply (A : Matrix; B : Vector; Var C : Vector);

(*
 * Forms the matrix-vector product C = A B.
 *)

Var
    I, J	: Integer;

Begin
    For I := 1 to MatrixSize do Begin
	C[I] := 0.0;
	For J := 1 to MatrixSize do
	    C[I] := C[I] + A[I,J] * B[J];
    End; {For}
End; {Matrix_Vector_Multiply}

(*-------------------------------------------------------------------------*)

Procedure Vector_Matrix_Multiply (A : Vector; B : Matrix; Var C : Vector);

(*
 * Forms the vector-matrix product C = A B.
 *)

Var
    I, J	: Integer;

Begin
    For I := 1 to MatrixSize do Begin
	C[I] := 0.0;
	For J := 1 to MatrixSize do
	    C[I] := C[I] + A[J] * B[J,I];
    End; {For}
End; {Vector_Matrix_Multiply}

(*-------------------------------------------------------------------------*)

Procedure Make_Identity_Matrix (Var M : Matrix);

(*
 * Produces an identity matrix.
 *)

Var
    I, J	: Integer;

Begin
    For J := 1 to MatrixSize do Begin
	For I := 1 to MatrixSize do
	    M[I,J] := 0.0;
	M[J,J] := 1.0;
    End; {For}
End; {Make_Identity_Matrix}

(*-------------------------------------------------------------------------*)

Procedure Translate_Matrix_3 (Tx, Ty, Tz : Real; Var M : Matrix);

Begin
    M[4,1] := M[4,1] + Tx;
    M[4,2] := M[4,2] + Ty;
    M[4,3] := M[4,3] + Tz;
End; {Translate_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Scale_Matrix_3 (Sx, Sy, Sz : Real; Var M : Matrix);

Var
    I	: Integer;

Begin
    For I := 1 to MatrixSize do Begin
	M[I,1] := M[I,1] * Sx;
	M[I,2] := M[I,2] * Sy;
	M[I,3] := M[I,3] * Sz;
    End; {For}
End; {Scale_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Rotate_X_Matrix_3 (A : Real; Var M : Matrix);

Var
    I		: Integer;
    C, S	: Real;
    T1, T2	: Real;

Begin
    C := Cos(A);
    S := Sin(A);
    For I := 1 to MatrixSize do Begin
	T1 := C * M[I,2] - S * M[I,3];
	T2 := S * M[I,2] + C * M[I,3];
	M[I,2] := T1;
	M[I,3] := T2;
    End; {For}
End; {Rotate_X_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Rotate_Y_Matrix_3 (A : Real; Var M : Matrix);

Var
    I		: Integer;
    C, S	: Real;
    T1, T2	: Real;

Begin
    C := Cos(A);
    S := Sin(A);
    For I := 1 to MatrixSize do Begin
	T1 := C * M[I,1] + S * M[I,3];
	T2 := - S * M[I,1] + C * M[I,3];
	M[I,1] := T1;
	M[I,3] := T2;
    End; {For}
End; {Rotate_Y_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Rotate_Z_Matrix_3 (A : Real; Var M : Matrix);

Var
    I		: Integer;
    C, S	: Real;
    T1, T2	: Real;

Begin
    C := Cos(A);
    S := Sin(A);
    For I := 1 to MatrixSize do Begin
	T1 := C * M[I,1] - S * M[I,2];
	T2 := S * M[I,1] + C * M[I,2];
	M[I,1] := T1;
	M[I,2] := T2;
    End; {For}
End; {Rotate_Z_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Set_Translate_Matrix_3 (Tx, Ty, Tz : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[4,1] := Tx;
    M[4,2] := Ty;
    M[4,3] := Tz;
End; {Set_Translate_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Set_Scale_Matrix_3 (Sx, Sy, Sz : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[1,1] := Sx;
    M[2,2] := Sy;
    M[3,3] := Sz;
End; {Set_Scale_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Set_Rotate_X_Matrix_3 (CosA, SinA : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[2,2] := CosA;
    M[3,3] := CosA;
    M[2,3] := SinA;
    M[3,2] := -SinA;
End; {Set_Rotate_X_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Set_Rotate_Y_Matrix_3 (CosA, SinA : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[1,1] := CosA;
    M[3,3] := CosA;
    M[1,3] := -SinA;
    M[3,1] := SinA;
End; {Set_Rotate_Y_Matrix_3}

(*-------------------------------------------------------------------------*)

Procedure Set_Rotate_Z_Matrix_3 (CosA, SinA : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[1,1] := CosA;
    M[2,2] := CosA;
    M[1,2] := SinA;
    M[2,1] := -SinA;
End; {Set_Rotate_Z_Matrix_3}

(*-------------------------------------------------------------------------*)
(*-------------------------------------------------------------------------*)
(*-------------------------------------------------------------------------*)

Procedure Set_Coordinates (Ex, Ey, Ez, VRx, VRy, VRz, Vx, Vy, Vz: Real;
			Var M : Matrix);

Var
    N	: Vector;
    V	: Vector;
    R	: Matrix;
    I	: Integer;
    D	: Real;

Begin

    (*
     * Set up normal vector `N' to viewing plane.  Points along -Z
     * axis of viewing coordinate system (VCS) assuming VCS is a
     * right-handed coordinate system.
     *)
    N[1] := VRx - Ex;
    N[2] := VRy - Ey;
    N[3] := VRz - Ez;
    N[4] := 1.0;
    D := Sqrt (Dot_Product(N, N, 3));
    For I := 1 to 3 do
	N[I] := N[I] / D;

    (*
     * Compute up-vector in direction of (Vx, Vy, Vz) but perpendicular
     * to the normal vector.  Essentially an orthogonal projection of
     * (Vx, Vy, Vz) onto plane perpendicular to `N'.  Make sure that
     * the refined up-vector is a unit vector.  `V' points along
     * +Y direction of VCS.
     *)
    V[1] := Vx;
    V[2] := Vy;
    V[3] := Vz;
    V[4] := 1.0;
    D := Dot_Product (V, N, 3);
    For I := 1 to 3 do
	V[I] := V[I] - D * N[I];
    D := Sqrt (Dot_Product (V, V, 3));
    For I := 1 to 3 do
	V[I] := V[I] / D;

    (*
     * Figure out how to translate and rotate to line the VCS up with
     * the world coordinate system (WCS).  Note that the normal vector `N'
     * points in the -Z direction of the VCS, assuming that the VCS is a
     * right-handed coordinate system, so that `-N' points in the +Z
     * direction.  Also, `V' points in the positive Y direction of the
     * VCS.
     *
     * Begin by translating VCS origin (eyepoint) to WCS origin.
     *)
    Make_Identity_Matrix (M);
    Translate_Matrix_3 (-Ex, -Ey, -Ez, M);

    (*
     * Now rotate about X axis to bring VCS Z axis into WCS XZ plane.
     *)
    D := Sqrt(N[2]*N[2] + N[3]*N[3]);
    If Abs(D) > AlmostZero then Begin
	Set_Rotate_X_Matrix_3 (-N[3]/D, -N[2]/D, R);
	Matrix_Matrix_Multiply (M, R, M);
	Vector_Matrix_Multiply (V, R, V);
    End; {If}

    (*
     * Rotate about Y axis to bring VCS Z axis coincident with WCS Z axis.
     *)
    Set_Rotate_Y_Matrix_3 (D, N[1], R);
    Matrix_Matrix_Multiply (M, R, M);
    Vector_Matrix_Multiply (V, R, V);

    (*
     * Finally, rotate about Z axis to make VCS Y axis coincident with
     * WCS Y axis.
     *)
    D := Sqrt(Sqr(V[1]) + Sqr(V[2]));
    If Abs(D) > AlmostZero then Begin
	Set_Rotate_Z_Matrix_3 (V[2]/D, V[1]/D, R);
	Matrix_Matrix_Multiply (M, R, M);
    End; {If}
End; {Set_Coordinates}

(*-------------------------------------------------------------------------*)

Procedure Make_Perspective_Matrix (D : Real; Var M : Matrix);

Begin
    Make_Identity_Matrix (M);
    M[3,3] := 0.0;
    M[3,4] := -1.0 / D;
End; {Make_Perspective_Matrix}

(*-------------------------------------------------------------------------*)

Procedure Deflate (V : Vector; M : Matrix; Var X, Y : Real);

Begin
    Vector_Matrix_Multiply (V, M, V);
    X := V[1] / V[4] + 0.5;
    Y := V[2] / V[4] + 0.5;
End; {Deflate}

(*-------------------------------------------------------------------------*)

End. {ThreeD}