path: root/src/lib/JEDI-SDL/OpenGL/Pas/geometry.pas

unit geometry;
{
  $Id: geometry.pas,v 1.1 2004/03/30 21:53:54 savage Exp $
  
}

// This unit contains many needed types, functions and procedures for
// quaternion, vector and matrix arithmetics. It is specifically designed
// for geometric calculations within R3 (affine vector space)
// and R4 (homogeneous vector space).
//
// Note: The terms 'affine' or 'affine coordinates' are not really correct here
//       because an 'affine transformation' describes generally a transformation which leads
//       to a uniquely solvable system of equations and has nothing to do with the dimensionality
//       of a vector. One could use 'projective coordinates' but this is also not really correct
//       and since I haven't found a better name (or even any correct one), 'affine' is as good
//       as any other one.
//
// Identifiers containing no dimensionality (like affine or homogeneous)
// and no datatype (integer..extended) are supposed as R4 representation
// with 'single' floating point type (examples are TVector, TMatrix,
// and TQuaternion). The default data type is 'single' ('GLFloat' for OpenGL)
// and used in all routines (except conversions and trigonometric functions).
//
// Routines with an open array as argument can either take Func([1,2,3,4,..]) or Func(Vect).
// The latter is prefered, since no extra stack operations is required.
// Note: Be careful while passing open array elements! If you pass more elements
//       than there's room in the result the behaviour will be unpredictable.
//
// If not otherwise stated, all angles are given in radians
// (instead of degrees). Use RadToDeg or DegToRad to convert between them.
//
// Geometry.pas was assembled from different sources (like GraphicGems)
// and relevant books or based on self written code, respectivly.
//
// Note: Some aspects need to be considered when using Delphi and pure
//       assembler code. Delphi ensures that the direction flag is always
//       cleared while entering a function and expects it cleared on return.
//       This is in particular important in routines with (CPU) string commands (MOVSD etc.)
//       The registers EDI, ESI and EBX (as well as the stack management
//       registers EBP and ESP) must not be changed! EAX, ECX and EDX are
//       freely available and mostly used for parameter.
//
// Version 2.5
// last change : 04. January 2000
//
// (c) Copyright 1999, Dipl. Ing. Mike Lischke (public@lischke-online.de)
{
  $Log: geometry.pas,v $
  Revision 1.1  2004/03/30 21:53:54  savage
  Moved to it's own folder.

  Revision 1.1  2004/02/05 00:08:19  savage
  Module 1.0 release

  
}

interface

{$I jedi-sdl.inc}

type
  // data types needed for 3D graphics calculation,
  // included are 'C like' aliases for each type (to be
  // conformal with OpenGL types)

  PByte = ^Byte;
  PWord = ^Word;
  PInteger = ^Integer;
  PFloat = ^Single;
  PDouble = ^Double;
  PExtended = ^Extended;
  PPointer = ^Pointer;

  // types to specify continous streams of a specific type
  // switch off range checking to access values beyond the limits
  PByteVector = ^TByteVector;
  PByteArray = PByteVector;
  TByteVector = array[0..0] of Byte;

  PWordVector = ^TWordVector;
  PWordArray = PWordVector;  // note: there's a same named type in SysUtils
  TWordVector = array[0..0] of Word;

  PIntegerVector = ^TIntegerVector;
  PIntegerArray = PIntegerVector;
  TIntegerVector = array[0..0] of Integer;

  PFloatVector = ^TFloatVector;
  PFloatArray = PFloatVector;
  TFloatVector = array[0..0] of Single;

  PDoubleVector = ^TDoubleVector;
  PDoubleArray = PDoubleVector;
  TDoubleVector = array[0..0] of Double;

  PExtendedVector = ^TExtendedVector;
  PExtendedArray = PExtendedVector;
  TExtendedVector = array[0..0] of Extended;

  PPointerVector = ^TPointerVector;
  PPointerArray = PPointerVector;
  TPointerVector = array[0..0] of Pointer;

  PCardinalVector = ^TCardinalVector;
  PCardinalArray = PCardinalVector;
  TCardinalVector = array[0..0] of Cardinal;

  // common vector and matrix types with predefined limits
  // indices correspond like: x -> 0
  //                          y -> 1
  //                          z -> 2
  //                          w -> 3

  PHomogeneousByteVector = ^THomogeneousByteVector;
  THomogeneousByteVector = array[0..3] of Byte;
  TVector4b = THomogeneousByteVector;

  PHomogeneousWordVector = ^THomogeneousWordVector;
  THomogeneousWordVector = array[0..3] of Word;
  TVector4w = THomogeneousWordVector;

  PHomogeneousIntVector = ^THomogeneousIntVector;
  THomogeneousIntVector = array[0..3] of Integer;
  TVector4i = THomogeneousIntVector;

  PHomogeneousFltVector = ^THomogeneousFltVector;
  THomogeneousFltVector = array[0..3] of Single;
  TVector4f = THomogeneousFltVector;

  PHomogeneousDblVector = ^THomogeneousDblVector;
  THomogeneousDblVector = array[0..3] of Double;
  TVector4d = THomogeneousDblVector;

  PHomogeneousExtVector = ^THomogeneousExtVector;
  THomogeneousExtVector = array[0..3] of Extended;
  TVector4e = THomogeneousExtVector;

  PHomogeneousPtrVector = ^THomogeneousPtrVector;
  THomogeneousPtrVector = array[0..3] of Pointer;
  TVector4p = THomogeneousPtrVector;

  PAffineByteVector = ^TAffineByteVector;
  TAffineByteVector = array[0..2] of Byte;
  TVector3b = TAffineByteVector;

  PAffineWordVector = ^TAffineWordVector;
  TAffineWordVector = array[0..2] of Word;
  TVector3w = TAffineWordVector;

  PAffineIntVector = ^TAffineIntVector;
  TAffineIntVector = array[0..2] of Integer;
  TVector3i = TAffineIntVector;

  PAffineFltVector = ^TAffineFltVector;
  TAffineFltVector = array[0..2] of Single;
  TVector3f = TAffineFltVector;

  PAffineDblVector = ^TAffineDblVector;
  TAffineDblVector = array[0..2] of Double;
  TVector3d = TAffineDblVector;

  PAffineExtVector = ^TAffineExtVector;
  TAffineExtVector = array[0..2] of Extended;
  TVector3e = TAffineExtVector;

  PAffinePtrVector = ^TAffinePtrVector;
  TAffinePtrVector = array[0..2] of Pointer;
  TVector3p = TAffinePtrVector;

  // some simplified names
  PVector = ^TVector;
  TVector = THomogeneousFltVector;

  PHomogeneousVector = ^THomogeneousVector;
  THomogeneousVector = THomogeneousFltVector;

  PAffineVector = ^TAffineVector;
  TAffineVector = TAffineFltVector;

  // arrays of vectors
  PVectorArray = ^TVectorArray;
  TVectorArray = array[0..0] of TAffineVector;

  // matrices
  THomogeneousByteMatrix = array[0..3] of THomogeneousByteVector;
  TMatrix4b = THomogeneousByteMatrix;

  THomogeneousWordMatrix = array[0..3] of THomogeneousWordVector;
  TMatrix4w = THomogeneousWordMatrix;

  THomogeneousIntMatrix = array[0..3] of THomogeneousIntVector;
  TMatrix4i = THomogeneousIntMatrix;

  THomogeneousFltMatrix  = array[0..3] of THomogeneousFltVector;
  TMatrix4f = THomogeneousFltMatrix;

  THomogeneousDblMatrix = array[0..3] of THomogeneousDblVector;
  TMatrix4d = THomogeneousDblMatrix;

  THomogeneousExtMatrix = array[0..3] of THomogeneousExtVector;
  TMatrix4e = THomogeneousExtMatrix;

  TAffineByteMatrix = array[0..2] of TAffineByteVector;
  TMatrix3b = TAffineByteMatrix;

  TAffineWordMatrix = array[0..2] of TAffineWordVector;
  TMatrix3w = TAffineWordMatrix;

  TAffineIntMatrix = array[0..2] of TAffineIntVector;
  TMatrix3i = TAffineIntMatrix;

  TAffineFltMatrix = array[0..2] of TAffineFltVector;
  TMatrix3f = TAffineFltMatrix;

  TAffineDblMatrix = array[0..2] of TAffineDblVector;
  TMatrix3d = TAffineDblMatrix;

  TAffineExtMatrix = array[0..2] of TAffineExtVector;
  TMatrix3e = TAffineExtMatrix;

  // some simplified names
  PMatrix = ^TMatrix;
  TMatrix = THomogeneousFltMatrix;

  PHomogeneousMatrix = ^THomogeneousMatrix;
  THomogeneousMatrix = THomogeneousFltMatrix;

  PAffineMatrix = ^TAffineMatrix;
  TAffineMatrix = TAffineFltMatrix;

  // q = ([x, y, z], w)
  TQuaternion = record
    case Integer of
      0:
        (ImagPart: TAffineVector;
         RealPart: Single);
      1:
        (Vector: TVector4f);
  end;

  TRectangle = record
    Left,
    Top,
    Width,
    Height: Integer;
  end;

  TTransType = (ttScaleX, ttScaleY, ttScaleZ,
                ttShearXY, ttShearXZ, ttShearYZ,
                ttRotateX, ttRotateY, ttRotateZ,
                ttTranslateX, ttTranslateY, ttTranslateZ,
                ttPerspectiveX, ttPerspectiveY, ttPerspectiveZ, ttPerspectiveW);

  // used to describe a sequence of transformations in following order:
  // [Sx][Sy][Sz][ShearXY][ShearXZ][ShearZY][Rx][Ry][Rz][Tx][Ty][Tz][P(x,y,z,w)]
  // constants are declared for easier access (see MatrixDecompose below)
  TTransformations  = array[TTransType] of Single;


