Merge remote-tracking branch 'jme-master/master'

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();
         }

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