unit _Geometry;
{
  ver 0.4

  Mefistofel
    (Костромин Кирилл)
  07.04.2012

  2d operations procedure pack
}

interface

const
  PI = 3.14159265358979;

type

//2d
TVector2 = packed record
    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 Multiply     (A: TVector2; X: Single): TVector2;             inline;
    class operator Multiply     (X: Single; A: TVector2): TVector2;             inline;
    class operator Divide       (A: TVector2; X: Single): TVector2;             inline;
    class operator Negative     (X: TVector2   ): TVector2;                     inline;
    class operator Positive     (X: TVector2   ): TVector2;                     inline;
                                                                           
    class operator Equal        (A, B: TVector2): Boolean;                      inline;
    class operator NotEqual     (A, B: TVector2): Boolean;                      inline;

    constructor Create          (X, Y: Single);
    constructor Vector2         (X, Y: Single);
    constructor GetVector       (Angle, Distance: Single);
    function Dot                (X: TVector2   ): Single;                       inline;
    function Distance           (X: TVector2   ): Single;                       inline;
    function ProjectOnLine      (A, B: TVector2): Single;                       inline;
    function InRectangle        (A, B: TVector2): Boolean;                      inline;
    function InTriangle         (A, B, C: TVector2): Boolean;                   inline; //is point in triangle
    function GetAngle           (A: TVector2   ): Single;            overload;  inline;
    function Middle             (B: TVector2): TVector2;             overload;          //middle point between Self & B
    function Lerp               (B: TVector2; I: Single): TVector2;                     //linear Interpolation
    function GetAngle           : Single;                            overload;  inline;
    function Length             : Single;                                       inline;
    function Normal             : TVector2;                                     inline;
    function Normalize          : TVector2;                                     inline;
  case Integer of
    0: (X, Y: Single);
    1: (U, V: Single);
    2: (A: array[0..1] of Single);
end;

TPoint = packed record
    class operator Explicit     (A: TPoint): TVector2;                overload;
    class operator Implicit     (A: TPoint): TVector2;                overload;
    class operator Trunc        (A: TVector2): TPoint;                overload;
    class operator Round        (A: TVector2): TPoint;                overload;
    class operator Add          (A, B: TPoint ): TPoint;                        inline;
    class operator Subtract     (A, B: TPoint ): TPoint;                        inline;
    class operator Multiply     (A, B: TPoint ): TPoint;              overload; inline;
    class operator Multiply     (A: TPoint; I: Integer ): TPoint;     overload; inline;
    class operator Multiply     (A: TPoint; S: Single  ): TVector2;   overload; inline;
    class operator Divide       (A: TPoint; X: Single  ): TVector2;             inline;
    class operator Negative     (A: TPoint ): TPoint;                           inline;
    class operator Positive     (A: TPoint ): TPoint;                           inline;
    class operator Equal        (A, B: TPoint ): Boolean;                       inline;
    class operator NotEqual     (A, B: TPoint ): Boolean;                       inline;

    constructor Create          (X, Y: Integer);                    
    constructor Point           (X, Y: Integer);
    constructor Vector2i        (X, Y: Integer);
                                 
    case Integer of
      0: (X, Y: integer);
      1: (A: array [0..1] of Integer);
end;

//3d
TVector3 = record
    class operator Explicit     (A: TVector3): TVector2;              overload;
    class operator Implicit     (A: TVector3): TVector2;              overload;
    class operator Explicit     (A: TVector2): TVector3;              overload;
    class operator Implicit     (A: TVector2): TVector3;              overload;
    class operator Explicit     (A: TPoint): TVector3;                overload;
    class operator Implicit     (A: TPoint): TVector3;                overload;
    class operator Add          (A, B: TVector3): TVector3;
    class operator Subtract     (A, B: TVector3): TVector3;
    class operator Multiply     (A, B: TVector3): TVector3;           overload;
    class operator Multiply     (A: TVector3; X: Single): TVector3;   overload;
    class operator Multiply     (X: Single; A: TVector3): TVector3;   overload;
    class operator Divide       (A: TVector3; X: Single): TVector3;
    class operator Negative     (X: TVector3): TVector3;
    class operator Positive     (X: TVector3): TVector3;
    class operator Equal        (A: TVector3; B: TVector3): Boolean;
    class operator NotEqual     (A: TVector3; B: TVector3): Boolean;
    constructor Create          (X, Y, Z: Single);
    constructor Vector3         (X, Y, Z: Single);
    function Length: Single;
    function Dot                (X: TVector3): Single;
    function Distance           (X: TVector3): Single;
    function ProjectOnLine      (A, B: TVector3): Single;
    function Middle             (B: TVector3): TVector3;              overload;  //middle point between Self & B
    function Lerp               (B: TVector3; I: Single): TVector3;              //linear Interpolation Between Self and B
    function InBox              (A, B: TVector3): Boolean;
    function Normalize: TVector3;

  case Integer of
    0: (X, Y, Z: Single);
    1: (R, G, B: Single);
    2: (U, V, W : Single);
    3: (A: array[0..2] of Single);
end;

const
  VECTOR3_UP   : TVector3 = (A : ( 0,  1, 0));
  VECTOR3_DOWN : TVector3 = (A : ( 0, -1, 0));
  VECTOR3_LEFT : TVector3 = (A : ( 1,  0, 0));
  VECTOR3_RIGHT: TVector3 = (A : (-1,  0, 0));   
  VECTOR3_NULL : TVector3 = (A : ( 0,  0, 0));

type

PVector3 = ^TVector3;

TVector4 = record
    class operator Explicit     (A: TVector3): TVector4;              overload;
    class operator Implicit     (A: TVector3): TVector4;              overload;
    class operator Explicit     (A: TVector4): TVector3;              overload;
    class operator Implicit     (A: TVector4): TVector3;              overload;
    class operator Add          (A, B: TVector4): TVector4;
    class operator Subtract     (A, B: TVector4): TVector4;
    class operator Multiply     (A, B: TVector4): TVector4;
    class operator Multiply     (A: TVector4; X: Single): TVector4;
    class operator Multiply     (X: Single; A: TVector4): TVector4;
    class operator Divide       (A: TVector4; X: Single): TVector4;
    class operator Negative     (X: TVector4): TVector4;
    class operator Positive     (X: TVector4): TVector4;
    class operator Equal        (A: TVector4; B: TVector4): Boolean;
    class operator NotEqual     (A: TVector4; B: TVector4): Boolean;
    constructor Create          (X, Y, Z, W: Single);
    constructor Vector4         (X, Y, Z, W: Single);
    function Distance           (X: TVector4): Single;   
    function Lerp               (B: TVector4; I: Single): TVector4;              //linear Interpolation Between Self and B
    function Length: Single;
    function Normalize: TVector4;

  case Integer of
    0: (X, Y, Z, W: Single);
    1: (R, G, B, Alpha: Single);
    2: (A: array[0..3] of Single);
