unit xVectR3;
{ Feel free to use this unit as you want.
  First developped by Bernard Gisin

  This unit helps manipulating vectors in the 3-dimensional space.
  ----------------------------------------------------------------}

INTERFACE
//"""""""

uses wGlobals2;

type
  RVectR3Float = array[1..3] of Float;
  PArrayRVectR3Float = ^TArrayRVectR3Float;
  TArrayRVectR3Float = array[0..100000000] of RVectR3Float;

const
  gglwVectR3Error : word32 = 0;
  // If a division by zero has been tempted, this value is incremented by 1.
  // It must be set to zero, before trying to test it.

  gglvV3Epsilon = 1.0e-12;
  // Small number. Number having absolute value under it can be considered as zero.
  // normally, the number taken as limite should be base on  vMachEps, defined
  // as the smallest number such that  1.0 + vMachEps <> 1.0

  // Very generally used vectors.
  gglvecV3Zero  : RVectR3Float = (0,0,0);
  gglvecV3AxisX : RVectR3Float = (1,0,0);
  gglvecV3AxisY : RVectR3Float = (0,1,0);
  gglvecV3AxisZ : RVectR3Float = (0,0,1);

operator + (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float;
operator - (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float;
operator * (vecV1, vecV2:RVectR3Float)  vScalarProduct:Float; // scalar product
operator ** (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float; // Cross product
// usually noted  V1 ^ V2  or  V1 x V2

operator * (vScal:Float; vecIn:RVectR3Float) vecOut:RVectR3Float;
// Scalar time a Vector

operator / (vecIn:RVectR3Float; vScal:Float) vecOut:RVectR3Float;
// inverse of a Scalar time a Vector

operator = (vecV1, vecV2:RVectR3Float) fRes:boolean;
// True if Norme(vecV1 - vecV2) <= gglvV3Epsilon

function V3Assign(vX,vY,vZ:Float):RVectR3Float;
//=============================================
// Define a vector from three Float numbers

function Norme2(const vecIn:RVectR3Float):Float;
//================================================
// Square of the norme of the vector

function Norme(const vecIn:RVectR3Float):Float;
//===============================================
// Norme of the vector

function V3Angle(const vecV1,vecV2:RVectR3Float):Float;
//=====================================================
// Angle in radian between the two vectors. in [0..Pi]
// No sign can be given, since no orientation is given.

function V3AngleOrientated(const vecV1,vecV2,vecOrient:RVectR3Float):Float;
//=========================================================================
// Angle in radian between the two vectors. in [-Pi..Pi]
// The sign is the sign of vecOrient * (vecV1 ** vecV2), i.e.
// is positif if the basis (vecV1, vecV2, vecOrient) is right oriented.

procedure V3Normalise(var vecV:RVectR3Float);
//===========================================
// Normalise the vector  vecV
// If  vecV = 0  then return  vecV = (0,0,1)

function V3NormaliseFunc(const vecV:RVectR3Float):RVectR3Float;
//=============================================================
// Return the normalised the vector  vecV
// If  vecV = 0  then return  = (0,0,1)

function V3RotationOrg(const vecIn,vecAxe:RVectR3Float; vTheta:Float):RVectR3Float;
//=================================================================================
// Rotate the vector  vecIn  around the vector  vecAxe
// The origin is used as the rotation center.
// with an angle of  vTheta  radian.
// If vecAxe = 0, return the  vecIn  vector.
// Rotation(V3Assign(1,0,0), V3Assign(0,0,1), Pi/2) = (0,1,0)
// i.e. a rotation of 90 degrees aroung the z axis,
// send the x axis on the y axis, and the y axis on  - x axis

function V3RotationVec(vecIn,vecCenter,vecAxe:RVectR3Float; vTheta:Float):RVectR3Float;
//=====================================================================================
// Rotate the vector  vecIn  around the vector  vecAxe
// having vecCenter as a rotation center
// with an angle of  vTheta  radian.
// If vecAxe = 0, return the  vecIn  vector.
// Rotation(V3Assign(1,0,0), V3Assign(0,0,1), Pi/2) = (0,1,0)
// i.e. a rotation of 90 degrees aroung the z axis,
// send the x axis on the y axis, and the y axis on  - x axis

function V3ToStr(const vecIn:RVectR3Float):ShortString;
//=====================================================
// Convert the vector into a string.
// usefull to display the vector in a debugging window.

IMPLEMENTATION
//""""""""""""

Uses wUtiles0, wDebug0;

var nNotUsed : integer; // only to avoid an error message while compliling in DevPascal 1.9

operator + (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float;
//========================================================
var nn : integer;
begin
for nn:=1 to 3 do vecV3[nn]:=vecV1[nn] + vecV2[nn];
end; // operator + 

operator - (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float;
//========================================================
var nn : integer;
begin
for nn:=1 to 3 do vecV3[nn]:=vecV1[nn] - vecV2[nn];
end; // operator -

operator * (vecV1, vecV2:RVectR3Float) vScalarProduct:Float; // scalar product
//==========================================================
var nn : integer;
begin
vScalarProduct:=0;
for nn:=1 to 3 do vScalarProduct:=vScalarProduct + vecV1[nn] * vecV2[nn];
end; // operator *

operator ** (vecV1, vecV2:RVectR3Float) vecV3:RVectR3Float; // Cross product
//========================================================
// usually noted  V1 ^ V2  or  V1 x V2
begin
vecV3[1]:=vecV1[2] * vecV2[3] - vecV1[3] * vecV2[2];
vecV3[2]:=vecV1[3] * vecV2[1] - vecV1[1] * vecV2[3];
vecV3[3]:=vecV1[1] * vecV2[2] - vecV1[2] * vecV2[1];
end; // operator **

operator * (vScal:Float; vecIn:RVectR3Float) vecOut:RVectR3Float;
//===============================================================
// Scalar time a Vector
begin
vecOut[1]:=vScal * vecIn[1];
vecOut[2]:=vScal * vecIn[2];
vecOut[3]:=vScal * vecIn[3];
end; // operator *

operator / (vecIn:RVectR3Float; vScal:Float) vecOut:RVectR3Float;
//===============================================================
// inverse of a Scalar time a Vector
begin
if (vScal = 0) then begin
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   vScal:=gglvV3Epsilon; // very small number, to avoid runtime error
   end;

vecOut[1]:=vecIn[1] / vScal;
vecOut[2]:=vecIn[2] / vScal;
vecOut[3]:=vecIn[3] / vScal;
end; // operator /

operator = (vecV1, vecV2:RVectR3Float) fRes:boolean;
//==================================================
// True if Norme(vecV1 - vecV2) <= gglvV3Epsilon
// The last terme is added, in case one of the vectors is zero.
begin
fRes:=Norme(vecV1 - vecV2) <= gglvV3Epsilon;
end;  //operator =

function V3Assign(vX,vY,vZ:Float):RVectR3Float;
//=============================================
// Define a vector from three Float numbers
begin
V3Assign[1]:=vX;
V3Assign[2]:=vY;
V3Assign[3]:=vZ;
end;  // V3Assign

function Norme2(const vecIn:RVectR3Float):Float;
//==============================================
// Square of the norme of the vector
begin
Norme2:=sqr(vecIn[1]) + sqr(vecIn[2]) + sqr(vecIn[3]);
end;  // Norme2

function Norme(const vecIn:RVectR3Float):Float;
//=============================================
// Norme of the vector
begin
Norme:=sqrt(sqr(vecIn[1]) + sqr(vecIn[2]) + sqr(vecIn[3]));
end;  // Norme

function V3Angle(const vecV1,vecV2:RVectR3Float):Float;
//=====================================================
// Angle in radian between the two vectors. in [0..Pi]
// No signe can be given
var
  vNormeProd : Float;
begin { - AngleR2 - }
vNormeProd:=Norme(vecV1) * Norme(vecV2);

if (vNormeProd <= 1e-30) then begin
   // Angle not defines
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   V3Angle:=0;
   end;

V3Angle:=ArcCos( (vecV1 * vecV2) / vNormeProd);
end;  // V3Angle

function V3AngleOrientated(const vecV1,vecV2,vecOrient:RVectR3Float):Float;
//=========================================================================
// Angle in radian between the two vectors. in [-Pi..Pi]
// The sign is the sign of vecOrient * (vecV1 ** vecV2), i.e.
// is positif if the basis (vecV1, vecV2, vecOrient) is right oriented.
var
  vNormeProd : Float;
begin { - AngleR2 - }
vNormeProd:=Norme(vecV1) * Norme(vecV2);

if (vNormeProd <= 1e-30) then begin
   // Angle not defines
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   V3AngleOrientated:=0;
   end;

V3AngleOrientated:=ArcCos( (vecV1 * vecV2) / vNormeProd);

if (vecOrient * (vecV1 ** vecV2) < 0) then V3AngleOrientated:=-V3AngleOrientated;
end;  // V3AngleOrientated

procedure V3Normalise(var vecV:RVectR3Float);
//===========================================
// Normalise the vector  vecV
// If  vecV = 0  then return  vecV = (0,0,1)
var vNorme : Float;
begin
vNorme:=sqrt(sqr(vecV[1]) + sqr(vecV[2]) + sqr(vecV[3]));

if (vNorme <= 1e-15) then begin
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   vecV[1]:=0;  vecV[2]:=0;  vecV[3]:=1;
   exit;
   end;

vecV[1]:=vecV[1] / vNorme;
vecV[2]:=vecV[2] / vNorme;
vecV[3]:=vecV[3] / vNorme;
end;  // V3Normalise

function V3NormaliseFunc(const vecV:RVectR3Float):RVectR3Float;
//=============================================================
// Return the normalised the vector  vecV
// If  vecV = 0  then return  = (0,0,1)
var vNorme : Float;
begin
vNorme:=sqrt(sqr(vecV[1]) + sqr(vecV[2]) + sqr(vecV[3]));

if (vNorme <= 1e-15) then begin
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   V3NormaliseFunc[1]:=0;  V3NormaliseFunc[2]:=0;  V3NormaliseFunc[3]:=1;
   exit;
   end;

V3NormaliseFunc[1]:=vecV[1] / vNorme;
V3NormaliseFunc[2]:=vecV[2] / vNorme;
V3NormaliseFunc[3]:=vecV[3] / vNorme;
end;  // V3Normalise

function V3RotationOrg(const vecIn,vecAxe:RVectR3Float; vTheta:Float):RVectR3Float;
//=================================================================================
// Rotate the vector  vecIn  around the vector  vecAxe
// with an angle of  vTheta  radian.
// If vecAxe = 0, return the  vecIn  vector.
var
  vNorme2, vCos, vFact1, vFact3 : Float;

begin
// if  W  = rotation of  V  around  A  with an angle of  t  then :
//                   A * V                           A x V
// W = (1-cos(t)) * ------ * A + cos(t)*V + sin(t)* -------
//                 || A ||^2                        || A ||
vNorme2:=sqr(vecAxe[1]) + sqr(vecAxe[2]) + sqr(vecAxe[3]);

if (vNorme2 <= 1e-30) then begin
   // Seldom
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   V3RotationOrg:=vecIn;
   exit;
   end;

vCos:=cos(vTheta);
vFact1:= (vecIn[1]*vecAxe[1] + vecIn[2]*vecAxe[2] + vecIn[3]*vecAxe[3]) *
         (1 - vCos) / vNorme2;
vFact3:=sin(vTheta) / sqrt(vNorme2);

V3RotationOrg[1]:=vFact1 * vecAxe[1]  +  vCos * vecIn[1] +
                  vFact3 * (vecAxe[2] * vecIn[3] - vecAxe[3] * vecIn[2]);
V3RotationOrg[2]:=vFact1 * vecAxe[2]  +  vCos * vecIn[2] +
                  vFact3 * (vecAxe[3] * vecIn[1] - vecAxe[1] * vecIn[3]);
V3RotationOrg[3]:=vFact1 * vecAxe[3]  +  vCos * vecIn[3] +
                  vFact3 * (vecAxe[1] * vecIn[2] - vecAxe[2] * vecIn[1]);
end;  // V3RotationOrg

function V3RotationVec(vecIn,vecCenter,vecAxe:RVectR3Float; vTheta:Float):RVectR3Float;
//=====================================================================================
// Rotate the vector  vecIn  around the vector  vecAxe
// having vecCenter as a rotation center
// with an angle of  vTheta  radian.
// If vecAxe = 0, return the  vecIn  vector.
// Rotation(V3Assign(1,0,0), V3Assign(0,0,1), Pi/2) = (0,1,0)
// i.e. a rotation of 90 degrees aroung the z axis,
// send the x axis on the y axis, and the y axis on  - x axis
var
  vNorme2, vCos, vFact1, vFact3 : Float;

begin
// if  W  = rotation of  V  around  A  with an angle of  t  then :
//                   A * V                           A x V
// W = (1-cos(t)) * ------ * A + cos(t)*V + sin(t)* -------
//                 || A ||^2                        || A ||
vNorme2:=sqr(vecAxe[1]) + sqr(vecAxe[2]) + sqr(vecAxe[3]);

if (vNorme2 <= 1e-30) then begin
   // Seldom
   inc(gglwVectR3Error); // signal that a division by zero has been tempted
   V3RotationVec:=vecIn;
   exit;
   end;

vecIn[1]:=vecIn[1] - vecCenter[1];
vecIn[2]:=vecIn[2] - vecCenter[2];
vecIn[3]:=vecIn[3] - vecCenter[3];

vCos:=cos(vTheta);
vFact1:= (vecIn[1]*vecAxe[1] + vecIn[2]*vecAxe[2] + vecIn[3]*vecAxe[3]) *
         (1 - vCos) / vNorme2;
vFact3:=sin(vTheta) / sqrt(vNorme2);

V3RotationVec[1]:= vecCenter[1] +
                   vFact1 * vecAxe[1]  +  vCos * vecIn[1] +
                   vFact3 * (vecAxe[2] * vecIn[3] - vecAxe[3] * vecIn[2]);
V3RotationVec[2]:= vecCenter[2] +
                   vFact1 * vecAxe[2]  +  vCos * vecIn[2] +
                   vFact3 * (vecAxe[3] * vecIn[1] - vecAxe[1] * vecIn[3]);
V3RotationVec[3]:= vecCenter[3] +
                   vFact1 * vecAxe[3]  +  vCos * vecIn[3] +
                   vFact3 * (vecAxe[1] * vecIn[2] - vecAxe[2] * vecIn[1]);
end;  // V3RotationVec

function V3ToStr(const vecIn:RVectR3Float):ShortString;
//=====================================================
// Convert the vector into a string.
// usefull to display the vector in a debugging window.
begin
V3ToStr:='(' + FloatToStr(vecIn[1],8,3) + ',' + FloatToStr(vecIn[2],8,3) + ','
             + FloatToStr(vecIn[3],8,3) + ')';
end;  // V3ToStr

end. // unit xVectR3;
