unit AdvancedMath3D;

interface

uses Math, Math3D;

const
  PLANE_BACKSIDE              = 1;
  PLANE_FRONT                 = 2;
  ON_PLANE                    = 4;

  BOX_FRONT                   = 0;
  BOX_BACK                    = 1;
  BOX_LEFT                    = 2;
  BOX_RIGHT                   = 3;
  BOX_TOP                     = 4;
  BOX_BOTTOM                  = 5;

  BOX_FRONT_LEFT_TOP          = 0;
  BOX_BACK_LEFT_TOP           = 1;
  BOX_FRONT_RIGHT_TOP         = 2;
  BOX_BACK_RIGHT_TOP          = 3;
  BOX_FRONT_LEFT_BOTTOM       = 4;
  BOX_BACK_LEFT_BOTTOM        = 5;
  BOX_FRONT_RIGHT_BOTTOM      = 6;
  BOX_BACK_RIGHT_BOTTOM       = 7;

type
  PGlobalVectorArray = ^TGlobalVectorArray;
  TGlobalVectorArray = array[0..99] of TVector4;
  TGlobalVectorArrayStruct = record
    p: PGlobalVectorArray;
    nr: integer;
  end;

  TPlanePrism = array[0..2] of TPlane;
  TTriangle4 = array[0..2] of TVector4;
  TPolygon4 = array of TVector4;
  TClipBox = array[0..5] of TPlane;
  // FT, BK, LT, RT, TP, BT
  TBox4 = array[0..7] of TVector4;
  // FT-LT-TP, BK-LT-TP, FT-RT-TP, BK-RT-TP,
  // FT-LT-BT, BK-LT-BT, FT-RT-BT, BK-RT-BT

function ClassifyPoint(point, p0, pN: TVector4): cardinal;
function IntersectRayPlane(rOrigin, rVector, pOrigin, pNormal: TVector4): double;
function IntersectRaySphere(rO, rV, sO: TVector4; sR: double): double;
function CheckPointInTriangle(point, a, b, c: TVector4): boolean;
function CheckPointInSphere(point, sO: TVector4; sR: double): boolean;
function ClosestPointOnLine(var a, b, p: TVector4): TVector4;
function ClosestPointOnTriangle(a, b, c, p: TVector4): TVector4;

function ClipBoxFromBox(box: TBox4): TClipBox;
function ClipBoxTriangle(var clipbox: TClipBox; var tri: TTriangle4): TGlobalVectorArrayStruct;
function ClippedPolygonArea(v: TGlobalVectorArrayStruct): single;
procedure CombineBoundVectors(var v1: TVector4; v2: TVector4);
function BoundVector(var box: TBox4; var plane: TPlane; var normal: TVector4; var prism: TPlanePrism): TVector4;

function BoxTriangleCollision(var box: TBox4; var tri: TTriangle4; var normal, fw, lf, up, col: TVector4): boolean;

implementation

var
  GlobalVectorArray: array[0..1] of TGlobalVectorArray;

function ClassifyPoint(point, p0, pN: TVector4): cardinal;
var
  dir: TVector4;
  d: double;
begin
  dir := VecSubtract4(p0, point);
  d := VecDot4(dir, pN);

  if d > EPSILON then result := PLANE_FRONT else
  if d < -EPSILON then result := PLANE_BACKSIDE else result := ON_PLANE;
end;

function IntersectRayPlane(rOrigin, rVector, pOrigin, pNormal: TVector4): double;
var
  d, numer, denom: double;
begin
  d := -VecDot4(pNormal, pOrigin);
  numer := VecDot4(pNormal, rOrigin) + d;
  denom := VecDot4(pNormal, rVector);

  if denom = 0 then result := -1 else result := -(numer / denom);
end;

function IntersectRaySphere(rO, rV, sO: TVector4; sR: double): double;
var
  Q: TVector4;
  c, v, d: double;
begin
  Q := VecSubtract4(sO, rO);

  c := VecLength4(Q);
  v := VecDot4(Q, rV);
  d := sR*sR - (c*c - v*v);

  if d < 0 then result := -1 else result := v - sqrt(d);
  
end;

function CheckPointInTriangle(point, a, b, c: TVector4): boolean;
var
  total_angles: double;
  v1, v2, v3: TVector4;
begin
  v1 := VecSubtract4(point, a);
  v2 := VecSubtract4(point, b);
  v3 := VecSubtract4(point, c);

  VecNormalize4(v1);
  VecNormalize4(v2);
  VecNormalize4(v3);

  total_angles := arccos(VecDot4(v1, v2)) + arccos(VecDot4(v2, v3)) + arccos(VecDot4(v3, v1));

  if abs(total_angles-2*Pi) <= 0.005 then result := true else result := false;
end;

function CheckPointInSphere(point, sO: TVector4; sR: double): boolean;
var
  d: single;
