Home > Code Samples > Minkowski Portal Refinement

Minkowski Portal Refinement

This is the implementation I made for MPR.

I did not modify this in any way before posting, except to remove commented out code blocks. Anything you see here directly corresponds to my own personal coding style and standards.

/******************************************************************************
  Author: Trevor Sundberg
    Date: 9/21/2008

 Purpose:
          This file contains all the elements for using the MPR algorithm to
          detect collisions between convex polyhedra, as well as find contact
          information.
          All content © 2009 DigiPen (USA) Corporation, all rights reserved.
******************************************************************************/


// Includes
#include "mpr.h"
#include "matrix4.h"
#include "float.h"
#include "math.h"

// Using directives
using namespace Wallaby;
using namespace Collision;

// Helper defines
#define SECOND_PHASE_ACCURACY   10
#define DEEP_PENETRATION        0.3F
#define NORMALIZATION_EPSILON   0.000000001F
#define DIPSLACE_EPSILON        0.001F
#define INIT_RANDOM_SEED        2457891
#define RANDOM_DISPLACE_RATIO   0.15F
#define INFINITE_LOOP           for(;;)
#define DEBUG_NO_BREAK          ((unsigned) -1)
#define PI                      3.141592653589793238462643F
#define V1                      0
#define V2                      1
#define V3                      2

// Debug defines
//#define MPR_DEBUG_INFO
//#define MPR_ERRORS


// Pre-defined vectors
static const Vector3 X(1.0F, 0.0F, 0.0F);
static const Vector3 Y(0.0F, 1.0F, 0.0F);
static const Vector3 Z(0.0F, 0.0F, 1.0F);
static const Vector3 ORIGIN(0.0F, 0.0F, 0.0F);

// If debug is enabled, define a check debug macro that helps debug drawing
#ifdef MPR_DEBUG_INFO
  #define CheckDebug(stage) if (DebugBreak(stage)) return false;
  #define DebugComplete() DebugDraw(STAGE_CONTACT_COMPLETED)
#else
  #define CheckDebug(stage)
  #define DebugComplete()
#endif

#if defined(MPR_DEBUG_INFO) || defined(MPR_ERRORS)
  // Prevent min/max from being #defined
  #define NOMINMAX
  // Prevent infrequently used stuff
  #define WIN32_LEAN_AND_MEAN
  #include "windows.h"
  #include "../architecture/wallaby.h"
#endif

// Constructor
MPR::MPR()
{
  // By default, don't breakpoint
  debug_breakpoint_ = DEBUG_NO_BREAK;
}

// Tests to see if two convex shapes are intersecting
bool MPR::TestIntersection(const ShapePair& shape_pair,
                           CollisionInfo&   collision_info_out,
                           const Vector3*   collision_direction)
{
  /********************************\
   * Phase #1: Portal Discovery
  \********************************/


  // Initialize for collision detection
  Initialize(&shape_pair, collision_info_out, collision_direction);

  // Find the geometric center that we'll use as a starting point
  FindGeometricCenter();

  // Find the origin ray from an interior point in the CSO
  FindOriginRay();

  // Finds the starting portal that we'll use, THIS MAY FAIL IN SPECIAL CASES!
  FindStartingPortal();

  // This do/while loop is for MTD correct
  do
  {
    // Initialize an iteration counter for this phase to avoid failure states
    InitIterationCounter();

    // Checks for the current debug breakpoints
    CheckDebug(STAGE_PORTAL_DISCOVERY);

    // Loop until we find a valid portal that the origin ray passes through (or we timeout)
    while (!CheckOriginRayIntersectPortal())
    {
      // Check if we timed out
      if (IterationTimedOut(MAX_PORTAL_DISCOVERY_STEPS))
      {
        // Show an error and return no collision
        AssertError("Max iterations reached in Portal Discovery!");
        return false;
      }

      // Count up an iteration
      CountIteration();

      // Choose the new starting portal
      ChooseNewStartingPortal();

      // Checks for the current debug breakpoints
      CheckDebug(STAGE_PORTAL_DISCOVERY);
    }


    /********************************\
    * Phase #2: Portal Refinement
    \********************************/


    // Initialize an iteration counter for this phase to avoid failure states
    InitIterationCounter();

    // Checks for the current debug breakpoints
    CheckDebug(STAGE_ENTERING_STAGE_PORTAL_REFINEMENT);

    // Loop until we either return with a hit or miss
    INFINITE_LOOP
    {
      // Check if we timed out
      if (IterationTimedOut(MAX_PORTAL_REFINEMENT_STEPS))
      {
        // Show an error and return no collision
        AssertError("Max iterations reached in Portal Refinement!");
        return false;
      }

      // Count up an iteration
      CountIteration();

      // If the origin is inside the portal, there's a collision!
      // Break out of the loop if this happens, since we'll continue below
      // Also, this will compute the normal of the portal
      if (CheckOriginInsidePortal())
        break;

      // Find the new supporting point, since we didn't find that it was colliding yet
      FindSupportFromPortalOutNormal();

      // If the origin is outside the supporting plane, no collision
      if (CheckOriginOutsideSupportPlane())
        return false;

      // If the support plane and portal are close enough, terminate
      if (CheckPortalCloseToSupportPlane())
        return false;

      // Since we got here, we didn't find a portal that gave us any information
      // The origin is within the supporting plane, but outside the portal
      // Find a new portal
      ChooseNewPortal();

      // Checks for the current debug breakpoints
      CheckDebug(STAGE_PORTAL_REFINEMENT);
    }


    /********************************\
    * Phase #3: Contact Discovery
    \********************************/


    // Initialize an iteration counter for this phase to avoid failure states
    InitIterationCounter();

    // Checks for the current debug breakpoints
    CheckDebug(STAGE_ENTERING_STAGE_CONTACT_DISCOVERY);

    // Loop and choose a new portal, but stop if the new portal's normal
    // is by some epsilon "close enough" to the last normal
    do
    {
      // Check if we timed out
      if (IterationTimedOut(MAX_CONTACT_DISCOVERY_STEPS))
      {
        // Show an error and return no collision
        AssertError("Max iterations reached in Contact Discovery!");
        return false;
      }

      // Count up an iteration
      CountIteration();

      // Save the last portal normal
      SaveLastPortalNormal();

      // Find the new supporting point, since we didn't find that it was colliding yet
      FindSupportFromPortalOutNormal();

      // Since we got here, we didn't find a portal that gave us any information
      // The origin is within the supporting plane, but outside the portal
      // Find a new portal
      ChooseNewPortal();

      // Compute the current portals normal
      ComputeCurrentPortalNormal();

      // Checks for the current debug breakpoints
      CheckDebug(STAGE_CONTACT_DISCOVERY);
    }
    // Loop until we find a normal that is close enough
    while (!CheckCurrentNormalCloseEnough());
  }
  // Loop until we no longer need to correct the center for the MTD
  while (CorrectCenterForMTD());


  // The last step is to compute the contact data: the collision normal,
  // penetration depth (MTD), and two applicable points of contact for each shape
  ComputeContactData();

  // The algorithm has now completed successfully, inform debug
  DebugComplete();

  // Since we made it here, there was a collision
  return true;
}

