{$MODE FPC}
{$MODESWITCH ADVANCEDRECORDS}
{$MODESWITCH AUTODEREF}
{$MODESWITCH DEFAULTPARAMETERS}
{$MODESWITCH OUT}
{$MODESWITCH RESULT}
unit dintersect3f;

interface

uses
  dmath,
  dequation,
  dvector3f,
  dquat4f;

type
PIntersectInfo3f = ^TIntersectInfo3f;
TIntersectInfo3f = record
  Pos: TVec3f;
  Normal: TVec3f;
  Delta: TVec3f;
  T: Single; // distance after raycast
end;

TTriangle3f = array[0 .. 2] of TVec3f;

TPlane3f = record
  N: TVec3f;
  D: Single;
end;

TTriangleWithPlane3f = record
  Vertices: TTriangle3f;
  Plane: TPlane3f;
end;

TSphere3f = record
  Pos: TVec3f;
  Radius: Single;
end;

TRay3f = record
  Pos: TVec3f;
  Dir: TVec3f;
end;

TBBox3f = record
  Min, Max: TVec3f;
end;

TOBox3f = record
public type
  TVertices = array[0 .. 8 - 1] of TVec3f;
public
  BBox: TBBox3f;
  Pos: TVec3f;
  Quat: TQuat4f;
  procedure ComputeVertices(var V: TVertices);
end;

// Project point P into plane
function Project(const N: TVec3f; D: Single; const P: TVec3f): TVec3f;
function Project(const T: TTriangle3f; const P: TVec3f): TVec3f;

function Sphere3f(const Pos: TVec3f; Radius: Single): TSphere3f;
function Ray3f(const Pos, Dir: TVec3f): TRay3f;

function Intersect(const Ray: TRay3f;
                   const T: TTriangle3f;
                   Info: PIntersectInfo3f = nil): Boolean;
function Intersect(const Ray: TRay3f;
                   const Sphere: TSphere3f;
                   Info: PIntersectInfo3f = nil): Boolean;
function Intersect(const Ray: TRay3f;
                   const Box: TBBox3f;
                   Info: PIntersectInfo3f = nil): Boolean;
function Intersect(const Ray: TRay3f;
                   const Box: TOBox3f;
                   Info: PIntersectInfo3f = nil): Boolean;
function Intersect(const Box: TOBox3f;
                   const T: TTriangle3f;
                   Info: PIntersectInfo3f = nil): Boolean;

function GetNormal(const A, B, C: TVec3f): TVec3f;
function GetNormal(const T: TTriangle3f): TVec3f;

function Intersect(const A: TTriangle3f;
                   const B: TSphere3f;
                   Info: PIntersectInfo3f = nil): Boolean;

implementation

procedure TOBox3f.ComputeVertices(var V: TVertices);
begin
  with BBox do begin
    V[0] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Min.X, Min.Y, Min.Z)));
    V[1] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Max.X, Min.Y, Min.Z)));
    V[2] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Min.X, Max.Y, Min.Z)));
    V[3] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Max.X, Max.Y, Min.Z)));
    V[4] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Min.X, Min.Y, Max.Z)));
    V[5] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Max.X, Min.Y, Max.Z)));
    V[6] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Min.X, Max.Y, Max.Z)));
    V[7] := Add3f(Pos, QuatRot4f(Quat, Vec3f(Max.X, Max.Y, Max.Z)));
  end;
end;

function Project(const N: TVec3f; D: Single; const P: TVec3f): TVec3f;
begin
  Result := Add3f(P, Mul3f(N, D - Dot3f(N, P)));
end;

function Project(const T: TTriangle3f; const P: TVec3f): TVec3f;
var
  Normal: TVec3f;
begin
  Normal := Norm3f(Cross3f(Sub3f(T[1], T[0]), Sub3f(T[2], T[0])));
  Result := Add3f(P, Mul3f(Normal, Dot3f(Normal, Sub3f(T[0], P))));
