Sordie.co.uk

libsassy/libSassy.Geometry.pas

Raw

{(
 )) libSassy.Geometry
((    2D/3D Geometry library
 ))
((  Copyright  Sordie Aranka Solomon-Smith 2015
 ))
((  This work is made available under the terms of the Creative Commons
 )) Attribution-NonCommercial-ShareAlike 3.0 Unported license
((  http://creativecommons.org/licenses/by-nc-sa/3.0/
 )}

unit libSassy.Geometry;

interface

uses
  libSassy.Maths,
  libSassy.Arrays;

type
{$REGION 'TVector2'}
  PVector2 = ^TVector2;
  TVector2 = record
    class function CreateZero:  TVector2; inline; static;
    class function CreateUnity: TVector2; inline; static;

    class function Create(X, Y: Single): TVector2; inline; static;

    class function CreateSinCos(Theta: Single): TVector2; inline; static;

    class operator Negative(V: TVector2): TVector2; inline;
    class operator Positive(V: TVector2): TVector2; inline;

    class operator Round(V: TVector2): TVector2; inline;
    class operator Trunc(V: TVector2): TVector2; inline;

    class operator Add     (A, B: TVector2): TVector2; inline;
    class operator Subtract(A, B: TVector2): TVector2; inline;
    class operator Multiply(A, B: TVector2): TVector2; inline;
    class operator Divide  (A, B: TVector2): TVector2; inline;

    class operator Add     (A: TVector2; B: Single): TVector2; inline;
    class operator Subtract(A: TVector2; B: Single): TVector2; inline;
    class operator Multiply(A: TVector2; B: Single): TVector2; inline;
    class operator Divide  (A: TVector2; B: Single): TVector2; inline;

    class operator Add     (B: Single; A: TVector2): TVector2; inline;
    class operator Subtract(B: Single; A: TVector2): TVector2; inline;
    class operator Multiply(B: Single; A: TVector2): TVector2; inline;
    class operator Divide  (B: Single; A: TVector2): TVector2; inline;

    function Abs: TVector2; inline;

    function Perp: TVector2; inline;
    function PerpDotProduct(V: TVector2): Single; inline;

    function DotProduct(V: TVector2): Single; overload; inline;
    function DotProduct:              Single; overload; inline;

    function CrossProduct(V: TVector2): Single; overload; inline;
    function CrossProduct:              Single; overload; inline;

    function Normalize: TVector2;

    function Length:    Single; inline;
    function Direction: Single; inline;

    function Distance(V: TVector2): Single; inline;
    function Heading (V: TVector2): Single; inline;

    function ApproxLength:                Single;
    function ApproxDistance(V: TVector2): Single; inline;

    function Lerp   (V: TVector2; Alpha: Single): TVector2;
    function CosLerp(V: TVector2; Alpha: Single): TVector2; inline;

    function Rotate(Angle: Single): TVector2;

  case Integer of
    0: (Data: array[0..1] of Single);
    1: (X, Y: Single);
    2: (U, V: Single);
    3: (A, B: Single);
  end;
{$ENDREGION}

{$REGION 'TVector4'}
  PVector4 = ^TVector4;
  TVector4 = record
    class function CreateZero(W1: Boolean = False): TVector4; static; inline;
    class function CreateUnity: TVector4;                     static; inline;

    class function Create(s1, s2, s3: Single; s4: Single = 1): TVector4; static;

    class operator Positive(V: TVector4): TVector4;
    class operator Negative(V: TVector4): TVector4;

    class operator Round(V: TVector4): TVector4;
    class operator Trunc(V: TVector4): TVector4;

    class operator Add     (A, B: TVector4): TVector4;
    class operator Subtract(A, B: TVector4): TVector4;
    class operator Multiply(A, B: TVector4): TVector4;
    class operator Divide  (A, B: TVector4): TVector4;

    class operator Add     (A: TVector4; B: Single): TVector4;
    class operator Subtract(A: TVector4; B: Single): TVector4;
    class operator Multiply(A: TVector4; B: Single): TVector4;
    class operator Divide  (A: TVector4; B: Single): TVector4;

    class operator Add     (B: Single; A: TVector4): TVector4; inline;
    class operator Subtract(B: Single; A: TVector4): TVector4; inline;
    class operator Multiply(B: Single; A: TVector4): TVector4; inline;
    class operator Divide  (B: Single; A: TVector4): TVector4; inline;

    class operator Implicit(V: TVector4): TVector2; inline;
    class operator Implicit(V: TVector2): TVector4; inline;

    class operator Add     (A: TVector4; B: TVector2): TVector4;
    class operator Subtract(A: TVector4; B: TVector2): TVector4;
    class operator Multiply(A: TVector4; B: TVector2): TVector4;
    class operator Divide  (A: TVector4; B: TVector2): TVector4;

    procedure SetValue(s1, s2, s3: Single; s4: Single = 1);

    function Abs: TVector4;

    function Used: Boolean; inline;

    function DotProduct(V: TVector4): Single; overload;
    function DotProduct:              Single; overload; inline;

    function CrossProduct(V: TVector4): TVector4; overload;
    function CrossProduct:              TVector4; overload; inline;

    function Length: Single;
    function Normal: TVector4;

    function Lerp   (V: TVector4; Alpha: Single): TVector4;
    function CosLerp(V: TVector4; Alpha: Single): TVector4; inline;

    function Reflect(SurfaceNormal: TVector4): TVector4;
    function ParallelNormal(A: TVector4): TVector4;

  case Integer of
    0: (Data: array[0..3] of Single);
    1: (X, Y, Z, W: Single);
    2: (R, G, B, A: Single);
    4: (XY, AB: TVector2);
  end;
{$ENDREGION}

{$REGION 'TMatrix3'}
  PMatrix3 = ^TMatrix3;
  TMatrix3 = record
    class function CreateZero:                     TMatrix3; static;
    class function CreateIdentity:                 TMatrix3; static;
    class function CreateTranslate(V: TVector2):   TMatrix3; static;
    class function CreateScale    (V: TVector2):   TMatrix3; static;
    class function CreateRotate   (Angle: Single): TMatrix3; static;

    class operator Add     (A, B: TMatrix3): TMatrix3;
    class operator Subtract(A, B: TMatrix3): TMatrix3;

    class operator Multiply(A: TMatrix3; B: Single): TMatrix3;
    class operator Divide  (A: TMatrix3; B: Single): TMatrix3;

    class operator Multiply(A, B: TMatrix3): TMatrix3;

    class operator Multiply(A: TVector2; B: TMatrix3): TVector2;

    procedure LoadZero;     inline;
    procedure LoadIdentity; inline;

    procedure Translate(V: TVector2);   inline;
    procedure Scale    (V: TVector2);   inline;
    procedure Rotate   (Angle: Single); inline;
  case Integer of
    0: (Data2D: array[0..2, 0..2] of Single);
    1: (Data1D: array[0..8]       of Single);
  end;
{$ENDREGION}