// Set the debug breaking point
void MPR::SetDebugBreakpoint(unsigned breakpoint)
{
  debug_breakpoint_ = breakpoint;
}

// Initialize the MPR given the passed in information (basically just stores it)
void MPR::Initialize(const ShapePair* pair, CollisionInfo& CI, const Vector3* direction)
{
  // Store away pointers
  pair_           = pair;
  collision_info_ = &CI;

  // If we were given a direction to check in
  if (direction != 0)
  {
    // Normalize the direction and set the direction vector
    direction->Normalize(collision_direction_);

    // Set the flag that we're using a direction vector
    using_direction_ = true;
  }
  else
  {
    // Set the flag that we're not using a direction vector
    using_direction_ = false;
  }

  // The MTD has not yet been corrected
  mtd_corrected_ = false;

  // Assume for right now that there is no collision
  collision_info_->was_collision = false;

  // Initialize the debug iteration counter
  debug_iteration_ = DEBUG_NO_BREAK;
}

// Find the the geometric centers of the objects, then translate them
// to a single center point in the Minkowski Difference space
void MPR::FindGeometricCenter()
{
  // Perform the Minkowski Difference on the two points, to get an interior point on the CSO
  // (B - A) centers
  V0_  = pair_->B_center;
  V0_ -= pair_->A_center;

  // Check if the difference point is the origin, if so offset the point
  if (V0_ == ORIGIN)
  {
    // Generate a random vector
    GenerateRandomVector(V0_);

    // Scale the vector down
    V0_ *= DIPSLACE_EPSILON;
  }
}

// Find the starting origin ray from the geometric centers to the origin
void MPR::FindOriginRay()
{
  // The 'origin ray' is simply just the negative ray to the center
  V0_.Opposite(origin_ray_);
}

