PB_gjk.cpp

/******************************************************************************
File: PB_gjk.c
 
Description: this file contains the functions for collision detection using
             the gjk algorithm.
******************************************************************************/
#include "PB_gjk.h"
#include "PB_n8math.h" //vector math operations
#include "PB_physics_debug_draw.h" //debug draw purposes

//helper function. returns a point on the shape.
//note: DONT USE THIS function to get a random point on the Minkowski Difference
//      because it may return a zero vector.
//
//      (It's still good for getting a random point on a single shape, though.
//
//      For a random point on the Minkowski Difference Hull, use
//      MinsowskiSupport(CreateVector2(0.0f,1.0f), A, B) instead.
static Vector2 PointOnShape(RigidBody * body)
{
  Vector2 vector;
  switch (body->mShapeData.mShape)
  {
    case circleShape:
      vector = body->mCenterMass;
      vector.x += body->mShapeData.mRadius;
      break;
    //case rectangleShape: <-- rectangles replaced with polygons
      //fallthrough!
    case polygonShape:
      vector = body->mShapeData.mVerticies[0];
      vector = VectorRotation(vector, body->mRotation);
      break;
  }
  return vector;
}

/******************************************************************************
Function: DetectCollisionGJK
 
Description: This function uses the GJK algorithm to detect collision between
             two rigidbodies.
          body - a pointer to a rigidbody. The shape is an member of the body.
  Inputs: A - a pointer to a possibly collding rigidBody.
          B - a pointer to a possibly collding rigidBody.
  Outputs: bool - true if there was a collision. False otherwise.
******************************************************************************/
bool DetectCollisionGJK (RigidBody * A, RigidBody * B, Vector2 simplex [3])
{
  Vector2 simplexPoints [3]; // <-- need a triangle to enclose a point
  int numberPoints; // number of points in the simplex
  Vector2 direction; // the search direction
  Vector2 currentPoint; // a vector pointing from the ORIGIN to a point on the minkowski difference
  
  ///////////////////////
  //  1. Initial Work: // 
  ///////////////////////

  // Initialize current point ( random point in the Minkowski Difference )
  // edit 2/2/2012: I don't want this to be a zero vector, so instead
  //                let's make currentPoint in the direction (0,1).
  //                Yes - some bugs were fixed.
  currentPoint = MinkowskiSupport(CreateVector2(0.0f, 1.0f),A,B); 

  // add it to the simplex
  simplexPoints[0] = currentPoint;
  numberPoints = 1;
  
  // initialize direction to opposite the current point
  direction = ScalarMultiply(currentPoint, -1.0f);
  
  /////////////////
  // 2. The Loop // 
  /////////////////
  while (1)
  {
    //Get the farthest point in the minkowski difference in the direction
    currentPoint = MinkowskiSupport(direction, A, B);   

    // if the dot product of currentPoint with direction is < 0...
    if ( Dot( currentPoint, direction) < 0.0f)
    {
      // NO INTERSECTION
      return false;
    }
    
    // add the current point to the array.
    simplexPoints[numberPoints] = currentPoint;
    ++numberPoints;
    
    // See if a collection of points enclose the origin
    // AND change the search direction.
    if (DoSimplex(simplexPoints,&numberPoints,&direction))
    {
      int i;

      ///////////////////
      // INTERSECTION! //
      ///////////////////

      // Return the simplex so that the EPA (expanding polytype algorithm) 
      // can use it as a starting point.
      for (i = 0; i < 3; ++i)
      {
        //Origin is one of the points. (normal, Pen depth = ???) Fuck that shit.
        if (simplexPoints[i].x == 0.0f && simplexPoints[i].y == 0.0f) return false;

        simplex[i] = simplexPoints[i];
      }

      return true;
    }
  }
}

