Vertical (Wall) Coll Detect

CollDetect.cpp

/*** Not optimized or perfect, but hopefully helps someone else learn **/

//https://gamedev.stackexchange.com/questions/96459/fast-ray-sphere-collision-code
static bool intersectRaySegmentSphere(float3 o, float3 d, float3 so, float radius2, float3 &ip)
{
  //we pass in d non-normalized to keep it's length
  //then we use that length later to compare the intersection point to make sure
  //we're within the actual ray segment
  float l = d.length();
  d /= l;

  float3 m = o - so;
  float b = m.dot(d);
  float c = m.dot(m) - radius2;
  
  // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
  if(c > 0.0f && b > 0.0f)
    return false;
  float discr = b*b - c;
  
  // A negative discriminant corresponds to ray missing sphere
  if(discr < 0.0f)
    return false;
  
  // Ray now found to intersect sphere, compute smallest t value of intersection
  float t = -b - sqrtf(discr);
  
  // If t is negative, ray started inside sphere so clamp t to zero
  if(t < 0.0f)
    t = 0.0f;
  ip = o + (d * t);
  
  //here's that last segment check I was talking about
  if(t > l)
    return false;
  
  return true;
}

//2D test for which side of a 2D line a 2D point lies on
static bool leftOf(const float2 &a, const float2 &b, const float2 &p)
{
  //3x3 determinant (can also think of this aprojecting onto 2D lines)
  // | ax  bx  px |
  // | ay  by  py |
  // | 1   1   1  |
  
  float area = 0.5f * (a.x * (b.y - p.y) +
                       b.x * (p.y - a.y) +
                       p.x * (a.y - b.y));
  return (area > 0.0f);
}

//2D test for point inside polygon
static bool pointInside(const float2 poly[], int pcount, const float2 &v)
{
  for(int i = 0; i < pcount; i++)
  {
    int next = i;
    next++;
    if(next == pcount)
      next = 0;
    
    if(!leftOf(poly[i], poly[next], v))
      return false;
  }
  return true;
}

void Character::collisionDetect(vom::Scene *scene)
{
  const float3 collSphereOrigin = model->getPos() + float3(0, 2.5f, 0);
  const float collSphereRadius = 3.0f;
  const float collSphereRadius2 = collSphereRadius*collSphereRadius;
  
  float3 shiftDelta = float3::zero;
  int numCollisions = 0;

  for(const auto &sceneObject : scene->getEntities())
  {
    if(sceneObject->getTag() != 0)
      continue;
    
    //sceneObject->getMeshes()[0]->getMaterial()->setDiffuse(float4(1, 1, 1, 1));
    
    const auto &mesh = sceneObject->getMeshes()[0];
    auto verts = sceneObject->getEditVerts();
    auto norms = sceneObject->getEditNorms();
    auto triangleInds = mesh->getEditInds();
    int numTris = triangleInds->getCount()/3;
    
    uint3 *triData = (uint3 *)triangleInds->getData();
    float3 *vertsData = (float3 *)verts->getData();
    float3 *normsData = (float3 *)norms->getData();
    
    //for each triangle in the collision geometry
    for(int i = 0; i < numTris; i++)
    {
      bool outsidePlane = false;
      bool outsideAllVerts = false;
      bool outsideAllEdges = false;
      bool fullyInsidePlane = false;
      
      float3 v1 = vertsData[triData[i].x];
      float3 v2 = vertsData[triData[i].y];
      float3 v3 = vertsData[triData[i].z];
      
      //assume flat normals for collision (all 3 n would be the same)
      float3 pN = (float4(normsData[triData[i].x].normalized(), 0.0f)).xyz();
      
      //only test vertical polygons
      if(fabs(pN.y) > 0.1f)
        continue;
      
      float d = -((v1 + v2 + v3) / 3.0f).dot(pN);
      
      //get point-to-plane distance from model center
      float ppd = pN.dot(collSphereOrigin) + d;
      
      //abs() check wasn't in the video
      if(fabs(ppd) > collSphereRadius)
      {
        //sphere outside of infinite triangle plane
        outsidePlane = true;
        continue;
      }
      
      //build 3 rays (line segments) (doing this earlier now to support plane projection)
      float3 a = v2-v1;
      float3 b = v3-v2;
      float3 c = v1-v3;
      
      //NOT INCLUDED IN VIDEO.. break the plane test should be more robust to consider triangle bounds////////////////////////////////////
      //project to triangle plane (3D -> 2D) and see if we are within its bounds
      float3 planeX = a.normalized();
      float3 planeY = pN.cross(a).normalized();

      //local function to do our projection for us
      auto project2D = [&](const float3 &p) { return float2(p.dot(planeX), p.dot(planeY)); };
      
      float2 planePos2D = project2D(collSphereOrigin);
      float2 triangle2D[3] = { project2D(v1), project2D(v2), project2D(v3) };
      
      if(pointInside(triangle2D, 3, planePos2D))
      {
        fullyInsidePlane = true;
      }

      /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      
      bool outsideV1 = ((v1-collSphereOrigin).lengthSquared() > collSphereRadius2);
      bool outsideV2 = ((v2-collSphereOrigin).lengthSquared() > collSphereRadius2);
      bool outsideV3 = ((v3-collSphereOrigin).lengthSquared() > collSphereRadius2);
      
      if(outsideV1 && outsideV2 && outsideV3)
      {
        //sphere outside of of all triangle vertices
        outsideAllVerts = true;
      }
      
      float3 ip;
      
      if(!intersectRaySegmentSphere(v1, a, collSphereOrigin, collSphereRadius2, ip) &&
         !intersectRaySegmentSphere(v2, b, collSphereOrigin, collSphereRadius2, ip) &&
         !intersectRaySegmentSphere(v3, c, collSphereOrigin, collSphereRadius2, ip))
      {
        //sphere outside of all triangle edges
        outsideAllEdges = true;
      }
      
      if(outsideAllVerts && outsideAllEdges && !fullyInsidePlane)
      {
        continue;
      }
      
      //sceneObject->getMeshes()[0]->getMaterial()->setDiffuse(float4(1, 0, 0, 1));
      
      //push the character (us) outside of the intersected body
      shiftDelta += pN*(collSphereRadius-ppd);
      numCollisions++;
    }
  }
  
  if(numCollisions != 0)
  {
    shiftDelta /= (float)numCollisions;
    
    if(shiftDelta.length() > lastWalkSpeed)
    {
      shiftDelta = shiftDelta.normalized();
      shiftDelta *= lastWalkSpeed*1.1f;
    }
    
    model->setPos(model->getPos() + shiftDelta);
  }
}