end;

PVector4 = ^TVector4;

//Matrix3
TMatrix3 = record
    class operator Multiply     (A: TMatrix3; X: Single  ): TMatrix3;
    class operator Multiply     (X: Single; A: TMatrix3  ): TMatrix3;
    class operator Multiply     (A: TMatrix3; X: TVector3): TVector3;
    class operator Multiply     (A: TMatrix3; B: TMatrix3): TMatrix3;
    class operator Negative     (X: TMatrix3   ): TMatrix3;
    class operator Positive     (X: TMatrix3   ): TMatrix3;
    class operator Equal        (A, B: TMatrix3): Boolean;
    class operator NotEqual     (A, B: TMatrix3): Boolean;
    constructor Create          (_00, _01, _02,
                                 _10, _11, _12,
                                 _20, _21, _22: Single);              overload;
    constructor Create          (A, B, C: TVector3);                  overload;
    constructor Matrix3         (_00, _01, _02,
                                 _10, _11, _12,
                                 _20, _21, _22: Single);              overload;
    constructor Matrix3         (A, B, C: TVector3);                  overload;
    constructor Rotate          (Angle: Single; Axis: TVector3);      overload;
    constructor Rotate          (Angle, X, Y, Z: Single);             overload;
    function Transpose: TMatrix3;

  case Integer of
    0: (_00, _01, _02,
        _10, _11, _12,
        _20, _21, _22: Single);
    1: (A: array[0..2, 0..2] of Single);
    2: (L: array[0..8] of Single);
end;               

const IDENTITY_MATRIX3: TMatrix3 = (L : ( 1, 0, 0,
                                          0, 1, 0,
                                          0, 0, 1));

type
PMatrix3 = ^TMatrix3;

//Matrix4
TMatrix4 = record
    class operator Explicit     (A: TMatrix3): TMatrix4;              overload;
    class operator Implicit     (A: TMatrix3): TMatrix4;              overload;
    class operator Explicit     (A: TMatrix4): TMatrix3;              overload;
    class operator Implicit     (A: TMatrix4): TMatrix3;              overload;
    class operator Multiply     (A: TMatrix4; X: Single  ): TMatrix4;
    class operator Multiply     (X: Single; A: TMatrix4  ): TMatrix4;
    class operator Multiply     (A: TMatrix4; X: TVector4): TVector4;
    class operator Multiply     (A, B: TMatrix4): TMatrix4;
    class operator Negative     (X: TMatrix4   ): TMatrix4;
    class operator Positive     (X: TMatrix4   ): TMatrix4;
    class operator Equal        (A, B: TMatrix4): Boolean;
    class operator NotEqual     (A, B: TMatrix4): Boolean;
    
    constructor Create          (A: TMatrix3);                        overload;
    constructor Create          (_00, _01, _02, _03,
                                 _10, _11, _12, _13,
                                 _20, _21, _22, _23,
                                 _30, _31, _32, _33: Single);         overload;
    constructor Create          (A: TMatrix3; X, Y, Z: Single);       overload;
    constructor Create          (A: TMatrix3; X: TVector3);           overload;
    constructor Create          (A, B, C, D: TVector4);               overload;
    constructor Matrix4         (A: TMatrix3);                        overload;
    constructor Matrix4         (_00, _01, _02, _03,
                                 _10, _11, _12, _13,
                                 _20, _21, _22, _23,
                                 _30, _31, _32, _33: Single);         overload;
    constructor Matrix4         (A: TMatrix3; X: TVector3);           overload;
    constructor Matrix4         (A: TMatrix3; X, Y, Z: Single);       overload;
    constructor Matrix4         (A, B, C, D: TVector4);               overload;
    function Transpose: TMatrix4;
    function GetPosition: TVector4;                                   overload;

  case Integer of
    0: (_00, _01, _02, _03,
        _10, _11, _12, _13,
        _20, _21, _22, _23,
        _30, _31, _32, _33: Single);
    1: (A: array[0..3] of Array [0..3] of Single);
    2: (L: array[0..15] of Single);
end;


const
  IDENTITY_MATRIX4: TMatrix4 = (L : ( 1, 0, 0, 0,
                                      0, 1, 0, 0,
                                      0, 0, 1, 0,
                                      0, 0, 0, 1));
type

PMatrix4 = ^TMatrix4;

function GetRound     (Angle: Single): Single;
function RotateToAngle(SourceAngle, DestAngle, Speed: Single): Single;

//2d integer functions
function Point        (X, Y: Integer): TPoint;
function Vector2i     (X, Y: Integer): TPoint;

//2d funcions
function Vector2      (X, Y: Single  ): TVector2;
function GetVector    (Angle, Distance: Single): TVector2;
function Distance     (A, B: TVector2): Single;                     overload;
function GetAngle     (A, B: TVector2): Single;                     overload;
function GetAngle     (A: TVector2   ): Single;                     overload;
function Cross        (AB, AE, BB, BE: TVector2): Boolean;          overload;
function Cross        (AB, AE, BB, BE: TVector2;
                       var CrossPoint: TVector2): Boolean;          overload;
function Middle       (A, B: TVector2): TVector2;                   overload;   //middle point between A & B
function Middle       (A, B, C: TVector2): TVector2;                overload;   //middle point between A,B & C
function Lerp         (A, B: TVector2; I: Single): TVector2;        overload;   //linear Interpolation Between A and B
function HalfPlaneSign(A, B, C: TVector2): Smallint;                            // sign show

//3d functions
function Vector3      (X, Y, Z: Single): TVector3;
function Distance     (A, B: TVector3): Single;                     overload;      
function Middle       (A, B: TVector3): TVector3;                   overload;   //middle point between A & B
function Middle       (A, B, C:TVector3): TVector3;                 overload;   //middle point between A,B & C
function Lerp         (A, B: TVector3; I: Single): TVector3;        overload;   //linear Interpolation Between A and B

//4f vector functions
function Vector4      (X, Y, Z, W: Single): TVector4;
function Lerp         (A, B: TVector4; I: Single): TVector4;        overload;   //linear Interpolation Between A and B

//Matrix3 basics
function Rotate(Angle: Single; Axis: TVector3): TMatrix3;

//Matrix4Basics

function Matrix4(A: TMatrix3): TMatrix4;                            overload;
function Matrix4(A: TMatrix3; X: TVector3): TMatrix4;               overload;

implementation

//all
{$REGION 'Basic Functions'}
function ArcTan2(Y, X: Extended): Extended;
asm
  FLD     Y
  FLD     X
  FPATAN
  FWAIT
end;

function ArcCos(X: Single): Single;
begin
  Result := ArcTan2(Sqrt(1 - X * X), X);
end;

