unit _Geometry;

{
  ver 0.019

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

  3d / 2d operations procedure pack
  matrix operations (3x3,4x4)
  plane operations (4x array)
}

interface

uses OpenGl;
type
 {
 TVector4d = array[0..3] of double;
 TMatrix4d = array[0..3] of TVector4d;

 TVector2f = array[0..1] of single;
 TVector3f = array[0..2] of single;
 TVector4f = array[0..3] of single;
 TMatrix3f = array[0..2] of TVector3f;
 TMatrix4f = array[0..3] of TVector4f;
 TVector3i = array[0..2] of integer;
 TVector2i = array[0..1] of integer;//without Opengl
 }

 {3d}
 TPoint = TVector3f;   {x,y,z}
 TPlane = TVector4f;   {x,y,z,a}
     
 TaPoint = Array of Single;   {x,y,z}

 TMatrix = TMatrix3f;  {rotate/translate/scale matrix 3*3}
 TPozition = TMatrix4f;  {rotate/translate/scale matrix 3*3}
 TFace = TVector3i;
 TCoord = TVector2i;


 {2d}
 TVect = TVector2f;
 TVector = TVector2f;

const
 pi=3.14159265358979;
 null_TVect:TVect = (0,0);
 null_TVector:TVector = (0,0);
 default_TVect:TVect = (0,0);
 default_TVector:TVector = (0,0);

 null_TPoint:TPoint = (0,0,0);
 default_TPoint:TPoint = (0,0,0);

 null_TFace:TFace = (0,0,0);
 default_TFace:TFace = (0,0,0);

 null_TCoord:TCoord = (0,0);
 default_TCoord:TCoord = (0,0);

 default_TMatrix:TMatrix =
                    ((1,0,0),
                     (0,1,0),
                     (0,0,1));
 default_TPozition:TPozition =
                    ((1,0,0,0),
                     (0,1,0,0),
                     (0,0,1,0),
                     (0,0,0,1));


