Merge pull request #212 from joeld42/jbd_picking

Raycast Picking Utilities

4 files changed, 164 insertions, 1 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 e2e4ee13..7252ba4e 100644
--- a/src/raylib.h
+++ b/src/raylib.h
@@ -97,6 +97,9 @@
 #define DEG2RAD (PI/180.0f)
 #define RAD2DEG (180.0f/PI)
 
+// A small number
+#define EPSILON 0.000001
+
 // raylib Config Flags
 #define FLAG_FULLSCREEN_MODE    1
 #define FLAG_RESIZABLE_WINDOW   2
@@ -491,6 +494,14 @@ typedef struct Ray {
     Vector3 direction;      // Ray direction
 } 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;
+
 // Wave type, defines audio wave data
 typedef struct Wave {
     unsigned int sampleCount;   // Number of samples
@@ -911,6 +922,13 @@ RLAPI bool CheckCollisionRaySphereEx(Ray ray, Vector3 spherePosition, float sphe
 RLAPI bool CheckCollisionRayBox(Ray ray, BoundingBox box);                                              // Detect collision between ray and 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)
 // NOTE: This functions are useless when using OpenGL 1.1
 //------------------------------------------------------------------------------------
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 3d3333c1..74480c83 100644
--- a/src/shapes.c
+++ b/src/shapes.c
@@ -533,4 +533,85 @@ Rectangle GetCollisionRec(Rectangle rec1, Rectangle rec2)
     }
 
     return retRec;
-}
\ No newline at end of file
+}
+
+
+RayHitInfo RaycastGroundPlane( Ray ray, float groundHeight )
+{
+    RayHitInfo result = {0};
+
+    if (fabs(ray.direction.y) > EPSILON)
+    {
+        float t = (ray.position.y - groundHeight) / -ray.direction.y;
+        if (t >= 0.0) {
+            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;
+}
+