function GetRound(Angle: Single): Single; // <= 360 degrees
begin
  Repeat
    If Angle < 0 then
      Angle := Angle + 360;
    If Angle >= 360 then
      Angle := Angle - 360;
  Until (Angle >= 0) and (Angle<360);
  Result:= Angle;
end;
{$ENDREGION}

//2i
{$REGION 'Vector2i'}    
class operator TPoint.Explicit(A: TPoint): TVector2;
begin
  Result.Create(A.X, A.Y);
end;

class operator TPoint.Implicit(A: TPoint): TVector2;
begin
  Result.Create(A.X, A.Y);
end;

class operator TPoint.Trunc(A: TVector2): TPoint;
begin
  Result.Create(Trunc(A.X), Trunc(A.Y));
end;

class operator TPoint.Round(A: TVector2): TPoint;
begin
  Result.Create(Round(A.X), Round(A.Y));
end;

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

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

class operator TPoint.Multiply(A: TPoint; I: Integer): TPoint;
begin
  Result.X:= A.X * I;
  Result.Y:= A.Y * I;
end;

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

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

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

class operator TPoint.Negative(A: TPoint): TPoint;
begin
  Result.X:= - A.X;
  Result.Y:= - A.Y;
end;

class operator TPoint.Positive(A: TPoint): TPoint;  
begin
  Result:= A;
end;

class operator TPoint.Equal(A, B: TPoint): Boolean;   
begin
  Result:= (A.X = B.X) and (A.Y = B.Y);
end;

class operator TPoint.NotEqual(A, B: TPoint): Boolean;
begin
  Result:= (A.X <> B.X) or (A.Y <> B.Y);
end;

constructor TPoint.Create(X, Y: Integer);
begin
  Self.X:= X;
  Self.Y:= Y;
end;
       
constructor TPoint.Point(X, Y: Integer);
begin
  Self.X:= X;
  Self.Y:= Y;
end;

constructor TPoint.Vector2i(X, Y: Integer);
begin
  Self.X:= X;
  Self.Y:= Y;
end;

function Vector2i(X, Y: Integer): TPoint;
begin
  Result.X:= X;
  Result.Y:= Y;
end;

function Point(X, Y: Integer): TPoint;
begin
  Result.X:= X;
  Result.Y:= Y;
end;
{$ENDREGION}

//Vector2
{$REGION 'Vector2'}
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: TVector2; X: Single): TVector2;
begin
  Result.X:= A.X * X;
  Result.Y:= A.Y * X;
end;

class operator TVector2.Multiply(X: Single; A: TVector2): TVector2;
begin
  Result.X:= A.X * X;
  Result.Y:= A.Y * X;
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: TVector2; X: Single): TVector2;
begin
  Result.X:= A.X / X;
  Result.Y:= A.Y / X;
end;

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

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

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

function TVector2.Normalize: TVector2;
var
  d: Double;
begin
  d:= Distance(TVector2.Create(0, 0));
  Result.X:= Self.X / d;
  Result.Y:= Self.Y / d;
end;

function TVector2.Length: Single;
begin
  Result:= Sqrt(Self.X * Self.X + Self.Y * Self.Y);
end;

function TVector2.Distance(X: TVector2): Single;
begin
  Result:= Sqrt(Sqr(X.X - Self.X) + Sqr(X.Y - Self.Y));
end;

function Distance(A, B: TVector2): Single;
var
  ab: TVector2; 
begin
  ab.Vector2(B.X - A.X, B.Y - A.Y);
  Result:= sqrt(ab.X * ab.X + ab.Y * ab.Y);
end;

function TVector2.ProjectOnLine(A, B: TVector2): Single;
var
  Line: TVector2;
  Length: Single;
begin
  Line:= B - A;
  Length:= Line.Length;
  Result:= Line.Dot(Self - A) / (Length * Length);
end;

constructor TVector2.Create(X, Y: Single);
begin
  Self.X:= X;
  Self.Y:= Y;
end;

constructor TVector2.Vector2(X, Y: Single);
begin
  Self.X:= X;
  Self.Y:= Y;
end;

constructor TVector2.GetVector(Angle, Distance: single);
begin
 Self.X:= -sin(Angle / 180 * PI) * Distance;
 Self.Y:=  cos(Angle / 180 * PI) * Distance;
end;

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

function GetAngle(A, B: TVector2): Single;
var
  S, C: Single;
begin
  C:= (B.Y - A.Y) / A.Distance(B);
  If C > 1 then
    C:= 1;
  If C < -1 then
    C:= -1;
  S:= (arccos(C)) * 180 / PI;
  If (A.X - B.X) > 0 then
     S:= GetRound(360-S);
  S:= GetRound(180 - S);
  Result:= S;
end;

function TVector2.GetAngle: Single;
var
  A: TVector2;
begin
  A.Create(0, 0);
  Result:= _Geometry.GetAngle(A, Self);
end;

function TVector2.GetAngle(A: TVector2): Single;
begin
  Result:= _Geometry.GetAngle(Self, A);
end;    

function Middle(A, B: TVector2): TVector2;  //middle point between A & B     
begin
  Result:= (A + B) / 2;
end;

function Middle(A, B, C: TVector2): TVector2;   //middle point between A,B & C   
begin
  Result:= (A + B + C) / 3;
end;

function Lerp(A, B: TVector2; I: Single): TVector2;   //linear Interpolation Between A and B
begin
  Result:= A * I + B * (1 - I);
end;

function TVector2.Middle(B: TVector2): TVector2;  //middle point between Self & B
begin
  Result:= (Self + B) / 2;
end;

function TVector2.Lerp(B: TVector2; I: Single): TVector2; //linear Interpolation
begin
  Result:= Self * I + B * (1 - I);
end;

function GetAngle(A: TVector2): single;
var
  B:TVector2;
begin
  B.Create(0 ,0);
  Result:= _Geometry.GetAngle(B, A);
end;

function TVector2.InRectangle(A, B: TVector2): Boolean;
var
  Temp: Single;//check Point in 2d rectangle
begin
  if A.X > B.X then
    begin
      temp:= A.X;
      A.X:= B.X;
      B.X:= Temp;
    end;
  if A.Y > B.Y then
    begin
      temp:= A.Y;
      A.Y:= B.Y;
      B.Y:= Temp;
    end;
  Result:= (
    (Self.X >= A.X) and
    (Self.X <= B.X) and
    (Self.Y >= A.Y) and
    (Self.Y <= B.Y));
end;  

function TVector2.InTriangle(A, B, C: TVector2): Boolean; //is point in triangle
begin
  Result:= False;
  if (HalfPlaneSign(A, B, C) = HalfPlaneSign(A, B, Self)) then
    if (HalfPlaneSign(C, A, B) = HalfPlaneSign(C, A, Self)) then
      if (HalfPlaneSign(B, C, A) = HalfPlaneSign(B, C, Self)) then
        Result:= True;
end;