{ ========== Math ===============}
function Get_X_V(ang,val:single):single; {отношение к sin}
function Get_Y_V(ang,val:single):single;{отношение к cos}
function Get_R(S:single):single; {GRADUS in 360}
function Head_Rotate(SA,RA,Speed:single):single;
function Point_in_Rect(X,Y,AX,AY,BX,BY:Single):boolean;overload;{check point in box AA:BB}
{ ========== 3D Geometry ===============}
function point(X,Y,Z:single):TPoint;overload;       {set point}
function point(Poz:TPozition):TPoint;overload;       {get point from transform matrix}
function ArcCos(const X: Single): Single;  {for calculation without Math}
function Get_S(X,Y,Z:single):single;overload;{get lendth of X,Y,Z Vector}
function Get_S(A:TPoint):single;overload; {get lendth of A(X,Y,Z) Vector}
function Get_S(A,B:TPoint):single;overload; {get lendth between A and B points}
function Normalize(A:TPoint):TPoint; {normalize}
function plus(A,B:TPoint):TPoint;overload; {Vector addition}
function minus(A,B:TPoint):TPoint;overload; {vector substraction}
function middle(A,B:TPoint):TPoint;overload;{middle point between A & B}
function middle(A,B,C:TPoint):TPoint;overload;{middle point between A,B & C}
function multi(A:TPoint;F:single):TPoint;overload;{multiplication 3d vector on number}
function multi(A:TaPoint;F:single):TPoint;overload;
function Between(A,B:TPoint;F:single):TPoint;{нахождение точки между а и б (F от 0 до 1)}
function Point_in_Rect(P:TPoint;A,B:TPoint):boolean;overload;{check point in box AAA:BBB}
function get_Normal(A,B,C:TPoint):TPoint;{нахождение нормали от плоскости(задается треугольником)}
{============= Face =========================}
function Face(A,B,C:integer):TFace;
{ ========== 3D Matrix Geometry ===============}
function Matrix(x1,y1,z1,x2,y2,z2,x3,y3,z3:single):TMatrix;overload;{to set the matrix (by string)}
function Matrix(t1,t2,t3:TPoint):TMatrix;overload;          {to set matrix (array3f as strins)}
function Matrix(Poz:TPozition):TMatrix;overload;          {get matrix from Pozition matrix}
function multi(F:TMatrix;A:TPoint):TPoint;overload; {multiply between 3x3 and 3 matrix}
function multi(F:TMatrix;A:TaPoint):TPoint;overload;
function multi(F:TMatrix;A:TMatrix):TMatrix;overload;{multiply between 3x3 and 3x3 matrix}
function multi(A:TMatrix;F:single):TMatrix;overload;{multiply between 3x3 matrix and single value}
function transpone(A:TMatrix):TMatrix;overload;{transponiruet matricy( A^(-1) )}
function rotate_X(ang:single;A:TMatrix):TMatrix;overload;{rotate A matrix above X}
function rotate_X(ang:single):TMatrix;overload;{create matrix with rotation above X}
function rotate_Y(ang:single;A:TMatrix):TMatrix;overload;{rotate A matrix above Y}
function rotate_Y(ang:single):TMatrix;overload;{create matrix with rotation above Y}
function rotate_Z(ang:single;A:TMatrix):TMatrix;overload;{rotate A matrix above Z}
function rotate_Z(ang:single):TMatrix;overload;{create matrix with rotation above Z}
function rotate(ang:single;V:TPoint):TMatrix;overload;{create matrix with rotation on ang above V Vector}
function rotate(ang,V0,V1,V2:single):TMatrix;overload;{create matrix with rotation on ang above V Vector}
function rotate_Euler(a,b,c:single):TMatrix;{create matrix with rotation on Euler angles(a,b,c - alpha,beta gamma)}
{ ========== 4x4 Matrix operations ===============}
function pozition(x1,y1,z1,t1,x2,y2,z2,t2,x3,y3,z3,t3,x4,y4,z4,t4:single):TPozition;overload;{to set the matrix 4x4 (by string)}
function pozition(Matrix:TMatrix;x,y,z:single):TPozition;overload;{to set the matrix 4x4 (by matrix and string)}
function pozition(Matrix:TMatrix;point:TPoint):TPozition;overload;{to set the matrix 4x4 (by matrix and Point)}
function pozition(Matrix:TMatrix):TPozition;overload;{to set the matrix 4x4 (by matrix 3x3)}
function pozition(AX,AY,Mash:single):TPozition;overload;{to set the matrix 4x4 (by matrix 3x3)}
function multi(F:TPozition;A:TPozition):TPozition;overload;
function multi(F:TPozition;A:TPlane):TPlane;overload;
function multi(F:TPozition;A:TPoint):TPoint;overload;
function multi(F:TPozition;A:Single):TPozition;overload;
function transpone(A:TPozition):TPozition;overload;
{ ========== 2D Geometry ===============}
function Coord(X,Y:integer):TCoord; //Set int coordinates
function Vector(X,Y:single):TVect; {задание вектора}
function Get_S(A,B:TVect):single;overload;{расстояние между точками}
function Get_S(X1,Y1,X2,Y2:single):single;overload;{расстояние между точками}
function Get_A(A,B:TVect):single;overload;{angle of B for A}
function Get_A(A:TVect):single;overload; {angle of A Vector}
function Get_A(X1,Y1,X2,Y2:Single):single;overload;{angle of point(x1,y1) for point(x2,y2)}
function Get_A(X,Y:Single):single;overload; {angle of Vector(X,Y)}
function Get_Vect(ang,spd:single):TVect;{отношение к sin и х и у}
function minus(A,B:TVect):TVect;overload; {векторное вычитание}
function plus(A,B:TCoord):TCoord;overload; {Vector addition}
function plus(A,B:TVect):TVect;overload;  {векторное сложение}
function middle(A,B:TVect):TVect;overload;  {векторное среднее}
function multi(A:TVect;F:single):TVect;overload;{multiplication 2d vector on number}
function Point_in_Rect(P:TVect;A,B:TVect):boolean;overload;{check point in box AAA:BBB}
function Get_LP_S(AX,AY,BX,BY,CX,CY:Single):Single;overload; {расстояние от точки с до отрезка аб}
function Get_LP_S(A,B,C:TVect):Single;overload; {расстояние от точки с до отрезка аб}
function Get_LP_Z(AX,AY,BX,BY,CX,CY:Single):smallint;overload; {расстояние от точки с до отрезка аб}
function Get_LP_Z(A,B,C:TVect):smallint;overload; {расстояние от точки с до отрезка аб}
function Point_in_Triang(P,A,B,C:TVect):boolean;overload;{находится P ли точка в треугольнике ABC}
function Point_in_Triang(PX,PY,AX,AY,BX,BY,CX,CY:Single):boolean;overload; {находится P ли точка в треугольнике ABC}
{============= Plane =========================}
function Plane(a,b,c,d:Single):TPlane;overload;
function Plane(a,b,c:TPoint):TPlane;overload;

implementation

{for all}
function ArcTan2(const Y, X: Extended): Extended;
asm
        FLD     Y
        FLD     X
        FPATAN
        FWAIT
end;

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

function Get_R(S:single):single; {GRADUS in 360}
begin
 Repeat
  If S<0 then S:=S+360;
  If S>=360 then S:=S-360;
 Until (S>=0) and (S<360);
 Get_R:=S;
end;

function Get_X_V(ang,val:single):single; {отношение к sin}
begin
 Get_X_V:=cos(ang/180*pi)*val;
end;

function Get_Y_V(ang,val:single):single;{отношение к cos}
begin
 Get_Y_V:=sin(ang/180*pi)*val;
end;

{для плавного поворота от одного угла к другому с заданной скоростью поворота}
{типа башен танков}
function Head_Rotate(SA,RA,Speed:single):single;
var s,src:single;    {SA - Source Angle RA - Rezult Angle}
begin
 src:=SA;
 If (RA>350) or (SA>350) or (RA<10) or (SA<10) then
  begin   {проверка убирает ошибку при обработке углов, близких к 0 или 359 - без него башня колеблется}
   RA:=Get_R(RA+180);SA:=Get_R(SA+180);
  end;
 S:=Round(Speed);
 If (ABS(RA-SA)<Speed) then S:=Round(ABS(RA-SA));{если до результата нужно повернуть меньше, чем скорость - изменяем скорость на остаток}
 If (SA<RA) then   {находит куда поворачивать ближе}
  If (RA-SA<=180) then  {и выдает градус текущего поворота}
   S:=S else S:=-1*S
  else
  If (SA-RA<180) then
   S:=-1*S else S:=S;  
 Head_Rotate:=Get_R(S+SA);
end;

