implemented face collision detection algorithm: ray-triangle + octree

Author: andresoviedo

README.md

@@ -12,10 +12,10 @@ The purpose of this application is to learn and share how to draw using OpenGL l
 * Collada format (DAE): https://en.wikipedia.org/wiki/COLLADA
 
 
-News (21/12/2017)
+News (22/12/2017)
 =================
 
-* Improved collision detection algorithm (ray-aabb)
+* Implemented collision detection algorithm: ray-aabb + ray-triangle + octree
 * Support for collada files with skeletal animations :)
 * Fixed #28: Load texture feature is now available
 
@@ -71,6 +71,7 @@ Features
     - rotate with 2 fingers to rotate camera
     - pinch & spread to zoom in/out the camera
   - skeletal animations
+  - ray collision detection
 
 
 Try it
@@ -123,36 +124,30 @@ ChangeLog
 
 (f) fixed, (i) improved, (n) new feature
 
+- 2.0.4 (22/12/2017)
+  - (n) Implemented face collision detection algorithm: ray-triangle + octree
 - 2.0.3 (21/12/2017)
   - (i) Improved collision detection algorithm (ray-aabb) for selecting objects
   - (i) BoundingBox code cleanup
-
 - 2.0.2 (17/12/2017)
   - (f) Collada XML parser is now android's XmlPullParser
   - (f) Animation engine frame times improved
   - (n) Camera now moves smoothly
-
 - 2.0.1 (08/12/2017)
   - (f) Multiple Collada parser fixes
   - (f) Camera now can look inside objects
-
 - 2.0.0 (24/11/2017)
   - (n) Support for collada files with skeletal animations :)
-
 - 1.4.1 (21/11/2017)
   - (f) #29: Crash loading obj with only vertex info
-
 - 1.4.0 (19/11/2017)
   - (f) #28: Load texture available for any model having texture coordinates
-
 - 1.3.1 (23/04/2017)
   - (f) #18: Removed asReadOnlyBuffer() because it is causing IndexOutOfBounds on Android 7
-
 - 1.3.0 (17/04/2017)
   - (n) #17: Added support for STL files
   - (n) #17: Asynchronous building of model so the build rendering is previewed
   - (f) #17: Added Toasts to buttons to show current state
-
 - 1.2.10 (16/04/2017)
   - (f) #16: Immersive mode is now configurable in the ModelActivity Intent: b.putString("immersiveMode", "false");
   - (f) #16: Background color configurable in the ModelActivity Intent: b.putString("backgroundColor", "0 0 0 1");
@@ -162,48 +157,35 @@ ChangeLog
   - (n) #16: Implemented Point Drawing, like wireframe mode but only the points are drawn
   - (f) #16: Removed trailing slash from parameter "assetDir"
   - (f) #16: Access to ByteBuffers made absolute so there are thread safe (future fixes need this)
-
 - 1.2.9 (11/04/2017)
   - (f) #15: Toggle rotating light
   - (f) #15: Wireframe with textures and colors
-
 - 1.2.8 (10/04/2017)
   - (f) Fixed #14: Camera movement improved. Only 1 rotation vector is used + space bounds set
-
 - 1.2.8 (04/04/2017)
   - (f) Fixed #13: parsing of vertices with multiple spaces
   - (i) Improved error handling on loading task
   - (i) Vertices are defaulted to (0,0,0) if parsing fails
-
 - 1.2.7 (03/04/2017)
   - (i) Removed commons-lang3 dependency
-
 - 1.2.6 (02/04/2017)
   - (f) Fixed #12. Drawing the wireframe using GL_LINES and the index buffer (drawElements)
-
 - 1.2.5 (01/04/2017)
   - (f) Fixed #10. Map faces to texture only when using the only loaded texture
   - (f) Fixed #11. Generation of missing vertex normals
-
 - 1.2.4 (30/03/2017)
   - (f) Fixed #5. Memory performance optimization
-
 - 1.2.3 (27/03/2017)
   - (f) Fixed #1. Cpu performance optimization