class operator TVector2.Equal(A: TVector2; B:TVector2): Boolean;
begin
  result:= (A.X = B.X) and (A.Y = B.Y);
end;

class operator TVector2.NotEqual(A: TVector2; B: TVector2): Boolean;
begin
  result:= (A.X <> B.X) or (A.Y <> B.Y);
end;

function Vector2(X, Y: Single): TVector2;
begin
  Result.X:= X;
  Result.Y:= Y;
end;

function GetVector(Angle, Distance: Single): TVector2;
begin
  Result.X:=  sin(Angle / 180 * PI) * Distance;
  Result.Y:= -cos(Angle / 180 * PI) * Distance;
end;


function Cross(AB, AE, BB, BE: TVector2): Boolean; overload;     
Var
  D,
  Ad, Bd,
  At, Bt: Extended;
begin
  Result:= false;
  D:= (AE.X - AB.X) * (BB.Y - BE.Y) - (BB.X - BE.X) * (AE.Y - AB.Y);
  If D <> 0 then
    begin
      Ad:= (BB.X - AB.X) * (BB.Y - BE.Y) - (BB.X - BE.X) * (BB.Y - AB.Y);
      Bd:= (AE.X - AB.X) * (BB.Y - AB.Y) - (BB.X - AB.X) * (AE.Y - AB.Y);
      At:= Ad / D;
      Bt:= Bd / D;
      If
        (At >= 0) and
        (At <= 1) and
        (Bt >= 0) and
        (Bt <= 1) then
        begin
          Result:= true;
        end;
    end;
end;

function Cross(AB, AE, BB, BE: TVector2; var CrossPoint:TVector2): Boolean; overload;
Var
  D,
  Ad, Bd,
  At, Bt: Extended;
begin
  Result:= false;
  CrossPoint.Vector2(0, 0);
  D:= (AE.X - AB.X) * (BB.Y - BE.Y) - (BB.X - BE.X) * (AE.Y - AB.Y);
  If D <> 0 then
    begin
      Ad:= (BB.X - AB.X) * (BB.Y - BE.Y) - (BB.X - BE.X) * (BB.Y - AB.Y);
      Bd:= (AE.X - AB.X) * (BB.Y - AB.Y) - (BB.X - AB.X) * (AE.Y - AB.Y);
      At:= Ad / D;
      Bt:= Bd / D;
      If
        (At >= 0) and
        (At <= 1) and
        (Bt >= 0) and
        (Bt <= 1) then
        begin
          CrossPoint.Vector2(
            At * AE.X + (1 - At) * AB.X,
            At * AE.Y + (1 - At) * AB.Y);
          Result:= true;
        end;
    end;
end;

function RotateToAngle(SourceAngle, DestAngle, Speed: Single): Single;
var
  Ang,
  Source: Single;
begin
  Source:= SourceAngle;
  if ((SourceAngle < 10) or (DestAngle<10)) then
    begin
      SourceAngle:= GetRound(SourceAngle + 90);
      DestAngle:= GetRound(DestAngle + 90);
    end;
  if ((SourceAngle > 350) or (DestAngle>350)) then
    begin
      SourceAngle:= GetRound(SourceAngle - 90);
      DestAngle:= GetRound(DestAngle - 90);
    end;
  Ang:= Speed;
  //если до результата нужно повернуть меньше, чем скорость - изменяем скорость на остаток
  If (Abs(DestAngle - SourceAngle) < Speed) then
    Ang:= Round(Abs(DestAngle - SourceAngle));
  //находит куда поворачивать ближе и выдает градус текущего поворота
  If (SourceAngle < DestAngle) then
    If (DestAngle-SourceAngle<=180) then
      Ang:= Ang
    else
      Ang:= -1 * Ang
  else
    If (SourceAngle-DestAngle<180) then
      Ang:=-1*Ang
    else
      Ang:=Ang;
  Result:= GetRound(Ang + Source);
end;

function HalfPlaneSign(A, B, C: TVector2): Smallint;// sign show
Var
  Delta: TVector2;
begin
  Delta:= A - B;
  if (Delta.X * (C.Y - A.Y) - Delta.Y * (C.X - A.X)) > 0 then
    Result:= 1
  else
    REsult:= -1;
end;

{$ENDREGION}

//Vector3
{$REGION 'Vector3'}      
class operator TVector3.Explicit(A: TVector3): TVector2;
begin
  Result.Create(A.X, A.Y);
end;

class operator TVector3.Implicit(A: TVector3): TVector2;
begin
  Result.Create(A.X, A.Y);
end;

class operator TVector3.Explicit(A: TVector2): TVector3;
begin
  Result.Create(A.X, A.Y, 0);
end;

class operator TVector3.Implicit(A: TVector2): TVector3;
begin
  Result.Create(A.X, A.Y, 0);
end;

class operator TVector3.Explicit(A: TPoint): TVector3;
begin
  Result.Create(A.X, A.Y, 0);
end;

class operator TVector3.Implicit(A: TPoint): TVector3;
begin
  Result.Create(A.X, A.Y, 0);
end;

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

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

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

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

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

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

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

class operator TVector3.Positive(X: TVector3): TVector3;
begin
  Result:= X;
end;

function TVector3.Normalize: TVector3;
var
  d: Double;
begin
  d:= Distance(TVector3.Create(0, 0, 0));
  Result.X:= Self.X / d;
  Result.Y:= Self.Y / d; 
  Result.Z:= Self.Z / d;
end;

function TVector3.Length: Single;
begin
  Result:= Sqrt(Self.X * Self.X + Self.Y * Self.Y + Self.Z * Self.Z);
end;

function TVector3.Distance(X: TVector3): Single;
begin
  Result:= Sqrt(Sqr(X.X - Self.X) + Sqr(X.Y - Self.Y) + Sqr(X.Z - Self.Z));
end;

function Distance(A, B: TVector3): Single;
var
  ab: TVector3; 
begin
  ab.Vector3(B.X - A.X, B.Y - A.Y, B.Z - A.Z);
  Result:= sqrt(ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z);
end;

constructor TVector3.Create(X, Y, Z: Single);
begin
  Self.X:= X;
  Self.Y:= Y;  
  Self.Z:= Z;
end;

constructor TVector3.Vector3(X, Y, Z: Single);
begin
  Self.X:= X;
  Self.Y:= Y;    
  Self.Z:= Z;
end;

function TVector3.Dot(X: TVector3): Single;
begin
  Result:= X.X * Self.X + X.Y * Self.Y + X.Z * Self.Z;
end;

function TVector3.InBox(A, B: TVector3): Boolean;
var
  Temp: Single;//check Point in 3d Box