const
  // useful constants

  // standard vectors
  XVector: TAffineVector = (1, 0, 0);
  YVector: TAffineVector = (0, 1, 0);
  ZVector: TAffineVector = (0, 0, 1);
  NullVector: TAffineVector = (0, 0, 0);

  IdentityMatrix: TMatrix = ((1, 0, 0, 0),
                             (0, 1, 0, 0),
                             (0, 0, 1, 0),
                             (0, 0, 0, 1));
  EmptyMatrix: TMatrix = ((0, 0, 0, 0),
                          (0, 0, 0, 0),
                          (0, 0, 0, 0),
                          (0, 0, 0, 0));
  // some very small numbers
  EPSILON  = 1e-100;
  EPSILON2 = 1e-50;

//----------------------------------------------------------------------------------------------------------------------

// vector functions
function  VectorAdd(V1, V2: TVector): TVector;
function  VectorAffineAdd(V1, V2: TAffineVector): TAffineVector;
function  VectorAffineCombine(V1, V2: TAffineVector; F1, F2: Single): TAffineVector;
function  VectorAffineDotProduct(V1, V2: TAffineVector): Single;
function  VectorAffineLerp(V1, V2: TAffineVector; t: Single): TAffineVector;
function  VectorAffineSubtract(V1, V2: TAffineVector): TAffineVector;
function  VectorAngle(V1, V2: TAffineVector): Single;
function  VectorCombine(V1, V2: TVector; F1, F2: Single): TVector;
function  VectorCrossProduct(V1, V2: TAffineVector): TAffineVector;
function  VectorDotProduct(V1, V2: TVector): Single;
function  VectorLength(V: array of Single): Single;
function  VectorLerp(V1, V2: TVector; t: Single): TVector;
procedure VectorNegate(V: array of Single);
function  VectorNorm(V: array of Single): Single;
function  VectorNormalize(V: array of Single): Single;
function  VectorPerpendicular(V, N: TAffineVector): TAffineVector;
function  VectorReflect(V, N: TAffineVector): TAffineVector;
procedure VectorRotate(var Vector: TVector4f; Axis: TVector3f; Angle: Single);
procedure VectorScale(V: array of Single; Factor: Single);
function  VectorSubtract(V1, V2: TVector): TVector;

// matrix functions
function  CreateRotationMatrixX(Sine, Cosine: Single): TMatrix;
function  CreateRotationMatrixY(Sine, Cosine: Single): TMatrix;
function  CreateRotationMatrixZ(Sine, Cosine: Single): TMatrix;
function  CreateScaleMatrix(V: TAffineVector): TMatrix;
function  CreateTranslationMatrix(V: TVector): TMatrix;
procedure MatrixAdjoint(var M: TMatrix);
function  MatrixAffineDeterminant(M: TAffineMatrix): Single;
procedure MatrixAffineTranspose(var M: TAffineMatrix);
function  MatrixDeterminant(M: TMatrix): Single;
procedure MatrixInvert(var M: TMatrix);
function  MatrixMultiply(M1, M2: TMatrix): TMatrix;
procedure MatrixScale(var M: TMatrix; Factor: Single);
procedure MatrixTranspose(var M: TMatrix);

// quaternion functions
function  QuaternionConjugate(Q: TQuaternion): TQuaternion;
function  QuaternionFromPoints(V1, V2: TAffineVector): TQuaternion;
function  QuaternionMultiply(qL, qR: TQuaternion): TQuaternion;
function  QuaternionSlerp(QStart, QEnd: TQuaternion; Spin: Integer; t: Single): TQuaternion;
function  QuaternionToMatrix(Q: TQuaternion): TMatrix;
procedure QuaternionToPoints(Q: TQuaternion; var ArcFrom, ArcTo: TAffineVector);

// mixed functions
function  ConvertRotation(Angles: TAffineVector): TVector;
function  CreateRotationMatrix(Axis: TVector3f; Angle: Single): TMatrix;
function  MatrixDecompose(M: TMatrix; var Tran: TTransformations): Boolean;
function  VectorAffineTransform(V: TAffineVector; M: TAffineMatrix): TAffineVector;
function  VectorTransform(V: TVector4f; M: TMatrix): TVector4f; overload;
function  VectorTransform(V: TVector3f; M: TMatrix): TVector3f; overload;

// miscellaneous functions
function  MakeAffineDblVector(V: array of Double): TAffineDblVector;
function  MakeDblVector(V: array of Double): THomogeneousDblVector;
function  MakeAffineVector(V: array of Single): TAffineVector;
function  MakeQuaternion(Imag: array of Single; Real: Single): TQuaternion;
function  MakeVector(V: array of Single): TVector;
function  PointInPolygon(xp, yp : array of Single; x, y: Single): Boolean;
function  VectorAffineDblToFlt(V: TAffineDblVector): TAffineVector;
function  VectorDblToFlt(V: THomogeneousDblVector): THomogeneousVector;
function  VectorAffineFltToDbl(V: TAffineVector): TAffineDblVector;
function  VectorFltToDbl(V: TVector): THomogeneousDblVector;

// trigonometric functions
function  ArcCos(X: Extended): Extended;
function  ArcSin(X: Extended): Extended;
function  ArcTan2(Y, X: Extended): Extended;
function  CoTan(X: Extended): Extended;
function  DegToRad(Degrees: Extended): Extended;
function  RadToDeg(Radians: Extended): Extended;
procedure SinCos(Theta: Extended; var Sin, Cos: Extended);
function  Tan(X: Extended): Extended;

