www.pudn.com > racecar.zip > Geometry.pas
unit Geometry;
// This unit contains many needed types, functions and procedures for
// quaternion, vector and matrix arithmetics. It is specifically designed
// for geometric calculations within R3 (affine vector space)
// and R4 (homogeneous vector space).
//
// Note: The terms 'affine' or 'affine coordinates' are not really correct here
// because an 'affine transformation' describes generally a transformation which leads
// to a uniquely solvable system of equations and has nothing to do with the dimensionality
// of a vector. One could use 'projective coordinates' but this is also not really correct
// and since I haven't found a better name (or even any correct one), 'affine' is as good
// as any other one.
//
// Identifiers containing no dimensionality (like affine or homogeneous)
// and no datatype (integer..extended) are supposed as R4 representation
// with 'single' floating point type (examples are TVector, TMatrix,
// and TQuaternion). The default data type is 'single' ('GLFloat' for OpenGL)
// and used in all routines (except conversions and trigonometric functions).
//
// Routines with an open array as argument can either take Func([1,2,3,4,..]) or Func(Vect).
// The latter is prefered, since no extra stack operations is required.
// Note: Be careful while passing open array elements! If you pass more elements
// than there's room in the result the behaviour will be unpredictable.
//
// If not otherwise stated, all angles are given in radians
// (instead of degrees). Use RadToDeg or DegToRad to convert between them.
//
// Geometry.pas was assembled from different sources (like GraphicGems)
// and relevant books or based on self written code, respectivly.
//
// Note: Some aspects need to be considered when using Delphi and pure
// assembler code. Delphi 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)
interface
type
// data types needed for 3D graphics calculation,
// included are 'C like' aliases for each type (to be
// conformal with OpenGL types)
PByte = ^Byte;
PWord = ^Word;
PInteger = ^Integer;
PFloat = ^Single;
PDouble = ^Double;
PExtended = ^Extended;
PPointer = ^Pointer;
// types to specify continous streams of a specific type
// switch off range checking to access values beyond the limits
PByteVector = ^TByteVector;
PByteArray = PByteVector;
TByteVector = array[0..0] of Byte;
PWordVector = ^TWordVector;
PWordArray = PWordVector; // note: there's a same named type in SysUtils
TWordVector = array[0..0] of Word;
PIntegerVector = ^TIntegerVector;
PIntegerArray = PIntegerVector;
TIntegerVector = array[0..0] of Integer;
PFloatVector = ^TFloatVector;
PFloatArray = PFloatVector;
TFloatVector = array[0..0] of Single;
PDoubleVector = ^TDoubleVector;
PDoubleArray = PDoubleVector;
TDoubleVector = array[0..0] of Double;
PExtendedVector = ^TExtendedVector;
PExtendedArray = PExtendedVector;
TExtendedVector = array[0..0] of Extended;
PPointerVector = ^TPointerVector;
PPointerArray = PPointerVector;
TPointerVector = array[0..0] of Pointer;
PCardinalVector = ^TCardinalVector;
PCardinalArray = PCardinalVector;
TCardinalVector = array[0..0] of Cardinal;
// common vector and matrix types with predefined limits
// indices correspond like: x -> 0
// y -> 1
// z -> 2
// w -> 3
PHomogeneousByteVector = ^THomogeneousByteVector;
THomogeneousByteVector = array[0..3] of Byte;
TVector4b = THomogeneousByteVector;
PHomogeneousWordVector = ^THomogeneousWordVector;
THomogeneousWordVector = array[0..3] of Word;
TVector4w = THomogeneousWordVector;
PHomogeneousIntVector = ^THomogeneousIntVector;
THomogeneousIntVector = array[0..3] of Integer;
TVector4i = THomogeneousIntVector;
PHomogeneousFltVector = ^THomogeneousFltVector;
THomogeneousFltVector = array[0..3] of Single;
TVector4f = THomogeneousFltVector;
PHomogeneousDblVector = ^THomogeneousDblVector;
THomogeneousDblVector = array[0..3] of Double;
TVector4d = THomogeneousDblVector;
PHomogeneousExtVector = ^THomogeneousExtVector;
THomogeneousExtVector = array[0..3] of Extended;
TVector4e = THomogeneousExtVector;
PHomogeneousPtrVector = ^THomogeneousPtrVector;
THomogeneousPtrVector = array[0..3] of Pointer;
TVector4p = THomogeneousPtrVector;
PAffineByteVector = ^TAffineByteVector;
TAffineByteVector = array[0..2] of Byte;
TVector3b = TAffineByteVector;
PAffineWordVector = ^TAffineWordVector;
TAffineWordVector = array[0..2] of Word;
TVector3w = TAffineWordVector;
PAffineIntVector = ^TAffineIntVector;
TAffineIntVector = array[0..2] of Integer;
TVector3i = TAffineIntVector;
PAffineFltVector = ^TAffineFltVector;
TAffineFltVector = array[0..2] of Single;
TVector3f = TAffineFltVector;
PAffineDblVector = ^TAffineDblVector;
TAffineDblVector = array[0..2] of Double;
TVector3d = TAffineDblVector;
PAffineExtVector = ^TAffineExtVector;
TAffineExtVector = array[0..2] of Extended;
TVector3e = TAffineExtVector;
PAffinePtrVector = ^TAffinePtrVector;
TAffinePtrVector = array[0..2] of Pointer;
TVector3p = TAffinePtrVector;
// some simplified names
PVector = ^TVector;
TVector = THomogeneousFltVector;
PHomogeneousVector = ^THomogeneousVector;
THomogeneousVector = THomogeneousFltVector;
PAffineVector = ^TAffineVector;
TAffineVector = TAffineFltVector;
// arrays of vectors
PVectorArray = ^TVectorArray;
TVectorArray = array[0..0] of TAffineVector;
// matrices
THomogeneousByteMatrix = array[0..3] of THomogeneousByteVector;
TMatrix4b = THomogeneousByteMatrix;
THomogeneousWordMatrix = array[0..3] of THomogeneousWordVector;
TMatrix4w = THomogeneousWordMatrix;
THomogeneousIntMatrix = array[0..3] of THomogeneousIntVector;
TMatrix4i = THomogeneousIntMatrix;
THomogeneousFltMatrix = array[0..3] of THomogeneousFltVector;
TMatrix4f = THomogeneousFltMatrix;
THomogeneousDblMatrix = array[0..3] of THomogeneousDblVector;
TMatrix4d = THomogeneousDblMatrix;
THomogeneousExtMatrix = array[0..3] of THomogeneousExtVector;
TMatrix4e = THomogeneousExtMatrix;
TAffineByteMatrix = array[0..2] of TAffineByteVector;
TMatrix3b = TAffineByteMatrix;
TAffineWordMatrix = array[0..2] of TAffineWordVector;
TMatrix3w = TAffineWordMatrix;
TAffineIntMatrix = array[0..2] of TAffineIntVector;
TMatrix3i = TAffineIntMatrix;
TAffineFltMatrix = array[0..2] of TAffineFltVector;
TMatrix3f = TAffineFltMatrix;
TAffineDblMatrix = array[0..2] of TAffineDblVector;
TMatrix3d = TAffineDblMatrix;
TAffineExtMatrix = array[0..2] of TAffineExtVector;
TMatrix3e = TAffineExtMatrix;
// some simplified names
PMatrix = ^TMatrix;
TMatrix = THomogeneousFltMatrix;
PHomogeneousMatrix = ^THomogeneousMatrix;
THomogeneousMatrix = THomogeneousFltMatrix;
PAffineMatrix = ^TAffineMatrix;
TAffineMatrix = TAffineFltMatrix;
// q = ([x, y, z], w)
TQuaternion = record
case Integer of
0:
(ImagPart: TAffineVector;
RealPart: Single);
1:
(Vector: TVector4f);
end;
TRectangle = record
Left,
Top,
Width,
Height: Integer;
end;
TTransType = (ttScaleX, ttScaleY, ttScaleZ,
ttShearXY, ttShearXZ, ttShearYZ,
ttRotateX, ttRotateY, ttRotateZ,
ttTranslateX, ttTranslateY, ttTranslateZ,
ttPerspectiveX, ttPerspectiveY, ttPerspectiveZ, ttPerspectiveW);
// used to describe a sequence of transformations in following order:
// [Sx][Sy][Sz][ShearXY][ShearXZ][ShearZY][Rx][Ry][Rz][Tx][Ty][Tz][P(x,y,z,w)]
// constants are declared for easier access (see MatrixDecompose below)
TTransformations = array[TTransType] of Single;
const
// useful constants
// standard vectors
XVector: TAffineVector = (1, 0, 0);
YVector: TAffineVector = (0, 1, 0);
ZVector: TAffineVector = (0, 0, 1);
NullVector: TAffineVector = (0, 0, 0);
IdentityMatrix: TMatrix = ((1, 0, 0, 0),
(0, 1, 0, 0),
(0, 0, 1, 0),
(0, 0, 0, 1));
EmptyMatrix: TMatrix = ((0, 0, 0, 0),
(0, 0, 0, 0),
(0, 0, 0, 0),
(0, 0, 0, 0));
// some very small numbers
EPSILON = 1e-100;
EPSILON2 = 1e-50;
//----------------------------------------------------------------------------------------------------------------------
// vector functions
function VectorAdd(V1, V2: TVector): TVector;
function VectorAffineAdd(V1, V2: TAffineVector): TAffineVector;
function VectorAffineCombine(V1, V2: TAffineVector; F1, F2: Single): TAffineVector;
function VectorAffineDotProduct(V1, V2: TAffineVector): Single;
function VectorAffineLerp(V1, V2: TAffineVector; t: Single): TAffineVector;
function VectorAffineSubtract(V1, V2: TAffineVector): TAffineVector;
function VectorAngle(V1, V2: TAffineVector): Single;
function VectorCombine(V1, V2: TVector; F1, F2: Single): TVector;
function VectorCrossProduct(V1, V2: TAffineVector): TAffineVector;
function VectorDotProduct(V1, V2: TVector): Single;
function VectorLength(V: array of Single): Single;
function VectorLerp(V1, V2: TVector; t: Single): TVector;
procedure VectorNegate(V: array of Single);
function VectorNorm(V: array of Single): Single;
function VectorNormalize(V: array of Single): Single;
function VectorPerpendicular(V, N: TAffineVector): TAffineVector;
function VectorReflect(V, N: TAffineVector): TAffineVector;
procedure VectorRotate(var Vector: TVector4f; Axis: TVector3f; Angle: Single);
procedure VectorScale(V: array of Single; Factor: Single);
function VectorSubtract(V1, V2: TVector): TVector;
// matrix functions
function CreateRotationMatrixX(Sine, Cosine: Single): TMatrix;
function CreateRotationMatrixY(Sine, Cosine: Single): TMatrix;
function CreateRotationMatrixZ(Sine, Cosine: Single): TMatrix;
function CreateScaleMatrix(V: TAffineVector): TMatrix;
function CreateTranslationMatrix(V: TVector): TMatrix;
procedure MatrixAdjoint(var M: TMatrix);
function MatrixAffineDeterminant(M: TAffineMatrix): Single;
procedure MatrixAffineTranspose(var M: TAffineMatrix);
function MatrixDeterminant(M: TMatrix): Single;
procedure MatrixInvert(var M: TMatrix);
function MatrixMultiply(M1, M2: TMatrix): TMatrix;
procedure MatrixScale(var M: TMatrix; Factor: Single);
procedure MatrixTranspose(var M: TMatrix);
// quaternion functions
function QuaternionConjugate(Q: TQuaternion): TQuaternion;
function QuaternionFromPoints(V1, V2: TAffineVector): TQuaternion;
function QuaternionMultiply(qL, qR: TQuaternion): TQuaternion;
function QuaternionSlerp(QStart, QEnd: TQuaternion; Spin: Integer; t: Single): TQuaternion;
function QuaternionToMatrix(Q: TQuaternion): TMatrix;
procedure QuaternionToPoints(Q: TQuaternion; var ArcFrom, ArcTo: TAffineVector);
// mixed functions
function ConvertRotation(Angles: TAffineVector): TVector;
function CreateRotationMatrix(Axis: TVector3f; Angle: Single): TMatrix;
function MatrixDecompose(M: TMatrix; var Tran: TTransformations): Boolean;
function VectorAffineTransform(V: TAffineVector; M: TAffineMatrix): TAffineVector;
function VectorTransform(V: TVector4f; M: TMatrix): TVector4f; overload;
function VectorTransform(V: TVector3f; M: TMatrix): TVector3f; overload;
// miscellaneous functions
function MakeAffineDblVector(V: array of Double): TAffineDblVector;
function MakeDblVector(V: array of Double): THomogeneousDblVector;
function MakeAffineVector(V: array of Single): TAffineVector;
function MakeQuaternion(Imag: array of Single; Real: Single): TQuaternion;
function MakeVector(V: array of Single): TVector;
function PointInPolygon(xp, yp : array of Single; x, y: Single): Boolean;
function VectorAffineDblToFlt(V: TAffineDblVector): TAffineVector;
function VectorDblToFlt(V: THomogeneousDblVector): THomogeneousVector;
function VectorAffineFltToDbl(V: TAffineVector): TAffineDblVector;
function VectorFltToDbl(V: TVector): THomogeneousDblVector;
// trigonometric functions
function ArcCos(X: Extended): Extended;
function ArcSin(X: Extended): Extended;
function ArcTan2(Y, X: Extended): Extended;
function CoTan(X: Extended): Extended;
function DegToRad(Degrees: Extended): Extended;
function RadToDeg(Radians: Extended): Extended;
procedure SinCos(Theta: Extended; var Sin, Cos: Extended);
function Tan(X: Extended): Extended;
// coordinate system manipulation functions
function Turn(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Turn(Matrix: TMatrix; MasterUp: TAffineVector; Angle: Single): TMatrix; overload;
function Pitch(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Pitch(Matrix: TMatrix; MasterRight: TAffineVector; Angle: Single): TMatrix; overload;
function Roll(Matrix: TMatrix; Angle: Single): TMatrix; overload;
function Roll(Matrix: TMatrix; MasterDirection: TAffineVector; Angle: Single): TMatrix; overload;
//----------------------------------------------------------------------------------------------------------------------
implementation
const
// FPU status flags (high order byte)
C0 = 1;
C1 = 2;
C2 = 4;
C3 = $40;
// to be used as descriptive indices
X = 0;
Y = 1;
Z = 2;
W = 3;
//----------------- trigonometric helper functions ---------------------------------------------------------------------
function DegToRad(Degrees: Extended): Extended;
begin
Result := Degrees * (PI / 180);
end;
//----------------------------------------------------------------------------------------------------------------------
function RadToDeg(Radians: Extended): Extended;
begin
Result := Radians * (180 / PI);
end;
//----------------------------------------------------------------------------------------------------------------------
procedure SinCos(Theta: Extended; var Sin, Cos: Extended); assembler; register;
// calculates sine and cosine from the given angle Theta
// EAX contains address of Sin
// EDX contains address of Cos
// Theta is passed over the stack
asm
FLD Theta
FSINCOS
FSTP TBYTE PTR [EDX] // cosine
FSTP TBYTE PTR [EAX] // sine
FWAIT
end;
//----------------------------------------------------------------------------------------------------------------------
function ArcCos(X: Extended): Extended;
begin
Result := ArcTan2(Sqrt(1 - X * X), X);
end;
//----------------------------------------------------------------------------------------------------------------------
function ArcSin(X: Extended): Extended;
begin
Result := ArcTan2(X, Sqrt(1 - X * X))
end;
//----------------------------------------------------------------------------------------------------------------------
function ArcTan2(Y, X: Extended): Extended;
asm
FLD Y
FLD X
FPATAN
FWAIT
end;
//----------------------------------------------------------------------------------------------------------------------
function Tan(X: Extended): Extended;
asm
FLD X
FPTAN
FSTP ST(0) // FPTAN pushes 1.0 after result
FWAIT
end;
//----------------------------------------------------------------------------------------------------------------------
function CoTan(X: Extended): Extended;
asm
FLD X
FPTAN
FDIVRP
FWAIT
end;
//----------------- miscellaneous vector functions ---------------------------------------------------------------------
function MakeAffineDblVector(V: array of Double): TAffineDblVector; assembler;
// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V
asm
PUSH EDI
PUSH ESI
MOV EDI, ECX
MOV ESI, EAX
MOV ECX, EDX
ADD ECX, 2
REP MOVSD
POP ESI
POP EDI
end;
//----------------------------------------------------------------------------------------------------------------------
function MakeDblVector(V: array of Double): THomogeneousDblVector; assembler;
// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V
asm
PUSH EDI
PUSH ESI
MOV EDI, ECX
MOV ESI, EAX
MOV ECX, EDX
ADD ECX, 2
REP MOVSD
POP ESI
POP EDI
end;
//----------------------------------------------------------------------------------------------------------------------
function MakeAffineVector(V: array of Single): TAffineVector; assembler;
// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V
asm
PUSH EDI
PUSH ESI
MOV EDI, ECX
MOV ESI, EAX
MOV ECX, EDX
INC ECX
CMP ECX, 3
JB @@1
MOV ECX, 3
@@1: REP MOVSD // copy given values
MOV ECX, 2
SUB ECX, EDX // determine missing entries
JS @@Finish
XOR EAX, EAX
REP STOSD // set remaining fields to 0
@@Finish: POP ESI
POP EDI
end;
//----------------------------------------------------------------------------------------------------------------------
function MakeQuaternion(Imag: array of Single; Real: Single): TQuaternion; assembler;
// creates a quaternion from the given values
// EAX contains address of Imag
// ECX contains address to result vector
// EDX contains highest index of Imag
// Real part is passed on the stack
asm
PUSH EDI
PUSH ESI
MOV EDI, ECX
MOV ESI, EAX
MOV ECX, EDX
INC ECX
REP MOVSD
MOV EAX, [Real]
MOV [EDI], EAX
POP ESI
POP EDI
end;
//----------------------------------------------------------------------------------------------------------------------
function MakeVector(V: array of Single): TVector; assembler;
// creates a vector from given values
// EAX contains address of V
// ECX contains address to result vector
// EDX contains highest index of V
asm
PUSH EDI
PUSH ESI
MOV EDI, ECX
MOV ESI, EAX
MOV ECX, EDX
INC ECX
CMP ECX, 4
JB @@1
MOV ECX, 4
@@1: REP MOVSD // copy given values
MOV ECX, 3
SUB ECX, EDX // determine missing entries
JS @@Finish
XOR EAX, EAX
REP STOSD // set remaining fields to 0
@@Finish: POP ESI
POP EDI
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorLength(V: array of Single): Single; assembler;
// calculates the length of a vector following the equation: sqrt(x * x + y * y + ...)
// Note: The parameter of this function is declared as open array. Thus
// there's no restriction about the number of the components of the vector.
//
// EAX contains address of V
// EDX contains the highest index of V
// the result is returned in ST(0)
asm
FLDZ // initialize sum
@@Loop: FLD DWORD PTR [EAX + 4 * EDX] // load a component
FMUL ST, ST
FADDP
SUB EDX, 1
JNL @@Loop
FSQRT
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAngle(V1, V2: TAffineVector): Single; assembler;
// calculates the cosine of the angle between Vector1 and Vector2
// Result = DotProduct(V1, V2) / (Length(V1) * Length(V2))
//
// EAX contains address of Vector1
// EDX contains address of Vector2
asm
FLD DWORD PTR [EAX] // V1[0]
FLD ST // double V1[0]
FMUL ST, ST // V1[0]^2 (prep. for divisor)
FLD DWORD PTR [EDX] // V2[0]
FMUL ST(2), ST // ST(2) := V1[0] * V2[0]
FMUL ST, ST // V2[0]^2 (prep. for divisor)
FLD DWORD PTR [EAX + 4] // V1[1]
FLD ST // double V1[1]
FMUL ST, ST // ST(0) := V1[1]^2
FADDP ST(3), ST // ST(2) := V1[0]^2 + V1[1] * * 2
FLD DWORD PTR [EDX + 4] // V2[1]
FMUL ST(1), ST // ST(1) := V1[1] * V2[1]
FMUL ST, ST // ST(0) := V2[1]^2
FADDP ST(2), ST // ST(1) := V2[0]^2 + V2[1]^2
FADDP ST(3), ST // ST(2) := V1[0] * V2[0] + V1[1] * V2[1]
FLD DWORD PTR [EAX + 8] // load V2[1]
FLD ST // same calcs go here
FMUL ST, ST // (compare above)
FADDP ST(3), ST
FLD DWORD PTR [EDX + 8]
FMUL ST(1), ST
FMUL ST, ST
FADDP ST(2), ST
FADDP ST(3), ST
FMULP // ST(0) := (V1[0]^2 + V1[1]^2 + V1[2]) *
// (V2[0]^2 + V2[1]^2 + V2[2])
FSQRT // sqrt(ST(0))
FDIVP // ST(0) := Result := ST(1) / ST(0)
// the result is expected in ST(0), if it's invalid, an error is raised
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorNorm(V: array of Single): Single; assembler; register;
// calculates norm of a vector which is defined as norm = x * x + y * y + ...
// EAX contains address of V
// EDX contains highest index in V
// result is passed in ST(0)
asm
FLDZ // initialize sum
@@Loop: FLD DWORD PTR [EAX + 4 * EDX] // load a component
FMUL ST, ST // make square
FADDP // add previous calculated sum
SUB EDX, 1
JNL @@Loop
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorNormalize(V: array of Single): Single; assembler; register;
// transforms a vector to unit length and return length
// EAX contains address of V
// EDX contains the highest index in V
// return former length of V in ST
asm
PUSH EBX
MOV ECX, EDX // save size of V
CALL VectorLength // calculate length of vector
FTST // test if length = 0
MOV EBX, EAX // save parameter address
FSTSW AX // get test result
TEST AH, C3 // check the test result
JNZ @@Finish
SUB EBX, 4 // simplyfied address calculation
INC ECX
FLD1 // calculate reciprocal of length
FDIV ST, ST(1)
@@1: FLD ST // double reciprocal
FMUL DWORD PTR [EBX + 4 * ECX] // scale component
WAIT
FSTP DWORD PTR [EBX + 4 * ECX] // store result
LOOP @@1
FSTP ST // remove reciprocal from FPU stack
@@Finish: POP EBX
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAffineSubtract(V1, V2: TAffineVector): TAffineVector; assembler; register;
// returns v1 minus v2
// EAX contains address of V1
// EDX contains address of V2
// ECX contains address of the result
asm
{Result[X] := V1[X]-V2[X];
Result[Y] := V1[Y]-V2[Y];
Result[Z] := V1[Z]-V2[Z];}
FLD DWORD PTR [EAX]
FSUB DWORD PTR [EDX]
FSTP DWORD PTR [ECX]
FLD DWORD PTR [EAX + 4]
FSUB DWORD PTR [EDX + 4]
FSTP DWORD PTR [ECX + 4]
FLD DWORD PTR [EAX + 8]
FSUB DWORD PTR [EDX + 8]
FSTP DWORD PTR [ECX + 8]
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorReflect(V, N: TAffineVector): TAffineVector; assembler; register;
// reflects vector V against N (assumes N is normalized)
// EAX contains address of V
// EDX contains address of N
// ECX contains address of the result
//var Dot : Single;
asm
{Dot := VectorAffineDotProduct(V, N);
Result[X] := V[X]-2 * Dot * N[X];
Result[Y] := V[Y]-2 * Dot * N[Y];
Result[Z] := V[Z]-2 * Dot * N[Z];}
CALL VectorAffineDotProduct // dot is now in ST(0)
FCHS // -dot
FADD ST, ST // -dot * 2
FLD DWORD PTR [EDX] // ST := N[X]
FMUL ST, ST(1) // ST := -2 * dot * N[X]
FADD DWORD PTR[EAX] // ST := V[X] - 2 * dot * N[X]
FSTP DWORD PTR [ECX] // store result
FLD DWORD PTR [EDX + 4] // etc.
FMUL ST, ST(1)
FADD DWORD PTR[EAX + 4]
FSTP DWORD PTR [ECX + 4]
FLD DWORD PTR [EDX + 8]
FMUL ST, ST(1)
FADD DWORD PTR[EAX + 8]
FSTP DWORD PTR [ECX + 8]
FSTP ST // clean FPU stack
end;
//----------------------------------------------------------------------------------------------------------------------
procedure VectorRotate(var Vector: TVector4f; Axis: TVector3f; Angle: Single);
// rotates Vector about Axis with Angle radiants
var RotMatrix : TMatrix4f;
begin
RotMatrix := CreateRotationMatrix(Axis, Angle);
Vector := VectorTransform(Vector, RotMatrix);
end;
//----------------------------------------------------------------------------------------------------------------------
procedure VectorScale(V: array of Single; Factor: Single); assembler; register;
// returns a vector scaled by a factor
// EAX contains address of V
// EDX contains highest index in V
// Factor is located on the stack
asm
{for I := Low(V) to High(V) do V[I] := V[I] * Factor;}
FLD DWORD PTR [Factor] // load factor
@@Loop: FLD DWORD PTR [EAX + 4 * EDX] // load a component
FMUL ST, ST(1) // multiply it with the factor
WAIT
FSTP DWORD PTR [EAX + 4 * EDX] // store the result
DEC EDX // do the entire array
JNS @@Loop
FSTP ST(0) // clean the FPU stack
end;
//----------------------------------------------------------------------------------------------------------------------
procedure VectorNegate(V: array of Single); assembler; register;
// returns a negated vector
// EAX contains address of V
// EDX contains highest index in V
asm
{V[X] := -V[X];
V[Y] := -V[Y];
V[Z] := -V[Z];}
@@Loop: FLD DWORD PTR [EAX + 4 * EDX]
FCHS
WAIT
FSTP DWORD PTR [EAX + 4 * EDX]
DEC EDX
JNS @@Loop
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAdd(V1, V2: TVector): TVector; register;
// returns the sum of two vectors
begin
Result[X] := V1[X] + V2[X];
Result[Y] := V1[Y] + V2[Y];
Result[Z] := V1[Z] + V2[Z];
Result[W] := V1[W] + V2[W];
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAffineAdd(V1, V2: TAffineVector): TAffineVector; register;
// returns the sum of two vectors
begin
Result[X] := V1[X] + V2[X];
Result[Y] := V1[Y] + V2[Y];
Result[Z] := V1[Z] + V2[Z];
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorSubtract(V1, V2: TVector): TVector; register;
// returns the difference of two vectors
begin
Result[X] := V1[X] - V2[X];
Result[Y] := V1[Y] - V2[Y];
Result[Z] := V1[Z] - V2[Z];
Result[W] := V1[W] - V2[W];
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorDotProduct(V1, V2: TVector): Single; register;
begin
Result := V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z] + V1[W] * V2[W];
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAffineDotProduct(V1, V2: TAffineVector): Single; assembler; register;
// calculates the dot product between V1 and V2
// EAX contains address of V1
// EDX contains address of V2
// result is stored in ST(0)
asm
//Result := V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z];
FLD DWORD PTR [EAX]
FMUL DWORD PTR [EDX]
FLD DWORD PTR [EAX + 4]
FMUL DWORD PTR [EDX + 4]
FADDP
FLD DWORD PTR [EAX + 8]
FMUL DWORD PTR [EDX + 8]
FADDP
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorCrossProduct(V1, V2: TAffineVector): TAffineVector;
// calculates the cross product between vector 1 and 2, Temp is necessary because
// either V1 or V2 could also be the result vector
//
// EAX contains address of V1
// EDX contains address of V2
// ECX contains address of result
var Temp: TAffineVector;
asm
{Temp[X] := V1[Y] * V2[Z]-V1[Z] * V2[Y];
Temp[Y] := V1[Z] * V2[X]-V1[X] * V2[Z];
Temp[Z] := V1[X] * V2[Y]-V1[Y] * V2[X];
Result := Temp;}
PUSH EBX // save EBX, must be restored to original value
LEA EBX, [Temp]
FLD DWORD PTR [EDX + 8] // first load both vectors onto FPU register stack
FLD DWORD PTR [EDX + 4]
FLD DWORD PTR [EDX + 0]
FLD DWORD PTR [EAX + 8]
FLD DWORD PTR [EAX + 4]
FLD DWORD PTR [EAX + 0]
FLD ST(1) // ST(0) := V1[Y]
FMUL ST, ST(6) // ST(0) := V1[Y] * V2[Z]
FLD ST(3) // ST(0) := V1[Z]
FMUL ST, ST(6) // ST(0) := V1[Z] * V2[Y]
FSUBP ST(1), ST // ST(0) := ST(1)-ST(0)
FSTP DWORD [EBX] // Temp[X] := ST(0)
FLD ST(2) // ST(0) := V1[Z]
FMUL ST, ST(4) // ST(0) := V1[Z] * V2[X]
FLD ST(1) // ST(0) := V1[X]
FMUL ST, ST(7) // ST(0) := V1[X] * V2[Z]
FSUBP ST(1), ST // ST(0) := ST(1)-ST(0)
FSTP DWORD [EBX + 4] // Temp[Y] := ST(0)
FLD ST // ST(0) := V1[X]
FMUL ST, ST(5) // ST(0) := V1[X] * V2[Y]
FLD ST(2) // ST(0) := V1[Y]
FMUL ST, ST(5) // ST(0) := V1[Y] * V2[X]
FSUBP ST(1), ST // ST(0) := ST(1)-ST(0)
FSTP DWORD [EBX + 8] // Temp[Z] := ST(0)
FSTP ST(0) // clear FPU register stack
FSTP ST(0)
FSTP ST(0)
FSTP ST(0)
FSTP ST(0)
FSTP ST(0)
MOV EAX, [EBX] // copy Temp to Result
MOV [ECX], EAX
MOV EAX, [EBX + 4]
MOV [ECX + 4], EAX
MOV EAX, [EBX + 8]
MOV [ECX + 8], EAX
POP EBX
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorPerpendicular(V, N: TAffineVector): TAffineVector;
// calculates a vector perpendicular to N (N is assumed to be of unit length)
// subtract out any component parallel to N
var Dot: Single;
begin
Dot := VectorAffineDotProduct(V, N);
Result[X] := V[X]-Dot * N[X];
Result[Y] := V[Y]-Dot * N[Y];
Result[Z] := V[Z]-Dot * N[Z];
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorTransform(V: TVector4f; M: TMatrix): TVector4f; register;
// transforms a homogeneous vector by multiplying it with a matrix
var TV: TVector4f;
begin
TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X] + V[W] * M[W, X];
TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y] + V[W] * M[W, Y];
TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z] + V[W] * M[W, Z];
TV[W] := V[X] * M[X, W] + V[Y] * M[Y, W] + V[Z] * M[Z, W] + V[W] * M[W, W];
Result := TV
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorTransform(V: TVector3f; M: TMatrix): TVector3f;
// transforms an affine vector by multiplying it with a (homogeneous) matrix
var TV: TVector3f;
begin
TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X] + M[W, X];
TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y] + M[W, Y];
TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z] + M[W, Z];
Result := TV;
end;
//----------------------------------------------------------------------------------------------------------------------
function VectorAffineTransform(V: TAffineVector; M: TAffineMatrix): TAffineVector; register;
// transforms an affine vector by multiplying it with a matrix
var TV: TAffineVector;
begin
TV[X] := V[X] * M[X, X] + V[Y] * M[Y, X] + V[Z] * M[Z, X];
TV[Y] := V[X] * M[X, Y] + V[Y] * M[Y, Y] + V[Z] * M[Z, Y];
TV[Z] := V[X] * M[X, Z] + V[Y] * M[Y, Z] + V[Z] * M[Z, Z];
Result := TV;
end;
//----------------------------------------------------------------------------------------------------------------------
function PointInPolygon(xp, yp : array of Single; x, y: Single): Boolean;
// The code below is from Wm. Randolph Franklin
// 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.