end;

function Sphere3f(const Pos: TVec3f; Radius: Single): TSphere3f;
begin
  Result.Pos := Pos;
  Result.Radius := Radius;
end;

function Ray3f(const Pos, Dir: TVec3f): TRay3f;
begin
  Result.Pos := Pos;
  Result.Dir := Dir;
end;

function Intersect(const Ray: TRay3f;
                   const T: TTriangle3f;
                   Info: PIntersectInfo3f = nil): Boolean;
begin
  Result := False; // TODO
end;

function Intersect(const Ray: TRay3f;
                   const Sphere: TSphere3f;
                   Info: PIntersectInfo3f = nil): Boolean;
var
  V: TIntersectInfo3f;
begin
  V.T := Dot3f(Sub3f(Sphere.Pos, Ray.Pos), Ray.Dir);
  if V.T < 0 then
    Exit(False);
  V.Pos := Add3f(Ray.Pos, Mul3f(Ray.Dir, V.T));
  if Dist3f(V.Pos, Sphere.Pos) > Sphere.Radius then
    Exit(False);
  if Info <> nil then begin
    Info^ := V;
  end;
  Exit(True);
end;

function Intersect(const Box: TOBox3f;
                   const T: TTriangle3f;
                   Info: PIntersectInfo3f = nil): Boolean;
begin
  Result := False;
end;

function GetNormal(const A, B, C: TVec3f): TVec3f;
begin
  Result := Cross3f(Sub3f(B, A), Sub3f(C, B));
end;

function GetNormal(const T: TTriangle3f): TVec3f;
begin
  Result := Cross3f(Sub3f(T[1], T[0]), Sub3f(T[2], T[1]));
end;

function CheckOrientation(const A, B, C: TVec3f;
                          const P1, P2: TVec3f): Boolean;
var
  N: TVec3f;
begin
  N := Norm3f(GetNormal(A, B, C));
  Result := Dot3f(N, Sub3f(P1, A)) * Dot3f(N, Sub3f(P2, A)) > 0;
end;

function Intersect(const A: TTriangle3f;
                   const B: TSphere3f;
                   Info: PIntersectInfo3f = nil): Boolean;
var
  V: TIntersectInfo3f;
  D, Dist: Single;
begin
  V.Normal := Norm3f(GetNormal(A));

  Dist := Dot3f(V.Normal, Sub3f(B.Pos, A[0]));
  if Abs(Dist) > B.Radius then
    Exit(False);

  V.Pos := Sub3f(B.Pos, Mul3f(V.Normal, Dist));
  if Dist < 0 then
    V.Normal := Neg3f(V.Normal);
  V.Delta := Mul3f(V.Normal, B.Radius - Abs(Dist));
  
  // Check if projection of B.Pos into plane is inside triangle
  if Dot3f(V.Normal, GetNormal(V.Pos, A[0], A[1])) < 0 then
    Exit(False);
  if Dot3f(V.Normal, GetNormal(V.Pos, A[1], A[2])) < 0 then
    Exit(False);
  if Dot3f(V.Normal, GetNormal(V.Pos, A[2], A[0])) < 0 then
    Exit(False);
  {if not (CheckOrientation(A[0], Add3f(A[0], V.Normal), A[1], V.Pos, A[2]) and 
          CheckOrientation(A[1], Add3f(A[1], V.Normal), A[2], V.Pos, A[0]) and 
          CheckOrientation(A[2], Add3f(A[2], V.Normal), A[0], V.Pos, A[1])) then
    Exit(False);}

  if Info <> nil then
    Info^ := V;
  Result := True;
end;

function _RayVsPlane(PosZ, DirZ, PlaneZ: Single;
                     PosX, PosY, DirX, DirY, PlaneX1, PlaneY1, PlaneX2, PlaneY2: Single;
                     out T: Single): Boolean; inline;
var
  X, Y: Single;