function Point_in_Rect(X,Y,AX,AY,BX,BY:Single):boolean;overload;
var Temp:Single;   {check point in box AA:BB}
begin
 if AX>BX then begin temp:=AX;AX:=BX;BX:=Temp; end;
 if AY>BY then begin temp:=AY;AY:=BY;BY:=Temp; end;
 if (X>=AX) and (X<=BX) and
    (Y>=AY) and (Y<=BY) then
    Point_in_Rect:=true else Point_in_Rect:=false;
end;

{-------------------   3d   ---------------------}

function point(X,Y,Z:single):TPoint;  {set x,y,z point}
begin
 point[0]:=X;
 point[1]:=Y;
 point[2]:=Z;
end;

function point(Poz:TPozition):TPoint;overload;       {get point from transform matrix}
begin
 point[0]:=Poz[3,0];
 point[1]:=Poz[3,1];
 point[2]:=Poz[3,2];
end;

function Get_S(X,Y,Z:single):single;overload;{get lendth of X,Y,Z Vector}
Var s:single;
begin
 S:=sqrt(X*X+Y*Y+Z*Z);
 If S=0 then S:=0.0000001;  {для избежания деления на 0}
 Get_S:=S;
end;

function Get_S(A:TPoint):single;overload; {get lendth of A(X,Y,Z) Vector}
Var s:single;
begin
 S:=sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);
 If S=0 then S:=0.0000001;  {для избежания деления на 0}
 Get_S:=S;
end;

function Get_S(A,B:TPoint):single;overload; {get lendth between A and B points}
Var s:single;
begin
 A[0]:=B[0]-A[0];A[1]:=B[1]-A[1];A[2]:=B[2]-A[2];
 S:=sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);
 If S=0 then S:=0.0000001;  {для избежания деления на 0}
 Get_S:=S;
end;

function Normalize(A:TPoint):TPoint; {normalize}
Var s:Single;
begin
 s:=Get_S(A);
 Normalize[0]:=A[0]/S;
 Normalize[1]:=A[1]/S;
 Normalize[2]:=A[2]/S;    {X^2+Y^2+Z^2=1}
end;

function plus(A,B:TPoint):TPoint; {Vector addition}
begin
 plus[0]:=A[0]+B[0];
 plus[1]:=A[1]+B[1];
 plus[2]:=A[2]+B[2];
end;

function minus(A,B:TPoint):TPoint; {vector substraction}
begin
 minus[0]:=A[0]-B[0];
 minus[1]:=A[1]-B[1];
 minus[2]:=A[2]-B[2];
end;

function middle(A,B:TPoint):TPoint;overload;{middle point between A & B}
begin
 middle[0]:=(A[0]+B[0])*0.5;
 middle[1]:=(A[1]+B[1])*0.5;
 middle[2]:=(A[2]+B[2])*0.5;
end;

function middle(A,B,C:TPoint):TPoint;overload;{middle point between A,B & C}
begin
 middle[0]:=(A[0]+B[0]+C[0])/3;
 middle[1]:=(A[1]+B[1]+C[1])/3;
 middle[2]:=(A[2]+B[2]+C[2])/3;
end;

function multi(A:TPoint;F:single):TPoint;overload;
begin
 multi[0]:=A[0]*F;
 multi[1]:=A[1]*F;
 multi[2]:=A[2]*F;
end;

function multi(A:TaPoint;F:single):TPoint;overload;
begin
 multi[0]:=A[0]*F;
 multi[1]:=A[1]*F;
 multi[2]:=A[2]*F;
end;

function Between(A,B:TPoint;F:single):TPoint;
begin
 Between[0]:=A[0]+(B[0]-A[0])*F;
 Between[1]:=A[1]+(B[1]-A[1])*F;
 Between[2]:=A[2]+(B[2]-A[2])*F;
end;

function Point_in_Rect(P:TPoint;A,B:TPoint):boolean;overload;
var Temp:Single;
begin
 if A[0]>B[0] then begin temp:=A[0];A[0]:=B[0];B[0]:=Temp; end;
 if A[1]>B[1] then begin temp:=A[1];A[1]:=B[1];B[1]:=Temp; end;
 if A[2]>B[2] then begin temp:=A[2];A[2]:=B[2];B[2]:=Temp; end;
 if (P[0]>=A[0]) and (P[0]<=B[0]) and
    (P[1]>=A[1]) and (P[1]<=B[1]) and
    (P[2]>=A[2]) and (P[2]<=B[2]) then
    Point_in_Rect:=true else Point_in_Rect:=false;
end;

function get_Normal(A,B,C:TPoint):TPoint;{нахождение нормали от плоскости(задается треугольником)}
Var ver,N1,N2:TPoint;
begin
 N1:=minus(A,B);
 N2:=minus(B,C);
 ver[0]:=N1[1]*N2[2] - N1[2]*N2[1];
 ver[1]:=N1[2]*N2[0] - N1[0]*N2[2];
 ver[2]:=N1[0]*N2[1] - N1[1]*N2[0];
 get_Normal:=Normalize(ver);
end;

function Face(A,B,C:integer):TFace;
begin
 Face[0]:=A;
 Face[1]:=B;
 Face[2]:=C;
end;
{---------------- 3d matrix ----------------}

function Matrix(x1,y1,z1,x2,y2,z2,x3,y3,z3:single):TMatrix;
begin
 Matrix[0,0]:=x1;Matrix[0,1]:=y1;Matrix[0,2]:=z1;
 Matrix[1,0]:=x2;Matrix[1,1]:=y2;Matrix[1,2]:=z2;
 Matrix[2,0]:=x3;Matrix[2,1]:=y3;Matrix[2,2]:=z3;
