unit Math3D;

interface

uses Math;

const
  EPSILON: single = 0.0001;

type
  TVector2 = record
    case boolean of
      true      : (x, y: single);
      false     : (xy: array[0..1] of single);
  end;
  TVector3 = record
    case boolean of
      true      : (x, y, z: single);
      false     : (xyz: array[0..2] of single);
  end;
  TVector4 = record
    case boolean of
      true      : (x, y, z, w: single);
      false     : (xyzw: array[0..3] of single);
  end;

function Vector2(x, y: single): TVector2;
function Vector3(x, y, z: single): TVector3; overload;
function Vector3(const v: TVector2; z: single): TVector3; overload;
function Vector4(x, y, z, w: single): TVector4; overload;
function Vector4(const v: TVector3; w: single): TVector4; overload;

const
  ORIGIN2       : TVector2 = (xy: (0, 0));
  X_AXIS2       : TVector2 = (xy: (1, 0));
  Y_AXIS2       : TVector2 = (xy: (0, 1));
  Z_AXIS2       : TVector2 = (xy: (0, 0));

  ORIGIN3       : TVector3 = (xyz: (0, 0, 0));
  X_AXIS3       : TVector3 = (xyz: (1, 0, 0));
  Y_AXIS3       : TVector3 = (xyz: (0, 1, 0));
  Z_AXIS3       : TVector3 = (xyz: (0, 0, 1));

  ORIGIN4       : TVector4 = (xyzw: (0, 0, 0, 1));
  X_AXIS4       : TVector4 = (xyzw: (1, 0, 0, 1));
  Y_AXIS4       : TVector4 = (xyzw: (0, 1, 0, 1));
  Z_AXIS4       : TVector4 = (xyzw: (0, 0, 1, 1));

type
  TMatrix2x2 = array[0..1, 0..1] of single;
  TMatrix3x3 = array[0..2, 0..2] of single;
  TMatrix4x4 = array[0..3, 0..3] of single;

const
  IDENTITY2     : TMatrix2x2 = ((1, 0),
                                (0, 1));
  NULLMATRIX2   : TMatrix2x2 = ((0, 0),
                                (0, 0));

  IDENTITY3     : TMatrix3x3 = ((1, 0, 0),
                                (0, 1, 0),
                                (0, 0, 1));
  NULLMATRIX3   : TMatrix3x3 = ((0, 0, 0),
                                (0, 0, 0),
                                (0, 0, 0));

  IDENTITY4     : TMatrix4x4 = ((1, 0, 0, 0),
                                (0, 1, 0, 0),
                                (0, 0, 1, 0),
                                (0, 0, 0, 1));
  NULLMATRIX4   : TMatrix4x4 = ((0, 0, 0, 0),
                                (0, 0, 0, 0),
                                (0, 0, 0, 0),
                                (0, 0, 0, 0));

type
  TPlane = record
    case boolean of
      true      : (a, b, c, d: single);
      false     : (normal: TVector3; dist: single);
  end;
  TQuaternion = record
    case boolean of
      true      : (w, x, y, z: single);
      false     : (v: array[0..3] of single);
  end;
  TSphereCoords = record
    r, theta, phi: single;
  end;
  TEuler = record
    pitch, yaw, roll: single;
  end;

function Plane(a, b, c, d: single): TPlane; overload;
function Plane(normal: TVector3; d: single): TPlane; overload;
function Quaternion(w, x, y, z: single): TQuaternion;
function Euler(pitch, yaw, roll: single): TEuler;

function VecComp2(v1, v2: TVector2): boolean;
function VecAdd2(v1, v2: TVector2): TVector2;
function VecSubtract2(v1, v2: TVector2): TVector2;
function VecScalarMult2(v: TVector2; s: single): TVector2;
function VecDot2(v1, v2: TVector2): single;
function VecNegate2(v: TVector2): TVector2;
procedure VecScale2(var v: TVector2; sx, sy: single); overload;
procedure VecScale2(var v: TVector2; s: single); overload;
function VecMult2(v1, v2: TVector2): TVector2;
function VecLength2(v: TVector2): single;
function VecLengthSquared2(v: TVector2): single;
function VecAngle2(v1, v2, v3: TVector2): single;
procedure VecNormalize2(var v: TVector2);
procedure VecRotate2(var v: TVector2; a: single); overload;
procedure VecRotate2(var v: TVector2; center: TVector2; a: single); overload;
procedure VecMatrixMult2(var v: TVector2; m: TMatrix2x2);
function VecLerp2(v1, v2: TVector2; w: single): TVector2;

function VecComp3(v1, v2: TVector3): boolean;
function VecAdd3(v1, v2: TVector3): TVector3;
function VecSubtract3(v1, v2: TVector3): TVector3;
function VecScalarMult3(v: TVector3; s: single): TVector3;
function VecDot3(v1, v2: TVector3): single;
function VecCross3(v1, v2: TVector3): TVector3;
function VecNegate3(v: TVector3): TVector3;
procedure VecScale3(var v: TVector3; sx, sy, sz: single); overload;
procedure VecScale3(var v: TVector3; s: single); overload;
function VecMult3(v1, v2: TVector3): TVector3;
function VecLength3(v: TVector3): single;
function VecLengthSquared3(v: TVector3): single;
function VecAngle3(v1, v2, v3: TVector3): single;
procedure VecNormalize3(var v: TVector3);
procedure VecRotateX3(var v: TVector3; a: single);
procedure VecRotateY3(var v: TVector3; a: single);
procedure VecRotateZ3(var v: TVector3; a: single);
procedure VecRotate3(var v: TVector3; axis: TVector3; a: single); overload;
procedure VecMatrixMult3(var v: TVector3; m: TMatrix3x3);
function VecLerp3(v1, v2: TVector3; w: single): TVector3;

function VecComp4(v1, v2: TVector4): boolean;
function VecAdd4(v1, v2: TVector4): TVector4;
function VecSubtract4(v1, v2: TVector4): TVector4;
function VecScalarMult4(v: TVector4; s: single): TVector4;
function VecDot4(v1, v2: TVector4): single;
function VecCross4(v1, v2: TVector4): TVector4;
function VecNegate4(v: TVector4): TVector4;
procedure VecScale4(var v: TVector4; sx, sy, sz: single); overload;
procedure VecScale4(var v: TVector4; s: single); overload;
function VecMult4(v1, v2: TVector4): TVector4;
function VecLength4(v: TVector4): single;
function VecLengthSquared4(v: TVector4): single;
function VecAngle4(v1, v2, v3: TVector4): single;
procedure VecNormalize4(var v: TVector4);
procedure VecRotateX4(var v: TVector4; a: single);
procedure VecRotateY4(var v: TVector4; a: single);
procedure VecRotateZ4(var v: TVector4; a: single);
procedure VecRotate4(var v: TVector4; axis: TVector4; a: single); overload;
procedure VecMatrixMult4(var v: TVector4; m: TMatrix4x4);
function VecLerp4(v1, v2: TVector4; w: single): TVector4;

