unit Response;

interface

uses Math, Math3D, AdvancedMath3D, Collision;

function GetCollidedPosition(position, eRadius, velocity: TVector4; var tri: TCollisionTriangles): TVector4;

implementation

var
  collision: TCollisionPacket;
  triangles: ^TCollisionTriangles;
  ellipseRadius: TVector4;

function CollideWithWorld(position, velocity: TVector4): TVector4;
var
  newSourcePoint, newDestinationPoint, newVelocityVector: TVector4;
  slidePlaneOrigin, slidePlaneNormal: TVector4;
  l: single;
  pos, v: TVector4;
  destinationPoint: TVector4;
begin
  if VecLength4(velocity) < EPSILON then
  begin
    result := position;
    exit;
  end;

  destinationPoint := VecAdd4(position, velocity);

  collision.velocity := velocity;
  collision.sourcePoint := position;
  collision.eRadius := ellipseRadius;
  collision.foundCollision := false;
  collision.stuck := false;
  collision.nearestDistance := -1;

  CheckCollision(collision, triangles^);

  if not collision.foundCollision then
  begin
    l := VecLength4(velocity);
    v := VecScalarMult4(velocity, (l-EPSILON) / l);

    collision.lastSafePosition := position;

    result := VecAdd4(position, v);
    exit;
  end else
  begin
    if collision.stuck then
    begin
      result := collision.lastSafePosition;
      exit;
    end;

    if collision.nearestDistance >= EPSILON then
    begin
      l := VecLength4(velocity);
      v := VecScalarMult4(velocity, (collision.nearestDistance-EPSILON) / l);
      newSourcePoint := VecAdd4(collision.sourcePoint, v);
    end else
      newSourcePoint := collision.sourcePoint;

    slidePlaneOrigin := collision.nearestPolygonIntersectionPoint;
    slidePlaneNormal := VecSubtract4(newSourcePoint, collision.nearestPolygonIntersectionPoint);

    l := IntersectRayPlane(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);

    newDestinationPoint := VecAdd4(destinationPoint, VecScalarMult4(slidePlaneNormal, l));

    newVelocityVector := VecSubtract4(newDestinationPoint, collision.nearestPolygonIntersectionPoint);

    collision.lastSafePosition := position;

    result := CollideWithWorld(newSourcePoint, newVelocityVector);
  end;
end;

function GetCollidedPosition(position, eRadius, velocity: TVector4; var tri: TCollisionTriangles): TVector4;
var
  scaledPosition, scaledVelocity: TVector4;
begin
  scaledPosition.x := position.x / eRadius.x;
  scaledPosition.y := position.y / eRadius.y;
  scaledPosition.z := position.z / eRadius.z;
  scaledPosition.w := 1;

  scaledVelocity.x := velocity.x / eRadius.x;
  scaledVelocity.y := velocity.y / eRadius.y;
  scaledVelocity.z := velocity.z / eRadius.z;
  scaledVelocity.w := 1;

  triangles := @tri;
  ellipseRadius := eRadius;

  result := CollideWithWorld(scaledPosition, scaledVelocity);

  result.x := result.x * eRadius.x;
  result.y := result.y * eRadius.y;
  result.z := result.z * eRadius.z;
  result.w := 1;
end;

end.