{$REGION 'TMatrix4'}
  PMatrix4 = ^TMatrix4;
  TMatrix4 = record
    class function CreateZero:     TMatrix4; static;
    class function CreateIdentity: TMatrix4; static;

    class function CreateFustrum(Left, Right, Top, Bottom, MinRange, MaxRange: Single): TMatrix4; static;

    class function CreatePerspective(FieldOfView, AspectRatio, MinRange, MaxRange: Single): TMatrix4; static;
    class function CreateOrthogonal(Left, Right, Top, Bottom,  MinRange, MaxRange: Single): TMatrix4; static;

    class function CreateTranslate(A: TVector4): TMatrix4; static;
    class function CreateScale    (A: TVector4): TMatrix4; static;
    class function CreateReflect  (A: TVector4): TMatrix4; static;

    class function CreateRotateX(A: Single): TMatrix4; static;
    class function CreateRotateY(A: Single): TMatrix4; static;
    class function CreateRotateZ(A: Single): TMatrix4; static;

    class function CreateRotate(A: Single; Axis: TVector4): TMatrix4; static;

    class function CreateHeadingPitchBank(Heading, Pitch, Bank: Single): TMatrix4; static;
    class function CreateYawPitchRoll    (Yaw,     Pitch, Roll: Single): TMatrix4; static;

    class function CreateLookAt(Origin, Target, Up: TVector4): TMatrix4; static;

    class function Create(D00, D10, D20, D30,
                          D01, D11, D21, D31,
                          D02, D12, D22, D32,
                          D03, D13, D23, D33: Single): TMatrix4; static;

    class operator Add     (A, B: TMatrix4): TMatrix4;
    class operator Subtract(A, B: TMatrix4): TMatrix4;
    class operator Multiply(A, B: TMatrix4): TMatrix4;

    class operator Add     (A: TMatrix4; B: Single): TMatrix4;
    class operator Subtract(A: TMatrix4; B: Single): TMatrix4;
    class operator Multiply(A: TMatrix4; B: Single): TMatrix4;
    class operator Divide  (A: TMatrix4; B: Single): TMatrix4;

    class operator Multiply(A: TVector4; B: TMatrix4): TVector4;

    procedure LoadIdentity; inline;

    function WorldPos: TVector4;
    function EyePos:   TVector4;

    function Transpose: TMatrix4;
    function Invert: TMatrix4;

    function Lerp(Dest: TMatrix4; Alpha: Single): TMatrix4;

    procedure Translate(A: TVector4); inline;
    procedure Scale    (A: TVector4); inline;
    procedure Reflect  (A: TVector4); inline;

    procedure RotateX(A: Single); inline;
    procedure RotateY(A: Single); inline;
    procedure RotateZ(A: Single); inline;

    procedure Rotate(A: Single; Axis: TVector4); inline;

    procedure RotateXLocal(A: Single); inline;
    procedure RotateYLocal(A: Single); inline;
    procedure RotateZLocal(A: Single); inline;

    procedure HeadingPitchBank(Heading, Pitch, Bank: Single); inline;
    procedure YawPitchRoll    (Yaw,     Pitch, Roll: Single); inline;

    procedure LookAt(Origin, Target, Up: TVector4); inline;
  case Integer of
    0: (Data2D: array[0..3, 0..3] of Single);
    1: (Data1D: array[0..15]      of Single);
  end;
{$ENDREGION}

{$REGION 'TFrustum'}
  PFrustum = ^TFrustum;
  TFrustum = record
    Planes: array[0..5, 0..3] of Single;

    procedure Update(ProjectionView:   TMatrix4); overload;
    procedure Update(Projection, View: TMatrix4); overload;

    function Clip(V: TVector4; const Radius: Single): Boolean;
  end;
{$ENDREGION}

  TInterpolateMode = (imLinear, imCosine, imCubic);

{$REGION 'TVectors2'}
  TVectors2 = class(TArray<TVector2>)
  public
    function InternalCompareProc(const A, B): Integer; override;

    function Interpolate(Index: Single; Mode: TInterpolateMode = imLinear; Closed: Boolean = False): TVector2;
  end;
{$ENDREGION}

{$REGION 'TVectors4'}
  TVectors4 = class(TArray<TVector4>)
    function InternalCompareProc(const A, B): Integer; override;

    function Interpolate(Index: Single; Mode: TInterpolateMode = imLinear; Closed: Boolean = False): TVector4;
  end;
{$ENDREGION}

implementation

{$REGION 'TVector2'}
class function TVector2.Create;
begin
  Result.X := X;
  Result.Y := Y;
end;

class function TVector2.CreateSinCos(Theta: Single): TVector2;
var
  s, c: Extended;
begin
  SinCos(Theta, s, c);

  Result.X := s;
  Result.Y := c;
end;

class function TVector2.CreateZero;
begin
  Result.X := 0;
  Result.Y := 0;
end;

class function TVector2.CreateUnity;
begin
  Result.X := 1;
  Result.Y := 1;
end;

class operator TVector2.Negative(V: TVector2): TVector2;
begin
  Result.X := -V.X;
  Result.Y := -V.Y;
end;

class operator TVector2.Positive(V: TVector2): TVector2;
begin
  Result.X := +V.X;
  Result.Y := +V.Y;
end;

class operator TVector2.Round(V: TVector2): TVector2;
begin
  Result.X := Round(V.X);
  Result.Y := Round(V.Y);
end;

class operator TVector2.Trunc(V: TVector2): TVector2;
begin
  Result.X := Trunc(V.X);
  Result.Y := Trunc(V.Y);
end;

class operator TVector2.Add(A, B: TVector2): TVector2;
begin
  Result.X := A.X + B.X;
  Result.Y := A.Y + B.Y;
end;

class operator TVector2.Subtract(A, B: TVector2): TVector2;
begin
  Result.X := A.X - B.X;
  Result.Y := A.Y - B.Y;
end;

class operator TVector2.Multiply(A, B: TVector2): TVector2;
begin
  Result.X := A.X * B.X;
  Result.Y := A.Y * B.Y;
end;

class operator TVector2.Divide(A, B: TVector2): TVector2;
begin
  Result.X := A.X / B.X;
  Result.Y := A.Y / B.Y;