function MatRotateX3(angle: single): TMatrix3x3;
function MatRotateY3(angle: single): TMatrix3x3;
function MatRotateZ3(angle: single): TMatrix3x3;
function MatRotate3(pyr: TEuler): TMatrix3x3; overload;
function MatRotate3(angle: single; axis: TVector3): TMatrix3x3; overload;
function MatScale3(sx, sy, sz: single): TMatrix3x3;
procedure MatAdd3(var m1: TMatrix3x3; m2: TMatrix3x3);
procedure MatSubtract3(var m1: TMatrix3x3; m2: TMatrix3x3);
procedure MatMult3(var m1: TMatrix3x3; m2: TMatrix3x3);
procedure MatScalarMult3(var m: TMatrix3x3; s: single);
procedure MatTranspose3(var m: TMatrix3x3);
function MatDeterminant3(const m: TMatrix3x3): single;
function MatToEuler3(const m: TMatrix3x3): TEuler;
procedure MatSetColumn3(var m: TMatrix3x3; const c: integer; const v: TVector3);
function MatGetColumn3(const m: TMatrix3x3; const c: integer): TVector3;
function MatGetRow3(const m: TMatrix3x3; const r: integer): TVector3;
function MatComp3(const m1, m2: TMatrix3x3): boolean;
function MatInverse3(var mr: TMatrix3x3; const ma: TMatrix3x3): boolean;
function MatMapAtoB3(const A, B: TVector3): TMatrix3x3;

function MatRotateX4(angle: single): TMatrix4x4;
function MatRotateY4(angle: single): TMatrix4x4;
function MatRotateZ4(angle: single): TMatrix4x4;
function MatRotate4(pyr: TEuler): TMatrix4x4; overload;
function MatRotate4(angle: single; axis: TVector4): TMatrix4x4; overload;
function MatTranslate4(x, y, z: single): TMatrix4x4;
function MatScale4(sx, sy, sz: single): TMatrix4x4;
procedure MatAdd4(var m1: TMatrix4x4; m2: TMatrix4x4);
procedure MatSubtract4(var m1: TMatrix4x4; m2: TMatrix4x4);
procedure MatMult4(var m1: TMatrix4x4; m2: TMatrix4x4);
procedure MatScalarMult4(var m: TMatrix4x4; s: single);
procedure MatTranspose4(var m: TMatrix4x4);
function MatDeterminant4(const m: TMatrix4x4): single;
function MatToEuler4(const m: TMatrix4x4): TEuler;
procedure MatSetColumn4(var m: TMatrix4x4; const c: integer; const v: TVector4);
function MatGetColumn4(const m: TMatrix4x4; const c: integer): TVector4;
function MatGetRow4(const m: TMatrix4x4; const r: integer): TVector4;
function MatComp4(const m1, m2: TMatrix4x4): boolean;
function MatInverse4(var mr: TMatrix4x4; const ma: TMatrix4x4): boolean;
function MatGet3x3SubMatrix4(const m: TMatrix4x4; i, j: integer): TMatrix3x3;

function PlaneVecDist(plane: TPlane; point: TVector3): single; overload;
function PlaneVecDist(plane: TPlane; point: TVector4): single; overload;
function PlaneFromPoints(p1, p2, p3: TVector3): TPlane; overload;
function PlaneFromPoints(p1, p2, p3: TVector4): TPlane; overload;

function QuatNorm(const q: TQuaternion): single;
procedure QuatNormalize(var q: TQuaternion);
function QuatMult(const q1, q2: TQuaternion): TQuaternion;
function QuatFromAxisAngle(const axis: TVector3; angle: single): TQuaternion; overload;
function QuatFromEuler(pitch, yaw, roll: single): TQuaternion; overload;
function QuatToMatrix(const q: TQuaternion): TMatrix3x3;
procedure QuatToAxisAngle(const q: TQuaternion; var axis: TVector3;
                          var angle: single);
function QuatSlerp(q1, q2: TQuaternion; t: single): TQuaternion;

function VecCartesianToSpherical3(const v: TVector3): TSphereCoords;
function VecCartesianToSpherical4(const v: TVector4): TSphereCoords;
function VecSphericalToCartesian3(const s: TSphereCoords): TVector3;
function VecSphericalToCartesian4(const s: TSphereCoords): TVector4;

implementation

{******************************************************************************}
{*** Vector types *************************************************************}
{******************************************************************************}

function Vector2(x, y: single): TVector2;
begin
  Result.x := x;
  Result.y := y;
end;

function Vector3(x, y, z: single): TVector3;
begin
  Result.x := x;
  Result.y := y;
  Result.z := z;
end;

function Vector3(const v: TVector2; z: single): TVector3;
begin
  Result.x := v.x;
  Result.y := v.y;
  Result.z := z;
end;

function Vector4(x, y, z, w: single): TVector4;
begin
  Result.x := x;
  Result.y := y;
  Result.z := z;
  Result.w := w;
end;

function Vector4(const v: TVector3; w: single): TVector4;
begin
  Result.x := v.x;
  Result.y := v.y;
  Result.z := v.z;
  Result.w := w;
end;

{******************************************************************************}
{*** Other types **************************************************************}
{******************************************************************************}

function Plane(a, b, c, d: single): TPlane;
begin
  Result.a := a;
  Result.b := b;
  Result.c := c;
  Result.d := d;
end;

function Plane(normal: TVector3; d: single): TPlane;
begin
  Result.normal := normal;
  Result.dist := d;
end;

function Quaternion(w, x, y, z: single): TQuaternion;
begin
  Result.w := w;
  Result.x := x;
  Result.y := y;
  Result.z := z;
end;

function Euler(pitch, yaw, roll: single): TEuler;
begin
  Result.pitch := pitch;
  Result.yaw := yaw;
  Result.roll := roll;
end;

{******************************************************************************}
{*** Vector2 operations *******************************************************}
{******************************************************************************}

function VecComp2(v1, v2: TVector2): boolean;
begin
  Result := (Abs(v1.x - v2.x) < EPSILON) and (Abs(v1.y - v2.y) < EPSILON);