begin
  d := VecLength4(VecSubtract4(point, sO));
  if d <= sR then result := true else result := false;  
end;

function ClosestPointOnLine(var a, b, p: TVector4): TVector4;
var
  c, v: TVector4;
  d, t: double;
begin
  c := VecSubtract4(p, a);
  v := VecSubtract4(b, a);

  d := VecLength4(v);

  VecNormalize4(v);
  t := VecDot4(v, c);

  if t < 0 then
  begin
    result := a;
    exit;
  end;
  if t > d then
  begin
    result := b;
    exit;
  end;

  v := VecScalarMult4(v, t);
  result := VecAdd4(a, v);
end;

function ClosestPointOnTriangle(a, b, c, p: TVector4): TVector4;
var
  Rab, Rbc, Rca: TVector4;
  dAB, dBC, dCA, min: double;
begin
  Rab := ClosestPointOnLine(a, b, p);
  Rbc := ClosestPointOnLine(b, c, p);
  Rca := ClosestPointOnLine(c, a, p);

  dAB := VecLength4(Rab);
  dBC := VecLength4(Rbc);
  dCA := VecLength4(Rca);

  min := dAB;
  result := Rab;

  if dBC < min then
  begin
    min := dBC;
    result := Rbc;
  end;

  if dCA < min then
    result := Rca;
end;

function ClipBoxFromBox(box: TBox4): TClipBox;
begin
  result[BOX_FRONT] :=
    PlaneFromPoints(
      box[BOX_FRONT_LEFT_BOTTOM],
      box[BOX_FRONT_LEFT_TOP],
      box[BOX_FRONT_RIGHT_TOP]
    );
  result[BOX_BACK] :=
    PlaneFromPoints(
      box[BOX_BACK_RIGHT_BOTTOM],
      box[BOX_BACK_RIGHT_TOP],
      box[BOX_BACK_LEFT_TOP]
    );
  result[BOX_LEFT] :=
    PlaneFromPoints(
      box[BOX_BACK_LEFT_BOTTOM],
      box[BOX_BACK_LEFT_TOP],
      box[BOX_FRONT_LEFT_TOP]
    );
  result[BOX_RIGHT] :=
    PlaneFromPoints(
      box[BOX_FRONT_RIGHT_BOTTOM],
      box[BOX_FRONT_RIGHT_TOP],
      box[BOX_BACK_RIGHT_TOP]
    );
  result[BOX_TOP] :=
    PlaneFromPoints(
      box[BOX_FRONT_LEFT_TOP],
      box[BOX_BACK_LEFT_TOP],
      box[BOX_BACK_RIGHT_TOP]
    );
  result[BOX_BOTTOM] :=
    PlaneFromPoints(
      box[BOX_BACK_LEFT_BOTTOM],
      box[BOX_FRONT_LEFT_BOTTOM],
      box[BOX_FRONT_RIGHT_BOTTOM]
    );
end;

function ClipBoxTriangle(var clipbox: TClipBox; var tri: TTriangle4): TGlobalVectorArrayStruct;
var
  src, dst: PGlobalVectorArray;
  nr_src, nr_dst, i, j: integer;
  A, B, V, N: TVector4;
  A_pos, B_pos, f: single;
  counter: integer;
begin
  counter := 0;
  src := @tri;
  nr_src := 3;
  dst := @GlobalVectorArray[counter];
  nr_dst := 0;
  for i := 0 to 5 do
  begin
    N := Vector4(clipbox[i].a, clipbox[i].b, clipbox[i].c, 1);
    A := src^[nr_src-1];
    A_pos := PlaneVecDist(clipbox[i], A);
    for j := 0 to nr_src-1 do
    begin
      B := src^[j];
      B_pos := PlaneVecDist(clipbox[i], B);

      if A_pos < 0 then
      begin
        if B_pos < 0 then
        begin
          dst^[nr_dst] := B;
          nr_dst := nr_dst+1;
        end else
        begin
          V := VecSubtract4(B, A);
          f := -PlaneVecDist(clipbox[i], A) / VecDot4(N, V);
          V := VecAdd4(A, VecScalarMult4(V, f));
          dst^[nr_dst] := V;
          nr_dst := nr_dst+1;
        end;
      end else
      begin
        if B_pos < 0 then
        begin
          V := VecSubtract4(B, A);
          f := -PlaneVecDist(clipbox[i], A) / VecDot4(N, V);
          V := VecAdd4(A, VecScalarMult4(V, f));
          dst^[nr_dst] := V;
          nr_dst := nr_dst+1;
          dst^[nr_dst] := B;
          nr_dst := nr_dst+1;
        end;
      end;

      A := B;
      A_pos := B_pos;
    end;
    if nr_dst = 0 then
    begin
      result.p := nil;
      result.nr := 0;
      exit;
    end;
    src := dst;
    nr_src := nr_dst;
    counter := counter xor 1;
    dst := @GlobalVectorArray[counter];
    nr_dst := 0;
  end;
  result.p := src;
  result.nr := nr_src;
