aboutsummaryrefslogtreecommitdiffstats
path: root/Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas
diff options
context:
space:
mode:
Diffstat (limited to 'Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas')
-rw-r--r--Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas3988
1 files changed, 1994 insertions, 1994 deletions
diff --git a/Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas b/Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas
index 15783515..166ec811 100644
--- a/Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas
+++ b/Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas
@@ -1,1994 +1,1994 @@
-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.
-
-
+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.
+
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-28 21:53:44 +0000