-
 - 1.2.2 (25/03/2017)
   - (f) Fixed #9. IOOBE loading face normals when faces had no texture or normals
- 
 - 1.2.1 (27/02/2017)
   - (f) Fixed loading external files issue #6
   - (i) Project moved to gradle
-
 - 1.2.0 (06/04/2016)
   - (n) Implemented selection of objects
-
 - 1.1.0 (30/03/2016)
   - (n) Implemented lighting & toggle textures & lights
   - (i) Refactoring of 3DObjectImpl
-
 - 1.0.0 (27/03/2016)
   - (n) First release in Google Play Android Market

app/src/main/java/org/andresoviedo/app/model3D/collision/CollisionDetection.java

@@ -1,13 +1,16 @@
 package org.andresoviedo.app.model3D.collision;
 
 import android.opengl.GLU;
+import android.opengl.Matrix;
 import android.util.Log;
 
 import org.andresoviedo.app.model3D.entities.BoundingBox;
 import org.andresoviedo.app.model3D.model.Object3DData;
 import org.andresoviedo.app.model3D.view.ModelRenderer;
 import org.andresoviedo.app.util.math.Math3DUtils;
 
+import java.nio.FloatBuffer;
+import java.util.Arrays;
 import java.util.List;
 
 /**
@@ -26,30 +29,32 @@ public class CollisionDetection {
      * @param windowY   the window y coordinate
      * @return the nearest object intersected by the specified coordinates or null
      */