end;

class operator TVector2.Add(A: TVector2; B: Single): TVector2;
begin
  Result.X := A.X + B;
  Result.Y := A.Y + B;
end;

class operator TVector2.Subtract(A: TVector2; B: Single): TVector2;
begin
  Result.X := A.X - B;
  Result.Y := A.Y - B;
end;

class operator TVector2.Multiply(A: TVector2; B: Single): TVector2;
begin
  Result.X := A.X * B;
  Result.Y := A.Y * B;
end;

class operator TVector2.Divide(A: TVector2; B: Single): TVector2;
begin
  Result.X := A.X / B;
  Result.Y := A.Y / B;
end;

class operator TVector2.Add(B: Single; A: TVector2): TVector2;
begin
  Result := A + B;
end;

class operator TVector2.Subtract(B: Single; A: TVector2): TVector2;
begin
  Result := A - B;
end;

class operator TVector2.Multiply(B: Single; A: TVector2): TVector2;
begin
  Result := A * B;
end;

class operator TVector2.Divide(B: Single; A: TVector2): TVector2;
begin
  Result := A / B;
end;

function TVector2.Abs;
begin
  Result.X := System.Abs(X);
  Result.Y := System.Abs(Y);
end;

function TVector2.Perp;
begin
  Result.X := -Y;
  Result.Y :=  X;
end;

function TVector2.PerpDotProduct;
begin
  Result := (X * V.Y) - (Y * V.X);
end;

function TVector2.DotProduct(V: TVector2): Single;
begin
  Result := (X * V.X) + (Y * V.Y);
end;

function TVector2.DotProduct: Single;
begin
  Result := DotProduct(Self);
end;

function TVector2.CrossProduct(V: TVector2): Single;
begin
  Result := (X * V.X) - (Y * V.Y);
end;

function TVector2.CrossProduct: Single;
begin
  Result := CrossProduct(Self);
end;

function TVector2.Normalize;
var
  l: Single;
begin
  l := Length;

  if l <> 0 then
    Result := Self / L
  else
    Result := CreateZero;
end;

function TVector2.Length;
begin
  Result := sqrt(DotProduct);
end;

function TVector2.Direction;
begin
  Result := ArcTan2(X, Y);
end;

function TVector2.Distance;
begin
  Result := (Self - V).Length;
end;

function TVector2.Heading;
begin
  Result := (Self - V).Direction;
end;

function TVector2.ApproxLength;
var
  dx, dy, dt: Single;
begin
  dx := System.Abs(X);
  dy := System.Abs(Y);

  if dx <= dy then
  begin
    dt := dx;
    dx := dy;
    dy := dt;
  end;

  Result := dx + dy * 0.414213562;
end;

function TVector2.ApproxDistance;
begin
  Result := (Self - V).ApproxLength;
end;

function TVector2.Lerp;
begin
  Result.X := X + (V.X - X) * Alpha;
  Result.Y := Y + (V.Y - Y) * Alpha;
end;

function TVector2.CosLerp;
begin
  Result := Lerp(V, (1 - Cos(Alpha * PI)) * 0.5);
end;

function TVector2.Rotate;
var
  s, c: Extended;
begin
  SinCos(Angle, s, c);

  Result.X :=  c * X - s * Y;
  Result.Y :=  s * X + c * Y;
end;
{$ENDREGION}

{$REGION 'TVector4'}
class function TVector4.CreateZero;
begin
  if W1 then
    Result := Create(0, 0, 0, 1)
  else
    Result := Create(0, 0, 0, 0);
end;

class function TVector4.CreateUnity;
begin
  Result := Create(1, 1, 1, 1);
end;

class function TVector4.Create;
begin
  Result.X := s1;
  Result.Y := s2;
  Result.Z := s3;
  Result.W := s4;
end;

class operator TVector4.Positive(V: TVector4): TVector4;
begin
  Result.X := +V.X;
  Result.Y := +V.Y;
  Result.Z := +V.Z;
  Result.W := +V.W;
end;

class operator TVector4.Negative(V: TVector4): TVector4;
begin
  Result.X := -V.X;
  Result.Y := -V.Y;
  Result.Z := -V.Z;
  Result.W := -V.W;
end;

class operator TVector4.Round(V: TVector4): TVector4;
begin
  Result.X := Round(V.X);
  Result.Y := Round(V.Y);
  Result.Z := Round(V.Z);
  Result.W := Round(V.W);
end;

class operator TVector4.Trunc(V: TVector4): TVector4;
begin
  Result.X := Trunc(V.X);
  Result.Y := Trunc(V.Y);
  Result.Z := Trunc(V.Z);
  Result.W := Trunc(V.W);
end;

class operator TVector4.Add(A, B: TVector4): TVector4;
begin
  Result.X := A.X + B.X;
  Result.Y := A.Y + B.Y;
  Result.Z := A.Z + B.Z;
  Result.W := A.W + B.W;
end;

class operator TVector4.Subtract(A, B: TVector4): TVector4;
begin
  Result.X := A.X - B.X;
  Result.Y := A.Y - B.Y;
  Result.Z := A.Z - B.Z;
  Result.W := A.W - B.W;
end;

class operator TVector4.Multiply(A, B: TVector4): TVector4;
begin
  Result.X := A.X * B.X;
  Result.Y := A.Y * B.Y;
  Result.Z := A.Z * B.Z;
  Result.W := A.W * B.W;
end;

class operator TVector4.Divide(A, B: TVector4): TVector4;
begin
  Result.X := A.X / B.X;
  Result.Y := A.Y / B.Y;
  Result.Z := A.Z / B.Z;
  Result.W := A.W / B.W;
end;

class operator TVector4.Add(A: TVector4; B: Single): TVector4;
begin
  Result.X := A.X + B;
  Result.Y := A.Y + B;
  Result.Z := A.Z + B;
  Result.W := A.W + B;
end;

class operator TVector4.Subtract(A: TVector4; B: Single): TVector4;
begin
  Result.X := A.X - B;
  Result.Y := A.Y - B;
  Result.Z := A.Z - B;
  Result.W := A.W - B;
end;

class operator TVector4.Multiply(A: TVector4; B: Single): TVector4;
begin
  Result.X := A.X * B;
  Result.Y := A.Y * B;
  Result.Z := A.Z * B;
  Result.W := A.W * B;
end;

class operator TVector4.Divide(A: TVector4; B: Single): TVector4;
begin
  Result.X := A.X / B;
  Result.Y := A.Y / B;
  Result.Z := A.Z / B;
  Result.W := A.W / B;