begin
  if A.X > B.X then
    begin
      temp:= A.X;
      A.X:= B.X;
      B.X:= Temp;
    end;
  if A.Y > B.Y then
    begin
      temp:= A.Y;
      A.Y:= B.Y;
      B.Y:= Temp;
    end;     
  if A.Z > B.Z then
    begin
      temp:= A.Z;
      A.Z:= B.Z;
      B.Z:= Temp;
    end;
  Result:= (
    (Self.X >= A.X) and
    (Self.X <= B.X) and
    (Self.Y >= A.Y) and
    (Self.Y <= B.Y) and
    (Self.Z >= A.Z) and
    (Self.Z <= B.Z));
end;

function TVector3.ProjectOnLine(A, B: TVector3) : Single;
var
  Line: TVector3;
  Length: Single;
begin
  Line:= B - A;
  Length:= Line.Length;
  Result:= Line.Dot(Self - A) / (Length * Length);
end;

function TVector3.Middle(B: TVector3): TVector3; //middle point between Self & B
begin
  Result:= (Self + B) / 2; 
end;

function Middle(A, B: TVector3): TVector3; //middle point between Self & B
begin
  Result:= (A + B) / 2;
end;

function Middle(A, B, C: TVector3): TVector3; //middle point between Self & B
begin
  Result:= (A + B + C) / 3; 
end;

function TVector3.Lerp(B: TVector3; I: Single): TVector3; //linear Interpolation
begin
  Result:= Self * I + B * (1 - I);
end;

function Lerp(A, B: TVector3; I: Single): TVector3; //linear Interpolation
begin
  Result:= A * I + B * (1 - I);
end;

class operator TVector3.Equal(A: TVector3; B:TVector3): Boolean;
begin
  result:= (A.X = B.X) and (A.Y = B.Y) and (A.Z = B.Z);
end;

class operator TVector3.NotEqual(A: TVector3; B: TVector3): Boolean;
begin
  result:= (A.X <> B.X) or (A.Y <> B.Y) or (A.Z <> B.Z);
end;

function Vector3(X, Y, Z: Single): TVector3;
begin
  Result.Vector3(X, Y, Z);
end;
{$ENDREGION}

//Vector4
{$REGION 'Vector4'}
class operator TVector4.Explicit(A: TVector3): TVector4;
begin
  Result.Create(A.X, A.Y, A.Z, 0);
end;

class operator TVector4.Implicit(A: TVector3): TVector4;
begin         
  Result.Create(A.X, A.Y, A.Z, 0);
end;

class operator TVector4.Explicit(A: TVector4): TVector3;
begin
  Result.Create(A.X, A.Y, A.Z);
end;

class operator TVector4.Implicit(A: TVector4): TVector3;
begin                  
  Result.Create(A.X, A.Y, A.Z);
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: TVector4; X: Single): TVector4;
begin
  Result.X:= A.X * X;
  Result.Y:= A.Y * X;
  Result.Z:= A.Z * X;
  Result.W:= A.W * X;
end;

class operator TVector4.Multiply(X: Single; A: TVector4): TVector4;
begin
  Result.X:= A.X * X;
  Result.Y:= A.Y * X;
  Result.Z:= A.Z * X;
  Result.W:= A.W * X;
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.Z;
end;

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

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

class operator TVector4.Positive(X: TVector4): TVector4;
begin
  Result:= X;
end;

function TVector4.Normalize: TVector4;
var
  d: Double;
begin
  d:= Distance(TVector4.Create(0, 0, 0, 0));
  Result.X:= Self.X / d;
  Result.Y:= Self.Y / d; 
  Result.Z:= Self.Z / d;  
  Result.Z:= Self.Z / d;
end;

function TVector4.Length: Single;
begin
  Result:= Sqrt(Self.X * Self.X + Self.Y * Self.Y + Self.Z * Self.Z + Self.W * Self.W);
end;

constructor TVector4.Create(X, Y, Z, W: Single);
begin
  Self.X:= X;
  Self.Y:= Y;
  Self.Z:= Z;
  Self.W:= W;
end;

constructor TVector4.Vector4(X, Y, Z, W: Single);
begin
  Self.X:= X;
  Self.Y:= Y;    
  Self.Z:= Z; 
  Self.W:= W;
end;

function TVector4.Distance(X: TVector4): Single;
begin
  Result:= Sqrt(
      Sqr(X.X - Self.X) +
      Sqr(X.Y - Self.Y) +
      Sqr(X.Z - Self.Z) +
      Sqr(X.W - Self.W));
end;

function TVector4.Lerp(B: TVector4; I: Single): TVector4; //linear Interpolation
begin
  Result:= Self * I + B * (1 - I);
end;

function Lerp(A, B: TVector4; I: Single): TVector4; //linear Interpolation
begin
  Result:= A * I + B * (1 - I);
end;

class operator TVector4.Equal(A: TVector4; B:TVector4): Boolean;
begin
  result:= (A.X = B.X) and (A.Y = B.Y) and (A.Z = B.Z) and (A.W = B.W);
end;

class operator TVector4.NotEqual(A: TVector4; B: TVector4): Boolean;
begin
  result:= (A.X <> B.X) or (A.Y <> B.Y) or (A.Z <> B.Z) or (A.W <> B.W);
end;

function Vector4(X, Y, Z, W: Single): TVector4;
begin
  Result.Vector4(X, Y, Z, W);
end;
{$ENDREGION}

//Matrix3
{$REGION 'Matrix3'}
class operator TMatrix3.Multiply(A: TMatrix3; X: Single): TMatrix3;
begin
  Result.A[0, 0]:= A.A[0, 0] * X;
  Result.A[0, 1]:= A.A[0, 1] * X;
  Result.A[0, 2]:= A.A[0, 2] * X;
  Result.A[1, 0]:= A.A[1, 0] * X;
  Result.A[1, 1]:= A.A[1, 1] * X;
  Result.A[1, 2]:= A.A[1, 2] * X;
  Result.A[2, 0]:= A.A[2, 0] * X;
  Result.A[2, 1]:= A.A[2, 1] * X;
  Result.A[2, 2]:= A.A[2, 2] * X;
end;

class operator TMatrix3.Multiply(X: Single; A: TMatrix3): TMatrix3;
begin                    
  Result.A[0, 0]:= A.A[0, 0] * X;
  Result.A[0, 1]:= A.A[0, 1] * X;
  Result.A[0, 2]:= A.A[0, 2] * X;
  Result.A[1, 0]:= A.A[1, 0] * X;
  Result.A[1, 1]:= A.A[1, 1] * X;
  Result.A[1, 2]:= A.A[1, 2] * X;
  Result.A[2, 0]:= A.A[2, 0] * X;
  Result.A[2, 1]:= A.A[2, 1] * X;
  Result.A[2, 2]:= A.A[2, 2] * X;
end;

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

class operator TMatrix3.Multiply(A:TMatrix3; B: TMatrix3): TMatrix3;
var
  t: TMatrix3; 