// Finds the starting candidate portal, not necessarily a portal that the origin ray goes through
// This can be found through several methods, but basically needs to find 3 non-collinear points
void MPR::FindStartingPortal()
{
  // Find the support point in the direction of the origin ray
  Support(origin_ray_, current_portal_.point[V1], shape_A_points_.point[V1], shape_B_points_.point[V1]);

  // Find the support point that is perpendicular to the plane formed by
  // the origin ray and the ray to the first supporting point
  // NOTE: This could be problematic if the support point was exactly in the same
  //       direction as the ray, thus resulting in a bad cross product
  Vector3 new_direction;
  current_portal_.point[V1].CrossProduct(V0_, new_direction);

  // Initialize an iteration counter for this phase to avoid failure states
  InitIterationCounter();

  // Seed random since we might use it below
  SeedRandom(INIT_RANDOM_SEED);

  // Loop until we find a valid first point (this might involve moving the center)
  // This will only loop if the support point is in the same direction as the origin ray
  while (new_direction == ORIGIN)
  {
    // Generate a random direction vector
    Vector3 random_dir;
    GenerateRandomVector(random_dir);

    // Since we're going to move the center, and we don't want it to leave the shape...
    // we know that supporting points are the exterior most points, thus we'll find a
    // random supporting point, and move by a little bit in that direction
    // Get the supporting point in the random direction
    Vector3 rand_support;
    Support(random_dir, rand_support, shape_A_points_.point[V1], shape_B_points_.point[V1]);

    // Use the random support and direction to displace the center
    rand_support -= V0_;
    rand_support *= RANDOM_DISPLACE_RATIO;
    random_dir   *= DIPSLACE_EPSILON;
    V0_ += rand_support;
    V0_ += random_dir;

    // Now recompute the origin ray
    FindOriginRay();

    // Find the support point in the direction of the origin ray
    Support(origin_ray_, current_portal_.point[V1], shape_A_points_.point[V1], shape_B_points_.point[V1]);

    // Find the perpendicular normal to the plane
    current_portal_.point[V1].CrossProduct(V0_, new_direction);

    // Count up an iteration
    CountIteration();

    // Check if we timed out
    if (IterationTimedOut(MAX_RANDOM_CENTER_MOVE_STEPS))
      // Show an error and return no collision
      AssertError("Max iterations reached in Find Starting Portal!");
  }

  // Now get the second supporting point in the new direction
  Support(new_direction, current_portal_.point[V2], shape_A_points_.point[V2], shape_B_points_.point[V2]);

  // Find the support that is perpendicular to the plane containing the
  // interior point and the first two supporting points
  Vector3 temp1, temp2;
  current_portal_.point[V2].Subtract(V0_, temp1);
  current_portal_.point[V1].Subtract(V0_, temp2);
  temp1.CrossProduct(temp2, new_direction);
  Support(new_direction, current_portal_.point[V3], shape_A_points_.point[V3], shape_B_points_.point[V3]);
}

// Checks if the origin ray intersects the portal by checking if the origin is inside of the
// three planes made up by the portal points and the start point, if it does: it returns true,
// if not: it returns false and fills out the plane that that the origin was outside of
// By the end of this method, the normal member will be filled in
bool MPR::CheckOriginRayIntersectPortal()
{
  // NOTE: We need to check that all 3 normal directions are outward facing (point ordering!)

  // Make a plane between (V0, V1, V2)
  temp_plane_.point[0] = V0_;
  temp_plane_.point[1] = current_portal_.point[V1];
  temp_plane_.point[2] = current_portal_.point[V2];

  // Grab the actual shape points and store them away
  temp_shape_A_points_.point[1] = shape_A_points_.point[V1];
  temp_shape_B_points_.point[1] = shape_B_points_.point[V1];
  temp_shape_A_points_.point[2] = shape_A_points_.point[V2];
  temp_shape_B_points_.point[2] = shape_B_points_.point[V2];

  // Test if the origin is outside the plane
  if (PlaneOriginOutside(temp_plane_))
    return false;

  // Make a plane between (V0, V2, V3)
  temp_plane_.point[0] = V0_;
  temp_plane_.point[1] = current_portal_.point[V2];
  temp_plane_.point[2] = current_portal_.point[V3];

  // Grab the actual shape points and store them away
  temp_shape_A_points_.point[1] = shape_A_points_.point[V2];
  temp_shape_B_points_.point[1] = shape_B_points_.point[V2];
  temp_shape_A_points_.point[2] = shape_A_points_.point[V3];
  temp_shape_B_points_.point[2] = shape_B_points_.point[V3];

  // Test if the origin is outside the plane
  if (PlaneOriginOutside(temp_plane_))
    return false;

  // Make a plane between (V0, V3, V1)
  temp_plane_.point[0] = V0_;
  temp_plane_.point[1] = current_portal_.point[V3];
  temp_plane_.point[2] = current_portal_.point[V1];

  // Grab the actual shape points and store them away
  temp_shape_A_points_.point[1] = shape_A_points_.point[V3];
  temp_shape_B_points_.point[1] = shape_B_points_.point[V3];
  temp_shape_A_points_.point[2] = shape_A_points_.point[V1];
  temp_shape_B_points_.point[2] = shape_B_points_.point[V1];

  // Test if the origin is outside the plane
  if (PlaneOriginOutside(temp_plane_))
    return false;

  // Otherwise, it's inside so return true
  return true;
}