end;

class operator TVector4.Add(B: Single; A: TVector4): TVector4;
begin
  Result := A + B;
end;

class operator TVector4.Subtract(B: Single; A: TVector4): TVector4;
begin
  Result := A - B;
end;

class operator TVector4.Multiply(B: Single; A: TVector4): TVector4;
begin
  Result := A * B;
end;

class operator TVector4.Divide(B: Single; A: TVector4): TVector4;
begin
  Result := A / B;
end;

class operator TVector4.Implicit(V: TVector4): TVector2;
begin
  Result.X := V.X;
  Result.Y := V.Y;
end;

class operator TVector4.Implicit(V: TVector2): TVector4;
begin
  Result.X := V.X;
  Result.Y := V.Y;
  Result.Z := 0;
  Result.W := 0;
end;

class operator TVector4.Add(A: TVector4; B: TVector2): TVector4;
begin
  Result.X := A.X + B.X;
  Result.Y := A.Y + B.Y;
  Result.Z := A.Z;
  Result.W := A.W;
end;

class operator TVector4.Subtract(A: TVector4; B: TVector2): TVector4;
begin
  Result.X := A.X - B.X;
  Result.Y := A.Y - B.Y;
  Result.Z := A.Z;
  Result.W := A.W;
end;

class operator TVector4.Multiply(A: TVector4; B: TVector2): TVector4;
begin
  Result.X := A.X * B.X;
  Result.Y := A.Y * B.Y;
  Result.Z := A.Z;
  Result.W := A.W;
end;

class operator TVector4.Divide(A: TVector4; B: TVector2): TVector4;
begin
  Result.X := A.X / B.X;
  Result.Y := A.Y / B.Y;
  Result.Z := A.Z;
  Result.W := A.W;
end;

procedure TVector4.SetValue;
begin
  Data[0] := s1;
  Data[1] := s2;
  Data[2] := s3;
  Data[3] := s4;
end;

function TVector4.Abs;
begin
  Result.X := System.Abs(X);
  Result.Y := System.Abs(Y);
  Result.Z := System.Abs(Z);
  Result.W := System.Abs(W);
end;

function TVector4.Used;
begin
  Result := Data[3] <> 0;
end;

function TVector4.DotProduct(V: TVector4): Single;
begin
  Result := (X * V.X) + (Y * V.Y) + (Z * V.Z);
end;

function TVector4.DotProduct: Single;
begin
  Result := DotProduct(Self);
end;

function TVector4.CrossProduct(V: TVector4): TVector4;
begin
  Result.X := (Y * V.Z) - (Z * V.Y);
  Result.Y := (Z * V.X) - (X * V.Z);
  Result.Z := (X * V.Y) - (Y * V.X);
  Result.W := W;
end;

function TVector4.CrossProduct: TVector4;
begin
  Result := CrossProduct(Self);
end;

function TVector4.Length;
begin
  Result := sqrt((X * X) + (Y * Y) + (Z * Z));
end;

function TVector4.Normal;
var
  Len: Single;
begin
  Len := Length;

  if System.Abs(Len) > 0.000001 then
  begin
    Result.X := X / Len;
    Result.Y := Y / Len;
    Result.Z := Z / Len;
  end
  else
    Result := TVector4.CreateZero;
end;

function TVector4.Lerp;
begin
  Result.X := X + (V.X - X) * Alpha;
  Result.Y := Y + (V.Y - Y) * Alpha;
  Result.Z := Z + (V.Z - Z) * Alpha;
  Result.W := W + (V.W - W) * Alpha;
end;

function TVector4.CosLerp;
begin
  Result := Lerp(V, (1 - Cos(Alpha * PI)) * 0.5);
end;

function TVector4.Reflect;
begin
  Result := Self - 2.0 * SurfaceNormal * DotProduct(SurfaceNormal);
end;

function TVector4.ParallelNormal;
begin
  Result := A * (DotProduct(A) / Sqr(Length));
end;
{$ENDREGION}

{$REGION 'TMatrix3'}
class function TMatrix3.CreateZero;
begin
  with Result do
  begin
    Data2D[0, 0] := 0; Data2D[0, 1] := 0; Data2D[0, 2] := 0;
    Data2D[1, 0] := 0; Data2D[1, 1] := 0; Data2D[1, 2] := 0;
    Data2D[2, 0] := 0; Data2D[2, 1] := 0; Data2D[2, 2] := 0;
  end;
end;

class function TMatrix3.CreateIdentity;
begin
  with Result do
  begin
    Data2D[0, 0] := 1; Data2D[0, 1] := 0; Data2D[0, 2] := 0;
    Data2D[1, 0] := 0; Data2D[1, 1] := 1; Data2D[1, 2] := 0;
    Data2D[2, 0] := 0; Data2D[2, 1] := 0; Data2D[2, 2] := 1;
  end;
end;

class function TMatrix3.CreateTranslate;
begin
  with Result do
  begin
    LoadIdentity;
    Data2D[2, 0] := V.X;
    Data2D[2, 1] := V.Y;
  end;
end;

class function TMatrix3.CreateScale;
begin
  with Result do
  begin
    LoadIdentity;
    Data2D[0, 0] := V.X;
    Data2D[1, 1] := V.Y;
  end;
end;

class function TMatrix3.CreateRotate;
var
  s, c: Extended;
begin
  SinCos(Angle, s, c);

  with Result do
  begin
    LoadIdentity;

    Data2D[0, 0] :=  c;
    Data2D[0, 1] :=  s;
    Data2D[1, 0] := -Data2D[0, 1];
    Data2D[1, 1] :=  Data2D[0, 0];
  end;
end;

class operator TMatrix3.Add(A, B: TMatrix3): TMatrix3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      Result.Data2D[i, j] := A.Data2D[i, j] + B.Data2D[i, j];
end;

class operator TMatrix3.Subtract(A, B: TMatrix3): TMatrix3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      Result.Data2D[i, j] := A.Data2D[i, j] - B.Data2D[i, j];
end;

class operator TMatrix3.Multiply(A: TMatrix3; B: Single): TMatrix3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      Result.Data2D[i, j] := A.Data2D[i, j] * B;
end;

class operator TMatrix3.Divide(A: TMatrix3; B: Single): TMatrix3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      Result.Data2D[i, j] := A.Data2D[i, j] / B;
end;

class operator TMatrix3.Multiply(A, B: TMatrix3): TMatrix3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      Result.Data2D[i, j]:= (A.Data2D[i, 0] * B.Data2D[0, j]) + (A.Data2D[i, 1] * B.Data2D[1, j]) + (A.Data2D[i, 2] * B.Data2D[2, j]);
end;