begin
  t._00:= (A._00 * B._00 + A._01 * B._10 + A._02 * B._20);
  t._01:= (A._00 * B._01 + A._01 * B._11 + A._02 * B._21);
  t._02:= (A._00 * B._02 + A._01 * B._12 + A._02 * B._22);
  t._10:= (A._10 * B._00 + A._11 * B._10 + A._12 * B._20);
  t._11:= (A._10 * B._01 + A._11 * B._11 + A._12 * B._21);
  t._12:= (A._10 * B._02 + A._11 * B._12 + A._12 * B._22);
  t._20:= (A._20 * B._00 + A._21 * B._10 + A._22 * B._20);
  t._21:= (A._20 * B._01 + A._21 * B._11 + A._22 * B._21);
  t._22:= (A._20 * B._02 + A._21 * B._12 + A._22 * B._22);
  Result:= t;
end;

class operator TMatrix3.Negative(X: TMatrix3): TMatrix3;  
begin   
  Result:= X * -1;
end;

class operator TMatrix3.Positive(X: TMatrix3): TMatrix3; 
begin
  Result:= X;
end;

class operator TMatrix3.Equal(A, B: TMatrix3): Boolean;
begin
  Result:= (A.A[0, 0] = B.A[0, 0]) and
           (A.A[1, 0] = B.A[1, 0]) and
           (A.A[2, 0] = B.A[2, 0]) and
           (A.A[0, 1] = B.A[0, 1]) and
           (A.A[1, 1] = B.A[1, 1]) and
           (A.A[2, 1] = B.A[2, 1]) and
           (A.A[0, 2] = B.A[0, 2]) and
           (A.A[1, 2] = B.A[1, 2]) and
           (A.A[2, 2] = B.A[2, 2]);
end;

class operator TMatrix3.NotEqual(A, B: TMatrix3): Boolean;
begin
  Result:= not (A = B);
end;

constructor TMatrix3.Create(_00, _01, _02,
                            _10, _11, _12,
                            _20, _21, _22: Single);   
begin
  Self.A[0, 0]:=_00; Self.A[0, 1]:=_01; Self.A[0, 2]:=_02;
  Self.A[1, 0]:=_10; Self.A[1, 1]:=_11; Self.A[1, 2]:=_12;
  Self.A[2, 0]:=_20; Self.A[2, 1]:=_21; Self.A[2, 2]:=_22;
end;                                              

constructor TMatrix3.Create(A, B, C: TVector3);
begin
  Self.A[0, 0]:=A.A[0]; Self.A[0, 1]:=A.A[1]; Self.A[0, 2]:=A.A[2];
  Self.A[1, 0]:=B.A[0]; Self.A[1, 1]:=B.A[1]; Self.A[1, 2]:=B.A[2];
  Self.A[2, 0]:=C.A[0]; Self.A[2, 1]:=C.A[1]; Self.A[2, 2]:=C.A[2];
end;                                                    

constructor TMatrix3.Matrix3(_00, _01, _02,
                             _10, _11, _12,
                             _20, _21, _22: Single);
begin                            
  Self.A[0, 0]:=_00; Self.A[0, 1]:=_01; Self.A[0, 2]:=_02;
  Self.A[1, 0]:=_10; Self.A[1, 1]:=_11; Self.A[1, 2]:=_12;
  Self.A[2, 0]:=_20; Self.A[2, 1]:=_21; Self.A[2, 2]:=_22;
end;                                              

constructor TMatrix3.Matrix3(A, B, C: TVector3);
begin
  Self.A[0, 0]:= A.A[0]; Self.A[0, 1]:= A.A[1]; Self.A[0, 2]:= A.A[2];
  Self.A[1, 0]:= B.A[0]; Self.A[1, 1]:= B.A[1]; Self.A[1, 2]:= B.A[2];
  Self.A[2, 0]:= C.A[0]; Self.A[2, 1]:= C.A[1]; Self.A[2, 2]:= C.A[2];
end;                                                      

constructor TMatrix3.Rotate(Angle: Single; Axis: TVector3);
begin
 Angle:= Angle/180*PI;
 Self.Matrix3(
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[0] * Axis.A[0],              //00
  (1 - Cos(Angle)) * Axis.A[0] * Axis.A[1] - Sin(Angle) * Axis.A[2],  //01
  (1 - Cos(Angle)) * Axis.A[0] * Axis.A[2] + Sin(Angle) * Axis.A[1],  //02
  (1 - Cos(Angle)) * Axis.A[1] * Axis.A[0] + Sin(Angle) * Axis.A[2],  //10
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[1] * Axis.A[1],              //11
  (1 - Cos(Angle)) * Axis.A[1] * Axis.A[2] - sin(Angle) * Axis.A[0],  //12
  (1 - Cos(Angle)) * Axis.A[2] * Axis.A[0] - sin(Angle) * Axis.A[1],  //20
  (1 - Cos(Angle)) * Axis.A[2] * Axis.A[1] + Sin(Angle) * Axis.A[0],  //21
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[2] * Axis.A[2]               //22
 );
end;

constructor TMatrix3.Rotate(Angle, X, Y, Z: Single);
begin
 Angle:= Angle / 180*PI;
 Self.Matrix3(
  Cos(Angle) + (1 - Cos(Angle)) * X * X,      //00
  (1 - Cos(Angle)) * X * Y - Sin(Angle) * Z,  //01
  (1 - Cos(Angle)) * X * Z + Sin(Angle) * Y,  //02
  (1 - Cos(Angle)) * Y * X + Sin(Angle) * Z,  //10
  Cos(Angle) + (1 - Cos(Angle)) * Y * Y,      //11
  (1 - Cos(Angle)) * Y * Z - sin(Angle) * X,  //12
  (1 - Cos(Angle)) * Z * X - sin(Angle) * Y,  //20
  (1 - Cos(Angle)) * Z * Y + Sin(Angle) * X,  //21
  Cos(Angle) + (1 - Cos(Angle)) * Z * Z       //22
 );
end;

function Rotate(Angle: Single; Axis: TVector3): TMatrix3;
begin
 Angle:= Angle/180*PI;
 Result.Matrix3(
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[0] * Axis.A[0],              //00
  (1 - Cos(Angle)) * Axis.A[0] * Axis.A[1] - Sin(Angle) * Axis.A[2],  //01
  (1 - Cos(Angle)) * Axis.A[0] * Axis.A[2] + Sin(Angle) * Axis.A[1],  //02
  (1 - Cos(Angle)) * Axis.A[1] * Axis.A[0] + Sin(Angle) * Axis.A[2],  //10
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[1] * Axis.A[1],              //11
  (1 - Cos(Angle)) * Axis.A[1] * Axis.A[2] - sin(Angle) * Axis.A[0],  //12
  (1 - Cos(Angle)) * Axis.A[2] * Axis.A[0] - sin(Angle) * Axis.A[1],  //20
  (1 - Cos(Angle)) * Axis.A[2] * Axis.A[1] + Sin(Angle) * Axis.A[0],  //21
  Cos(Angle) + (1 - Cos(Angle)) * Axis.A[2] * Axis.A[2]               //22
 );