// coordinate system manipulation functions
function Turn(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Turn(Matrix: TMatrix; MasterUp: TAffineVector; Angle: Single): TMatrix; overload;
function Pitch(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Pitch(Matrix: TMatrix; MasterRight: TAffineVector; Angle: Single): TMatrix; overload;
function Roll(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Roll(Matrix: TMatrix; MasterDirection: TAffineVector; Angle: Single): TMatrix; overload;

//----------------------------------------------------------------------------------------------------------------------

implementation

const
  // FPU status flags (high order byte)
  C0 = 1;
  C1 = 2;
  C2 = 4;
  C3 = $40;

  // to be used as descriptive indices
  X = 0;
  Y = 1;
  Z = 2;
  W = 3;

//----------------- trigonometric helper functions ---------------------------------------------------------------------

function DegToRad(Degrees: Extended): Extended;

begin
  Result := Degrees * (PI / 180);
end;

//----------------------------------------------------------------------------------------------------------------------

function RadToDeg(Radians: Extended): Extended;

begin
  Result := Radians * (180 / PI);
end;

//----------------------------------------------------------------------------------------------------------------------

procedure SinCos(Theta: Extended; var Sin, Cos: Extended); assembler; register;

// calculates sine and cosine from the given angle Theta
// EAX contains address of Sin
// EDX contains address of Cos
// Theta is passed over the stack

asm
              FLD  Theta
              FSINCOS
              FSTP TBYTE PTR [EDX]    // cosine
              FSTP TBYTE PTR [EAX]    // sine
              FWAIT
end;

//----------------------------------------------------------------------------------------------------------------------

function ArcCos(X: Extended): Extended;

begin
  Result := ArcTan2(Sqrt(1 - X * X), X);
end;

//----------------------------------------------------------------------------------------------------------------------

function ArcSin(X: Extended): Extended;

begin
  Result := ArcTan2(X, Sqrt(1 - X * X))
end;

//----------------------------------------------------------------------------------------------------------------------

function ArcTan2(Y, X: Extended): Extended;

asm
              FLD  Y
              FLD  X
              FPATAN
              FWAIT
end;

//----------------------------------------------------------------------------------------------------------------------

function Tan(X: Extended): Extended;

asm
              FLD  X
              FPTAN
              FSTP ST(0)      // FPTAN pushes 1.0 after result
              FWAIT
end;

//----------------------------------------------------------------------------------------------------------------------

function CoTan(X: Extended): Extended;

asm
              FLD  X
              FPTAN
              FDIVRP
              FWAIT
end;

//----------------- miscellaneous vector functions ---------------------------------------------------------------------

function MakeAffineDblVector(V: array of Double): TAffineDblVector; assembler;

// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V

asm
              PUSH EDI
              PUSH ESI
              MOV EDI, ECX
              MOV ESI, EAX
              MOV ECX, EDX
              ADD ECX, 2
              REP MOVSD
              POP ESI
              POP EDI
end;

//----------------------------------------------------------------------------------------------------------------------

function MakeDblVector(V: array of Double): THomogeneousDblVector; assembler;

// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V

asm
              PUSH EDI
              PUSH ESI
              MOV EDI, ECX
              MOV ESI, EAX
              MOV ECX, EDX
              ADD ECX, 2
              REP MOVSD
              POP ESI
              POP EDI
end;

//----------------------------------------------------------------------------------------------------------------------

function MakeAffineVector(V: array of Single): TAffineVector; assembler;

// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V

asm
              PUSH EDI
              PUSH ESI
              MOV EDI, ECX
              MOV ESI, EAX
              MOV ECX, EDX
              INC ECX
              CMP ECX, 3
              JB  @@1
              MOV ECX, 3
@@1:          REP MOVSD                     // copy given values
              MOV ECX, 2
              SUB ECX, EDX                   // determine missing entries
              JS  @@Finish
              XOR EAX, EAX
              REP STOSD                     // set remaining fields to 0
@@Finish:     POP ESI
              POP EDI
end;

//----------------------------------------------------------------------------------------------------------------------

function MakeQuaternion(Imag: array of Single; Real: Single): TQuaternion; assembler;

// creates a quaternion from the given values
// EAX contains address of Imag
// ECX contains address to result vector
// EDX contains highest index of Imag
// Real part is passed on the stack

asm
              PUSH EDI
              PUSH ESI
              MOV EDI, ECX
              MOV ESI, EAX
              MOV ECX, EDX
              INC ECX
              REP MOVSD
              MOV EAX, [Real]
              MOV [EDI], EAX
              POP ESI
              POP EDI
end;

//----------------------------------------------------------------------------------------------------------------------

function MakeVector(V: array of Single): TVector; assembler;

// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V

asm
              PUSH EDI
              PUSH ESI
              MOV EDI, ECX
              MOV ESI, EAX
              MOV ECX, EDX
              INC ECX
              CMP ECX, 4
              JB  @@1
              MOV ECX, 4
@@1:          REP MOVSD                     // copy given values
              MOV ECX, 3
              SUB ECX, EDX                   // determine missing entries
              JS  @@Finish
              XOR EAX, EAX
              REP STOSD                     // set remaining fields to 0
@@Finish:     POP ESI
              POP EDI
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorLength(V: array of Single): Single; assembler;

// calculates the length of a vector following the equation: sqrt(x * x + y * y + ...)
// Note: The parameter of this function is declared as open array. Thus
// there's no restriction about the number of the components of the vector.
//
// EAX contains address of V
// EDX contains the highest index of V
// the result is returned in ST(0)

asm
              FLDZ                           // initialize sum
@@Loop:       FLD  DWORD PTR [EAX  +  4 * EDX] // load a component
              FMUL ST, ST
              FADDP
              SUB  EDX, 1
              JNL  @@Loop
              FSQRT
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAngle(V1, V2: TAffineVector): Single; assembler;

// calculates the cosine of the angle between Vector1 and Vector2
// Result = DotProduct(V1, V2) / (Length(V1) * Length(V2))
//
// EAX contains address of Vector1
// EDX contains address of Vector2

asm
              FLD DWORD PTR [EAX]           // V1[0]
              FLD ST                        // double V1[0]
              FMUL ST, ST                   // V1[0]^2 (prep. for divisor)
              FLD DWORD PTR [EDX]           // V2[0]
              FMUL ST(2), ST                // ST(2) := V1[0] * V2[0]
              FMUL ST, ST                   // V2[0]^2 (prep. for divisor)
              FLD DWORD PTR [EAX + 4]       // V1[1]
              FLD ST                        // double V1[1]
              FMUL ST, ST                   // ST(0) := V1[1]^2
              FADDP ST(3), ST               // ST(2) := V1[0]^2 + V1[1] *  * 2
              FLD DWORD PTR [EDX + 4]       // V2[1]
              FMUL ST(1), ST                // ST(1) := V1[1] * V2[1]
              FMUL ST, ST                   // ST(0) := V2[1]^2
              FADDP ST(2), ST               // ST(1) := V2[0]^2 + V2[1]^2
              FADDP ST(3), ST               // ST(2) := V1[0] * V2[0] + V1[1] * V2[1]
              FLD DWORD PTR [EAX + 8]       // load V2[1]
              FLD ST                        // same calcs go here
              FMUL ST, ST                   // (compare above)
              FADDP ST(3), ST
              FLD DWORD PTR [EDX + 8]
              FMUL ST(1), ST
              FMUL ST, ST
              FADDP ST(2), ST
              FADDP ST(3), ST
              FMULP                         // ST(0) := (V1[0]^2 + V1[1]^2 + V1[2]) *
                                            //          (V2[0]^2 + V2[1]^2 + V2[2])
              FSQRT                         // sqrt(ST(0))
              FDIVP                         // ST(0) := Result := ST(1) / ST(0)
  // the result is expected in ST(0), if it's invalid, an error is raised
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorNorm(V: array of Single): Single; assembler; register;

// calculates norm of a vector which is defined as norm = x * x + y * y + ...
// EAX contains address of V
// EDX contains highest index in V
// result is passed in ST(0)

asm
              FLDZ                           // initialize sum
@@Loop:       FLD  DWORD PTR [EAX + 4 * EDX] // load a component
              FMUL ST, ST                    // make square
              FADDP                          // add previous calculated sum
              SUB  EDX, 1
              JNL  @@Loop
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorNormalize(V: array of Single): Single; assembler; register;

// transforms a vector to unit length and return length
// EAX contains address of V
// EDX contains the highest index in V
// return former length of V in ST

asm
              PUSH EBX
              MOV ECX, EDX                  // save size of V
              CALL VectorLength             // calculate length of vector
              FTST                          // test if length = 0
              MOV EBX, EAX                  // save parameter address
              FSTSW AX                      // get test result
              TEST AH, C3                   // check the test result
              JNZ @@Finish
              SUB EBX, 4                    // simplyfied address calculation
              INC ECX
              FLD1                          // calculate reciprocal of length
              FDIV ST, ST(1)
@@1:          FLD ST                        // double reciprocal
              FMUL DWORD PTR [EBX + 4 * ECX] // scale component
              WAIT
              FSTP DWORD PTR [EBX + 4 * ECX] // store result
              LOOP @@1
              FSTP ST                       // remove reciprocal from FPU stack
@@Finish:     POP EBX
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineSubtract(V1, V2: TAffineVector): TAffineVector; assembler; register;

// returns v1 minus v2
// EAX contains address of V1
// EDX contains address of V2
// ECX contains address of the result

asm
  {Result[X] := V1[X]-V2[X];
  Result[Y] := V1[Y]-V2[Y];
  Result[Z] := V1[Z]-V2[Z];}

                   FLD DWORD PTR [EAX]
                   FSUB DWORD PTR [EDX]
                   FSTP DWORD PTR [ECX]
                   FLD DWORD PTR [EAX + 4]
                   FSUB DWORD PTR [EDX + 4]
                   FSTP DWORD PTR [ECX + 4]
                   FLD DWORD PTR [EAX + 8]
                   FSUB DWORD PTR [EDX + 8]
                   FSTP DWORD PTR [ECX + 8]
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorReflect(V, N: TAffineVector): TAffineVector; assembler; register;

// reflects vector V against N (assumes N is normalized)
// EAX contains address of V
// EDX contains address of N
// ECX contains address of the result

//var Dot : Single;

asm
   {Dot := VectorAffineDotProduct(V, N);
   Result[X] := V[X]-2 * Dot * N[X];
   Result[Y] := V[Y]-2 * Dot * N[Y];
   Result[Z] := V[Z]-2 * Dot * N[Z];}

                   CALL VectorAffineDotProduct   // dot is now in ST(0)
                   FCHS                          // -dot
                   FADD ST, ST                   // -dot * 2
                   FLD DWORD PTR [EDX]           // ST := N[X]
                   FMUL ST, ST(1)                // ST := -2 * dot * N[X]
                   FADD DWORD PTR[EAX]           // ST := V[X] - 2 * dot * N[X]
                   FSTP DWORD PTR [ECX]          // store result
                   FLD DWORD PTR [EDX + 4]       // etc.
                   FMUL ST, ST(1)
                   FADD DWORD PTR[EAX + 4]
                   FSTP DWORD PTR [ECX + 4]
                   FLD DWORD PTR [EDX + 8]
                   FMUL ST, ST(1)
                   FADD DWORD PTR[EAX + 8]
                   FSTP DWORD PTR [ECX + 8]
                   FSTP ST                       // clean FPU stack
end;

//----------------------------------------------------------------------------------------------------------------------

procedure VectorRotate(var Vector: TVector4f; Axis: TVector3f; Angle: Single);

// rotates Vector about Axis with Angle radiants

var RotMatrix : TMatrix4f;

begin
  RotMatrix := CreateRotationMatrix(Axis, Angle);
  Vector := VectorTransform(Vector, RotMatrix);
end;

//----------------------------------------------------------------------------------------------------------------------

procedure VectorScale(V: array of Single; Factor: Single); assembler; register;

// returns a vector scaled by a factor
// EAX contains address of V
// EDX contains highest index in V
// Factor is located on the stack

asm
  {for I := Low(V) to High(V) do V[I] := V[I] * Factor;}

              FLD DWORD PTR [Factor]        // load factor
@@Loop:       FLD DWORD PTR [EAX + 4 * EDX] // load a component
              FMUL ST, ST(1)                // multiply it with the factor
              WAIT
              FSTP DWORD PTR [EAX + 4 * EDX] // store the result
              DEC EDX                       // do the entire array
              JNS @@Loop
              FSTP ST(0)                    // clean the FPU stack
end;

//----------------------------------------------------------------------------------------------------------------------

procedure VectorNegate(V: array of Single); assembler; register;

// returns a negated vector
// EAX contains address of V
// EDX contains highest index in V

asm
  {V[X] := -V[X];
  V[Y] := -V[Y];
  V[Z] := -V[Z];}

@@Loop:       FLD DWORD PTR [EAX + 4 * EDX]
              FCHS
              WAIT
              FSTP DWORD PTR [EAX + 4 * EDX]
              DEC EDX
              JNS @@Loop
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAdd(V1, V2: TVector): TVector; register;

// returns the sum of two vectors

begin
  Result[X] := V1[X] + V2[X];
  Result[Y] := V1[Y] + V2[Y];
  Result[Z] := V1[Z] + V2[Z];
  Result[W] := V1[W] + V2[W];
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineAdd(V1, V2: TAffineVector): TAffineVector; register;

// returns the sum of two vectors

begin
  Result[X] := V1[X] + V2[X];
  Result[Y] := V1[Y] + V2[Y];
  Result[Z] := V1[Z] + V2[Z];
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorSubtract(V1, V2: TVector): TVector; register;

// returns the difference of two vectors

begin
  Result[X] := V1[X] - V2[X];
  Result[Y] := V1[Y] - V2[Y];
  Result[Z] := V1[Z] - V2[Z];
  Result[W] := V1[W] - V2[W];
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorDotProduct(V1, V2: TVector): Single; register;

begin
  Result := V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z] + V1[W] * V2[W];
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineDotProduct(V1, V2: TAffineVector): Single; assembler; register;

// calculates the dot product between V1 and V2
// EAX contains address of V1
// EDX contains address of V2
// result is stored in ST(0)

asm
  //Result := V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z];

                   FLD DWORD PTR [EAX]
                   FMUL DWORD PTR [EDX]
                   FLD DWORD PTR [EAX + 4]
                   FMUL DWORD PTR [EDX + 4]
                   FADDP
                   FLD DWORD PTR [EAX + 8]
                   FMUL DWORD PTR [EDX + 8]
                   FADDP
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorCrossProduct(V1, V2: TAffineVector): TAffineVector;

// calculates the cross product between vector 1 and 2, Temp is necessary because
// either V1 or V2 could also be the result vector
//
// EAX contains address of V1
// EDX contains address of V2
// ECX contains address of result

var Temp: TAffineVector;

asm
  {Temp[X] := V1[Y] * V2[Z]-V1[Z] * V2[Y];
  Temp[Y] := V1[Z] * V2[X]-V1[X] * V2[Z];
  Temp[Z] := V1[X] * V2[Y]-V1[Y] * V2[X];
  Result := Temp;}

              PUSH EBX                      // save EBX, must be restored to original value
              LEA EBX, [Temp]
              FLD DWORD PTR [EDX + 8]       // first load both vectors onto FPU register stack
              FLD DWORD PTR [EDX + 4]
              FLD DWORD PTR [EDX + 0]
              FLD DWORD PTR [EAX + 8]
              FLD DWORD PTR [EAX + 4]
              FLD DWORD PTR [EAX + 0]

              FLD ST(1)                     // ST(0) := V1[Y]
              FMUL ST, ST(6)                // ST(0) := V1[Y] * V2[Z]
              FLD ST(3)                     // ST(0) := V1[Z]
              FMUL ST, ST(6)                // ST(0) := V1[Z] * V2[Y]
              FSUBP ST(1), ST               // ST(0) := ST(1)-ST(0)
              FSTP DWORD [EBX]              // Temp[X] := ST(0)
              FLD ST(2)                     // ST(0) := V1[Z]
              FMUL ST, ST(4)                // ST(0) := V1[Z] * V2[X]
              FLD ST(1)                     // ST(0) := V1[X]
              FMUL ST, ST(7)                // ST(0) := V1[X] * V2[Z]
              FSUBP ST(1), ST               // ST(0) := ST(1)-ST(0)
              FSTP DWORD [EBX + 4]          // Temp[Y] := ST(0)
              FLD ST                        // ST(0) := V1[X]
              FMUL ST, ST(5)                // ST(0) := V1[X] * V2[Y]
              FLD ST(2)                     // ST(0) := V1[Y]
              FMUL ST, ST(5)                // ST(0) := V1[Y] * V2[X]
              FSUBP ST(1), ST               // ST(0) := ST(1)-ST(0)
              FSTP DWORD [EBX + 8]          // Temp[Z] := ST(0)
              FSTP ST(0)                    // clear FPU register stack
              FSTP ST(0)
              FSTP ST(0)
              FSTP ST(0)
              FSTP ST(0)
              FSTP ST(0)
              MOV EAX, [EBX]                // copy Temp to Result
              MOV [ECX], EAX
              MOV EAX, [EBX + 4]
              MOV [ECX + 4], EAX
              MOV EAX, [EBX + 8]
              MOV [ECX + 8], EAX
              POP EBX
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorPerpendicular(V, N: TAffineVector): TAffineVector;

// calculates a vector perpendicular to N (N is assumed to be of unit length)
// subtract out any component parallel to N

var Dot: Single;

begin
   Dot := VectorAffineDotProduct(V, N);
   Result[X] := V[X]-Dot * N[X];
   Result[Y] := V[Y]-Dot * N[Y];
   Result[Z] := V[Z]-Dot * N[Z];
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorTransform(V: TVector4f; M: TMatrix): TVector4f; register;

// transforms a homogeneous vector by multiplying it with a matrix

var TV: TVector4f;

begin
  TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X] + V[W] * M[W, X];
  TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y] + V[W] * M[W, Y];
  TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z] + V[W] * M[W, Z];
  TV[W] := V[X] * M[X, W] + V[Y] * M[Y, W] + V[Z] * M[Z, W] + V[W] * M[W, W];
  Result := TV
