Collision detection: Separating Axis Theorem - Circle versus Polygon

Question

I've been trying to implement collision detection between circles and polygons based on Randy Gaul's C++ Impulse Engine, following the code pretty closely, but the algorithm never returns true.

Here's the JSFiddle. (the bodies are rendered using the HTML5 Canvas API for convenience)

A snippet of the code (just collision detection):

const circPoly = (a, b) => {
  let data = {},
    center = a.pos;
  data.contacts = [];
  center = b.mat.clone().trans().mult(center.clone().sub(b.pos));
  let sep = -Number.MAX_VALUE,
    faceNorm = 0;
  for (let i = 0; i < b.verts2.length; ++i) {
    let sep2 = b.norms[i].dot(center.clone().sub(b.verts2[i]));
    if (sep2 > a.radius) return data;
    if (sep2 > sep) { sep = sep2; faceNorm = i; }
  }
  let v1 = b.verts2[faceNorm],
    v2 = b.verts2[faceNorm + 1 < b.verts2.length ? faceNorm + 1 : 0];
  if (sep < 0.0001) {
    data.depth = a.radius;
    data.norm = b.mat.clone().mult(b.norms[faceNorm]).neg();
    data.contacts[0] = data.norm.clone().vmult(a.pos.clone().sadd(a.radius));
    return data;
  }
  let dot1 = center.clone().sub(v1).dot(v2.clone().sub(v1)),
    dot2 = center.clone().sub(v2).dot(v1.clone().sub(v2));
  data.depth = a.radius - sep;
  if (dot1 <= 0) {
    if (center.dist2(v1) > a.radius * a.radius) return data;
    let norm = v1.clone().sub(center);
    norm = b.mat.clone().mult(norm);
    norm.norm();
    data.norm = norm;
    v1 = b.mat.clone().mult(v1.clone().add(b.pos));
    data.contacts[0] = v1;
  } else if (dot2 <= 0) {
    if (center.dist2(v2) > a.radius * a.radius) return data;
    let norm = v2.clone().sub(center);
    norm = b.mat.clone().mult(norm);
    norm.norm();
    data.norm = norm;
    v2 = b.mat.clone().mult(v2.clone().add(b.pos));
    data.contacts[0] = v2;
  } else {
    let norm = b.norms[faceNorm];
    if (center.clone().sub(v1).dot(norm) > a.radius) return data;
    norm = b.mat.clone().mult(norm);
    data.norm = norm.clone().neg();
    data.contacts[0] = data.norm.clone().vmult(a.pos.clone().sadd(a.radius));
  }
  return data;
};

Note that b.verts2 refers to the polygon's vertices in real world coordinates.

I know for a fact that there are no problems with the Vector class but as I don't exactly have very much experience with transformation matrices, that class could be the root of these errors, although the code for it is pretty much entirely derived from the Impulse Engine as well, so it should work. As mentioned before, the algorithm always returns false, even when a collision really has occurred. What am I doing wrong here? I tried taking out the early returns, but that just returns weird results like contact points with negative coordinates which obviously is not quite correct.

EDIT: Modified my vector class's perpendicular function to work the same way as the Impulse Engine's (both ways are right, but I think one is clockwise and the other one counterclockwise -- I also modified my vertices to reflect the counterclockwise-ness). Unfortunately, it still fails the test.

https://jsfiddle.net/khanfused/tv359kgL/4/

Answer 1

Well the are many problems and I really dont understand what you are trying to do as it seem overly complex. Eg why does matrix have trans??? and why are you using the Y up screen as the coordinate system for the transform??? (rhetorical)

In the first loop.

• The first is that you are testing the distance of the normal vectors of each vert, should be testing the vert position.
• Also you are finding the distance using the vec.dot function that returns the square of the distance. But you test for the radius, you should be testing for if(sep2 < radius * radius)
• And you have the comparison the wrong way around you should be testing if less than radius squared (not greater than)
• Then when you do detect a vert within the radius you return the data object but forget to put the vert that was found inside the circle on the data.contacts array.
• I am not sure what the intention of keeping the index of the most distant vect is but then the rest of the function make zero sense to me???? :( and I have tried to understand it.

All you need to do is

A check if any verts on the poly are closer than radius, if so then you have a intercept (or is completely inside)

Then you need to check the distance of each line segment

Can be done for each line segment with the following if you dont need the intercepts (or below that if you need intercepts) only use one or the other.

// circle is a point {x:?,y:?}
// radius = is the you know what
// p1,p2 are the start and end points of a line
        checkLineCircle = function(circle,radius,p1,p2){
            var v1 = {};
            var v2 = {};
            var v3 = {};
            var u;
            // get dist to end of line
            v2.x = circle.x - p1.x;
            v2.y = circle.y - p1.y;
            // check if end points are inside the circle
            if( Math.min(
                    Math.hypot(p2.x - circle.x, p2.y - circle.y),
                    Math.hypot(v2.x, v2.y)
                ) <= radius){
                return true;
            }
            // get the line as a vector
            v1.x = p2.x - p1.x;
            v1.y = p2.y - p1.y;
            // get the unit distance of the closest point on the line
            u = (v2.x * v1.x + v2.y * v1.y)/(v1.y * v1.y + v1.x * v1.x);
            // is this on the line segment
            if(u >= 0 && u <= 1){
                v3.x = v1.x * u;  // get the point on the line segment
                v3.y = v1.y * u;
                // get the distance to that point and return true or false depending on the 
                // it being inside the circle
                return (Math.hypot(v3.y - v2.y, v3.x - v2.x) <= radius);
            }
            return false; // no intercept
      }

Do that for each line.To save time transform the circle center to the polygon local, rather than transform each point on the poly.

If you need the points of intercept then use the following function

// p1,p2 are the start and end points of a line
 // returns an array empty if no points found or one or two points depending on the number of intercepts found
 // If two points found the first point in the array is the point closest to the line start (p1)
 function circleLineIntercept(circle,radius,p1,p2){
        var v1 = {};
        var v2 = {};
        var ret = [];
        var u1,u2,b,c,d;
        // line as vector
        v1.x = p2.x - p1.x;
        v1.y = p2.y - p1.y;
        // vector to circle center
        v2.x = p1.x - circle.x;
        v2.y = p1.y - circle.y;
        // dot of line and circle
        b = (v1.x * v2.x + v1.y * v2.y) * -2;
        // length of line squared * 2
        c = 2 * (v1.x * v1.x + v1.y * v1.y);
        // some math to solve the two triangles made by the intercept points, the circle center and the perpendicular line to the line.
        d = Math.sqrt(b * b - 2 * c * (v2.x * v2.x + v2.y * v2.y - radius * radius));
        // will give a NaN if no solution
        if(isNaN(d)){ // no intercept
            return ret;
        }
        // get the unit distance of each intercept to the line
        u1 = (b - d) / c;
        u2 = (b + d) / c;

        // check the intercept is on the line segment
        if(u1 <= 1 && u1 >= 0){  
            ret.push({x:line.p1.x + v1.x * u1, y : line.p1.y + v1.y * u1 });
        }
        // check the intercept is on the line segment
        if(u2 <= 1 && u2 >= 0){  
            ret.push({x:line.p1.x + v1.x * u2, y : line.p1.y + v1.y * u2});
        }
        return ret;
    }

I will leave it up to you to do the polygon iteration.