From d5e20d53d0935b5fd8019364df53130e7db68c7c Mon Sep 17 00:00:00 2001 From: pspeed42 Date: Sun, 27 Jul 2014 18:11:43 -0400 Subject: [PATCH] Implemented accurate bounding sphere to triangle collision. The old code had an implementation but it missed tons of cases. For 100,000 random points, the old way would process that in about 12+ ms. The new way does it in about 18+ ms... but the old way missed 643 collisions (180 versus 823). Plus, with the new way accurate contact normal and contact point can be provided trivially... so it is. --- .../com/jme3/bounding/BoundingSphere.java | 197 ++++++++++++++++-- 1 file changed, 183 insertions(+), 14 deletions(-) diff --git a/jme3-core/src/main/java/com/jme3/bounding/BoundingSphere.java b/jme3-core/src/main/java/com/jme3/bounding/BoundingSphere.java index aa48658fa..390d568b7 100644 --- a/jme3-core/src/main/java/com/jme3/bounding/BoundingSphere.java +++ b/jme3-core/src/main/java/com/jme3/bounding/BoundingSphere.java @@ -792,27 +792,196 @@ public class BoundingSphere extends BoundingVolume { return 1; } } + + private int collideWithTri(Triangle tri, CollisionResults results) { + TempVars tvars = TempVars.get(); + try { + + // Much of this is based on adaptation from this algorithm: + // http://realtimecollisiondetection.net/blog/?p=103 + // ...mostly the stuff about eliminating sqrts wherever + // possible. - public int collideWith(Collidable other, CollisionResults results) { - if (other instanceof Ray) { - Ray ray = (Ray) other; - return collideWithRay(ray, results); - } else if (other instanceof Triangle){ - Triangle t = (Triangle) other; + // Math is done in center-relative space. + Vector3f a = tri.get1().subtract(center, tvars.vect1); + Vector3f b = tri.get2().subtract(center, tvars.vect2); + Vector3f c = tri.get3().subtract(center, tvars.vect3); + + Vector3f ab = b.subtract(a, tvars.vect4); + Vector3f ac = c.subtract(a, tvars.vect5); + + // Check the plane... if it doesn't intersect the plane + // then it doesn't intersect the triangle. + Vector3f n = ab.cross(ac, tvars.vect6); + float d = a.dot(n); + float e = n.dot(n); + if( d * d > radius * radius * e ) { + // Can't possibly intersect + return 0; + } + + // We intersect the verts, or the edges, or the face... + + // First check against the face since it's the most + // specific. + + // Calculate the barycentric coordinates of the + // sphere center + Vector3f v0 = ac; + Vector3f v1 = ab; + // a was P relative, so p.subtract(a) is just -a + // instead of wasting a vector we'll just negate the + // dot products below... it's all v2 is used for. + Vector3f v2 = a; + + float dot00 = v0.dot(v0); + float dot01 = v0.dot(v1); + float dot02 = -v0.dot(v2); + float dot11 = v1.dot(v1); + float dot12 = -v1.dot(v2); - float r2 = radius * radius; - float d1 = center.distanceSquared(t.get1()); - float d2 = center.distanceSquared(t.get2()); - float d3 = center.distanceSquared(t.get3()); + float invDenom = 1 / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; - if (d1 <= r2 || d2 <= r2 || d3 <= r2) { - CollisionResult r = new CollisionResult(); - r.setDistance(FastMath.sqrt(Math.min(Math.min(d1, d2), d3)) - radius); + if( u >= 0 && v >= 0 && (u + v) < 1 ) { + // We intersect... and we even know where + Vector3f part1 = ac; + Vector3f part2 = ab; + Vector3f p = center.add(a.add(part1.mult(u)).addLocal(part2.mult(v))); + + CollisionResult r = new CollisionResult(); + r.setDistance((float)Math.sqrt(d) - radius); + r.setContactNormal(n.normalize()); + r.setContactPoint(p); results.addCollision(r); return 1; } - + + // Check the edges looking for the nearest point + // that is also less than the radius. We don't care + // about points that are farther away than that. + Vector3f nearestPt = null; + float nearestDist = radius * radius; + + Vector3f base; + Vector3f edge; + float t; + + // Edge AB + base = a; + edge = ab; + + t = -edge.dot(base) / edge.dot(edge); + if( t >= 0 && t <= 1 ) { + Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect8); + float distSq = Q.dot(Q); // distance squared to origin + if( distSq < nearestDist ) { + nearestPt = Q; + nearestDist = distSq; + } + } + + // Edge AC + base = a; + edge = ac; + + t = -edge.dot(base) / edge.dot(edge); + if( t >= 0 && t <= 1 ) { + Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect8); + float distSq = Q.dot(Q); // distance squared to origin + if( distSq < nearestDist ) { + nearestPt = Q; + nearestDist = distSq; + } + } + + // Edge BC + base = b; + Vector3f bc = c.subtract(b); + edge = bc; + + t = -edge.dot(base) / edge.dot(edge); + if( t >= 0 && t <= 1 ) { + Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect8); + float distSq = Q.dot(Q); // distance squared to origin + if( distSq < nearestDist ) { + nearestPt = Q; + nearestDist = distSq; + } + } + + // If we have a point at all then it is going to be + // closer than any vertex to center distance... so we're + // done. + if( nearestPt != null ) { + // We have a hit + float dist = FastMath.sqrt(nearestDist); + Vector3f cn = nearestPt.divide(-dist); + + CollisionResult r = new CollisionResult(); + r.setDistance(dist); + r.setContactNormal(cn); + r.setContactPoint(nearestPt.add(center)); + results.addCollision(r); + + return 1; + } + + // Finally check each of the triangle corners + + // Vert A + base = a; + t = base.dot(base); // distance squared to origin + if( t < nearestDist ) { + nearestDist = t; + nearestPt = base; + } + + // Vert B + base = b; + t = base.dot(base); // distance squared to origin + if( t < nearestDist ) { + nearestDist = t; + nearestPt = base; + } + + // Vert C + base = c; + t = base.dot(base); // distance squared to origin + if( t < nearestDist ) { + nearestDist = t; + nearestPt = base; + } + + if( nearestPt != null ) { + // We have a hit + float dist = FastMath.sqrt(nearestDist); + Vector3f cn = nearestPt.divide(-dist); + + CollisionResult r = new CollisionResult(); + r.setDistance(dist); + r.setContactNormal(cn); + r.setContactPoint(nearestPt.add(center)); + results.addCollision(r); + + return 1; + } + + // Nothing hit... oh, well return 0; + } finally { + tvars.release(); + } + } + + public int collideWith(Collidable other, CollisionResults results) { + if (other instanceof Ray) { + Ray ray = (Ray) other; + return collideWithRay(ray, results); + } else if (other instanceof Triangle){ + Triangle t = (Triangle) other; + return collideWithTri(t, results); } else { throw new UnsupportedCollisionException(); }