unit l_math;
(*************************************)
(* Math module by XProger            *)
(*************************************)
(* site : http://XProger.mirgames.ru *)
(* mail : XProger@list.ru            *)
(*************************************)
interface

uses
 Windows;

const
 rad2deg = 180/pi;
 deg2rad = pi/180;

type
 TFace = array [0..2] of WORD;

 TVector2D = record
  X, Y : single;
 end;

 TVector = record
  X, Y, Z : single;
 end;

 TQuat = record
  X, Y, Z, W : single;
 end; 

 TMatrix4 = array [0..3, 0..3] of single;

 TPlane = record
  Normal : TVector;
  V      : TVector;
  D      : single;
 end;

 function max(x, y: integer): integer; overload;
 function max(x, y: single): single; overload;
 function min(x, y: integer): integer; overload;
 function min(x, y: single): single; overload;

 function ArcTan2(const Y, X: single): single;
 function ArcCos(const X: single): single;
// 3D
 function Vector(X, Y, Z: single): TVector;
 function V_Add(v1, v2: TVector): TVector;
 function V_Sub(v1, v2: TVector): TVector;
 function V_Mult(v: TVector; d: single): TVector;
 function V_Dist(var v1, v2: TVector): single;
 function V_Distq(var v1, v2: TVector): single;
 function V_Length(v: TVector): single;
 function V_Normalize(var v: TVector): TVector;
 function V_Dot(v1, v2: TVector): single;
 function V_Cross(var v1, v2: TVector): TVector;
 function V_Angle(v1, v2: TVector): single;
 function V_Interpolate(v1, v2: TVector; k: single): TVector;
// Plane
 function P_Normal(v1, v2, v3: TVector): TVector;
 function P_Plane(v1, v2, v3: TVector): TPlane;
 function P_Dist(Pos: TVector; Plane: TPlane): single;
 function P_Classify(Pos: TVector; Plane: TPlane): single;
 function P_InsideTri(v, v1, v2, v3: TVector): boolean;

// QUATERNIONS //
function Quaternion(X, Y, Z, W: single): TQuat; overload;
function Quaternion(v: TVector; Angle: single): TQuat; overload;
procedure Q_Matrix(q: TQuat; var m: TMatrix4);

// Matrix
function M_Mult(var m: TMatrix4; v: TVector): TVector; overload;
function M_Mult(var a, b: TMatrix4): TMatrix4; overload;
function M_Rotation(v: TVector; Angle: single): TMatrix4;

implementation


function max(x, y: integer): integer; overload;
begin
if x > y then
 Result := x
else
 Result := y;
end;

function max(x, y: single): single; overload;
begin
if x > y then
 Result := x
else
 Result := y;
end;

function min(x, y: integer): integer; overload;
begin
if x < y then
 Result := x
else
 Result := y;
end;

function min(x, y: single): single; overload;
begin
if x < y then
 Result := x
else
 Result := y;
end;


(***   Тригонометрия   ***)

// Арктангенс
function ArcTan2(const Y, X: single): single;
asm
 FLD     Y
 FLD     X
 FPATAN
 FWAIT
end;

function ArcCos(const X: single): single;
begin
if abs(X) > 1 then
 Result := 0
else
 Result := ArcTan2(Sqrt(1 - X * X), X);
end;

(***   Векторы   ***)

// Построить вектор
function Vector(X, Y, Z: single): TVector;
begin
Result.X := X;
Result.Y := Y;
Result.Z := Z;
end;

// Сложение векторов
function V_Add(v1, v2: TVector): TVector;
begin
Result.X := v1.X + v2.X;
Result.Y := v1.Y + v2.Y;
Result.Z := v1.Z + v2.Z;
end;

// Вычитание векторов
function V_Sub(v1, v2: TVector): TVector;
begin
Result.X := v1.X - v2.X;
Result.Y := v1.Y - v2.Y;
Result.Z := v1.Z - v2.Z;
end;

function V_Mult(v: TVector; d: single): TVector;
begin
Result.X := v.X * d;
Result.Y := v.Y * d;
Result.Z := v.Z * d;
end;

function V_Div(v: TVector; d: single): TVector;
begin
if d <> 0 then
 Result := Vector(v.X/d, v.Y/d, v.Z/d)
else
 Result := v;
end;

// Расстояние между двумя точками
function V_Dist(var v1, v2: TVector): single;
begin
Result := sqrt(sqr(v2.X - v1.X) +
               sqr(v2.Y - v1.Y) +
               sqr(v2.Z - v1.Z));
end;

// Квадрат расстояния между двумя точками
function V_Distq(var v1, v2: TVector): single;
begin
Result := sqr(v2.X - v1.X) +
          sqr(v2.Y - v1.Y) +
          sqr(v2.Z - v1.Z);
end;

function V_Length(v: TVector): single;
begin
Result := sqrt(sqr(v.X) + sqr(v.Y) + sqr(v.Z));
end;

function V_Normalize(var v: TVector): TVector;
var
 len : single;
begin
len := V_Length(v);
if len <> 0 then
 Result := V_Mult(v, 1/len)
else
 Result := v; 
end;

function V_Dot(v1, v2: TVector): single;
begin
Result := v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
end;

function V_Cross(var 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;
end;

function V_Angle(v1, v2: TVector): single;
begin
Result := ArcCos(V_Dot(V_Normalize(v1), V_Normalize(v2)));
end;

function V_Interpolate(v1, v2: TVector; k: single): TVector;
begin
Result.X := v1.X + (v2.X - v1.X)/k;
Result.Y := v1.Y + (v2.Y - v1.Y)/k;
Result.Z := v1.Z + (v2.Z - v1.Z)/k;
end;

// Plane
function P_Normal(v1, v2, v3: TVector): TVector;
begin
v1 := V_Sub(v2, v1);
v2 := V_Sub(v3, v2);
v3 := V_Cross(v1, v2);
Result := V_Normalize(v3);
end;

function P_Plane(v1, v2, v3: TVector): TPlane;
begin
Result.Normal := P_Normal(v1, v2, v3);
Result.D      := -V_Dot(Result.Normal, v1);
Result.V      := v1;
end;

function P_Dist(Pos: TVector; Plane: TPlane): single;
begin
Result := (V_Dot(Plane.Normal, Pos) + Plane.D);
end;

function P_Classify(Pos: TVector; Plane: TPlane): single;
begin
Result := V_Dot(Plane.Normal, V_Sub(Pos, Plane.V));
end;

function P_InsideTri(v, v1, v2, v3: TVector): boolean;
var
 i      : Integer;
 Angle  : Single;
 vec    : array [0..2] of TVector;
begin
Result := False;
vec[0] := V_Sub(v1, v);
vec[1] := V_Sub(v2, v);
vec[2] := V_Sub(v3, v);
Angle := 0;
for i := 0 to 2 do
 Angle := Angle + V_Angle(vec[i], vec[(i + 1) mod 3]);
if Angle >= (0.9 * 2 * pi) then
 Result := true;
end;

//==-- QUATERNIONS --==//
function Quaternion(X, Y, Z, W: single): TQuat; overload;
begin
Result.X := X;
Result.Y := Y;
Result.Z := Z;
Result.W := W;
end;

function Quaternion(v: TVector; Angle: single): TQuat; overload;
var
 s, c : single;
begin
c := Angle*0.5;
s := sin(c);
Result.X := v.X * s;
Result.Y := v.Y * s;
Result.Z := v.Z * s;
Result.W := cos(c);
end;

procedure Q_Matrix(q: TQuat; var m: TMatrix4);
var
 wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2 : single;
begin
with q do
 begin
 x2 := x + x;   y2 := y + y;   z2 := z + z;
 xx := x * x2;  xy := x * y2;  xz := x * z2;
 yy := y * y2;  yz := y * z2;  zz := z * z2;
 wx := w * x2;  wy := w * y2;  wz := w * z2;
 end;
m[0][0] := 1 - (yy + zz);  m[0][1] := xy + wz;        m[0][2] := xz - wy;
m[1][0] := xy - wz;        m[1][1] := 1 - (xx + zz);  m[1][2] := yz + wx;
m[2][0] := xz + wy;        m[2][1] := yz - wx;        m[2][2] := 1 - (xx + yy);

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

function M_Mult(var m: TMatrix4; v: TVector): TVector; overload;
begin
Result.X := m[0, 0] * v.X + m[1, 0] * v.Y + m[2, 0] * v.Z + m[3, 0];
Result.Y := m[0, 1] * v.X + m[1, 1] * v.Y + m[2, 1] * v.Z + m[3, 1];
Result.Z := m[0, 2] * v.X + m[1, 2] * v.Y + m[2, 2] * v.Z + m[3, 2];
end;

function M_Mult(var a, b: TMatrix4): TMatrix4; overload;
begin
Result[0, 0] := a[0, 0] * b[0, 0] + a[0, 1] * b[1, 0] + a[0, 2] * b[2, 0] + a[0, 3] * b[3, 0];
Result[1, 0] := a[1, 0] * b[0, 0] + a[1, 1] * b[1, 0] + a[1, 2] * b[2, 0] + a[1, 3] * b[3, 0];
Result[2, 0] := a[2, 0] * b[0, 0] + a[2, 1] * b[1, 0] + a[2, 2] * b[2, 0] + a[2, 3] * b[3, 0];
Result[3, 0] := a[3, 0] * b[0, 0] + a[3, 1] * b[1, 0] + a[3, 2] * b[2, 0] + a[3, 3] * b[3, 0];
Result[0, 1] := a[0, 0] * b[0, 1] + a[0, 1] * b[1, 1] + a[0, 2] * b[2, 1] + a[0, 3] * b[3, 1];
Result[1, 1] := a[1, 0] * b[0, 1] + a[1, 1] * b[1, 1] + a[1, 2] * b[2, 1] + a[1, 3] * b[3, 1];
Result[2, 1] := a[2, 0] * b[0, 1] + a[2, 1] * b[1, 1] + a[2, 2] * b[2, 1] + a[2, 3] * b[3, 1];
Result[3, 1] := a[3, 0] * b[0, 1] + a[3, 1] * b[1, 1] + a[3, 2] * b[2, 1] + a[3, 3] * b[3, 1];
Result[0, 2] := a[0, 0] * b[0, 2] + a[0, 1] * b[1, 2] + a[0, 2] * b[2, 2] + a[0, 3] * b[3, 2];
Result[1, 2] := a[1, 0] * b[0, 2] + a[1, 1] * b[1, 2] + a[1, 2] * b[2, 2] + a[1, 3] * b[3, 2];
Result[2, 2] := a[2, 0] * b[0, 2] + a[2, 1] * b[1, 2] + a[2, 2] * b[2, 2] + a[2, 3] * b[3, 2];
Result[3, 2] := a[3, 0] * b[0, 2] + a[3, 1] * b[1, 2] + a[3, 2] * b[2, 2] + a[3, 3] * b[3, 2];
Result[0, 3] := a[0, 0] * b[0, 3] + a[0, 1] * b[1, 3] + a[0, 2] * b[2, 3] + a[0, 3] * b[3, 3];
Result[1, 3] := a[1, 0] * b[0, 3] + a[1, 1] * b[1, 3] + a[1, 2] * b[2, 3] + a[1, 3] * b[3, 3];
Result[2, 3] := a[2, 0] * b[0, 3] + a[2, 1] * b[1, 3] + a[2, 2] * b[2, 3] + a[2, 3] * b[3, 3];
Result[3, 3] := a[3, 0] * b[0, 3] + a[3, 1] * b[1, 3] + a[3, 2] * b[2, 3] + a[3, 3] * b[3, 3];
end;

function M_Rotation(v: TVector; Angle: single): TMatrix4;
var
 s, c, inv_c : single;
begin
s := sin(Angle);
c := cos(Angle);
inv_c := 1 - c;

Result[0][0] := (inv_c * v.X * v.X) + c;
Result[0][1] := (inv_c * v.X * v.Y) - (v.Z * s);
Result[0][2] := (inv_c * v.Z * v.X) + (v.Y * s);

Result[1][0] := (inv_c * v.X * v.Y) + (v.Z * s);
Result[1][1] := (inv_c * v.Y * v.Y) + c;
Result[1][2] := (inv_c * v.Y * v.Z) - (v.X * s);

Result[2][0] := (inv_c * v.Z * v.X) - (v.Y * s);
Result[2][1] := (inv_c * v.Y * v.Z) + (v.X * s);
Result[2][2] := (inv_c * v.Z * v.Z) + c;

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


end.
