Added RaycastMesh function and example test case

4 files changed, 134 insertions, 6 deletions

diff --git a/src/models.c b/src/models.c
index a2043913..41e527dc 100644
--- a/src/models.c
+++ b/src/models.c
@@ -1918,3 +1918,41 @@ static Material LoadMTL(const char *fileName)
 
     return material;
 }
+
+RayHitInfo RaycastMesh( Ray ray, Mesh *mesh )
+{
+    RayHitInfo result = {0};
+
+    // If mesh doesn't have vertex data on CPU, can't test it.
+    if (!mesh->vertices) {
+        return result;
+    }
+
+    // mesh->triangleCount may not be set, vertexCount is more reliable
+    int triangleCount = mesh->vertexCount / 3;
+
+    // Test against all triangles in mesh
+    for (int i=0; i < triangleCount; i++) {
+        Vector3 a, b, c;
+        Vector3 *vertdata = (Vector3*)mesh->vertices;
+        if (mesh->indices) {
+            a = vertdata[ mesh->indices[i*3+0] ];
+            b = vertdata[ mesh->indices[i*3+1] ];
+            c = vertdata[ mesh->indices[i*3+2] ];            
+        } else {            
+            a = vertdata[i*3+0];
+            b = vertdata[i*3+1];
+            c = vertdata[i*3+2];
+        }
+
+        RayHitInfo triHitInfo = RaycastTriangle( ray, a, b, c );
+        if (triHitInfo.hit) {
+            // Save the closest hit triangle
+            if ((!result.hit)||(result.distance > triHitInfo.distance)) {
+                result = triHitInfo;
+            }
+        }
+    }
+
+    return result; 
+}
diff --git a/src/raylib.h b/src/raylib.h
index f291ce85..7252ba4e 100644
--- a/src/raylib.h
+++ b/src/raylib.h
@@ -497,6 +497,7 @@ typedef struct Ray {
 // Information returned from a raycast
 typedef struct RayHitInfo {
     bool hit;             // Did the ray hit something?
+    float distance;       // Distance to nearest hit
     Vector3 hitPosition;  // Position of nearest hit
     Vector3 hitNormal;    // Surface normal of hit
 } RayHitInfo;
@@ -924,6 +925,8 @@ RLAPI bool CheckCollisionRayBox(Ray ray, BoundingBox box);
 // Ray Casts
 //------------------------------------------------------------------------------------
 RLAPI RayHitInfo RaycastGroundPlane( Ray ray, float groundHeight );
+RLAPI RayHitInfo RaycastTriangle( Ray ray, Vector3 a, Vector3 b, Vector3 c );
+RLAPI RayHitInfo RaycastMesh( Ray ray, Mesh *mesh );
 
 //------------------------------------------------------------------------------------
 // Shaders System Functions (Module: rlgl)
diff --git a/src/raymath.h b/src/raymath.h
index 3cd1394e..5871e350 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -130,6 +130,7 @@ RMDEF void VectorTransform(Vector3 *v, Matrix mat);           // Transforms a Ve
 RMDEF Vector3 VectorZero(void);                               // Return a Vector3 init to zero
 RMDEF Vector3 VectorMin(Vector3 vec1, Vector3 vec2);          // Return min value for each pair of components
 RMDEF Vector3 VectorMax(Vector3 vec1, Vector3 vec2);          // Return max value for each pair of components
+RMDEF Vector3 Barycentric(Vector3 p, Vector3 a, Vector3 b, Vector3 c); // Barycentric coords for p in triangle abc
 
 //------------------------------------------------------------------------------------
 // Functions Declaration to work with Matrix
@@ -382,6 +383,31 @@ RMDEF Vector3 VectorMax(Vector3 vec1, Vector3 vec2)
     return result;
 }
 