end;

//----------------------------------------------------------------------------------------------------------------------

function  VectorTransform(V: TVector3f; M: TMatrix): TVector3f;

// transforms an affine vector by multiplying it with a (homogeneous) matrix

var TV: TVector3f;

begin
  TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X] + M[W, X];
  TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y] + M[W, Y];
  TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z] + M[W, Z];
  Result := TV;
end;


//----------------------------------------------------------------------------------------------------------------------

function VectorAffineTransform(V: TAffineVector; M: TAffineMatrix): TAffineVector; register;

// transforms an affine vector by multiplying it with a matrix

var TV: TAffineVector;

begin
  TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X];
  TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y];
  TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z];
  Result := TV;
end;

//----------------------------------------------------------------------------------------------------------------------

function PointInPolygon(xp, yp : array of Single; x, y: Single): Boolean;

// The code below is from Wm. Randolph Franklin <wrf@ecse.rpi.edu>
// with some minor modifications for speed.  It returns 1 for strictly
// interior points, 0 for strictly exterior, and 0 or 1 for points on
// the boundary.
// This code is not yet tested!

var I, J: Integer;

begin
  Result := False;
  if High(XP) <> High(YP) then Exit;
  J := High(XP);
  for I := 0 to High(XP) do
  begin
    if ((((yp[I] <= y) and (y < yp[J])) or ((yp[J] <= y) and (y < yp[I]))) and
        (x < (xp[J] - xp[I]) * (y - yp[I]) / (yp[J] - yp[I]) + xp[I]))
    then Result := not Result;
    J := I + 1;
  end;
