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..3a175e83b 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,200 @@ 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) { + 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(FastMath.sqrt(Math.min(Math.min(d1, d2), d3)) - radius); + Vector3f normal = n.normalize(); + float dist = -normal.dot(a); // a is center relative, so -a points to center + dist = dist - radius; + + r.setDistance(dist); + r.setContactNormal(normal); + 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.vect9); + 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.vect10); + 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 - radius); + 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 - radius); + 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(); } diff --git a/jme3-core/src/main/java/com/jme3/util/BufferUtils.java b/jme3-core/src/main/java/com/jme3/util/BufferUtils.java index dcc6ab34d..84c6f6743 100644 --- a/jme3-core/src/main/java/com/jme3/util/BufferUtils.java +++ b/jme3-core/src/main/java/com/jme3/util/BufferUtils.java @@ -224,6 +224,28 @@ public final class BufferUtils { return buff; } + /** + * Generate a new FloatBuffer using the given array of ColorRGBA objects. + * The FloatBuffer will be 4 * data.length long and contain the color data. + * + * @param data array of ColorRGBA objects to place into a new FloatBuffer + */ + public static FloatBuffer createFloatBuffer(ColorRGBA... data) { + if (data == null) { + return null; + } + FloatBuffer buff = createFloatBuffer(4 * data.length); + for (int x = 0; x < data.length; x++) { + if (data[x] != null) { + buff.put(data[x].getRed()).put(data[x].getGreen()).put(data[x].getBlue()).put(data[x].getAlpha()); + } else { + buff.put(0).put(0).put(0).put(0); + } + } + buff.flip(); + return buff; + } + /** * Generate a new FloatBuffer using the given array of float primitives. * @param data array of float primitives to place into a new FloatBuffer