begin
  T := (PlaneZ - PosZ) / DirZ;
  if T < 0 then
    Exit(False);
  X := PosX + T * DirX;
  if (X < PlaneX1) or (X > PlaneX2) then
    Exit(False);
  Y := PosY + T * DirY;
  if (Y < PlaneY1) or (Y > PlaneY2) then
    Exit(False);
  Exit(True);
end;

function Intersect(const Ray: TRay3f;
                   const Box: TBBox3f;
                   Info: PIntersectInfo3f = nil): Boolean;
var
  T, TMin: Single;
begin
  TMin := 99999.0;
  T := 0;
  if Abs(Ray.Dir.X) > EPS then begin
    if _RayVsPlane(Ray.Pos.X, Ray.Dir.X, Box.Min.X,
                   Ray.Pos.Y, Ray.Pos.Z, Ray.Dir.Y, Ray.Dir.Z,
                   Box.Min.Y, Box.Min.Z, Box.Max.Y, Box.Max.Z,
                   T)
       and (T < TMin) then
      TMin := T;
    if _RayVsPlane(Ray.Pos.X, Ray.Dir.X, Box.Max.X,
                   Ray.Pos.Y, Ray.Pos.Z, Ray.Dir.Y, Ray.Dir.Z,
                   Box.Min.Y, Box.Min.Z, Box.Max.Y, Box.Max.Z,
                   T)
       and (T < TMin) then
      TMin := T;
  end;
  if Abs(Ray.Dir.Y) > EPS then begin
    if _RayVsPlane(Ray.Pos.Y, Ray.Dir.Y, Box.Min.Y,
                   Ray.Pos.X, Ray.Pos.Z, Ray.Dir.X, Ray.Dir.Z,
                   Box.Min.X, Box.Min.Z, Box.Max.X, Box.Max.Z,
                   T)
       and (T < TMin) then
      TMin := T;
    if _RayVsPlane(Ray.Pos.Y, Ray.Dir.Y, Box.Max.Y,
                   Ray.Pos.X, Ray.Pos.Z, Ray.Dir.X, Ray.Dir.Z,
                   Box.Min.X, Box.Min.Z, Box.Max.X, Box.Max.Z,
                   T)
       and (T < TMin) then
      TMin := T;
  end;
  if Abs(Ray.Dir.Z) > EPS then begin
    if _RayVsPlane(Ray.Pos.Z, Ray.Dir.Z, Box.Min.Z,
                   Ray.Pos.Y, Ray.Pos.X, Ray.Dir.Y, Ray.Dir.X,
                   Box.Min.Y, Box.Min.X, Box.Max.Y, Box.Max.X,
                   T)
       and (T < TMin) then
      TMin := T;
    if _RayVsPlane(Ray.Pos.Z, Ray.Dir.Z, Box.Max.Z,
                   Ray.Pos.Y, Ray.Pos.X, Ray.Dir.Y, Ray.Dir.X,
                   Box.Min.Y, Box.Min.X, Box.Max.Y, Box.Max.X,
                   T)
       and (T < TMin) then
      TMin := T;
  end;
  if (TMin >= 0) and (TMin < 99999.0) then begin
    if Info <> nil then
      Info^.T := TMin;
    Exit(True);
  end else
    Exit(False);
end;

function Intersect(const Ray: TRay3f;
                   const Box: TOBox3f;
                   Info: PIntersectInfo3f = nil): Boolean;
var
  LocalRay: TRay3f;
begin
  LocalRay.Pos := QuatRot4f(QuatInv4f(Box.Quat), Sub3f(Ray.Pos, Box.Pos));
  LocalRay.Dir := QuatRot4f(QuatInv4f(Box.Quat), Ray.Dir);
  //LocalRay.Pos := Sub3f(Ray.Pos, Box.Pos);
  //LocalRay.Dir := Ray.Dir;
  Result := Intersect(LocalRay, Box.BBox, Info);
end;

end.