end;

function VecAdd2(v1, v2: TVector2): TVector2;
begin
  Result.x := v1.x + v2.x;
  Result.y := v1.y + v2.y;
end;

function VecSubtract2(v1, v2: TVector2): TVector2;
begin
  Result.x := v1.x - v2.x;
  Result.y := v1.y - v2.y;
end;

function VecScalarMult2(v: TVector2; s: single): TVector2;
begin
  Result.x := v.x * s;
  Result.y := v.y * s;
end;

function VecDot2(v1, v2: TVector2): single;
begin
  Result := v1.x*v2.x + v1.y*v2.y;
end;

function VecNegate2(v: TVector2): TVector2;
begin
  Result.x := -v.x;
  Result.y := -v.y;
end;

procedure VecScale2(var v: TVector2; sx, sy: single); overload;
begin
  v.x := v.x * sx;
  v.y := v.y * sy;
end;

procedure VecScale2(var v: TVector2; s: single); overload;
begin
  v.x := v.x * s;
  v.y := v.y * s;
end;

function VecMult2(v1, v2: TVector2): TVector2;
begin
  Result.x := v1.x * v2.x;
  Result.y := v1.y * v2.y;
end;

function VecLength2(v: TVector2): single;
begin
  Result := sqrt(v.x*v.x + v.y*v.y);
end;

function VecLengthSquared2(v: TVector2): single;
begin
  Result := v.x*v.x + v.y*v.y;
end;

function VecAngle2(v1, v2, v3: TVector2): single;
var
  a1, a2: TVector2;
  L1, L2: single;
begin
  a1 := VecSubtract2(v1, v2);
  a2 := VecSubtract2(v3, v2);
  L1 := VecLength2(a1);
  L2 := VecLength2(a2);
  if (L1 = 0) or (L2 = 0) then Result := 0
                          else Result := ArcCos(VecDot2(a1, a2) / (L1*L2));
end;

procedure VecNormalize2(var v: TVector2);
var
  L: single;
begin
  L := VecLengthSquared2(v);
  if L = 0 then v := ORIGIN2 else
  begin
    L := sqrt(L);
    v.x := v.x / L;
    v.y := v.y / L;
  end;
end;

procedure VecRotate2(var v: TVector2; a: single);
var
  r: TVector2;
begin
  r.x := v.x*cos(a) - v.y*sin(a);
  r.y := v.y*cos(a) + v.x*sin(a);
  v := r;
end;

procedure VecRotate2(var v: TVector2; center: TVector2; a: single);
var
  v0, r: TVector2;
begin
  v0 := VecSubtract2(v, center);
  r.x := v0.x*cos(a) - v0.y*sin(a);
  r.y := v0.y*cos(a) + v0.x*sin(a);
  v := VecAdd2(r, center);
end;

procedure VecMatrixMult2(var v: TVector2; m: TMatrix2x2);
var
  t: TVector2;
  r, c: integer;
begin
  // Multiply v with the matrix m.
  for c := 0 to 1 do
  begin
    t.xy[c] := 0;
    for r := 0 to 1 do
    begin
      t.xy[c] := t.xy[c] + v.xy[r] * m[r,c];
    end;
  end;
  v := t;
end;

function VecLerp2(v1, v2: TVector2; w: single): TVector2;
begin
  Result.x := (1-w)*v1.x + w*v2.x;
  Result.y := (1-w)*v1.y + w*v2.y;
end;

{******************************************************************************}
{*** Vector3 operations *******************************************************}
{******************************************************************************}

function VecComp3(v1, v2: TVector3): boolean;
begin
  Result := (Abs(v1.x - v2.x) < EPSILON) and
            (Abs(v1.y - v2.y) < EPSILON) and
            (Abs(v1.z - v2.z) < EPSILON);
end;

function VecAdd3(v1, v2: TVector3): TVector3;
begin
  Result.x := v1.x + v2.x;
  Result.y := v1.y + v2.y;
  Result.z := v1.z + v2.z;
end;

function VecSubtract3(v1, v2: TVector3): TVector3;
begin
  Result.x := v1.x - v2.x;
  Result.y := v1.y - v2.y;
  Result.z := v1.z - v2.z;
end;

function VecScalarMult3(v: TVector3; s: single): TVector3;
begin
  Result.x := v.x * s;
  Result.y := v.y * s;
  Result.z := v.z * s;
end;

function VecDot3(v1, v2: TVector3): single;
begin
  Result := v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
end;

function VecCross3(v1, v2: TVector3): TVector3;
begin
  Result.x := v1.y * v2.z - v2.y * v1.z;
  Result.y := v2.x * v1.z - v1.x * v2.z;
  Result.z := v1.x * v2.y - v2.x * v1.y;
end;

function VecNegate3(v: TVector3): TVector3;
begin
  Result.x := -v.x;
  Result.y := -v.y;
  Result.z := -v.z;
end;

procedure VecScale3(var v: TVector3; sx, sy, sz: single); overload;
begin
  v.x := v.x * sx;
  v.y := v.y * sy;
  v.z := v.z * sz;
end;

procedure VecScale3(var v: TVector3; s: single); overload;
begin
  v.x := v.x * s;
  v.y := v.y * s;
  v.z := v.z * s;
end;

function VecMult3(v1, v2: TVector3): TVector3;
begin
  Result.x := v1.x * v2.x;
  Result.y := v1.y * v2.y;
  Result.z := v1.z * v2.z;
end;

function VecLength3(v: TVector3): single;
begin
  Result := sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
end;

function VecLengthSquared3(v: TVector3): single;
begin
  Result := v.x*v.x + v.y*v.y + v.z*v.z;
end;

function VecAngle3(v1, v2, v3: TVector3): single;
var
  a1, a2: TVector3;
  L1, L2: single;
begin
  a1 := VecSubtract3(v1, v2);
  a2 := VecSubtract3(v3, v2);
  L1 := VecLength3(a1);
  L2 := VecLength3(a2);
  if (L1 = 0) or (L2 = 0) then Result := 0
                          else Result := ArcCos(VecDot3(a1, a2) / (L1*L2));
end;

procedure VecNormalize3(var v: TVector3);
var
  L: single;
begin
  L := VecLengthSquared3(v);
  if L = 0 then v := ORIGIN3 else
  begin
    L := sqrt(L);
    v.x := v.x / L;
    v.y := v.y / L;
    v.z := v.z / L;
  end;
end;