end;

function ClippedPolygonArea(v: TGlobalVectorArrayStruct): single;
var
  i: Integer;
  c: TVector4;
begin
  result := 0;
  for i := 1 to v.nr-2 do
  begin
    c := VecCross4(VecSubtract4(v.p^[i], v.p^[0]), VecSubtract4(v.p^[i+1], v.p^[0]));
    result := result + VecLength4(c);
  end;
  result := result/2;
end;

procedure CombineBoundVectors(var v1: TVector4; v2: TVector4);
begin
  if abs(v1.x) < abs(v2.x) then v1.x := v2.x;
  if abs(v1.y) < abs(v2.y) then v1.y := v2.y;
  if abs(v1.z) < abs(v2.z) then v1.z := v2.z;
end;

function BoundVector(var box: TBox4; var plane: TPlane; var normal: TVector4; var prism: TPlanePrism): TVector4;
var
  i: integer;
  min, new: single;
begin
  min := 0;
  for i := 0 to 7 do
  begin
    if (PlaneVecDist(prism[0], box[i]) > 0) and
       (PlaneVecDist(prism[1], box[i]) > 0) and
       (PlaneVecDist(prism[2], box[i]) > 0) then
    begin
      new := PlaneVecDist(plane, box[i]);
      if new < min then min := new;
    end;
  end;
  result := VecScalarMult4(normal, -min);
end;

function BoxTriangleCollision(var box: TBox4; var tri: TTriangle4; var normal, fw, lf, up, col: TVector4): boolean;

  function TestAxis(var axis: TVector4): single;
  var
    tproj: array[0..2] of single;
    bproj: array[0..7] of single;
    tmin, tmax: single;
    bmin, bmax: single;
    i: integer;
  begin
    for i := 0 to 2 do
      tproj[i] := VecDot4(tri[i], axis);
    tmin := tproj[0]; tmax := tproj[0];
    for i := 1 to 2 do
      if tmin > tproj[i] then tmin := tproj[i] else
      if tmax < tproj[i] then tmax := tproj[i];
    for i := 0 to 5 do
      bproj[i] := VecDot4(box[i], axis);
    bmin := bproj[0]; bmax := bproj[0];
    for i := 1 to 5 do
      if bmin > bproj[i] then bmin := bproj[i] else
      if bmax < bproj[i] then bmax := bproj[i];
    if (tmin >= bmax) or (bmin >= tmax) then
    begin
      result := 0;
      exit;
    end else
    if tmin <= bmin then
    begin
      if tmax >= bmax then
      begin
        if bmin-tmin > tmax-bmax then
        begin
          result := tmax - bmin;
          exit;
        end else
        begin
          result := tmin - bmax;
          exit;
        end;
      end else
      begin
        result := tmax - bmin;
        exit;
      end;
    end else
    if bmin <= tmin  then
    begin
      if tmax >= bmax then
      begin
        result := tmin - bmax;
        exit;
      end else
      begin
        if tmin-bmin > bmax-tmax then
        begin
          result := tmin - bmax;
          exit;
        end else
        begin
          result := tmax - bmin;
          exit;
        end;
      end;
    end;
    result := 0;
  end;

var
  dif: array[0..12] of single;
  min, i: integer;
  f: array[0..2] of TVector4;
  a: array[0..12] of TVector4;

begin
  f[0] := VecSubtract4(tri[1], tri[0]);
  f[1] := VecSubtract4(tri[2], tri[1]);
  f[2] := VecSubtract4(tri[0], tri[2]);
  a[0] := fw;
  a[1] := lf;
  a[2] := up;
  a[3] := normal;
  a[4] := VecCross4(fw, f[0]);
  a[5] := VecCross4(fw, f[1]);
  a[6] := VecCross4(fw, f[2]);
  a[7] := VecCross4(lf, f[0]);
  a[8] := VecCross4(lf, f[1]);
  a[9] := VecCross4(lf, f[2]);
  a[10] := VecCross4(up, f[0]);
  a[11] := VecCross4(up, f[1]);
  a[12] := VecCross4(up, f[2]);
  for i := 0 to 12 do
  begin
    if VecComp4(a[i], ORIGIN4) then
    begin
      dif[i] := 1000000;
    end else
    begin
      dif[i] := TestAxis(a[i]);
      if (dif[i] = 0) then
      begin
        result := false;
        exit;
      end;
    end;
  end;
  min := 0;
  for i := 1 to 12 do
    
    if abs(dif[i]) < abs(dif[min]) then min := i;
  col := VecScalarMult4(a[min], dif[min]);
  result := true;
end;

end.
