Bilateral advancement

// Bilateral advancement
function TEnginePhysics.GetTimeOfImpactForContinuousCollisionDetection(const ShapeA:TEnginePhysicsShape;const SweepA:TEnginePhysicsSweep;const ShapeB:TEnginePhysicsShape;const ShapeBTriangleIndex:longint;const SweepB:TEnginePhysicsSweep;const DeltaTime:single;const ThreadIndex:longint;var Beta:single):boolean;
const sfmNONE=0;
      sfmVERTICES=1;
      sfmEDGEA=2;
      sfmEDGEB=3;
      sfmFACEA=4;
      sfmFACEB=5;
      sfmEDGES=6;
var Iteration,TryIteration,RootIteration,Index,SubIndex,SeparationFunctionMode,
    ia0,ia1,ia2,ib0,ib1,ib2,ic0,ic1,ic2,a0i,a1i,b0i,b1i:longint;
    t0,t1,s0,s1,a0,a1,t,s,tS,tT0,tT1,TotalRadius,Target,Tolerance,Distance,CurrentDistance,L:single;
    Unprocessed,Overlapping:boolean;
    SegmentTriangle:TSegmentTriangle;
    ShapeTriangle:TEnginePhysicsShapeTriangle;
    MeshShape:TEnginePhysicsShapeMesh;
    MeshTriangle:PEnginePhysicsMeshTriangle;
    Shapes:array[0..1] of TEnginePhysicsShape;
    Points,WitnessPoints:array[0..1] of TVector3;
    TempPoints,Normals:array[0..1] of TVector3;
    Transforms:array[0..1] of TMatrix4x4;
    Axis,LocalVertex,va0,va1,va2,va,vb0,vb1,vb2,vb,eA,eB:TVector3;
    Plane:TPlane;
    FeatureGJKState:TEnginePhysicsFeatureGJKState;
    VertexIndexRows:array[0..2] of PEnginePhysicsFeatureGJKStateVertexIndexSimplexRow;