end;

function TMatrix3.Transpose: TMatrix3;
begin
  Result.A[0, 0]:= Self.A[0, 0];
  Result.A[0, 1]:= Self.A[1, 0];
  Result.A[0, 2]:= Self.A[2, 0];
  Result.A[1, 0]:= Self.A[0, 1];
  Result.A[1, 1]:= Self.A[1, 1];
  Result.A[1, 2]:= Self.A[2, 1];
  Result.A[2, 0]:= Self.A[0, 2];
  Result.A[2, 1]:= Self.A[1, 2];
  Result.A[2, 2]:= Self.A[2, 2];
end;
{$ENDREGION}

//Matrix4
{$REGION 'Matrix4'}
class operator TMatrix4.Explicit(A: TMatrix3): TMatrix4;
begin          
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0, 2];  Result.A[0, 3]:= 0;
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1, 2];  Result.A[1, 3]:= 0;
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2, 2];  Result.A[2, 3]:= 0;
  Result.A[3, 0]:= 0;         Result.A[3, 1]:= 0;         Result.A[3, 2]:= 0;          Result.A[3, 3]:= 1;
end;

class operator TMatrix4.Implicit(A: TMatrix3): TMatrix4;
begin
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0, 2];  Result.A[0, 3]:= 0;
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1, 2];  Result.A[1, 3]:= 0;
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2, 2];  Result.A[2, 3]:= 0;
  Result.A[3, 0]:= 0;         Result.A[3, 1]:= 0;         Result.A[3, 2]:= 0;          Result.A[3, 3]:= 1;
end;

class operator TMatrix4.Explicit(A: TMatrix4): TMatrix3;
begin
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0, 2];
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1, 2];
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2, 2];
end;

class operator TMatrix4.Implicit(A: TMatrix4): TMatrix3;
begin
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0, 2];
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1, 2];
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2, 2];
end;

class operator TMatrix4.Multiply(A: TMatrix4; X: Single): TMatrix4;
begin
  Result.A[0, 0]:= A.A[0, 0] * X;
  Result.A[0, 1]:= A.A[0, 1] * X;
  Result.A[0, 2]:= A.A[0, 2] * X;
  Result.A[0, 3]:= A.A[0, 3] * X;
  Result.A[1, 0]:= A.A[1, 0] * X;
  Result.A[1, 1]:= A.A[1, 1] * X;
  Result.A[1, 2]:= A.A[1, 2] * X;   
  Result.A[1, 3]:= A.A[1, 3] * X;
  Result.A[2, 0]:= A.A[2, 0] * X;
  Result.A[2, 1]:= A.A[2, 1] * X;
  Result.A[2, 2]:= A.A[2, 2] * X;
  Result.A[2, 3]:= A.A[2, 3] * X;     
  Result.A[3, 0]:= A.A[3, 0] * X;
  Result.A[3, 1]:= A.A[3, 1] * X;
  Result.A[3, 2]:= A.A[3, 2] * X;
  Result.A[3, 3]:= A.A[3, 3] * X;
end;

class operator TMatrix4.Multiply(X: Single; A: TMatrix4): TMatrix4;
begin                    
  Result.A[0, 0]:= A.A[0, 0] * X;
  Result.A[0, 1]:= A.A[0, 1] * X;
  Result.A[0, 2]:= A.A[0, 2] * X;
  Result.A[0, 3]:= A.A[0, 3] * X;
  Result.A[1, 0]:= A.A[1, 0] * X;
  Result.A[1, 1]:= A.A[1, 1] * X;
  Result.A[1, 2]:= A.A[1, 2] * X;   
  Result.A[1, 3]:= A.A[1, 3] * X;
  Result.A[2, 0]:= A.A[2, 0] * X;
  Result.A[2, 1]:= A.A[2, 1] * X;
  Result.A[2, 2]:= A.A[2, 2] * X;
  Result.A[2, 3]:= A.A[2, 3] * X;     
  Result.A[3, 0]:= A.A[3, 0] * X;
  Result.A[3, 1]:= A.A[3, 1] * X;
  Result.A[3, 2]:= A.A[3, 2] * X;
  Result.A[3, 3]:= A.A[3, 3] * X;
end;

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

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

class operator TMatrix4.Negative(X: TMatrix4): TMatrix4;  
begin   
  Result:= X * -1;
end;

class operator TMatrix4.Positive(X: TMatrix4): TMatrix4; 
begin
  Result:= X;
end;

class operator TMatrix4.Equal(A, B: TMatrix4): Boolean;
begin
  Result:= (A.A[0, 0] = B.A[0, 0]) and
           (A.A[1, 0] = B.A[1, 0]) and
           (A.A[2, 0] = B.A[2, 0]) and     
           (A.A[3, 0] = B.A[3, 0]) and
           (A.A[0, 1] = B.A[0, 1]) and
           (A.A[1, 1] = B.A[1, 1]) and
           (A.A[2, 1] = B.A[2, 1]) and   
           (A.A[3, 1] = B.A[3, 1]) and
           (A.A[0, 2] = B.A[0, 2]) and
           (A.A[1, 2] = B.A[1, 2]) and
           (A.A[2, 2] = B.A[2, 2]) and
           (A.A[3, 2] = B.A[3, 2]) and
           (A.A[0, 3] = B.A[0, 3]) and
           (A.A[1, 3] = B.A[1, 3]) and
           (A.A[2, 3] = B.A[2, 3]) and
           (A.A[3, 3] = B.A[3, 3]);
end;

class operator TMatrix4.NotEqual(A, B: TMatrix4): Boolean;
begin
  Result:= not (A = B);
end;

constructor TMatrix4.Create(_00, _01, _02, _03,
                            _10, _11, _12, _13,
                            _20, _21, _22, _23,
                            _30, _31, _32, _33: Single);  
begin
  Self.A[0, 0]:= _00; Self.A[0, 1]:= _01; Self.A[0, 2]:= _02;  Self.A[0, 3]:= _03;
  Self.A[1, 0]:= _10; Self.A[1, 1]:= _11; Self.A[1, 2]:= _12;  Self.A[1, 3]:= _13;
  Self.A[2, 0]:= _20; Self.A[2, 1]:= _21; Self.A[2, 2]:= _22;  Self.A[2, 3]:= _23;  
  Self.A[3, 0]:= _30; Self.A[3, 1]:= _31; Self.A[3, 2]:= _32;  Self.A[3, 3]:= _33;
end;