end;

function Matrix(t1,t2,t3:TPoint):TMatrix;
begin
 Matrix[0,0]:=t1[0];Matrix[0,1]:=t1[1];Matrix[0,2]:=t1[2];
 Matrix[1,0]:=t2[0];Matrix[1,1]:=t2[1];Matrix[1,2]:=t2[2];
 Matrix[2,0]:=t3[0];Matrix[2,1]:=t3[1];Matrix[2,2]:=t3[2];
end;

function Matrix(Poz:TPozition):TMatrix;overload;          {get matrix from Pozition matrix}
begin
 Matrix[0,0]:=Poz[0,0];Matrix[0,1]:=Poz[0,1];Matrix[0,2]:=Poz[0,2];
 Matrix[1,0]:=Poz[1,0];Matrix[1,1]:=Poz[1,1];Matrix[1,2]:=Poz[1,2];
 Matrix[2,0]:=Poz[2,0];Matrix[2,1]:=Poz[2,1];Matrix[2,2]:=Poz[2,2];
end;

function multi(F:TMatrix;A:TPoint):TPoint;overload;
begin
 multi[0]:=(F[0,0]*A[0]+F[0,1]*A[1]+F[0,2]*A[2]);
 multi[1]:=(F[1,0]*A[0]+F[1,1]*A[1]+F[1,2]*A[2]);
 multi[2]:=(F[2,0]*A[0]+F[2,1]*A[1]+F[2,2]*A[2]);
end;

function multi(F:TMatrix;A:TaPoint):TPoint;overload;
begin
 multi[0]:=(F[0,0]*A[0]+F[0,1]*A[1]+F[0,2]*A[2]);
 multi[1]:=(F[1,0]*A[0]+F[1,1]*A[1]+F[1,2]*A[2]);
 multi[2]:=(F[2,0]*A[0]+F[2,1]*A[1]+F[2,2]*A[2]);
end;

function multi(F:TMatrix;A:TMatrix):TMatrix;overload;
begin
 multi[0,0]:=(F[0,0]*A[0,0]+F[0,1]*A[1,0]+F[0,2]*A[2,0]);
 multi[0,1]:=(F[0,0]*A[0,1]+F[0,1]*A[1,1]+F[0,2]*A[2,1]);
 multi[0,2]:=(F[0,0]*A[0,2]+F[0,1]*A[1,2]+F[0,2]*A[2,2]);
 multi[1,0]:=(F[1,0]*A[0,0]+F[1,1]*A[1,0]+F[1,2]*A[2,0]);
 multi[1,1]:=(F[1,0]*A[0,1]+F[1,1]*A[1,1]+F[1,2]*A[2,1]);
 multi[1,2]:=(F[1,0]*A[0,2]+F[1,1]*A[1,2]+F[1,2]*A[2,2]);
 multi[2,0]:=(F[2,0]*A[0,0]+F[2,1]*A[1,0]+F[2,2]*A[2,0]);
 multi[2,1]:=(F[2,0]*A[0,1]+F[2,1]*A[1,1]+F[2,2]*A[2,1]);
 multi[2,2]:=(F[2,0]*A[0,2]+F[2,1]*A[1,2]+F[2,2]*A[2,2]);
end;

function multi(A:TMatrix;F:single):TMatrix;overload;{multiply between 3x3 matrix and single value}
begin
 multi[0,0]:=A[0,0]*F;
 multi[0,1]:=A[0,1]*F;
 multi[0,2]:=A[0,2]*F;
 multi[1,0]:=A[1,0]*F;
 multi[1,1]:=A[1,1]*F;
 multi[1,2]:=A[1,2]*F;
 multi[2,0]:=A[2,0]*F;
 multi[2,1]:=A[2,1]*F;
 multi[2,2]:=A[2,2]*F;
end;

function transpone(A:TMatrix):TMatrix;
begin
 transpone[0,0]:=A[0,0];
 transpone[0,1]:=A[1,0];
 transpone[0,2]:=A[2,0];
 transpone[1,0]:=A[0,1];
 transpone[1,1]:=A[1,1];
 transpone[1,2]:=A[2,1];
 transpone[2,0]:=A[0,2];
 transpone[2,1]:=A[1,2];
 transpone[2,2]:=A[2,2];
end;

function rotate_X(ang:single;A:TMatrix):TMatrix;
begin
 ang:=ang/180*pi;
 rotate_X:=multi(
  Matrix( 1,      0,      0,
          0, cos(ang),-sin(ang),
          0, sin(ang), cos(ang))
  ,A);
end;

function rotate_X(ang:single):TMatrix;{create matrix with rotation above X}
begin
 ang:=ang/180*pi;
 rotate_X:=
  Matrix( 1,      0,      0,
          0, cos(ang),-sin(ang),
          0, sin(ang), cos(ang));
end;

function rotate_Y(ang:single;A:TMatrix):TMatrix;
begin
 ang:=ang/180*pi;
 rotate_Y:=multi(
  Matrix( cos(ang), 0, sin(ang),
                 0, 1,        0,
         -sin(ang), 0, cos(ang))
  ,A);
end;

function rotate_Y(ang:single):TMatrix;{create matrix with rotation above X}
begin
 ang:=ang/180*pi;
 rotate_Y:=
  Matrix( cos(ang), 0, sin(ang),
                 0, 1,        0,
         -sin(ang), 0, cos(ang));