// Chooses a new starting portal since the last one didn't work; the input is the point plane made up by
// the starting point and two other old points, and it returns out a new portal as current_portal
// The first point will always be V0, keep this in mind!
void MPR::ChooseNewStartingPortal()
{
  // Replace V0 with the supporting point in the normal direction
  Support(temp_plane_.normal, temp_plane_.point[0], temp_shape_A_points_.point[0], temp_shape_B_points_.point[0]);

  // Now swap the order of the points to make all planes face outward
  Vector3 temp = temp_plane_.point[0];
  temp_plane_.point[0] = temp_plane_.point[1];
  temp_plane_.point[1] = temp;

  // Swap the order for the shape points too (shape A)
  temp = temp_shape_A_points_.point[0];
  temp_shape_A_points_.point[0] = temp_shape_A_points_.point[1];
  temp_shape_A_points_.point[1] = temp;

  // Swap the order for the shape points too (shape B)
  temp = temp_shape_B_points_.point[0];
  temp_shape_B_points_.point[0] = temp_shape_B_points_.point[1];
  temp_shape_B_points_.point[1] = temp;

  // The current portal is now the plane portal, but the portal normal is not valid!
  current_portal_ = temp_plane_;

  // Copy over the shape points also
  shape_A_points_ = temp_shape_A_points_;
  shape_B_points_ = temp_shape_B_points_;
}

// Since we now have a valid portal...
// Checks to see if the origin ray intersects with the current portal: it does so by checking
// if the origin is inside the portal plane or outside the portal plane (inside means hit!)
// This method needs to fill out the normal field for the portal
bool MPR::CheckOriginInsidePortal()
{
  // Return if it's inside the portal
  return PlaneOriginOutsideOrOn(current_portal_);
}

// Finds the supporting point in the direction of the portal's outward normal,
// only runs if the origin was found to be outside the current portal
void MPR::FindSupportFromPortalOutNormal()
{
  // Since the normal always points inside here, we'll use the negative normal
  Vector3 neg_normal;
  current_portal_.normal.Opposite(neg_normal);

  // Find the supporting point in the portal's normal direction
  Support(neg_normal, new_support_, new_support_A_, new_support_B_);
}

// Checks if the origin is outside the supporting plane, which means that the
// origin is not contained within the CSO and the shapes are not intersecting
bool MPR::CheckOriginOutsideSupportPlane()
{
  // Use the dot product between the point and the normal to compare if it's inside
  // The normal points inward, so we want to check if the origin is on the negative side
  return new_support_.DotProduct(current_portal_.normal) > 0.0F;
}

// Checks if the supporting plane is very close to the portal, specifically used for curved shapes
// The check works by checking the distance between the parallel portal and supporting plane
bool MPR::CheckPortalCloseToSupportPlane()
{
  // False for now, only using discrete shapes!
  return false;
}

// Generates a new portal by finding which face the origin ray will go through
void MPR::ChooseNewPortal()
{
  // NOTE: At first I didn't understand why we used points (V4, V0, V[1-3])
  //       Since we're trying to find what triangle to use as the new portal, why wouldn't
  //       we just use the points made by some combo of V[1-3] and V4... why V0?
  //       Well, without a ray intersect triangle test, we need to use the V0 point to make
  //       planes that will give us the inside information


  // Create a plane that we'll use (the only thing that changes is the last point)
  PointPlane plane;
  plane.point[0] = V0_;
  plane.point[2] = new_support_;

  // Bit field to represent which planes it was inside
  unsigned bit_field = 0x00;
  unsigned result = 0;

  // Add the point to the plane
  plane.point[1] = current_portal_.point[V1];
  // Check if the origin is outside or on the plane, and put the result in the bit field
  result     = (int) PlaneOriginOutsideOrOn(plane);
  //           (Positive     )   (Negative      )
  bit_field |= (result * 0x01) + (!result * 0x02);

  // Add the point to the plane
  plane.point[1] = current_portal_.point[V2];
  // Check if the origin is outside or on the plane, and put the result in the bit field
  result     = (int) PlaneOriginOutsideOrOn(plane);
  //           (Positive     )   (Negative      )
  bit_field |= (result * 0x04) + (!result * 0x08);

  // Add the point to the plane
  plane.point[1] = current_portal_.point[V3];
  // Check if the origin is outside or on the plane, and put the result in the bit field
  result     = (int) PlaneOriginOutsideOrOn(plane);
  //           (Positive     )   (Negative      )
  bit_field |= (result * 0x10) + (!result * 0x20);


  // Based on the cases, we choose the new portal
  switch (bit_field)
  {
  // Plane #1 positive and #2 negative, #3 is anything (and kill point #3)
  case (0x01 | 0x08) | 0x10:
  case (0x01 | 0x08) | 0x20:
      // Get rid of vertex 3
      current_portal_.point[V3] = new_support_;
      shape_A_points_.point[V3] = new_support_A_;
      shape_B_points_.point[V3] = new_support_B_;
    break;

  // Plane #2 positive and #3 negative, #1 is anything (and kill point #1)
  case (0x04 | 0x20) | 0x01:
  case (0x04 | 0x20) | 0x02:
    // Get rid of vertex 1
    current_portal_.point[V1] = new_support_;
    shape_A_points_.point[V1] = new_support_A_;
    shape_B_points_.point[V1] = new_support_B_;
    break;

  // Plane #3 positive and #1 negative, #2 is anything (and kill point #2)
  case (0x10 | 0x02) | 0x04:
  case (0x10 | 0x02) | 0x08:
    // Get rid of vertex 2
    current_portal_.point[V2] = new_support_;
    shape_A_points_.point[V2] = new_support_A_;
    shape_B_points_.point[V2] = new_support_B_;
    break;

  /*
  // Special case where it lies on all planes, since it lies in the direct center of
  // the portal. This is very very rare, but... we'll just choose any one of the sides
  case (0x10 | 0x04 | 0x01):
    // Note: I was wrong, this case can also be bad!
    break;
  */


  // Something went terribly wrong! This case should never happen!
  default:
    AssertError("Error occurred in ChooseNewPortal(), an invalid case!");
    break;
  }
}

