Sweep AABB 3d

sweep.cpp

// Extended from 2d implementation found here https://www.gamedev.net/tutorials/programming/general-and-gameplay-programming/swept-aabb-collision-detection-and-response-r3084/
// Broad-Phasing not implemented but needed to work accurately

#include <iostream>
#include <limits>
#include <algorithm>
#include <initializer_list>

struct Box 
{ 
  // position of top-left corner 
  float x, y, z; 

  // dimensions 
  float w, h, d; 

  // velocity 
  float vx, vy, vz; 
};

float SweptAABB(Box b1, Box b2, float& normalx, float& normaly, float& normalz) {
  float xInvEntry, yInvEntry, zInvEntry;
  float xInvExit, yInvExit, zInvExit;

  // find the distance between the objects on the near and far sides for both x and y
  if (b1.vx > 0.0f)
  {
    xInvEntry = b2.x - (b1.x + b1.w);
    xInvExit = (b2.x + b2.w) - b1.x;
  }
  else
  {
    xInvEntry = (b2.x + b2.w) - b1.x;
    xInvExit = b2.x - (b1.x + b1.w);
  }

  if (b1.vy > 0.0f)
  {
    yInvEntry = b2.y - (b1.y + b1.h);
    yInvExit = (b2.y + b2.h) - b1.y;
  }
  else
  {
    yInvEntry = (b2.y + b2.h) - b1.y;
    yInvExit = b2.y - (b1.y + b1.h);
  }

  if (b1.vz > 0.0f)
  {
    zInvEntry = b2.z - (b1.z + b1.d);
    zInvExit = (b2.z + b2.d) - b1.z;
  }
  else
  {
    zInvEntry = (b2.z + b2.d) - b1.z;
    zInvExit = b2.z - (b1.z + b1.d);
  }

  // find time of collision and time of leaving for each axis (if statement is to prevent divide by zero)
  float xEntry, yEntry, zEntry;
  float xExit, yExit, zExit;

  if (b1.vx == 0.0f)
  {
    xEntry = -std::numeric_limits<float>::infinity();
    xExit = std::numeric_limits<float>::infinity();
  }
  else
  {
    xEntry = xInvEntry / b1.vx;
    xExit = xInvExit / b1.vx;
  }

  if (b1.vy == 0.0f)
  {
    yEntry = -std::numeric_limits<float>::infinity();
    yExit = std::numeric_limits<float>::infinity();
  }
  else
  {
    yEntry = yInvEntry / b1.vy;
    yExit = yInvExit / b1.vy;
  }

  if (b1.vz == 0.0f)
  {
    zEntry = -std::numeric_limits<float>::infinity();
    zExit = std::numeric_limits<float>::infinity();
  }
  else
  {
    zEntry = zInvEntry / b1.vz;
    zExit = zInvExit / b1.vz;
  }

  // find the earliest/latest times of collisionfloat
  float entryTime = std::max({xEntry, yEntry, zEntry});
  float exitTime = std::min({xExit, yExit, zExit});

  std::cout << entryTime << " " << exitTime << std::endl;

  std::cout << xEntry << " " << yEntry << " " << zEntry << std::endl;
  std::cout << xExit << " " << yExit << " " << zExit << std::endl;

  std::cout << (yEntry < zEntry) << std::endl;

  // if there was no collision
  if (entryTime > exitTime || (xEntry < 0.0f && yEntry < 0.0f) || (xEntry < 0.0f && zEntry < 0.0f) || (yEntry < 0.0f && zEntry < 0.0f) || xEntry > 1.0f || yEntry > 1.0f || zEntry > 1.0f)
  {
    normalx = 0.0f;
    normaly = 0.0f;
    normalz = 0.0f;
    return 1.0f;
  }
  else // if there was a collision 
  { 
    // calculate normal of collided surface
    if (xEntry > yEntry && xEntry > zEntry)
    { 
      if (xInvEntry < 0.0f) 
      { 
        normalx = 1.0f; 
        normaly = 0.0f; 
        normalz = 0.0f; 
      } 
      else 
      { 
        normalx = -1.0f; 
        normaly = 0.0f;
        normalz = 0.0f;
      } 
    } 
    else if (yEntry > zEntry)
    { 
      if (yInvEntry < 0.0f) 
      { 
        normalx = 0.0f; 
        normaly = 1.0f; 
        normalz = 0.0f;
      } 
      else 
      { 
        normalx = 0.0f; 
        normaly = -1.0f; 
        normalz = 0.0f;
      } 
    }
    else
    { 
      if (zInvEntry < 0.0f) 
      { 
        normalx = 0.0f; 
        normaly = 0.0f; 
        normalz = 1.0f;
      } 
      else 
      { 
        normalx = 0.0f; 
        normaly = 0.0f; 
        normalz = -1.0f;
      } 
    }
    return entryTime;
  }

}

int main() {
  Box *b1 = new Box();
  b1->x = 1;
  b1->y = 1;
  b1->z = 1;

  b1->h = 1;
  b1->w = 1;
  b1->d = 1;

  b1->vx = 3;
  b1->vy = 4;
  b1->vz = 7;

  Box *b2 = new Box();
  b2->x = 3;
  b2->y = 3;
  b2->z = 3;

  b2->h = 1;
  b2->w = 1;
  b2->d = 1;

  b2->vx = 0;
  b2->vy = 0;
  b2->vz = 0;

  float normX = 0,normY = 0, normZ = 0;

  std::cout << SweptAABB(*b1, *b2, normX, normY, normZ) << std::endl;
  std::cout << normX << " " << normY << " " << normZ << std::endl;


  return 0;
}