end;

function rotate_Z(ang:single;A:TMatrix):TMatrix;
begin
 ang:=ang/180*pi;
 rotate_Z:=multi(
  Matrix( cos(ang),-sin(ang), 0,
          sin(ang), cos(ang), 0,
                 0,        0, 1)
  ,A);
end;

function rotate_Z(ang:single):TMatrix;{create matrix with rotation above X}
begin
 ang:=ang/180*pi;
 rotate_Z:=
  Matrix( cos(ang),-sin(ang), 0,
          sin(ang), cos(ang), 0,
                 0,        0, 1);
end;

function rotate(ang:single;V:TPoint):TMatrix;{create matrix with rotation on ang above Vector}
begin
 ang:=ang/180*pi;
 rotate:=
 Matrix(
  cos(ang)+(1-cos(ang))*V[0]*V[0],       {00}
  (1-cos(ang))*V[0]*V[1]-sin(ang)*V[2],  {01}
  (1-cos(ang))*V[0]*V[2]+sin(ang)*V[1],  {02}
  (1-cos(ang))*V[1]*V[0]+sin(ang)*V[2],  {10}
  cos(ang)+(1-cos(ang))*V[1]*V[1],       {11}
  (1-cos(ang))*V[1]*V[2]-sin(ang)*V[0],  {12}
  (1-cos(ang))*V[2]*V[0]-sin(ang)*V[1],  {20}
  (1-cos(ang))*V[2]*V[1]+sin(ang)*V[0],  {21}
  cos(ang)+(1-cos(ang))*V[2]*V[2]        {22}
 );
end;

function rotate(ang,V0,V1,V2:single):TMatrix;{create matrix with rotation on ang above Vector}
begin
 ang:=ang/180*pi;
 rotate:=
 Matrix(
  cos(ang)+(1-cos(ang))*V0*V0,       {00}
  (1-cos(ang))*V0*V1-sin(ang)*V2,    {01}
  (1-cos(ang))*V0*V2+sin(ang)*V1,    {02}
  (1-cos(ang))*V1*V0+sin(ang)*V2,    {10}
  cos(ang)+(1-cos(ang))*V1*V1,       {11}
  (1-cos(ang))*V1*V2-sin(ang)*V0,    {12}
  (1-cos(ang))*V2*V0-sin(ang)*V1,    {20}
  (1-cos(ang))*V2*V1+sin(ang)*V0,    {21}
  cos(ang)+(1-cos(ang))*V2*V2        {22}
 );
end;

function rotate_Euler(a,b,c:single):TMatrix;{create matrix with rotation on Euler angles(a,b,c - alpha,beta gamma)}
begin
 a:=a/180*pi;
 b:=b/180*pi;
 c:=c/180*pi;
 rotate_Euler:=
 Matrix(
  cos(a)*cos(b),       {00}
  cos(a)*sin(b)*sin(c)-sin(a)*cos(c),    {01}
  cos(a)*sin(b)*cos(c)+sin(a)*sin(c),    {02}
  sin(a)*cos(b),    {10}
  sin(a)*sin(b)*sin(c)+cos(a)*cos(c),    {11}
  sin(a)*sin(b)*cos(c)-cos(a)*sin(c),    {12}
  -sin(b),    {20}
  cos(b)*sin(c),    {21}
  cos(b)*cos(c)        {22}
 );
end;
{-------------- 4x4 matrix --------------}
function pozition(x1,y1,z1,t1,x2,y2,z2,t2,x3,y3,z3,t3,x4,y4,z4,t4:single):TPozition;overload;{to set the matrix 4x4 (by string)}
begin
 pozition[0,0]:=x1;pozition[0,1]:=y1;pozition[0,2]:=z1;pozition[0,3]:=t1;
 pozition[1,0]:=x2;pozition[1,1]:=y2;pozition[1,2]:=z2;pozition[1,3]:=t2;
 pozition[2,0]:=x3;pozition[2,1]:=y3;pozition[2,2]:=z3;pozition[2,3]:=t3;
 pozition[3,0]:=x4;pozition[3,1]:=y4;pozition[3,2]:=z4;pozition[3,3]:=t4;
end;

function pozition(Matrix:TMatrix;x,y,z:single):TPozition;overload;{to set the matrix 4x4 (by matrix and string)}
begin
 pozition[0,0]:=Matrix[0,0];pozition[0,1]:=Matrix[0,1];pozition[0,2]:=Matrix[0,2];pozition[0,3]:=0;
 pozition[1,0]:=Matrix[1,0];pozition[1,1]:=Matrix[1,1];pozition[1,2]:=Matrix[1,2];pozition[1,3]:=0;
 pozition[2,0]:=Matrix[2,0];pozition[2,1]:=Matrix[2,1];pozition[2,2]:=Matrix[2,2];pozition[2,3]:=0;
 pozition[3,0]:=x;pozition[3,1]:=y;pozition[3,2]:=z;pozition[3,3]:=1;
end;

function pozition(Matrix:TMatrix;point:TPoint):TPozition;overload;{to set the matrix 4x4 (by matrix and Point)}
begin
 pozition[0,0]:=Matrix[0,0];pozition[0,1]:=Matrix[0,1];pozition[0,2]:=Matrix[0,2];pozition[0,3]:=0;
 pozition[1,0]:=Matrix[1,0];pozition[1,1]:=Matrix[1,1];pozition[1,2]:=Matrix[1,2];pozition[1,3]:=0;
 pozition[2,0]:=Matrix[2,0];pozition[2,1]:=Matrix[2,1];pozition[2,2]:=Matrix[2,2];pozition[2,3]:=0;
 pozition[3,0]:=Point[0];pozition[3,1]:=Point[1];pozition[3,2]:=Point[2];pozition[3,3]:=1;