class operator TMatrix3.Multiply(A: TVector2; B: TMatrix3): TVector2;
begin
  Result.X := (A.X * B.Data2D[0, 0]) + (A.Y * B.Data2D[1, 0]) + B.Data2D[2, 0];
  Result.Y := (A.X * B.Data2D[0, 1]) + (A.Y * B.Data2D[1, 1]) + B.Data2D[2, 1];
end;

procedure TMatrix3.LoadZero;
begin
  Self := CreateZero;
end;

procedure TMatrix3.LoadIdentity;
begin
  Self := CreateIdentity;
end;

procedure TMatrix3.Translate;
begin
  Self := Self * CreateTranslate(V);
end;

procedure TMatrix3.Scale;
begin
  Self := Self * CreateScale(V);
end;

procedure TMatrix3.Rotate;
begin
  Self := Self * CreateRotate(Angle);
end;
{$ENDREGION}

{$REGION 'TMatrix4'}
class function TMatrix4.CreateZero;
begin
  with Result do
  begin
    Data2D[0, 0] := 0; Data2D[1, 0] := 0; Data2D[2, 0] := 0; Data2D[3, 0] := 0;
    Data2D[0, 1] := 0; Data2D[1, 1] := 0; Data2D[2, 1] := 0; Data2D[3, 1] := 0;
    Data2D[0, 2] := 0; Data2D[1, 2] := 0; Data2D[2, 2] := 0; Data2D[3, 2] := 0;
    Data2D[0, 3] := 0; Data2D[1, 3] := 0; Data2D[2, 3] := 0; Data2D[3, 3] := 0;
  end;
end;

class function TMatrix4.CreateIdentity;
begin
  with Result do
  begin
    Data2D[0, 0] := 1; Data2D[1, 0] := 0; Data2D[2, 0] := 0; Data2D[3, 0] := 0;
    Data2D[0, 1] := 0; Data2D[1, 1] := 1; Data2D[2, 1] := 0; Data2D[3, 1] := 0;
    Data2D[0, 2] := 0; Data2D[1, 2] := 0; Data2D[2, 2] := 1; Data2D[3, 2] := 0;
    Data2D[0, 3] := 0; Data2D[1, 3] := 0; Data2D[2, 3] := 0; Data2D[3, 3] := 1;
  end;
end;

class function TMatrix4.CreateFustrum;
var
  t1, t2, t3, t4: Single;
begin
  Result := CreateZero;

  t1 := 2 * MinRange;
  t2 := Right - Left;
  t3 := Top - Bottom;
  t4 := MaxRange - MinRange;

	Result.Data1D[0]  := t1 / t2;
	Result.Data1D[5]  := t1 / t3;
	Result.Data1D[8]  := (Right + Left) / t2;
	Result.Data1D[9]  := (Top + Bottom) / t3;
	Result.Data1D[10] := (-MaxRange - MinRange) / t4;
	Result.Data1D[11] := -1.0;
	Result.Data1D[14] := (-t1 * MaxRange) / t4;
end;

class function TMatrix4.CreatePerspective;
var
  XMax, YMax: Single;
begin
  YMax := MinRange * Tangent(FieldOfView * PI / 360);
  XMax := YMax * AspectRatio;

  Result := CreateFustrum(-XMax, XMax, YMax, -YMax, MinRange, MaxRange);
end;

class function TMatrix4.CreateOrthogonal;
begin
  Result := CreateZero;

  Result.Data2D[0, 0] :=  2 / (Right - Left);
  Result.Data2D[1, 1] :=  2 / (Top - Bottom);
  Result.Data2D[2, 2] := -2 / (MaxRange - MinRange);

  Result.Data2D[3, 0] := (Right + Left) / (Right - Left);
  Result.Data2D[3, 1] := (Top + Bottom) / (Top - Bottom);
  Result.Data2D[3, 2] := -(MaxRange + MinRange) / (MaxRange - MinRange);

  Result.Data2D[3, 3] := 1;
end;

class function TMatrix4.CreateTranslate;
begin
  Result := CreateIdentity;

  with Result do
  begin
    Data2D[3, 0] := A.X;
    Data2D[3, 1] := A.Y;
    Data2D[3, 2] := A.Z;
  end;
end;

class function TMatrix4.CreateScale;
begin
  Result := CreateIdentity;

  with Result do
  begin
    Data2D[0, 0] := A.X;
    Data2D[1, 1] := A.Y;
    Data2D[2, 2] := A.Z;
  end;
end;

class function TMatrix4.CreateReflect;
var
  xy, yz, xz: Single;
begin
  Result := CreateIdentity;

  xy := -2.0 * A.X * A.Y;
  xz := -2.0 * A.X * A.Z;
  yz := -2.0 * A.Y * A.Z;

  with Result do
  begin
    Data2D[0, 0] := 1.0 - (2.0 * Sqr(A.x));
    Data2D[0, 1] := xy;
    Data2D[0, 2] := xz;
    Data2D[1, 0] := xy;
    Data2D[1, 1] := 1.0 - (2.0 * Sqr(A.y));
    Data2D[1, 2] := yz;
    Data2D[2, 0] := xz;
    Data2D[2, 1] := yz;
    Data2D[2, 2] := 1.0 - (2.0 * Sqr(A.z));
  end;
end;

class function TMatrix4.CreateRotateX;
var
  s, c: Extended;
begin
  Result := CreateIdentity;

  SinCos(-A, s, c);

  with Result do
  begin
    Data2D[1, 1] :=  c;
    Data2D[1, 2] :=  s;
    Data2D[2, 1] := -Data2D[1, 2];
    Data2D[2, 2] :=  Data2D[1, 1];
  end;
end;

class function TMatrix4.CreateRotateY;
var
  s, c: Extended;
begin
  Result := CreateIdentity;

  SinCos(-A, s, c);

  with Result do
  begin
    Data2D[0, 0] :=  c;
    Data2D[0, 2] :=  s;
    Data2D[2, 0] := -Data2D[0, 2];
    Data2D[2, 2] :=  Data2D[0, 0];
  end;
end;

class function TMatrix4.CreateRotateZ;
var
  s, c: Extended;
begin
  Result := CreateIdentity;

  SinCos(-A, s, c);

  with Result do
  begin
    Data2D[0, 0] :=  c;
    Data2D[0, 1] :=  s;
    Data2D[1, 0] := -Data2D[0, 1];
    Data2D[1, 1] :=  Data2D[0, 0];
  end;
end;