/******************************************************************************
Function: Support
 
Description: this function simply returns a vector2 pointing from the origin
             to a point on a shape furthest in a direction. This is found by 
             dotting each vector with the direction vector. Highest wins.
  Inputs: direction- the vector2 direction.
          body - a pointer to a rigidbody. The shape is an member of the body.
  Outputs: bool - true if the points enclose the origin, false otherwise
******************************************************************************/
Vector2 Support ( Vector2 direction, RigidBody * body)
{
  // variables for the circle:
  Vector2 furthestPoint;  //This is the return value of this support function.
  
  int i; // looping variable
  //The dot product of this vertex with the direction.
  float currentDotProduct;
  float highestDotProduct;
  Vector2 absolutePoint;  // point relative to origin
  
  switch (body->mShapeData.mShape)
  {
    case pointShape:
      furthestPoint = body->mCenterMass;
      break;
    case circleShape:
      // we think that the furthest point is the CM + radius * direction.normalized
      {
        Vector2 normalizedDirection;
        normalizedDirection = direction;
        Normalize(&normalizedDirection);
        furthestPoint = VectorAddition(body->mCenterMass, 
                                     ScalarMultiply(normalizedDirection,
                                                    body->mShapeData.mRadius));
      }
      break;
    //case rectangleShape: <-- rectangles replaced with polygons
      // fallthrough?
    case polygonShape:
      ////////////////////////////////// 
      //  for the first point index 0: // 
      ////////////////////////////////// 
      
      // get the absolutePoint. Rotate than add CM. There should be a function for this (used again shortly below)
      absolutePoint = VectorAddition(VectorRotation(body->mShapeData.mVerticies[0],body->mRotation), body->mCenterMass);

      // get the dot product of the direction with the point relative to the origin
      highestDotProduct = Dot(direction, absolutePoint);
      furthestPoint = absolutePoint;
      
      ////////////////////////////////////////////////////// 
      //  loop through each other point index 1 and above: // 
      ////////////////////////////////////////////////////// 
      for (i = 1; i < body->mShapeData.mVerticiesCount; ++i)
      {
        // get the absolute point
        absolutePoint = VectorAddition(VectorRotation(body->mShapeData.mVerticies[i],body->mRotation), body->mCenterMass);

        
        currentDotProduct = Dot(direction, absolutePoint);
        
        // if (the resulting dot product is higher than highestDotProduct)
        if ( currentDotProduct > highestDotProduct)
        {
          // new farthest point and accompanying dot product
          furthestPoint = absolutePoint;
          highestDotProduct = currentDotProduct;
        }
      }
      break;
  }
  
  // return the furthest Point
  return furthestPoint;
}

//This function returns the furthest point in a direction of the minkowski difference formed by A - B
Vector2 MinkowskiSupport (Vector2 direction, RigidBody * A, RigidBody * B)
{
  //declare
  Vector2 supportA;
  Vector2 supportB;
  Vector2 theOppositeDirection;

  //init
  theOppositeDirection = ScalarMultiply(direction, -1.0f);
  supportA = Support(direction, A);
  supportB = Support(theOppositeDirection, B);

  return VectorSubtraction(supportA, supportB);
}