end;

function pozition(Matrix:TMatrix):TPozition;overload;{to set the matrix 4x4 (by matrix 3x3)}
begin
 pozition[0,0]:=Matrix[0,0];pozition[0,1]:=Matrix[0,1];pozition[0,2]:=Matrix[0,2];pozition[0,3]:=0;
 pozition[1,0]:=Matrix[1,0];pozition[1,1]:=Matrix[1,1];pozition[1,2]:=Matrix[1,2];pozition[1,3]:=0;
 pozition[2,0]:=Matrix[2,0];pozition[2,1]:=Matrix[2,1];pozition[2,2]:=Matrix[2,2];pozition[2,3]:=0;
 pozition[3,0]:=0;pozition[3,1]:=0;pozition[3,2]:=0;pozition[3,3]:=1;
end;

function pozition(AX,AY,Mash:single):TPozition;overload;{to set the matrix 4x4 (by matrix 3x3)}
begin
 pozition:=pozition(
  multi(  multi(Rotate(AX,0,0,1),Rotate(AY,1,0,0))
  ,Matrix(Mash,0,0,0,Mash,0,0,0,Mash)));
end;

function multi(F:TPozition;A:TPozition):TPozition;overload;
begin
 multi[0,0]:=(F[0,0]*A[0,0]+F[0,1]*A[1,0]+F[0,2]*A[2,0]+F[0,3]*A[3,0]);
 multi[0,1]:=(F[0,0]*A[0,1]+F[0,1]*A[1,1]+F[0,2]*A[2,1]+F[0,3]*A[3,1]);
 multi[0,2]:=(F[0,0]*A[0,2]+F[0,1]*A[1,2]+F[0,2]*A[2,2]+F[0,3]*A[3,2]);
 multi[0,3]:=(F[0,0]*A[0,3]+F[0,1]*A[1,3]+F[0,2]*A[2,3]+F[0,3]*A[3,3]);
 multi[1,0]:=(F[1,0]*A[0,0]+F[1,1]*A[1,0]+F[1,2]*A[2,0]+F[1,3]*A[3,0]);
 multi[1,1]:=(F[1,0]*A[0,1]+F[1,1]*A[1,1]+F[1,2]*A[2,1]+F[1,3]*A[3,1]);
 multi[1,2]:=(F[1,0]*A[0,2]+F[1,1]*A[1,2]+F[1,2]*A[2,2]+F[1,3]*A[3,2]);
 multi[1,3]:=(F[1,0]*A[0,3]+F[1,1]*A[1,3]+F[1,2]*A[2,3]+F[1,3]*A[3,3]);
 multi[2,0]:=(F[2,0]*A[0,0]+F[2,1]*A[1,0]+F[2,2]*A[2,0]+F[2,3]*A[3,0]);
 multi[2,1]:=(F[2,0]*A[0,1]+F[2,1]*A[1,1]+F[2,2]*A[2,1]+F[2,3]*A[3,1]);
 multi[2,2]:=(F[2,0]*A[0,2]+F[2,1]*A[1,2]+F[2,2]*A[2,2]+F[2,3]*A[3,2]);
 multi[2,3]:=(F[2,0]*A[0,3]+F[2,1]*A[1,3]+F[2,2]*A[2,3]+F[2,3]*A[3,3]);
 multi[3,0]:=(F[3,0]*A[0,0]+F[3,1]*A[1,0]+F[3,2]*A[2,0]+F[3,3]*A[3,0]);
 multi[3,1]:=(F[3,0]*A[0,1]+F[3,1]*A[1,1]+F[3,2]*A[2,1]+F[3,3]*A[3,1]);
 multi[3,2]:=(F[3,0]*A[0,2]+F[3,1]*A[1,2]+F[3,2]*A[2,2]+F[3,3]*A[3,2]);
 multi[3,3]:=(F[3,0]*A[0,3]+F[3,1]*A[1,3]+F[3,2]*A[2,3]+F[3,3]*A[3,3]);
end;

function multi(F:TPozition;A:TPlane):TPlane;overload;
begin
 multi[0]:=(F[0,0]*A[0]+F[1,0]*A[1]+F[2,0]*A[2]+F[3,0]*A[3]);
 multi[1]:=(F[0,1]*A[0]+F[1,1]*A[1]+F[2,1]*A[2]+F[3,1]*A[3]);
 multi[2]:=(F[0,2]*A[0]+F[1,2]*A[1]+F[2,2]*A[2]+F[3,2]*A[3]);
 multi[3]:=(F[0,3]*A[0]+F[1,3]*A[1]+F[2,3]*A[2]+F[3,3]*A[3]);
end;

function multi(F:TPozition;A:TPoint):TPoint;overload;
Var V:TPlane;
begin
 V[0]:=A[0];
 V[1]:=A[1];
 V[2]:=A[2];
 V[3]:=0;
 V:=multi(F,V);
 multi[0]:=V[0];
 multi[1]:=V[1];
 multi[2]:=V[2];
end;