class function TMatrix4.CreateRotate;
var
  CosTh, iCosTh, SinTh: Extended;
  XY,    XZ,     YZ:    Single;
  XSin,  YSin,   ZSin:  Single;
begin
  Result := CreateIdentity;

  SinCos(-A, SinTh, CosTh);
  iCosTh := 1.0 - CosTh;

  XY := Axis.x * Axis.y * iCosTh;
  XZ := Axis.x * Axis.z * iCosTh;
  YZ := Axis.y * Axis.z * iCosTh;

  XSin := Axis.x * SinTh;
  YSin := Axis.y * SinTh;
  ZSin := Axis.z * SinTh;

  Result.Data2D[0, 0] := (sqr(Axis.x) * iCosTh) + CosTh;
  Result.Data2D[0, 1] := XY + ZSin;
  Result.Data2D[0, 2] := XZ - YSin;
  Result.Data2D[1, 0] := XY - ZSin;
  Result.Data2D[1, 1] := (sqr(Axis.y) * iCosTh) + CosTh;
  Result.Data2D[1, 2] := YZ + XSin;
  Result.Data2D[2, 0] := XZ + YSin;
  Result.Data2D[2, 1] := YZ - XSin;
  Result.Data2D[2, 2] := (sqr(Axis.z) * iCosTh) + CosTh;
end;

class function TMatrix4.CreateHeadingPitchBank;
var
  CosH, SinH: Extended;
  CosP, SinP: Extended;
  CosB, SinB: Extended;
begin
  Result := CreateIdentity;

  SinCos(Heading, SinH, CosH);
  SinCos(Pitch,   SinP, CosP);
  SinCos(Bank,    SinB, CosB);

  Result.Data2D[0, 0] := ( CosH * CosB) + (SinH * SinP * SinB);
  Result.Data2D[0, 1] := (-CosH * SinB) + (SinH * SinP * CosB);
  Result.Data2D[0, 2] := SinH * CosP;

  Result.Data2D[1, 0] := SinB * CosP;
  Result.Data2D[1, 1] := CosB * CosP;
  Result.Data2D[1, 2] := -SinP;

  Result.Data2D[2, 0] := (-SinH * CosB) + (CosH * SinP * SinB);
  Result.Data2D[2, 1] := ( SinB * SinH) + (CosH * SinP * CosB);
  Result.Data2D[2, 2] := CosH * CosP;
end;

class function TMatrix4.CreateYawPitchRoll;
var
  SinYaw, SinPitch, SinRoll: Extended;
  CosYaw, CosPitch, CosRoll: Extended;
begin
  Result := CreateIdentity;

  SinCos(Yaw,   SinYaw,   CosYaw);
  SinCos(Pitch, SinPitch, CosPitch);
  SinCos(Roll,  SinRoll,  CosRoll);

  Result.Data2D[0, 0] := CosRoll * CosYaw + SinPitch * SinRoll * SinYaw;
  Result.Data2D[0, 1] := CosYaw * SinPitch * SinRoll - CosRoll * SinYaw;
  Result.Data2D[0, 2] := -CosPitch * SinRoll;

  Result.Data2D[1, 0] := CosPitch * SinYaw;
  Result.Data2D[1, 1] := CosPitch * CosYaw;
  Result.Data2D[1, 2] := SinPitch;

  Result.Data2D[2, 0] := CosYaw * SinRoll - CosRoll * SinPitch * SinYaw;
  Result.Data2D[2, 1] := -CosRoll * CosYaw * SinPitch - SinRoll * SinYaw;
  Result.Data2D[2, 2] := CosPitch * CosRoll;
end;

class function TMatrix4.CreateLookAt;
var
  ForwardV: TVector4;
  RightV:   TVector4;
  UpV:      TVector4;
begin
  ForwardV := (Target - Origin).Normal;
  RightV   := ForwardV.CrossProduct(Up).Normal;
  UpV      := RightV.CrossProduct(ForwardV).Normal;

  Result.Data1D[0] := RightV.X;
  Result.Data1D[1] := UpV.X;
  Result.Data1D[2] := -ForwardV.X;

  Result.Data1D[3] := 0;

  Result.Data1D[4] := RightV.Y;
  Result.Data1D[5] := UpV.Y;
  Result.Data1D[6] := -ForwardV.Y;

  Result.Data1D[7] := 0;

  Result.Data1D[8]  := RightV.Z;
  Result.Data1D[9]  := UpV.Z;
  Result.Data1D[10] := -ForwardV.Z;

  Result.Data1D[11] := 0;

  Result.Data1D[12] := -RightV.DotProduct(Origin);
  Result.Data1D[13] := -UpV.DotProduct(Origin);
  Result.Data1D[14] := ForwardV.DotProduct(Origin);

  Result.Data1D[15] := 1;
end;

class function TMatrix4.Create;
begin
  with Result do
  begin
    Data2D[0, 0] := D00; Data2D[1, 0] := D10; Data2D[2, 0] := D20; Data2D[3, 0] := D30;
    Data2D[0, 1] := D01; Data2D[1, 1] := D11; Data2D[2, 1] := D21; Data2D[3, 1] := D30;
    Data2D[0, 2] := D02; Data2D[1, 2] := D12; Data2D[2, 2] := D22; Data2D[3, 2] := D30;
    Data2D[0, 3] := D03; Data2D[1, 3] := D13; Data2D[2, 3] := D23; Data2D[3, 3] := D30;
  end;
end;

class operator TMatrix4.Add(A, B: TMatrix4): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, y] + B.Data2D[x, x];
end;

class operator TMatrix4.Subtract(A, B: TMatrix4): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, x] - B.Data2D[y, x];
end;

class operator TMatrix4.Multiply(A, B: TMatrix4): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := (A.Data2D[y, 0] * B.Data2D[0, x]) +
                           (A.Data2D[y, 1] * B.Data2D[1, x]) +
                           (A.Data2D[y, 2] * B.Data2D[2, x]) +
                           (A.Data2D[y, 3] * B.Data2D[3, x]);
end;

class operator TMatrix4.Add(A: TMatrix4; B: Single): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, x] + B;
end;

class operator TMatrix4.Subtract(A: TMatrix4; B: Single): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, x] - B;
end;

class operator TMatrix4.Multiply(A: TMatrix4; B: Single): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, x] * B;
end;

class operator TMatrix4.Divide(A: TMatrix4; B: Single): TMatrix4;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := A.Data2D[y, x] / B;
end;