procedure VecRotateX3(var v: TVector3; a: single);
var
  temp: TVector3;
begin
  with temp do
  begin
    x := v.x;
    y := (v.y * cos(a)) + (v.z * -sin(a));
    z := (v.y * sin(a)) + (v.z * cos(a));
  end;
  v := temp;
end;

procedure VecRotateY3(var v: TVector3; a: single);
var
  temp: TVector3;
begin
  with temp do
  begin
    x := (v.x * cos(a)) + (v.z * sin(a));
    y := v.y;
    z := (v.x * -sin(a)) + (v.z * cos(a));
  end;
  v := temp;
end;

procedure VecRotateZ3(var v: TVector3; a: single);
var
  temp: TVector3;
begin
  with temp do
  begin
    x := (v.x * cos(a)) + (v.y * -sin(a));
    y := (v.x * sin(a)) + (v.y * cos(a));
    z := v.z;
  end;
  v := temp;
end;

procedure VecRotate3(var v: TVector3; axis: TVector3; a: single);
begin
  VecMatrixMult3(v, MatRotate3(a, axis));
end;

procedure VecMatrixMult3(var v: TVector3; m: TMatrix3x3);
var
  t: TVector3;
  r, c: integer;
begin
  // Multiply v with the matrix m.
  for c := 0 to 2 do
  begin
    t.xyz[c] := 0;
    for r := 0 to 2 do
    begin
      t.xyz[c] := t.xyz[c] + v.xyz[r] * m[r,c];
    end;
  end;
  v := t;
end;

function VecLerp3(v1, v2: TVector3; w: single): TVector3;
begin
  Result.x := (1-w)*v1.x + w*v2.x;
  Result.y := (1-w)*v1.y + w*v2.y;
  Result.z := (1-w)*v1.z + w*v2.z;
end;

{******************************************************************************}
{*** Vector4 operations *******************************************************}
{******************************************************************************}

function VecComp4(v1, v2: TVector4): boolean;
begin
  Result := (Abs(v1.x - v2.x) < EPSILON) and
            (Abs(v1.y - v2.y) < EPSILON) and
            (Abs(v1.z - v2.z) < EPSILON) and
            (Abs(v1.w - v2.w) < EPSILON);
end;

function VecAdd4(v1, v2: TVector4): TVector4;
begin
  Result.x := v1.x + v2.x;
  Result.y := v1.y + v2.y;
  Result.z := v1.z + v2.z;
  Result.w := 1;
end;

function VecSubtract4(v1, v2: TVector4): TVector4;
begin
  Result.x := v1.x - v2.x;
  Result.y := v1.y - v2.y;
  Result.z := v1.z - v2.z;
  Result.w := 1;
end;

function VecScalarMult4(v: TVector4; s: single): TVector4;
begin
  Result.x := v.x * s;
  Result.y := v.y * s;
  Result.z := v.z * s;
  Result.w := 1;
end;

function VecDot4(v1, v2: TVector4): single;
begin
  Result := v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
end;

function VecCross4(v1, v2: TVector4): TVector4;
begin
  Result.x := v1.y * v2.z - v2.y * v1.z;
  Result.y := v2.x * v1.z - v1.x * v2.z;
  Result.z := v1.x * v2.y - v2.x * v1.y;
  Result.w := 1;
end;

function VecNegate4(v: TVector4): TVector4;
begin
  Result.x := -v.x;
  Result.y := -v.y;
  Result.z := -v.z;
  Result.w := 1;
end;

procedure VecScale4(var v: TVector4; sx, sy, sz: single); overload;
begin
  v.x := v.x * sx;
  v.y := v.y * sy;
  v.z := v.z * sz;
end;

procedure VecScale4(var v: TVector4; s: single); overload;
begin
  v.x := v.x * s;
  v.y := v.y * s;
  v.z := v.z * s;
end;

function VecMult4(v1, v2: TVector4): TVector4;
begin
  Result.x := v1.x * v2.x;
  Result.y := v1.y * v2.y;
  Result.z := v1.z * v2.z;
  Result.w := 1;
end;

function VecLength4(v: TVector4): single;
begin
  Result := sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
end;

function VecLengthSquared4(v: TVector4): single;
begin
  Result := v.x*v.x + v.y*v.y + v.z*v.z;
end;

function VecAngle4(v1, v2, v3: TVector4): single;
var
  a1, a2: TVector4;
  L1, L2: single;
begin
  a1 := VecSubtract4(v1, v2);
  a2 := VecSubtract4(v3, v2);
  L1 := VecLength4(a1);
  L2 := VecLength4(a2);
  if (L1 = 0) or (L2 = 0) then Result := 0
                          else Result := ArcCos(VecDot4(a1, a2) / (L1*L2));
end;

procedure VecNormalize4(var v: TVector4);
var
  L: single;
begin
  L := VecLengthSquared4(v);
  if L = 0 then v := ORIGIN4 else
  begin
    L := sqrt(L);
    v.x := v.x / L;
    v.y := v.y / L;
    v.z := v.z / L;
  end;
end;

procedure VecRotateX4(var v: TVector4; a: single);
var
  temp: TVector4;
begin
  with temp do
  begin
    x := v.x;
    y := (v.y * cos(a)) + (v.z * -sin(a));
    z := (v.y * sin(a)) + (v.z * cos(a));
    w := 1;
  end;
  v := temp;
end;

procedure VecRotateY4(var v: TVector4; a: single);
var
  temp: TVector4;
begin
  with temp do
  begin
    x := (v.x * cos(a)) + (v.z * sin(a));
    y := v.y;
    z := (v.x * -sin(a)) + (v.z * cos(a));
    w := 1;
  end;
  v := temp;
end;

procedure VecRotateZ4(var v: TVector4; a: single);
var
  temp: TVector4;
begin
  with temp do
  begin
    x := (v.x * cos(a)) + (v.y * -sin(a));
    y := (v.x * sin(a)) + (v.y * cos(a));
    z := v.z;
    w := 1;
  end;
  v := temp;
end;

procedure VecRotate4(var v: TVector4; axis: TVector4; a: single);
begin
  VecMatrixMult4(v, MatRotate4(a, axis));
end;

procedure VecMatrixMult4(var v: TVector4; m: TMatrix4x4);
var
  t: TVector4;
  r, c: integer;
begin
  // Multiply v with the matrix m.
  for c := 0 to 3 do
  begin
    t.xyzw[c] := 0;
    for r := 0 to 3 do
    begin
      t.xyzw[c] := t.xyzw[c] + v.xyzw[r] * m[r,c];
    end;
  end;
  v := t;