// Saves the last portal normal
void MPR::SaveLastPortalNormal()
{
  last_normal_ = current_portal_.normal;
}

// Computes the current portal's normal, only explicitly used in finding contact data
void MPR::ComputeCurrentPortalNormal()
{
  // Create the first and second side
  Vector3 side1, side2;
  current_portal_.point[0].Subtract(current_portal_.point[2], side1);
  current_portal_.point[0].Subtract(current_portal_.point[1], side2);

  // The cross product of the two sides is the normal
  side2.CrossProduct(side1, current_portal_.normal);
}

// Checks the condition that the previous normal is close enough to the current normal
bool MPR::CheckCurrentNormalCloseEnough()
{
  // Unfortunately, we need to normalize the normals here
  last_normal_.Normalize();
  current_portal_.normal.Normalize();

  // Compute the dot product
  float dot = last_normal_.DotProduct(current_portal_.normal);

  // Now check if the dot product between them shows that they are close enough
  return dot > (1.0F - CLOSENESS_NORMAL_EPSILON);
}

// Repositions the center so we can get the correct Minimum Translational Distance
// Returns true if we should restart and correct the center for the MTD
bool MPR::CorrectCenterForMTD()
{
  // As long as we haven't corrected the center to account for the MTD...
  if (mtd_corrected_ == false)
  {
    // If we're using a direction vector, correct V0_ by changing the collision direction
    if (using_direction_)
    {
      // Set V0_ to be at the collision direction spot
      V0_ = collision_direction_;

      // Multiply V0_ by an epsilon to keep it small
      V0_ *= DIPSLACE_EPSILON;
    }
    else
    {
      // Compute the intersection between the origin ray and the current portal
      Vector3 intersection_point;
      IntersectLineTriangle(V0_, ORIGIN, current_portal_, intersection_point);

      // Check if the penetration distance is dep (relative to the object's own size)
      if (intersection_point.Length() > DEEP_PENETRATION)
      {
        // Stores the support point
        Vector3 support, temp;

        // Initialize a minimum distance value
        float minimum_dist = FLT_MAX;

        // Loop through all the stacks
        for (int g = 0; g <= SECOND_PHASE_ACCURACY; ++g)
        {
          // Loop through all the slices
          for (int t = 0; t <= SECOND_PHASE_ACCURACY; ++t)
          {
            // Get a normalized value for the current stack/slice
            float t_norm = (float)t / (float)(SECOND_PHASE_ACCURACY);
            float g_norm = (float)g / (float)(SECOND_PHASE_ACCURACY);

            // Create the direction vector from sphereical coordiantes
            Vector3 dir(sin(t_norm * PI * 2.0F) * sin(g_norm * PI),
                        cos(t_norm * PI * 2.0F) * sin(g_norm * PI),
                        cos(g_norm * PI));

            // Support in the direction
            Support(dir, support, temp, temp);

            // Compute the distance that the support lies along the direction
            float dist = support.DotProduct(dir);

            // Check if the distance is less than
            if (dist < minimum_dist)
            {
              // Store the new minimum distance
              minimum_dist = dist;

              // Store the opposite direction into V0_
              dir.Opposite(V0_);
            }
          }
        }
       
        // Multiply V0_ by an epsilon to keep it small
        V0_ *= DIPSLACE_EPSILON;
      }
      else
      {
        // The MTD didn't have to be corrected, just continue on
        return false;
      }
    }

    // Find the origin ray from an interior point in the CSO
    // This needs to be called since we just found ourselves a new center
    FindOriginRay();

    // The MTD will now be corrected
    mtd_corrected_ = true;
    return true;
  }

  // Since we got here, the MTD no longer needs to be corrected
  return false;
}

// Returns if the MTD has been corrected
bool MPR::IsMTDCorrected()
{
  return mtd_corrected_;
}