+// Compute barycentric coordinates (u, v, w) for
+// point p with respect to triangle (a, b, c)
+// Assumes P is on the plane of the triangle
+RMDEF Vector3 Barycentric(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
+{
+
+    //Vector v0 = b - a, v1 = c - a, v2 = p - a;
+    Vector3 v0 = VectorSubtract( b, a );
+    Vector3 v1 = VectorSubtract( c, a );
+    Vector3 v2 = VectorSubtract( p, a );
+    float d00 = VectorDotProduct(v0, v0);
+    float d01 = VectorDotProduct(v0, v1);
+    float d11 = VectorDotProduct(v1, v1);
+    float d20 = VectorDotProduct(v2, v0);
+    float d21 = VectorDotProduct(v2, v1);
+    float denom = d00 * d11 - d01 * d01;
+    
+    Vector3 result;
+    result.y = (d11 * d20 - d01 * d21) / denom;
+    result.z = (d00 * d21 - d01 * d20) / denom;
+    result.x = 1.0f - (result.z + result.y);
+    
+    return result;
+}
+
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Matrix math
 //----------------------------------------------------------------------------------
diff --git a/src/shapes.c b/src/shapes.c
index 4b2de4f2..74480c83 100644
--- a/src/shapes.c
+++ b/src/shapes.c
@@ -544,13 +544,74 @@ RayHitInfo RaycastGroundPlane( Ray ray, float groundHeight )
     {
         float t = (ray.position.y - groundHeight) / -ray.direction.y;
         if (t >= 0.0) {
-        Vector3 camDir = ray.direction;
-        VectorScale( &camDir, t );
-        result.hit = true;
-        result.hitNormal = (Vector3){ 0.0, 1.0, 0.0};
-        result.hitPosition = VectorAdd( ray.position, camDir );
+            Vector3 rayDir = ray.direction;
+            VectorScale( &rayDir, t );
+            result.hit = true;
+            result.distance = t;
+            result.hitNormal = (Vector3){ 0.0, 1.0, 0.0};
+            result.hitPosition = VectorAdd( ray.position, rayDir );
         }
     }
+    return result;
+}
+// Adapted from: 
+// https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+RayHitInfo RaycastTriangle( Ray ray, Vector3 a, Vector3 b, Vector3 c )
+{
+    Vector3 e1, e2;  //Edge1, Edge2
+    Vector3 p, q, tv;
+    float det, inv_det, u, v;
+    float t;
+    RayHitInfo result = {0};
+    
+    //Find vectors for two edges sharing V1
+    e1 = VectorSubtract( b, a);
+    e2 = VectorSubtract( c, a);
+    
+    //Begin calculating determinant - also used to calculate u parameter
+    p = VectorCrossProduct( ray.direction, e2);
+    
+    //if determinant is near zero, ray lies in plane of triangle or ray is parallel to plane of triangle
+    det = VectorDotProduct(e1, p);
+    
+    //NOT CULLING
+    if(det > -EPSILON && det < EPSILON) return result;
+    inv_det = 1.f / det;
+    
+    //calculate distance from V1 to ray origin
+    tv = VectorSubtract( ray.position, a );
+    
+    //Calculate u parameter and test bound
+    u = VectorDotProduct(tv, p) * inv_det;
+    
+    //The intersection lies outside of the triangle
+    if(u < 0.f || u > 1.f) return result;
+    
+    //Prepare to test v parameter
+    q = VectorCrossProduct( tv, e1 );
+    
+    //Calculate V parameter and test bound
+    v = VectorDotProduct( ray.direction, q) * inv_det;
+    
+    //The intersection lies outside of the triangle
+    if(v < 0.f || (u + v)  > 1.f) return result;
+    
+    t = VectorDotProduct(e2, q) * inv_det;
+    
+    
+    if(t > EPSILON) { 
+        // ray hit, get hit point and normal
+        result.hit = true;
+        result.distance = t;
 
+        result.hit = true;
+        result.hitNormal = VectorCrossProduct( e1, e2 );
+        VectorNormalize( &result.hitNormal );
+        Vector3 rayDir = ray.direction;
+        VectorScale( &rayDir, t );        
+        result.hitPosition = VectorAdd( ray.position, rayDir );
+    }
+    
     return result;
-}
\ No newline at end of file
+}
+