end;

function VecLerp4(v1, v2: TVector4; w: Single): TVector4;
begin
  Result.x := (1-w)*v1.x + w*v2.x;
  Result.y := (1-w)*v1.y + w*v2.y;
  Result.z := (1-w)*v1.z + w*v2.z;
  Result.w := 1;
end;

{******************************************************************************}
{*** Matrix3x3 operations *****************************************************}
{******************************************************************************}

function MatRotateX3(angle: single): TMatrix3x3;
begin
  Result := IDENTITY3;
  Result[1,1] := cos(angle);
  Result[2,2] := Result[1,1];
  Result[1,2] := sin(angle);
  Result[2,1] := -Result[1,2];
end;

function MatRotateY3(angle: single): TMatrix3x3;
begin
  Result := IDENTITY3;
  Result[0,0] := cos(angle);
  Result[2,2] := Result[0,0];
  Result[0,2] := -sin(angle);
  Result[2,0] := -Result[0,2];
end;

function MatRotateZ3(angle: single): TMatrix3x3;
begin
  Result := IDENTITY3;
  Result[0,0] := cos(angle);
  Result[1,1] := Result[0,0];
  Result[0,1] := sin(angle);
  Result[1,0] := -Result[0,1];
end;

function MatRotate3(pyr: TEuler): TMatrix3x3;
var
  A, B, C, D, E, F, AD, BD: single;
begin
  A := cos(pyr.pitch);       B := sin(pyr.pitch);
  C := cos(pyr.yaw);         D := sin(pyr.yaw);
  E := cos(pyr.roll);        F := sin(pyr.roll);

  AD := A*D;
  BD := B*D;

  Result[0,0] := C*E;
  Result[1,0] := -C*F;
  Result[2,0] := D;

  Result[0,1] := BD*E + A*F;
  Result[1,1] := -BD*F + A*E;
  Result[2,1] := -B*C;

  Result[0,2] := -AD*E + B*F;
  Result[1,2] := AD*F + B*E;
  Result[2,2] := A*C;
end;

function MatRotate3(angle: single; axis: TVector3): TMatrix3x3;
var
  m: TMatrix3x3;
  cosA, sinA: single;
begin
  // Return a rotation matrix for an arbitrary axis through the origin.
  cosA := cos(angle);
  sinA := sin(angle);

  m[0,0] := cosA + (1 - cosA)*axis.x*axis.x;
  m[1,0] := (1 - cosA)*axis.x*axis.y - axis.z*sinA;
  m[2,0] := (1 - cosA)*axis.x*axis.z + axis.y*sinA;

  m[0,1] := (1 - cosA)*axis.x*axis.z + axis.z*sinA;
  m[1,1] := cosA + (1 - cosA)*axis.y*axis.y;
  m[2,1] := (1 - cosA)*axis.y*axis.z - axis.x*sinA;

  m[0,2] := (1 - cosA)*axis.x*axis.z - axis.y*sinA;
  m[1,2] := (1 - cosA)*axis.y*axis.z + axis.x*sinA;
  m[2,2] := cosA + (1 - cosA)*axis.z*axis.z;

  Result := m;
end;

function MatScale3(sx, sy, sz: single): TMatrix3x3;
begin
  Result := IDENTITY3;
  Result[0,0] := sx;
  Result[1,1] := sy;
  Result[2,2] := sz;
end;

procedure MatAdd3(var m1: TMatrix3x3; m2: TMatrix3x3);
var
  i, j: integer;
begin
  for i := 0 to 2 do
  begin
    for j := 0 to 2 do
    begin
      m1[i,j] := m1[i,j] + m2[i,j];
    end;
  end;
end;

procedure MatSubtract3(var m1: TMatrix3x3; m2: TMatrix3x3);
var
  i, j: integer;
begin
  for i := 0 to 2 do
  begin
    for j := 0 to 2 do
    begin
      m1[i,j] := m1[i,j] - m2[i,j];
    end;
  end;
end;

procedure MatMult3(var m1: TMatrix3x3; m2: TMatrix3x3);
var
  r, c, i: byte;
  t: TMatrix3x3;
begin
  // Multiply two matrices.
  t := NULLMATRIX3;
  for r := 0 to 2 do
  begin
    for c := 0 to 2 do
    begin
      for i := 0 to 2 do
      begin
        t[r,c] := t[r,c] + (m1[r,i] * m2[i,c]);
      end;
    end;
  end;
  m1 := t;
end;

procedure MatScalarMult3(var m: TMatrix3x3; s: single);
var
  i, j: integer;
begin
  for i := 0 to 2 do
  begin
    for j := 0 to 2 do
    begin
      m[i,j] := m[i,j] * s;
    end;
  end;
end;

procedure MatTranspose3(var m: TMatrix3x3);
var
  mt: TMatrix3x3;
  r, c: integer;
begin
  mt := m;
  for r := 0 to 2 do
  begin
    for c := 0 to 2 do
    begin
      mt[r,c] := m[c,r];
    end;
  end;
  m := mt;
end;

function MatDeterminant3(const m: TMatrix3x3): single;
begin
  Result := m[0,0] * (m[1,1]*m[2,2] - m[2,1]*m[1,2])
          - m[0,1] * (m[1,0]*m[2,2] - m[2,0]*m[1,2])
          + m[0,2] * (m[1,0]*m[2,1] - m[2,0]*m[1,1]);
end;

function MatToEuler3(const m: TMatrix3x3): TEuler;
var
  angle_x, angle_y, angle_z: single;
  C, D: single;
  tx, ty: single;
begin
  D :=  ArcSin(m[2,0]);
  angle_y := D;
  C := cos(angle_y);

  if Abs(C) > 0.005 then
  begin
    tx := m[2,2] / C;
    ty := -m[2,1] / C;

    angle_x := ArcTan2(ty, tx);

    tx := m[0,0] / C;
    ty := -m[1,0] / C;

    angle_z := ArcTan2(ty, tx);
  end else
  begin
    angle_x := 0;

    tx := m[1,1];
    ty := m[0,1];

    angle_z := ArcTan2(ty, tx);
  end;

  if angle_x < 0 then angle_x := angle_x + 2*PI;
  if angle_y < 0 then angle_y := angle_y + 2*PI;
  if angle_z < 0 then angle_z := angle_z + 2*PI;

  Result.pitch := angle_x;
  Result.yaw := angle_y;
  Result.roll := angle_z;
end;

