From 46bb010ca7c5eb04551c030105f9999ca80e472f Mon Sep 17 00:00:00 2001 From: tobigun Date: Sun, 8 Jun 2008 15:33:48 +0000 Subject: - set svn:eol-style to native - removed some svn:executable properties from non-executable files git-svn-id: svn://svn.code.sf.net/p/ultrastardx/svn/trunk@1144 b956fd51-792f-4845-bead-9b4dfca2ff2c --- Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas | 3988 ++++++++++++------------ 1 file changed, 1994 insertions(+), 1994 deletions(-) (limited to 'Game/Code/lib/JEDI-SDL/OpenGL/Pas/geometry.pas') 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 -// 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 +// 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. + + -- cgit v1.2.3