function multi(F:TPozition;A:Single):TPozition;overload;
begin    
 multi[0,0]:=(F[0,0]*A);
 multi[0,1]:=(F[0,1]*A);
 multi[0,2]:=(F[0,2]*A);
 multi[0,3]:=(F[0,3]*A);
 multi[1,0]:=(F[1,0]*A);
 multi[1,1]:=(F[1,1]*A);
 multi[1,2]:=(F[1,2]*A);
 multi[1,3]:=(F[1,3]*A);
 multi[2,0]:=(F[2,0]*A);
 multi[2,1]:=(F[2,1]*A);
 multi[2,2]:=(F[2,2]*A);
 multi[2,3]:=(F[2,3]*A);
 multi[3,0]:=(F[3,0]*A);
 multi[3,1]:=(F[3,1]*A);
 multi[3,2]:=(F[3,2]*A);
 multi[3,3]:=(F[3,3]*A);
end;

function transpone(A:TPozition):TPozition;
begin
 transpone[0,0]:=A[0,0];
 transpone[0,1]:=A[1,0];
 transpone[0,2]:=A[2,0];
 transpone[0,3]:=A[3,0];
 transpone[1,0]:=A[0,1];
 transpone[1,1]:=A[1,1];
 transpone[1,2]:=A[2,1];
 transpone[1,3]:=A[3,1];
 transpone[2,0]:=A[0,2];
 transpone[2,1]:=A[1,2];
 transpone[2,2]:=A[2,2];
 transpone[2,3]:=A[3,2];
 transpone[3,0]:=A[0,3];
 transpone[3,1]:=A[1,3];
 transpone[3,2]:=A[2,3];
 transpone[3,3]:=A[3,3];
end;
{----------------- 2d -------------------}

function Coord(X,Y:integer):TCoord; //Set int coordinates
begin
 Coord[0]:=X;
 Coord[1]:=Y;
end;

function Vector(X,Y:single):TVect; {задание вектора}
begin
 Vector[0]:=X;
 Vector[1]:=Y;
end;

function Get_S(A,B:TVect):single;overload;{расстояние между точками}
var S:Single;
begin
 S:=sqrt((B[0]-A[0])*(B[0]-A[0])+(B[1]-A[1])*(B[1]-A[1]));
 If S=0 then S:=0.0000001;  {для избежания деления на 0}
 Get_S:=S;
end;

function Get_S(X1,Y1,X2,Y2:single):single;overload;{расстояние между точками}
var S:Single;
begin
 S:=sqrt((X2-X1)*(X2-X1)+(Y2-Y1)*(Y2-Y1));
 If S=0 then S:=0.0000001;  {для избежания деления на 0}
 Get_S:=S;
end;

function Get_A(A,B:TVect):single;overload;  {angle of B point for A}
Var S,C:single;
begin
 C:=(B[0]-A[0])/Get_S(A,B);  {соотношения для арккосинуса}
 If C>1 then C:=1;            {чтобы не сбойнул arcsin}
 If C<-1 then C:=-1;
 S:=(arccos(C))*180/pi;        {перевод в градусы}
 If (B[1]-A[1])>0 then S:=Get_R(360-S); {инверсия градусов, почему - не знаю :) выяснил опытным путем}
 S:=Get_R(-S);
 Get_A:=S;
end;

function Get_A(A:TVect):single;overload;  {угол к цели}
Var S,C:single;B:TVect;
begin
 B[0]:=0;
 B[1]:=0;
 C:=(A[0]-B[0])/Get_S(A,B); {соотношения для арккосинуса}
 If C>1 then C:=1;            {чтобы не сбойнул}
 If C<-1 then C:=-1;
 S:=(arccos(C))*180/pi;        {перевод в градусы}
 If (A[1]-B[1])>0 then S:=Get_R(360-S); {инверсия градусов, почему - не знаю :) выяснил опытным путем}
 S:=Get_R(-S);
 Get_A:=S;
end;

function Get_A(X1,Y1,X2,Y2:Single):single;overload;{angle of point(x1,y1) for point(x2,y2)}
Var S,C:single;
begin
 C:=(X2-X1)/Get_S(X1,Y1,X2,Y2);  {соотношения для арккосинуса}
 If C>1 then C:=1;            {чтобы не сбойнул arcsin}
 If C<-1 then C:=-1;
 S:=(arccos(C))*180/pi;        {перевод в градусы}
 If (Y2-Y1)>0 then S:=Get_R(360-S); {инверсия градусов, почему - не знаю :) выяснил опытным путем}
 S:=Get_R(-S);
 Get_A:=S;
end;

function Get_A(X,Y:Single):single;overload; {angle of Vector(X,Y)}
Var S,C,X1,Y1:single;
begin
 X1:=0;
 Y1:=0;
 C:=(X-X1)/Get_S(X1,Y1,X,Y);  {соотношения для арккосинуса}
 If C>1 then C:=1;            {чтобы не сбойнул arcsin}
 If C<-1 then C:=-1;
 S:=(arccos(C))*180/pi;        {перевод в градусы}
 If (Y-Y1)>0 then S:=Get_R(360-S); {инверсия градусов, почему - не знаю :) выяснил опытным путем}
 S:=Get_R(-S);
 Get_A:=S;
end;

function Get_Vect(ang,spd:single):TVect;{отношение к sin и х и у}
begin
 Get_Vect[0]:=cos(ang/180*pi)*spd;
 Get_Vect[1]:=sin(ang/180*pi)*spd;
end;