procedure MatSetColumn3(var m: TMatrix3x3; const c: integer; const v: TVector3);
begin
  m[c,0] := v.x;
  m[c,1] := v.y;
  m[c,2] := v.z;
end;

function MatGetColumn3(const m: TMatrix3x3; const c: integer): TVector3;
begin
  with Result do
  begin
    x := m[c,0];
    y := m[c,1];
    z := m[c,2];
  end;
end;

function MatGetRow3(const m: TMatrix3x3; const r: integer): TVector3;
begin
  with Result do
  begin
    x := m[0,r];
    y := m[1,r];
    z := m[2,r];
  end;
end;

function MatComp3(const m1, m2: TMatrix3x3): boolean;
var
  r, c: integer;
begin
  Result := TRUE;
  for r := 0 to 2 do
  begin
    for c := 0 to 2 do
    begin
      if Abs(m1[r,c] - m2[r,c]) > EPSILON then
      begin
        Result := FALSE;
        Exit;
      end;
    end;
  end;
end;

function MatInverse3(var mr: TMatrix3x3; const ma: TMatrix3x3): boolean;
var
  det: single;
begin
  det := MatDeterminant3(ma);
  if Abs(det) < EPSILON then
  begin
    mr := IDENTITY3;
    Result := FALSE;
    Exit;
  end;

  mr[0,0] :=    ma[1,1]*ma[2,2] - ma[1,2]*ma[2,1]   / det;
  mr[0,1] := -( ma[0,1]*ma[2,2] - ma[2,1]*ma[0,2] ) / det;
  mr[0,2] :=    ma[0,1]*ma[1,2] - ma[1,1]*ma[0,2]   / det;

  mr[1,0] := -( ma[1,0]*ma[2,2] - ma[1,2]*ma[2,0] ) / det;
  mr[1,1] :=    ma[0,0]*ma[2,2] - ma[2,0]*ma[0,2]   / det;
  mr[1,2] := -( ma[0,0]*ma[1,2] - ma[1,0]*ma[0,2] ) / det;

  mr[2,0] :=    ma[1,0]*ma[2,1] - ma[2,0]*ma[1,1]   / det;
  mr[2,1] := -( ma[0,0]*ma[2,1] - ma[2,0]*ma[0,1] ) / det;
  mr[2,2] :=    ma[0,0]*ma[1,1] - ma[0,1]*ma[1,0]   / det;

  Result := TRUE;
end;

function MatMapAtoB3(const A, B: TVector3): TMatrix3x3;
var
  axis: TVector3;
  angle, dot: single;
  mat: TMatrix3x3;
  q: TQuaternion;
begin
  dot := VecDot3(A, B);
  // If the two inputs identical, we don't need a rotation at all.
  if Abs(dot - 1) < EPSILON then
  begin
    mat := IDENTITY3;
  end else
  begin
    // Rotation axis is the cross product of the two inputs.
    axis := VecCross3(A, B);
    // BUT! If the inputs are eachother's opposite, just use a 180° rotation.
    if Abs(dot + 1) < EPSILON then
    begin
      //TODO: This won't work if the inputs are on the X axis?
      q := QuatFromAxisAngle(Vector3(1, 0, 0), PI);
      mat := QuatToMatrix(q);
    end else
    begin
      // The vectors are identical nor opposite, so proceed.
      angle := ArcCos(dot);
      //TODO: Isn't this redundant? Shouldn't it be in the "if opposite" test?
      if VecComp3(axis, ORIGIN3) then
      begin
        axis := Vector3(B.y, A.z, A.x);
        VecNormalize3(axis);
        axis := VecCross3(A, axis);
        angle := PI;
      end;
      VecNormalize3(axis);
      // Now that we have axis/angle, convert it to a matrix.
      q := QuatFromAxisAngle(axis, angle);
      mat := QuatToMatrix(q);
    end;
  end;
  Result := mat;
end;

{******************************************************************************}
{*** Matrix4x4 operations *****************************************************}
{******************************************************************************}

function MatRotateX4(angle: single): TMatrix4x4;
begin
  Result := IDENTITY4;
  Result[1,1] := cos(angle);
  Result[2,2] := Result[1,1];
  Result[1,2] := sin(angle);
  Result[2,1] := -Result[1,2];
end;

function MatRotateY4(angle: single): TMatrix4x4;
begin
  Result := IDENTITY4;
  Result[0,0] := cos(angle);
  Result[2,2] := Result[0,0];
  Result[0,2] := -sin(angle);
  Result[2,0] := -Result[0,2];
end;

function MatRotateZ4(angle: single): TMatrix4x4;
begin
  Result := IDENTITY4;
  Result[0,0] := cos(angle);
  Result[1,1] := Result[0,0];
  Result[0,1] := sin(angle);
  Result[1,0] := -Result[0,1];
end;

function MatRotate4(pyr: TEuler): TMatrix4x4;
var
  A, B, C, D, E, F, AD, BD: single;
begin
  A := cos(pyr.pitch);       B := sin(pyr.pitch);
  C := cos(pyr.yaw);         D := sin(pyr.yaw);
  E := cos(pyr.roll);        F := sin(pyr.roll);

  AD := A*D;
  BD := B*D;

  Result := IDENTITY4;

  Result[0,0] := C*E;
  Result[1,0] := -C*F;
  Result[2,0] := D;

  Result[0,1] := BD*E + A*F;
  Result[1,1] := -BD*F + A*E;
  Result[2,1] := -B*C;

  Result[0,2] := -AD*E + B*F;
  Result[1,2] := AD*F + B*E;
  Result[2,2] := A*C;
end;

function MatRotate4(angle: single; axis: TVector4): TMatrix4x4;
var
  m: TMatrix4x4;
  cosA, sinA: single;
begin
  // Return a rotation matrix for an arbitrary axis through the origin.
  m := IDENTITY4;
  cosA := cos(angle);
  sinA := sin(angle);

  m[0,0] := cosA + (1 - cosA)*axis.x*axis.x;
  m[1,0] := (1 - cosA)*axis.x*axis.y - axis.z*sinA;
  m[2,0] := (1 - cosA)*axis.x*axis.z + axis.y*sinA;

  m[0,1] := (1 - cosA)*axis.x*axis.z + axis.z*sinA;
  m[1,1] := cosA + (1 - cosA)*axis.y*axis.y;
  m[2,1] := (1 - cosA)*axis.y*axis.z - axis.x*sinA;

  m[0,2] := (1 - cosA)*axis.x*axis.z - axis.y*sinA;
  m[1,2] := (1 - cosA)*axis.y*axis.z + axis.x*sinA;
  m[2,2] := cosA + (1 - cosA)*axis.z*axis.z;

  Result := m;