begin

 result:=false;

 Shapes[0]:=ShapeA;

 if (ShapeBTriangleIndex>=0) and (ShapeB is TEnginePhysicsShapeMesh) then begin
  MeshShape:=TEnginePhysicsShapeMesh(ShapeB);
  ShapeTriangle:=TEnginePhysicsShapeTriangle(TriangleShapes[ThreadIndex]);
  Shapes[1]:=ShapeTriangle;
  MeshTriangle:=@MeshShape.Mesh.Triangles[ShapeBTriangleIndex];
  ShapeTriangle.LocalTransform:=MeshShape.LocalTransform;
  ShapeTriangle.WorldTransform:=MeshShape.WorldTransform;
  ShapeTriangle.ConvexHull.Vertices[0].Position:=MeshShape.Mesh.Vertices[MeshTriangle^.Vertices[0]];
  ShapeTriangle.ConvexHull.Vertices[1].Position:=MeshShape.Mesh.Vertices[MeshTriangle^.Vertices[1]];
  ShapeTriangle.ConvexHull.Vertices[2].Position:=MeshShape.Mesh.Vertices[MeshTriangle^.Vertices[2]];
  ShapeTriangle.UpdateData;
 end else begin
  Shapes[1]:=ShapeB;
 end;

 TotalRadius:=Shapes[0].FeatureRadius+Shapes[1].FeatureRadius;

 Target:=Max(LinearSlop,TotalRadius-(3.0*LinearSlop));

 Tolerance:=LinearSlop*0.25;

 ClearFeatureGJKState(FeatureGJKState);

 FeatureGJKState.Shapes[0]:=Shapes[0];
 FeatureGJKState.Shapes[1]:=Shapes[1];
 FeatureGJKState.Transforms[0]:=@Transforms[0];
 FeatureGJKState.Transforms[1]:=@Transforms[1];
 FeatureGJKState.UseRadii:=false;

 // Compute current closest features. Setup a separation function to evaluate
 // overlap on the axis between the closest features. Terminate if closest
 // features are repeated.

 t0:=0.0;

 Axis:=Vector3Origin;

 SeparationFunctionMode:=sfmNONE;

 for Iteration:=1 to TimeOfImpactMaximumIterations do begin

  Transforms[0]:=Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,t0));
  Transforms[1]:=Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,t0));

  FeatureGJKState.SimplexSize:=0;
  FeatureGJKState.Initialized:=false;
  if not FeatureGJKRun(FeatureGJKState) then begin
   exit;
  end;

  // TOI is not defined if shapes began in an overlapping configuration
  if FeatureGJKState.Distance<EPSILON then begin
   Beta:=0.0;
   result:=false;
   exit;
  end;

  // Check for initial convergent state
  if FeatureGJKState.Distance<(Target+Tolerance) then begin
   Beta:=t0;
   result:=true;
   exit;
  end;

  case FeatureGJKState.SimplexSize of
   1:begin
    // Vertex-vertex
    VertexIndexRows[0]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[0]];
    Axis:=Vector3NormEx(Vector3Sub(Shapes[1].GetLocalFeatureSupportVertex(VertexIndexRows[0]^[1]),
                                   Shapes[0].GetLocalFeatureSupportVertex(VertexIndexRows[0]^[0])));
    SeparationFunctionMode:=sfmVERTICES;
   end;
   2:begin
    VertexIndexRows[0]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[0]];
    VertexIndexRows[1]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[1]];
    ia0:=VertexIndexRows[0]^[0];
    ia1:=VertexIndexRows[1]^[0];
    ib0:=VertexIndexRows[0]^[1];
    ib1:=VertexIndexRows[1]^[1];
    if ia0<>ia1 then begin
     // Edge-vertex
     va0:=Shapes[0].GetLocalFeatureSupportVertex(ia0);
     va1:=Shapes[0].GetLocalFeatureSupportVertex(ia1);
     vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib0),Transforms[1]),Transforms[0]);
     Axis:=Vector3Sub(va1,va0);
     Axis:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(vb0,va0)),Axis));
     LocalVertex:=va0;
     SeparationFunctionMode:=sfmEDGEA;
    end else if ib0<>ib1 then begin
     // Vertex-edge
     va0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(ia0),Transforms[0]),Transforms[1]);
     vb0:=Shapes[1].GetLocalFeatureSupportVertex(ib0);
     vb1:=Shapes[1].GetLocalFeatureSupportVertex(ib1);
     Axis:=Vector3Sub(vb1,vb0);
     Axis:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(va0,vb0)),Axis));
     LocalVertex:=vb0;
     SeparationFunctionMode:=sfmEDGEB;
    end else begin
     // Vertex-vertex
     Axis:=Vector3NormEx(Vector3Sub(Shapes[1].GetLocalFeatureSupportVertex(ib0),Shapes[0].GetLocalFeatureSupportVertex(ia0)));
     SeparationFunctionMode:=sfmVERTICES;
    end;
   end;
   3:begin
    VertexIndexRows[0]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[0]];
    VertexIndexRows[1]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[1]];
    VertexIndexRows[2]:=@FeatureGJKState.VertexIndexSimplices[FeatureGJKState.Permutation[2]];
    ia0:=VertexIndexRows[0]^[0];
    ia1:=VertexIndexRows[1]^[0];
    ia2:=VertexIndexRows[2]^[0];
    ib0:=VertexIndexRows[0]^[1];
    ib1:=VertexIndexRows[1]^[1];
    ib2:=VertexIndexRows[2]^[1];
    if (ia0<>ia1) and (ia0<>ia2) and (ia1<>ia2) then begin
     // Face-vertex or face-edge
     if (ib0<>ib1) and ((ib0=ib2) or (ib1=ib2)) then begin
      // TO-DO: Do correct face-edge handling
      va0:=Shapes[0].GetLocalFeatureSupportVertex(ia0);
      va1:=Shapes[0].GetLocalFeatureSupportVertex(ia1);
      va2:=Shapes[0].GetLocalFeatureSupportVertex(ia2);
      vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib0),Transforms[1]),Transforms[0]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(va0,va1),Vector3Sub(va2,va1)));
      if Vector3Dot(vb0,Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=va0;
      SeparationFunctionMode:=sfmFACEA;
     end else if (ib0<>ib2) and ((ib0=ib1) or (ib1=ib2)) then begin
      // TO-DO: Do correct face-edge handling
      va0:=Shapes[0].GetLocalFeatureSupportVertex(ia0);
      va1:=Shapes[0].GetLocalFeatureSupportVertex(ia1);
      va2:=Shapes[0].GetLocalFeatureSupportVertex(ia2);
      vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib0),Transforms[1]),Transforms[0]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(va0,va1),Vector3Sub(va2,va1)));
      if Vector3Dot(vb0,Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=va0;
      SeparationFunctionMode:=sfmFACEA;
     end else begin
      // Face-vertex
      va0:=Shapes[0].GetLocalFeatureSupportVertex(ia0);
      va1:=Shapes[0].GetLocalFeatureSupportVertex(ia1);
      va2:=Shapes[0].GetLocalFeatureSupportVertex(ia2);
      vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib0),Transforms[1]),Transforms[0]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(va0,va1),Vector3Sub(va2,va1)));
      if Vector3Dot(vb0,Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=va0;
      SeparationFunctionMode:=sfmFACEA;
     end;
    end else if (ib0<>ib1) and (ib0<>ib2) and (ib1<>ib2) then begin
     // Vertex-face or edge-face
     if (ia0<>ia1) and ((ia0=ia2) or (ia1=ia2)) then begin
      // TO-DO: Do correct face-edge handling
      va0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(ia0),Transforms[0]),Transforms[1]);
      vb0:=Shapes[1].GetLocalFeatureSupportVertex(ib0);
      vb1:=Shapes[1].GetLocalFeatureSupportVertex(ib1);
      vb2:=Shapes[1].GetLocalFeatureSupportVertex(ib2);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(vb0,vb1),Vector3Sub(vb2,vb1)));
      if Vector3Dot(va0,Axis)<0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=vb0;
      SeparationFunctionMode:=sfmFACEB;
     end else if (ia0<>ia2) and ((ia0=ia1) or (ia1=ia2)) then begin
      // TO-DO: Do correct face-edge handling
      va0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(ia0),Transforms[0]),Transforms[1]);
      vb0:=Shapes[1].GetLocalFeatureSupportVertex(ib0);
      vb1:=Shapes[1].GetLocalFeatureSupportVertex(ib1);
      vb2:=Shapes[1].GetLocalFeatureSupportVertex(ib2);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(vb0,vb1),Vector3Sub(vb2,vb1)));
      if Vector3Dot(va0,Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=vb0;
      SeparationFunctionMode:=sfmFACEB;
     end else begin
      // Face-vertex
      va0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(ia0),Transforms[0]),Transforms[1]);
      vb0:=Shapes[1].GetLocalFeatureSupportVertex(ib0);
      vb1:=Shapes[1].GetLocalFeatureSupportVertex(ib1);
      vb2:=Shapes[1].GetLocalFeatureSupportVertex(ib2);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(vb0,vb1),Vector3Sub(vb2,vb1)));
      if Vector3Dot(va0,Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalVertex:=vb0;
      SeparationFunctionMode:=sfmFACEB;
     end;
    end else begin
     // Vertex-vertex or edge-vertex or vertex-edge or edge-edge
     if (ia0<>ia1) and ((ia0=ia2) or (ia1=ia2)) then begin
      a0i:=ia0;
      a1i:=ia1;
     end else if (ia0<>ia2) and ((ia0=ia1) or (ia1=ia2)) then begin
      a0i:=ia0;
      a1i:=ia2;
     end else begin
      a0i:=ia0;
      a1i:=ia0;
     end;
     if (ib0<>ib1) and ((ib0=ib2) or (ib1=ib2)) then begin
      b0i:=ib0;
      b1i:=ib1;
     end else if (ib0<>ib2) and ((ib0=ib1) or (ib1=ib2)) then begin
      b0i:=ib0;
      b1i:=ib2;
     end else begin
      b0i:=ib0;
      b1i:=ib0;
     end;
     if a0i<>a1i then begin
      if b0i<>b1i then begin
       // Edge-edge
       va0:=Shapes[0].GetLocalFeatureSupportVertex(a0i);
       va1:=Shapes[0].GetLocalFeatureSupportVertex(a1i);
       vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib0),Transforms[1]),Transforms[0]);
       vb1:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(ib1),Transforms[1]),Transforms[0]);
       eA:=Vector3Sub(va1,va0);
       eB:=Vector3Sub(vb1,vb0);
       Axis:=Vector3NormEx(Vector3Cross(eA,eB));
       if Vector3Dot(Vector3Sub(eB,eA),Axis)<0.0 then begin
        Axis:=Vector3Neg(Axis);
       end;
       LocalVertex:=eA;
       SeparationFunctionMode:=sfmEDGES;
      end else begin
       // Edge-vertex
       va0:=Shapes[0].GetLocalFeatureSupportVertex(a0i);
       va1:=Shapes[0].GetLocalFeatureSupportVertex(a1i);
       vb0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(b0i),Transforms[1]),Transforms[0]);
       Axis:=Vector3Sub(va1,va0);
       Axis:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(vb0,va0)),Axis));
       LocalVertex:=va0;
       SeparationFunctionMode:=sfmEDGEA;
      end;
     end else begin
      if b0i<>b1i then begin
       // Vertex-edge
       va0:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(a0i),Transforms[0]),Transforms[1]);
       vb0:=Shapes[1].GetLocalFeatureSupportVertex(b0i);
       vb1:=Shapes[1].GetLocalFeatureSupportVertex(b1i);
       Axis:=Vector3Sub(vb1,vb0);
       Axis:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(va0,vb0)),Axis));
       LocalVertex:=vb0;
       SeparationFunctionMode:=sfmEDGEB;
      end else begin
       // Vertex-vertex
       Axis:=Vector3NormEx(Vector3Sub(Shapes[1].GetLocalFeatureSupportVertex(b0i),Shapes[0].GetLocalFeatureSupportVertex(a0i)));
       SeparationFunctionMode:=sfmVERTICES;
      end;
     end;
    end;
   end;
  end;

  // Successively resolve the deepest point to compute the TOI, loop is bounded by the number of vertices to be resolved.
  t1:=1.0;
  for TryIteration:=1 to 64 do begin

   // Compute deepest witness points at t1
   Transforms[0]:=Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,t1));
   Transforms[1]:=Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,t1));
   case SeparationFunctionMode of
    sfmVERTICES:begin
     WitnessPoints[0]:=Shapes[0].GetLocalFeatureSupportVertex(Shapes[0].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Axis,Transforms[0])));
     WitnessPoints[1]:=Shapes[1].GetLocalFeatureSupportVertex(Shapes[1].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Vector3Neg(Axis),Transforms[1])));
     s1:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Vector3TermMatrixMul(WitnessPoints[0],Transforms[0])));
    end;
    sfmEDGEA,sfmFACEA,sfmEDGES:begin
     WitnessPoints[1]:=Shapes[1].GetLocalFeatureSupportVertex(Shapes[1].GetLocalFeatureSupportIndex(Vector3Neg(Vector3TermMatrixMulTransposedBasis(Vector3TermMatrixMulBasis(Axis,Transforms[0]),Transforms[1]))));
     s1:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Transforms[0]),LocalVertex));
    end;
    sfmEDGEB,sfmFACEB:begin
     WitnessPoints[0]:=Shapes[0].GetLocalFeatureSupportVertex(Shapes[01].GetLocalFeatureSupportIndex(Vector3Neg(Vector3TermMatrixMulTransposedBasis(Vector3TermMatrixMulBasis(Axis,Transforms[1]),Transforms[0]))));
     s1:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[0],Transforms[0]),Transforms[1]),LocalVertex));
    end;
    else begin
     s1:=0.0;
     Assert(false);
    end;
   end;

   // Is the final configuration separated?
   if s1>(Target+Tolerance) then begin
    exit;
   end;

   // Has the separation reached tolerance?
   if s1>(Target-Tolerance) then begin
    // Advance the sweeps
    t0:=t1;
    if t0>=(1.0-EPSILON) then begin
     exit;
    end else begin
     break;
    end;
   end;

   // Compute the initial separation of the witness points
   Transforms[0]:=Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,t0));
   Transforms[1]:=Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,t0));
   case SeparationFunctionMode of
    sfmVERTICES:begin
     s0:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Vector3TermMatrixMul(WitnessPoints[0],Transforms[0])));
    end;
    sfmEDGEA,sfmFACEA,sfmEDGES:begin
     s0:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Transforms[0]),LocalVertex));
    end;
    sfmEDGEB,sfmFACEB:begin
     s0:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[0],Transforms[0]),Transforms[1]),LocalVertex));
    end;
    else begin
     s0:=0.0;
     Assert(false);
    end;
   end;

   // Check for initial overlap
   if s0<(Target-Tolerance) then begin
    // This might happen if the root finder runs out of iterations. Also more likely to happen with a poor separation function.
    Beta:=t0;
    result:=false;
    exit;
   end;

   // Check for touching, t0 should hold the TOI (could be 0.0)
   if s0<=(Target+Tolerance) then begin
    Beta:=t0;
    result:=true;
    exit;
   end;

   // Compute the 1D root of: f(x)-Target=0
   a0:=t0;
   a1:=t1;
   for RootIteration:=0 to 63 do begin

    if (RootIteration and 1)<>0 then begin
     t:=a1+((Target-s0)*((a1-a0)/(s1-s0)));
    end else begin
     t:=(a0+a1)*0.5;
    end;

    Transforms[0]:=Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,t));
    Transforms[1]:=Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,t));
    case SeparationFunctionMode of
     sfmVERTICES:begin
      s:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Vector3TermMatrixMul(WitnessPoints[0],Transforms[0])));
     end;
     sfmEDGEA,sfmFACEA,sfmEDGES:begin
      s:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Transforms[0]),LocalVertex));
     end;
     sfmEDGEB,sfmFACEB:begin
      s:=Vector3Dot(Axis,Vector3Sub(Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[0],Transforms[0]),Transforms[1]),LocalVertex));
     end;
     else begin
      s:=0.0;
      Assert(false);
     end;
    end;

    if abs(s-Target)<Tolerance then begin
     // t1 holds a tentative value for t0
     t1:=t;
     break;
    end else if s>Target then begin
     a0:=t;
     s0:=s;
    end else begin
     a1:=t;
     s1:=s;
    end;

   end;

  end;

 end;

end;