// Computes the contact data at the end
void MPR::ComputeContactData()
{
  // This will hold the barycentric coordinates of the final contact point
  Vector3 barycentric_coords;


  // If we're not using a direction
  if (using_direction_ == false)
  {
    // Now find the barycentric coordinates of the projected point in the triangle/portal
    // Note, we don't actually need to project the point, since barycentric coordinates
    // can only express positions along the plane, and thus any movement in the normal
    // direction of the plane is ignored
    ComputeBarycentricCoords(current_portal_, ORIGIN, barycentric_coords);

    // Check to see if the point is outside the triangle, if so we'll have to approximate
    // the MTD by using the intersection of the origin ray with the current portal
    if (barycentric_coords.x < 0.0F ||
        barycentric_coords.y < 0.0F ||
        barycentric_coords.z < 0.0F)
    {
      // Compute the intersection between the origin ray and the current portal
      Vector3 intersection_point;
      IntersectLineTriangle(V0_, ORIGIN, current_portal_, intersection_point);

      // Now compute the barycentric coordinates of the intersection point
      ComputeBarycentricCoords(current_portal_, intersection_point, barycentric_coords);
    }

    // Now use the shape points to determine contact points on each
    // Shape A:
    shape_A_points_.point[0] *= barycentric_coords.x;
    shape_A_points_.point[1] *= barycentric_coords.y;
    shape_A_points_.point[2] *= barycentric_coords.z;
    // Shape B:
    shape_B_points_.point[0] *= barycentric_coords.x;
    shape_B_points_.point[1] *= barycentric_coords.y;
    shape_B_points_.point[2] *= barycentric_coords.z;


    // The contact points are the sum of each barycentric weighted point
    // Shape A:
    contact_A_  = shape_A_points_.point[0];
    contact_A_ += shape_A_points_.point[1];
    contact_A_ += shape_A_points_.point[2];
    // Shape B:
    contact_B_  = shape_B_points_.point[0];
    contact_B_ += shape_B_points_.point[1];
    contact_B_ += shape_B_points_.point[2];

    // The contact for the shapes will be the average for now
    contact_A_.Add(contact_B_, collision_info_->point_of_contact);
    collision_info_->point_of_contact /= 2.0F;
  }
  else
  {
    // Compute the intersection between the origin ray and the current portal
    Vector3 intersection_point;
    IntersectLineTriangle(V0_, ORIGIN, current_portal_, intersection_point);

    // Now compute the barycentric coordinates of the intersection point
    ComputeBarycentricCoords(current_portal_, intersection_point, barycentric_coords);

    // Now use the shape points to determine contact points on each
    // Shape A:
    shape_A_points_.point[0] *= barycentric_coords.x;
    shape_A_points_.point[1] *= barycentric_coords.y;
    shape_A_points_.point[2] *= barycentric_coords.z;

    // The contact points are the sum of each barycentric weighted point
    // Shape A:
    contact_A_  = shape_A_points_.point[0];
    contact_A_ += shape_A_points_.point[1];
    contact_A_ += shape_A_points_.point[2];

    // The contact for the shapes will simply just be the contact_A_
    collision_info_->point_of_contact = contact_A_;
  }

  // Compute the collision normal between A and B
  contact_A_.Subtract(contact_B_, collision_info_->contact_normal);

  // Get the penetration depth/length (length of the normal)
  collision_info_->penetration_depth = collision_info_->contact_normal.Length();

  // Now normalize the collision normal by dividing by length
  // Add a little epsilon just in case length is zero for some reason
  collision_info_->contact_normal /= collision_info_->penetration_depth + NORMALIZATION_EPSILON;

  // Also specify that there was a collision (duh, I know)
  collision_info_->was_collision = true;

  // If some invalid case happened, return no collision
  if (collision_info_->contact_normal == ORIGIN)
    collision_info_->was_collision = false;
}

// The supporting function we'll use to sample support
void MPR::Support(const Vector3& dir, Vector3& support_out, Vector3& A_out, Vector3& B_out)
{
  // Call the supporting function
  pair_->MinkowskiSupport(dir, support_out, B_out, A_out);
}

// Simple check for 3 points and a plane (to see if the point is strictly outside the plane)
bool MPR::PlaneOriginOutside(PointPlane& plane_in_plane_out)
{
  // Create the first and second side
  Vector3 side1, side2;
  plane_in_plane_out.point[0].Subtract(plane_in_plane_out.point[2], side1);
  plane_in_plane_out.point[0].Subtract(plane_in_plane_out.point[1], side2);

  // The cross product of the two sides is the normal
  side2.CrossProduct(side1, plane_in_plane_out.normal);
  // Use the dot product between point1 and the normal as the d value
  float d = -plane_in_plane_out.point[0].DotProduct(plane_in_plane_out.normal);
  // If the d-value is greater than zero, return true
  return d > 0.0F;
}

// Simple check for 3 points and a plane (to see if the point is outside or on the plane)
bool MPR::PlaneOriginOutsideOrOn(PointPlane& plane_in_plane_out)
{
  // Create the first and second side
  Vector3 side1, side2;
  plane_in_plane_out.point[0].Subtract(plane_in_plane_out.point[2], side1);
  plane_in_plane_out.point[0].Subtract(plane_in_plane_out.point[1], side2);

  // The cross product of the two sides is the normal
  side2.CrossProduct(side1, plane_in_plane_out.normal);
  // Use the dot product between point1 and the normal as the d value
  float d = -plane_in_plane_out.point[0].DotProduct(plane_in_plane_out.normal);
  // If the d-value is greater than zero, return true
  return d >= 0.0F;
}