end;

function MatTranslate4(x, y, z: single): TMatrix4x4;
begin
  Result := IDENTITY4;
  Result[3,0] := x;
  Result[3,1] := y;
  Result[3,2] := z;
end;

function MatScale4(sx, sy, sz: single): TMatrix4x4;
begin
  Result := IDENTITY4;
  Result[0,0] := sx;
  Result[1,1] := sy;
  Result[2,2] := sz;
end;

procedure MatAdd4(var m1: TMatrix4x4; m2: TMatrix4x4);
var
  i, j: integer;
begin
  for i := 0 to 3 do
  begin
    for j := 0 to 3 do
    begin
      m1[i,j] := m1[i,j] + m2[i,j];
    end;
  end;
end;

procedure MatSubtract4(var m1: TMatrix4x4; m2: TMatrix4x4);
var
  i, j: integer;
begin
  for i := 0 to 3 do
  begin
    for j := 0 to 3 do
    begin
      m1[i,j] := m1[i,j] - m2[i,j];
    end;
  end;
end;

procedure MatMult4(var m1: TMatrix4x4; m2: TMatrix4x4);
var
  r, c, i: byte;
  t: TMatrix4x4;
begin
  // Multiply two matrices.
  t := NULLMATRIX4;
  for r := 0 to 3 do
  begin
    for c := 0 to 3 do
    begin
      for i := 0 to 3 do
      begin
        t[r,c] := t[r,c] + (m1[r,i] * m2[i,c]);
      end;
    end;
  end;
  m1 := t;
end;

procedure MatScalarMult4(var m: TMatrix4x4; s: single);
var
  i, j: integer;
begin
  for i := 0 to 3 do
  begin
    for j := 0 to 3 do
    begin
      m[i,j] := m[i,j] * s;
    end;
  end;
end;

procedure MatTranspose4(var m: TMatrix4x4);
var
  mt: TMatrix4x4;
  r, c: integer;
begin
  mt := m;
  for r := 0 to 3 do
  begin
    for c := 0 to 3 do
    begin
      mt[r,c] := m[c,r];
    end;
  end;
  m := mt;
end;

function MatDeterminant4(const m: TMatrix4x4): single;
var
  det, i: single;
  msub3: TMatrix3x3;
  n: integer;
begin
  Result := 0;
  i := 1;
  for n := 0 to 3 do
  begin
    msub3 := MatGet3x3SubMatrix4(m, n, 0);
    det := MatDeterminant3(msub3);
    Result := Result + m[n,0] * det * i;
    i := -i;
  end;
end;

function MatToEuler4(const m: TMatrix4x4): TEuler;
var
  angle_x, angle_y, angle_z: single;
  C, D: single;
  tx, ty: single;
begin
  D :=  ArcSin(m[2,0]);
  angle_y := D;
  C := cos(angle_y);
  if Abs(C) > 0.005 then
  begin
    tx := m[2,2] / C;
    ty := -m[2,1] / C;

    angle_x := ArcTan2(ty, tx);

    tx := m[0,0] / C;
    ty := -m[1,0] / C;

    angle_z := ArcTan2(ty, tx);
  end else
  begin
    angle_x := 0;

    tx := m[1,1];
    ty := m[0,1];

    angle_z := ArcTan2(ty, tx);
  end;
  if angle_x < 0 then angle_x := angle_x + 2*PI;
  if angle_y < 0 then angle_y := angle_y + 2*PI;
  if angle_z < 0 then angle_z := angle_z + 2*PI;

  Result.pitch := angle_x;
  Result.yaw := angle_y;
  Result.roll := angle_z;
end;

procedure MatSetColumn4(var m: TMatrix4x4; const c: integer; const v: TVector4);
begin
  m[c,0] := v.x;
  m[c,1] := v.y;
  m[c,2] := v.z;
  m[c,3] := v.w;
end;

function MatGetColumn4(const m: TMatrix4x4; const c: integer): TVector4;
begin
  with Result do
  begin
    x := m[c,0];
    y := m[c,1];
    z := m[c,2];
    w := m[c,3];
  end;
end;

function MatGetRow4(const m: TMatrix4x4; const r: integer): TVector4;
begin
  with Result do
  begin
    x := m[0,r];
    y := m[1,r];
    z := m[2,r];
    w := m[3,r];
  end;
end;

function MatComp4(const m1, m2: TMatrix4x4): boolean;
var
  r, c: integer;
begin
  Result := TRUE;
  for r := 0 to 3 do
  begin
    for c := 0 to 3 do
    begin
      if Abs(m1[r,c] - m2[r,c]) > EPSILON then
      begin
        Result := FALSE;
        Exit;
      end;
    end;
  end;
end;

function MatInverse4(var mr: TMatrix4x4; const ma: TMatrix4x4): boolean;
var
  mdet: single;
  mtemp: TMatrix3x3;
  i, j, sign: integer;
begin
  mdet := MatDeterminant4(ma);
  if Abs(mdet) < EPSILON then
  begin
    mr := IDENTITY4;
    Result := FALSE;
    Exit;
  end;
  for i := 0 to 3 do
  begin
    for j := 0 to 3 do
    begin
      sign := 1 - ((i+j) mod 2) * 2;
      mtemp := MatGet3x3SubMatrix4(ma, i, j);
      mr[j,i] := MatDeterminant3(mtemp) * sign / mdet;
    end;
  end;
  Result := TRUE;
end;

function MatGet3x3SubMatrix4(const m: TMatrix4x4; i, j: integer): TMatrix3x3;
var
  di, dj, si, sj: integer;
begin
  for di := 0 to 2 do
  begin
    for dj := 0 to 2 do
    begin
      if di >= i then si := di + 1
                 else si := di;
      if dj >= j then sj := dj + 1
                 else sj := dj;
      Result[di,dj] := m[si,sj];
    end;
  end;
end;

{******************************************************************************}
{*** Plane operations *********************************************************}
{******************************************************************************}

function PlaneVecDist(plane: TPlane; point: TVector3): single;
begin
  Result := (plane.a * point.x) + (plane.b * point.y) + (plane.c * point.z) + plane.d;
end;

function PlaneVecDist(plane: TPlane; point: TVector4): single;
begin
  Result := (plane.a * point.x) + (plane.b * point.y) + (plane.c * point.z) + plane.d;
end;

function PlaneFromPoints(p1, p2, p3: TVector3): TPlane;
var
  n: TVector3;