end;

//----------------------------------------------------------------------------------------------------------------------

function QuaternionConjugate(Q: TQuaternion): TQuaternion; assembler;

// returns the conjugate of a quaternion
// EAX contains address of Q
// EDX contains address of result

asm
              FLD DWORD PTR [EAX]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX]
              FLD DWORD PTR [EAX + 4]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX + 4]
              FLD DWORD PTR [EAX + 8]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX + 8]
              MOV EAX, [EAX + 12]
              MOV [EDX + 12], EAX
end;

//----------------------------------------------------------------------------------------------------------------------

function QuaternionFromPoints(V1, V2: TAffineVector): TQuaternion; assembler;

// constructs a unit quaternion from two points on unit sphere
// EAX contains address of V1
// ECX contains address to result
// EDX contains address of V2

asm
  {Result.ImagPart := VectorCrossProduct(V1, V2);
   Result.RealPart :=  Sqrt((VectorAffineDotProduct(V1, V2) + 1)/2);}

              PUSH EAX
              CALL VectorCrossProduct       // determine axis to rotate about
              POP EAX
              FLD1                          // prepare next calculation
              Call VectorAffineDotProduct   // calculate cos(angle between V1 and V2)
              FADD ST, ST(1)                // transform angle to angle/2 by: cos(a/2)=sqrt((1 + cos(a))/2)
              FXCH ST(1)
              FADD ST, ST
              FDIVP ST(1), ST
              FSQRT
              FSTP DWORD PTR [ECX + 12]     // Result.RealPart := ST(0)
end;

//----------------------------------------------------------------------------------------------------------------------

function QuaternionMultiply(qL, qR: TQuaternion): TQuaternion;

// Returns quaternion product qL * qR.  Note: order is important!
// To combine rotations, use the product QuaternionMuliply(qSecond, qFirst),
// which gives the effect of rotating by qFirst then qSecond.

var Temp : TQuaternion;

begin
  Temp.RealPart := qL.RealPart * qR.RealPart - qL.ImagPart[X] * qR.ImagPart[X] -
                   qL.ImagPart[Y] * qR.ImagPart[Y] - qL.ImagPart[Z] * qR.ImagPart[Z];
  Temp.ImagPart[X] := qL.RealPart * qR.ImagPart[X] + qL.ImagPart[X] * qR.RealPart +
                      qL.ImagPart[Y] * qR.ImagPart[Z] - qL.ImagPart[Z] * qR.ImagPart[Y];
  Temp.ImagPart[Y] := qL.RealPart * qR.ImagPart[Y] + qL.ImagPart[Y] * qR.RealPart +
                      qL.ImagPart[Z] * qR.ImagPart[X] - qL.ImagPart[X] * qR.ImagPart[Z];
  Temp.ImagPart[Z] := qL.RealPart * qR.ImagPart[Z] + qL.ImagPart[Z] * qR.RealPart +
                      qL.ImagPart[X] * qR.ImagPart[Y] - qL.ImagPart[Y] * qR.ImagPart[X];
  Result := Temp;
end;

//----------------------------------------------------------------------------------------------------------------------

function QuaternionToMatrix(Q: TQuaternion): TMatrix;

// Constructs rotation matrix from (possibly non-unit) quaternion.
// Assumes matrix is used to multiply column vector on the left:
// vnew = mat vold.  Works correctly for right-handed coordinate system
// and right-handed rotations.

// Essentially, this function is the same as CreateRotationMatrix and you can consider it as
// being for reference here.

{var Norm, S,
    XS, YS, ZS,
    WX, WY, WZ,
    XX, XY, XZ,
    YY, YZ, ZZ   : Single;

begin
  Norm := Q.Vector[X] * Q.Vector[X] + Q.Vector[Y] * Q.Vector[Y] + Q.Vector[Z] * Q.Vector[Z] + Q.RealPart * Q.RealPart;
  if Norm > 0 then S := 2 / Norm
              else S := 0;

  XS := Q.Vector[X] * S;   YS := Q.Vector[Y] * S;   ZS := Q.Vector[Z] * S;
  WX := Q.RealPart * XS;   WY := Q.RealPart * YS;   WZ := Q.RealPart * ZS;
  XX := Q.Vector[X] * XS;  XY := Q.Vector[X] * YS;  XZ := Q.Vector[X] * ZS;
  YY := Q.Vector[Y] * YS;  YZ := Q.Vector[Y] * ZS;  ZZ := Q.Vector[Z] * ZS;

  Result[X, X] := 1 - (YY + ZZ); Result[Y, X] := XY + WZ;       Result[Z, X] := XZ - WY;       Result[W, X] := 0;
  Result[X, Y] := XY - WZ;       Result[Y, Y] := 1 - (XX + ZZ); Result[Z, Y] := YZ + WX;       Result[W, Y] := 0;
  Result[X, Z] := XZ + WY;       Result[Y, Z] := YZ - WX;       Result[Z, Z] := 1 - (XX + YY); Result[W, Z] := 0;
  Result[X, W] := 0;             Result[Y, W] := 0;             Result[Z, W] := 0;             Result[W, W] := 1;}

var
  V: TAffineVector;
  SinA, CosA,
  A, B, C: Extended;

begin
  V := Q.ImagPart;
  VectorNormalize(V);
  SinCos(Q.RealPart / 2, SinA, CosA);
  A := V[X] * SinA;
  B := V[Y] * SinA;
  C := V[Z] * SinA;

  Result := IdentityMatrix;
  Result[X, X] := 1 - 2 * B * B - 2 * C * C;
  Result[X, Y] := 2 * A * B - 2 * CosA * C;
  Result[X, Z] := 2 * A * C + 2 * CosA * B;

  Result[Y, X] := 2 * A * B + 2 * CosA * C;
  Result[Y, Y] := 1 - 2 * A * A - 2 * C * C;
  Result[Y, Z] := 2 * B * C - 2 * CosA * A;

  Result[Z, X] := 2 * A * C - 2 * CosA * B;
  Result[Z, Y] := 2 * B * C + 2 * CosA * A;
  Result[Z, Z] := 1 - 2 * A * A - 2 * B * B;
end;

//----------------------------------------------------------------------------------------------------------------------

procedure QuaternionToPoints(Q: TQuaternion; var ArcFrom, ArcTo: TAffineVector); register;

// converts a unit quaternion into two points on a unit sphere

var S: Single;

begin
  S := Sqrt(Q.ImagPart[X] * Q.ImagPart[X] + Q.ImagPart[Y] * Q.ImagPart[Y]);
  if S = 0 then ArcFrom := MakeAffineVector([0, 1, 0])
           else ArcFrom := MakeAffineVector([-Q.ImagPart[Y] / S, Q.ImagPart[X] / S, 0]);
  ArcTo[X] := Q.RealPart * ArcFrom[X] - Q.ImagPart[Z] * ArcFrom[Y];
  ArcTo[Y] := Q.RealPart * ArcFrom[Y] + Q.ImagPart[Z] * ArcFrom[X];
  ArcTo[Z] := Q.ImagPart[X] * ArcFrom[Y] - Q.ImagPart[Y] * ArcFrom[X];
  if Q.RealPart < 0 then ArcFrom := MakeAffineVector([-ArcFrom[X], -ArcFrom[Y], 0]);
end;

//----------------------------------------------------------------------------------------------------------------------

function MatrixAffineDeterminant(M: TAffineMatrix): Single; register;

// determinant of a 3x3 matrix

begin
  Result := M[X, X] * (M[Y, Y] * M[Z, Z] - M[Z, Y] * M[Y, Z]) -
            M[X, Y] * (M[Y, X] * M[Z, Z] - M[Z, X] * M[Y, Z]) +
            M[X, Z] * (M[Y, X] * M[Z, Y] - M[Z, X] * M[Y, Y]);
end;

//----------------------------------------------------------------------------------------------------------------------

function MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3: Single): Single;

// internal version for the determinant of a 3x3 matrix

begin
  Result := a1 * (b2 * c3 - b3 * c2) -
            b1 * (a2 * c3 - a3 * c2) +
            c1 * (a2 * b3 - a3 * b2);
end;

//----------------------------------------------------------------------------------------------------------------------

procedure MatrixAdjoint(var M: TMatrix); register;

// Adjoint of a 4x4 matrix - used in the computation of the inverse
// of a 4x4 matrix

var a1, a2, a3, a4,
    b1, b2, b3, b4,
    c1, c2, c3, c4,
    d1, d2, d3, d4: Single;