function minus(A,B:TVect):TVect;overload; {векторное вычитание}
begin
 minus[0]:=A[0]-B[0];
 minus[1]:=A[1]-B[1];
end;

function plus(A,B:TCoord):TCoord;overload;  {векторное сложение}
begin
 plus[0]:=A[0]+B[0];
 plus[1]:=A[1]+B[1];
end;

function plus(A,B:TVect):TVect;overload;  {векторное сложение}
begin
 plus[0]:=A[0]+B[0];
 plus[1]:=A[1]+B[1];
end;

function middle(A,B:TVect):TVect;overload;  {векторное среднее}
begin
 middle[0]:=(A[0]+B[0])/2;
 middle[1]:=(A[1]+B[1])/2;
end;

function multi(A:TVect;F:single):TVect;overload; {умножение на скаляр}
begin
 multi[0]:=A[0]*F;
 multi[1]:=A[1]*F;
end;

function Point_in_Rect(P:TVect;A,B:TVect):boolean;overload;
var Temp:Single;{check point in 2d box}
begin
 if A[0]>B[0] then begin temp:=A[0];A[0]:=B[0];B[0]:=Temp; end;
 if A[1]>B[1] then begin temp:=A[1];A[1]:=B[1];B[1]:=Temp; end;
 if (P[0]>=A[0]) and (P[0]<=B[0]) and
    (P[1]>=A[1]) and (P[1]<=B[1]) then
    Point_in_Rect:=true else Point_in_Rect:=false;
end;

function Get_LP_S(AX,AY,BX,BY,CX,CY:Single):Single;overload; {расстояние от точки с до отрезка аб}
Var dx,dy,d:Single;
begin
 dx:=AX-BX;
 dy:=AY-BY;
 D:=dx*(CY-AY)-dy*(CX-AX);
 Get_LP_S:=abs(D/sqrt(dx*dx+dy*dy));{знак показывает полуплоскость - убираем}
end;

function Get_LP_S(A,B,C:TVect):Single;overload; {расстояние от точки с до отрезка аб}
Var dx,dy,d:Single;
begin
 dx:=A[0]-B[0];
 dy:=A[1]-B[1];
 D:=dx*(C[1]-A[1])-dy*(C[0]-A[0]);
 Get_LP_S:=abs(D/sqrt(dx*dx+dy*dy));{знак показывает полуплоскость - убираем}
end;

function Get_LP_Z(AX,AY,BX,BY,CX,CY:Single):smallint;overload; {расстояние от точки с до отрезка аб}
Var dx,dy:Single;
begin
 dx:=AX-BX;
 dy:=AY-BY;
 if (dx*(CY-AY)-dy*(CX-AX)>0) then
 Get_LP_Z:=1 else Get_LP_Z:=-1;
{знак показывает полуплоскость}
end;

function Get_LP_Z(A,B,C:TVect):smallint;overload; {расстояние от точки с до отрезка аб}
Var dx,dy:Single;
begin
 dx:=A[0]-B[0];
 dy:=A[1]-B[1];
 if (dx*(C[1]-A[1])-dy*(C[0]-A[0])>0) then
 Get_LP_Z:=1 else Get_LP_Z:=-1;
{знак показывает полуплоскость}
end;

function Point_in_Triang(P,A,B,C:TVect):boolean;overload;{находится P ли точка в треугольнике ABC}
begin
 Point_in_Triang:=false;
 if (Get_LP_Z(A,B,C)=Get_LP_Z(A,B,P)) then
 if (Get_LP_Z(C,A,B)=Get_LP_Z(C,A,P)) then
 if (Get_LP_Z(B,C,A)=Get_LP_Z(B,C,P)) then
  Point_in_Triang:=true;
end;

function Point_in_Triang(PX,PY,AX,AY,BX,BY,CX,CY:Single):boolean;overload; {находится P ли точка в треугольнике ABC}
begin
 Point_in_Triang:=false;
 if (Get_LP_Z(AX,AY,BX,BY,CX,CY)=Get_LP_Z(AX,AY,BX,BY,PX,PY)) then
 if (Get_LP_Z(AX,AY,CX,CY,BX,BY)=Get_LP_Z(AX,AY,CX,CY,PX,PY)) then
 if (Get_LP_Z(BX,BY,CX,CY,AX,AY)=Get_LP_Z(BX,BY,CX,CY,PX,PY)) then
  Point_in_Triang:=true;            
end;

{============ Plane =================}
function Plane(a,b,c,d:Single):TPlane;
begin
  Plane[0]:=a;
  Plane[1]:=b;
  Plane[2]:=c;
  Plane[3]:=d;
end;

function Plane(a,b,c:TPoint):TPlane;
begin
  {plane (uravnenie ploskosti)}
  {ax + by + cz + d = 0 }
  {a}Plane[0]:= a[1]*(b[2]-c[2]) + b[1]*(c[2]-a[2]) + c[1]*(a[2]-b[2]);
  {b}Plane[1]:= a[2]*(b[0]-c[0]) + b[2]*(c[0]-a[0]) + c[2]*(a[0]-b[0]);
  {c}Plane[2]:= a[0]*(b[1]-c[1]) + b[0]*(c[1]-a[1]) + c[0]*(a[1]-b[1]);
  {d}Plane[3]:= -( a[0] * (b[1]*c[2] - c[1]*b[2]) +
                      b[0] * (c[1]*a[2] - a[1]*c[2]) +
                      c[0] * (a[1]*b[2] - b[1]*a[2]));
end;



end.
