Geometry

Top  Previous  Next

unit Geometry;

 

// 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 esnures 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)

 

interface

 

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.