begin
    a1 :=  M[X, X]; b1 :=  M[X, Y];
    c1 :=  M[X, Z]; d1 :=  M[X, W];
    a2 :=  M[Y, X]; b2 :=  M[Y, Y];
    c2 :=  M[Y, Z]; d2 :=  M[Y, W];
    a3 :=  M[Z, X]; b3 :=  M[Z, Y];
    c3 :=  M[Z, Z]; d3 :=  M[Z, W];
    a4 :=  M[W, X]; b4 :=  M[W, Y];
    c4 :=  M[W, Z]; d4 :=  M[W, W];

    // row column labeling reversed since we transpose rows & columns
    M[X, X] :=  MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4);
    M[Y, X] := -MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4);
    M[Z, X] :=  MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4);
    M[W, X] := -MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);

    M[X, Y] := -MatrixDetInternal(b1, b3, b4, c1, c3, c4, d1, d3, d4);
    M[Y, Y] :=  MatrixDetInternal(a1, a3, a4, c1, c3, c4, d1, d3, d4);
    M[Z, Y] := -MatrixDetInternal(a1, a3, a4, b1, b3, b4, d1, d3, d4);
    M[W, Y] :=  MatrixDetInternal(a1, a3, a4, b1, b3, b4, c1, c3, c4);

    M[X, Z] :=  MatrixDetInternal(b1, b2, b4, c1, c2, c4, d1, d2, d4);
    M[Y, Z] := -MatrixDetInternal(a1, a2, a4, c1, c2, c4, d1, d2, d4);
    M[Z, Z] :=  MatrixDetInternal(a1, a2, a4, b1, b2, b4, d1, d2, d4);
    M[W, Z] := -MatrixDetInternal(a1, a2, a4, b1, b2, b4, c1, c2, c4);

    M[X, W] := -MatrixDetInternal(b1, b2, b3, c1, c2, c3, d1, d2, d3);
    M[Y, W] :=  MatrixDetInternal(a1, a2, a3, c1, c2, c3, d1, d2, d3);
    M[Z, W] := -MatrixDetInternal(a1, a2, a3, b1, b2, b3, d1, d2, d3);
    M[W, W] :=  MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3);
end;

//----------------------------------------------------------------------------------------------------------------------

function MatrixDeterminant(M: TMatrix): Single; register;

// Determinant of a 4x4 matrix

var a1, a2, a3, a4,
    b1, b2, b3, b4,
    c1, c2, c3, c4,
    d1, d2, d3, d4  : Single;

begin
  a1 := M[X, X];  b1 := M[X, Y];  c1 := M[X, Z];  d1 := M[X, W];
  a2 := M[Y, X];  b2 := M[Y, Y];  c2 := M[Y, Z];  d2 := M[Y, W];
  a3 := M[Z, X];  b3 := M[Z, Y];  c3 := M[Z, Z];  d3 := M[Z, W];
  a4 := M[W, X];  b4 := M[W, Y];  c4 := M[W, Z];  d4 := M[W, W];

  Result := a1 * MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
            b1 * MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
            c1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
            d1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
end;

//----------------------------------------------------------------------------------------------------------------------

procedure MatrixScale(var M: TMatrix; Factor: Single); register;

// multiplies all elements of a 4x4 matrix with a factor

var I, J: Integer;

begin
  for I := 0 to 3 do
    for J := 0 to 3 do M[I, J] := M[I, J] * Factor;
end;

//----------------------------------------------------------------------------------------------------------------------

procedure MatrixInvert(var M: TMatrix); register;

// finds the inverse of a 4x4 matrix

var Det: Single;

begin
  Det := MatrixDeterminant(M);
  if Abs(Det) < EPSILON then M := IdentityMatrix
                        else
  begin
    MatrixAdjoint(M);
    MatrixScale(M, 1 / Det);
  end;
end;

//----------------------------------------------------------------------------------------------------------------------

procedure MatrixTranspose(var M: TMatrix); register;

// computes transpose of 4x4 matrix

var I, J: Integer;
    TM: TMatrix;

begin
  for I := 0 to 3 do
    for J := 0 to 3 do TM[J, I] := M[I, J];
  M := TM;
end;

//----------------------------------------------------------------------------------------------------------------------

procedure MatrixAffineTranspose(var M: TAffineMatrix); register;

// computes transpose of 3x3 matrix

var I, J: Integer;
    TM: TAffineMatrix;

begin
  for I := 0 to 2 do
    for J := 0 to 2 do TM[J, I] := M[I, J];
  M := TM;
end;

//----------------------------------------------------------------------------------------------------------------------

function MatrixMultiply(M1, M2: TMatrix): TMatrix; register;

// multiplies two 4x4 matrices

var I, J: Integer;
    TM: TMatrix;

begin
  for I := 0 to 3 do
    for J := 0 to 3 do
      TM[I, J] := M1[I, X] * M2[X, J] +
                  M1[I, Y] * M2[Y, J] +
                  M1[I, Z] * M2[Z, J] +
                  M1[I, W] * M2[W, J];
  Result := TM;
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateRotationMatrix(Axis: TVector3f; Angle: Single): TMatrix; register;

// Creates a rotation matrix along the given Axis by the given Angle in radians.

var cosine,
    sine,
    Len,
    one_minus_cosine: Extended;

begin
  SinCos(Angle, Sine, Cosine);
  one_minus_cosine := 1 - cosine;
  Len := VectorNormalize(Axis);

  if Len = 0 then Result := IdentityMatrix
             else
  begin
    Result[X, X] := (one_minus_cosine * Sqr(Axis[0])) + Cosine;
    Result[X, Y] := (one_minus_cosine * Axis[0] * Axis[1]) - (Axis[2] * Sine);
    Result[X, Z] := (one_minus_cosine * Axis[2] * Axis[0]) + (Axis[1] * Sine);
    Result[X, W] := 0;

    Result[Y, X] := (one_minus_cosine * Axis[0] * Axis[1]) + (Axis[2] * Sine);
    Result[Y, Y] := (one_minus_cosine * Sqr(Axis[1])) + Cosine;
    Result[Y, Z] := (one_minus_cosine * Axis[1] * Axis[2]) - (Axis[0] * Sine);
    Result[Y, W] := 0;

    Result[Z, X] := (one_minus_cosine * Axis[2] * Axis[0]) - (Axis[1] * Sine);
    Result[Z, Y] := (one_minus_cosine * Axis[1] * Axis[2]) + (Axis[0] * Sine);
    Result[Z, Z] := (one_minus_cosine * Sqr(Axis[2])) + Cosine;
    Result[Z, W] := 0;

    Result[W, X] := 0;
    Result[W, Y] := 0;
    Result[W, Z] := 0;
    Result[W, W] := 1;
  end;
end;

//----------------------------------------------------------------------------------------------------------------------

function ConvertRotation(Angles: TAffineVector): TVector; register;

{ Turn a triplet of rotations about x, y, and z (in that order) into an
   equivalent rotation around a single axis (all in radians).

   Rotation of the Angle t about the axis (X, Y, Z) is given by:

     | X^2 + (1-X^2) Cos(t),    XY(1-Cos(t))  +  Z Sin(t), XZ(1-Cos(t))-Y Sin(t) |
 M = | XY(1-Cos(t))-Z Sin(t), Y^2 + (1-Y^2) Cos(t),      YZ(1-Cos(t)) + X Sin(t) |
     | XZ(1-Cos(t)) + Y Sin(t), YZ(1-Cos(t))-X Sin(t),   Z^2 + (1-Z^2) Cos(t)    |

   Rotation about the three axes (Angles a1, a2, a3) can be represented as
   the product of the individual rotation matrices:

      | 1  0       0       | | Cos(a2) 0 -Sin(a2) | |  Cos(a3) Sin(a3) 0 |
      | 0  Cos(a1) Sin(a1) | * | 0       1  0       | * | -Sin(a3) Cos(a3) 0 |
      | 0 -Sin(a1) Cos(a1) | | Sin(a2) 0  Cos(a2) | |  0       0       1 |
	     Mx                       My                     Mz

   We now want to solve for X, Y, Z, and t given 9 equations in 4 unknowns.
   Using the diagonal elements of the two matrices, we get:

      X^2 + (1-X^2) Cos(t) = M[0][0]
      Y^2 + (1-Y^2) Cos(t) = M[1][1]
      Z^2 + (1-Z^2) Cos(t) = M[2][2]

   Adding the three equations, we get:

      X^2  +  Y^2  +  Z^2 - (M[0][0]  +  M[1][1]  +  M[2][2]) =
	 - (3 - X^2 - Y^2 - Z^2) Cos(t)

   Since (X^2  +  Y^2  +  Z^2) = 1, we can rewrite as:

      Cos(t) = (1 - (M[0][0]  +  M[1][1]  +  M[2][2])) / 2

   Solving for t, we get:

      t = Acos(((M[0][0]  +  M[1][1]  +  M[2][2]) - 1) / 2)

    We can substitute t into the equations for X^2, Y^2, and Z^2 above
    to get the values for X, Y, and Z.  To find the proper signs we note
    that:

	2 X Sin(t) = M[1][2] - M[2][1]
	2 Y Sin(t) = M[2][0] - M[0][2]
	2 Z Sin(t) = M[0][1] - M[1][0]
}