/******************************************************************************
Function: DoSimplex
 
Description: Tests if some points enclose the origin. If they do, then there's
             an intersection. If not, this function changes the simplex and 
             search direction.
  Inputs: simplexPoints - an array of Vector2s in the Simplex.
          numberPoints - a pointer to an int of the number of poitns composing
                         the simplex.
          direction - a pointer to a vector2 direction to search next GJK loop
  Outputs: bool - true if the points enclose the origin, false otherwise
******************************************************************************/
bool DoSimplex(Vector2 simplexPoints[], int * numberPoints, Vector2 * direction)
{
  //  reference 1: http:// physics2d.com/content/gjk-algorithm
  //  reference 2: https:// mollyrocket.com/855.mp4
  switch (*numberPoints)
  {
    // by the time this function is called, numberPoints is at least 2. Thus case starts at 2
    
    case 2: // 1-simplex formed by B, A
    {
      // and by that I mean simplexPoints[0] is B, simplexPoints[1] is A
      // 2 points cannot enclose anything, so see where on the two points the origin lies
      // simplex is B,A
      Vector2 AB;
      Vector2 AO;
      AB = VectorSubtraction(simplexPoints[0],simplexPoints[1]);
      AO = Negate(simplexPoints[1]);
    
      if (Dot(AB,AO) > 0.0f)
      {
        //  search direction is perpendicular to AB, and points towards origin
        //  direction = (ABxAO)x AB
        *direction = CreateVector2FromVector3(Cross(CrossVector2 (AB,AO),
                                                    CreateVector3FromVector2(AB)));
      }    
      else
      {
        // simplex is A (drop B)
        simplexPoints[0] = simplexPoints[1];
        *numberPoints = 1;
        
        // search direction is AO
        *direction = Negate(simplexPoints[0]);
      }
      // only 2 points, return false
      return false; // returning, so no need to break;
    }
    case 3: // 2-simplex
    {
      //  the 2 simplex is made up of 3 points, forming the triangle.
      //  the origin is either inside the triangle (in which we're done)
      //  [OR] in a region (in which we should remove a point or two and update the direction)
      //  
      //  see: reference 1: http:// physics2d.com/content/gjk-algorithm
      //     our algorithm is the same, but for 2d (we stop at 2 simplex)
      
      ////////////////////////// 
      //  1. declare variables // 
      ////////////////////////// 
      Vector2 AC; // vector pointing from A to C
      Vector2 AB; // vector pointing from A to B
      Vector2 AO; // vector pointing from A to the Origin
      Vector2 edgeAC; // vector pointing outside the triangle, perpendicular to AC
      Vector2 edgeAB; // vector pointing outside the triangle, perpendicular to AB
      
      /////////////////////////////
      //  2. initialize variables // 
      /////////////////////////////
      AC = VectorSubtraction(simplexPoints[0], simplexPoints[2]);
      AB = VectorSubtraction(simplexPoints[1], simplexPoints[2]);
      AO = Negate(simplexPoints[2]);
      
      // edgeAC = (AB x AC )x AC
      edgeAC = CreateVector2FromVector3(Cross(CrossVector2(AB,AC),
                                              CreateVector3FromVector2(AC)));
      // edgeAB = (AC x AB )x AB
      edgeAB = CreateVector2FromVector3(Cross(CrossVector2(AC,AB),
                                              CreateVector3FromVector2(AB)));
      
      ////////////////////// /
      //  3. all the checks // 
      ////////////////////// /
      
      // is it outside AC?
      if ( Dot ( edgeAC, AO) > 0.0f)
      {
        // is it in the edge region and not the corner region?
        if ( Dot (AC, AO) > 0.0f)
        {
          // simplex is [A,C] direction = (AC x AO) x AC, or in 2d, direction = edgeAC  
          simplexPoints[1] = simplexPoints[0]; // simplex[1] is C
          simplexPoints[0] = simplexPoints[2]; // simplex[0] is A  
          *numberPoints = 2;
          *direction = edgeAC;
          return false;
        }
        else
        {  
          goto specialCase;
        }
      }
      else
      {
        if (Dot(edgeAB, AO) > 0.0f)
        {
          goto specialCase;
        }
        else
        {
          //  The Origin is Inside the Triangle. SUCCESS!
          return true;
        }
      }
      
      // if we haven't returned true yet, and didn't GOTO the special case, return false
      return false;
      
      // Special Case: the origin is outside A somewhere.
      specialCase:
      
      if (Dot(AB, AO) > 0.0f)
      {
        // simplex is [A,B] direction = (AB x AO) x AB, or in 2d, direction = edgeAB  
        simplexPoints[0] = simplexPoints[2]; // simplex[0] is A
        // simplexPoints[1] = simplexPoints[1]; // simplex[1] is B still, so line is commented out
        *numberPoints = 2;
        *direction = edgeAB;
        return false;
      }
      else
      {
        // simplex is [A], direction = AO
        simplexPoints[0] = simplexPoints[3];
        *numberPoints = 1;
        *direction = AO;
        return false;
      }
    }
  }
  // return false if we haven't already returned true
  return false;
}

PB_gjk.h

#pragma once

#include "PB_physics.h" //RigidBody
#include "PB_n8geom.h" //vector2

bool DetectCollisionGJK (RigidBody * A, RigidBody * B, Vector2 simplex [3]);

Vector2 Support ( Vector2 direction, RigidBody * body);

Vector2 MinkowskiSupport (Vector2 direction, RigidBody * A, RigidBody * B);

bool DoSimplex(Vector2 simplexPoints[], int * numberPoints, Vector2 * direction);