-    public static Object3DData getIntersection(List<Object3DData> objects, ModelRenderer mRenderer, float windowX, float windowY) {
+    public static Object3DData getBoxIntersection(List<Object3DData> objects, ModelRenderer mRenderer, float windowX, float windowY) {
         float[] nearHit = unproject(mRenderer, windowX, windowY, 0);
         float[] farHit = unproject(mRenderer, windowX, windowY, 1);
-        return getIntersection(objects, nearHit, farHit);
+        float[] direction = Math3DUtils.substract(farHit, nearHit);
+        Math3DUtils.normalize(direction);
+        return getBoxIntersection(objects, nearHit, direction);
     }
 
     /**
      * Get the nearest object intersected by the specified ray or null if no object is intersected
      *
-     * @param objects the list of objects to test
-     * @param p1      the ray start point
-     * @param p2      the ray end point
+     * @param objects   the list of objects to test
+     * @param p1        the ray start point
+     * @param direction the ray direction
      * @return the object intersected by the specified ray
      */
-    public static Object3DData getIntersection(List<Object3DData> objects, float[] p1, float[] p2) {
-        float[] direction = Math3DUtils.substract(p2, p1);
-        Math3DUtils.normalize(direction);
+    public static Object3DData getBoxIntersection(List<Object3DData> objects, float[] p1, float[] direction) {
         float min = Float.MAX_VALUE;
         Object3DData ret = null;
         for (Object3DData obj : objects) {
-            if (obj.getId().startsWith("Line")) continue;
+            if (obj.getId().equals("Point") || obj.getId().equals("Line")) {
+                continue;
+            }
             BoundingBox box = obj.getBoundingBox();
             float[] intersection = getBoxIntersection(p1, direction, box);
-            if (intersection[0] > 0 && intersection[0] < intersection[1] && intersection[0] < min) {
+            if (intersection[0] > 0 && intersection[0] <= intersection[1] && intersection[0] < min) {
                 min = intersection[0];
                 ret = obj;
             }
@@ -68,17 +73,16 @@ public static Object3DData getIntersection(List<Object3DData> objects, float[] p
      * @param p2      ray end point
      * @return the entry and exit point of the ray intersecting the nearest object
      */
-    public static float[] getIntersectionPoint(List<Object3DData> objects, float[] p1, float[] p2) {
+    public static float[] getBoxIntersectionPoint(List<Object3DData> objects, float[] p1, float[] p2) {
         float[] direction = Math3DUtils.substract(p2, p1);
         Math3DUtils.normalize(direction);
         float min = Float.MAX_VALUE;
         float[] intersection2 = null;
         Object3DData ret = null;
         for (Object3DData obj : objects) {
-            if (obj.getId().startsWith("Line")) continue;
             BoundingBox box = obj.getBoundingBox();
             float[] intersection = getBoxIntersection(p1, direction, box);
-            if (intersection[0] > 0 && intersection[0] < intersection[1] && intersection[0] < min) {
+            if (intersection[0] > 0 && intersection[0] <= intersection[1] && intersection[0] < min) {
                 min = intersection[0];
                 ret = obj;
                 intersection2 = intersection;
@@ -113,8 +117,8 @@ public static boolean isBoxIntersection(float[] origin, float[] dir, BoundingBox
      * @return the intersection points of the near and far plane
      */
     public static float[] getBoxIntersection(float[] origin, float[] dir, BoundingBox b) {
-        float[] tMin = Math3DUtils.divide(Math3DUtils.substract(b.getCurrentMin(), origin), dir);
-        float[] tMax = Math3DUtils.divide(Math3DUtils.substract(b.getCurrentMax(), origin), dir);
+        float[] tMin = Math3DUtils.divide(Math3DUtils.substract(b.getMin(), origin), dir);
+        float[] tMax = Math3DUtils.divide(Math3DUtils.substract(b.getMax(), origin), dir);
         float[] t1 = Math3DUtils.min(tMin, tMax);
         float[] t2 = Math3DUtils.max(tMin, tMax);
         float tNear = Math.max(Math.max(t1[0], t1[1]), t1[2]);
@@ -143,4 +147,136 @@ public static float[] unproject(ModelRenderer mRenderer, float rx, float ry, flo
         xyzw[3] = 1;
         return xyzw;
     }
+
+    public static float[] getTriangleIntersection(List<Object3DData> objects, ModelRenderer mRenderer, float windowX, float windowY) {
+        float[] nearHit = unproject(mRenderer, windowX, windowY, 0);
+        float[] farHit = unproject(mRenderer, windowX, windowY, 1);
+        float[] direction = Math3DUtils.substract(farHit, nearHit);
+        Math3DUtils.normalize(direction);
+        Object3DData intersected = getBoxIntersection(objects, nearHit, direction);
+        if (intersected != null) {
+            Log.d("CollisionDetection", "intersected:" + intersected.getId() + ", rayOrigin:" + Arrays.toString(nearHit) + ", rayVector:" + Arrays.toString(direction));
+            FloatBuffer buffer = intersected.getVertexArrayBuffer().asReadOnlyBuffer();
+            float[] modelMatrix = intersected.getModelMatrix();
+            buffer.position(0);
+            float[] selectedv1 = null;
+            float[] selectedv2 = null;
+            float[] selectedv3 = null;
+            float min = Float.MAX_VALUE;
+            for (int i = 0; i < buffer.capacity(); i += 9) {
+                float[] v1 = new float[]{buffer.get(), buffer.get(), buffer.get(), 1};
+                float[] v2 = new float[]{buffer.get(), buffer.get(), buffer.get(), 1};
+                float[] v3 = new float[]{buffer.get(), buffer.get(), buffer.get(), 1};
+                Matrix.multiplyMV(v1, 0, modelMatrix, 0, v1, 0);
+                Matrix.multiplyMV(v2, 0, modelMatrix, 0, v2, 0);
+                Matrix.multiplyMV(v3, 0, modelMatrix, 0, v3, 0);
+                float t = getTriangleIntersection(nearHit, direction, v1, v2, v3);
+                if (t != -1 && t < min) {
+                    min = t;
+                    selectedv1 = v1;
+                    selectedv2 = v2;
+                    selectedv3 = v3;
+                }
+            }
+            if (selectedv1 != null) {
+                float[] outIntersectionPoint = Math3DUtils.add(nearHit, Math3DUtils.multiply(direction, min));
+                return outIntersectionPoint;
+            }
+        }
+        return null;
+    }
+
+    public static float[] getTriangleIntersection2(List<Object3DData> objects, ModelRenderer mRenderer, float windowX, float windowY) {
+        float[] nearHit = unproject(mRenderer, windowX, windowY, 0);
+        float[] farHit = unproject(mRenderer, windowX, windowY, 1);
+        float[] direction = Math3DUtils.substract(farHit, nearHit);
+        Math3DUtils.normalize(direction);
+        Object3DData intersected = getBoxIntersection(objects, nearHit, direction);
+        if (intersected != null) {
+            Log.d("CollisionDetection", "intersected 2:"+intersected.getId());
+            Octree octree = null;
+            synchronized (intersected) {
+                octree = intersected.getOctree();
+                if (octree == null) {
+                    octree = Octree.build(intersected);
+                    intersected.setOctree(octree);
+                }
+            }
+            float intersection = getTriangleIntersection2_Impl(octree, nearHit, direction);
+            if (intersection != -1) {
+                return Math3DUtils.add(nearHit, Math3DUtils.multiply(direction, intersection));
+            }
+            else {
+                return null;
+            }
+        }
+        return null;
+    }
+
+    private static float getTriangleIntersection2_Impl(Octree octree, float[] rayOrigin, float[] rayDirection){
+        //Log.v("CollisionDetection","Testing octree "+octree);
+        if (!isBoxIntersection(rayOrigin, rayDirection, octree.boundingBox)) {
+            return -1;
+        }
+        Octree selected = null;
+        float min = Float.MAX_VALUE;
+        for (Octree child : octree.children) {
+            if (child == null) {
+                continue;
+            }
+            float intersection = getTriangleIntersection2_Impl(child, rayOrigin, rayDirection);
+            if (intersection != -1 && intersection < min){
+                min = intersection;
+                selected = child;
+            }
+        }
+        float[] selectedTriangle = null;
+        for (float[] triangle : octree.triangles){
+            float[] vertex0 = new float[]{triangle[0], triangle[1], triangle[2]};
+            float[] vertex1 = new float[]{triangle[4], triangle[5], triangle[6]};
+            float[] vertex2 = new float[]{triangle[8], triangle[9], triangle[10]};
+            float intersection = getTriangleIntersection(rayOrigin, rayDirection, vertex0, vertex1, vertex2);
+            if (intersection != -1 && intersection < min){
+                min = intersection;
+                selectedTriangle = triangle;
+                selected = octree;
+            }
+        }
+        if (min != Float.MAX_VALUE){
+            return min;
+        }
+        return -1;
+    }
+
+    public static float getTriangleIntersection(float[] rayOrigin,
+                                                float[] rayVector,
+                                                float[] vertex0, float[] vertex1, float[] vertex2) {
+        float EPSILON = 0.0000001f;
+        float[] edge1, edge2, h, s, q;
+        float a, f, u, v;
+        edge1 = Math3DUtils.substract(vertex1, vertex0);
+        edge2 = Math3DUtils.substract(vertex2, vertex0);
+        h = Math3DUtils.crossProduct(rayVector, edge2);
+        a = Math3DUtils.dotProduct(edge1, h);
+        if (a > -EPSILON && a < EPSILON)
+            return -1;
+        f = 1 / a;
+        s = Math3DUtils.substract(rayOrigin, vertex0);
+        u = f * Math3DUtils.dotProduct(s, h);
+        if (u < 0.0 || u > 1.0)
+            return -1;
+        q = Math3DUtils.crossProduct(s, edge1);
+        v = f * Math3DUtils.dotProduct(rayVector, q);
+        if (v < 0.0 || u + v > 1.0)
+            return -1;
+        // At this stage we can compute t to find out where the intersection point is on the line.
+        float t = f * Math3DUtils.dotProduct(edge2, q);
+        if (t > EPSILON) // ray intersection
+        {
+            Log.d("CollisionDetection", "Triangle intersection at: " + t);
+            return t;
+        } else // This means that there is a line intersection but not a ray intersection.
+            return -1;
+    }
 }
+