Conservative advancement

procedure CalculateVelocity(const cA:TVector3;const qA:TQuaternion;const cB:TVector3;const qB:TQuaternion;const DeltaTime:single;var LinearVelocity,AngularVelocity:TVector3);
var InverseDeltaTime,Angle:single;
    Axis:TVector3;
    qD,qS,qB0:TQuaternion;
begin
 InverseDeltaTime:=1.0/DeltaTime;
 LinearVelocity:=Vector3ScalarMul(Vector3Sub(cB,cA),InverseDeltaTime);
 if (abs(qA.x-qB.x)<EPSILON) and (abs(qA.y-qB.y)<EPSILON) and (abs(qA.z-qB.z)<EPSILON) and (abs(qA.w-qB.w)<EPSILON) then begin
  AngularVelocity:=Vector3Origin;
 end else begin
  qD:=QuaternionSub(qA,qB);
  qS:=QuaternionAdd(qA,qB);
  if QuaternionLengthSquared(qD)<QuaternionLengthSquared(qS) then begin
   qB0:=qD;
  end else begin
   qB0:=QuaternionNeg(qD);
  end;
  qD:=QuaternionMul(qB0,QuaternionInverse(qA));
  QuaternionToAxisAngle(qD,Axis,Angle);
  AngularVelocity:=Vector3ScalarMul(Vector3SafeNorm(Axis),Angle*InverseDeltaTime);
 end;
end;

// Conservative 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;
 function GetDistanceOfShapes(const ShapeA,ShapeB:TEnginePhysicsShape;const TransformA,TransformB:TMatrix4x4;out Normal:TVector3):single;
 var GJKState:TEnginePhysicsGJKState;
 begin
  ClearGJKState(GJKState);
  GJKState.Shapes[0]:=ShapeA;
  GJKState.Shapes[1]:=ShapeB;
  GJKState.Transforms[0]:=@TransformA;
  GJKState.Transforms[1]:=@TransformB;
  if not GJKRun(GJKState) then begin
   result:=0.0;
  end else begin
   result:=GJKState.Distance;
   Normal:=GJKState.v;
  end;
 end;
var Tries:longint;
    BoundingRadiusA,BoundingRadiusB,MaximumAngularProjectedVelocity,RelativeLinearVelocityLength,Lambda,LastLambda,Radius,
    ProjectedLinearVelocity,DistanceLambda,Distance:single;
    MeshShape:TEnginePhysicsShapeMesh;
    MeshTriangle:PEnginePhysicsMeshTriangle;
    ShapeTriangle:TEnginePhysicsShapeTriangle;
    Shapes:array[0..1] of TEnginePhysicsShape;
    RelativeLinearVelocity,Normal:TVector3;
    LinearVelocities,AngularVelocities:array[0..1] of TVector3;
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;

 CalculateVelocity(SweepA.c0,SweepA.q0,SweepA.c,SweepA.q,1.0,LinearVelocities[0],AngularVelocities[0]);
 CalculateVelocity(SweepB.c0,SweepB.q0,SweepB.c,SweepB.q,1.0,LinearVelocities[1],AngularVelocities[1]);

 BoundingRadiusA:=Shapes[0].AngularMotionDisc;
 BoundingRadiusB:=Shapes[1].AngularMotionDisc;

 MaximumAngularProjectedVelocity:=((Vector3Length(AngularVelocities[0])*BoundingRadiusA)+(Vector3Length(AngularVelocities[1])*BoundingRadiusB));

 RelativeLinearVelocity:=Vector3Sub(LinearVelocities[1],LinearVelocities[0]);

 RelativeLinearVelocityLength:=Vector3Length(RelativeLinearVelocity);

 if abs(RelativeLinearVelocityLength+MaximumAngularProjectedVelocity)<EPSILON then begin
  exit;
 end;

 if RelativeLinearVelocityLength<Max(EPSILON,Min(BoundingRadiusA,BoundingRadiusB)*0.25) then begin
  exit;
 end;

 Lambda:=0.0;

 LastLambda:=Lambda;

 Radius:=1e-4;

 Distance:=GetDistanceOfShapes(Shapes[0],
                               Shapes[1],
                               Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,0.0)),
                               Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,0.0)),
                               Normal);
 if Distance<EPSILON then begin
  exit;
 end;

 ProjectedLinearVelocity:=Vector3Dot(RelativeLinearVelocity,Normal);
 if (ProjectedLinearVelocity+MaximumAngularProjectedVelocity)<EPSILON then begin
  exit;
 end;

 Tries:=TimeOfImpactMaximumIterations;
 while Radius<Distance do begin

  ProjectedLinearVelocity:=Vector3Dot(RelativeLinearVelocity,Normal);

  if (ProjectedLinearVelocity+MaximumAngularProjectedVelocity)<EPSILON then begin
   break;
  end;

  DistanceLambda:=Distance/(ProjectedLinearVelocity+MaximumAngularProjectedVelocity);

  Lambda:=Lambda+DistanceLambda;
  if ((Lambda<0.0) or (Lambda>1.0)) or (IsSameValue(Lambda,LastLambda) or (Lambda<LastLambda)) then begin
   break;
  end;

  LastLambda:=Lambda;

  Distance:=GetDistanceOfShapes(Shapes[0],
                                Shapes[1],
                                Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,Lambda)),
                                Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,Lambda)),Normal);
  if Distance<EPSILON then begin
   Beta:=Lambda;
   result:=true;
   exit;
  end;

  dec(Tries);
  if Tries<=0 then begin
   exit;
  end;

 end;

 if (Lambda>=0.0) and (Lambda<=1.0) then begin
  Distance:=GetDistanceOfShapes(Shapes[0],
                                Shapes[1],
                                Matrix4x4TermMul(Shapes[0].LocalTransform,SweepTransform(SweepA,Lambda)),
                                Matrix4x4TermMul(Shapes[1].LocalTransform,SweepTransform(SweepB,Lambda)),
                                Normal);
  if Distance<EPSILON then begin
   Beta:=Lambda;
   result:=true;
  end;
 end;

end;