{ ****************************************************************************** }
{ * geometry 3D library writen by QQ 600585@qq.com                             * }
{ * https://zpascal.net                                                        * }
{ * https://github.com/PassByYou888/zAI                                        * }
{ * https://github.com/PassByYou888/ZServer4D                                  * }
{ * https://github.com/PassByYou888/PascalString                               * }
{ * https://github.com/PassByYou888/zRasterization                             * }
{ * https://github.com/PassByYou888/CoreCipher                                 * }
{ * https://github.com/PassByYou888/zSound                                     * }
{ * https://github.com/PassByYou888/zChinese                                   * }
{ * https://github.com/PassByYou888/zExpression                                * }
{ * https://github.com/PassByYou888/zGameWare                                  * }
{ * https://github.com/PassByYou888/zAnalysis                                  * }
{ * https://github.com/PassByYou888/FFMPEG-Header                              * }
{ * https://github.com/PassByYou888/zTranslate                                 * }
{ * https://github.com/PassByYou888/InfiniteIoT                                * }
{ * https://github.com/PassByYou888/FastMD5                                    * }
{ ****************************************************************************** }

unit GeometryLib;

{$INCLUDE zDefine.inc}

interface

uses CoreClasses, Geometry2DUnit;

type
  TVector2f = array [0 .. 1] of TGeoFloat;
  TVector3f = array [0 .. 2] of TGeoFloat;
  TVector4f = array [0 .. 3] of TGeoFloat;
  TVector4i = array [0 .. 3] of Integer;
  TMatrix2f = array [0 .. 1] of TVector2f;
  TMatrix3f = array [0 .. 2] of TVector3f;
  TMatrix4f = array [0 .. 3] of TVector4f;

const
  cMaxArray = (MaxInt shr 4);
  cColinearBias = 1E-8;

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

  PTexPoint = ^TTexPoint;

  TTexPoint = record
    s, t: TGeoFloat;
  end;

  // 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 .. cMaxArray] of Byte;
  PWordVector = ^TWordVector;
  TWordVector = array [0 .. cMaxArray] of Word;
  PIntegerVector = ^TIntegerVector;
  PIntegerArray = PIntegerVector;
  TIntegerVector = array [0 .. cMaxArray] of Integer;
  PFloatVector = ^TFloatVector;
  PFloatArray = PFloatVector;
  PSingleArray = PFloatArray;
  TFloatVector = array [0 .. cMaxArray] of TGeoFloat;
  TSingleArray = array of TGeoFloat;
  PDoubleVector = ^TDoubleVector;
  PDoubleArray = PDoubleVector;
  TDoubleVector = array [0 .. cMaxArray] of Double;
  PPointerVector = ^TPointerVector;
  PPointerArray = PPointerVector;
  TPointerVector = array [0 .. cMaxArray] of Pointer;
  PCardinalVector = ^TCardinalVector;
  PCardinalArray = PCardinalVector;
  TCardinalVector = array [0 .. cMaxArray] of Cardinal;

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

  PHomogeneousFltVector = ^THomogeneousFltVector;
  THomogeneousFltVector = TVector4f;
  PAffineFltVector = ^TAffineFltVector;
  TAffineFltVector = TVector3f;
  PVector2f = ^TVector2f;

  // some simplified names
  PVector = ^TVector;
  TVector = THomogeneousFltVector;
  PHomogeneousVector = ^THomogeneousVector;
  THomogeneousVector = THomogeneousFltVector;
  PAffineVector = ^TAffineVector;
  TAffineVector = TVector3f;
  PVertex = ^TVertex;
  TVertex = TAffineVector;
  // arrays of vectors
  PAffineVectorArray = ^TAffineVectorArray;
  TAffineVectorArray = array [0 .. (MaxInt div SizeOf(TAffineVector)) - 1] of TAffineVector;
  PVectorArray = ^TVectorArray;
  TVectorArray = array [0 .. (MaxInt div SizeOf(TVector)) - 1] of TVector;
  PTexPointArray = ^TTexPointArray;
  TTexPointArray = array [0 .. (MaxInt div SizeOf(TTexPoint)) - 1] of TTexPoint;
  // matrices
  THomogeneousFltMatrix = TMatrix4f;
  TAffineFltMatrix = TMatrix3f;

  // some simplified names
  PMatrix = ^TMatrix;
  TMatrix = THomogeneousFltMatrix;
  TMatrixArray = array [0 .. (MaxInt div SizeOf(TMatrix)) - 1] of TMatrix;
  PMatrixArray = ^TMatrixArray;
  PHomogeneousMatrix = ^THomogeneousMatrix;
  THomogeneousMatrix = THomogeneousFltMatrix;
  PAffineMatrix = ^TAffineMatrix;
  TAffineMatrix = TAffineFltMatrix;

  { A plane equation.
    Defined by its equation A.x+B.y+C.z+D, a plane can be mapped to the
    homogeneous space coordinates, and this is what we are doing here.<br>
    The typename is just here for easing up data manipulation. }
  THmgPlane = TVector;

  // q = ([x, y, z], w)
  PQuaternion = ^TQuaternion;

  TQuaternion = record
    ImagPart: TAffineVector;
    RealPart: TGeoFloat;
  end;

  PQuaternionArray = ^TQuaternionArray;
  TQuaternionArray = array [0 .. (MaxInt div SizeOf(TQuaternion)) - 1] of TQuaternion;

  TRectangle = record
    Left, Top, width, height: Integer;
  end;

  TFrustum = record
    pLeft, pTop, pRight, pBottom, pNear, pFar: THmgPlane;
  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 TGeoFloat;

  TPackedRotationMatrix = array [0 .. 2] of SmallInt;

const
  // TexPoints (2D space)
  XTexPoint: TTexPoint = (s: 1; t: 0);
  YTexPoint: TTexPoint = (s: 0; t: 1);
  XYTexPoint: TTexPoint = (s: 1; t: 1);
  NullTexPoint: TTexPoint = (s: 0; t: 0);
  MidTexPoint: TTexPoint = (s: 0.5; t: 0.5);

  // standard vectors
  XVector: TAffineVector = (1, 0, 0);
  YVector: TAffineVector = (0, 1, 0);
  ZVector: TAffineVector = (0, 0, 1);
  XYVector: TAffineVector = (1, 1, 0);
  XZVector: TAffineVector = (1, 0, 1);
  YZVector: TAffineVector = (0, 1, 1);
  XYZVector: TAffineVector = (1, 1, 1);
  NullVector: TAffineVector = (0, 0, 0);
  MinusXVector: TAffineVector = (-1, 0, 0);
  MinusYVector: TAffineVector = (0, -1, 0);
  MinusZVector: TAffineVector = (0, 0, -1);

  // standard homogeneous vectors
  XHmgVector: THomogeneousVector = (1, 0, 0, 0);
  YHmgVector: THomogeneousVector = (0, 1, 0, 0);
  ZHmgVector: THomogeneousVector = (0, 0, 1, 0);
  WHmgVector: THomogeneousVector = (0, 0, 0, 1);
  XYHmgVector: THomogeneousVector = (1, 1, 0, 0);
  YZHmgVector: THomogeneousVector = (0, 1, 1, 0);
  XZHmgVector: THomogeneousVector = (1, 0, 1, 0);
  XYZHmgVector: THomogeneousVector = (1, 1, 1, 0);
  XYZWHmgVector: THomogeneousVector = (1, 1, 1, 1);
  NullHmgVector: THomogeneousVector = (0, 0, 0, 0);

  // standard homogeneous points
  XHmgPoint: THomogeneousVector = (1, 0, 0, 1);
  YHmgPoint: THomogeneousVector = (0, 1, 0, 1);
  ZHmgPoint: THomogeneousVector = (0, 0, 1, 1);
  WHmgPoint: THomogeneousVector = (0, 0, 0, 1);
  NullHmgPoint: THomogeneousVector = (0, 0, 0, 1);

  IdentityMatrix: TAffineMatrix =
    ((1, 0, 0),
    (0, 1, 0),
    (0, 0, 1));
  IdentityHmgMatrix: TMatrix =
    ((1, 0, 0, 0),
    (0, 1, 0, 0),
    (0, 0, 1, 0),
    (0, 0, 0, 1));
  EmptyMatrix: TAffineMatrix =
    ((0, 0, 0),
    (0, 0, 0),
    (0, 0, 0));
  EmptyHmgMatrix: TMatrix =
    ((0, 0, 0, 0),
    (0, 0, 0, 0),
    (0, 0, 0, 0),
    (0, 0, 0, 0));

  // Quaternions

  IdentityQuaternion: TQuaternion = (ImagPart: (0, 0, 0); RealPart: 1);

  // some very small numbers
  Epsilon: TGeoFloat = 1E-40;
  EPSILON2: TGeoFloat = 1E-30;

  // ------------------------------------------------------------------------------
  // Vector functions
  // ------------------------------------------------------------------------------

function TexPointMake(const s, t: TGeoFloat): TTexPoint; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function AffineVectorMake(const x, y, z: TGeoFloat): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function AffineVectorMake(const v: TVector): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetAffineVector(out v: TAffineVector; const x, y, z: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TAffineVector; const x, y, z: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TAffineVector; const vSrc: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TAffineVector; const vSrc: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function VectorMake(const v: TAffineVector; w: TGeoFloat = 0): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function VectorMake(const x, y, z: TGeoFloat; w: TGeoFloat = 0): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function PointMake(const x, y, z: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function PointMake(const v: TAffineVector): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function PointMake(const v: TVector): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TVector; const x, y, z: TGeoFloat; w: TGeoFloat = 0); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TVector; const av: TAffineVector; w: TGeoFloat = 0); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure SetVector(out v: TVector; const vSrc: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakePoint(out v: TVector; const x, y, z: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakePoint(out v: TVector; const av: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakePoint(out v: TVector; const av: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakeVector(out v: TAffineVector; const x, y, z: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakeVector(out v: TVector; const x, y, z: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakeVector(out v: TVector; const av: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure MakeVector(out v: TVector; const av: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure RstVector(var v: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure RstVector(var v: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// 2
function VectorEquals(const v1, v2: TVector2f): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// 3x3
function MatrixEquals(const Matrix1, Matrix2: TMatrix3f): Boolean; overload;

// 4x4
function MatrixEquals(const Matrix1, Matrix2: TMatrix4f): Boolean; overload;

// 2x
function Vector2fMake(const x, y: TGeoFloat): TVector2f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector2fMake(const Vector: TVector3f): TVector2f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector2fMake(const Vector: TVector4f): TVector2f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// 3x
function Vector3fMake(const x: TGeoFloat; const y: TGeoFloat = 0; const z: TGeoFloat = 0): TVector3f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector3fMake(const Vector: TVector2f; const z: TGeoFloat = 0): TVector3f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector3fMake(const Vector: TVector4f): TVector3f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// 4x
function Vector4fMake(const x: TGeoFloat; const y: TGeoFloat = 0; const z: TGeoFloat = 0; const w: TGeoFloat = 0): TVector4f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector4fMake(const Vector: TVector3f; const w: TGeoFloat = 0): TVector4f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function Vector4fMake(const Vector: TVector2f; const z: TGeoFloat = 0; const w: TGeoFloat = 0): TVector4f; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Vector comparison functions:
// ComparedVector
// 3f
function VectorMoreThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
function VectorMoreEqualThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
function VectorLessThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
function VectorLessEqualThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;

// 4f
function VectorMoreThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
function VectorMoreEqualThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
function VectorLessThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
function VectorLessEqualThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;

// ComparedNumber
// 3f
function VectorMoreThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorMoreEqualThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorLessThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorLessEqualThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;

// 4f
function VectorMoreThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorMoreEqualThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorLessThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
function VectorLessEqualThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;

function VectorAdd(const v1, v2: TVector2f): TVector2f; overload;

// Returns the sum of two affine vectors
function VectorAdd(const v1, v2: TAffineVector): TAffineVector; overload;

// Adds two vectors and places result in vr
procedure VectorAdd(const v1, v2: TAffineVector; var vr: TAffineVector); overload;
procedure VectorAdd(const v1, v2: TAffineVector; vr: PAffineVector); overload;

// Returns the sum of two homogeneous vectors
function VectorAdd(const v1, v2: TVector): TVector; overload;
procedure VectorAdd(const v1, v2: TVector; var vr: TVector); overload;

// Sums up f to each component of the vector
function VectorAdd(const v: TAffineVector; const f: TGeoFloat): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Sums up f to each component of the vector
function VectorAdd(const v: TVector; const f: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Adds V2 to V1, result is placed in V1
procedure AddVector(var v1: TAffineVector; const v2: TAffineVector); overload;

// Adds V2 to V1, result is placed in V1
procedure AddVector(var v1: TAffineVector; const v2: TVector); overload;

// Adds V2 to V1, result is placed in V1
procedure AddVector(var v1: TVector; const v2: TVector); overload;

// Sums up f to each component of the vector
procedure AddVector(var v: TAffineVector; const f: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Sums up f to each component of the vector
procedure AddVector(var v: TVector; const f: TGeoFloat); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Adds V2 to V1, result is placed in V1. W coordinate is always 1.
procedure AddPoint(var v1: TVector; const v2: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Returns the sum of two homogeneous vectors. W coordinate is always 1.
function PointAdd(var v1: TVector; const v2: TVector): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Adds delta to nb texpoints in src and places result in dest
procedure TexPointArrayAdd(const Src: PTexPointArray; const Delta: TTexPoint; const nb: Integer; dest: PTexPointArray); overload;
procedure TexPointArrayScaleAndAdd(const Src: PTexPointArray; const Delta: TTexPoint; const nb: Integer; const Scale: TTexPoint; dest: PTexPointArray); overload;

// Adds delta to nb vectors in src and places result in dest
procedure VectorArrayAdd(const Src: PAffineVectorArray; const Delta: TAffineVector; const nb: Integer; dest: PAffineVectorArray); overload;

// Returns V1-V2
function VectorSubtract(const v1, v2: TVector2f): TVector2f; overload;

// Subtracts V2 from V1, result is placed in V1
procedure SubtractVector(var v1: TVector2f; const v2: TVector2f); overload;

// Returns V1-V2
function VectorSubtract(const v1, v2: TAffineVector): TAffineVector; overload;

// Subtracts V2 from V1 and return value in result
procedure VectorSubtract(const v1, v2: TAffineVector; var Result: TAffineVector); overload;

// Subtracts V2 from V1 and return value in result
procedure VectorSubtract(const v1, v2: TAffineVector; var Result: TVector); overload;

// Subtracts V2 from V1 and return value in result
procedure VectorSubtract(const v1: TVector; v2: TAffineVector; var Result: TVector); overload;

// Returns V1-V2
function VectorSubtract(const v1, v2: TVector): TVector; overload;

// Subtracts V2 from V1 and return value in result
procedure VectorSubtract(const v1, v2: TVector; var Result: TVector); overload;

// Subtracts V2 from V1 and return value in result
procedure VectorSubtract(const v1, v2: TVector; var Result: TAffineVector); overload;
function VectorSubtract(const v1: TAffineVector; Delta: TGeoFloat): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function VectorSubtract(const v1: TVector; Delta: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Subtracts V2 from V1, result is placed in V1
procedure SubtractVector(var v1: TAffineVector; const v2: TAffineVector); overload;

// Subtracts V2 from V1, result is placed in V1
procedure SubtractVector(var v1: TVector; const v2: TVector); overload;

// Combine the first vector with the second : vr:=vr+v*f
procedure CombineVector(var vr: TAffineVector; const v: TAffineVector; var f: TGeoFloat); overload;
procedure CombineVector(var vr: TAffineVector; const v: TAffineVector; pf: PFloat); overload;

// Makes a linear combination of two texpoints
function TexPointCombine(const t1, t2: TTexPoint; f1, f2: TGeoFloat): TTexPoint; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of two vectors and return the result
function VectorCombine(const v1, v2: TAffineVector; const f1, f2: TGeoFloat): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of three vectors and return the result
function VectorCombine3(const v1, v2, v3: TAffineVector; const f1, f2, F3: TGeoFloat): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure VectorCombine3(const v1, v2, v3: TAffineVector; const f1, f2, F3: TGeoFloat; var vr: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Combine the first vector with the second : vr:=vr+v*f
procedure CombineVector(var vr: TVector; const v: TVector; var f: TGeoFloat); overload;

// Combine the first vector with the second : vr:=vr+v*f
procedure CombineVector(var vr: TVector; const v: TAffineVector; var f: TGeoFloat); overload;

// Makes a linear combination of two vectors and return the result
function VectorCombine(const v1, v2: TVector; const f1, f2: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of two vectors and return the result
function VectorCombine(const v1: TVector; const v2: TAffineVector; const f1, f2: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of two vectors and place result in vr
procedure VectorCombine(const v1: TVector; const v2: TAffineVector; const f1, f2: TGeoFloat; var vr: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of two vectors and place result in vr
procedure VectorCombine(const v1, v2: TVector; const f1, f2: TGeoFloat; var vr: TVector); overload;

// Makes a linear combination of two vectors and place result in vr, F1=1.0
procedure VectorCombine(const v1, v2: TVector; const f2: TGeoFloat; var vr: TVector); overload;

// Makes a linear combination of three vectors and return the result
function VectorCombine3(const v1, v2, v3: TVector; const f1, f2, F3: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Makes a linear combination of three vectors and return the result
procedure VectorCombine3(const v1, v2, v3: TVector; const f1, f2, F3: TGeoFloat; var vr: TVector); overload;

{ Calculates the dot product between V1 and V2.
  Result:=V1[X] * V2[X] + V1[Y] * V2[Y] }
function VectorDotProduct(const v1, v2: TVector2f): TGeoFloat; overload;

{ Calculates the dot product between V1 and V2.
  Result:=V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z] }
function VectorDotProduct(const v1, v2: TAffineVector): TGeoFloat; overload;

{ Calculates the dot product between V1 and V2.
  Result:=V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z] }
function VectorDotProduct(const v1, v2: TVector): TGeoFloat; overload;

{ Calculates the dot product between V1 and V2.
  Result:=V1[X] * V2[X] + V1[Y] * V2[Y] + V1[Z] * V2[Z] }
function VectorDotProduct(const v1: TVector; const v2: TAffineVector): TGeoFloat; overload;

{ Projects p on the line defined by o and direction.
  Performs VectorDotProduct(VectorSubtract(p, origin), direction), which,
  if direction is normalized, computes the distance between origin and the
  projection of p on the (origin, direction) line. }
function PointProject(const p, origin, direction: TAffineVector): TGeoFloat; overload;
function PointProject(const p, origin, direction: TVector): TGeoFloat; overload;

// Calculates the cross product between vector 1 and 2
function VectorCrossProduct(const v1, v2: TAffineVector): TAffineVector; overload;

// Calculates the cross product between vector 1 and 2
function VectorCrossProduct(const v1, v2: TVector): TVector; overload;

// Calculates the cross product between vector 1 and 2, place result in vr
procedure VectorCrossProduct(const v1, v2: TVector; var vr: TVector); overload;

// Calculates the cross product between vector 1 and 2, place result in vr
procedure VectorCrossProduct(const v1, v2: TAffineVector; var vr: TVector); overload;

// Calculates the cross product between vector 1 and 2, place result in vr
procedure VectorCrossProduct(const v1, v2: TVector; var vr: TAffineVector); overload;

// Calculates the cross product between vector 1 and 2, place result in vr
procedure VectorCrossProduct(const v1, v2: TAffineVector; var vr: TAffineVector); overload;

// Calculates linear interpolation between start and stop at point t
function Lerp(const Start, stop, t: TGeoFloat): TGeoFloat; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Calculates angular interpolation between start and stop at point t
function AngleLerp(Start, stop, t: TGeoFloat): TGeoFloat; {$IFDEF INLINE_ASM} inline; {$ENDIF}

{ This is used for interpolating between 2 matrices. The result
  is used to reposition the model parts each frame. }
function MatrixLerp(const m1, m2: TMatrix; const Delta: TGeoFloat): TMatrix;

{ Calculates the angular distance between two angles in radians.
  Result is in the [0; PI] range. }
function DistanceBetweenAngles(angle1, angle2: TGeoFloat): TGeoFloat;

// Calculates linear interpolation between texpoint1 and texpoint2 at point t
function TexPointLerp(const t1, t2: TTexPoint; t: TGeoFloat): TTexPoint; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Calculates linear interpolation between vector1 and vector2 at point t
function VectorLerp(const v1, v2: TAffineVector; t: TGeoFloat): TAffineVector; overload;

// Calculates linear interpolation between vector1 and vector2 at point t, places result in vr
procedure VectorLerp(const v1, v2: TAffineVector; t: TGeoFloat; var vr: TAffineVector); overload;

// Calculates linear interpolation between vector1 and vector2 at point t
function VectorLerp(const v1, v2: TVector; t: TGeoFloat): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Calculates linear interpolation between vector1 and vector2 at point t, places result in vr
procedure VectorLerp(const v1, v2: TVector; t: TGeoFloat; var vr: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

function VectorAngleLerp(const v1, v2: TAffineVector; t: TGeoFloat): TAffineVector; overload;
function VectorAngleCombine(const v1, v2: TAffineVector; f: TGeoFloat): TAffineVector; overload;

// Calculates linear interpolation between vector arrays
procedure VectorArrayLerp(const src1, src2: PVectorArray; t: TGeoFloat; n: Integer; dest: PVectorArray); overload;
procedure VectorArrayLerp(const src1, src2: PAffineVectorArray; t: TGeoFloat; n: Integer; dest: PAffineVectorArray); overload;
procedure VectorArrayLerp(const src1, src2: PTexPointArray; t: TGeoFloat; n: Integer; dest: PTexPointArray); overload;

type
  TGLInterpolationType = (itLinear, itPower, itSin, itSinAlt, itTan, itLn, itExp);

  { There functions that do the same as "Lerp", but add some distortions. }
function InterpolatePower(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
function InterpolateLn(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
function InterpolateExp(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;

{ Only valid where Delta belongs to [0..1] }
function InterpolateSin(const Start, stop, Delta: TGeoFloat): TGeoFloat;
function InterpolateTan(const Start, stop, Delta: TGeoFloat): TGeoFloat;

{ "Alt" functions are valid everywhere }
function InterpolateSinAlt(const Start, stop, Delta: TGeoFloat): TGeoFloat;

function InterpolateCombinedFastPower(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
function InterpolateCombinedSafe(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;
function InterpolateCombinedFast(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;
function InterpolateCombined(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;

{ Calculates the length of a vector following the equation sqrt(x*x+y*y). }
function VectorLength(const x, y: TGeoFloat): TGeoFloat; overload;
{ Calculates the length of a vector following the equation sqrt(x*x+y*y+z*z). }
function VectorLength(const x, y, z: TGeoFloat): TGeoFloat; overload;
// Calculates the length of a vector following the equation sqrt(x*x+y*y).
function VectorLength(const v: TVector2f): TGeoFloat; overload;
// Calculates the length of a vector following the equation sqrt(x*x+y*y+z*z).
function VectorLength(const v: TAffineVector): TGeoFloat; overload;
// Calculates the length of a vector following the equation sqrt(x*x+y*y+z*z+w*w).
function VectorLength(const v: TVector): TGeoFloat; overload;
{ 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. }
function VectorLength(const v: array of TGeoFloat): TGeoFloat; overload;

{ Calculates norm of a vector which is defined as norm = x * x + y * y
  Also known as "Norm 2" in the math world, this is sqr(VectorLength). }
function VectorNorm(const x, y: TGeoFloat): TGeoFloat; overload;
{ Calculates norm of a vector which is defined as norm = x*x + y*y + z*z
  Also known as "Norm 2" in the math world, this is sqr(VectorLength). }
function VectorNorm(const v: TAffineVector): TGeoFloat; overload;
{ Calculates norm of a vector which is defined as norm = x*x + y*y + z*z
  Also known as "Norm 2" in the math world, this is sqr(VectorLength). }
function VectorNorm(const v: TVector): TGeoFloat; overload;
{ Calculates norm of a vector which is defined as norm = v[0]*v[0] + ...
  Also known as "Norm 2" in the math world, this is sqr(VectorLength). }
function VectorNorm(var v: array of TGeoFloat): TGeoFloat; overload;

// Transforms a vector to unit length
procedure NormalizeVector(var v: TVector2f); overload;
// Returns the vector transformed to unit length
// Transforms a vector to unit length
procedure NormalizeVector(var v: TAffineVector); overload;
// Transforms a vector to unit length
procedure NormalizeVector(var v: TVector); overload;
// Returns the vector transformed to unit length
function VectorNormalize(const v: TVector2f): TVector2f; overload;
// Returns the vector transformed to unit length
function VectorNormalize(const v: TAffineVector): TAffineVector; overload;
// Returns the vector transformed to unit length (w component dropped)
function VectorNormalize(const v: TVector): TVector; overload;

// Transforms vectors to unit length
procedure NormalizeVectorArray(List: PAffineVectorArray; n: Integer); overload;

{ Calculates the cosine of the angle between Vector1 and Vector2.
  Result = DotProduct(V1, V2) / (Length(V1) * Length(V2)) }
function VectorAngleCosine(const v1, v2: TAffineVector): TGeoFloat; overload;

{ Calculates the cosine of the angle between Vector1 and Vector2.
  Result = DotProduct(V1, V2) / (Length(V1) * Length(V2)) }
function VectorAngleCosine(const v1, v2: TVector): TGeoFloat; overload;

// Negates the vector
function VectorNegate(const v: TAffineVector): TAffineVector; overload;
function VectorNegate(const v: TVector): TVector; overload;

// Negates the vector
procedure NegateVector(var v: TAffineVector); overload;
// Negates the vector
procedure NegateVector(var v: TVector); overload;
// Negates the vector
procedure NegateVector(var v: array of TGeoFloat); overload;

// Scales given vector by a factor
procedure ScaleVector(var v: TVector2f; factor: TGeoFloat); overload;
// Scales given vector by a factor
procedure ScaleVector(var v: TAffineVector; factor: TGeoFloat); overload;
{ Scales given vector by another vector.
  v[x]:=v[x]*factor[x], v[y]:=v[y]*factor[y] etc. }
procedure ScaleVector(var v: TAffineVector; const factor: TAffineVector); overload;
// Scales given vector by a factor
procedure ScaleVector(var v: TVector; factor: TGeoFloat); overload;
{ Scales given vector by another vector.
  v[x]:=v[x]*factor[x], v[y]:=v[y]*factor[y] etc. }
procedure ScaleVector(var v: TVector; const factor: TVector); overload;

// Returns a vector scaled by a factor
function VectorScale(const v: TVector2f; factor: TGeoFloat): TVector2f; overload;
// Returns a vector scaled by a factor
function VectorScale(const v: TAffineVector; factor: TGeoFloat): TAffineVector; overload;
// Scales a vector by a factor and places result in vr
procedure VectorScale(const v: TAffineVector; factor: TGeoFloat; var vr: TAffineVector); overload;
// Returns a vector scaled by a factor
function VectorScale(const v: TVector; factor: TGeoFloat): TVector; overload;
// Scales a vector by a factor and places result in vr
procedure VectorScale(const v: TVector; factor: TGeoFloat; var vr: TVector); overload;
// Scales a vector by a factor and places result in vr
procedure VectorScale(const v: TVector; factor: TGeoFloat; var vr: TAffineVector); overload;
// Scales given vector by another vector
function VectorScale(const v: TAffineVector; const factor: TAffineVector): TAffineVector; overload;
// RScales given vector by another vector
function VectorScale(const v: TVector; const factor: TVector): TVector; overload;

{ Divides given vector by another vector.
  v[x]:=v[x]/divider[x], v[y]:=v[y]/divider[y] etc. }
procedure DivideVector(var v: TVector; const divider: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
procedure DivideVector(var v: TAffineVector; const divider: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

function VectorDivide(const v: TVector; const divider: TVector): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function VectorDivide(const v: TAffineVector; const divider: TAffineVector): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// True if all components are equal.
function TexpointEquals(const p1, p2: TTexPoint): Boolean; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// True if all components are equal.
// True if all components are equal.
function VectorEquals(const v1, v2: TVector): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// True if all components are equal.
function VectorEquals(const v1, v2: TAffineVector): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// True if X, Y and Z components are equal.
function AffineVectorEquals(const v1, v2: TVector): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// True if x=y=z=0, w ignored
function VectorIsNull(const v: TVector): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// True if x=y=z=0, w ignored
function VectorIsNull(const v: TAffineVector): Boolean; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

{ Calculates Abs(v1[x]-v2[x])+Abs(v1[y]-v2[y]), also know as "Norm1". }
function VectorSpacing(const v1, v2: TTexPoint): TGeoFloat; overload;
{ Calculates Abs(v1[x]-v2[x])+Abs(v1[y]-v2[y])+..., also know as "Norm1". }
function VectorSpacing(const v1, v2: TAffineVector): TGeoFloat; overload;
{ Calculates Abs(v1[x]-v2[x])+Abs(v1[y]-v2[y])+..., also know as "Norm1". }
function VectorSpacing(const v1, v2: TVector): TGeoFloat; overload;

{ Calculates distance between two vectors.
  ie. sqrt(sqr(v1[x]-v2[x])+...) }
function VectorDistance(const v1, v2: TAffineVector): TGeoFloat; overload;
{ Calculates distance between two vectors.
  ie. sqrt(sqr(v1[x]-v2[x])+...) (w component ignored) }
function VectorDistance(const v1, v2: TVector): TGeoFloat; overload;

{ Calculates the "Norm 2" between two vectors.
  ie. sqr(v1[x]-v2[x])+... }
function VectorDistance2(const v1, v2: TAffineVector): TGeoFloat; overload;
{ Calculates the "Norm 2" between two vectors.
  ie. sqr(v1[x]-v2[x])+... (w component ignored) }
function VectorDistance2(const v1, v2: TVector): TGeoFloat; overload;

{ Calculates a vector perpendicular to N.
  N is assumed to be of unit length, subtract out any component Parallel to N }
function VectorPerpendicular(const v, n: TAffineVector): TAffineVector;
// Reflects vector V against N (assumes N is normalized)
function VectorReflect(const v, n: TAffineVector): TAffineVector;
// Rotates Vector about Axis with Angle radians
procedure RotateVector(var Vector: TVector; const axis: TAffineVector; angle: TGeoFloat); overload;
// Rotates Vector about Axis with Angle radians
procedure RotateVector(var Vector: TVector; const axis: TVector; angle: TGeoFloat); overload;

// Rotate given vector around the Y axis (alpha is in rad)
procedure RotateVectorAroundY(var v: TAffineVector; alpha: TGeoFloat);
// Returns given vector rotated around the X axis (alpha is in rad)
function VectorRotateAroundX(const v: TAffineVector; alpha: TGeoFloat): TAffineVector; overload;
// Returns given vector rotated around the Y axis (alpha is in rad)
function VectorRotateAroundY(const v: TAffineVector; alpha: TGeoFloat): TAffineVector; overload;
// Returns given vector rotated around the Y axis in vr (alpha is in rad)
procedure VectorRotateAroundY(const v: TAffineVector; alpha: TGeoFloat; var vr: TAffineVector); overload;
// Returns given vector rotated around the Z axis (alpha is in rad)
function VectorRotateAroundZ(const v: TAffineVector; alpha: TGeoFloat): TAffineVector; overload;

// Vector components are replaced by their Abs() value.
procedure AbsVector(var v: TVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// Vector components are replaced by their Abs() value.
procedure AbsVector(var v: TAffineVector); overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// Returns a vector with components replaced by their Abs value.
function VectorAbs(const v: TVector): TVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
// Returns a vector with components replaced by their Abs value.
function VectorAbs(const v: TAffineVector): TAffineVector; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

// Returns true if both vector are colinear
function IsColinear(const v1, v2: TVector2f): Boolean; overload;
// Returns true if both vector are colinear
function IsColinear(const v1, v2: TAffineVector): Boolean; overload;
// Returns true if both vector are colinear
function IsColinear(const v1, v2: TVector): Boolean; overload;
// ------------------------------------------------------------------------------
// Matrix functions
// ------------------------------------------------------------------------------

procedure SetMatrix(var dest: TAffineMatrix; const Src: TMatrix); overload;
procedure SetMatrix(var dest: TMatrix; const Src: TAffineMatrix); overload;

procedure SetMatrixRow(var dest: TMatrix; rowNb: Integer; const aRow: TVector); overload;

// Creates scale matrix
function CreateScaleMatrix(const v: TAffineVector): TMatrix; overload;
// Creates scale matrix
function CreateScaleMatrix(const v: TVector): TMatrix; overload;
// Creates translation matrix
function CreateTranslationMatrix(const v: TAffineVector): TMatrix; overload;
// Creates translation matrix
function CreateTranslationMatrix(const v: TVector): TMatrix; overload;
{ Creates a scale+translation matrix.
  Scale is applied BEFORE applying offset }
function CreateScaleAndTranslationMatrix(const Scale, Offset: TVector): TMatrix; overload;
// Creates matrix for rotation about x-axis (angle in rad)
function CreateRotationMatrixX(const sine, cosine: TGeoFloat): TMatrix; overload;
function CreateRotationMatrixX(const angle: TGeoFloat): TMatrix; overload;
// Creates matrix for rotation about y-axis (angle in rad)
function CreateRotationMatrixY(const sine, cosine: TGeoFloat): TMatrix; overload;
function CreateRotationMatrixY(const angle: TGeoFloat): TMatrix; overload;
// Creates matrix for rotation about z-axis (angle in rad)
function CreateRotationMatrixZ(const sine, cosine: TGeoFloat): TMatrix; overload;
function CreateRotationMatrixZ(const angle: TGeoFloat): TMatrix; overload;
// Creates a rotation matrix along the given Axis by the given Angle in radians.
function CreateRotationMatrix(const anAxis: TAffineVector; angle: TGeoFloat): TMatrix; overload;
function CreateRotationMatrix(const anAxis: TVector; angle: TGeoFloat): TMatrix; overload;
// Creates a rotation matrix along the given Axis by the given Angle in radians.
function CreateAffineRotationMatrix(const anAxis: TAffineVector; angle: TGeoFloat): TAffineMatrix;

// Multiplies two 3x3 matrices
function MatrixMultiply(const m1, m2: TAffineMatrix): TAffineMatrix; overload;
// Multiplies two 4x4 matrices
function MatrixMultiply(const m1, m2: TMatrix): TMatrix; overload;
// Multiplies M1 by M2 and places result in MResult
procedure MatrixMultiply(const m1, m2: TMatrix; var MResult: TMatrix); overload;

// Transforms a homogeneous vector by multiplying it with a matrix
function VectorTransform(const v: TVector; const M: TMatrix): TVector; overload;
// Transforms a homogeneous vector by multiplying it with a matrix
function VectorTransform(const v: TVector; const M: TAffineMatrix): TVector; overload;
// Transforms an affine vector by multiplying it with a matrix
function VectorTransform(const v: TAffineVector; const M: TMatrix): TAffineVector; overload;
// Transforms an affine vector by multiplying it with a matrix
function VectorTransform(const v: TAffineVector; const M: TAffineMatrix): TAffineVector; overload;

// Determinant of a 3x3 matrix
function MatrixDeterminant(const M: TAffineMatrix): TGeoFloat; overload;
// Determinant of a 4x4 matrix
function MatrixDeterminant(const M: TMatrix): TGeoFloat; overload;

{ Adjoint of a 4x4 matrix.
  used in the computation of the inverse of a 4x4 matrix }
procedure AdjointMatrix(var M: TMatrix); overload;
{ Adjoint of a 3x3 matrix.
  used in the computation of the inverse of a 3x3 matrix }
procedure AdjointMatrix(var M: TAffineMatrix); overload;

// Multiplies all elements of a 3x3 matrix with a factor
procedure ScaleMatrix(var M: TAffineMatrix; const factor: TGeoFloat); overload;
// Multiplies all elements of a 4x4 matrix with a factor
procedure ScaleMatrix(var M: TMatrix; const factor: TGeoFloat); overload;

// Adds the translation vector into the matrix
procedure TranslateMatrix(var M: TMatrix; const v: TAffineVector); overload;
procedure TranslateMatrix(var M: TMatrix; const v: TVector); overload;

{ Normalize the matrix and remove the translation component.
  The resulting matrix is an orthonormal matrix (Y direction preserved, then Z) }
procedure NormalizeMatrix(var M: TMatrix);

// Computes transpose of 3x3 matrix
procedure TransposeMatrix(var M: TAffineMatrix); overload;
// Computes transpose of 4x4 matrix
procedure TransposeMatrix(var M: TMatrix); overload;

// Finds the inverse of a 4x4 matrix
procedure InvertMatrix(var M: TMatrix); overload;
function MatrixInvert(const M: TMatrix): TMatrix; overload;

// Finds the inverse of a 3x3 matrix;
procedure InvertMatrix(var M: TAffineMatrix); overload;
function MatrixInvert(const M: TAffineMatrix): TAffineMatrix; overload;

{ Finds the inverse of an angle preserving matrix.
  Angle preserving matrices can combine translation, rotation and isotropic
  scaling, other matrices won't be properly inverted by this function. }
function AnglePreservingMatrixInvert(const mat: TMatrix): TMatrix;

{ Decompose a non-degenerated 4x4 transformation matrix into the sequence of transformations that produced it.
  Modified by ml then eg, original Author: Spencer W. Thomas, University of Michigan
  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. }
function MatrixDecompose(const M: TMatrix; var Tran: TTransformations): Boolean;

function CreateLookAtMatrix(const eye, center, normUp: TVector): TMatrix;
function CreateMatrixFromFrustum(Left, Right, Bottom, Top, ZNear, ZFar: TGeoFloat): TMatrix;
function CreatePerspectiveMatrix(FOV, Aspect, ZNear, ZFar: TGeoFloat): TMatrix;
function CreateOrthoMatrix(Left, Right, Bottom, Top, ZNear, ZFar: TGeoFloat): TMatrix;
function CreatePickMatrix(x, y, deltax, deltay: TGeoFloat; const viewport: TVector4i): TMatrix;
function Project(objectVector: TVector; const ViewProjMatrix: TMatrix; const viewport: TVector4i; out WindowVector: TVector): Boolean;
function UnProject(WindowVector: TVector; ViewProjMatrix: TMatrix; const viewport: TVector4i; out objectVector: TVector): Boolean;

// ------------------------------------------------------------------------------
// Plane functions
// ------------------------------------------------------------------------------

// Computes the parameters of a plane defined by three points.
function PlaneMake(const p1, p2, p3: TAffineVector): THmgPlane; overload;
function PlaneMake(const p1, p2, p3: TVector): THmgPlane; overload;
// Computes the parameters of a plane defined by a point and a normal.
function PlaneMake(const Point, normal: TAffineVector): THmgPlane; overload;
function PlaneMake(const Point, normal: TVector): THmgPlane; overload;

// Normalize a plane so that point evaluation = plane distance. }
procedure NormalizePlane(var plane: THmgPlane);

{ Calculates the cross-product between the plane normal and plane to point vector.
  This functions gives an hint as to were the point is, if the point is in the
  half-space pointed by the vector, result is positive.
  This function performs an homogeneous space dot-product. }
function PlaneEvaluatePoint(const plane: THmgPlane; const Point: TAffineVector): TGeoFloat; overload;
function PlaneEvaluatePoint(const plane: THmgPlane; const Point: TVector): TGeoFloat; overload;

{ Calculate the normal of a plane defined by three points. }
function CalcPlaneNormal(const p1, p2, p3: TAffineVector): TAffineVector; overload;
procedure CalcPlaneNormal(const p1, p2, p3: TAffineVector; var vr: TAffineVector); overload;
procedure CalcPlaneNormal(const p1, p2, p3: TVector; var vr: TAffineVector); overload;

{ Returns true if point is in the half-space defined by a plane with normal.
  The plane itself is not considered to be in the tested halfspace. }
function PointIsInHalfSpace(const Point, planePoint, planeNormal: TVector): Boolean; overload;
function PointIsInHalfSpace(const Point, planePoint, planeNormal: TAffineVector): Boolean; overload;
function PointIsInHalfSpace(const Point: TAffineVector; plane: THmgPlane): Boolean; overload;

{ Computes algebraic distance between point and plane.
  Value will be positive if the point is in the halfspace pointed by the normal,
  negative on the other side. }
function PointPlaneDistance(const Point, planePoint, planeNormal: TVector): TGeoFloat; overload;
function PointPlaneDistance(const Point, planePoint, planeNormal: TAffineVector): TGeoFloat; overload;
function PointPlaneDistance(const Point: TAffineVector; plane: THmgPlane): TGeoFloat; overload;

{ Computes point to plane projection. Plane and direction have to be normalized }
function PointPlaneOrthoProjection(const Point: TAffineVector; const plane: THmgPlane; var inter: TAffineVector; bothface: Boolean): Boolean;
function PointPlaneProjection(const Point, direction: TAffineVector; const plane: THmgPlane; var inter: TAffineVector; bothface: Boolean): Boolean;

{ Computes segment / plane intersection return false if there isn't an intersection }
function SegmentPlaneIntersection(const ptA, ptB: TAffineVector; const plane: THmgPlane; var inter: TAffineVector): Boolean;

{ Computes point to triangle projection. Direction has to be normalized }
function PointTriangleOrthoProjection(const Point, ptA, ptB, ptC: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;
function PointTriangleProjection(const Point, direction, ptA, ptB, ptC: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;

{ Returns true if line intersect ABC triangle. }
function IsLineIntersectTriangle(const Point, direction, ptA, ptB, ptC: TAffineVector): Boolean;

{ Computes point to Quad projection. Direction has to be normalized. Quad have to be flat and convex }
function PointQuadOrthoProjection(const Point, ptA, ptB, ptC, ptD: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;
function PointQuadProjection(const Point, direction, ptA, ptB, ptC, ptD: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;

{ Returns true if line intersect ABCD quad. Quad have to be flat and convex }
function IsLineIntersectQuad(const Point, direction, ptA, ptB, ptC, ptD: TAffineVector): Boolean;

{ Computes point to disk projection. Direction has to be normalized }
function PointDiskOrthoProjection(const Point, center, up: TAffineVector; const radius: TGeoFloat; var inter: TAffineVector; bothface: Boolean): Boolean;
function PointDiskProjection(const Point, direction, center, up: TAffineVector; const radius: TGeoFloat; var inter: TAffineVector; bothface: Boolean): Boolean;

{ Computes closest point on a segment (a segment is a limited line). }
function PointSegmentClosestPoint(const Point, segmentStart, segmentStop: TAffineVector): TAffineVector; overload;
function PointSegmentClosestPoint(const Point, segmentStart, segmentStop: TVector): TVector; overload;

{ Computes algebraic distance between segment and line (a segment is a limited line). }
function PointSegmentDistance(const Point, segmentStart, segmentStop: TAffineVector): TGeoFloat;

{ Computes closest point on a line. }
function PointLineClosestPoint(const Point, linePoint, lineDirection: TAffineVector): TAffineVector;

{ Computes algebraic distance between point and line. }
function PointLineDistance(const Point, linePoint, lineDirection: TAffineVector): TGeoFloat;

{ Computes the closest points (2) given two segments. }
procedure SegmentSegmentClosestPoint(const S0Start, S0Stop, S1Start, S1Stop: TAffineVector; var Segment0Closest, Segment1Closest: TAffineVector);

{ Computes the closest distance between two segments. }
function SegmentSegmentDistance(const S0Start, S0Stop, S1Start, S1Stop: TAffineVector): TGeoFloat;

{ Computes the closest distance between two lines. }
function LineLineDistance(const linePt0, lineDir0, linePt1, lineDir1: TAffineVector): TGeoFloat;

// ------------------------------------------------------------------------------
// Quaternion functions
// ------------------------------------------------------------------------------

type
  TEulerOrder = (eulXYZ, eulXZY, eulYXZ, eulYZX, eulZXY, eulZYX);

  // Creates a quaternion from the given values
function QuaternionMake(const Imag: array of TGeoFloat; Real: TGeoFloat): TQuaternion;
// Returns the conjugate of a quaternion
function QuaternionConjugate(const q: TQuaternion): TQuaternion;
// Returns the magnitude of the quaternion
function QuaternionMagnitude(const q: TQuaternion): TGeoFloat;
// Normalizes the given quaternion
procedure NormalizeQuaternion(var q: TQuaternion);

// Constructs a unit quaternion from two points on unit sphere
function QuaternionFromPoints(const v1, v2: TAffineVector): TQuaternion;
// Converts a unit quaternion into two points on a unit sphere
procedure QuaternionToPoints(const q: TQuaternion; var ArcFrom, ArcTo: TAffineVector);
// Constructs a unit quaternion from a rotation matrix
function QuaternionFromMatrix(const mat: TMatrix): TQuaternion;
{ Constructs a rotation matrix from (possibly non-unit) quaternion.
  Assumes matrix is used to multiply column vector on the left:<br>
  vnew = mat vold.
  Works correctly for right-handed coordinate system and right-handed rotations. }
function QuaternionToMatrix(quat: TQuaternion): TMatrix;
{ Constructs an affine rotation matrix from (possibly non-unit) quaternion. }
function QuaternionToAffineMatrix(quat: TQuaternion): TAffineMatrix;
// Constructs quaternion from angle (in deg) and axis
function QuaternionFromAngleAxis(const angle: TGeoFloat; const axis: TAffineVector): TQuaternion;
// Constructs quaternion from Euler angles
function QuaternionFromRollPitchYaw(const r, p, y: TGeoFloat): TQuaternion;
// Constructs quaternion from Euler angles in arbitrary order (angles in degrees)
function QuaternionFromEuler(const x, y, z: TGeoFloat; eulerOrder: TEulerOrder): 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. }
function QuaternionMultiply(const qL, qR: TQuaternion): TQuaternion;

{ Spherical linear interpolation of unit quaternions with spins.
  QStart, QEnd - start and end unit quaternions<br>
  t            - interpolation parameter (0 to 1)<br>
  Spin         - number of extra spin rotations to involve<br> }
function QuaternionSlerp(const QStart, QEnd: TQuaternion; Spin: Integer; t: TGeoFloat): TQuaternion; overload;
function QuaternionSlerp(const Source, dest: TQuaternion; const t: TGeoFloat): TQuaternion; overload;

// ------------------------------------------------------------------------------
// Logarithmic and exponential functions
// ------------------------------------------------------------------------------

{ Return ln(1 + X),  accurate for X near 0. }
function LnXP1(x: Extended): Extended;
{ Log base 10 of X }
function Log10(x: Extended): Extended;
{ Log base 2 of X }
function Log2(x: Extended): Extended; overload;
{ Log base 2 of X }
function Log2(x: TGeoFloat): TGeoFloat; overload;
{ Log base N of X }
function LogN(Base, x: Extended): Extended;
{ Raise base to an integer. }
function IntPower(Base: Extended; Exponent: Integer): Extended;
{ Raise base to any power.
  For fractional exponents, or |exponents| > MaxInt, base must be > 0. }
function Power(const Base, Exponent: TGeoFloat): TGeoFloat; overload;
{ Raise base to an integer. }
function Power(Base: TGeoFloat; Exponent: Integer): TGeoFloat; overload;
function Power(Base: TGeoFloat; Exponent: Int64): TGeoFloat; overload;

// ------------------------------------------------------------------------------
// Trigonometric functions
// ------------------------------------------------------------------------------

function DegToRad(const Degrees: Extended): Extended; overload;
function DegToRad(const Degrees: TGeoFloat): TGeoFloat; overload;
function RadToDeg(const Radians: Extended): Extended; overload;
function RadToDeg(const Radians: TGeoFloat): TGeoFloat; overload;

// Normalize to an angle in the [-PI; +PI] range
function NormalizeAngle(angle: TGeoFloat): TGeoFloat;
// Normalize to an angle in the [-180; 180] range
function NormalizeDegAngle(angle: TGeoFloat): TGeoFloat;

// Calculates sine and cosine from the given angle Theta
procedure SinCos(const Theta: Extended; out Sin, Cos: Extended); overload;
// Calculates sine and cosine from the given angle Theta
procedure SinCos(const Theta: TGeoFloat; out Sin, Cos: TGeoFloat); overload;
{ Calculates sine and cosine from the given angle Theta and Radius.
  sin and cos values calculated from theta are multiplicated by radius. }
procedure SinCos(const Theta, radius: Double; out Sin, Cos: Extended); overload;
{ Calculates sine and cosine from the given angle Theta and Radius.
  sin and cos values calculated from theta are multiplicated by radius. }
procedure SinCos(const Theta, radius: TGeoFloat; out Sin, Cos: TGeoFloat); overload;

{ Fills up the two given dynamic arrays with sin cos values.
  start and stop angles must be given in degrees, the number of steps is
  determined by the length of the given arrays. }
procedure PrepareSinCosCache(var s, c: array of TGeoFloat; startAngle, stopAngle: TGeoFloat);

function ArcCos(const x: Extended): Extended; overload;
function ArcCos(const x: TGeoFloat): TGeoFloat; overload;
function ArcSin(const x: Extended): Extended; overload;
function ArcSin(const x: TGeoFloat): TGeoFloat; overload;
function ArcTan2(const y, x: Extended): Extended; overload;
function ArcTan2(const y, x: TGeoFloat): TGeoFloat; overload;

{ Fast ArcTan2 approximation, about 0.07 rads accuracy. }
function FastArcTan2(y, x: TGeoFloat): TGeoFloat;

function Tan(const x: Extended): Extended; overload;
function Tan(const x: TGeoFloat): TGeoFloat; overload;
function CoTan(const x: Extended): Extended; overload;
function CoTan(const x: TGeoFloat): TGeoFloat; overload;

// ------------------------------------------------------------------------------
// Hyperbolic Trigonometric functions
// ------------------------------------------------------------------------------
function Sinh(const x: TGeoFloat): TGeoFloat; overload;
function Sinh(const x: Double): Double; overload;
function Cosh(const x: TGeoFloat): TGeoFloat; overload;
function Cosh(const x: Double): Double; overload;

// ------------------------------------------------------------------------------
// Miscellanious math functions
// ------------------------------------------------------------------------------

{ Computes 1/Sqrt(v). }
function RSqrt(v: TGeoFloat): TGeoFloat;
{ Computes 1/Sqrt(Sqr(x)+Sqr(y)). }
function RLength(x, y: TGeoFloat): TGeoFloat;
{ Computes an integer sqrt approximation. }
function ISqrt(i: Integer): Integer;
{ Computes an integer length Result:=Sqrt(x*x+y*y). }
function ILength(x, y: Integer): Integer; overload;
function ILength(x, y, z: Integer): Integer; overload;

{ Generates a random point on the unit sphere.
  Point repartition is correctly isotropic with no privilegied direction. }
procedure RandomPointOnSphere(var p: TAffineVector);

{ Rounds the floating point value to the closest integer.
  Behaves like Round but returns a floating point value like Int. }
function RoundInt(v: TGeoFloat): TGeoFloat; overload;
function RoundInt(v: Extended): Extended; overload;

function Trunc(x: Extended): Int64;
function Round(x: Extended): Int64;
function Frac(x: Extended): Extended;

function Ceil(v: TGeoFloat): Integer; overload;
function Ceil64(v: Extended): Int64; overload;
function Floor(v: TGeoFloat): Integer; overload;
function Floor64(v: Extended): Int64; overload;

{ Multiples i by s and returns the rounded result. }
function ScaleAndRound(i: Integer; var s: TGeoFloat): Integer;

{ Returns the sign of the x value using the (-1, 0, +1) convention }
function Sign(x: TGeoFloat): Integer;
function SignStrict(x: TGeoFloat): Integer;

{ Returns True if x is in [a; b] }
function IsInRange(const x, a, b: TGeoFloat): Boolean; overload;
function IsInRange(const x, a, b: Double): Boolean; overload;

{ Returns True if p is in the cube defined by d. }
function IsInCube(const p, d: TAffineVector): Boolean; overload;
function IsInCube(const p, d: TVector): Boolean; overload;

{ Returns the minimum value of the array. }
function MinFloat(values: PSingleArray; nbItems: Integer): TGeoFloat; overload;
function MinFloat(values: PDoubleArray; nbItems: Integer): Double; overload;
{ Returns the minimum of given values. }
function MinFloat(const v1, v2: TGeoFloat): TGeoFloat; overload;
function MinFloat(const v: array of TGeoFloat): TGeoFloat; overload;
function MinFloat(const v1, v2: Double): Double; overload;
function MinFloat(const v1, v2, v3: TGeoFloat): TGeoFloat; overload;
function MinFloat(const v1, v2, v3: Double): Double; overload;
{ Returns the maximum value of the array. }
function MaxFloat(values: PSingleArray; nbItems: Integer): TGeoFloat; overload;
function MaxFloat(values: PDoubleArray; nbItems: Integer): Double; overload;
function MaxFloat(const v: array of TGeoFloat): TGeoFloat; overload;
{ Returns the maximum of given values. }
function MaxFloat(const v1, v2: TGeoFloat): TGeoFloat; overload;
function MaxFloat(const v1, v2: Double): Double; overload;
function MaxFloat(const v1, v2, v3: TGeoFloat): TGeoFloat; overload;
function MaxFloat(const v1, v2, v3: Double): Double; overload;

function MinInteger(const v1, v2: Integer): Integer; overload;
function MinInteger(const v1, v2: Cardinal): Cardinal; overload;
function MinInteger(const v1, v2, v3: Integer): Integer; overload;
function MinInteger(const v1, v2, v3: Cardinal): Cardinal; overload;

function MaxInteger(const v1, v2: Integer): Integer; overload;
function MaxInteger(const v1, v2: Cardinal): Cardinal; overload;
function MaxInteger(const v1, v2, v3: Integer): Integer; overload;
function MaxInteger(const v1, v2, v3: Cardinal): Cardinal; overload;

function ClampInteger(const Value, Min, Max: Integer): Integer; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}
function ClampInteger(const Value, Min, Max: Cardinal): Cardinal; overload; {$IFDEF INLINE_ASM} inline; {$ENDIF}

{ Computes the triangle's area. }
function TriangleArea(const p1, p2, p3: TAffineVector): TGeoFloat; overload;
{ Computes the polygons's area.
  Points must be coplanar. Polygon needs not be convex. }
function PolygonArea(const p: PAffineVectorArray; nSides: Integer): TGeoFloat; overload;
{ Computes a 2D triangle's signed area.
  Only X and Y coordinates are used, Z is ignored. }
function TriangleSignedArea(const p1, p2, p3: TAffineVector): TGeoFloat; overload;
{ Computes a 2D polygon's signed area.
  Only X and Y coordinates are used, Z is ignored. Polygon needs not be convex. }
function PolygonSignedArea(const p: PAffineVectorArray; nSides: Integer): TGeoFloat; overload;

{ Multiplies values in the array by factor.
  This function is especially efficient for large arrays, it is not recommended
  for arrays that have less than 10 items.<br>
  Expected performance is 4 to 5 times that of a Deliph-compiled loop on AMD
  CPUs, and 2 to 3 when 3DNow! isn't available. }
procedure ScaleFloatArray(values: PSingleArray; nb: Integer; var factor: TGeoFloat); overload;
procedure ScaleFloatArray(var values: TSingleArray; factor: TGeoFloat); overload;

{ Adds delta to values in the array.
  Array size must be a multiple of four. }
procedure OffsetFloatArray(values: PSingleArray; nb: Integer; var Delta: TGeoFloat); overload;
procedure OffsetFloatArray(var values: array of TGeoFloat; Delta: TGeoFloat); overload;
procedure OffsetFloatArray(valuesDest, valuesDelta: PSingleArray; nb: Integer); overload;

{ Returns the max of the X, Y and Z components of a vector (W is ignored). }
function MaxXYZComponent(const v: TVector): TGeoFloat; overload;
function MaxXYZComponent(const v: TAffineVector): TGeoFloat; overload;
{ Returns the min of the X, Y and Z components of a vector (W is ignored). }
function MinXYZComponent(const v: TVector): TGeoFloat; overload;
function MinXYZComponent(const v: TAffineVector): TGeoFloat; overload;
{ Returns the max of the Abs(X), Abs(Y) and Abs(Z) components of a vector (W is ignored). }
function MaxAbsXYZComponent(v: TVector): TGeoFloat;
{ Returns the min of the Abs(X), Abs(Y) and Abs(Z) components of a vector (W is ignored). }
function MinAbsXYZComponent(v: TVector): TGeoFloat;
{ Replace components of v with the max of v or v1 component.
  Maximum is computed per component. }
procedure MaxVector(var v: TVector; const v1: TVector); overload;
procedure MaxVector(var v: TAffineVector; const v1: TAffineVector); overload;
{ Replace components of v with the min of v or v1 component.
  Minimum is computed per component. }
procedure MinVector(var v: TVector; const v1: TVector); overload;
procedure MinVector(var v: TAffineVector; const v1: TAffineVector); overload;

{ Sorts given array in ascending order.
  NOTE : current implementation is a slow bubble sort... }
procedure SortArrayAscending(var a: array of Extended);

{ Clamps aValue in the aMin-aMax interval. }
function ClampValue(const AValue, aMin, aMax: TGeoFloat): TGeoFloat; overload;
{ Clamps aValue in the aMin-INF interval. }
function ClampValue(const AValue, aMin: TGeoFloat): TGeoFloat; overload;

{ Turn a triplet of rotations about x, y, and z (in that order) into an equivalent rotation around a TGeoFloat axis (all in radians). }
function ConvertRotation(const Angles: TAffineVector): TVector;

// miscellaneous functions
function PointInPolygon(var xp, yp: array of TGeoFloat; x, y: TGeoFloat): Boolean;

procedure DivMod(Dividend: Integer; Divisor: Word; var Result, Remainder: Word);

// coordinate system manipulation functions

// Rotates the given coordinate system (represented by the matrix) around its Y-axis
function Turn(const Matrix: TMatrix; angle: TGeoFloat): TMatrix; overload;
// Rotates the given coordinate system (represented by the matrix) around MasterUp
function Turn(const Matrix: TMatrix; const MasterUp: TAffineVector; angle: TGeoFloat): TMatrix; overload;
// Rotates the given coordinate system (represented by the matrix) around its X-axis
function Pitch(const Matrix: TMatrix; angle: TGeoFloat): TMatrix; overload;
// Rotates the given coordinate system (represented by the matrix) around MasterRight
function Pitch(const Matrix: TMatrix; const MasterRight: TAffineVector; angle: TGeoFloat): TMatrix; overload;
// Rotates the given coordinate system (represented by the matrix) around its Z-axis
function Roll(const Matrix: TMatrix; angle: TGeoFloat): TMatrix; overload;
// Rotates the given coordinate system (represented by the matrix) around MasterDirection
function Roll(const Matrix: TMatrix; const MasterDirection: TAffineVector; angle: TGeoFloat): TMatrix; overload;

// intersection functions

{ Compute the intersection point "res" of a line with a plane.
  Return value:<ul>
  <li>0 : no intersection, line Parallel to plane
  <li>1 : res is valid
  <li>-1 : line is inside plane
  </ul><br>
  Adapted from:<br>
  E.Hartmann, Computeruntersttzte Darstellende Geometrie, B.G. Teubner Stuttgart 1988 }
function IntersectLinePlane(const Point, direction: TVector; const plane: THmgPlane; intersectPoint: PVector = nil): Integer; overload;

{ Compute intersection between a triangle and a box.
  Returns True if an intersection was found. }
function IntersectTriangleBox(const p1, p2, p3, aMinExtent, aMaxExtent: TAffineVector): Boolean;

{ Compute intersection between a Sphere and a box.
  Up, Direction and Right must be normalized!
  Use CubDepht, CubeHeight and CubeWidth to scale TGLCube. }
function IntersectSphereBox(
  const SpherePos: TVector; const SphereRadius: TGeoFloat; const BoxMatrix: TMatrix;
  const BoxScale: TAffineVector; intersectPoint: PAffineVector = nil; normal: PAffineVector = nil; Depth: PSingle = nil): Boolean;

{ Compute intersection between a ray and a plane.
  Returns True if an intersection was found, the intersection point is placed
  in intersectPoint is the reference is not nil. }
function RayCastPlaneIntersect(const rayStart, rayVector: TVector; const planePoint, planeNormal: TVector; intersectPoint: PVector = nil): Boolean; overload;
function RayCastPlaneXZIntersect(const rayStart, rayVector: TVector; const planeY: TGeoFloat; intersectPoint: PVector = nil): Boolean; overload;

{ Compute intersection between a ray and a triangle. }
function RayCastTriangleIntersect(const rayStart, rayVector: TVector; const p1, p2, p3: TAffineVector; intersectPoint: PVector = nil; intersectNormal: PVector = nil): Boolean; overload;
{ Compute the min distance a ray will pass to a point. }
function RayCastMinDistToPoint(const rayStart, rayVector: TVector; const Point: TVector): TGeoFloat;
{ Determines if a ray will intersect with a given sphere. }
function RayCastIntersectsSphere(const rayStart, rayVector: TVector; const sphereCenter: TVector; const SphereRadius: TGeoFloat): Boolean; overload;
{ Calculates the intersections between a sphere and a ray.
  Returns 0 if no intersection is found (i1 and i2 untouched), 1 if one
  intersection was found (i1 defined, i2 untouched), and 2 is two intersections
  were found (i1 and i2 defined). }
function RayCastSphereIntersect(const rayStart, rayVector: TVector; const sphereCenter: TVector; const SphereRadius: TGeoFloat; var i1, i2: TVector): Integer; overload;
{ Compute intersection between a ray and a box.
  Returns True if an intersection was found, the intersection point is
  placed in intersectPoint if the reference is not nil. }
function RayCastBoxIntersect(const rayStart, rayVector, aMinExtent, aMaxExtent: TAffineVector; intersectPoint: PAffineVector = nil): Boolean;

// Some 2d intersection functions.

{ Determine if 2 rectanges intersect. }
function RectanglesIntersect(const ACenterOfRect1, ACenterOfRect2, ASizeOfRect1, ASizeOfRect2: TVector2f): Boolean;

{ Determine if BigRect completely contains SmallRect. }
function RectangleContains(const ACenterOfBigRect1, ACenterOfSmallRect2, ASizeOfBigRect1, ASizeOfSmallRect2: TVector2f; const AEps: TGeoFloat = 0.0): Boolean;

{ Computes the visible radius of a sphere in a perspective projection.
  This radius can be used for occlusion culling (cone extrusion) or 2D
  intersection testing. }
function SphereVisibleRadius(Distance, radius: TGeoFloat): TGeoFloat;

{ Extracts a TFrustum for combined modelview and projection matrices. }
function ExtractFrustumFromModelViewProjection(const modelViewProj: TMatrix): TFrustum;

// Determines if volume is clipped or not
function IsVolumeClipped(const objPos: TAffineVector; const objRadius: TGeoFloat; const Frustum: TFrustum): Boolean; overload;
function IsVolumeClipped(const objPos: TVector; const objRadius: TGeoFloat; const Frustum: TFrustum): Boolean; overload;
function IsVolumeClipped(const Min, Max: TAffineVector; const Frustum: TFrustum): Boolean; overload;

// misc funcs

{
  Creates a Parallel projection matrix.
  Transformed points will projected on the plane along the specified direction. }
function MakeParallelProjectionMatrix(const plane: THmgPlane; const dir: TVector): TMatrix;

{
  Creates a shadow projection matrix.
  Shadows will be projected onto the plane defined by planePoint and planeNormal,
  from lightPos. }
function MakeShadowMatrix(const planePoint, planeNormal, lightPos: TVector): TMatrix;

{ Builds a reflection matrix for the given plane.
  Reflection matrix allow implementing planar reflectors in OpenGL (mirrors). }
function MakeReflectionMatrix(const planePoint, planeNormal: TAffineVector): TMatrix;

{
  Packs an homogeneous rotation matrix to 6 bytes.
  The 6:64 (or 6:36) compression ratio is achieved by computing the quaternion
  associated to the matrix and storing its Imaginary components at 16 bits
  precision each.<br>
  Deviation is typically below 0.01% and around 0.1% in worst case situations.
  Note: quaternion conversion is faster and more robust than an angle decomposition. }
function PackRotationMatrix(const mat: TMatrix): TPackedRotationMatrix;
{
  Restores a packed rotation matrix.
  See PackRotationMatrix. }
function UnPackRotationMatrix(const packedMatrix: TPackedRotationMatrix): TMatrix;

{
  Calculates the barycentric coordinates for the point p on the triangle
  defined by the vertices v1, v2 and v3. That is, solves
  p = u * v1 + v * v2 + (1-u-v) * v3
  for u,v.
  Returns true if the point is inside the triangle, false otherwise.
  NOTE: This function assumes that the point lies on the plane defined by the triangle.
  If this is not the case, the function will not work correctly! }
function BarycentricCoordinates(const v1, v2, v3, p: TAffineVector; var u, v: TGeoFloat): Boolean;

{ Extracted from Camera.MoveAroundTarget(pitch, turn). }
function MoveObjectAround(const AMovingObjectPosition, AMovingObjectUp, ATargetPosition: TVector; pitchDelta, turnDelta: TGeoFloat): TVector;

{ Calcualtes Angle between 2 Vectors: (A-CenterPoint) and (B-CenterPoint). In radians. }
function AngleBetweenVectors(const a, b, ACenterPoint: TVector): TGeoFloat; overload;
function AngleBetweenVectors(const a, b, ACenterPoint: TAffineVector): TGeoFloat; overload;

{
  AOriginalPosition - Object initial position.
  ACenter - some point, from which is should be distanced.

  ADistance + AFromCenterSpot - distance, which object should keep from ACenter
  or
  ADistance + not AFromCenterSpot - distance, which object should shift from his current position away from center.
}
function ShiftObjectFromCenter(const AOriginalPosition: TVector; const ACenter: TVector; const aDistance: TGeoFloat; const AFromCenterSpot: Boolean): TVector; overload;
function ShiftObjectFromCenter(const AOriginalPosition: TAffineVector; const ACenter: TAffineVector; const aDistance: TGeoFloat; const AFromCenterSpot: Boolean): TAffineVector; overload;

const
  cPI: TGeoFloat = 3.141592654;
  cPIdiv180: TGeoFloat = 0.017453292;
  c180divPI: TGeoFloat = 57.29577951;
  c2PI: TGeoFloat = 6.283185307;
  cPIdiv2: TGeoFloat = 1.570796326;
  cPIdiv4: TGeoFloat = 0.785398163;
  c3PIdiv2: TGeoFloat = 4.71238898;
  c3PIdiv4: TGeoFloat = 2.35619449;
  cInv2PI: TGeoFloat = 1 / 6.283185307;
  cInv360: TGeoFloat = 1 / 360;
  c180: TGeoFloat = 180;
  c360: TGeoFloat = 360;
  cOneHalf: TGeoFloat = 0.5;
  cLn10: TGeoFloat = 2.302585093;

  // Ranges of the IEEE floating point types, including denormals
  // with Math.pas compatible name
  MinSingle = 1.5E-45;
  MaxSingle = 3.4E+38;
  MinDouble = 5.0E-324;
  MaxDouble = 1.7E+308;
  MinComp = -9.223372036854775807E+18;
  MaxComp = 9.223372036854775807E+18;

  // --------------------------------------------------------------
  // --------------------------------------------------------------
  // --------------------------------------------------------------
implementation

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

uses Math;

const
  // to be used as descriptive indices
  x = 0;
  y = 1;
  z = 2;
  w = 3;

  cZero: TGeoFloat = 0.0;
  cOne: TGeoFloat = 1.0;
  cOneDotFive: TGeoFloat = 0.5;

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

  // TexPointMake
  //
function TexPointMake(const s, t: TGeoFloat): TTexPoint;
begin
  Result.s := s;
  Result.t := t;
end;

// AffineVectorMake
//
function AffineVectorMake(const x, y, z: TGeoFloat): TAffineVector; overload;
begin
  Result[0] := x;
  Result[1] := y;
  Result[2] := z;
end;

// AffineVectorMake
//
function AffineVectorMake(const v: TVector): TAffineVector;
begin
  Result[0] := v[0];
  Result[1] := v[1];
  Result[2] := v[2];
end;

// SetAffineVector
//
procedure SetAffineVector(out v: TAffineVector; const x, y, z: TGeoFloat); overload;
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
end;

// SetVector (affine)
//
procedure SetVector(out v: TAffineVector; const x, y, z: TGeoFloat);
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
end;

// SetVector (affine-hmg)
//
procedure SetVector(out v: TAffineVector; const vSrc: TVector);
begin
  v[0] := vSrc[0];
  v[1] := vSrc[1];
  v[2] := vSrc[2];
end;

// SetVector (affine-affine)
//
procedure SetVector(out v: TAffineVector; const vSrc: TAffineVector);
begin
  v[0] := vSrc[0];
  v[1] := vSrc[1];
  v[2] := vSrc[2];
end;

// VectorMake
//
function VectorMake(const v: TAffineVector; w: TGeoFloat = 0): TVector;
begin
  Result[0] := v[0];
  Result[1] := v[1];
  Result[2] := v[2];
  Result[3] := w;
end;

// VectorMake
//
function VectorMake(const x, y, z: TGeoFloat; w: TGeoFloat = 0): TVector;
begin
  Result[0] := x;
  Result[1] := y;
  Result[2] := z;
  Result[3] := w;
end;

// PointMake (xyz)
//
function PointMake(const x, y, z: TGeoFloat): TVector; overload;
begin
  Result[0] := x;
  Result[1] := y;
  Result[2] := z;
  Result[3] := 1;
end;

// PointMake (affine)
//
function PointMake(const v: TAffineVector): TVector; overload;
begin
  Result[0] := v[0];
  Result[1] := v[1];
  Result[2] := v[2];
  Result[3] := 1;
end;

// PointMake (hmg)
//
function PointMake(const v: TVector): TVector; overload;
begin
  Result[0] := v[0];
  Result[1] := v[1];
  Result[2] := v[2];
  Result[3] := 1;
end;

// SetVector
//
procedure SetVector(out v: TVector; const x, y, z: TGeoFloat; w: TGeoFloat = 0);
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
  v[3] := w;
end;

// SetVector
//
procedure SetVector(out v: TVector; const av: TAffineVector; w: TGeoFloat = 0);
begin
  v[0] := av[0];
  v[1] := av[1];
  v[2] := av[2];
  v[3] := w;
end;

// SetVector
//
procedure SetVector(out v: TVector; const vSrc: TVector);
begin
  // faster than memcpy, move or ':=' on the TVector...
  v[0] := vSrc[0];
  v[1] := vSrc[1];
  v[2] := vSrc[2];
  v[3] := vSrc[3];
end;

// MakePoint
//
procedure MakePoint(out v: TVector; const x, y, z: TGeoFloat);
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
  v[3] := 1;
end;

// MakePoint
//
procedure MakePoint(out v: TVector; const av: TAffineVector);
begin
  v[0] := av[0];
  v[1] := av[1];
  v[2] := av[2];
  v[3] := 1;
end;

// MakePoint
//
procedure MakePoint(out v: TVector; const av: TVector);
begin
  v[0] := av[0];
  v[1] := av[1];
  v[2] := av[2];
  v[3] := 1;
end;

// MakeVector
//
procedure MakeVector(out v: TAffineVector; const x, y, z: TGeoFloat); overload;
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
end;

// MakeVector
//
procedure MakeVector(out v: TVector; const x, y, z: TGeoFloat);
begin
  v[0] := x;
  v[1] := y;
  v[2] := z;
  v[3] := 0;
end;

// MakeVector
//
procedure MakeVector(out v: TVector; const av: TAffineVector);
begin
  v[0] := av[0];
  v[1] := av[1];
  v[2] := av[2];
  v[3] := 0;
end;

// MakeVector
//
procedure MakeVector(out v: TVector; const av: TVector);
begin
  v[0] := av[0];
  v[1] := av[1];
  v[2] := av[2];
  v[3] := 0;
end;

// RstVector (affine)
//
procedure RstVector(var v: TAffineVector);
begin
  v[0] := 0;
  v[1] := 0;
  v[2] := 0;
end;

// RstVector (hmg)
//
procedure RstVector(var v: TVector);
begin
  v[0] := 0;
  v[1] := 0;
  v[2] := 0;
  v[3] := 0;
end;

// VectorAdd (func)
//
function VectorAdd(const v1, v2: TVector2f): TVector2f;
begin
  Result[0] := v1[0] + v2[0];
  Result[1] := v1[1] + v2[1];
end;

// VectorAdd (func, affine)
//
function VectorAdd(const v1, v2: TAffineVector): TAffineVector;
begin
  Result[0] := v1[0] + v2[0];
  Result[1] := v1[1] + v2[1];
  Result[2] := v1[2] + v2[2];
end;

// VectorAdd (proc, affine)
//
procedure VectorAdd(const v1, v2: TAffineVector; var vr: TAffineVector); overload;
begin
  vr[0] := v1[0] + v2[0];
  vr[1] := v1[1] + v2[1];
  vr[2] := v1[2] + v2[2];
end;

// VectorAdd (proc, affine)
//
procedure VectorAdd(const v1, v2: TAffineVector; vr: PAffineVector); overload;
begin
  vr^[0] := v1[0] + v2[0];
  vr^[1] := v1[1] + v2[1];
  vr^[2] := v1[2] + v2[2];
end;

// VectorAdd (hmg)
//
function VectorAdd(const v1, v2: TVector): TVector;
begin
  Result[0] := v1[0] + v2[0];
  Result[1] := v1[1] + v2[1];
  Result[2] := v1[2] + v2[2];
  Result[3] := v1[3] + v2[3];
end;

// VectorAdd (hmg, proc)
//
procedure VectorAdd(const v1, v2: TVector; var vr: TVector);
begin
  vr[0] := v1[0] + v2[0];
  vr[1] := v1[1] + v2[1];
  vr[2] := v1[2] + v2[2];
  vr[3] := v1[3] + v2[3];
end;

// VectorAdd (affine, TGeoFloat)
//
function VectorAdd(const v: TAffineVector; const f: TGeoFloat): TAffineVector;
begin
  Result[0] := v[0] + f;
  Result[1] := v[1] + f;
  Result[2] := v[2] + f;
end;

// VectorAdd (hmg, TGeoFloat)
//
function VectorAdd(const v: TVector; const f: TGeoFloat): TVector;
begin
  Result[0] := v[0] + f;
  Result[1] := v[1] + f;
  Result[2] := v[2] + f;
  Result[3] := v[3] + f;
end;

// PointAdd (hmg, W = 1)
//
function PointAdd(var v1: TVector; const v2: TVector): TVector;
begin
  Result[0] := v1[0] + v2[0];
  Result[1] := v1[1] + v2[1];
  Result[2] := v1[2] + v2[2];
  Result[3] := 1;
end;

// AddVector (affine)
//
procedure AddVector(var v1: TAffineVector; const v2: TAffineVector);
begin
  v1[0] := v1[0] + v2[0];
  v1[1] := v1[1] + v2[1];
  v1[2] := v1[2] + v2[2];
end;

// AddVector (affine)
//
procedure AddVector(var v1: TAffineVector; const v2: TVector);
begin
  v1[0] := v1[0] + v2[0];
  v1[1] := v1[1] + v2[1];
  v1[2] := v1[2] + v2[2];
end;

// AddVector (hmg)
//
procedure AddVector(var v1: TVector; const v2: TVector);
begin
  v1[0] := v1[0] + v2[0];
  v1[1] := v1[1] + v2[1];
  v1[2] := v1[2] + v2[2];
  v1[3] := v1[3] + v2[3];
end;

// AddVector (affine)
//
procedure AddVector(var v: TAffineVector; const f: TGeoFloat);
begin
  v[0] := v[0] + f;
  v[1] := v[1] + f;
  v[2] := v[2] + f;
end;

// AddVector (hmg)
//
procedure AddVector(var v: TVector; const f: TGeoFloat);
begin
  v[0] := v[0] + f;
  v[1] := v[1] + f;
  v[2] := v[2] + f;
  v[3] := v[3] + f;
end;

// AddPoint (hmg, W = 1)
//
procedure AddPoint(var v1: TVector; const v2: TVector);
begin
  v1[0] := v1[0] + v2[0];
  v1[1] := v1[1] + v2[1];
  v1[2] := v1[2] + v2[2];
  v1[3] := 1;
end;

// TexPointArrayAdd
//
procedure TexPointArrayAdd(const Src: PTexPointArray; const Delta: TTexPoint;
  const nb: Integer;
  dest: PTexPointArray); overload;
var
  i: Integer;
begin
  for i := 0 to nb - 1 do begin
      dest^[i].s := Src^[i].s + Delta.s;
      dest^[i].t := Src^[i].t + Delta.t;
    end;
end;

// TexPointArrayScaleAndAdd
//
procedure TexPointArrayScaleAndAdd(const Src: PTexPointArray; const Delta: TTexPoint;
  const nb: Integer; const Scale: TTexPoint;
  dest: PTexPointArray); overload;
var
  i: Integer;
begin
  for i := 0 to nb - 1 do begin
      dest^[i].s := Src^[i].s * Scale.s + Delta.s;
      dest^[i].t := Src^[i].t * Scale.t + Delta.t;
    end;
end;

// VectorArrayAdd
//
procedure VectorArrayAdd(const Src: PAffineVectorArray; const Delta: TAffineVector;
  const nb: Integer; dest: PAffineVectorArray);
var
  i: Integer;
begin
  for i := 0 to nb - 1 do begin
      dest^[i][0] := Src^[i][0] + Delta[0];
      dest^[i][1] := Src^[i][1] + Delta[1];
      dest^[i][2] := Src^[i][2] + Delta[2];
    end;
end;

// VectorSubtract (func, affine)
//
function VectorSubtract(const v1, v2: TAffineVector): TAffineVector;
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
end;

// VectorSubtract (func, 2f)
//
function VectorSubtract(const v1, v2: TVector2f): TVector2f;
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
end;

// VectorSubtract (proc, affine)
//
procedure VectorSubtract(const v1, v2: TAffineVector; var Result: TAffineVector);
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
end;

// VectorSubtract (proc, affine-hmg)
//
procedure VectorSubtract(const v1, v2: TAffineVector; var Result: TVector);
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
  Result[3] := 0;
end;

// VectorSubtract
//
procedure VectorSubtract(const v1: TVector; v2: TAffineVector; var Result: TVector);
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
  Result[3] := v1[0];
end;

// VectorSubtract (hmg)
//
function VectorSubtract(const v1, v2: TVector): TVector;
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
  Result[3] := v1[3] - v2[3];
end;

// VectorSubtract (proc, hmg)
//
procedure VectorSubtract(const v1, v2: TVector; var Result: TVector);
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
  Result[3] := v1[3] - v2[3];
end;

// VectorSubtract (proc, affine)
//
procedure VectorSubtract(const v1, v2: TVector; var Result: TAffineVector); overload;
begin
  Result[0] := v1[0] - v2[0];
  Result[1] := v1[1] - v2[1];
  Result[2] := v1[2] - v2[2];
end;

// VectorSubtract (affine, TGeoFloat)
//
function VectorSubtract(const v1: TAffineVector; Delta: TGeoFloat): TAffineVector;
begin
  Result[0] := v1[0] - Delta;
  Result[1] := v1[1] - Delta;
  Result[2] := v1[2] - Delta;
end;

// VectorSubtract (hmg, TGeoFloat)
//
function VectorSubtract(const v1: TVector; Delta: TGeoFloat): TVector;
begin
  Result[0] := v1[0] - Delta;
  Result[1] := v1[1] - Delta;
  Result[2] := v1[2] - Delta;
  Result[3] := v1[3] - Delta;
end;

// SubtractVector (affine)
//
procedure SubtractVector(var v1: TAffineVector; const v2: TAffineVector);
begin
  v1[0] := v1[0] - v2[0];
  v1[1] := v1[1] - v2[1];
  v1[2] := v1[2] - v2[2];
end;

// SubtractVector (2f)
//
procedure SubtractVector(var v1: TVector2f; const v2: TVector2f);
begin
  v1[0] := v1[0] - v2[0];
  v1[1] := v1[1] - v2[1];
end;

// SubtractVector (hmg)
//
procedure SubtractVector(var v1: TVector; const v2: TVector);
begin
  v1[0] := v1[0] - v2[0];
  v1[1] := v1[1] - v2[1];
  v1[2] := v1[2] - v2[2];
  v1[3] := v1[3] - v2[3];
end;

// CombineVector (var)
//
procedure CombineVector(var vr: TAffineVector; const v: TAffineVector; var f: TGeoFloat);
begin
  vr[0] := vr[0] + v[0] * f;
  vr[1] := vr[1] + v[1] * f;
  vr[2] := vr[2] + v[2] * f;
end;

// CombineVector (pointer)
//
procedure CombineVector(var vr: TAffineVector; const v: TAffineVector; pf: PFloat);
begin
  vr[0] := vr[0] + v[0] * pf^;
  vr[1] := vr[1] + v[1] * pf^;
  vr[2] := vr[2] + v[2] * pf^;
end;

// TexPointCombine
//
function TexPointCombine(const t1, t2: TTexPoint; f1, f2: TGeoFloat): TTexPoint;
begin
  Result.s := (f1 * t1.s) + (f2 * t2.s);
  Result.t := (f1 * t1.t) + (f2 * t2.t);
end;

// VectorCombine
//
function VectorCombine(const v1, v2: TAffineVector; const f1, f2: TGeoFloat): TAffineVector;
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;

// VectorCombine3 (func)
//
function VectorCombine3(const v1, v2, v3: TAffineVector; const f1, f2, F3: TGeoFloat): TAffineVector;
begin
  Result[x] := (f1 * v1[x]) + (f2 * v2[x]) + (F3 * v3[x]);
  Result[y] := (f1 * v1[y]) + (f2 * v2[y]) + (F3 * v3[y]);
  Result[z] := (f1 * v1[z]) + (f2 * v2[z]) + (F3 * v3[z]);
end;

// VectorCombine3 (vector)
//
procedure VectorCombine3(const v1, v2, v3: TAffineVector; const f1, f2, F3: TGeoFloat; var vr: TAffineVector);
begin
  vr[x] := (f1 * v1[x]) + (f2 * v2[x]) + (F3 * v3[x]);
  vr[y] := (f1 * v1[y]) + (f2 * v2[y]) + (F3 * v3[y]);
  vr[z] := (f1 * v1[z]) + (f2 * v2[z]) + (F3 * v3[z]);
end;

// CombineVector
//
procedure CombineVector(var vr: TVector; const v: TVector; var f: TGeoFloat); overload;
begin
  vr[0] := vr[0] + v[0] * f;
  vr[1] := vr[1] + v[1] * f;
  vr[2] := vr[2] + v[2] * f;
  vr[3] := vr[3] + v[3] * f;
end;

// CombineVector
//
procedure CombineVector(var vr: TVector; const v: TAffineVector; var f: TGeoFloat); overload;
begin
  vr[0] := vr[0] + v[0] * f;
  vr[1] := vr[1] + v[1] * f;
  vr[2] := vr[2] + v[2] * f;
end;

// VectorCombine
//
function VectorCombine(const v1, v2: TVector; const f1, f2: TGeoFloat): TVector;
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;

// VectorCombine
//
function VectorCombine(const v1: TVector; const v2: TAffineVector; const f1, f2: TGeoFloat): TVector; overload;
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];
end;

// VectorCombine
//
procedure VectorCombine(const v1, v2: TVector; const f1, f2: TGeoFloat; var vr: TVector); overload;
begin
  vr[0] := (f1 * v1[0]) + (f2 * v2[0]);
  vr[1] := (f1 * v1[1]) + (f2 * v2[1]);
  vr[2] := (f1 * v1[2]) + (f2 * v2[2]);
  vr[3] := (f1 * v1[3]) + (f2 * v2[3]);
end;

// VectorCombine (F1=1.0)
//
procedure VectorCombine(const v1, v2: TVector; const f2: TGeoFloat; var vr: TVector); overload;
begin
  vr[0] := v1[0] + (f2 * v2[0]);
  vr[1] := v1[1] + (f2 * v2[1]);
  vr[2] := v1[2] + (f2 * v2[2]);
  vr[3] := v1[3] + (f2 * v2[3]);
end;

// VectorCombine
//
procedure VectorCombine(const v1: TVector; const v2: TAffineVector; const f1, f2: TGeoFloat; var vr: TVector);
begin
  vr[x] := (f1 * v1[x]) + (f2 * v2[x]);
  vr[y] := (f1 * v1[y]) + (f2 * v2[y]);
  vr[z] := (f1 * v1[z]) + (f2 * v2[z]);
  vr[w] := f1 * v1[w];
end;

// VectorCombine3
//
function VectorCombine3(const v1, v2, v3: TVector; const f1, f2, F3: TGeoFloat): TVector;
begin
  Result[x] := (f1 * v1[x]) + (f2 * v2[x]) + (F3 * v3[x]);
  Result[y] := (f1 * v1[y]) + (f2 * v2[y]) + (F3 * v3[y]);
  Result[z] := (f1 * v1[z]) + (f2 * v2[z]) + (F3 * v3[z]);
  Result[w] := (f1 * v1[w]) + (f2 * v2[w]) + (F3 * v3[w]);
end;

// VectorCombine3
//
procedure VectorCombine3(const v1, v2, v3: TVector; const f1, f2, F3: TGeoFloat; var vr: TVector);
begin
  vr[x] := (f1 * v1[x]) + (f2 * v2[x]) + (F3 * v3[x]);
  vr[y] := (f1 * v1[y]) + (f2 * v2[y]) + (F3 * v3[y]);
  vr[z] := (f1 * v1[z]) + (f2 * v2[z]) + (F3 * v3[z]);
  vr[w] := (f1 * v1[w]) + (f2 * v2[w]) + (F3 * v3[w]);
end;

// VectorDotProduct (2f)
//
function VectorDotProduct(const v1, v2: TVector2f): TGeoFloat;
begin
  Result := v1[0] * v2[0] + v1[1] * v2[1];
end;

// VectorDotProduct (affine)
//
function VectorDotProduct(const v1, v2: TAffineVector): TGeoFloat;
begin
  Result := v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
end;

// VectorDotProduct (hmg)
//
function VectorDotProduct(const v1, v2: TVector): TGeoFloat;
begin
  Result := v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] + v1[3] * v2[3];
end;

// VectorDotProduct
//
function VectorDotProduct(const v1: TVector; const v2: TAffineVector): TGeoFloat;
begin
  Result := v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
end;

// PointProject (affine)
//
function PointProject(const p, origin, direction: TAffineVector): TGeoFloat;
begin
  Result := direction[0] * (p[0] - origin[0])
    + direction[1] * (p[1] - origin[1])
    + direction[2] * (p[2] - origin[2]);
end;

// PointProject (vector)
//
function PointProject(const p, origin, direction: TVector): TGeoFloat;
begin
  Result := direction[0] * (p[0] - origin[0])
    + direction[1] * (p[1] - origin[1])
    + direction[2] * (p[2] - origin[2]);
end;

// VectorCrossProduct
//
function VectorCrossProduct(const v1, v2: TAffineVector): TAffineVector;
begin
  Result[x] := v1[y] * v2[z] - v1[z] * v2[y];
  Result[y] := v1[z] * v2[x] - v1[x] * v2[z];
  Result[z] := v1[x] * v2[y] - v1[y] * v2[x];
end;

// VectorCrossProduct
//
function VectorCrossProduct(const v1, v2: TVector): TVector;
begin
  Result[x] := v1[y] * v2[z] - v1[z] * v2[y];
  Result[y] := v1[z] * v2[x] - v1[x] * v2[z];
  Result[z] := v1[x] * v2[y] - v1[y] * v2[x];
  Result[w] := 0;
end;

// VectorCrossProduct
//
procedure VectorCrossProduct(const v1, v2: TVector; var vr: TVector);
begin
  vr[x] := v1[y] * v2[z] - v1[z] * v2[y];
  vr[y] := v1[z] * v2[x] - v1[x] * v2[z];
  vr[z] := v1[x] * v2[y] - v1[y] * v2[x];
  vr[w] := 0;
end;

// VectorCrossProduct
//
procedure VectorCrossProduct(const v1, v2: TAffineVector; var vr: TVector); overload;
begin
  vr[x] := v1[y] * v2[z] - v1[z] * v2[y];
  vr[y] := v1[z] * v2[x] - v1[x] * v2[z];
  vr[z] := v1[x] * v2[y] - v1[y] * v2[x];
  vr[w] := 0;
end;

// VectorCrossProduct
//
procedure VectorCrossProduct(const v1, v2: TVector; var vr: TAffineVector); overload;
begin
  vr[x] := v1[y] * v2[z] - v1[z] * v2[y];
  vr[y] := v1[z] * v2[x] - v1[x] * v2[z];
  vr[z] := v1[x] * v2[y] - v1[y] * v2[x];
end;

// VectorCrossProduct
//
procedure VectorCrossProduct(const v1, v2: TAffineVector; var vr: TAffineVector); overload;
begin
  vr[x] := v1[y] * v2[z] - v1[z] * v2[y];
  vr[y] := v1[z] * v2[x] - v1[x] * v2[z];
  vr[z] := v1[x] * v2[y] - v1[y] * v2[x];
end;

// Lerp
//
function Lerp(const Start, stop, t: TGeoFloat): TGeoFloat;
begin
  Result := Start + (stop - Start) * t;
end;

// Angle Lerp
//
function AngleLerp(Start, stop, t: TGeoFloat): TGeoFloat;
var
  d: TGeoFloat;
begin
  Start := NormalizeAngle(Start);
  stop := NormalizeAngle(stop);
  d := stop - Start;
  if d > pi then begin
      // positive d, angle on opposite side, becomes negative i.e. changes direction
      d := -d - c2PI;
    end
  else if d < -pi then begin
      // negative d, angle on opposite side, becomes positive i.e. changes direction
      d := d + c2PI;
    end;
  Result := Start + d * t;
end;

// DistanceBetweenAngles
//
function DistanceBetweenAngles(angle1, angle2: TGeoFloat): TGeoFloat;
begin
  angle1 := NormalizeAngle(angle1);
  angle2 := NormalizeAngle(angle2);
  Result := Abs(angle2 - angle1);
  if Result > pi then
      Result := c2PI - Result;
end;

// TexPointLerp
//
function TexPointLerp(const t1, t2: TTexPoint; t: TGeoFloat): TTexPoint; overload;
begin
  Result.s := t1.s + (t2.s - t1.s) * t;
  Result.t := t1.t + (t2.t - t1.t) * t;
end;

// VectorAffineLerp
//
function VectorLerp(const v1, v2: TAffineVector; t: TGeoFloat): TAffineVector;
begin
  Result[x] := v1[x] + (v2[x] - v1[x]) * t;
  Result[y] := v1[y] + (v2[y] - v1[y]) * t;
  Result[z] := v1[z] + (v2[z] - v1[z]) * t;
end;

// VectorLerp
//
procedure VectorLerp(const v1, v2: TAffineVector; t: TGeoFloat; var vr: TAffineVector);
begin
  vr[x] := v1[x] + (v2[x] - v1[x]) * t;
  vr[y] := v1[y] + (v2[y] - v1[y]) * t;
  vr[z] := v1[z] + (v2[z] - v1[z]) * t;
end;

// VectorLerp
//
function VectorLerp(const v1, v2: TVector; t: TGeoFloat): TVector;
begin
  Result[x] := v1[x] + (v2[x] - v1[x]) * t;
  Result[y] := v1[y] + (v2[y] - v1[y]) * t;
  Result[z] := v1[z] + (v2[z] - v1[z]) * t;
  Result[w] := v1[w] + (v2[w] - v1[w]) * t;
end;

// VectorLerp
//
procedure VectorLerp(const v1, v2: TVector; t: TGeoFloat; var vr: TVector);
begin
  vr[x] := v1[x] + (v2[x] - v1[x]) * t;
  vr[y] := v1[y] + (v2[y] - v1[y]) * t;
  vr[z] := v1[z] + (v2[z] - v1[z]) * t;
  vr[w] := v1[w] + (v2[w] - v1[w]) * t;
end;

// VectorAngleLerp
//
function VectorAngleLerp(const v1, v2: TAffineVector; t: TGeoFloat): TAffineVector;
var
  q1, q2, qR: TQuaternion;
  M: TMatrix;
  Tran: TTransformations;
begin
  if VectorEquals(v1, v2) then begin
      Result := v1;
    end
  else begin
      q1 := QuaternionFromEuler(GeometryLib.RadToDeg(v1[0]), GeometryLib.RadToDeg(v1[1]), GeometryLib.RadToDeg(v1[2]), eulZYX);
      q2 := QuaternionFromEuler(GeometryLib.RadToDeg(v2[0]), GeometryLib.RadToDeg(v2[1]), GeometryLib.RadToDeg(v2[2]), eulZYX);
      qR := QuaternionSlerp(q1, q2, t);
      M := QuaternionToMatrix(qR);
      MatrixDecompose(M, Tran);
      Result[0] := Tran[ttRotateX];
      Result[1] := Tran[ttRotateY];
      Result[2] := Tran[ttRotateZ];
    end;
end;

// VectorAngleCombine
//
function VectorAngleCombine(const v1, v2: TAffineVector; f: TGeoFloat): TAffineVector;
begin
  Result := VectorCombine(v1, v2, 1, f);
end;

// VectorArrayLerp (hmg)
//
procedure VectorArrayLerp(const src1, src2: PVectorArray; t: TGeoFloat; n: Integer; dest: PVectorArray);
var
  i: Integer;
begin
  for i := 0 to n - 1 do begin
      dest^[i][0] := src1^[i][0] + (src2^[i][0] - src1^[i][0]) * t;
      dest^[i][1] := src1^[i][1] + (src2^[i][1] - src1^[i][1]) * t;
      dest^[i][2] := src1^[i][2] + (src2^[i][2] - src1^[i][2]) * t;
      dest^[i][3] := src1^[i][3] + (src2^[i][3] - src1^[i][3]) * t;
    end;
end;

// VectorArrayLerp (affine)
//
procedure VectorArrayLerp(const src1, src2: PAffineVectorArray; t: TGeoFloat; n: Integer; dest: PAffineVectorArray);
var
  i: Integer;
begin
  for i := 0 to n - 1 do begin
      dest^[i][0] := src1^[i][0] + (src2^[i][0] - src1^[i][0]) * t;
      dest^[i][1] := src1^[i][1] + (src2^[i][1] - src1^[i][1]) * t;
      dest^[i][2] := src1^[i][2] + (src2^[i][2] - src1^[i][2]) * t;
    end;
end;

procedure VectorArrayLerp(const src1, src2: PTexPointArray; t: TGeoFloat; n: Integer; dest: PTexPointArray);
var
  i: Integer;
begin
  for i := 0 to n - 1 do begin
      dest^[i].s := src1^[i].s + (src2^[i].s - src1^[i].s) * t;
      dest^[i].t := src1^[i].t + (src2^[i].t - src1^[i].t) * t;
    end;
end;

// InterpolateCombined
//
function InterpolateCombined(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;
begin
  case InterpolationType of
    itLinear: Result := Lerp(Start, stop, Delta);
    itPower: Result := InterpolatePower(Start, stop, Delta, DistortionDegree);
    itSin: Result := InterpolateSin(Start, stop, Delta);
    itSinAlt: Result := InterpolateSinAlt(Start, stop, Delta);
    itTan: Result := InterpolateTan(Start, stop, Delta);
    itLn: Result := InterpolateLn(Start, stop, Delta, DistortionDegree);
    itExp: Result := InterpolateExp(Start, stop, Delta, DistortionDegree);
    else
      begin
        Result := -1;
        Assert(False);
      end;
  end;
end;

// InterpolateCombinedFastPower
//
function InterpolateCombinedFastPower(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
begin
  Result := InterpolatePower(TargetStart, TargetStop, (OriginalCurrent - OriginalStart) / (OriginalStop - OriginalStart), DistortionDegree);
end;

// InterpolateCombinedSafe
//
function InterpolateCombinedSafe(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;
var
  ChangeDelta: TGeoFloat;
begin
  if OriginalStop = OriginalStart then
      Result := TargetStart
  else
    begin
      ChangeDelta := (OriginalCurrent - OriginalStart) / (OriginalStop - OriginalStart);
      Result := InterpolateCombined(TargetStart, TargetStop, ChangeDelta, DistortionDegree, InterpolationType);
    end;
end;

// InterpolateCombinedFast
//
function InterpolateCombinedFast(const OriginalStart, OriginalStop, OriginalCurrent: TGeoFloat; const TargetStart, TargetStop: TGeoFloat; const DistortionDegree: TGeoFloat; const InterpolationType: TGLInterpolationType): TGeoFloat;
var
  ChangeDelta: TGeoFloat;
begin
  ChangeDelta := (OriginalCurrent - OriginalStart) / (OriginalStop - OriginalStart);
  Result := InterpolateCombined(TargetStart, TargetStop, ChangeDelta, DistortionDegree, InterpolationType);
end;

// InterpolateLn
//
function InterpolateLn(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
begin
  Result := (stop - Start) * ln(1 + Delta * DistortionDegree) / ln(1 + DistortionDegree) + Start;
end;

// InterpolateExp
//
function InterpolateExp(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
begin
  Result := (stop - Start) * Exp(-DistortionDegree * (1 - Delta)) + Start;
end;

// InterpolateSinAlt
//
function InterpolateSinAlt(const Start, stop, Delta: TGeoFloat): TGeoFloat;
begin
  Result := (stop - Start) * Delta * Sin(Delta * pi / 2) + Start;
end;

// InterpolateSin
//
function InterpolateSin(const Start, stop, Delta: TGeoFloat): TGeoFloat;
begin
  Result := (stop - Start) * Sin(Delta * pi / 2) + Start;
end;

// InterpolateTan
//
function InterpolateTan(const Start, stop, Delta: TGeoFloat): TGeoFloat;
begin
  Result := (stop - Start) * GeometryLib.Tan(Delta * pi / 4) + Start;
end;

// InterpolatePower
//
function InterpolatePower(const Start, stop, Delta: TGeoFloat; const DistortionDegree: TGeoFloat): TGeoFloat;
begin
  if (Round(DistortionDegree) <> DistortionDegree) and (Delta < 0) then
      Result := (stop - Start) * GeometryLib.Power(Delta, Round(DistortionDegree)) + Start
  else
      Result := (stop - Start) * GeometryLib.Power(Delta, DistortionDegree) + Start;
end;

// MatrixLerp
//
function MatrixLerp(const m1, m2: TMatrix; const Delta: TGeoFloat): TMatrix;
var
  i, j: Integer;
begin
  for j := 0 to 3 do
    for i := 0 to 3 do
        Result[i][j] := m1[i][j] + (m2[i][j] - m1[i][j]) * Delta;
end;

// VectorLength (array)
//
function VectorLength(const v: array of TGeoFloat): TGeoFloat;
var
  i: Integer;
begin
  Result := 0;
  for i := low(v) to high(v) do
      Result := Result + Sqr(v[i]);
  Result := Sqrt(Result);
end;

// VectorLength  (x, y)
//
function VectorLength(const x, y: TGeoFloat): TGeoFloat;
begin
  Result := Sqrt(x * x + y * y);
end;

// VectorLength (x, y, z)
//
function VectorLength(const x, y, z: TGeoFloat): TGeoFloat;
begin
  Result := Sqrt(x * x + y * y + z * z);
end;

// VectorLength
//
function VectorLength(const v: TVector2f): TGeoFloat;
begin
  Result := Sqrt(VectorNorm(v[0], v[1]));
end;

// VectorLength
//
function VectorLength(const v: TAffineVector): TGeoFloat;
begin
  Result := Sqrt(VectorNorm(v));
end;

// VectorLength
//
function VectorLength(const v: TVector): TGeoFloat;
begin
  Result := Sqrt(VectorNorm(v));
end;

// VectorNorm
//
function VectorNorm(const x, y: TGeoFloat): TGeoFloat;
begin
  Result := Sqr(x) + Sqr(y);
end;

// VectorNorm (affine)
//
function VectorNorm(const v: TAffineVector): TGeoFloat;
begin
  Result := v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
end;

// VectorNorm (hmg)
//
function VectorNorm(const v: TVector): TGeoFloat;
begin
  Result := v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
end;

// VectorNorm
//
function VectorNorm(var v: array of TGeoFloat): TGeoFloat;
var
  i: Integer;
begin
  Result := 0;
  for i := low(v) to high(v) do
      Result := Result + v[i] * v[i];
end;

// NormalizeVector (2f)
//
procedure NormalizeVector(var v: TVector2f);
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v);
  if vn > 0 then begin
      InvLen := RSqrt(vn);
      v[0] := v[0] * InvLen;
      v[1] := v[1] * InvLen;
    end;
end;

// NormalizeVector (affine)
//
procedure NormalizeVector(var v: TAffineVector);
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v);
  if vn > 0 then begin
      InvLen := RSqrt(vn);
      v[0] := v[0] * InvLen;
      v[1] := v[1] * InvLen;
      v[2] := v[2] * InvLen;
    end;
end;

// VectorNormalize
//
function VectorNormalize(const v: TVector2f): TVector2f;
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v[0], v[1]);
  if vn = 0 then
      Result := v
  else begin
      InvLen := RSqrt(vn);
      Result[0] := v[0] * InvLen;
      Result[1] := v[1] * InvLen;
    end;
end;

// VectorNormalize
//
function VectorNormalize(const v: TAffineVector): TAffineVector;
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v);
  if vn = 0 then
      SetVector(Result, v)
  else begin
      InvLen := RSqrt(vn);
      Result[0] := v[0] * InvLen;
      Result[1] := v[1] * InvLen;
      Result[2] := v[2] * InvLen;
    end;
end;

// NormalizeVectorArray
//
procedure NormalizeVectorArray(List: PAffineVectorArray; n: Integer);
var
  i: Integer;
begin
  for i := 0 to n - 1 do
      NormalizeVector(List^[i]);
end;

// NormalizeVector (hmg)
//
procedure NormalizeVector(var v: TVector);
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v);
  if vn > 0 then begin
      InvLen := RSqrt(vn);
      v[0] := v[0] * InvLen;
      v[1] := v[1] * InvLen;
      v[2] := v[2] * InvLen;
    end;
  v[3] := 0;
end;

// VectorNormalize (hmg, func)
//
function VectorNormalize(const v: TVector): TVector;
var
  InvLen: TGeoFloat;
  vn: TGeoFloat;
begin
  vn := VectorNorm(v);
  if vn = 0 then
      SetVector(Result, v)
  else begin
      InvLen := RSqrt(vn);
      Result[0] := v[0] * InvLen;
      Result[1] := v[1] * InvLen;
      Result[2] := v[2] * InvLen;
    end;
  Result[3] := 0;
end;

// VectorAngleCosine
//
function VectorAngleCosine(const v1, v2: TAffineVector): TGeoFloat;
begin
  Result := VectorDotProduct(v1, v2) / (VectorLength(v1) * VectorLength(v2));
end;

// VectorAngleCosine
//
function VectorAngleCosine(const v1, v2: TVector): TGeoFloat;
begin
  Result := VectorDotProduct(v1, v2) / (VectorLength(v1) * VectorLength(v2));
end;

// VectorNegate (affine)
//
function VectorNegate(const v: TAffineVector): TAffineVector;
begin
  Result[0] := -v[0];
  Result[1] := -v[1];
  Result[2] := -v[2];
end;

// VectorNegate (hmg)
//
function VectorNegate(const v: TVector): TVector;
begin
  Result[0] := -v[0];
  Result[1] := -v[1];
  Result[2] := -v[2];
  Result[3] := -v[3];
end;

// NegateVector
//
procedure NegateVector(var v: TAffineVector);
begin
  v[0] := -v[0];
  v[1] := -v[1];
  v[2] := -v[2];
end;

// NegateVector
//
procedure NegateVector(var v: TVector);
begin
  v[0] := -v[0];
  v[1] := -v[1];
  v[2] := -v[2];
  v[3] := -v[3];
end;

// NegateVector
//
procedure NegateVector(var v: array of TGeoFloat);
var
  i: Integer;
begin
  for i := low(v) to high(v) do
      v[i] := -v[i];
end;

// ScaleVector (2f)
//
procedure ScaleVector(var v: TVector2f; factor: TGeoFloat);
begin
  v[0] := v[0] * factor;
  v[1] := v[1] * factor;
end;

// ScaleVector (affine)
//
procedure ScaleVector(var v: TAffineVector; factor: TGeoFloat);
begin
  v[0] := v[0] * factor;
  v[1] := v[1] * factor;
  v[2] := v[2] * factor;
end;

// ScaleVector (hmg)
//
procedure ScaleVector(var v: TVector; factor: TGeoFloat);
begin
  v[0] := v[0] * factor;
  v[1] := v[1] * factor;
  v[2] := v[2] * factor;
  v[3] := v[3] * factor;
end;

// ScaleVector (affine vector)
//
procedure ScaleVector(var v: TAffineVector; const factor: TAffineVector);
begin
  v[0] := v[0] * factor[0];
  v[1] := v[1] * factor[1];
  v[2] := v[2] * factor[2];
end;

// ScaleVector (hmg vector)
//
procedure ScaleVector(var v: TVector; const factor: TVector);
begin
  v[0] := v[0] * factor[0];
  v[1] := v[1] * factor[1];
  v[2] := v[2] * factor[2];
  v[3] := v[3] * factor[3];
end;

// VectorScale (2f)
//
function VectorScale(const v: TVector2f; factor: TGeoFloat): TVector2f;
begin
  Result[0] := v[0] * factor;
  Result[1] := v[1] * factor;
end;

// VectorScale (affine)
//
function VectorScale(const v: TAffineVector; factor: TGeoFloat): TAffineVector;
begin
  Result[0] := v[0] * factor;
  Result[1] := v[1] * factor;
  Result[2] := v[2] * factor;
end;

// VectorScale (proc, affine)
//
procedure VectorScale(const v: TAffineVector; factor: TGeoFloat; var vr: TAffineVector);
begin
  vr[0] := v[0] * factor;
  vr[1] := v[1] * factor;
  vr[2] := v[2] * factor;
end;

// VectorScale (hmg)
//
function VectorScale(const v: TVector; factor: TGeoFloat): TVector;
begin
  Result[0] := v[0] * factor;
  Result[1] := v[1] * factor;
  Result[2] := v[2] * factor;
  Result[3] := v[3] * factor;
end;

// VectorScale (proc, hmg)
//
procedure VectorScale(const v: TVector; factor: TGeoFloat; var vr: TVector);
begin
  vr[0] := v[0] * factor;
  vr[1] := v[1] * factor;
  vr[2] := v[2] * factor;
  vr[3] := v[3] * factor;
end;

// VectorScale (proc, hmg-affine)
//
procedure VectorScale(const v: TVector; factor: TGeoFloat; var vr: TAffineVector);
begin
  vr[0] := v[0] * factor;
  vr[1] := v[1] * factor;
  vr[2] := v[2] * factor;
end;

// VectorScale (func, affine)
//
function VectorScale(const v: TAffineVector; const factor: TAffineVector): TAffineVector;
begin
  Result[0] := v[0] * factor[0];
  Result[1] := v[1] * factor[1];
  Result[2] := v[2] * factor[2];
end;

// VectorScale (func, hmg)
//
function VectorScale(const v: TVector; const factor: TVector): TVector;
begin
  Result[0] := v[0] * factor[0];
  Result[1] := v[1] * factor[1];
  Result[2] := v[2] * factor[2];
  Result[3] := v[3] * factor[3];
end;

// DivideVector
//
procedure DivideVector(var v: TVector; const divider: TVector);
begin
  v[0] := v[0] / divider[0];
  v[1] := v[1] / divider[1];
  v[2] := v[2] / divider[2];
  v[3] := v[3] / divider[3];
end;

// DivideVector
//
procedure DivideVector(var v: TAffineVector; const divider: TAffineVector); overload;
begin
  v[0] := v[0] / divider[0];
  v[1] := v[1] / divider[1];
  v[2] := v[2] / divider[2];
end;

// VectorDivide
//
function VectorDivide(const v: TVector; const divider: TVector): TVector; overload;
begin
  Result[0] := v[0] / divider[0];
  Result[1] := v[1] / divider[1];
  Result[2] := v[2] / divider[2];
  Result[3] := v[3] / divider[3];
end;

// VectorDivide
//
function VectorDivide(const v: TAffineVector; const divider: TAffineVector): TAffineVector; overload;
begin
  Result[0] := v[0] / divider[0];
  Result[1] := v[1] / divider[1];
  Result[2] := v[2] / divider[2];
end;

// TexpointEquals
//
function TexpointEquals(const p1, p2: TTexPoint): Boolean;
begin
  Result := (p1.s = p2.s) and (p1.t = p2.t);
end;

// VectorEquals (hmg vector)
//
function VectorEquals(const v1, v2: TVector): Boolean;
begin
  Result := (v1[0] = v2[0]) and (v1[1] = v2[1]) and (v1[2] = v2[2]) and (v1[3] = v2[3]);
end;

// VectorEquals (affine vector)
//
function VectorEquals(const v1, v2: TAffineVector): Boolean;
begin
  Result := (v1[0] = v2[0]) and (v1[1] = v2[1]) and (v1[2] = v2[2]);
end;

// AffineVectorEquals (hmg vector)
//
function AffineVectorEquals(const v1, v2: TVector): Boolean;
begin
  Result := (v1[0] = v2[0]) and (v1[1] = v2[1]) and (v1[2] = v2[2]);
end;

// VectorIsNull (hmg)
//
function VectorIsNull(const v: TVector): Boolean;
begin
  Result := ((v[0] = 0) and (v[1] = 0) and (v[2] = 0));
end;

// VectorIsNull (affine)
//
function VectorIsNull(const v: TAffineVector): Boolean; overload;
begin
  Result := ((v[0] = 0) and (v[1] = 0) and (v[2] = 0));
end;

// VectorSpacing (texpoint)
//
function VectorSpacing(const v1, v2: TTexPoint): TGeoFloat; overload;
begin
  Result := Abs(v2.s - v1.s) + Abs(v2.t - v1.t);
end;

// VectorSpacing (affine)
//
function VectorSpacing(const v1, v2: TAffineVector): TGeoFloat;
begin
  Result := Abs(v2[0] - v1[0]) + Abs(v2[1] - v1[1]) + Abs(v2[2] - v1[2]);
end;

// VectorSpacing (Hmg)
//
function VectorSpacing(const v1, v2: TVector): TGeoFloat;
begin
  Result := Abs(v2[0] - v1[0]) + Abs(v2[1] - v1[1]) + Abs(v2[2] - v1[2]) + Abs(v2[3] - v1[3]);
end;

// VectorDistance (affine)
//
function VectorDistance(const v1, v2: TAffineVector): TGeoFloat;
begin
  Result := Sqrt(Sqr(v2[0] - v1[0]) + Sqr(v2[1] - v1[1]) + Sqr(v2[2] - v1[2]));
end;

// VectorDistance (hmg)
//
function VectorDistance(const v1, v2: TVector): TGeoFloat;
begin
  Result := Sqrt(Sqr(v2[0] - v1[0]) + Sqr(v2[1] - v1[1]) + Sqr(v2[2] - v1[2]));
end;

// VectorDistance2 (affine)
//
function VectorDistance2(const v1, v2: TAffineVector): TGeoFloat;
begin
  Result := Sqr(v2[0] - v1[0]) + Sqr(v2[1] - v1[1]) + Sqr(v2[2] - v1[2]);
end;

// VectorDistance2 (hmg)
//
function VectorDistance2(const v1, v2: TVector): TGeoFloat;
begin
  Result := Sqr(v2[0] - v1[0]) + Sqr(v2[1] - v1[1]) + Sqr(v2[2] - v1[2]);
end;

// VectorPerpendicular
//
function VectorPerpendicular(const v, n: TAffineVector): TAffineVector;
var
  dot: TGeoFloat;
begin
  dot := VectorDotProduct(v, n);
  Result[x] := v[x] - dot * n[x];
  Result[y] := v[y] - dot * n[y];
  Result[z] := v[z] - dot * n[z];
end;

// VectorReflect
//
function VectorReflect(const v, n: TAffineVector): TAffineVector;
begin
  Result := VectorCombine(v, n, 1, -2 * VectorDotProduct(v, n));
end;

// RotateVector
//
procedure RotateVector(var Vector: TVector; const axis: TAffineVector; angle: TGeoFloat);
var
  rotMatrix: TMatrix4f;
begin
  rotMatrix := CreateRotationMatrix(axis, angle);
  Vector := VectorTransform(Vector, rotMatrix);
end;

// RotateVector
//
procedure RotateVector(var Vector: TVector; const axis: TVector; angle: TGeoFloat); overload;
var
  rotMatrix: TMatrix4f;
begin
  rotMatrix := CreateRotationMatrix(PAffineVector(@axis)^, angle);
  Vector := VectorTransform(Vector, rotMatrix);
end;

// RotateVectorAroundY
//
procedure RotateVectorAroundY(var v: TAffineVector; alpha: TGeoFloat);
var
  c, s, v0: TGeoFloat;
begin
  GeometryLib.SinCos(alpha, s, c);
  v0 := v[0];
  v[0] := c * v0 + s * v[2];
  v[2] := c * v[2] - s * v0;
end;

// VectorRotateAroundX (func)
//
function VectorRotateAroundX(const v: TAffineVector; alpha: TGeoFloat): TAffineVector;
var
  c, s: TGeoFloat;
begin
  GeometryLib.SinCos(alpha, s, c);
  Result[0] := v[0];
  Result[1] := c * v[1] + s * v[2];
  Result[2] := c * v[2] - s * v[1];
end;

// VectorRotateAroundY (func)
//
function VectorRotateAroundY(const v: TAffineVector; alpha: TGeoFloat): TAffineVector;
var
  c, s: TGeoFloat;
begin
  GeometryLib.SinCos(alpha, s, c);
  Result[1] := v[1];
  Result[0] := c * v[0] + s * v[2];
  Result[2] := c * v[2] - s * v[0];
end;

// VectorRotateAroundY (proc)
//
procedure VectorRotateAroundY(const v: TAffineVector; alpha: TGeoFloat; var vr: TAffineVector);
var
  c, s: TGeoFloat;
begin
  GeometryLib.SinCos(alpha, s, c);
  vr[1] := v[1];
  vr[0] := c * v[0] + s * v[2];
  vr[2] := c * v[2] - s * v[0];
end;

// VectorRotateAroundZ (func)
//
function VectorRotateAroundZ(const v: TAffineVector; alpha: TGeoFloat): TAffineVector;
var
  c, s: TGeoFloat;
begin
  GeometryLib.SinCos(alpha, s, c);
  Result[0] := c * v[0] + s * v[1];
  Result[1] := c * v[1] - s * v[0];
  Result[2] := v[2];
end;

// AbsVector (hmg)
//
procedure AbsVector(var v: TVector);
begin
  v[0] := Abs(v[0]);
  v[1] := Abs(v[1]);
  v[2] := Abs(v[2]);
  v[3] := Abs(v[3]);
end;

// AbsVector (affine)
//
procedure AbsVector(var v: TAffineVector);
begin
  v[0] := Abs(v[0]);
  v[1] := Abs(v[1]);
  v[2] := Abs(v[2]);
end;

// VectorAbs (hmg)
//
function VectorAbs(const v: TVector): TVector;
begin
  Result[0] := Abs(v[0]);
  Result[1] := Abs(v[1]);
  Result[2] := Abs(v[2]);
  Result[3] := Abs(v[3]);
end;

// VectorAbs (affine)
//
function VectorAbs(const v: TAffineVector): TAffineVector;
begin
  Result[0] := Abs(v[0]);
  Result[1] := Abs(v[1]);
  Result[2] := Abs(v[2]);
end;

// IsColinear (2f)
//
function IsColinear(const v1, v2: TVector2f): Boolean; overload;
var
  a, b, c: TGeoFloat;
begin
  a := VectorDotProduct(v1, v1);
  b := VectorDotProduct(v1, v2);
  c := VectorDotProduct(v2, v2);
  Result := (a * c - b * b) < cColinearBias;
end;

// IsColinear (affine)
//
function IsColinear(const v1, v2: TAffineVector): Boolean; overload;
var
  a, b, c: TGeoFloat;
begin
  a := VectorDotProduct(v1, v1);
  b := VectorDotProduct(v1, v2);
  c := VectorDotProduct(v2, v2);
  Result := (a * c - b * b) < cColinearBias;
end;

// IsColinear (hmg)
//
function IsColinear(const v1, v2: TVector): Boolean; overload;
var
  a, b, c: TGeoFloat;
begin
  a := VectorDotProduct(v1, v1);
  b := VectorDotProduct(v1, v2);
  c := VectorDotProduct(v2, v2);
  Result := (a * c - b * b) < cColinearBias;
end;

// SetMatrix (hmg->affine)
//
procedure SetMatrix(var dest: TAffineMatrix; const Src: TMatrix);
begin
  dest[0, 0] := Src[0, 0];
  dest[0, 1] := Src[0, 1];
  dest[0, 2] := Src[0, 2];
  dest[1, 0] := Src[1, 0];
  dest[1, 1] := Src[1, 1];
  dest[1, 2] := Src[1, 2];
  dest[2, 0] := Src[2, 0];
  dest[2, 1] := Src[2, 1];
  dest[2, 2] := Src[2, 2];
end;

// SetMatrix (affine->hmg)
//
procedure SetMatrix(var dest: TMatrix; const Src: TAffineMatrix);
begin
  dest[0, 0] := Src[0, 0];
  dest[0, 1] := Src[0, 1];
  dest[0, 2] := Src[0, 2];
  dest[0, 3] := 0;
  dest[1, 0] := Src[1, 0];
  dest[1, 1] := Src[1, 1];
  dest[1, 2] := Src[1, 2];
  dest[1, 3] := 0;
  dest[2, 0] := Src[2, 0];
  dest[2, 1] := Src[2, 1];
  dest[2, 2] := Src[2, 2];
  dest[2, 3] := 0;
  dest[3, 0] := 0;
  dest[3, 1] := 0;
  dest[3, 2] := 0;
  dest[3, 3] := 1;
end;

// SetMatrixRow
//
procedure SetMatrixRow(var dest: TMatrix; rowNb: Integer; const aRow: TVector);
begin
  dest[0, rowNb] := aRow[0];
  dest[1, rowNb] := aRow[1];
  dest[2, rowNb] := aRow[2];
  dest[3, rowNb] := aRow[3];
end;

// CreateScaleMatrix (affine)
//
function CreateScaleMatrix(const v: TAffineVector): TMatrix;
begin
  Result := IdentityHmgMatrix;
  Result[x, x] := v[x];
  Result[y, y] := v[y];
  Result[z, z] := v[z];
end;

// CreateScaleMatrix (Hmg)
//
function CreateScaleMatrix(const v: TVector): TMatrix;
begin
  Result := IdentityHmgMatrix;
  Result[x, x] := v[x];
  Result[y, y] := v[y];
  Result[z, z] := v[z];
end;

// CreateTranslationMatrix (affine)
//
function CreateTranslationMatrix(const v: TAffineVector): TMatrix;
begin
  Result := IdentityHmgMatrix;
  Result[w, x] := v[x];
  Result[w, y] := v[y];
  Result[w, z] := v[z];
end;

// CreateTranslationMatrix (hmg)
//
function CreateTranslationMatrix(const v: TVector): TMatrix;
begin
  Result := IdentityHmgMatrix;
  Result[w, x] := v[x];
  Result[w, y] := v[y];
  Result[w, z] := v[z];
end;

// CreateScaleAndTranslationMatrix
//
function CreateScaleAndTranslationMatrix(const Scale, Offset: TVector): TMatrix;
begin
  Result := IdentityHmgMatrix;
  Result[x, x] := Scale[x];
  Result[w, x] := Offset[x];
  Result[y, y] := Scale[y];
  Result[w, y] := Offset[y];
  Result[z, z] := Scale[z];
  Result[w, z] := Offset[z];
end;

// CreateRotationMatrixX
//
function CreateRotationMatrixX(const sine, cosine: TGeoFloat): TMatrix;
begin
  Result := EmptyHmgMatrix;
  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;

// CreateRotationMatrixX
//
function CreateRotationMatrixX(const angle: TGeoFloat): TMatrix;
var
  s, c: TGeoFloat;
begin
  GeometryLib.SinCos(angle, s, c);
  Result := CreateRotationMatrixX(s, c);
end;

// CreateRotationMatrixY
//
function CreateRotationMatrixY(const sine, cosine: TGeoFloat): TMatrix;
begin
  Result := EmptyHmgMatrix;
  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;

// CreateRotationMatrixY
//
function CreateRotationMatrixY(const angle: TGeoFloat): TMatrix;
var
  s, c: TGeoFloat;
begin
  GeometryLib.SinCos(angle, s, c);
  Result := CreateRotationMatrixY(s, c);
end;

// CreateRotationMatrixZ
//
function CreateRotationMatrixZ(const sine, cosine: TGeoFloat): TMatrix;
begin
  Result := EmptyHmgMatrix;
  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;

// CreateRotationMatrixZ
//
function CreateRotationMatrixZ(const angle: TGeoFloat): TMatrix;
var
  s, c: TGeoFloat;
begin
  GeometryLib.SinCos(angle, s, c);
  Result := CreateRotationMatrixZ(s, c);
end;

// CreateRotationMatrix (affine)
//
function CreateRotationMatrix(const anAxis: TAffineVector; angle: TGeoFloat): TMatrix;
var
  axis: TAffineVector;
  cosine, sine, one_minus_cosine: TGeoFloat;
begin
  GeometryLib.SinCos(angle, sine, cosine);
  one_minus_cosine := 1 - cosine;
  axis := VectorNormalize(anAxis);

  Result[x, x] := (one_minus_cosine * axis[0] * 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 * axis[1] * 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 * axis[2] * axis[2]) + cosine;
  Result[z, w] := 0;

  Result[w, x] := 0;
  Result[w, y] := 0;
  Result[w, z] := 0;
  Result[w, w] := 1;
end;

// CreateRotationMatrix (hmg)
//
function CreateRotationMatrix(const anAxis: TVector; angle: TGeoFloat): TMatrix;
begin
  Result := CreateRotationMatrix(PAffineVector(@anAxis)^, angle);
end;

// CreateAffineRotationMatrix
//
function CreateAffineRotationMatrix(const anAxis: TAffineVector; angle: TGeoFloat): TAffineMatrix;
var
  axis: TAffineVector;
  cosine, sine, one_minus_cosine: TGeoFloat;
begin
  GeometryLib.SinCos(angle, sine, cosine);
  one_minus_cosine := 1 - cosine;
  axis := VectorNormalize(anAxis);

  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[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[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;
end;

// MatrixMultiply (3x3 func)
//
function MatrixMultiply(const m1, m2: TAffineMatrix): TAffineMatrix;
begin
  Result[x, x] := m1[x, x] * m2[x, x] + m1[x, y] * m2[y, x] + m1[x, z] * m2[z, x];
  Result[x, y] := m1[x, x] * m2[x, y] + m1[x, y] * m2[y, y] + m1[x, z] * m2[z, y];
  Result[x, z] := m1[x, x] * m2[x, z] + m1[x, y] * m2[y, z] + m1[x, z] * m2[z, z];
  Result[y, x] := m1[y, x] * m2[x, x] + m1[y, y] * m2[y, x] + m1[y, z] * m2[z, x];
  Result[y, y] := m1[y, x] * m2[x, y] + m1[y, y] * m2[y, y] + m1[y, z] * m2[z, y];
  Result[y, z] := m1[y, x] * m2[x, z] + m1[y, y] * m2[y, z] + m1[y, z] * m2[z, z];
  Result[z, x] := m1[z, x] * m2[x, x] + m1[z, y] * m2[y, x] + m1[z, z] * m2[z, x];
  Result[z, y] := m1[z, x] * m2[x, y] + m1[z, y] * m2[y, y] + m1[z, z] * m2[z, y];
  Result[z, z] := m1[z, x] * m2[x, z] + m1[z, y] * m2[y, z] + m1[z, z] * m2[z, z];
end;

// MatrixMultiply (4x4, func)
//
function MatrixMultiply(const m1, m2: TMatrix): TMatrix;
begin
  Result[x, x] := m1[x, x] * m2[x, x] + m1[x, y] * m2[y, x] + m1[x, z] * m2[z, x] + m1[x, w] * m2[w, x];
  Result[x, y] := m1[x, x] * m2[x, y] + m1[x, y] * m2[y, y] + m1[x, z] * m2[z, y] + m1[x, w] * m2[w, y];
  Result[x, z] := m1[x, x] * m2[x, z] + m1[x, y] * m2[y, z] + m1[x, z] * m2[z, z] + m1[x, w] * m2[w, z];
  Result[x, w] := m1[x, x] * m2[x, w] + m1[x, y] * m2[y, w] + m1[x, z] * m2[z, w] + m1[x, w] * m2[w, w];
  Result[y, x] := m1[y, x] * m2[x, x] + m1[y, y] * m2[y, x] + m1[y, z] * m2[z, x] + m1[y, w] * m2[w, x];
  Result[y, y] := m1[y, x] * m2[x, y] + m1[y, y] * m2[y, y] + m1[y, z] * m2[z, y] + m1[y, w] * m2[w, y];
  Result[y, z] := m1[y, x] * m2[x, z] + m1[y, y] * m2[y, z] + m1[y, z] * m2[z, z] + m1[y, w] * m2[w, z];
  Result[y, w] := m1[y, x] * m2[x, w] + m1[y, y] * m2[y, w] + m1[y, z] * m2[z, w] + m1[y, w] * m2[w, w];
  Result[z, x] := m1[z, x] * m2[x, x] + m1[z, y] * m2[y, x] + m1[z, z] * m2[z, x] + m1[z, w] * m2[w, x];
  Result[z, y] := m1[z, x] * m2[x, y] + m1[z, y] * m2[y, y] + m1[z, z] * m2[z, y] + m1[z, w] * m2[w, y];
  Result[z, z] := m1[z, x] * m2[x, z] + m1[z, y] * m2[y, z] + m1[z, z] * m2[z, z] + m1[z, w] * m2[w, z];
  Result[z, w] := m1[z, x] * m2[x, w] + m1[z, y] * m2[y, w] + m1[z, z] * m2[z, w] + m1[z, w] * m2[w, w];
  Result[w, x] := m1[w, x] * m2[x, x] + m1[w, y] * m2[y, x] + m1[w, z] * m2[z, x] + m1[w, w] * m2[w, x];
  Result[w, y] := m1[w, x] * m2[x, y] + m1[w, y] * m2[y, y] + m1[w, z] * m2[z, y] + m1[w, w] * m2[w, y];
  Result[w, z] := m1[w, x] * m2[x, z] + m1[w, y] * m2[y, z] + m1[w, z] * m2[z, z] + m1[w, w] * m2[w, z];
  Result[w, w] := m1[w, x] * m2[x, w] + m1[w, y] * m2[y, w] + m1[w, z] * m2[z, w] + m1[w, w] * m2[w, w];
end;

// MatrixMultiply (4x4, proc)
//
procedure MatrixMultiply(const m1, m2: TMatrix; var MResult: TMatrix);
begin
  MResult := MatrixMultiply(m1, m2);
end;

// VectorTransform
//
function VectorTransform(const v: TVector; const M: TMatrix): TVector;
begin
  Result[x] := v[x] * M[x, x] + v[y] * M[y, x] + v[z] * M[z, x] + v[w] * M[w, x];
  Result[y] := v[x] * M[x, y] + v[y] * M[y, y] + v[z] * M[z, y] + v[w] * M[w, y];
  Result[z] := v[x] * M[x, z] + v[y] * M[y, z] + v[z] * M[z, z] + v[w] * M[w, z];
  Result[w] := v[x] * M[x, w] + v[y] * M[y, w] + v[z] * M[z, w] + v[w] * M[w, w];
end;

// VectorTransform
//
function VectorTransform(const v: TVector; const M: TAffineMatrix): TVector;
begin
  Result[x] := v[x] * M[x, x] + v[y] * M[y, x] + v[z] * M[z, x];
  Result[y] := v[x] * M[x, y] + v[y] * M[y, y] + v[z] * M[z, y];
  Result[z] := v[x] * M[x, z] + v[y] * M[y, z] + v[z] * M[z, z];
  Result[w] := v[w];
end;

// VectorTransform
//
function VectorTransform(const v: TAffineVector; const M: TMatrix): TAffineVector;
begin
  Result[x] := v[x] * M[x, x] + v[y] * M[y, x] + v[z] * M[z, x] + M[w, x];
  Result[y] := v[x] * M[x, y] + v[y] * M[y, y] + v[z] * M[z, y] + M[w, y];
  Result[z] := v[x] * M[x, z] + v[y] * M[y, z] + v[z] * M[z, z] + M[w, z];
end;

// VectorTransform
//
function VectorTransform(const v: TAffineVector; const M: TAffineMatrix): TAffineVector;
begin
  Result[x] := v[x] * M[x, x] + v[y] * M[y, x] + v[z] * M[z, x];
  Result[y] := v[x] * M[x, y] + v[y] * M[y, y] + v[z] * M[z, y];
  Result[z] := v[x] * M[x, z] + v[y] * M[y, z] + v[z] * M[z, z];
end;

// MatrixDeterminant (affine)
//
function MatrixDeterminant(const M: TAffineMatrix): TGeoFloat;
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;

// MatrixDetInternal
//
function MatrixDetInternal(const a1, a2, a3, b1, b2, b3, c1, c2, c3: TGeoFloat): TGeoFloat;
// 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;

// MatrixDeterminant (hmg)
//
function MatrixDeterminant(const M: TMatrix): TGeoFloat;
begin
  Result := M[x, x] * MatrixDetInternal(M[y, y], M[z, y], M[w, y], M[y, z], M[z, z], M[w, z], M[y, w], M[z, w], M[w, w])
    - M[x, y] * MatrixDetInternal(M[y, x], M[z, x], M[w, x], M[y, z], M[z, z], M[w, z], M[y, w], M[z, w], M[w, w])
    + M[x, z] * MatrixDetInternal(M[y, x], M[z, x], M[w, x], M[y, y], M[z, y], M[w, y], M[y, w], M[z, w], M[w, w])
    - M[x, w] * MatrixDetInternal(M[y, x], M[z, x], M[w, x], M[y, y], M[z, y], M[w, y], M[y, z], M[z, z], M[w, z]);
end;

// AdjointMatrix
//
procedure AdjointMatrix(var M: TMatrix);
var
  a1, a2, a3, a4,
    b1, b2, b3, b4,
    c1, c2, c3, c4,
    d1, d2, d3, d4: TGeoFloat;
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;

// AdjointMatrix (affine)
//
procedure AdjointMatrix(var M: TAffineMatrix);
var
  a1, a2, a3,
    b1, b2, b3,
    c1, c2, c3: TGeoFloat;
begin
  a1 := M[x, x];
  a2 := M[x, y];
  a3 := M[x, z];
  b1 := M[y, x];
  b2 := M[y, y];
  b3 := M[y, z];
  c1 := M[z, x];
  c2 := M[z, y];
  c3 := M[z, z];
  M[x, x] := (b2 * c3 - c2 * b3);
  M[y, x] := -(b1 * c3 - c1 * b3);
  M[z, x] := (b1 * c2 - c1 * b2);

  M[x, y] := -(a2 * c3 - c2 * a3);
  M[y, y] := (a1 * c3 - c1 * a3);
  M[z, y] := -(a1 * c2 - c1 * a2);

  M[x, z] := (a2 * b3 - b2 * a3);
  M[y, z] := -(a1 * b3 - b1 * a3);
  M[z, z] := (a1 * b2 - b1 * a2);
end;

// ScaleMatrix (affine)
//
procedure ScaleMatrix(var M: TAffineMatrix; const factor: TGeoFloat);
var
  i: Integer;
begin
  for i := 0 to 2 do begin
      M[i, 0] := M[i, 0] * factor;
      M[i, 1] := M[i, 1] * factor;
      M[i, 2] := M[i, 2] * factor;
    end;
end;

// ScaleMatrix (hmg)
//
procedure ScaleMatrix(var M: TMatrix; const factor: TGeoFloat);
var
  i: Integer;
begin
  for i := 0 to 3 do begin
      M[i, 0] := M[i, 0] * factor;
      M[i, 1] := M[i, 1] * factor;
      M[i, 2] := M[i, 2] * factor;
      M[i, 3] := M[i, 3] * factor;
    end;
end;

// TranslateMatrix (affine vec)
//
procedure TranslateMatrix(var M: TMatrix; const v: TAffineVector);
begin
  M[3][0] := M[3][0] + v[0];
  M[3][1] := M[3][1] + v[1];
  M[3][2] := M[3][2] + v[2];
end;

// TranslateMatrix
//
procedure TranslateMatrix(var M: TMatrix; const v: TVector);
begin
  M[3][0] := M[3][0] + v[0];
  M[3][1] := M[3][1] + v[1];
  M[3][2] := M[3][2] + v[2];
end;

// NormalizeMatrix
//
procedure NormalizeMatrix(var M: TMatrix);
begin
  M[0][3] := 0;
  NormalizeVector(M[0]);
  M[1][3] := 0;
  NormalizeVector(M[1]);
  M[2] := VectorCrossProduct(M[0], M[1]);
  M[0] := VectorCrossProduct(M[1], M[2]);
  M[3] := WHmgVector;
end;

// TransposeMatrix
//
procedure TransposeMatrix(var M: TAffineMatrix);
var
  f: TGeoFloat;
begin
  f := M[0, 1];
  M[0, 1] := M[1, 0];
  M[1, 0] := f;
  f := M[0, 2];
  M[0, 2] := M[2, 0];
  M[2, 0] := f;
  f := M[1, 2];
  M[1, 2] := M[2, 1];
  M[2, 1] := f;
end;

// TransposeMatrix
//
procedure TransposeMatrix(var M: TMatrix);
var
  f: TGeoFloat;
begin
  f := M[0, 1];
  M[0, 1] := M[1, 0];
  M[1, 0] := f;
  f := M[0, 2];
  M[0, 2] := M[2, 0];
  M[2, 0] := f;
  f := M[0, 3];
  M[0, 3] := M[3, 0];
  M[3, 0] := f;
  f := M[1, 2];
  M[1, 2] := M[2, 1];
  M[2, 1] := f;
  f := M[1, 3];
  M[1, 3] := M[3, 1];
  M[3, 1] := f;
  f := M[2, 3];
  M[2, 3] := M[3, 2];
  M[3, 2] := f;
end;

// InvertMatrix
//
procedure InvertMatrix(var M: TMatrix);
var
  det: TGeoFloat;
begin
  det := MatrixDeterminant(M);
  if Abs(det) < Epsilon then
      M := IdentityHmgMatrix
  else begin
      AdjointMatrix(M);
      ScaleMatrix(M, 1 / det);
    end;
end;

// MatrixInvert
//
function MatrixInvert(const M: TMatrix): TMatrix;
begin
  Result := M;
  InvertMatrix(Result);
end;

// InvertMatrix (affine)
//
procedure InvertMatrix(var M: TAffineMatrix);
var
  det: TGeoFloat;
begin
  det := MatrixDeterminant(M);
  if Abs(det) < Epsilon then
      M := IdentityMatrix
  else begin
      AdjointMatrix(M);
      ScaleMatrix(M, 1 / det);
    end;
end;

// MatrixInvert (affine)
//
function MatrixInvert(const M: TAffineMatrix): TAffineMatrix;
begin
  Result := M;
  InvertMatrix(Result);
end;

// transpose_scale_m33
//
procedure transpose_scale_m33(const Src: TMatrix; var dest: TMatrix; var Scale: TGeoFloat);
begin
  dest[0][0] := Scale * Src[0][0];
  dest[1][0] := Scale * Src[0][1];
  dest[2][0] := Scale * Src[0][2];
  dest[0][1] := Scale * Src[1][0];
  dest[1][1] := Scale * Src[1][1];
  dest[2][1] := Scale * Src[1][2];
  dest[0][2] := Scale * Src[2][0];
  dest[1][2] := Scale * Src[2][1];
  dest[2][2] := Scale * Src[2][2];
end;

// AnglePreservingMatrixInvert
//
function AnglePreservingMatrixInvert(const mat: TMatrix): TMatrix;
var
  Scale: TGeoFloat;
begin
  Scale := VectorNorm(mat[0]);

  // Is the submatrix A singular?
  if Abs(Scale) < Epsilon then begin
      // Matrix M has no inverse
      Result := IdentityHmgMatrix;
      Exit;
    end
  else begin
      // Calculate the inverse of the square of the isotropic scale factor
      Scale := 1.0 / Scale;
    end;

  // Fill in last row while CPU is busy with the division
  Result[0][3] := 0.0;
  Result[1][3] := 0.0;
  Result[2][3] := 0.0;
  Result[3][3] := 1.0;

  // Transpose and scale the 3 by 3 upper-left submatrix
  transpose_scale_m33(mat, Result, Scale);

  // Calculate -(transpose(A) / s*s) C
  Result[3][0] := -(Result[0][0] * mat[3][0]
    + Result[1][0] * mat[3][1]
    + Result[2][0] * mat[3][2]);
  Result[3][1] := -(Result[0][1] * mat[3][0]
    + Result[1][1] * mat[3][1]
    + Result[2][1] * mat[3][2]);
  Result[3][2] := -(Result[0][2] * mat[3][0]
    + Result[1][2] * mat[3][1]
    + Result[2][2] * mat[3][2]);
end;

// MatrixDecompose
//
function MatrixDecompose(const M: TMatrix; var Tran: TTransformations): Boolean;
var
  i, j: Integer;
  LocMat, pmat, invpmat: TMatrix;
  prhs, psol: TVector;
  row0, row1, row2: TAffineVector;
  f: TGeoFloat;
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;
      InvertMatrix(invpmat);
      TransposeMatrix(invpmat);
      psol := VectorTransform(prhs, invpmat);

      // 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
  SetVector(row0, LocMat[0]);
  SetVector(row1, LocMat[1]);
  SetVector(row2, LocMat[2]);

  // compute X scale factor and normalize first row
  Tran[ttScaleX] := VectorNorm(row0);
  ScaleVector(row0, RSqrt(Tran[ttScaleX]));

  // compute XY shear factor and make 2nd row orthogonal to 1st
  Tran[ttShearXY] := VectorDotProduct(row0, row1);
  f := -Tran[ttShearXY];
  CombineVector(row1, row0, f);

  // now, compute Y scale and normalize 2nd row
  Tran[ttScaleY] := VectorNorm(row1);
  ScaleVector(row1, RSqrt(Tran[ttScaleY]));
  Tran[ttShearXY] := Tran[ttShearXY] / Tran[ttScaleY];

  // compute XZ and YZ shears, orthogonalize 3rd row
  Tran[ttShearXZ] := VectorDotProduct(row0, row2);
  f := -Tran[ttShearXZ];
  CombineVector(row2, row0, f);
  Tran[ttShearYZ] := VectorDotProduct(row1, row2);
  f := -Tran[ttShearYZ];
  CombineVector(row2, row1, f);

  // next, get Z scale and normalize 3rd row
  Tran[ttScaleZ] := VectorNorm(row2);
  ScaleVector(row2, RSqrt(Tran[ttScaleZ]));
  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 VectorDotProduct(row0, VectorCrossProduct(row1, row2)) < 0 then begin
      for i := 0 to 2 do
          Tran[TTransType(Ord(ttScaleX) + i)] := -Tran[TTransType(Ord(ttScaleX) + i)];
      NegateVector(row0);
      NegateVector(row1);
      NegateVector(row2);
    end;

  // now, get the rotations out, as described in the gem
  Tran[ttRotateY] := GeometryLib.ArcSin(-row0[z]);
  if Cos(Tran[ttRotateY]) <> 0 then begin
      Tran[ttRotateX] := GeometryLib.ArcTan2(row1[z], row2[z]);
      Tran[ttRotateZ] := GeometryLib.ArcTan2(row0[y], row0[x]);
    end
  else begin
      Tran[ttRotateX] := GeometryLib.ArcTan2(row1[x], row1[y]);
      Tran[ttRotateZ] := 0;
    end;
  // All done!
  Result := True;
end;

function CreateLookAtMatrix(const eye, center, normUp: TVector): TMatrix;
var
  XAxis, YAxis, ZAxis, negEye: TVector;
begin
  ZAxis := VectorSubtract(center, eye);
  NormalizeVector(ZAxis);
  XAxis := VectorCrossProduct(ZAxis, normUp);
  NormalizeVector(XAxis);
  YAxis := VectorCrossProduct(XAxis, ZAxis);
  Result[0] := XAxis;
  Result[1] := YAxis;
  Result[2] := ZAxis;
  NegateVector(Result[2]);
  Result[3] := NullHmgPoint;
  TransposeMatrix(Result);
  negEye := eye;
  NegateVector(negEye);
  negEye[3] := 1;
  negEye := VectorTransform(negEye, Result);
  Result[3] := negEye;
end;

function CreateMatrixFromFrustum(Left, Right, Bottom, Top, ZNear, ZFar: TGeoFloat): TMatrix;
begin
  Result[0][0] := 2 * ZNear / (Right - Left);
  Result[0][1] := 0;
  Result[0][2] := 0;
  Result[0][3] := 0;

  Result[1][0] := 0;
  Result[1][1] := 2 * ZNear / (Top - Bottom);
  Result[1][2] := 0;
  Result[1][3] := 0;

  Result[2][0] := (Right + Left) / (Right - Left);
  Result[2][1] := (Top + Bottom) / (Top - Bottom);
  Result[2][2] := -(ZFar + ZNear) / (ZFar - ZNear);
  Result[2][3] := -1;

  Result[3][0] := 0;
  Result[3][1] := 0;
  Result[3][2] := -2 * ZFar * ZNear / (ZFar - ZNear);
  Result[3][3] := 0;
end;

function CreatePerspectiveMatrix(FOV, Aspect, ZNear, ZFar: TGeoFloat): TMatrix;
var
  x, y: TGeoFloat;
begin
  FOV := MinFloat(179.9, MaxFloat(0, FOV));
  y := ZNear * GeometryLib.Tan(GeometryLib.DegToRad(FOV) * 0.5);
  x := y * Aspect;
  Result := CreateMatrixFromFrustum(-x, x, -y, y, ZNear, ZFar);
end;

function CreateOrthoMatrix(Left, Right, Bottom, Top, ZNear, ZFar: TGeoFloat): TMatrix;
begin
  Result[0][0] := 2 / (Right - Left);
  Result[0][1] := 0;
  Result[0][2] := 0;
  Result[0][3] := 0;

  Result[1][0] := 0;
  Result[1][1] := 2 / (Top - Bottom);
  Result[1][2] := 0;
  Result[1][3] := 0;

  Result[2][0] := 0;
  Result[2][1] := 0;
  Result[2][2] := -2 / (ZFar - ZNear);
  Result[2][3] := 0;

  Result[3][0] := (Left + Right) / (Left - Right);
  Result[3][1] := (Bottom + Top) / (Bottom - Top);
  Result[3][2] := (ZNear + ZFar) / (ZNear - ZFar);
  Result[3][3] := 1;
end;

function CreatePickMatrix(x, y, deltax, deltay: TGeoFloat; const viewport: TVector4i): TMatrix;
begin
  if (deltax <= 0) or (deltay <= 0) then
    begin
      Result := IdentityHmgMatrix;
      Exit;
    end;
  // Translate and scale the picked region to the entire window
  Result := CreateTranslationMatrix(AffineVectorMake(
    (viewport[2] - 2 * (x - viewport[0])) / deltax,
    (viewport[3] - 2 * (y - viewport[1])) / deltay,
    0.0));
  Result[0][0] := viewport[2] / deltax;
  Result[1][1] := viewport[3] / deltay;
end;

function Project(
  objectVector: TVector;
  const ViewProjMatrix: TMatrix;
  const viewport: TVector4i;
  out WindowVector: TVector): Boolean;
begin
  Result := False;
  objectVector[3] := 1.0;
  WindowVector := VectorTransform(objectVector, ViewProjMatrix);
  if WindowVector[3] = 0.0 then
      Exit;
  WindowVector[0] := WindowVector[0] / WindowVector[3];
  WindowVector[1] := WindowVector[1] / WindowVector[3];
  WindowVector[2] := WindowVector[2] / WindowVector[3];
  // Map x, y and z to range 0-1
  WindowVector[0] := WindowVector[0] * 0.5 + 0.5;
  WindowVector[1] := WindowVector[1] * 0.5 + 0.5;
  WindowVector[2] := WindowVector[2] * 0.5 + 0.5;

  // Map x,y to viewport
  WindowVector[0] := WindowVector[0] * viewport[2] + viewport[0];
  WindowVector[1] := WindowVector[1] * viewport[3] + viewport[1];
  Result := True;
end;

function UnProject(
  WindowVector: TVector;
  ViewProjMatrix: TMatrix;
  const viewport: TVector4i;
  out objectVector: TVector): Boolean;
begin
  Result := False;
  InvertMatrix(ViewProjMatrix);
  WindowVector[3] := 1.0;
  // Map x and y from window coordinates
  WindowVector[0] := (WindowVector[0] - viewport[0]) / viewport[2];
  WindowVector[1] := (WindowVector[1] - viewport[1]) / viewport[3];
  // Map to range -1 to 1
  WindowVector[0] := WindowVector[0] * 2 - 1;
  WindowVector[1] := WindowVector[1] * 2 - 1;
  WindowVector[2] := WindowVector[2] * 2 - 1;
  objectVector := VectorTransform(WindowVector, ViewProjMatrix);
  if objectVector[3] = 0.0 then
      Exit;
  objectVector[0] := objectVector[0] / objectVector[3];
  objectVector[1] := objectVector[1] / objectVector[3];
  objectVector[2] := objectVector[2] / objectVector[3];
  Result := True;
end;

// CalcPlaneNormal (func, affine)
//
function CalcPlaneNormal(const p1, p2, p3: TAffineVector): TAffineVector;
var
  v1, v2: TAffineVector;
begin
  VectorSubtract(p2, p1, v1);
  VectorSubtract(p3, p1, v2);
  VectorCrossProduct(v1, v2, Result);
  NormalizeVector(Result);
end;

// CalcPlaneNormal (proc, affine)
//
procedure CalcPlaneNormal(const p1, p2, p3: TAffineVector; var vr: TAffineVector);
var
  v1, v2: TAffineVector;
begin
  VectorSubtract(p2, p1, v1);
  VectorSubtract(p3, p1, v2);
  VectorCrossProduct(v1, v2, vr);
  NormalizeVector(vr);
end;

// CalcPlaneNormal (proc, hmg)
//
procedure CalcPlaneNormal(const p1, p2, p3: TVector; var vr: TAffineVector); overload;
var
  v1, v2: TVector;
begin
  VectorSubtract(p2, p1, v1);
  VectorSubtract(p3, p1, v2);
  VectorCrossProduct(v1, v2, vr);
  NormalizeVector(vr);
end;

// PlaneMake (point + normal, affine)
//
function PlaneMake(const Point, normal: TAffineVector): THmgPlane;
begin
  PAffineVector(@Result)^ := normal;
  Result[3] := -VectorDotProduct(Point, normal);
end;

// PlaneMake (point + normal, hmg)
//
function PlaneMake(const Point, normal: TVector): THmgPlane;
begin
  PAffineVector(@Result)^ := PAffineVector(@normal)^;
  Result[3] := -VectorDotProduct(PAffineVector(@Point)^, PAffineVector(@normal)^);
end;

// PlaneMake (3 points, affine)
//
function PlaneMake(const p1, p2, p3: TAffineVector): THmgPlane;
begin
  CalcPlaneNormal(p1, p2, p3, PAffineVector(@Result)^);
  Result[3] := -VectorDotProduct(p1, PAffineVector(@Result)^);
end;

// PlaneMake (3 points, hmg)
//
function PlaneMake(const p1, p2, p3: TVector): THmgPlane;
begin
  CalcPlaneNormal(p1, p2, p3, PAffineVector(@Result)^);
  Result[3] := -VectorDotProduct(p1, PAffineVector(@Result)^);
end;

// NormalizePlane
//
procedure NormalizePlane(var plane: THmgPlane);
var
  n: TGeoFloat;
begin
  n := RSqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
  ScaleVector(plane, n);
end;

// PlaneEvaluatePoint (affine)
//
function PlaneEvaluatePoint(const plane: THmgPlane; const Point: TAffineVector): TGeoFloat;
begin
  Result := plane[0] * Point[0] + plane[1] * Point[1] + plane[2] * Point[2] + plane[3];
end;

// PlaneEvaluatePoint (hmg)
//
function PlaneEvaluatePoint(const plane: THmgPlane; const Point: TVector): TGeoFloat;
begin
  Result := plane[0] * Point[0] + plane[1] * Point[1] + plane[2] * Point[2] + plane[3];
end;

// PointIsInHalfSpace
//
function PointIsInHalfSpace(const Point, planePoint, planeNormal: TVector): Boolean;
begin
  Result := (PointPlaneDistance(Point, planePoint, planeNormal) > 0); // 44
end;

// PointIsInHalfSpace
//
function PointIsInHalfSpace(const Point, planePoint, planeNormal: TAffineVector): Boolean;
begin
  Result := (PointPlaneDistance(Point, planePoint, planeNormal) > 0);
end;

// PointIsInHalfSpace
//
function PointIsInHalfSpace(const Point: TAffineVector; plane: THmgPlane): Boolean;
begin
  Result := (PointPlaneDistance(Point, plane) > 0);
end;

// PointPlaneDistance
//
function PointPlaneDistance(const Point, planePoint, planeNormal: TVector): TGeoFloat;
begin
  Result := (Point[0] - planePoint[0]) * planeNormal[0]
    + (Point[1] - planePoint[1]) * planeNormal[1]
    + (Point[2] - planePoint[2]) * planeNormal[2];
end;

// PointPlaneDistance
//
function PointPlaneDistance(const Point, planePoint, planeNormal: TAffineVector): TGeoFloat;
begin
  Result := (Point[0] - planePoint[0]) * planeNormal[0]
    + (Point[1] - planePoint[1]) * planeNormal[1]
    + (Point[2] - planePoint[2]) * planeNormal[2];
end;

// PointPlaneDistance
//
function PointPlaneDistance(const Point: TAffineVector; plane: THmgPlane): TGeoFloat;
begin
  Result := PlaneEvaluatePoint(plane, Point);
end;

// PointPlaneOrthoProjection
//
function PointPlaneOrthoProjection(const Point: TAffineVector; const plane: THmgPlane;
  var inter: TAffineVector; bothface: Boolean): Boolean;
var
  h: TGeoFloat;
  normal: TAffineVector;
begin
  Result := False;

  h := PointPlaneDistance(Point, plane);

  if (not bothface) and (h < 0) then
      Exit;

  normal := Vector3fMake(plane);
  inter := VectorAdd(Point, VectorScale(normal, -h));
  Result := True;
end;

// PointPlaneProjection
//
function PointPlaneProjection(const Point, direction: TAffineVector; const plane: THmgPlane;
  var inter: TAffineVector; bothface: Boolean): Boolean;
var
  h, dot: TGeoFloat;
  normal: TAffineVector;
begin
  Result := False;

  normal := Vector3fMake(plane);
  dot := VectorDotProduct(VectorNormalize(direction), normal);

  if (not bothface) and (dot > 0) then
      Exit;

  if Abs(dot) >= 0.000000001 then begin
      h := PointPlaneDistance(Point, plane);
      inter := VectorAdd(Point, VectorScale(direction, -h / dot));
      Result := True;
    end;
end;

// SegmentPlaneIntersection
//
function SegmentPlaneIntersection(const ptA, ptB: TAffineVector; const plane: THmgPlane; var inter: TAffineVector): Boolean;
var
  hA, hB, dot: TGeoFloat;
  normal, direction: TVector3f;
begin
  Result := False;
  hA := PointPlaneDistance(ptA, plane);
  hB := PointPlaneDistance(ptB, plane);
  if hA * hB <= 0 then
    begin
      normal := Vector3fMake(plane);
      direction := VectorNormalize(VectorSubtract(ptB, ptA));
      dot := VectorDotProduct(direction, normal);
      if Abs(dot) >= 0.000000001 then begin
          inter := VectorAdd(ptA, VectorScale(direction, -hA / dot));
          Result := True;
        end;
    end;
end;

// PointTriangleOrthoProjection
//
function PointTriangleOrthoProjection(const Point, ptA, ptB, ptC: TAffineVector;
  var inter: TAffineVector; bothface: Boolean): Boolean;
var
  plane: THmgPlane;
begin
  Result := False;

  plane := PlaneMake(ptA, ptB, ptC);
  if not IsLineIntersectTriangle(Point, Vector3fMake(plane), ptA, ptB, ptC) then
      Exit;

  Result := PointPlaneOrthoProjection(Point, plane, inter, bothface);
end;

// PointTriangleProjection
//
function PointTriangleProjection(const Point, direction, ptA, ptB, ptC: TAffineVector;
  var inter: TAffineVector; bothface: Boolean): Boolean;
var
  plane: THmgPlane;
begin
  Result := False;

  if not IsLineIntersectTriangle(Point, direction, ptA, ptB, ptC) then
      Exit;

  plane := PlaneMake(ptA, ptB, ptC);
  Result := PointPlaneProjection(Point, direction, plane, inter, bothface);
end;

// IsLineIntersectTriangle
//
function IsLineIntersectTriangle(const Point, direction, ptA, ptB, ptC: TAffineVector): Boolean;
var
  PA, PB, PC: TAffineVector;
  crossAB, crossBC, crossCA: TAffineVector;
begin
  Result := False;

  PA := VectorSubtract(ptA, Point);
  PB := VectorSubtract(ptB, Point);
  PC := VectorSubtract(ptC, Point);

  crossAB := VectorCrossProduct(PA, PB);
  crossBC := VectorCrossProduct(PB, PC);

  if VectorDotProduct(crossAB, direction) > 0 then
    begin
      if VectorDotProduct(crossBC, direction) > 0 then
        begin
          crossCA := VectorCrossProduct(PC, PA);
          if VectorDotProduct(crossCA, direction) > 0 then
              Result := True;
        end;
    end
  else
    if VectorDotProduct(crossBC, direction) < 0 then
    begin
      crossCA := VectorCrossProduct(PC, PA);
      if VectorDotProduct(crossCA, direction) < 0 then
          Result := True;
    end
end;

// PointQuadOrthoProjection
//
function PointQuadOrthoProjection(const Point, ptA, ptB, ptC, ptD: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;
var
  plane: THmgPlane;
begin
  Result := False;

  plane := PlaneMake(ptA, ptB, ptC);
  if not IsLineIntersectQuad(Point, Vector3fMake(plane), ptA, ptB, ptC, ptD) then
      Exit;

  Result := PointPlaneOrthoProjection(Point, plane, inter, bothface);
end;

// PointQuadProjection
//
function PointQuadProjection(const Point, direction, ptA, ptB, ptC, ptD: TAffineVector; var inter: TAffineVector; bothface: Boolean): Boolean;
var
  plane: THmgPlane;
begin
  Result := False;

  if not IsLineIntersectQuad(Point, direction, ptA, ptB, ptC, ptD) then
      Exit;

  plane := PlaneMake(ptA, ptB, ptC);
  Result := PointPlaneProjection(Point, direction, plane, inter, bothface);
end;

// IsLineIntersectQuad
//
function IsLineIntersectQuad(const Point, direction, ptA, ptB, ptC, ptD: TAffineVector): Boolean;
var
  PA, PB, PC, PD: TAffineVector;
  crossAB, crossBC, crossCD, crossDA: TAffineVector;
begin
  Result := False;

  PA := VectorSubtract(ptA, Point);
  PB := VectorSubtract(ptB, Point);
  PC := VectorSubtract(ptC, Point);
  PD := VectorSubtract(ptD, Point);

  crossAB := VectorCrossProduct(PA, PB);
  crossBC := VectorCrossProduct(PB, PC);

  if VectorDotProduct(crossAB, direction) > 0 then
    begin
      if VectorDotProduct(crossBC, direction) > 0 then
        begin
          crossCD := VectorCrossProduct(PC, PD);
          if VectorDotProduct(crossCD, direction) > 0 then
            begin
              crossDA := VectorCrossProduct(PD, PA);
              if VectorDotProduct(crossDA, direction) > 0 then
                  Result := True;
            end;
        end;
    end
  else
    if VectorDotProduct(crossBC, direction) < 0 then
    begin
      crossCD := VectorCrossProduct(PC, PD);
      if VectorDotProduct(crossCD, direction) < 0 then
        begin
          crossDA := VectorCrossProduct(PD, PA);
          if VectorDotProduct(crossDA, direction) < 0 then
              Result := True;
        end;
    end
end;

// PointDiskOrthoProjection
//
function PointDiskOrthoProjection(const Point, center, up: TAffineVector; const radius: TGeoFloat; var inter: TAffineVector; bothface: Boolean): Boolean;
begin
  if PointPlaneOrthoProjection(Point, PlaneMake(center, up), inter, bothface) then
      Result := (VectorDistance2(inter, center) <= radius * radius)
  else
      Result := False;
end;

// PointDiskProjection
//
function PointDiskProjection(const Point, direction, center, up: TAffineVector; const radius: TGeoFloat; var inter: TAffineVector; bothface: Boolean): Boolean;
begin
  if PointPlaneProjection(Point, direction, PlaneMake(center, up), inter, bothface) then
      Result := VectorDistance2(inter, center) <= radius * radius
  else
      Result := False;
end;

// PointLineClosestPoint
//
function PointLineClosestPoint(const Point, linePoint, lineDirection: TAffineVector): TAffineVector;
var
  w: TAffineVector;
  c1, c2, b: TGeoFloat;
begin
  w := VectorSubtract(Point, linePoint);

  c1 := VectorDotProduct(w, lineDirection);
  c2 := VectorDotProduct(lineDirection, lineDirection);
  b := c1 / c2;

  VectorAdd(linePoint, VectorScale(lineDirection, b), Result);
end;

// PointLineDistance
//
function PointLineDistance(const Point, linePoint, lineDirection: TAffineVector): TGeoFloat;
var
  PB: TAffineVector;
begin
  PB := PointLineClosestPoint(Point, linePoint, lineDirection);
  Result := VectorDistance(Point, PB);
end;

// PointSegmentClosestPoint
//
function PointSegmentClosestPoint(const Point, segmentStart, segmentStop: TVector): TVector;
var
  w, lineDirection: TVector;
  c1, c2, b: TGeoFloat;
begin
  lineDirection := VectorSubtract(segmentStop, segmentStart);
  w := VectorSubtract(Point, segmentStart);

  c1 := VectorDotProduct(w, lineDirection);
  c2 := VectorDotProduct(lineDirection, lineDirection);
  b := ClampValue(c1 / c2, 0, 1);

  VectorAdd(segmentStart, VectorScale(lineDirection, b), Result);
end;

// PointSegmentClosestPoint
//
function PointSegmentClosestPoint(const Point, segmentStart, segmentStop: TAffineVector): TAffineVector;
var
  w, lineDirection: TAffineVector;
  c1, c2, b: TGeoFloat;
begin
  lineDirection := VectorSubtract(segmentStop, segmentStart);
  w := VectorSubtract(Point, segmentStart);

  c1 := VectorDotProduct(w, lineDirection);
  c2 := VectorDotProduct(lineDirection, lineDirection);
  b := ClampValue(c1 / c2, 0, 1);

  VectorAdd(segmentStart, VectorScale(lineDirection, b), Result);
end;

// PointSegmentDistance
//
function PointSegmentDistance(const Point, segmentStart, segmentStop: TAffineVector): TGeoFloat;
var
  PB: TAffineVector;
begin
  PB := PointSegmentClosestPoint(Point, segmentStart, segmentStop);
  Result := VectorDistance(Point, PB);
end;

// http://geometryalgorithms.com/Archive/algorithm_0104/algorithm_0104B.htm
// SegmentSegmentClosestPoint
//
procedure SegmentSegmentClosestPoint(const S0Start, S0Stop, S1Start, S1Stop: TAffineVector; var Segment0Closest, Segment1Closest: TAffineVector);
const
  cSMALL_NUM = 0.000000001;
var
  u, v, w: TAffineVector;
  a, b, c, smalld, E, largeD, sc, sn, sD, tc, tN, tD: TGeoFloat;
begin
  VectorSubtract(S0Stop, S0Start, u);
  VectorSubtract(S1Stop, S1Start, v);
  VectorSubtract(S0Start, S1Start, w);

  a := VectorDotProduct(u, u);
  b := VectorDotProduct(u, v);
  c := VectorDotProduct(v, v);
  smalld := VectorDotProduct(u, w);
  E := VectorDotProduct(v, w);
  largeD := a * c - b * b;

  sD := largeD;
  tD := largeD;

  if largeD < cSMALL_NUM then
    begin
      sn := 0.0;
      sD := 1.0;
      tN := E;
      tD := c;
    end
  else
    begin
      sn := (b * E - c * smalld);
      tN := (a * E - b * smalld);
      if (sn < 0.0) then
        begin
          sn := 0.0;
          tN := E;
          tD := c;
        end
      else if (sn > sD) then
        begin
          sn := sD;
          tN := E + b;
          tD := c;
        end;
    end;

  if (tN < 0.0) then
    begin
      tN := 0.0;
      // recompute sc for this edge
      if (-smalld < 0.0) then
          sn := 0.0
      else if (-smalld > a) then
          sn := sD
      else
        begin
          sn := -smalld;
          sD := a;
        end;
    end
  else if (tN > tD) then
    begin
      tN := tD;
      // recompute sc for this edge
      if ((-smalld + b) < 0.0) then
          sn := 0
      else if ((-smalld + b) > a) then
          sn := sD
      else
        begin
          sn := (-smalld + b);
          sD := a;
        end;
    end;

  // finally do the division to get sc and tc
  // sc := (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
  if Abs(sn) < cSMALL_NUM then
      sc := 0
  else
      sc := sn / sD;

  // tc := (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
  if Abs(tN) < cSMALL_NUM then
      tc := 0
  else
      tc := tN / tD;

  // get the difference of the two closest points
  // Vector   dP = w + (sc * u) - (tc * v);  // = S0(sc) - S1(tc)

  Segment0Closest := VectorAdd(S0Start, VectorScale(u, sc));
  Segment1Closest := VectorAdd(S1Start, VectorScale(v, tc));
end;

// SegmentSegmentDistance
//
function SegmentSegmentDistance(const S0Start, S0Stop, S1Start, S1Stop: TAffineVector): TGeoFloat;
var
  Pb0, PB1: TAffineVector;
begin
  SegmentSegmentClosestPoint(S0Start, S0Stop, S1Start, S1Stop, Pb0, PB1);
  Result := VectorDistance(Pb0, PB1);
end;

// LineLineDistance
//
function LineLineDistance(const linePt0, lineDir0, linePt1, lineDir1: TAffineVector): TGeoFloat;
const
  cBIAS = 0.000000001;
var
  det: TGeoFloat;
begin
  det := Abs((linePt1[0] - linePt0[0]) * (lineDir0[1] * lineDir1[2] - lineDir1[1] * lineDir0[2]) -
    (linePt1[1] - linePt0[1]) * (lineDir0[0] * lineDir1[2] - lineDir1[0] * lineDir0[2]) +
    (linePt1[2] - linePt0[2]) * (lineDir0[0] * lineDir1[1] - lineDir1[0] * lineDir0[1]));
  if det < cBIAS then
      Result := PointLineDistance(linePt0, linePt1, lineDir1)
  else
      Result := det / VectorLength(VectorCrossProduct(lineDir0, lineDir1));
end;

// QuaternionMake
//
function QuaternionMake(const Imag: array of TGeoFloat; Real: TGeoFloat): TQuaternion;
var
  n: Integer;
begin
  n := length(Imag);
  if n >= 1 then
      Result.ImagPart[0] := Imag[0];
  if n >= 2 then
      Result.ImagPart[1] := Imag[1];
  if n >= 3 then
      Result.ImagPart[2] := Imag[2];
  Result.RealPart := Real;
end;

// QuaternionConjugate
//
function QuaternionConjugate(const q: TQuaternion): TQuaternion;
begin
  Result.ImagPart[0] := -q.ImagPart[0];
  Result.ImagPart[1] := -q.ImagPart[1];
  Result.ImagPart[2] := -q.ImagPart[2];
  Result.RealPart := q.RealPart;
end;

// QuaternionMagnitude
//
function QuaternionMagnitude(const q: TQuaternion): TGeoFloat;
begin
  Result := Sqrt(VectorNorm(q.ImagPart) + Sqr(q.RealPart));
end;

// NormalizeQuaternion
//
procedure NormalizeQuaternion(var q: TQuaternion);
var
  M, f: TGeoFloat;
begin
  M := QuaternionMagnitude(q);
  if M > EPSILON2 then begin
      f := 1 / M;
      ScaleVector(q.ImagPart, f);
      q.RealPart := q.RealPart * f;
    end
  else
      q := IdentityQuaternion;
end;

// QuaternionFromPoints
//
function QuaternionFromPoints(const v1, v2: TAffineVector): TQuaternion;
begin
  Result.ImagPart := VectorCrossProduct(v1, v2);
  Result.RealPart := Sqrt((VectorDotProduct(v1, v2) + 1) / 2);
end;

// QuaternionFromMatrix
//
function QuaternionFromMatrix(const mat: TMatrix): TQuaternion;
// the matrix must be a rotation matrix!
var
  traceMat, s, invS: Double;
begin
  traceMat := 1 + mat[0, 0] + mat[1, 1] + mat[2, 2];
  if traceMat > EPSILON2 then begin
      s := Sqrt(traceMat) * 2;
      invS := 1 / s;
      Result.ImagPart[0] := (mat[1, 2] - mat[2, 1]) * invS;
      Result.ImagPart[1] := (mat[2, 0] - mat[0, 2]) * invS;
      Result.ImagPart[2] := (mat[0, 1] - mat[1, 0]) * invS;
      Result.RealPart := 0.25 * s;
    end
  else if (mat[0, 0] > mat[1, 1]) and (mat[0, 0] > mat[2, 2]) then begin // Row 0:
      s := Sqrt(MaxFloat(EPSILON2, cOne + mat[0, 0] - mat[1, 1] - mat[2, 2])) * 2;
      invS := 1 / s;
      Result.ImagPart[0] := 0.25 * s;
      Result.ImagPart[1] := (mat[0, 1] + mat[1, 0]) * invS;
      Result.ImagPart[2] := (mat[2, 0] + mat[0, 2]) * invS;
      Result.RealPart := (mat[1, 2] - mat[2, 1]) * invS;
    end
  else if (mat[1, 1] > mat[2, 2]) then begin // Row 1:
      s := Sqrt(MaxFloat(EPSILON2, cOne + mat[1, 1] - mat[0, 0] - mat[2, 2])) * 2;
      invS := 1 / s;
      Result.ImagPart[0] := (mat[0, 1] + mat[1, 0]) * invS;
      Result.ImagPart[1] := 0.25 * s;
      Result.ImagPart[2] := (mat[1, 2] + mat[2, 1]) * invS;
      Result.RealPart := (mat[2, 0] - mat[0, 2]) * invS;
    end
  else begin // Row 2:
      s := Sqrt(MaxFloat(EPSILON2, cOne + mat[2, 2] - mat[0, 0] - mat[1, 1])) * 2;
      invS := 1 / s;
      Result.ImagPart[0] := (mat[2, 0] + mat[0, 2]) * invS;
      Result.ImagPart[1] := (mat[1, 2] + mat[2, 1]) * invS;
      Result.ImagPart[2] := 0.25 * s;
      Result.RealPart := (mat[0, 1] - mat[1, 0]) * invS;
    end;
  NormalizeQuaternion(Result);
end;

// QuaternionMultiply
//
function QuaternionMultiply(const qL, qR: TQuaternion): TQuaternion;
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;

// QuaternionToMatrix
//
function QuaternionToMatrix(quat: TQuaternion): TMatrix;
var
  w, x, y, z, xx, xy, xz, xw, yy, yz, yw, zz, zw: TGeoFloat;
begin
  NormalizeQuaternion(quat);
  w := quat.RealPart;
  x := quat.ImagPart[0];
  y := quat.ImagPart[1];
  z := quat.ImagPart[2];
  xx := x * x;
  xy := x * y;
  xz := x * z;
  xw := x * w;
  yy := y * y;
  yz := y * z;
  yw := y * w;
  zz := z * z;
  zw := z * w;
  Result[0, 0] := 1 - 2 * (yy + zz);
  Result[1, 0] := 2 * (xy - zw);
  Result[2, 0] := 2 * (xz + yw);
  Result[3, 0] := 0;
  Result[0, 1] := 2 * (xy + zw);
  Result[1, 1] := 1 - 2 * (xx + zz);
  Result[2, 1] := 2 * (yz - xw);
  Result[3, 1] := 0;
  Result[0, 2] := 2 * (xz - yw);
  Result[1, 2] := 2 * (yz + xw);
  Result[2, 2] := 1 - 2 * (xx + yy);
  Result[3, 2] := 0;
  Result[0, 3] := 0;
  Result[1, 3] := 0;
  Result[2, 3] := 0;
  Result[3, 3] := 1;
end;

// QuaternionToAffineMatrix
//
function QuaternionToAffineMatrix(quat: TQuaternion): TAffineMatrix;
var
  w, x, y, z, xx, xy, xz, xw, yy, yz, yw, zz, zw: TGeoFloat;
begin
  NormalizeQuaternion(quat);
  w := quat.RealPart;
  x := quat.ImagPart[0];
  y := quat.ImagPart[1];
  z := quat.ImagPart[2];
  xx := x * x;
  xy := x * y;
  xz := x * z;
  xw := x * w;
  yy := y * y;
  yz := y * z;
  yw := y * w;
  zz := z * z;
  zw := z * w;
  Result[0, 0] := 1 - 2 * (yy + zz);
  Result[1, 0] := 2 * (xy - zw);
  Result[2, 0] := 2 * (xz + yw);
  Result[0, 1] := 2 * (xy + zw);
  Result[1, 1] := 1 - 2 * (xx + zz);
  Result[2, 1] := 2 * (yz - xw);
  Result[0, 2] := 2 * (xz - yw);
  Result[1, 2] := 2 * (yz + xw);
  Result[2, 2] := 1 - 2 * (xx + yy);
end;

// QuaternionFromAngleAxis
//
function QuaternionFromAngleAxis(const angle: TGeoFloat; const axis: TAffineVector): TQuaternion;
var
  f, s, c: TGeoFloat;
begin
  GeometryLib.SinCos(GeometryLib.DegToRad(angle * cOneDotFive), s, c);
  Result.RealPart := c;
  f := s / VectorLength(axis);
  Result.ImagPart[0] := axis[0] * f;
  Result.ImagPart[1] := axis[1] * f;
  Result.ImagPart[2] := axis[2] * f;
end;

// QuaternionFromRollPitchYaw
//
function QuaternionFromRollPitchYaw(const r, p, y: TGeoFloat): TQuaternion;
var
  qp, qy: TQuaternion;
begin
  Result := QuaternionFromAngleAxis(r, ZVector);
  qp := QuaternionFromAngleAxis(p, XVector);
  qy := QuaternionFromAngleAxis(y, YVector);

  Result := QuaternionMultiply(qp, Result);
  Result := QuaternionMultiply(qy, Result);
end;

// QuaternionFromEuler
//
function QuaternionFromEuler(const x, y, z: TGeoFloat; eulerOrder: TEulerOrder): TQuaternion;
// input angles in degrees
var
  gimbalLock: Boolean;
  quat1, quat2: TQuaternion;

  function EulerToQuat(const x, y, z: TGeoFloat; eulerOrder: TEulerOrder): TQuaternion;
  const
    cOrder: array [low(TEulerOrder) .. high(TEulerOrder)] of array [1 .. 3] of Byte =
      ((1, 2, 3), (1, 3, 2), (2, 1, 3), // eulXYZ, eulXZY, eulYXZ,
      (3, 1, 2), (2, 3, 1), (3, 2, 1)); // eulYZX, eulZXY, eulZYX
  var
    q: array [1 .. 3] of TQuaternion;
  begin
    q[cOrder[eulerOrder][1]] := QuaternionFromAngleAxis(x, XVector);
    q[cOrder[eulerOrder][2]] := QuaternionFromAngleAxis(y, YVector);
    q[cOrder[eulerOrder][3]] := QuaternionFromAngleAxis(z, ZVector);
    Result := QuaternionMultiply(q[2], q[3]);
    Result := QuaternionMultiply(q[1], Result);
  end;

const
  SMALL_ANGLE = 0.001;
begin
  NormalizeDegAngle(x);
  NormalizeDegAngle(y);
  NormalizeDegAngle(z);
  case eulerOrder of
    eulXYZ, eulZYX: gimbalLock := Abs(Abs(y) - 90.0) <= EPSILON2; // cos(Y) = 0;
    eulYXZ, eulZXY: gimbalLock := Abs(Abs(x) - 90.0) <= EPSILON2; // cos(X) = 0;
    eulXZY, eulYZX: gimbalLock := Abs(Abs(z) - 90.0) <= EPSILON2; // cos(Z) = 0;
    else
      Assert(False);
      gimbalLock := False;
  end;
  if gimbalLock then begin
      case eulerOrder of
        eulXYZ, eulZYX: quat1 := EulerToQuat(x, y - SMALL_ANGLE, z, eulerOrder);
        eulYXZ, eulZXY: quat1 := EulerToQuat(x - SMALL_ANGLE, y, z, eulerOrder);
        eulXZY, eulYZX: quat1 := EulerToQuat(x, y, z - SMALL_ANGLE, eulerOrder);
      end;
      case eulerOrder of
        eulXYZ, eulZYX: quat2 := EulerToQuat(x, y + SMALL_ANGLE, z, eulerOrder);
        eulYXZ, eulZXY: quat2 := EulerToQuat(x + SMALL_ANGLE, y, z, eulerOrder);
        eulXZY, eulYZX: quat2 := EulerToQuat(x, y, z + SMALL_ANGLE, eulerOrder);
      end;
      Result := QuaternionSlerp(quat1, quat2, 0.5);
    end
  else begin
      Result := EulerToQuat(x, y, z, eulerOrder);
    end;
end;

// QuaternionToPoints
//
procedure QuaternionToPoints(const q: TQuaternion; var ArcFrom, ArcTo: TAffineVector);
var
  s, invS: TGeoFloat;
begin
  s := q.ImagPart[x] * q.ImagPart[x] + q.ImagPart[y] * q.ImagPart[y];
  if s = 0 then
      SetAffineVector(ArcFrom, 0, 1, 0)
  else begin
      invS := RSqrt(s);
      SetAffineVector(ArcFrom, -q.ImagPart[y] * invS, q.ImagPart[x] * invS, 0);
    end;
  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
      SetAffineVector(ArcFrom, -ArcFrom[x], -ArcFrom[y], 0);
end;

// LnXP1
//
function LnXP1(x: Extended): Extended;
begin
  Result := Math.LnXP1(x);
end;

// Log10
//
function Log10(x: Extended): Extended;
begin
  Result := Math.Log10(x);
end;

// Log2
//
function Log2(x: Extended): Extended;
begin
  Result := Math.Log2(x);
end;

// Log2
//
function Log2(x: TGeoFloat): TGeoFloat;
begin
  Result := Math.Log2(x);
end;

// LogN
//
function LogN(Base, x: Extended): Extended;
begin
  Result := Math.LogN(Base, x);
end;

// IntPower
//
function IntPower(Base: Extended; Exponent: Integer): Extended;
begin
  Result := Math.IntPower(Base, Exponent);
end;

// Power
//
function Power(const Base, Exponent: TGeoFloat): TGeoFloat;
begin
  if Exponent = cZero then
      Result := cOne
  else if (Base = cZero) and (Exponent > cZero) then
      Result := cZero
  else if RoundInt(Exponent) = Exponent then
      Result := Power(Base, Integer(Round(Exponent)))
  else
      Result := Exp(Exponent * ln(Base));
end;

// Power (int exponent)
//
function Power(Base: TGeoFloat; Exponent: Integer): TGeoFloat;
begin
  Result := Math.Power(Base, Exponent);
end;

function Power(Base: TGeoFloat; Exponent: Int64): TGeoFloat;
begin
  Result := Math.Power(Base, Exponent);
end;

// DegToRad (extended)
//
function DegToRad(const Degrees: Extended): Extended;
begin
  Result := Degrees * (pi / 180);
end;

// DegToRad (TGeoFloat)
//
function DegToRad(const Degrees: TGeoFloat): TGeoFloat;
// Result:=Degrees * cPIdiv180;
// don't laugh, Delphi's compiler manages to make a nightmare of this one
// with pushs, pops, etc. in its default compile... (this one is twice faster !)
begin
  Result := Degrees * cPIdiv180;
end;

// RadToDeg (extended)
//
function RadToDeg(const Radians: Extended): Extended;
begin
  Result := Radians * (180 / pi);
end;

// RadToDeg (TGeoFloat)
//
function RadToDeg(const Radians: TGeoFloat): TGeoFloat;
// Result:=Radians * c180divPI;
// don't laugh, Delphi's compiler manages to make a nightmare of this one
// with pushs, pops, etc. in its default compile... (this one is twice faster !)
begin
  Result := Radians * c180divPI;
end;

// NormalizeAngle
//
function NormalizeAngle(angle: TGeoFloat): TGeoFloat;
begin
  Result := angle - Int(angle * cInv2PI) * c2PI;
  if Result > pi then
      Result := Result - 2 * pi
  else if Result < -pi then
      Result := Result + 2 * pi;
end;

// NormalizeDegAngle
//
function NormalizeDegAngle(angle: TGeoFloat): TGeoFloat;
begin
  Result := angle - Int(angle * cInv360) * c360;
  if Result > c180 then
      Result := Result - c360
  else if Result < -c180 then
      Result := Result + c360;
end;

// SinCos (Extended)
//
procedure SinCos(const Theta: Extended; out Sin, Cos: Extended);
begin
  Math.SinCos(Theta, Sin, Cos);
end;

// SinCos (TGeoFloat)
//
procedure SinCos(const Theta: TGeoFloat; out Sin, Cos: TGeoFloat);
var
  s, c: Extended;
begin
  Math.SinCos(Theta, s, c);
  Sin := s;
  Cos := c;
end;

// SinCos (Extended w radius)
//
procedure SinCos(const Theta, radius: Double; out Sin, Cos: Extended);
var
  s, c: Extended;
begin
  Math.SinCos(Theta, s, c);
  Sin := s * radius;
  Cos := c * radius;
end;

// SinCos (TGeoFloat w radius)
//
procedure SinCos(const Theta, radius: TGeoFloat; out Sin, Cos: TGeoFloat);
var
  s, c: Extended;
begin
  Math.SinCos(Theta, s, c);
  Sin := s * radius;
  Cos := c * radius;
end;

// PrepareSinCosCache
//
procedure PrepareSinCosCache(var s, c: array of TGeoFloat; startAngle, stopAngle: TGeoFloat);
var
  i: Integer;
  d, alpha, beta: TGeoFloat;
begin
  Assert((high(s) = high(c)) and (low(s) = low(c)));
  stopAngle := stopAngle + 1E-5;
  if high(s) > low(s) then
      d := cPIdiv180 * (stopAngle - startAngle) / (high(s) - low(s))
  else
      d := 0;

  if high(s) - low(s) < 1000 then begin
      // Fast computation (approx 5.5x)
      alpha := 2 * Sqr(Sin(d * 0.5));
      beta := Sin(d);
      GeometryLib.SinCos(startAngle * cPIdiv180, s[low(s)], c[low(s)]);
      for i := low(s) to high(s) - 1 do begin
          // Make use of the incremental formulae:
          // cos (theta+delta) = cos(theta) - [alpha*cos(theta) + beta*sin(theta)]
          // sin (theta+delta) = sin(theta) - [alpha*sin(theta) - beta*cos(theta)]
          c[i + 1] := c[i] - alpha * c[i] - beta * s[i];
          s[i + 1] := s[i] - alpha * s[i] + beta * c[i];
        end;
    end
  else begin
      // Slower, but maintains precision when steps are small
      startAngle := startAngle * cPIdiv180;
      for i := low(s) to high(s) do
          GeometryLib.SinCos((i - low(s)) * d + startAngle, s[i], c[i]);
    end;
end;

// ArcCos (Extended)
//
function ArcCos(const x: Extended): Extended;
begin
  Result := GeometryLib.ArcTan2(Sqrt(1 - Sqr(x)), x);
end;

// ArcCos (TGeoFloat)
//
function ArcCos(const x: TGeoFloat): TGeoFloat;
// Result:=ArcTan2(Sqrt(c1 - X * X), X);
begin
{$IFDEF FPC}
  if Abs(x) > 1.0 then
      Result := Math.ArcCos(Sign(x))
  else
{$ENDIF}
      Result := Math.ArcCos(x);
end;

// ArcSin (Extended)
//
function ArcSin(const x: Extended): Extended;
begin
  Result := GeometryLib.ArcTan2(x, Sqrt(1 - Sqr(x)))
end;

// ArcSin (TGeoFloat)
//
function ArcSin(const x: TGeoFloat): TGeoFloat;
// Result:=ArcTan2(X, Sqrt(1 - X * X))
begin
  Result := Math.ArcSin(x);
end;

// ArcTan2 (Extended)
//
function ArcTan2(const y, x: Extended): Extended;
begin
  Result := Math.ArcTan2(y, x);
end;

// ArcTan2 (TGeoFloat)
//
function ArcTan2(const y, x: TGeoFloat): TGeoFloat;
begin
  Result := Math.ArcTan2(y, x);
end;

// FastArcTan2
//
function FastArcTan2(y, x: TGeoFloat): TGeoFloat;
// accuracy of about 0.07 rads
const
  cEpsilon: TGeoFloat = 1E-10;
var
  abs_y: TGeoFloat;
begin
  abs_y := Abs(y) + cEpsilon; // prevent 0/0 condition
  if y < 0 then begin
      if x >= 0 then
          Result := cPIdiv4 * (x - abs_y) / (x + abs_y) - cPIdiv4
      else
          Result := cPIdiv4 * (x + abs_y) / (abs_y - x) - c3PIdiv4;
    end
  else begin
      if x >= 0 then
          Result := cPIdiv4 - cPIdiv4 * (x - abs_y) / (x + abs_y)
      else
          Result := c3PIdiv4 - cPIdiv4 * (x + abs_y) / (abs_y - x);
    end;
end;

// Tan (Extended)
//
function Tan(const x: Extended): Extended;
begin
  Result := Math.Tan(x);
end;

// Tan (TGeoFloat)
//
function Tan(const x: TGeoFloat): TGeoFloat;
begin
  Result := Math.Tan(x);
end;

// CoTan (Extended)
//
function CoTan(const x: Extended): Extended;
begin
  Result := Math.CoTan(x);
end;

// CoTan (TGeoFloat)
//
function CoTan(const x: TGeoFloat): TGeoFloat;
begin
  Result := Math.CoTan(x);
end;

// Sinh
//
function Sinh(const x: TGeoFloat): TGeoFloat;
begin
  Result := 0.5 * (Exp(x) - Exp(-x));
end;

// Sinh
//
function Sinh(const x: Double): Double;
begin
  Result := 0.5 * (Exp(x) - Exp(-x));
end;

// Cosh
//
function Cosh(const x: TGeoFloat): TGeoFloat;
begin
  Result := 0.5 * (Exp(x) + Exp(-x));
end;

// Cosh
//
function Cosh(const x: Double): Double;
begin
  Result := 0.5 * (Exp(x) + Exp(-x));
end;

// RSqrt
//
function RSqrt(v: TGeoFloat): TGeoFloat;
begin
  Result := 1 / Sqrt(v);
end;

// ISqrt
//
function ISqrt(i: Integer): Integer;
begin
  Result := Round(Sqrt(i));
end;

// ILength
//
function ILength(x, y: Integer): Integer;
begin
  Result := Round(Sqrt(x * x + y * y));
end;

// ILength
//
function ILength(x, y, z: Integer): Integer;
begin
  Result := Round(Sqrt(x * x + y * y + z * z));
end;

// RLength
//
function RLength(x, y: TGeoFloat): TGeoFloat;
begin
  Result := 1 / Sqrt(x * x + y * y);
end;

// RandomPointOnSphere
//
procedure RandomPointOnSphere(var p: TAffineVector);
var
  t, w: TGeoFloat;
begin
  p[2] := 2 * MT19937RandF - 1;
  t := 2 * pi * MT19937RandF;
  w := Sqrt(1 - p[2] * p[2]);
  GeometryLib.SinCos(t, w, p[1], p[0]);
end;

// RoundInt (TGeoFloat)
//
function RoundInt(v: TGeoFloat): TGeoFloat;
begin
  Result := Int(v + cOneDotFive);
end;

// RoundInt (extended)
//
function RoundInt(v: Extended): Extended;
begin
  Result := Int(v + 0.5);
end;

function Trunc(x: Extended): Int64;
begin
  Result := System.Trunc(x);
end;

function Round(x: Extended): Int64;
begin
  Result := System.Round(x);
end;

function Frac(x: Extended): Extended;
begin
  Result := System.Frac(x);
end;

// Ceil64 (Extended)
//
function Ceil64(v: Extended): Int64; overload;
begin
  if Frac(v) > 0 then
      Result := Trunc(v) + 1
  else
      Result := Trunc(v);
end;

// Ceil (TGeoFloat)
//
function Ceil(v: TGeoFloat): Integer; overload;
begin
  if Frac(v) > 0 then
      Result := Trunc(v) + 1
  else
      Result := Trunc(v);
end;

// Floor64 (Extended)
//
function Floor64(v: Extended): Int64; overload;
begin
  if v < 0 then
      Result := Trunc(v) - 1
  else
      Result := Trunc(v);
end;

// Floor (TGeoFloat)
//
function Floor(v: TGeoFloat): Integer; overload;
begin
  if v < 0 then
      Result := Trunc(v) - 1
  else
      Result := Trunc(v);
end;

// Sign
//
function Sign(x: TGeoFloat): Integer;
begin
  if x < 0 then
      Result := -1
  else if x > 0 then
      Result := 1
  else
      Result := 0;
end;

// SignStrict
//
function SignStrict(x: TGeoFloat): Integer;
begin
  if x < 0 then
      Result := -1
  else
      Result := 1
end;

// ScaleAndRound
//
function ScaleAndRound(i: Integer; var s: TGeoFloat): Integer;
begin
  Result := Round(i * s);
end;

// IsInRange (TGeoFloat)
//
function IsInRange(const x, a, b: TGeoFloat): Boolean;
begin
  if a < b then
      Result := (a <= x) and (x <= b)
  else
      Result := (b <= x) and (x <= a);
end;

// IsInRange (double)
//
function IsInRange(const x, a, b: Double): Boolean;
begin
  if a < b then
      Result := (a <= x) and (x <= b)
  else
      Result := (b <= x) and (x <= a);
end;

// IsInCube (affine)
//
function IsInCube(const p, d: TAffineVector): Boolean; overload;
begin
  Result := ((p[0] >= -d[0]) and (p[0] <= d[0]))
    and ((p[1] >= -d[1]) and (p[1] <= d[1]))
    and ((p[2] >= -d[2]) and (p[2] <= d[2]));
end;

// IsInCube (hmg)
//
function IsInCube(const p, d: TVector): Boolean; overload;
begin
  Result := ((p[0] >= -d[0]) and (p[0] <= d[0]))
    and ((p[1] >= -d[1]) and (p[1] <= d[1]))
    and ((p[2] >= -d[2]) and (p[2] <= d[2]));
end;

// MinFloat (TGeoFloat)
//
function MinFloat(values: PSingleArray; nbItems: Integer): TGeoFloat;
var
  i, k: Integer;
begin
  if nbItems > 0 then begin
      k := 0;
      for i := 1 to nbItems - 1 do
        if values^[i] < values^[k] then
            k := i;
      Result := values^[k];
    end
  else
      Result := 0;
end;

// MinFloat (double)
//
function MinFloat(values: PDoubleArray; nbItems: Integer): Double;
var
  i, k: Integer;
begin
  if nbItems > 0 then begin
      k := 0;
      for i := 1 to nbItems - 1 do
        if values^[i] < values^[k] then
            k := i;
      Result := values^[k];
    end
  else
      Result := 0;
end;

// MinFloat (array)
//
function MinFloat(const v: array of TGeoFloat): TGeoFloat;
var
  i: Integer;
begin
  if length(v) > 0 then begin
      Result := v[0];
      for i := 1 to high(v) do
        if v[i] < Result then
            Result := v[i];
    end
  else
      Result := 0;
end;

// MinFloat (TGeoFloat 2)
//
function MinFloat(const v1, v2: TGeoFloat): TGeoFloat;
begin
  if v1 < v2 then
      Result := v1
  else
      Result := v2;
end;

// MinFloat (double 2)
//
function MinFloat(const v1, v2: Double): Double;
begin
  if v1 < v2 then
      Result := v1
  else
      Result := v2;
end;

// MinFloat
//
function MinFloat(const v1, v2, v3: TGeoFloat): TGeoFloat;
begin
  if v1 <= v2 then
    if v1 <= v3 then
        Result := v1
    else if v3 <= v2 then
        Result := v3
    else
        Result := v2
  else if v2 <= v3 then
      Result := v2
  else if v3 <= v1 then
      Result := v3
  else
      Result := v1;
end;

// MinFloat (double)
//
function MinFloat(const v1, v2, v3: Double): Double;
begin
  if v1 <= v2 then
    if v1 <= v3 then
        Result := v1
    else if v3 <= v2 then
        Result := v3
    else
        Result := v2
  else if v2 <= v3 then
      Result := v2
  else if v3 <= v1 then
      Result := v3
  else
      Result := v1;
end;

// MaxFloat (TGeoFloat)
//
function MaxFloat(values: PSingleArray; nbItems: Integer): TGeoFloat; overload;
var
  i, k: Integer;
begin
  if nbItems > 0 then begin
      k := 0;
      for i := 1 to nbItems - 1 do
        if values^[i] > values^[k] then
            k := i;
      Result := values^[k];
    end
  else
      Result := 0;
end;

// MaxFloat (double)
//
function MaxFloat(values: PDoubleArray; nbItems: Integer): Double; overload;
var
  i, k: Integer;
begin
  if nbItems > 0 then begin
      k := 0;
      for i := 1 to nbItems - 1 do
        if values^[i] > values^[k] then
            k := i;
      Result := values^[k];
    end
  else
      Result := 0;
end;

// MaxFloat
//
function MaxFloat(const v: array of TGeoFloat): TGeoFloat;
var
  i: Integer;
begin
  if length(v) > 0 then begin
      Result := v[0];
      for i := 1 to high(v) do
        if v[i] > Result then
            Result := v[i];
    end
  else
      Result := 0;
end;

// MaxFloat
//
function MaxFloat(const v1, v2: TGeoFloat): TGeoFloat;
begin
  if v1 > v2 then
      Result := v1
  else
      Result := v2;
end;

// MaxFloat
//
function MaxFloat(const v1, v2: Double): Double;
begin
  if v1 > v2 then
      Result := v1
  else
      Result := v2;
end;

// MaxFloat
//
function MaxFloat(const v1, v2, v3: TGeoFloat): TGeoFloat;
begin
  if v1 >= v2 then
    if v1 >= v3 then
        Result := v1
    else if v3 >= v2 then
        Result := v3
    else
        Result := v2
  else if v2 >= v3 then
      Result := v2
  else if v3 >= v1 then
      Result := v3
  else
      Result := v1;
end;

// MaxFloat
//
function MaxFloat(const v1, v2, v3: Double): Double;
begin
  if v1 >= v2 then
    if v1 >= v3 then
        Result := v1
    else if v3 >= v2 then
        Result := v3
    else
        Result := v2
  else if v2 >= v3 then
      Result := v2
  else if v3 >= v1 then
      Result := v3
  else
      Result := v1;
end;

// MinInteger (2 int)
//
function MinInteger(const v1, v2: Integer): Integer;
begin
  if v1 < v2 then
      Result := v1
  else
      Result := v2;
end;

// MinInteger (2 card)
//
function MinInteger(const v1, v2: Cardinal): Cardinal;
begin
  if v1 < v2 then
      Result := v1
  else
      Result := v2;
end;

// MinInteger
//
function MinInteger(const v1, v2, v3: Integer): Integer;
begin
  if v1 <= v2 then
    if v1 <= v3 then
        Result := v1
    else if v3 <= v2 then
        Result := v3
    else
        Result := v2
  else if v2 <= v3 then
      Result := v2
  else if v3 <= v1 then
      Result := v3
  else
      Result := v1;
end;

// MinInteger
//
function MinInteger(const v1, v2, v3: Cardinal): Cardinal;
begin
  if v1 <= v2 then
    if v1 <= v3 then
        Result := v1
    else if v3 <= v2 then
        Result := v3
    else
        Result := v2
  else if v2 <= v3 then
      Result := v2
  else if v3 <= v1 then
      Result := v3
  else
      Result := v1;
end;

// MaxInteger (2 int)
//
function MaxInteger(const v1, v2: Integer): Integer;
begin
  if v1 > v2 then
      Result := v1
  else
      Result := v2;
end;

// MaxInteger (2 card)
//
function MaxInteger(const v1, v2: Cardinal): Cardinal;
begin
  if v1 > v2 then
      Result := v1
  else
      Result := v2;
end;

// MaxInteger
//
function MaxInteger(const v1, v2, v3: Integer): Integer;
begin
  if v1 >= v2 then
    if v1 >= v3 then
        Result := v1
    else if v3 >= v2 then
        Result := v3
    else
        Result := v2
  else if v2 >= v3 then
      Result := v2
  else if v3 >= v1 then
      Result := v3
  else
      Result := v1;
end;

// MaxInteger
//
function MaxInteger(const v1, v2, v3: Cardinal): Cardinal;
begin
  if v1 >= v2 then
    if v1 >= v3 then
        Result := v1
    else if v3 >= v2 then
        Result := v3
    else
        Result := v2
  else if v2 >= v3 then
      Result := v2
  else if v3 >= v1 then
      Result := v3
  else
      Result := v1;
end;

function ClampInteger(const Value, Min, Max: Integer): Integer;
begin
  Result := MinInteger(MaxInteger(Value, Min), Max);
end;

function ClampInteger(const Value, Min, Max: Cardinal): Cardinal;
begin
  Result := MinInteger(MaxInteger(Value, Min), Max);
end;

// TriangleArea
//
function TriangleArea(const p1, p2, p3: TAffineVector): TGeoFloat;
begin
  Result := 0.5 * VectorLength(VectorCrossProduct(VectorSubtract(p2, p1),
    VectorSubtract(p3, p1)));
end;

// PolygonArea
//
function PolygonArea(const p: PAffineVectorArray; nSides: Integer): TGeoFloat;
var
  r: TAffineVector;
  i: Integer;
  p1, p2, p3: PAffineVector;
begin
  Result := 0;
  if nSides > 2 then begin
      RstVector(r);
      p1 := @p^[0];
      p2 := @p^[1];
      for i := 2 to nSides - 1 do begin
          p3 := @p^[i];
          AddVector(r, VectorCrossProduct(VectorSubtract(p2^, p1^),
            VectorSubtract(p3^, p1^)));
          p2 := p3;
        end;
      Result := VectorLength(r) * 0.5;
    end;
end;

// TriangleSignedArea
//
function TriangleSignedArea(const p1, p2, p3: TAffineVector): TGeoFloat;
begin
  Result := 0.5 * ((p2[0] - p1[0]) * (p3[1] - p1[1])
    - (p3[0] - p1[0]) * (p2[1] - p1[1]));
end;

// PolygonSignedArea
//
function PolygonSignedArea(const p: PAffineVectorArray; nSides: Integer): TGeoFloat;
var
  i: Integer;
  p1, p2, p3: PAffineVector;
begin
  Result := 0;
  if nSides > 2 then begin
      p1 := @(p^[0]);
      p2 := @(p^[1]);
      for i := 2 to nSides - 1 do begin
          p3 := @(p^[i]);
          Result := Result + (p2^[0] - p1^[0]) * (p3^[1] - p1^[1])
            - (p3^[0] - p1^[0]) * (p2^[1] - p1^[1]);
          p2 := p3;
        end;
      Result := Result * 0.5;
    end;
end;

// ScaleFloatArray (raw)
//
procedure ScaleFloatArray(values: PSingleArray; nb: Integer;
  var factor: TGeoFloat);
var
  i: Integer;
begin
  for i := 0 to nb - 1 do
      values^[i] := values^[i] * factor;
end;

// ScaleFloatArray (array)
//
procedure ScaleFloatArray(var values: TSingleArray;
  factor: TGeoFloat);
begin
  if length(values) > 0 then
      ScaleFloatArray(@values[0], length(values), factor);
end;

// OffsetFloatArray (raw)
//
procedure OffsetFloatArray(values: PSingleArray; nb: Integer;
  var Delta: TGeoFloat);
var
  i: Integer;
begin
  for i := 0 to nb - 1 do
      values^[i] := values^[i] + Delta;
end;

// ScaleFloatArray (array)
//
procedure OffsetFloatArray(var values: array of TGeoFloat;
  Delta: TGeoFloat);
begin
  if length(values) > 0 then
      ScaleFloatArray(@values[0], length(values), Delta);
end;

// OffsetFloatArray (raw, raw)
//
procedure OffsetFloatArray(valuesDest, valuesDelta: PSingleArray; nb: Integer);
var
  i: Integer;
begin
  for i := 0 to nb - 1 do
      valuesDest^[i] := valuesDest^[i] + valuesDelta^[i];
end;

// MaxXYZComponent
//
function MaxXYZComponent(const v: TVector): TGeoFloat; overload;
begin
  Result := MaxFloat(v[0], v[1], v[2]);
end;

// MaxXYZComponent
//
function MaxXYZComponent(const v: TAffineVector): TGeoFloat; overload;
begin
  Result := MaxFloat(v[0], v[1], v[2]);
end;

// MinXYZComponent
//
function MinXYZComponent(const v: TVector): TGeoFloat; overload;
begin
  if v[0] <= v[1] then
    if v[0] <= v[2] then
        Result := v[0]
    else if v[2] <= v[1] then
        Result := v[2]
    else
        Result := v[1]
  else if v[1] <= v[2] then
      Result := v[1]
  else if v[2] <= v[0] then
      Result := v[2]
  else
      Result := v[0];
end;

// MinXYZComponent
//
function MinXYZComponent(const v: TAffineVector): TGeoFloat; overload;
begin
  Result := MinFloat(v[0], v[1], v[2]);
end;

// MaxAbsXYZComponent
//
function MaxAbsXYZComponent(v: TVector): TGeoFloat;
begin
  AbsVector(v);
  Result := MaxXYZComponent(v);
end;

// MinAbsXYZComponent
//
function MinAbsXYZComponent(v: TVector): TGeoFloat;
begin
  AbsVector(v);
  Result := MinXYZComponent(v);
end;

// MaxVector (hmg)
//
procedure MaxVector(var v: TVector; const v1: TVector);
begin
  if v1[0] > v[0] then
      v[0] := v1[0];
  if v1[1] > v[1] then
      v[1] := v1[1];
  if v1[2] > v[2] then
      v[2] := v1[2];
  if v1[3] > v[3] then
      v[3] := v1[3];
end;

// MaxVector (affine)
//
procedure MaxVector(var v: TAffineVector; const v1: TAffineVector); overload;
begin
  if v1[0] > v[0] then
      v[0] := v1[0];
  if v1[1] > v[1] then
      v[1] := v1[1];
  if v1[2] > v[2] then
      v[2] := v1[2];
end;

// MinVector (hmg)
//
procedure MinVector(var v: TVector; const v1: TVector);
begin
  if v1[0] < v[0] then
      v[0] := v1[0];
  if v1[1] < v[1] then
      v[1] := v1[1];
  if v1[2] < v[2] then
      v[2] := v1[2];
  if v1[3] < v[3] then
      v[3] := v1[3];
end;

// MinVector (affine)
//
procedure MinVector(var v: TAffineVector; const v1: TAffineVector);
begin
  if v1[0] < v[0] then
      v[0] := v1[0];
  if v1[1] < v[1] then
      v[1] := v1[1];
  if v1[2] < v[2] then
      v[2] := v1[2];
end;

// SortArrayAscending (extended)
//
procedure SortArrayAscending(var a: array of Extended);
var
  i, j, M: Integer;
  Buf: Extended;
begin
  for i := low(a) to high(a) - 1 do begin
      M := i;
      for j := i + 1 to high(a) do
        if a[j] < a[M] then
            M := j;
      if M <> i then begin
          Buf := a[M];
          a[M] := a[i];
          a[i] := Buf;
        end;
    end;
end;

// ClampValue (min-max)
//
function ClampValue(const AValue, aMin, aMax: TGeoFloat): TGeoFloat;
begin
  if aMin > aMax then
      Result := ClampValue(AValue, aMax, aMin)
  else if AValue > aMax then
      Result := aMax
  else if AValue < aMin then
      Result := aMin
  else
      Result := AValue;
end;

// ClampValue (min-)
//
function ClampValue(const AValue, aMin: TGeoFloat): TGeoFloat;
begin
  if AValue < aMin then
      Result := aMin
  else
      Result := AValue;
end;

// PointInPolygon
//
function PointInPolygon(var xp, yp: array of TGeoFloat; x, y: TGeoFloat): Boolean;
// The code below is from Wm. Randolph Franklin <wrf@ecse.rpi.edu>
// with some minor modifications for speed.  It returns 1 for strictly
// interior points, 0 for strictly exterior, and 0 or 1 for points on
// the boundary.
var
  i, j: Integer;
begin
  Result := False;
  if high(xp) = high(yp) then begin
      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;
        end;
    end;
end;

// DivMod
//
procedure DivMod(Dividend: Integer; Divisor: Word; var Result, Remainder: Word);
begin
  Result := Dividend div Divisor;
  Remainder := Dividend mod Divisor;
end;

// ConvertRotation
//
function ConvertRotation(const Angles: TAffineVector): TVector;

{ 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: TGeoFloat;
  i: Integer;
begin
  // see if we are only rotating about a TGeoFloat Axis
  if Abs(Angles[x]) < Epsilon then begin
      if Abs(Angles[y]) < Epsilon then begin
          SetVector(Result, 0, 0, 1, Angles[z]);
          Exit;
        end
      else if Abs(Angles[z]) < Epsilon then begin
          SetVector(Result, 0, 1, 0, Angles[y]);
          Exit;
        end
    end
  else if (Abs(Angles[y]) < Epsilon) and (Abs(Angles[z]) < Epsilon) then begin
      SetVector(Result, 1, 0, 0, Angles[x]);
      Exit;
    end;

  // make the rotation matrix
  Axis1 := XVector;
  M := CreateRotationMatrix(Axis1, Angles[x]);

  Axis2 := YVector;
  m2 := CreateRotationMatrix(Axis2, Angles[y]);
  m1 := MatrixMultiply(M, m2);

  Axis2 := ZVector;
  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
      SetVector(Result, XHmgVector);
      Exit;
    end;

  cost1 := 1 - cost;
  SetVector(Result, Sqrt((M[x, x] - cost) / cost1),
    Sqrt((M[y, y] - cost) / cost1),
    Sqrt((M[z, z] - cost) / cost1),
    GeometryLib.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;

// QuaternionSlerp
//
function QuaternionSlerp(const QStart, QEnd: TQuaternion; Spin: Integer; t: TGeoFloat): TQuaternion;
var
  beta,           // complementary interp parameter
  Theta,          // Angle between A and B
  sint, cost,     // sine, cosine of theta
  Phi: TGeoFloat; // theta plus spins
  bflip: Boolean; // use negativ t?
begin
  // cosine theta
  cost := VectorAngleCosine(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 := GeometryLib.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;

// QuaternionSlerp
//
function QuaternionSlerp(const Source, dest: TQuaternion; const t: TGeoFloat): TQuaternion;
var
  to1: array [0 .. 4] of TGeoFloat;
  omega, cosom, sinom, scale0, scale1: Extended;
  // t goes from 0 to 1
  // absolute rotations
begin
  // calc cosine
  cosom := Source.ImagPart[0] * dest.ImagPart[0]
    + Source.ImagPart[1] * dest.ImagPart[1]
    + Source.ImagPart[2] * dest.ImagPart[2]
    + Source.RealPart * dest.RealPart;
  // adjust signs (if necessary)
  if cosom < 0 then begin
      cosom := -cosom;
      to1[0] := -dest.ImagPart[0];
      to1[1] := -dest.ImagPart[1];
      to1[2] := -dest.ImagPart[2];
      to1[3] := -dest.RealPart;
    end
  else begin
      to1[0] := dest.ImagPart[0];
      to1[1] := dest.ImagPart[1];
      to1[2] := dest.ImagPart[2];
      to1[3] := dest.RealPart;
    end;
  // calculate coefficients
  if ((1.0 - cosom) > EPSILON2) then begin // standard case (slerp)
      omega := GeometryLib.ArcCos(cosom);
      sinom := 1 / Sin(omega);
      scale0 := Sin((1.0 - t) * omega) * sinom;
      scale1 := Sin(t * omega) * sinom;
    end
  else begin // "from" and "to" quaternions are very close
      // ... so we can do a linear interpolation
      scale0 := 1.0 - t;
      scale1 := t;
    end;
  // calculate final values
  Result.ImagPart[0] := scale0 * Source.ImagPart[0] + scale1 * to1[0];
  Result.ImagPart[1] := scale0 * Source.ImagPart[1] + scale1 * to1[1];
  Result.ImagPart[2] := scale0 * Source.ImagPart[2] + scale1 * to1[2];
  Result.RealPart := scale0 * Source.RealPart + scale1 * to1[3];
  NormalizeQuaternion(Result);
end;

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

// Turn (Y axis)
//
function Turn(const Matrix: TMatrix; angle: TGeoFloat): TMatrix;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(AffineVectorMake(Matrix[1][0], Matrix[1][1], Matrix[1][2]), angle));
end;

// Turn (direction)
//
function Turn(const Matrix: TMatrix; const MasterUp: TAffineVector; angle: TGeoFloat): TMatrix;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterUp, angle));
end;

// Pitch (X axis)
//
function Pitch(const Matrix: TMatrix; angle: TGeoFloat): TMatrix;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(AffineVectorMake(Matrix[0][0], Matrix[0][1], Matrix[0][2]), angle));
end;

// Pitch (direction)
//
function Pitch(const Matrix: TMatrix; const MasterRight: TAffineVector; angle: TGeoFloat): TMatrix; overload;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterRight, angle));
end;

// Roll (Z axis)
//
function Roll(const Matrix: TMatrix; angle: TGeoFloat): TMatrix;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(AffineVectorMake(Matrix[2][0], Matrix[2][1], Matrix[2][2]), angle));
end;

// Roll (direction)
//
function Roll(const Matrix: TMatrix; const MasterDirection: TAffineVector; angle: TGeoFloat): TMatrix; overload;
begin
  Result := MatrixMultiply(Matrix, CreateRotationMatrix(MasterDirection, angle));
end;

// RayCastPlaneIntersect (plane defined by point+normal)
//
function RayCastPlaneIntersect(const rayStart, rayVector: TVector;
  const planePoint, planeNormal: TVector;
  intersectPoint: PVector = nil): Boolean;
var
  sp: TVector;
  t, d: TGeoFloat;
begin
  d := VectorDotProduct(rayVector, planeNormal);
  Result := ((d > EPSILON2) or (d < -EPSILON2));
  if Result and Assigned(intersectPoint) then begin
      VectorSubtract(planePoint, rayStart, sp);
      d := 1 / d; // will keep one FPU unit busy during dot product calculation
      t := VectorDotProduct(sp, planeNormal) * d;
      if t > 0 then
          VectorCombine(rayStart, rayVector, t, intersectPoint^)
      else
          Result := False;
    end;
end;

// RayCastPlaneXZIntersect
//
function RayCastPlaneXZIntersect(const rayStart, rayVector: TVector;
  const planeY: TGeoFloat;
  intersectPoint: PVector = nil): Boolean;
var
  t: TGeoFloat;
begin
  if rayVector[1] = 0 then
      Result := False
  else begin
      t := (rayStart[1] - planeY) / rayVector[1];
      if t < 0 then begin
          if Assigned(intersectPoint) then
              VectorCombine(rayStart, rayVector, t, intersectPoint^);
          Result := True;
        end
      else
          Result := False;
    end;
end;

// RayCastTriangleIntersect
//
function RayCastTriangleIntersect(const rayStart, rayVector: TVector;
  const p1, p2, p3: TAffineVector;
  intersectPoint: PVector = nil;
  intersectNormal: PVector = nil): Boolean;
var
  pvec: TAffineVector;
  v1, v2, qvec, tvec: TVector;
  t, u, v, det, invDet: TGeoFloat;
begin
  VectorSubtract(p2, p1, v1);
  VectorSubtract(p3, p1, v2);
  VectorCrossProduct(rayVector, v2, pvec);
  det := VectorDotProduct(v1, pvec);
  if ((det < EPSILON2) and (det > -EPSILON2)) then begin // vector is Parallel to triangle's plane
      Result := False;
      Exit;
    end;
  invDet := cOne / det;
  VectorSubtract(rayStart, p1, tvec);
  u := VectorDotProduct(tvec, pvec) * invDet;
  if (u < 0) or (u > 1) then
      Result := False
  else begin
      qvec := VectorCrossProduct(tvec, v1);
      v := VectorDotProduct(rayVector, qvec) * invDet;
      Result := (v >= 0) and (u + v <= 1);
      if Result then begin
          t := VectorDotProduct(v2, qvec) * invDet;
          if t > 0 then begin
              if intersectPoint <> nil then
                  VectorCombine(rayStart, rayVector, t, intersectPoint^);
              if intersectNormal <> nil then
                  VectorCrossProduct(v1, v2, intersectNormal^);
            end
          else
              Result := False;
        end;
    end;
end;

// RayCastMinDistToPoint
//
function RayCastMinDistToPoint(const rayStart, rayVector: TVector;
  const Point: TVector): TGeoFloat;
var
  proj: TGeoFloat;
begin
  proj := PointProject(Point, rayStart, rayVector);
  if proj <= 0 then
      proj := 0; // rays don't go backward!
  Result := VectorDistance(Point, VectorCombine(rayStart, rayVector, 1, proj));
end;

// RayCastIntersectsSphere
//
function RayCastIntersectsSphere(const rayStart, rayVector: TVector;
  const sphereCenter: TVector;
  const SphereRadius: TGeoFloat): Boolean;
var
  proj: TGeoFloat;
begin
  proj := PointProject(sphereCenter, rayStart, rayVector);
  if proj <= 0 then
      proj := 0; // rays don't go backward!
  Result := (VectorDistance2(sphereCenter, VectorCombine(rayStart, rayVector, 1, proj)) <= Sqr(SphereRadius));
end;

// RayCastSphereIntersect
//
function RayCastSphereIntersect(const rayStart, rayVector: TVector;
  const sphereCenter: TVector;
  const SphereRadius: TGeoFloat;
  var i1, i2: TVector): Integer;
var
  proj, d2: TGeoFloat;
  id2: Integer;
  projPoint: TVector;
begin
  proj := PointProject(sphereCenter, rayStart, rayVector);
  VectorCombine(rayStart, rayVector, proj, projPoint);
  d2 := SphereRadius * SphereRadius - VectorDistance2(sphereCenter, projPoint);
  id2 := PInteger(@d2)^;
  if id2 >= 0 then begin
      if id2 = 0 then begin
          if PInteger(@proj)^ > 0 then begin
              VectorCombine(rayStart, rayVector, proj, i1);
              Result := 1;
              Exit;
            end;
        end
      else if id2 > 0 then begin
          d2 := Sqrt(d2);
          if proj >= d2 then begin
              VectorCombine(rayStart, rayVector, proj - d2, i1);
              VectorCombine(rayStart, rayVector, proj + d2, i2);
              Result := 2;
              Exit;
            end
          else if proj + d2 >= 0 then begin
              VectorCombine(rayStart, rayVector, proj + d2, i1);
              Result := 1;
              Exit;
            end;
        end;
    end;
  Result := 0;
end;

// RayCastBoxIntersect
//
function RayCastBoxIntersect(
  const rayStart, rayVector, aMinExtent, aMaxExtent: TAffineVector;
  intersectPoint: PAffineVector = nil): Boolean;
var
  i, planeInd: Integer;
  ResAFV, MaxDist, plane: TAffineVector;
  isMiddle: array [0 .. 2] of Boolean;
begin
  // Find plane.
  Result := True;
  for i := 0 to 2 do
    if rayStart[i] < aMinExtent[i] then begin
        plane[i] := aMinExtent[i];
        isMiddle[i] := False;
        Result := False;
      end
    else if rayStart[i] > aMaxExtent[i] then begin
        plane[i] := aMaxExtent[i];
        isMiddle[i] := False;
        Result := False;
      end
    else begin
        isMiddle[i] := True;
      end;
  if Result then begin
      // rayStart inside box.
      if intersectPoint <> nil
      then
          intersectPoint^ := rayStart;
    end
  else begin
      // Distance to plane.
      planeInd := 0;
      for i := 0 to 2 do
        if isMiddle[i]
          or (rayVector[i] = 0)
        then
            MaxDist[i] := -1
        else begin
            MaxDist[i] := (plane[i] - rayStart[i]) / rayVector[i];
            if MaxDist[i] > 0 then begin
                if MaxDist[planeInd] < MaxDist[i]
                then
                    planeInd := i;
                Result := True;
              end;
          end;
      // Inside box ?
      if Result then begin
          for i := 0 to 2 do
            if planeInd = i
            then
                ResAFV[i] := plane[i]
            else begin
                ResAFV[i] := rayStart[i] + MaxDist[planeInd] * rayVector[i];
                Result := (ResAFV[i] >= aMinExtent[i])
                  and (ResAFV[i] <= aMaxExtent[i]);
                if not Result then
                    Exit;
              end;
          if intersectPoint <> nil
          then
              intersectPoint^ := ResAFV;
        end;
    end;
end;

// SphereVisibleRadius
//
function SphereVisibleRadius(Distance, radius: TGeoFloat): TGeoFloat;
var
  d2, r2, IR, tr: TGeoFloat;
begin
  d2 := Distance * Distance;
  r2 := radius * radius;
  IR := Sqrt(d2 - r2);
  tr := (d2 + r2 - Sqr(IR)) / (2 * IR);

  Result := Sqrt(r2 + Sqr(tr));
end;

// IntersectLinePlane
//
function IntersectLinePlane(const Point, direction: TVector;
  const plane: THmgPlane;
  intersectPoint: PVector = nil): Integer;
var
  a, b: Extended;
  t: TGeoFloat;
begin
  a := VectorDotProduct(plane, direction); // direction projected to plane normal
  b := PlaneEvaluatePoint(plane, Point);   // distance to plane
  if a = 0 then begin                      // direction is Parallel to plane
      if b = 0 then
          Result := -1 // line is inside plane
      else
          Result := 0; // line is outside plane
    end
  else begin
      if Assigned(intersectPoint) then begin
          t := -b / a; // parameter of intersection
          intersectPoint^ := Point;
          // calculate intersection = p + t*d
          CombineVector(intersectPoint^, direction, t);
        end;
      Result := 1;
    end;
end;

// TriangleBoxIntersect
//
function IntersectTriangleBox(
  const p1, p2, p3, aMinExtent, aMaxExtent: TAffineVector): Boolean;
var
  RayDir, iPoint: TAffineVector;
  BoxDiagPt, BoxDiagPt2,
    BoxDiagDir, iPnt: TVector;
begin
  // Triangle edge (p2, p1) - Box intersection
  VectorSubtract(p2, p1, RayDir);
  Result := RayCastBoxIntersect(p1, RayDir, aMinExtent, aMaxExtent, @iPoint);
  if Result then
      Result := VectorNorm(VectorSubtract(p1, iPoint))
      < VectorNorm(VectorSubtract(p1, p2));
  if Result then
      Exit;

  // Triangle edge (p3, p1) - Box intersection
  VectorSubtract(p3, p1, RayDir);
  Result := RayCastBoxIntersect(p1, RayDir, aMinExtent, aMaxExtent, @iPoint);
  if Result then
      Result := VectorNorm(VectorSubtract(p1, iPoint))
      < VectorNorm(VectorSubtract(p1, p3));
  if Result then
      Exit;

  // Triangle edge (p2, p3) - Box intersection
  VectorSubtract(p2, p3, RayDir);
  Result := RayCastBoxIntersect(p3, RayDir, aMinExtent, aMaxExtent, @iPoint);
  if Result then
      Result := VectorNorm(VectorSubtract(p3, iPoint))
      < VectorNorm(VectorSubtract(p3, p2));
  if Result then
      Exit;

  // Triangle - Box diagonal 1 intersection
  BoxDiagPt := VectorMake(aMinExtent);
  VectorSubtract(aMaxExtent, aMinExtent, BoxDiagDir);
  Result := RayCastTriangleIntersect(BoxDiagPt, BoxDiagDir, p1, p2, p3, @iPnt);
  if Result then
      Result := VectorNorm(VectorSubtract(BoxDiagPt, iPnt))
      < VectorNorm(VectorSubtract(aMaxExtent, aMinExtent));
  if Result then
      Exit;

  // Triangle - Box diagonal 2 intersection
  BoxDiagPt := VectorMake(aMinExtent[0], aMinExtent[1], aMaxExtent[2]);
  BoxDiagPt2 := VectorMake(aMaxExtent[0], aMaxExtent[1], aMinExtent[2]);
  VectorSubtract(BoxDiagPt2, BoxDiagPt, BoxDiagDir);
  Result := RayCastTriangleIntersect(BoxDiagPt, BoxDiagDir, p1, p2, p3, @iPnt);
  if Result then
      Result := VectorNorm(VectorSubtract(BoxDiagPt, iPnt))
      < VectorNorm(VectorSubtract(BoxDiagPt, BoxDiagPt2));
  if Result then
      Exit;

  // Triangle - Box diagonal 3 intersection
  BoxDiagPt := VectorMake(aMinExtent[0], aMaxExtent[1], aMinExtent[2]);
  BoxDiagPt2 := VectorMake(aMaxExtent[0], aMinExtent[1], aMaxExtent[2]);
  VectorSubtract(BoxDiagPt, BoxDiagPt, BoxDiagDir);
  Result := RayCastTriangleIntersect(BoxDiagPt, BoxDiagDir, p1, p2, p3, @iPnt);
  if Result then
      Result := VectorLength(VectorSubtract(BoxDiagPt, iPnt))
      < VectorLength(VectorSubtract(BoxDiagPt, BoxDiagPt));
  if Result then
      Exit;

  // Triangle - Box diagonal 4 intersection
  BoxDiagPt := VectorMake(aMaxExtent[0], aMinExtent[1], aMinExtent[2]);
  BoxDiagPt2 := VectorMake(aMinExtent[0], aMaxExtent[1], aMaxExtent[2]);
  VectorSubtract(BoxDiagPt, BoxDiagPt, BoxDiagDir);
  Result := RayCastTriangleIntersect(BoxDiagPt, BoxDiagDir, p1, p2, p3, @iPnt);
  if Result then
      Result := VectorLength(VectorSubtract(BoxDiagPt, iPnt))
      < VectorLength(VectorSubtract(BoxDiagPt, BoxDiagPt));
end;

// IntersectSphereBox
//
function IntersectSphereBox(
  const SpherePos: TVector;
  const SphereRadius: TGeoFloat;
  const BoxMatrix: TMatrix; // Up Direction and Right must be normalized!
  // Use CubDepht, CubeHeight and CubeWidth
  // for scale TGLCube.
  const BoxScale: TAffineVector
  ; intersectPoint: PAffineVector = nil
  ; normal: PAffineVector = nil
  ; Depth: PSingle = nil
  ): Boolean;

  function dDOTByColumn(const v: TAffineVector; const M: TMatrix;
    const aColumn: Integer): TGeoFloat;
  begin
    Result := v[0] * M[0, aColumn]
      + v[1] * M[1, aColumn]
      + v[2] * M[2, aColumn];
  end;

  function dDotByRow(const v: TAffineVector;
    const M: TMatrix; const aRow: Integer): TGeoFloat;
  begin
    // Equal with: Result := VectorDotProduct(v, AffineVectorMake(m[aRow]));
    Result := v[0] * M[aRow, 0]
      + v[1] * M[aRow, 1]
      + v[2] * M[aRow, 2];
  end;

  function dDotMatrByColumn(const v: TAffineVector;
    const M: TMatrix): TAffineVector;
  begin
    Result[0] := dDOTByColumn(v, M, 0);
    Result[1] := dDOTByColumn(v, M, 1);
    Result[2] := dDOTByColumn(v, M, 2);
  end;

  function dDotMatrByRow(const v: TAffineVector;
    const M: TMatrix): TAffineVector;
  begin
    Result[0] := dDotByRow(v, M, 0);
    Result[1] := dDotByRow(v, M, 1);
    Result[2] := dDotByRow(v, M, 2);
  end;

var
  tmp, L, t, p, q, r: TAffineVector;
  FaceDistance,
    MinDistance, Depth1: TGeoFloat;
  mini, i: Integer;
  isSphereCenterInsideBox: Boolean;
begin
  // this is easy. get the sphere center `p' relative to the box, and then clip
  // that to the boundary of the box (call that point `q'). if q is on the
  // boundary of the box and |p-q| is <= sphere radius, they touch.
  // if q is inside the box, the sphere is inside the box, so set a contact
  // normal to push the sphere to the closest box face.

  p[0] := SpherePos[0] - BoxMatrix[3, 0];
  p[1] := SpherePos[1] - BoxMatrix[3, 1];
  p[2] := SpherePos[2] - BoxMatrix[3, 2];

  isSphereCenterInsideBox := True;
  for i := 0 to 2 do begin
      L[i] := 0.5 * BoxScale[i];
      t[i] := dDotByRow(p, BoxMatrix, i);
      if t[i] < -L[i] then begin
          t[i] := -L[i];
          isSphereCenterInsideBox := False;
        end
      else if t[i] > L[i] then begin
          t[i] := L[i];
          isSphereCenterInsideBox := False;
        end;
    end;

  if isSphereCenterInsideBox then begin

      MinDistance := L[0] - Abs(t[0]);
      mini := 0;
      for i := 1 to 2 do begin
          FaceDistance := L[i] - Abs(t[i]);
          if FaceDistance < MinDistance then begin
              MinDistance := FaceDistance;
              mini := i;
            end;
        end;

      if intersectPoint <> nil then
          intersectPoint^ := AffineVectorMake(SpherePos);

      if normal <> nil then begin
          tmp := NullVector;
          if t[mini] > 0 then
              tmp[mini] := 1
          else
              tmp[mini] := -1;
          normal^ := dDotMatrByRow(tmp, BoxMatrix);
        end;

      if Depth <> nil then
          Depth^ := MinDistance + SphereRadius;

      Result := True;
    end
  else begin
      q := dDotMatrByColumn(t, BoxMatrix);
      r := VectorSubtract(p, q);
      Depth1 := SphereRadius - VectorLength(r);
      if Depth1 < 0 then begin
          Result := False;
        end
      else begin
          if intersectPoint <> nil then
              intersectPoint^ := VectorAdd(q, AffineVectorMake(BoxMatrix[3]));
          if normal <> nil then begin
              normal^ := VectorNormalize(r);
            end;
          if Depth <> nil then
              Depth^ := Depth1;
          Result := True;
        end;
    end;
end;

// ExtractFrustumFromModelViewProjection
//
function ExtractFrustumFromModelViewProjection(const modelViewProj: TMatrix): TFrustum;
begin
  with Result do begin
      // extract left plane
      pLeft[0] := modelViewProj[0][3] + modelViewProj[0][0];
      pLeft[1] := modelViewProj[1][3] + modelViewProj[1][0];
      pLeft[2] := modelViewProj[2][3] + modelViewProj[2][0];
      pLeft[3] := modelViewProj[3][3] + modelViewProj[3][0];
      NormalizePlane(pLeft);
      // extract top plane
      pTop[0] := modelViewProj[0][3] - modelViewProj[0][1];
      pTop[1] := modelViewProj[1][3] - modelViewProj[1][1];
      pTop[2] := modelViewProj[2][3] - modelViewProj[2][1];
      pTop[3] := modelViewProj[3][3] - modelViewProj[3][1];
      NormalizePlane(pTop);
      // extract right plane
      pRight[0] := modelViewProj[0][3] - modelViewProj[0][0];
      pRight[1] := modelViewProj[1][3] - modelViewProj[1][0];
      pRight[2] := modelViewProj[2][3] - modelViewProj[2][0];
      pRight[3] := modelViewProj[3][3] - modelViewProj[3][0];
      NormalizePlane(pRight);
      // extract bottom plane
      pBottom[0] := modelViewProj[0][3] + modelViewProj[0][1];
      pBottom[1] := modelViewProj[1][3] + modelViewProj[1][1];
      pBottom[2] := modelViewProj[2][3] + modelViewProj[2][1];
      pBottom[3] := modelViewProj[3][3] + modelViewProj[3][1];
      NormalizePlane(pBottom);
      // extract far plane
      pFar[0] := modelViewProj[0][3] - modelViewProj[0][2];
      pFar[1] := modelViewProj[1][3] - modelViewProj[1][2];
      pFar[2] := modelViewProj[2][3] - modelViewProj[2][2];
      pFar[3] := modelViewProj[3][3] - modelViewProj[3][2];
      NormalizePlane(pFar);
      // extract near plane
      pNear[0] := modelViewProj[0][3] + modelViewProj[0][2];
      pNear[1] := modelViewProj[1][3] + modelViewProj[1][2];
      pNear[2] := modelViewProj[2][3] + modelViewProj[2][2];
      pNear[3] := modelViewProj[3][3] + modelViewProj[3][2];
      NormalizePlane(pNear);
    end;
end;

// IsVolumeClipped
//
function IsVolumeClipped(const objPos: TAffineVector; const objRadius: TGeoFloat;
  const Frustum: TFrustum): Boolean;
var
  negRadius: TGeoFloat;
begin
  negRadius := -objRadius;
  Result := (PlaneEvaluatePoint(Frustum.pLeft, objPos) < negRadius)
    or (PlaneEvaluatePoint(Frustum.pTop, objPos) < negRadius)
    or (PlaneEvaluatePoint(Frustum.pRight, objPos) < negRadius)
    or (PlaneEvaluatePoint(Frustum.pBottom, objPos) < negRadius)
    or (PlaneEvaluatePoint(Frustum.pNear, objPos) < negRadius)
    or (PlaneEvaluatePoint(Frustum.pFar, objPos) < negRadius);
end;

// IsVolumeClipped
//
function IsVolumeClipped(const objPos: TVector; const objRadius: TGeoFloat;
  const Frustum: TFrustum): Boolean;
begin
  Result := IsVolumeClipped(PAffineVector(@objPos)^, objRadius, Frustum);
end;

// IsVolumeClipped
//
function IsVolumeClipped(const Min, Max: TAffineVector;
  const Frustum: TFrustum): Boolean;
begin
  // change box to sphere
  Result := IsVolumeClipped(VectorScale(VectorAdd(Min, Max), 0.5),
    VectorDistance(Min, Max) * 0.5, Frustum);
end;

// MakeParallelProjectionMatrix
//
function MakeParallelProjectionMatrix(const plane: THmgPlane;
  const dir: TVector): TMatrix;
// Based on material from a course by William D. Shoaff (www.cs.fit.edu)
var
  dot, invDot: TGeoFloat;
begin
  dot := plane[0] * dir[0] + plane[1] * dir[1] + plane[2] * dir[2];
  if Abs(dot) < 1E-5 then begin
      Result := IdentityHmgMatrix;
      Exit;
    end;
  invDot := 1 / dot;

  Result[0][0] := (plane[1] * dir[1] + plane[2] * dir[2]) * invDot;
  Result[1][0] := (-plane[1] * dir[0]) * invDot;
  Result[2][0] := (-plane[2] * dir[0]) * invDot;
  Result[3][0] := (-plane[3] * dir[0]) * invDot;

  Result[0][1] := (-plane[0] * dir[1]) * invDot;
  Result[1][1] := (plane[0] * dir[0] + plane[2] * dir[2]) * invDot;
  Result[2][1] := (-plane[2] * dir[1]) * invDot;
  Result[3][1] := (-plane[3] * dir[1]) * invDot;

  Result[0][2] := (-plane[0] * dir[2]) * invDot;
  Result[1][2] := (-plane[1] * dir[2]) * invDot;
  Result[2][2] := (plane[0] * dir[0] + plane[1] * dir[1]) * invDot;
  Result[3][2] := (-plane[3] * dir[2]) * invDot;

  Result[0][3] := 0;
  Result[1][3] := 0;
  Result[2][3] := 0;
  Result[3][3] := 1;
end;

// MakeShadowMatrix
//
function MakeShadowMatrix(const planePoint, planeNormal, lightPos: TVector): TMatrix;
var
  planeNormal3, dot: TGeoFloat;
begin
  // Find the last coefficient by back substitutions
  planeNormal3 := -(planeNormal[0] * planePoint[0]
    + planeNormal[1] * planePoint[1]
    + planeNormal[2] * planePoint[2]);
  // Dot product of plane and light position
  dot := planeNormal[0] * lightPos[0]
    + planeNormal[1] * lightPos[1]
    + planeNormal[2] * lightPos[2]
    + planeNormal3 * lightPos[3];
  // Now do the projection
  // First column
  Result[0][0] := dot - lightPos[0] * planeNormal[0];
  Result[1][0] := -lightPos[0] * planeNormal[1];
  Result[2][0] := -lightPos[0] * planeNormal[2];
  Result[3][0] := -lightPos[0] * planeNormal3;
  // Second column
  Result[0][1] := -lightPos[1] * planeNormal[0];
  Result[1][1] := dot - lightPos[1] * planeNormal[1];
  Result[2][1] := -lightPos[1] * planeNormal[2];
  Result[3][1] := -lightPos[1] * planeNormal3;
  // Third Column
  Result[0][2] := -lightPos[2] * planeNormal[0];
  Result[1][2] := -lightPos[2] * planeNormal[1];
  Result[2][2] := dot - lightPos[2] * planeNormal[2];
  Result[3][2] := -lightPos[2] * planeNormal3;
  // Fourth Column
  Result[0][3] := -lightPos[3] * planeNormal[0];
  Result[1][3] := -lightPos[3] * planeNormal[1];
  Result[2][3] := -lightPos[3] * planeNormal[2];
  Result[3][3] := dot - lightPos[3] * planeNormal3;
end;

// MakeReflectionMatrix
//
function MakeReflectionMatrix(const planePoint, planeNormal: TAffineVector): TMatrix;
var
  pv2: TGeoFloat;
begin
  // Precalcs
  pv2 := 2 * VectorDotProduct(planePoint, planeNormal);
  // 1st column
  Result[0][0] := 1 - 2 * Sqr(planeNormal[0]);
  Result[0][1] := -2 * planeNormal[0] * planeNormal[1];
  Result[0][2] := -2 * planeNormal[0] * planeNormal[2];
  Result[0][3] := 0;
  // 2nd column
  Result[1][0] := -2 * planeNormal[1] * planeNormal[0];
  Result[1][1] := 1 - 2 * Sqr(planeNormal[1]);
  Result[1][2] := -2 * planeNormal[1] * planeNormal[2];
  Result[1][3] := 0;
  // 3rd column
  Result[2][0] := -2 * planeNormal[2] * planeNormal[0];
  Result[2][1] := -2 * planeNormal[2] * planeNormal[1];
  Result[2][2] := 1 - 2 * Sqr(planeNormal[2]);
  Result[2][3] := 0;
  // 4th column
  Result[3][0] := pv2 * planeNormal[0];
  Result[3][1] := pv2 * planeNormal[1];
  Result[3][2] := pv2 * planeNormal[2];
  Result[3][3] := 1;
end;

// PackRotationMatrix
//
function PackRotationMatrix(const mat: TMatrix): TPackedRotationMatrix;
var
  q: TQuaternion;
const
  cFact: TGeoFloat = 32767;
begin
  q := QuaternionFromMatrix(mat);
  NormalizeQuaternion(q);
  if q.RealPart < 0 then begin
      Result[0] := Round(-q.ImagPart[0] * cFact);
      Result[1] := Round(-q.ImagPart[1] * cFact);
      Result[2] := Round(-q.ImagPart[2] * cFact);
    end
  else begin
      Result[0] := Round(q.ImagPart[0] * cFact);
      Result[1] := Round(q.ImagPart[1] * cFact);
      Result[2] := Round(q.ImagPart[2] * cFact);
    end;
end;

// UnPackRotationMatrix
//
function UnPackRotationMatrix(const packedMatrix: TPackedRotationMatrix): TMatrix;
var
  q: TQuaternion;
const
  cFact: TGeoFloat = 1 / 32767;
begin
  q.ImagPart[0] := packedMatrix[0] * cFact;
  q.ImagPart[1] := packedMatrix[1] * cFact;
  q.ImagPart[2] := packedMatrix[2] * cFact;
  q.RealPart := 1 - VectorNorm(q.ImagPart);
  if q.RealPart < 0 then
      q.RealPart := 0
  else
      q.RealPart := Sqrt(q.RealPart);
  Result := QuaternionToMatrix(q);
end;

// BarycentricCoordinates
//
function BarycentricCoordinates(const v1, v2, v3, p: TAffineVector; var u, v: TGeoFloat): Boolean;
var
  a1, a2: Integer;
  n, e1, e2, pt: TAffineVector;
begin
  // calculate edges
  VectorSubtract(v1, v3, e1);
  VectorSubtract(v2, v3, e2);

  // calculate p relative to v3
  VectorSubtract(p, v3, pt);

  // find the dominant axis
  n := VectorCrossProduct(e1, e2);
  AbsVector(n);
  a1 := 0;
  if n[1] > n[a1] then
      a1 := 1;
  if n[2] > n[a1] then
      a1 := 2;

  // use dominant axis for projection
  case a1 of
    0: begin
        a1 := 1;
        a2 := 2;
      end;
    1: begin
        a1 := 0;
        a2 := 2;
      end;
    else // 2:
      a1 := 0;
      a2 := 1;
  end;

  // solve for u and v
  u := (pt[a2] * e2[a1] - pt[a1] * e2[a2]) / (e1[a2] * e2[a1] - e1[a1] * e2[a2]);
  v := (pt[a2] * e1[a1] - pt[a1] * e1[a2]) / (e2[a2] * e1[a1] - e2[a1] * e1[a2]);

  Result := (u >= 0) and (v >= 0) and (u + v <= 1);
end;

{ ***************************************************************************** }

// VectorMake functions
// 2x
function Vector2fMake(const x, y: TGeoFloat): TVector2f;
begin
  Result[0] := x;
  Result[1] := y;
end;


// **************

function Vector2fMake(const Vector: TVector3f): TVector2f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
end;

// **********

function Vector2fMake(const Vector: TVector4f): TVector2f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
end;

{ ***************************************************************************** }

// 3x
function Vector3fMake(const x, y, z: TGeoFloat): TVector3f;
begin
  Result[0] := x;
  Result[1] := y;
  Result[2] := z;
end;

// *******

function Vector3fMake(const Vector: TVector2f; const z: TGeoFloat): TVector3f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
  Result[2] := z;
end;

// *******

function Vector3fMake(const Vector: TVector4f): TVector3f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
  Result[2] := Vector[2];
end;

{ ***************************************************************************** }

// 4x
function Vector4fMake(const x, y, z, w: TGeoFloat): TVector4f;
begin
  Result[0] := x;
  Result[1] := y;
  Result[2] := z;
  Result[3] := w;
end;

// ********

function Vector4fMake(const Vector: TVector3f; const w: TGeoFloat): TVector4f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
  Result[2] := Vector[2];
  Result[3] := w;
end;

// *******

function Vector4fMake(const Vector: TVector2f; const z: TGeoFloat; const w: TGeoFloat): TVector4f;
begin
  Result[0] := Vector[0];
  Result[1] := Vector[1];
  Result[2] := z;
  Result[3] := w;
end;

{ ***************************************************************************** }

// 2
function VectorEquals(const v1, v2: TVector2f): Boolean;
begin
  Result := (v1[0] = v2[0]) and (v1[1] = v2[1]);
end;

{ ***************************************************************************** }

// 3x3f
function MatrixEquals(const Matrix1, Matrix2: TMatrix3f): Boolean;
begin
  Result := VectorEquals(Matrix1[0], Matrix2[0]) and
    VectorEquals(Matrix1[1], Matrix2[1]) and
    VectorEquals(Matrix1[2], Matrix2[2]);
end;

// 3x3i
{ ***************************************************************************** }

// 4x4f
function MatrixEquals(const Matrix1, Matrix2: TMatrix4f): Boolean;
begin
  Result := VectorEquals(Matrix1[0], Matrix2[0]) and
    VectorEquals(Matrix1[1], Matrix2[1]) and
    VectorEquals(Matrix1[2], Matrix2[2]) and
    VectorEquals(Matrix1[3], Matrix2[3]);
end;

{ ***************************************************************************** }

// Vector comparison functions:
// 3f
function VectorMoreThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
begin
  Result := (SourceVector[0] > ComparedVector[0]) and
    (SourceVector[1] > ComparedVector[1]) and
    (SourceVector[2] > ComparedVector[2]);
end;

function VectorMoreEqualThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
begin
  Result := (SourceVector[0] >= ComparedVector[0]) and
    (SourceVector[1] >= ComparedVector[1]) and
    (SourceVector[2] >= ComparedVector[2]);
end;

function VectorLessThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
begin
  Result := (SourceVector[0] < ComparedVector[0]) and
    (SourceVector[1] < ComparedVector[1]) and
    (SourceVector[2] < ComparedVector[2]);
end;

function VectorLessEqualThen(const SourceVector, ComparedVector: TVector3f): Boolean; overload;
begin
  Result := (SourceVector[0] <= ComparedVector[0]) and
    (SourceVector[1] <= ComparedVector[1]) and
    (SourceVector[2] <= ComparedVector[2]);
end;

// 4f
function VectorMoreThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
begin
  Result := (SourceVector[0] > ComparedVector[0]) and
    (SourceVector[1] > ComparedVector[1]) and
    (SourceVector[2] > ComparedVector[2]) and
    (SourceVector[3] > ComparedVector[3]);
end;

function VectorMoreEqualThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
begin
  Result := (SourceVector[0] >= ComparedVector[0]) and
    (SourceVector[1] >= ComparedVector[1]) and
    (SourceVector[2] >= ComparedVector[2]) and
    (SourceVector[3] >= ComparedVector[3]);
end;

function VectorLessThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
begin
  Result := (SourceVector[0] < ComparedVector[0]) and
    (SourceVector[1] < ComparedVector[1]) and
    (SourceVector[2] < ComparedVector[2]) and
    (SourceVector[3] < ComparedVector[3]);
end;

function VectorLessEqualThen(const SourceVector, ComparedVector: TVector4f): Boolean; overload;
begin
  Result := (SourceVector[0] <= ComparedVector[0]) and
    (SourceVector[1] <= ComparedVector[1]) and
    (SourceVector[2] <= ComparedVector[2]) and
    (SourceVector[3] <= ComparedVector[3]);
end;

// ComparedNumber
// 3f
function VectorMoreThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] > ComparedNumber) and
    (SourceVector[1] > ComparedNumber) and
    (SourceVector[2] > ComparedNumber);
end;

function VectorMoreEqualThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] >= ComparedNumber) and
    (SourceVector[1] >= ComparedNumber) and
    (SourceVector[2] >= ComparedNumber);
end;

function VectorLessThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] < ComparedNumber) and
    (SourceVector[1] < ComparedNumber) and
    (SourceVector[2] < ComparedNumber);
end;

function VectorLessEqualThen(const SourceVector: TVector3f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] <= ComparedNumber) and
    (SourceVector[1] <= ComparedNumber) and
    (SourceVector[2] <= ComparedNumber);
end;

// 4f
function VectorMoreThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] > ComparedNumber) and
    (SourceVector[1] > ComparedNumber) and
    (SourceVector[2] > ComparedNumber) and
    (SourceVector[3] > ComparedNumber);
end;

function VectorMoreEqualThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] >= ComparedNumber) and
    (SourceVector[1] >= ComparedNumber) and
    (SourceVector[2] >= ComparedNumber) and
    (SourceVector[3] >= ComparedNumber);
end;

function VectorLessThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] < ComparedNumber) and
    (SourceVector[1] < ComparedNumber) and
    (SourceVector[2] < ComparedNumber) and
    (SourceVector[3] < ComparedNumber);
end;

function VectorLessEqualThen(const SourceVector: TVector4f; const ComparedNumber: TGeoFloat): Boolean; overload;
begin
  Result := (SourceVector[0] <= ComparedNumber) and
    (SourceVector[1] <= ComparedNumber) and
    (SourceVector[2] <= ComparedNumber) and
    (SourceVector[3] <= ComparedNumber);
end;

{ Determine if 2 rectanges intersect. }
function RectanglesIntersect(const ACenterOfRect1, ACenterOfRect2, ASizeOfRect1, ASizeOfRect2: TVector2f): Boolean;
begin
  Result := (Abs(ACenterOfRect1[0] - ACenterOfRect2[0]) < (ASizeOfRect1[0] + ASizeOfRect2[0]) / 2) and
    (Abs(ACenterOfRect1[1] - ACenterOfRect2[1]) < (ASizeOfRect1[1] + ASizeOfRect2[1]) / 2);
end;

{ Determine if BigRect completely contains SmallRect. }
function RectangleContains(const ACenterOfBigRect1, ACenterOfSmallRect2,
  ASizeOfBigRect1, ASizeOfSmallRect2: TVector2f; const AEps: TGeoFloat = 0.0): Boolean;
begin
  Result := (Abs(ACenterOfBigRect1[0] - ACenterOfSmallRect2[0]) + ASizeOfSmallRect2[0] / 2 - ASizeOfBigRect1[0] / 2 < AEps) and
    (Abs(ACenterOfBigRect1[1] - ACenterOfSmallRect2[1]) + ASizeOfSmallRect2[1] / 2 - ASizeOfBigRect1[1] / 2 < AEps);
end;

function MoveObjectAround(const AMovingObjectPosition, AMovingObjectUp, ATargetPosition: TVector;
  pitchDelta, turnDelta: TGeoFloat): TVector;
var
  originalT2C, normalT2C, normalCameraRight: TVector;
  pitchNow, Dist: TGeoFloat;
begin
  // normalT2C points away from the direction the camera is looking
  originalT2C := VectorSubtract(AMovingObjectPosition,
    ATargetPosition);
  SetVector(normalT2C, originalT2C);
  Dist := VectorLength(normalT2C);
  NormalizeVector(normalT2C);
  // normalRight points to the camera's right
  // the camera is pitching around this axis.
  normalCameraRight := VectorCrossProduct(AMovingObjectUp, normalT2C);
  if VectorLength(normalCameraRight) < 0.001 then
      SetVector(normalCameraRight, XVector) // arbitrary vector
  else
      NormalizeVector(normalCameraRight);
  // calculate the current pitch.
  // 0 is looking down and PI is looking up
  pitchNow := GeometryLib.ArcCos(VectorDotProduct(AMovingObjectUp, normalT2C));
  pitchNow := ClampValue(pitchNow + GeometryLib.DegToRad(pitchDelta), 0 + 0.025, pi -
    0.025);
  // create a new vector pointing up and then rotate it down
  // into the new position
  SetVector(normalT2C, AMovingObjectUp);
  RotateVector(normalT2C, normalCameraRight, -pitchNow);
  RotateVector(normalT2C, AMovingObjectUp, -GeometryLib.DegToRad(turnDelta));
  ScaleVector(normalT2C, Dist);
  Result := VectorAdd(AMovingObjectPosition, VectorSubtract(normalT2C,
    originalT2C));
end;

{ Calcualtes Angle between 2 Vectors: (A-CenterPoint) and (B-CenterPoint). In radians. }
function AngleBetweenVectors(const a, b, ACenterPoint: TVector): TGeoFloat;
begin
  Result := GeometryLib.ArcCos(VectorAngleCosine(
    VectorNormalize(VectorSubtract(a, ACenterPoint)),
    VectorNormalize(VectorSubtract(b, ACenterPoint))));
end;

{ Calcualtes Angle between 2 Vectors: (A-CenterPoint) and (B-CenterPoint). In radians. }
function AngleBetweenVectors(const a, b, ACenterPoint: TAffineVector): TGeoFloat;
begin
  Result := GeometryLib.ArcCos(VectorAngleCosine(
    VectorNormalize(VectorSubtract(a, ACenterPoint)),
    VectorNormalize(VectorSubtract(b, ACenterPoint))));
end;

{ AOriginalPosition - Object initial position.
  ACenter - some point, from which is should be distanced.

  ADistance + AFromCenterSpot - distance, which object should keep from ACenter
  or
  ADistance + not AFromCenterSpot - distance, which object should shift from his current position away from center.
}
function ShiftObjectFromCenter(const AOriginalPosition: TVector;
  const ACenter: TVector; const aDistance: TGeoFloat; const AFromCenterSpot: Boolean): TVector;
var
  lDirection: TVector;
begin
  lDirection := VectorNormalize(VectorSubtract(AOriginalPosition, ACenter));
  if AFromCenterSpot then
      Result := VectorAdd(ACenter, VectorScale(lDirection, aDistance))
  else
      Result := VectorAdd(AOriginalPosition, VectorScale(lDirection, aDistance))
end;

{ AOriginalPosition - Object initial position.
  ACenter - some point, from which is should be distanced.

  ADistance + AFromCenterSpot - distance, which object should keep from ACenter
  or
  ADistance + not AFromCenterSpot - distance, which object should shift from his current position away from center.
}
function ShiftObjectFromCenter(const AOriginalPosition: TAffineVector;
  const ACenter: TAffineVector; const aDistance: TGeoFloat; const AFromCenterSpot: Boolean): TAffineVector;
var
  lDirection: TAffineVector;
begin
  lDirection := VectorNormalize(VectorSubtract(AOriginalPosition, ACenter));
  if AFromCenterSpot then
      Result := VectorAdd(ACenter, VectorScale(lDirection, aDistance))
  else
      Result := VectorAdd(AOriginalPosition, VectorScale(lDirection, aDistance))
end;

// --------------------------------------------------------------
// --------------------------------------------------------------
// --------------------------------------------------------------
initialization

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

end.