var Axis1, Axis2: TVector3f;
    M, M1, M2: TMatrix;
    cost, cost1,
    sint,
    s1, s2, s3: Single;
    I: Integer;


begin
  // see if we are only rotating about a single Axis
  if Abs(Angles[X]) < EPSILON then
  begin
    if Abs(Angles[Y]) < EPSILON then
    begin
      Result := MakeVector([0, 0, 1, Angles[Z]]);
      Exit;
    end
    else
      if Abs(Angles[Z]) < EPSILON then
      begin
        Result := MakeVector([0, 1, 0, Angles[Y]]);
        Exit;
      end
   end
   else
     if (Abs(Angles[Y]) < EPSILON) and
        (Abs(Angles[Z]) < EPSILON) then
     begin
       Result := MakeVector([1, 0, 0, Angles[X]]);
       Exit;
     end;

  // make the rotation matrix
  Axis1 := MakeAffineVector([1, 0, 0]);
  M := CreateRotationMatrix(Axis1, Angles[X]);

  Axis2 := MakeAffineVector([0, 1, 0]);
  M2 := CreateRotationMatrix(Axis2, Angles[Y]);
  M1 := MatrixMultiply(M, M2);

  Axis2 := MakeAffineVector([0, 0, 1]);
  M2 := CreateRotationMatrix(Axis2, Angles[Z]);
  M := MatrixMultiply(M1, M2);

  cost := ((M[X, X] + M[Y, Y] + M[Z, Z])-1) / 2;
  if cost < -1 then cost := -1
               else
    if cost > 1 - EPSILON then
    begin
      // Bad Angle - this would cause a crash
      Result := MakeVector([1, 0, 0, 0]);
      Exit;
    end;

  cost1 := 1 - cost;
  Result := Makevector([Sqrt((M[X, X]-cost) / cost1),
                      Sqrt((M[Y, Y]-cost) / cost1),
                      sqrt((M[Z, Z]-cost) / cost1),
                      arccos(cost)]);

  sint := 2 * Sqrt(1 - cost * cost); // This is actually 2 Sin(t)

  // Determine the proper signs
  for I := 0 to 7 do
  begin
    if (I and 1) > 1 then s1 := -1 else s1 := 1;
    if (I and 2) > 1 then s2 := -1 else s2 := 1;
    if (I and 4) > 1 then s3 := -1 else s3 := 1;
    if (Abs(s1 * Result[X] * sint-M[Y, Z] + M[Z, Y]) < EPSILON2) and
       (Abs(s2 * Result[Y] * sint-M[Z, X] + M[X, Z]) < EPSILON2) and
       (Abs(s3 * Result[Z] * sint-M[X, Y] + M[Y, X]) < EPSILON2) then
        begin
          // We found the right combination of signs
          Result[X] := Result[X] * s1;
          Result[Y] := Result[Y] * s2;
          Result[Z] := Result[Z] * s3;
          Exit;
        end;
  end;
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateRotationMatrixX(Sine, Cosine: Single): TMatrix; register;

// creates matrix for rotation about x-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := 1;
  Result[Y, Y] := Cosine;
  Result[Y, Z] := Sine;
  Result[Z, Y] := -Sine;
  Result[Z, Z] := Cosine;
  Result[W, W] := 1;
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateRotationMatrixY(Sine, Cosine: Single): TMatrix; register;

// creates matrix for rotation about y-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := Cosine;
  Result[X, Z] := -Sine;
  Result[Y, Y] := 1;
  Result[Z, X] := Sine;
  Result[Z, Z] := Cosine;
  Result[W, W] := 1;
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateRotationMatrixZ(Sine, Cosine: Single): TMatrix; register;

// creates matrix for rotation about z-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := Cosine;
  Result[X, Y] := Sine;
  Result[Y, X] := -Sine;
  Result[Y, Y] := Cosine;
  Result[Z, Z] := 1;
  Result[W, W] := 1;
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateScaleMatrix(V: TAffineVector): TMatrix; register;

// creates scaling matrix

begin
  Result := IdentityMatrix;
  Result[X, X] := V[X];
  Result[Y, Y] := V[Y];
  Result[Z, Z] := V[Z];
end;

//----------------------------------------------------------------------------------------------------------------------

function CreateTranslationMatrix(V: TVector): TMatrix; register;

// creates translation matrix

begin
  Result := IdentityMatrix;
  Result[W, X] := V[X];
  Result[W, Y] := V[Y];
  Result[W, Z] := V[Z];
  Result[W, W] := V[W];
end;

//----------------------------------------------------------------------------------------------------------------------

function Lerp(Start, Stop, t: Single): Single;

// calculates linear interpolation between start and stop at point t

begin
  Result := Start + (Stop - Start) * t;
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineLerp(V1, V2: TAffineVector; t: Single): TAffineVector;

// calculates linear interpolation between vector1 and vector2 at point t

begin
  Result[X] := Lerp(V1[X], V2[X], t);
  Result[Y] := Lerp(V1[Y], V2[Y], t);
  Result[Z] := Lerp(V1[Z], V2[Z], t);
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorLerp(V1, V2: TVector; t: Single): TVector;

// calculates linear interpolation between vector1 and vector2 at point t

begin
  Result[X] := Lerp(V1[X], V2[X], t);
  Result[Y] := Lerp(V1[Y], V2[Y], t);
  Result[Z] := Lerp(V1[Z], V2[Z], t);
  Result[W] := Lerp(V1[W], V2[W], t);
end;

//----------------------------------------------------------------------------------------------------------------------

function QuaternionSlerp(QStart, QEnd: TQuaternion; Spin: Integer; t: Single): TQuaternion;

// spherical linear interpolation of unit quaternions with spins
// QStart, QEnd - start and end unit quaternions
// t            - interpolation parameter (0 to 1)
// Spin         - number of extra spin rotations to involve

var beta,                   // complementary interp parameter
    theta,                  // Angle between A and B
    sint, cost,             // sine, cosine of theta
    phi: Single;            // theta plus spins
    bflip: Boolean;         // use negativ t?


begin
  // cosine theta
  cost := VectorAngle(QStart.ImagPart, QEnd.ImagPart);

  // if QEnd is on opposite hemisphere from QStart, use -QEnd instead
  if cost < 0 then
  begin
    cost := -cost;
    bflip := True;
  end
  else bflip := False;

  // if QEnd is (within precision limits) the same as QStart,
  // just linear interpolate between QStart and QEnd.
  // Can't do spins, since we don't know what direction to spin.

  if (1 - cost) < EPSILON then beta := 1 - t
                          else
  begin
    // normal case
    theta := arccos(cost);
    phi := theta + Spin * Pi;
    sint := sin(theta);
    beta := sin(theta - t * phi) / sint;
    t := sin(t * phi) / sint;
  end;

  if bflip then t := -t;

  // interpolate
  Result.ImagPart[X] := beta * QStart.ImagPart[X] + t * QEnd.ImagPart[X];
  Result.ImagPart[Y] := beta * QStart.ImagPart[Y] + t * QEnd.ImagPart[Y];
  Result.ImagPart[Z] := beta * QStart.ImagPart[Z] + t * QEnd.ImagPart[Z];
  Result.RealPart := beta * QStart.RealPart + t * QEnd.RealPart;
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineCombine(V1, V2: TAffineVector; F1, F2: Single): TAffineVector;

// makes a linear combination of two vectors and return the result

begin
  Result[X] := (F1 * V1[X]) + (F2 * V2[X]);
  Result[Y] := (F1 * V1[Y]) + (F2 * V2[Y]);
  Result[Z] := (F1 * V1[Z]) + (F2 * V2[Z]);
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorCombine(V1, V2: TVector; F1, F2: Single): TVector;

// makes a linear combination of two vectors and return the result

begin
  Result[X] := (F1 * V1[X]) + (F2 * V2[X]);
  Result[Y] := (F1 * V1[Y]) + (F2 * V2[Y]);
  Result[Z] := (F1 * V1[Z]) + (F2 * V2[Z]);
  Result[W] := (F1 * V1[W]) + (F2 * V2[W]);
end;

//----------------------------------------------------------------------------------------------------------------------

function MatrixDecompose(M: TMatrix; var Tran: TTransformations): Boolean; register;