// Computes the barycentric coordinates of a point inside of a triangle
void MPR::ComputeBarycentricCoords(const Triangle& triangle, const Vector3& point, Vector3& uvw_out)
{
  // You can find a great explanation of this function in Chapter 3 of
  // Real-time Collision Detection (Christer Ericson), starting on page 46

  // Will hold the side vectors and origin transform
  Vector3 S0, S1, S2;

  // Compute the side vector S0
  triangle.point[1].Subtract(triangle.point[0], S0);
  // Compute the side vector S1
  triangle.point[2].Subtract(triangle.point[0], S1);
  // Compute the origin transformation
  point.Subtract(triangle.point[0], S2);

  // Compute the dot products that we'll use to solve for a and b
  // In the equation a(S0) + b(S1) = S2
  float dot_00 = S0.DotProduct(S0);
  float dot_01 = S0.DotProduct(S1);
  float dot_11 = S1.DotProduct(S1);
  float dot_20 = S2.DotProduct(S0);
  float dot_21 = S2.DotProduct(S1);

  // Compute the denominator (again just to solve for a and b)
  float denominator = (dot_00 * dot_11) - (dot_01 * dot_01);

  // Now compute the u, v, and w values
  // Note, for efficiency we just used a vector, so u = x, v = y, w = z
  uvw_out.y = (dot_11 * dot_20 - dot_01 * dot_21) / denominator;  // v
  uvw_out.z = (dot_00 * dot_21 - dot_01 * dot_20) / denominator;  // w
  uvw_out.x = 1.0F - uvw_out.y - uvw_out.z;                       // u
}

// Intersect a line with a triangle and get the intersection point
void MPR::IntersectLineTriangle(const Vector3& P1, const Vector3& P2, const Triangle& tri, Vector3& point_out)
{
  // Compute the numerator and denominator values
  Vector3 N1, D1;
  tri.point[0].Subtract(P1, N1);
  P2.Subtract(P1, D1);
 
  // Compute the t-value for the intersection time
  float t = tri.normal.DotProduct(N1) / tri.normal.DotProduct(D1);

  // Now compute the actual intersection point
  D1.Scale(t, point_out);
  point_out += P1;
}

// Seeds the random number generator
void MPR::SeedRandom(unsigned seed)
{
  rand_seed_ = seed;
}

// Generates a random number
void MPR::GenerateRandomVector(Vector3& out_rand)
{
  // Generate a new random seed, and then a random number from that
  // X Value:
  rand_seed_ = rand_seed_ * 1103515245 + 12345;
  out_rand.x = ((float)((rand_seed_ / 65536) % 32768) / 32768.0F) - 0.5F;
  // Y Value:
  rand_seed_ = rand_seed_ * 1103515245 + 12345;
  out_rand.y = ((float)((rand_seed_ / 65536) % 32768) / 32768.0F) - 0.5F;
  // Z Value:
  rand_seed_ = rand_seed_ * 1103515245 + 12345;
  out_rand.z = ((float)((rand_seed_ / 65536) % 32768) / 32768.0F) - 0.5F;
}

// Asserts an error
void MPR::AssertError(const char* error)
{
  // This just makes sure that we "use" the error
  (void*) error;

#if defined(MPR_ERRORS) && !defined(MPR_DEBUG_INFO)

  // Show a message box
  ::MessageBox(0, error, "MPR Error:", 0);

#elif defined(MPR_ERRORS) && defined(MPR_DEBUG_INFO)

  // Show the error in the console
  ShowMessage(Engine::MESSAGE_ERROR, error);

  // Print the error on the top
  GetMgr("Graphics")->Call("DebugDrawScreenText", 0x00, 0xFFFF0000, Vector3(0.0F, 0.05F, 0.0F), error);

#endif
}

// Initializes the iteration counter
void MPR::InitIterationCounter()
{
  iteration_counter_ = 0;
}

// Counts up each time an iteration occurs
void MPR::CountIteration()
{
  ++iteration_counter_;
}

// Counts up debug iterations (returns if we should stop)
bool MPR::DebugBreak(Stage stage)
{
  // Count up the debug iterations
  ++debug_iteration_;

  // If we're on the breakpoint, draw the current state
  if (debug_iteration_ == debug_breakpoint_)
  {
    // Draw the current debug state
    DebugDraw(stage);

    // Return true, since we hit the breakpoint
    return true;
  }

  // Return false
  return false;
}


