unit Collision;

interface

uses Math, Math3D, AdvancedMath3D;

type
  TCollisionTriangles = array of TTriangle4;
  TCollisionPacket = record
    velocity            : TVector4;
    sourcePoint         : TVector4;
    eRadius             : TVector4;
    lastSafePosition    : TVector4;
    stuck               : boolean;
    foundCollision      : boolean;
    nearestDistance     : double;
    nearestIntersectionPoint        : TVector4;
    nearestPolygonIntersectionPoint : TVector4;
  end;

procedure CheckCollision(var colPackage: TCollisionPacket; var triangles: TCollisionTriangles);

implementation

procedure CheckCollision(var colPackage: TCollisionPacket; var triangles: TCollisionTriangles);
var
  p1, p2, p3: TVector4;
  pNormal: TVector4;
  pOrigin: TVector4;
  v1, v2: TVector4;
  source: TVector4;
  eRadius: TVector4;
  velocity: TVector4;
  normalizedVelocity: TVector4;
  sIPoint: TVector4;
  pIPoint: TVector4;
  polyIPoint: TVector4;
  distanceToTravel: double;
  distToPlaneIntersection: double;
  distToEllipsoidIntersection: double;
  pClass: cardinal;
  i: integer;
begin
  // get data from package
  source := colPackage.sourcePoint;
  eRadius := colPackage.eRadius;
  velocity := colPackage.velocity;

  // calculate normalized velocity
  normalizedVelocity := velocity;
  VecNormalize4(normalizedVelocity);

  // how long is our velocity
  distanceToTravel := VecLength4(velocity);

  writeln('number of tris: ', Length(triangles));
  // let's go through all of the triangles
  for i := 0 to Length(triangles) - 1 do
  begin
    // scale the triangle in question to ellipsoid space
    p3.x := triangles[i][0].x / eRadius.x;
    p3.y := triangles[i][0].y / eRadius.y;
    p3.z := triangles[i][0].z / eRadius.z;
    p3.w := 1;

    p2.x := triangles[i][1].x / eRadius.x;
    p2.y := triangles[i][1].y / eRadius.y;
    p2.z := triangles[i][1].z / eRadius.z;
    p2.w := 1;

    p1.x := triangles[i][2].x / eRadius.x;
    p1.y := triangles[i][2].y / eRadius.y;
    p1.z := triangles[i][2].z / eRadius.z;
    p1.w := 1;

    // make plane data for triangle
    pOrigin := p1;
    v1 := VecSubtract4(p2, p1);
    v2 := VecSubtract4(p3, p1);

    // calculate normal of plane
    pNormal := VecCross4(v1, v2);
    VecNormalize4(pNormal);

    // calculate sphere intersection point
    sIPoint := VecSubtract4(source, pNormal);

    // classify point to determine if ellipsoid span the plane
    pClass := ClassifyPoint(sIPoint, pOrigin, pNormal);

    // find the plane intersection point
    if pClass = PLANE_BACKSIDE then // plane is embedded in ellipsoid
    begin
      // find plane intersection point by shooting a ray from the 
			// sphere intersection point along the planes normal.
      distToPlaneIntersection := IntersectRayPlane(sIPoint, pNormal, pOrigin, pNormal);

      // calculate plane intersection point
      pIPoint := VecAdd4(sIPoint, VecScalarMult4(pNormal, distToPlaneIntersection));
    end else
    begin
      // shoot ray along the velocity vector
      distToPlaneIntersection := IntersectRayPlane(sIPoint, normalizedVelocity, pOrigin, pNormal);

      // calculate plane intersection point
      pIPoint := VecAdd4(sIPoint, VecScalarMult4(normalizedVelocity, distToPlaneIntersection));
    end;

    // find polygon intersection point, by default we assume its equal to the
 		// plane intersection point
    polyIPoint := pIPoint;
    distToEllipsoidIntersection := distToPlaneIntersection;

    if not CheckPointInTriangle(pIPoint, p1, p2, p3) then // if not in triangle
    begin
      polyIPoint := ClosestPointOnTriangle(p1, p2, p3, pIPoint);

      distToEllipsoidIntersection := IntersectRaySphere(polyIPoint, VecNegate4(normalizedVelocity), source, 1);

      if distToEllipsoidIntersection > 0 then
        // calculate true sphere intersection point
        sIPoint := VecAdd4(polyIPoint, VecScalarMult4(normalizedVelocity, -distToEllipsoidIntersection));
    end;

    // here we do the error checking to see if we got ourself stuck last frame
    if CheckPointInSphere(polyIPoint, source, 1) then
      colPackage.stuck := true;

    // Ok, now we might update the collision data if we hit something
    if (distToEllipsoidIntersection > 0) and (distToEllipsoidIntersection <= distanceToTravel) then
    begin
      writeln('COLLISION DETECTED');
      writeln('triangle: ');
      writeln(triangles[i][0].x:2:2, ' ', triangles[i][0].y:2:2, ' ', triangles[i][0].z:2:2);
      writeln(triangles[i][1].x:2:2, ' ', triangles[i][1].y:2:2, ' ', triangles[i][1].z:2:2);
      writeln(triangles[i][2].x:2:2, ' ', triangles[i][2].y:2:2, ' ', triangles[i][2].z:2:2);
      if (not colPackage.foundCollision) or (distToEllipsoidIntersection < colPackage.nearestDistance) then
      begin
        // if we are hit we have a closest hit so far. We save the information
        colPackage.nearestDistance := distToEllipsoidIntersection;
        colPackage.nearestIntersectionPoint := sIPoint;
        colPackage.nearestPolygonIntersectionPoint := polyIPoint;
        colPackage.foundCollision := true;
      end;
    end;

  end;
end;

end.