// Author: Spencer W. Thomas, University of Michigan
//
// MatrixDecompose - Decompose a non-degenerated 4x4 transformation matrix into
// the sequence of transformations that produced it.
//
// The coefficient of each transformation is returned in the corresponding
// element of the vector Tran.
//
// Returns true upon success, false if the matrix is singular.

var I, J: Integer;
    LocMat,
    pmat,
    invpmat,
    tinvpmat: TMatrix;
    prhs,
    psol: TVector;
    Row: array[0..2] of TAffineVector;

begin
  Result := False;
  locmat := M;
  // normalize the matrix
  if locmat[W, W] = 0 then Exit;
  for I := 0 to 3 do
    for J := 0 to 3 do
      locmat[I, J] := locmat[I, J] / locmat[W, W];

  // pmat is used to solve for perspective, but it also provides
  // an easy way to test for singularity of the upper 3x3 component.

  pmat := locmat;
  for I := 0 to 2 do pmat[I, W] := 0;
  pmat[W, W] := 1;

  if MatrixDeterminant(pmat) = 0 then Exit;

  // First, isolate perspective.  This is the messiest.
  if (locmat[X, W] <> 0) or
     (locmat[Y, W] <> 0) or
     (locmat[Z, W] <> 0) then
  begin
    // prhs is the right hand side of the equation.
    prhs[X] := locmat[X, W];
    prhs[Y] := locmat[Y, W];
    prhs[Z] := locmat[Z, W];
    prhs[W] := locmat[W, W];

    // Solve the equation by inverting pmat and multiplying
    // prhs by the inverse.  (This is the easiest way, not
    // necessarily the best.)

    invpmat := pmat;
    MatrixInvert(invpmat);
    MatrixTranspose(invpmat);
    psol := VectorTransform(prhs, tinvpmat);

    // stuff the answer away
    Tran[ttPerspectiveX] := psol[X];
    Tran[ttPerspectiveY] := psol[Y];
    Tran[ttPerspectiveZ] := psol[Z];
    Tran[ttPerspectiveW] := psol[W];

    // clear the perspective partition
    locmat[X, W] := 0;
    locmat[Y, W] := 0;
    locmat[Z, W] := 0;
    locmat[W, W] := 1;
  end
  else
  begin
    // no perspective
    Tran[ttPerspectiveX] := 0;
    Tran[ttPerspectiveY] := 0;
    Tran[ttPerspectiveZ] := 0;
    Tran[ttPerspectiveW] := 0;
  end;

  // next take care of translation (easy)
  for I := 0 to 2 do
  begin
    Tran[TTransType(Ord(ttTranslateX) + I)] := locmat[W, I];
    locmat[W, I] := 0;
  end;

  // now get scale and shear
  for I := 0 to 2 do
  begin
    row[I, X] := locmat[I, X];
    row[I, Y] := locmat[I, Y];
    row[I, Z] := locmat[I, Z];
  end;

  // compute X scale factor and normalize first row
  Tran[ttScaleX] := Sqr(VectorNormalize(row[0])); // ml: calculation optimized

  // compute XY shear factor and make 2nd row orthogonal to 1st
  Tran[ttShearXY] := VectorAffineDotProduct(row[0], row[1]);
  row[1] := VectorAffineCombine(row[1], row[0], 1, -Tran[ttShearXY]);

  // now, compute Y scale and normalize 2nd row
  Tran[ttScaleY] := Sqr(VectorNormalize(row[1])); // ml: calculation optimized
  Tran[ttShearXY] := Tran[ttShearXY]/Tran[ttScaleY];

  // compute XZ and YZ shears, orthogonalize 3rd row
  Tran[ttShearXZ] := VectorAffineDotProduct(row[0], row[2]);
  row[2] := VectorAffineCombine(row[2], row[0], 1, -Tran[ttShearXZ]);
  Tran[ttShearYZ] := VectorAffineDotProduct(row[1], row[2]);
  row[2] := VectorAffineCombine(row[2], row[1], 1, -Tran[ttShearYZ]);

  // next, get Z scale and normalize 3rd row
  Tran[ttScaleZ] := Sqr(VectorNormalize(row[1])); // (ML) calc. optimized
  Tran[ttShearXZ] := Tran[ttShearXZ] / tran[ttScaleZ];
  Tran[ttShearYZ] := Tran[ttShearYZ] / Tran[ttScaleZ];

  // At this point, the matrix (in rows[]) is orthonormal.
  // Check for a coordinate system flip.  If the determinant
  // is -1, then negate the matrix and the scaling factors.
  if VectorAffineDotProduct(row[0], VectorCrossProduct(row[1], row[2])) < 0 then
    for I := 0 to 2 do
    begin
      Tran[TTransType(Ord(ttScaleX) + I)] := -Tran[TTransType(Ord(ttScaleX) + I)];
      row[I, X] := -row[I, X];
      row[I, Y] := -row[I, Y];
      row[I, Z] := -row[I, Z];
    end;

  // now, get the rotations out, as described in the gem
  Tran[ttRotateY] := arcsin(-row[0, Z]);
  if cos(Tran[ttRotateY]) <> 0 then
  begin
    Tran[ttRotateX] := arctan2(row[1, Z], row[2, Z]);
    Tran[ttRotateZ] := arctan2(row[0, Y], row[0, X]);
  end
  else
  begin
    tran[ttRotateX] := arctan2(row[1, X], row[1, Y]);
    tran[ttRotateZ] := 0;
  end;
  // All done!
  Result := True;
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorDblToFlt(V: THomogeneousDblVector): THomogeneousVector; assembler;

// converts a vector containing double sized values into a vector with single sized values

asm
              FLD  QWORD PTR [EAX]
              FSTP DWORD PTR [EDX]
              FLD  QWORD PTR [EAX + 8]
              FSTP DWORD PTR [EDX + 4]
              FLD  QWORD PTR [EAX + 16]
              FSTP DWORD PTR [EDX + 8]
              FLD  QWORD PTR [EAX + 24]
              FSTP DWORD PTR [EDX + 12]
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineDblToFlt(V: TAffineDblVector): TAffineVector; assembler;

// converts a vector containing double sized values into a vector with single sized values

asm
              FLD  QWORD PTR [EAX]
              FSTP DWORD PTR [EDX]
              FLD  QWORD PTR [EAX + 8]
              FSTP DWORD PTR [EDX + 4]
              FLD  QWORD PTR [EAX + 16]
              FSTP DWORD PTR [EDX + 8]
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorAffineFltToDbl(V: TAffineVector): TAffineDblVector; assembler;

// converts a vector containing single sized values into a vector with double sized values

asm
              FLD  DWORD PTR [EAX]
              FSTP QWORD PTR [EDX]
              FLD  DWORD PTR [EAX + 8]
              FSTP QWORD PTR [EDX + 4]
              FLD  DWORD PTR [EAX + 16]
              FSTP QWORD PTR [EDX + 8]
end;

//----------------------------------------------------------------------------------------------------------------------

function VectorFltToDbl(V: TVector): THomogeneousDblVector; assembler;

// converts a vector containing single sized values into a vector with double sized values

asm
              FLD  DWORD PTR [EAX]
              FSTP QWORD PTR [EDX]
              FLD  DWORD PTR [EAX + 8]
              FSTP QWORD PTR [EDX + 4]
              FLD  DWORD PTR [EAX + 16]
              FSTP QWORD PTR [EDX + 8]
              FLD  DWORD PTR [EAX + 24]
              FSTP QWORD PTR [EDX + 12]
end;

//----------------- coordinate system manipulation functions -----------------------------------------------------------

function Turn(Matrix: TMatrix; Angle: Single): TMatrix;

// rotates the given coordinate system (represented by the matrix) around its Y-axis

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MakeAffineVector(Matrix[1]), Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

function Turn(Matrix: TMatrix; MasterUp: TAffineVector; Angle: Single): TMatrix;

// rotates the given coordinate system (represented by the matrix) around MasterUp

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterUp, Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

function Pitch(Matrix: TMatrix; Angle: Single): TMatrix;

// rotates the given coordinate system (represented by the matrix) around its X-axis

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MakeAffineVector(Matrix[0]), Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

function Pitch(Matrix: TMatrix; MasterRight: TAffineVector; Angle: Single): TMatrix; overload;

// rotates the given coordinate system (represented by the matrix) around MasterRight

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterRight, Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

function Roll(Matrix: TMatrix; Angle: Single): TMatrix;

// rotates the given coordinate system (represented by the matrix) around its Z-axis

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MakeAffineVector(Matrix[2]), Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

function Roll(Matrix: TMatrix; MasterDirection: TAffineVector; Angle: Single): TMatrix; overload;

// rotates the given coordinate system (represented by the matrix) around MasterDirection

begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterDirection, Angle));
end;

//----------------------------------------------------------------------------------------------------------------------

end.