begin
  n := VecCross3(VecSubtract3(p2, p1), VecSubtract3(p3, p1));
  VecNormalize3(n);
  with Result do
  begin
    a := n.x;
    b := n.y;
    c := n.z;
    d := -(a * p1.x + b * p1.y + c * p1.z);
  end;
end;

function PlaneFromPoints(p1, p2, p3: TVector4): TPlane;
var
  n: TVector4;
begin
  n := VecCross4(VecSubtract4(p2, p1), VecSubtract4(p3, p1));
  VecNormalize4(n);
  with Result do
  begin
    a := n.x;
    b := n.y;
    c := n.z;
    d := -(a * p1.x + b * p1.y + c * p1.z);
  end;
end;

{******************************************************************************}
{*** Quaternion operations ****************************************************}
{******************************************************************************}

function QuatNorm(const q: TQuaternion): single;
begin
  Result := sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
end;

procedure QuatNormalize(var q: TQuaternion);
var
  mag: single;
begin
  mag := QuatNorm(q);
  if mag <> 0 then
  begin
    q.x := q.x / mag;
    q.y := q.y / mag;
    q.z := q.z / mag;
    q.w := q.w / mag;
  end;
end;

function QuatMult(const q1, q2: TQuaternion): TQuaternion;
var
  v1, v2, qvec: TVector3;
  qr: TQuaternion;
begin
  v1 := Vector3(q1.x, q1.y, q1.z);
  v2 := Vector3(q2.x, q2.y, q2.z);

  qr.w := q1.w * q2.w - VecDot3(v1, v2);
  qvec := VecAdd3(VecScalarMult3(v2, q1.w),
          VecAdd3(VecScalarMult3(v1, q2.w),
          VecCross3(v1, v2)));
  qr.x := qvec.x;
  qr.y := qvec.y;
  qr.z := qvec.z;

  QuatNormalize(qr);

  Result := qr;
end;

function QuatFromAxisAngle(const axis: TVector3; angle: single): TQuaternion; overload;
var
  sa2: single;
begin
  Result.w := cos(angle/2);

  sa2 := sin(angle/2);

  Result.x := axis.x * sa2;
  Result.y := axis.y * sa2;
  Result.z := axis.z * sa2;

  QuatNormalize(Result);
end;

function QuatFromEuler(pitch, yaw, roll: Single): TQuaternion; overload;
var
  q1, q2, q3: TQuaternion;
begin
  q1 := Quaternion(cos(pitch/2), sin(pitch/2), 0, 0);
  q2 := Quaternion(cos(yaw/2), 0, sin(yaw/2), 0);
  q3 := Quaternion(cos(roll/2), 0, 0, sin(roll/2));
  Result := QuatMult(QuatMult(q3, q2), q1);
end;

function QuatToMatrix(const q: TQuaternion): TMatrix3x3;
begin
  Result[0,0] := 1 - 2*q.y*q.y - 2*q.z*q.z;
  Result[0,1] := 2*q.x*q.y + 2*q.w*q.z;
  Result[0,2] := 2*q.x*q.z - 2*q.w*q.y;

  Result[1,0] := 2*q.x*q.y - 2*q.w*q.z;
  Result[1,1] := 1 - 2*q.x*q.x - 2*q.z*q.z;
  Result[1,2] := 2*q.y*q.z + 2*q.w*q.x;

  Result[2,0] := 2*q.x*q.z + 2*q.w*q.y;
  Result[2,1] := 2*q.y*q.z - 2*q.w*q.x;
  Result[2,2] := 1 - 2*q.x*q.x - 2*q.y*q.y;
end;

procedure QuatToAxisAngle(const q: TQuaternion; var axis: TVector3;
                          var angle: single);
var
  s: single;
begin
  s := q.x*q.x + q.y*q.y + q.z*q.z;
  angle := 2 * ArcCos(q.w);
  axis.x := q.x / s;
  axis.y := q.y / s;
  axis.z := q.z / s;
end;

function QuatSlerp(q1, q2: TQuaternion; t: single): TQuaternion;
var
  q: array [0..3] of single;
  omega, cosom, sinom, scale0, scale1: single;
begin
  cosom := q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
  if cosom < 0 then
  begin
    cosom  := -cosom;
    q[0] := -q2.x;
    q[1] := -q2.y;
    q[2] := -q2.z;
    q[3] := -q2.w;
  end else
  begin
    q[0] := q2.x;
    q[1] := q2.y;
    q[2] := q2.z;
    q[3] := q2.w;
  end;
  if (1 - cosom) > EPSILON then
  begin
    omega := ArcCos(cosom);
    sinom := sin(omega);
    scale0 := sin((1.0 - t) * omega) / sinom;
    scale1 := sin(t * omega) / sinom;
  end else
  begin
    scale0 := 1 - t;
    scale1 := t;
  end;
  // calculate final values
  Result.x := (scale0 * q1.x) + (scale1 * q[0]);
  Result.y := (scale0 * q1.y) + (scale1 * q[1]);
  Result.z := (scale0 * q1.z) + (scale1 * q[2]);
  Result.w := (scale0 * q1.w) + (scale1 * q[3]);
end;

{******************************************************************************}
{*** Spherical coordinate operations ******************************************}
{******************************************************************************}

function VecCartesianToSpherical3(const v: TVector3): TSphereCoords;
begin
  with Result do
  begin
    r := VecLength3(v);
    theta := ArcCos(v.z/r);
    phi := Sign(v.y)*ArcCos(v.x/sqrt(v.x*v.x + v.y*v.y));
  end;
end;

function VecCartesianToSpherical4(const v: TVector4): TSphereCoords;
begin
  with Result do
  begin
    r := VecLength4(v);
    theta := ArcCos(v.z/r);
    phi := Sign(v.y)*ArcCos(v.x/sqrt(v.x*v.x + v.y*v.y));
  end;
end;

function VecSphericalToCartesian3(const s: TSphereCoords): TVector3;
begin
  with Result do
  begin
    x := s.r*sin(s.theta)*cos(s.phi);
    y := s.r*sin(s.theta)*sin(s.phi);
    z := s.r*cos(s.theta);
  end;
end;

function VecSphericalToCartesian4(const s: TSphereCoords): TVector4;
begin
  with Result do
  begin
    x := s.r*sin(s.theta)*cos(s.phi);
    y := s.r*sin(s.theta)*sin(s.phi);
    z := s.r*cos(s.theta);
    w := 1;
  end;
end;

end.