constructor TMatrix4.Create(A, B, C, D: TVector4);
begin
  Self.A[0, 0]:= A.A[0]; Self.A[0, 1]:= A.A[1]; Self.A[0, 2]:= A.A[2]; Self.A[0, 3]:= A.A[3];
  Self.A[1, 0]:= B.A[0]; Self.A[1, 1]:= B.A[1]; Self.A[1, 2]:= B.A[2]; Self.A[1, 3]:= B.A[3];
  Self.A[2, 0]:= C.A[0]; Self.A[2, 1]:= C.A[1]; Self.A[2, 2]:= C.A[2]; Self.A[2, 3]:= C.A[3];
  Self.A[3, 0]:= D.A[0]; Self.A[3, 1]:= D.A[1]; Self.A[3, 2]:= D.A[2]; Self.A[3, 3]:= D.A[3];
end;

constructor TMatrix4.Create(A: TMatrix3);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= 0;         Self.A[3, 1]:= 0;         Self.A[3, 2]:= 0;         Self.A[3, 3]:= 1;
end;

constructor TMatrix4.Create(A: TMatrix3; X, Y, Z: Single);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= X;         Self.A[3, 1]:= Y;         Self.A[3, 2]:= Z;         Self.A[3, 3]:= 1;
end;

constructor TMatrix4.Create(A: TMatrix3; X: TVector3);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= X.X;       Self.A[3, 1]:= X.Y;       Self.A[3, 2]:= X.Z;       Self.A[3, 3]:= 1;
end;

constructor TMatrix4.Matrix4(_00, _01, _02, _03,
                             _10, _11, _12, _13,
                             _20, _21, _22, _23,
                             _30, _31, _32, _33: Single);  
begin
  Self.A[0, 0]:= _00; Self.A[0, 1]:= _01; Self.A[0, 2]:= _02;  Self.A[0, 3]:= _03;
  Self.A[1, 0]:= _10; Self.A[1, 1]:= _11; Self.A[1, 2]:= _12;  Self.A[1, 3]:= _13;
  Self.A[2, 0]:= _20; Self.A[2, 1]:= _21; Self.A[2, 2]:= _22;  Self.A[2, 3]:= _23;  
  Self.A[3, 0]:= _30; Self.A[3, 1]:= _31; Self.A[3, 2]:= _32;  Self.A[3, 3]:= _33;
end;

constructor TMatrix4.Matrix4(A, B, C, D: TVector4);
begin
  Self.A[0, 0]:= A.A[0]; Self.A[0, 1]:= A.A[1]; Self.A[0, 2]:= A.A[2]; Self.A[0, 3]:= A.A[3];
  Self.A[1, 0]:= B.A[0]; Self.A[1, 1]:= B.A[1]; Self.A[1, 2]:= B.A[2]; Self.A[1, 3]:= B.A[3];
  Self.A[2, 0]:= C.A[0]; Self.A[2, 1]:= C.A[1]; Self.A[2, 2]:= C.A[2]; Self.A[2, 3]:= C.A[3];
  Self.A[3, 0]:= D.A[0]; Self.A[3, 1]:= D.A[1]; Self.A[3, 2]:= D.A[2]; Self.A[3, 3]:= D.A[3];
end;

constructor TMatrix4.Matrix4(A: TMatrix3);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= 0;         Self.A[3, 1]:= 0;         Self.A[3, 2]:= 0;         Self.A[3, 3]:= 1;
end;

function Matrix4(A: TMatrix3): TMatrix4;
begin
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0,2];  Result.A[0, 3]:= 0;
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1,2];  Result.A[1, 3]:= 0;
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2,2];  Result.A[2, 3]:= 0;
  Result.A[3, 0]:= 0;         Result.A[3, 1]:= 0;         Result.A[3, 2]:= 0;         Result.A[3, 3]:= 1;
end;

constructor TMatrix4.Matrix4(A: TMatrix3; X, Y, Z: Single);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= X;         Self.A[3, 1]:= Y;         Self.A[3, 2]:= Z;         Self.A[3, 3]:= 1;
end;

constructor TMatrix4.Matrix4(A: TMatrix3; X: TVector3);
begin
  Self.A[0, 0]:= A.A[0, 0]; Self.A[0, 1]:= A.A[0, 1]; Self.A[0, 2]:= A.A[0,2];  Self.A[0, 3]:= 0;
  Self.A[1, 0]:= A.A[1, 0]; Self.A[1, 1]:= A.A[1, 1]; Self.A[1, 2]:= A.A[1,2];  Self.A[1, 3]:= 0;
  Self.A[2, 0]:= A.A[2, 0]; Self.A[2, 1]:= A.A[2, 1]; Self.A[2, 2]:= A.A[2,2];  Self.A[2, 3]:= 0;
  Self.A[3, 0]:= X.X;       Self.A[3, 1]:= X.Y;       Self.A[3, 2]:= X.Z;       Self.A[3, 3]:= 1;
end;

function Matrix4(A: TMatrix3; X: TVector3): TMatrix4;
begin
  Result.A[0, 0]:= A.A[0, 0]; Result.A[0, 1]:= A.A[0, 1]; Result.A[0, 2]:= A.A[0,2];  Result.A[0, 3]:= 0;
  Result.A[1, 0]:= A.A[1, 0]; Result.A[1, 1]:= A.A[1, 1]; Result.A[1, 2]:= A.A[1,2];  Result.A[1, 3]:= 0;
  Result.A[2, 0]:= A.A[2, 0]; Result.A[2, 1]:= A.A[2, 1]; Result.A[2, 2]:= A.A[2,2];  Result.A[2, 3]:= 0;
  Result.A[3, 0]:= X.X;       Result.A[3, 1]:= X.Y;       Result.A[3, 2]:= X.Z;       Result.A[3, 3]:= 1;
end;

function TMatrix4.Transpose: TMatrix4;
begin
  Result.A[0, 0]:= Self.A[0, 0];
  Result.A[0, 1]:= Self.A[1, 0];
  Result.A[0, 2]:= Self.A[2, 0];  
  Result.A[0, 3]:= Self.A[3, 0];
  Result.A[1, 0]:= Self.A[0, 1];
  Result.A[1, 1]:= Self.A[1, 1];
  Result.A[1, 2]:= Self.A[2, 1]; 
  Result.A[1, 3]:= Self.A[3, 1];
  Result.A[2, 0]:= Self.A[0, 2];
  Result.A[2, 1]:= Self.A[1, 2];
  Result.A[2, 2]:= Self.A[2, 2];
  Result.A[2, 3]:= Self.A[3, 2];
  Result.A[3, 0]:= Self.A[0, 3];
  Result.A[3, 1]:= Self.A[1, 3];
  Result.A[3, 2]:= Self.A[2, 3];
  Result.A[3, 3]:= Self.A[3, 3];
end;

function TMatrix4.GetPosition: TVector4;
begin
  Result.A[0]:= Self.A[3, 0];
  Result.A[1]:= Self.A[3, 1];
  Result.A[2]:= Self.A[3, 2];
  Result.A[3]:= Self.A[3, 3];
end;
{$ENDREGION}
end.