// Draws the current state
void MPR::DebugDraw(Stage stage)
{
  // Make sure we don't get any unused warnings
  (int) stage;

  // If debug is defined
#ifdef MPR_DEBUG_INFO

  // This defines what text to write
  const char* stage_text[] =
  {
    "Stage: Portal Discovery",
    "Stage: Entering Portal Refinement",
    "Stage: Portal Refinement",
    "Stage: Entering Contact Discovery",
    "Stage: Contact Discovery",
    "Stage: Contact Completed"
  };

  // Draw text based off the current stage
  GetMgr("Graphics")->Call("DebugDrawScreenText", 0x00, 0xFFFFFFFF, Vector3(0.0F, 0.0F, 0.0F), stage_text[stage]);

  // Create a vector of vertices
  vector<Vector3> convex_hull;
  convex_hull.reserve(pair_->A_num_verts * pair_->B_num_verts);

  // Loop through all vertices in shape A
  for (unsigned a = 0; a < pair_->A_num_verts; ++a)
  {
    // Loop through all vertices in shape B
    for (unsigned b = 0; b < pair_->B_num_verts; ++b)
    {
      // Compute the Minkowski difference point and add it to the vector
      Vector3 pt = pair_->B_vertices[b];
      pt -= pair_->A_vertices[a];
      convex_hull.push_back(pt);
    }
  }

  // Draw the hull
  GetMgr("Graphics")->Call("DebugDrawHull", 0x00, 0xFF000000, &convex_hull[0], convex_hull.size());

  // Draw the inner point
  GetMgr("Graphics")->Call("DebugDrawPoint", 0x00, 0xFF000000, V0_, 2.0F);

  // Draw the origin ray
  GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFF0000FF, V0_, origin_ray_);

  // Draw the current portal
  GetMgr("Graphics")->Call("DebugDrawTriangle", 0x00, (unsigned) 0x40AA00AA, current_portal_.point[0], current_portal_.point[1], current_portal_.point[2]);

  // Draw the sides of the current portal
  GetMgr("Graphics")->Call("DebugDrawTriangle", 0x00, (unsigned) 0x150000AA, V0_, current_portal_.point[1], current_portal_.point[2]);
  GetMgr("Graphics")->Call("DebugDrawTriangle", 0x00, (unsigned) 0x150000AA, V0_, current_portal_.point[2], current_portal_.point[0]);
  GetMgr("Graphics")->Call("DebugDrawTriangle", 0x00, (unsigned) 0x150000AA, V0_, current_portal_.point[0], current_portal_.point[1]);


  // Draw which points are which
  GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFFFFFFFF, V0_, "V0");
  GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFFFFFFFF, current_portal_.point[V1], "V1");
  GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFFFFFFFF, current_portal_.point[V2], "V2");
  GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFFFFFFFF, current_portal_.point[V3], "V3");
  GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFFFF0000, ORIGIN, "Origin");


  // Find an average point in the center of the portal (just for drawing the normal)
  Vector3 avg = current_portal_.point[0];
  avg += current_portal_.point[1];
  avg += current_portal_.point[2];
  avg /= 3.0F;

  // Compute the current portal's normal, just to ensure it's correct
  Vector3 side1, side2, portal_normal;
  current_portal_.point[0].Subtract(current_portal_.point[2], side1);
  current_portal_.point[0].Subtract(current_portal_.point[1], side2);

  // The cross product of the two sides is the normal
  side2.CrossProduct(side1, portal_normal);
  portal_normal.Normalize();

  // Draw the portal's normal in the center of the portal
  GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFF0000FF, avg, portal_normal);

  // Draw the origin coordinates
  GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFFFF0000, ORIGIN, Vector3(1.0F, 0.0F, 0.0F));
  GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFFFF0000, ORIGIN, Vector3(0.0F, 1.0F, 0.0F));
  GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFFFF0000, ORIGIN, Vector3(0.0F, 0.0F, 1.0F));


  // If the current stage is contact completed
  if (stage == STAGE_CONTACT_COMPLETED)
  {
    // Draw the points of contact
    GetMgr("Graphics")->Call("DebugDrawSphere", 0x00, 0xFF0000FF, contact_A_, 0.5F);
    GetMgr("Graphics")->Call("DebugDrawSphere", 0x00, 0xFF00FF00, contact_B_, 0.5F);

    // Draw the contact point on the portal
    GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFF00FF00, contact_point, Vector3(1.0F, 0.0F, 0.0F));
    GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFF00FF00, contact_point, Vector3(0.0F, 1.0F, 0.0F));
    GetMgr("Graphics")->Call("DebugDrawVector", 0x00, 0xFF00FF00, contact_point, Vector3(0.0F, 0.0F, 1.0F));

    // Draw the point of intersection text
    GetMgr("Graphics")->Call("DebugDrawWorldText", 0x00, 0xFF00FF00, contact_point, "POI");
  }

#endif
}


// Triggers a collision/algorithm timeout if it reaches the given number of iterations
bool MPR::IterationTimedOut(unsigned max_iterations)
{
  return iteration_counter_ == max_iterations;
}
Categories: Code Samples Tags:
  1. No comments yet.
  1. No trackbacks yet.