class operator TMatrix4.Multiply(A: TVector4; B: TMatrix4): TVector4;
begin
  Result.X := (A.X * B.Data2D[0, 0]) + (A.Y * B.Data2D[1, 0]) + (A.Z * B.Data2D[2, 0]) + B.Data2D[3, 0];
  Result.Y := (A.X * B.Data2D[0, 1]) + (A.Y * B.Data2D[1, 1]) + (A.Z * B.Data2D[2, 1]) + B.Data2D[3, 1];
  Result.Z := (A.X * B.Data2D[0, 2]) + (A.Y * B.Data2D[1, 2]) + (A.Z * B.Data2D[2, 2]) + B.Data2D[3, 2];
  Result.W := A.W;
end;

procedure TMatrix4.LoadIdentity;
begin
  Self := CreateIdentity;
end;

function TMatrix4.WorldPos;
begin
  Result.X := Data2D[3, 0];
  Result.Y := Data2D[3, 1];
  Result.Z := Data2D[3, 2];
end;

function TMatrix4.EyePos;
begin
  Result.X := -Data2D[0, 0] * Data2D[3, 0] - Data2D[0, 1] * Data2D[3, 1] - Data2D[0, 2] * Data2D[3, 2];
  Result.Y := -Data2D[1, 0] * Data2D[3, 0] - Data2D[1, 1] * Data2D[3, 1] - Data2D[1, 2] * Data2D[3, 2];
  Result.Z := -Data2D[2, 0] * Data2D[3, 0] - Data2D[2, 1] * Data2D[3, 1] - Data2D[2, 2] * Data2D[3, 2];
  Result.W := 1;
end;

function TMatrix4.Transpose;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := Data2D[x, y];
end;

function TMatrix4.Invert;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := -Data2D[y, x];
end;


function TMatrix4.Lerp;
var
  x, y: Integer;
begin
  for y := 0 to 3 do
    for x := 0 to 3 do
      Result.Data2D[y, x] := Data2D[y, x] + (Dest.Data2D[y, x] - Data2D[y, x]) * Alpha
end;

procedure TMatrix4.Translate;
begin
  Self := Self * CreateTranslate(A);
end;

procedure TMatrix4.Scale;
begin
  Self := Self * CreateScale(A);
end;

procedure TMatrix4.Reflect;
begin
  Self := Self * CreateReflect(A);
end;

procedure TMatrix4.RotateX;
begin
  Self := Self * CreateRotateX(A);
end;

procedure TMatrix4.RotateY;
begin
  Self := Self * CreateRotateY(A);
end;

procedure TMatrix4.RotateZ;
begin
  Self := Self * CreateRotateZ(A);
end;

procedure TMatrix4.Rotate;
begin
  Self := Self * CreateRotate(A, Axis);
end;

procedure TMatrix4.RotateXLocal;
begin
  Self := Self * CreateRotate(A, TVector4.Create(Data2D[0, 0], Data2D[0, 1], Data2D[0, 2]));
end;

procedure TMatrix4.RotateYLocal;
begin
  Self := Self * CreateRotate(A, TVector4.Create(Data2D[1, 0], Data2D[1, 1], Data2D[1, 2]));
end;

procedure TMatrix4.RotateZLocal;
begin
  Self := Self * CreateRotate(A, TVector4.Create(Data2D[2, 0], Data2D[2, 1], Data2D[2, 2]));
end;

procedure TMatrix4.HeadingPitchBank;
begin
  Self := Self * CreateHeadingPitchBank(Heading, Pitch, Bank);
end;

procedure TMatrix4.YawPitchRoll;
begin
  Self := Self * CreateYawPitchRoll(Yaw, Pitch, Roll);
end;

procedure TMatrix4.LookAt;
begin
  Self := Self * CreateLookAt(Origin, Target, Up);
end;
{$ENDREGION}

{$REGION 'TFrustum'}
procedure TFrustum.Update(ProjectionView: TMatrix4);
var
  t: Single;
begin
  Planes[0, 0] := ProjectionView.Data1D[ 3] - ProjectionView.Data1D[ 0];
  Planes[0, 1] := ProjectionView.Data1D[ 7] - ProjectionView.Data1D[ 4];
  Planes[0, 2] := ProjectionView.Data1D[11] - ProjectionView.Data1D[ 8];
  Planes[0, 3] := ProjectionView.Data1D[15] - ProjectionView.Data1D[12];

  t := sqrt(Planes[0, 0] * Planes[0, 0] + Planes[0, 1] * Planes[0, 1] + Planes[0, 2] * Planes[0, 2]);

  Planes[0, 0] := Planes[0, 0] / t;
  Planes[0, 1] := Planes[0, 1] / t;
  Planes[0, 2] := Planes[0, 2] / t;
  Planes[0, 3] := Planes[0, 3] / t;

  Planes[1, 0] := ProjectionView.Data1D[ 3] + ProjectionView.Data1D[ 0];
  Planes[1, 1] := ProjectionView.Data1D[ 7] + ProjectionView.Data1D[ 4];
  Planes[1, 2] := ProjectionView.Data1D[11] + ProjectionView.Data1D[ 8];
  Planes[1, 3] := ProjectionView.Data1D[15] + ProjectionView.Data1D[12];

  t := sqrt(Planes[1, 0] * Planes[1, 0] + Planes[1, 1] * Planes[1, 1] + Planes[1, 2] * Planes[1, 2]);

  Planes[1, 0] := Planes[1, 0] / t;
  Planes[1, 1] := Planes[1, 1] / t;
  Planes[1, 2] := Planes[1, 2] / t;
  Planes[1, 3] := Planes[1, 3] / t;

  Planes[2, 0] := ProjectionView.Data1D[ 3] + ProjectionView.Data1D[ 1];
  Planes[2, 1] := ProjectionView.Data1D[ 7] + ProjectionView.Data1D[ 5];
  Planes[2, 2] := ProjectionView.Data1D[11] + ProjectionView.Data1D[ 9];
  Planes[2, 3] := ProjectionView.Data1D[15] + ProjectionView.Data1D[13];

  t := sqrt(Planes[2, 0] * Planes[2, 0] + Planes[2, 1] * Planes[2, 1] + Planes[2, 2] * Planes[2, 2]);

  Planes[2, 0] := Planes[2, 0] / t;
  Planes[2, 1] := Planes[2, 1] / t;
  Planes[2, 2] := Planes[2, 2] / t;
  Planes[2, 3] := Planes[2, 3] / t;

  Planes[3, 0] := ProjectionView.Data1D[ 3] - ProjectionView.Data1D[ 1];
  Planes[3, 1] := ProjectionView.Data1D[ 7] - ProjectionView.Data1D[ 5];
  Planes[3, 2] := ProjectionView.Data1D[11] - ProjectionView.Data1D[ 9];
  Planes[3, 3] := ProjectionView.Data1D[15] - ProjectionView.Data1D[13];

  t := sqrt(Planes[3, 0] * Planes[3, 0] + Planes[3, 1] * Planes[3, 1] + Planes[3, 2] * Planes[3, 2]);

  Planes[3, 0] := Planes[3, 0] / t;
  Planes[3, 1] := Planes[3, 1] / t;
  Planes[3, 2] := Planes[3, 2] / t;
  Planes[3, 3] := Planes[3, 3] / t;

  Planes[4, 0] := ProjectionView.Data1D[ 3] - ProjectionView.Data1D[ 2];
  Planes[4, 1] := ProjectionView.Data1D[ 7] - ProjectionView.Data1D[ 6];
  Planes[4, 2] := ProjectionView.Data1D[11] - ProjectionView.Data1D[10];
  Planes[4, 3] := ProjectionView.Data1D[15] - ProjectionView.Data1D[14];

  t := sqrt(Planes[4, 0] * Planes[4, 0] + Planes[4, 1] * Planes[4, 1] + Planes[4, 2] * Planes[4, 2]);

  Planes[4, 0] := Planes[4, 0] / t;
  Planes[4, 1] := Planes[4, 1] / t;
  Planes[4, 2] := Planes[4, 2] / t;
  Planes[4, 3] := Planes[4, 3] / t;

  Planes[5, 0] := ProjectionView.Data1D[ 3] + ProjectionView.Data1D[ 2];
  Planes[5, 1] := ProjectionView.Data1D[ 7] + ProjectionView.Data1D[ 6];
  Planes[5, 2] := ProjectionView.Data1D[11] + ProjectionView.Data1D[10];
  Planes[5, 3] := ProjectionView.Data1D[15] + ProjectionView.Data1D[14];

  t := sqrt(Planes[5, 0] * Planes[5, 0] + Planes[5, 1] * Planes[5, 1] + Planes[5, 2] * Planes[5, 2]);

  Planes[5, 0] := Planes[5, 0] / t;
  Planes[5, 1] := Planes[5, 1] / t;
  Planes[5, 2] := Planes[5, 2] / t;
  Planes[5, 3] := Planes[5, 3] / t;
end;

procedure TFrustum.Update(Projection, View: TMatrix4);
begin
  Update(Projection * View);
end;

function TFrustum.Clip;
var
  i: Integer;
begin
  for i := 0 to 5 do
    if Planes[i, 0] * V.X + Planes[i, 1] * V.Y + Planes[i, 2] * V.Z + Planes[i, 3] <= -Radius then
      exit(True);

  Result := False;
end;
{$ENDREGION}

{$REGION 'TVectors2'}
function TVectors2.InternalCompareProc;
var
  d1, d2: Single;
begin
  d1 := TVector2(A).Length;
  d2 := TVector2(B).Length;

  if d1 > d2 then
    Result := 1
  else if d1 < d2 then
    Result := -1
  else
    Result := 0;
end;

function TVectors2.Interpolate;
var
  p1, p2, p3, p4: TVector2;
  iPath:          Integer;
  dPath:          Single;
begin
  iPath := trunc(Index);
  dPath := Index - iPath;

  if (iPath - 1) < 0 then
  begin
    if Closed then p1 := Items[Count - 2] else p1 := Items[0];
  end
  else
    p1 := Items[iPath - 1];

  p2 := Items[iPath];
  if (iPath + 1) > (Count - 1) then
  begin
    if Closed then p3 := Items[0] else p3 := Items[Count - 1];
  end
  else
    p3 := Items[iPath + 1];

  if (iPath + 2) > (Count - 1) then
  begin
    if Closed then p4 := Items[1] else p4 := Items[Count];
  end
  else
    p4 := Items[iPath + 2];

  case Mode of
    imLinear:
    begin
      Result.x := LinearInterpolate(p2.x, p3.x, dPath);
      Result.y := LinearInterpolate(p2.y, p3.y, dPath);
    end;

    imCosine:
    begin
      Result.x := CosineInterpolate(p2.x, p3.x, dPath);
      Result.y := CosineInterpolate(p2.y, p3.y, dPath);
    end;

    imCubic:
    begin
      Result.x := CubicInterpolate(p1.x, p2.x, p3.x, p4.x, dPath);
      Result.y := CubicInterpolate(p1.y, p2.y, p3.y, p4.y, dPath);
    end;
  end;
end;
{$ENDREGION}

{$REGION 'TVectors4'}
function TVectors4.InternalCompareProc;
begin
  if TVector4(A).Z > TVector4(B).Z then
    Result := 1
  else if TVector4(A).Z < TVector4(B).Z then
    Result := -1
  else
    Result := 0;
end;

function TVectors4.Interpolate;
var
  p1, p2, p3, p4: TVector4;
  iPath:          Integer;
  dPath:          Single;
begin
  iPath := trunc(Index);
  dPath := Index - iPath;

  if (iPath - 1) < 0 then
  begin
    if Closed then p1 := Items[Count - 2] else p1 := Items[0];
  end
  else
    p1 := Items[iPath - 1];

  p2 := Items[iPath];
  if (iPath + 1) > (Count - 1) then
  begin
    if Closed then p3 := Items[0] else p3 := Items[Count - 1];
  end
  else
    p3 := Items[iPath + 1];

  if (iPath + 2) > (Count - 1) then
  begin
    if Closed then p4 := Items[1] else p4 := Items[Count];
  end
  else
    p4 := Items[iPath + 2];

  case Mode of
    imLinear:
    begin
      Result.x := LinearInterpolate(p2.x, p3.x, dPath);
      Result.y := LinearInterpolate(p2.y, p3.y, dPath);
      Result.z := LinearInterpolate(p2.z, p3.z, dPath);
    end;

    imCosine:
    begin
      Result.x := CosineInterpolate(p2.x, p3.x, dPath);
      Result.y := CosineInterpolate(p2.y, p3.y, dPath);
      Result.z := CosineInterpolate(p2.z, p3.z, dPath);
    end;

    imCubic:
    begin
      Result.x := CubicInterpolate(p1.x, p2.x, p3.x, p4.x, dPath);
      Result.y := CubicInterpolate(p1.y, p2.y, p3.y, p4.y, dPath);
      Result.z := CubicInterpolate(p1.z, p2.z, p3.z, p4.z, dPath);
    end;
  end;
end;
{$ENDREGION}

end.