SphereCast bug fixes, appears to be working now. More testing needed

--HG--
branch : nfb

KREngine/kraken/KRCollider.cpp

@@ -129,7 +129,7 @@ bool KRCollider::rayCast(const KRVector3 &v0, const KRVector3 &dir, KRHitInfo &h
                 KRHitInfo hitinfo_model_space;
                 if(hitinfo.didHit()) {
                     KRVector3 hit_position_model_space = KRMat4::Dot(getInverseModelMatrix(), hitinfo.getPosition());
-                    hitinfo_model_space = KRHitInfo(KRMat4::Dot(getInverseModelMatrix(), hitinfo.getPosition()), KRVector3::Normalize(KRMat4::DotNoTranslate(getInverseModelMatrix(), hitinfo.getNormal())), (hit_position_model_space - v0_model_space).magnitude(), hitinfo.getNode());
+                    hitinfo_model_space = KRHitInfo(hit_position_model_space, KRVector3::Normalize(KRMat4::DotNoTranslate(getInverseModelMatrix(), hitinfo.getNormal())), (hit_position_model_space - v0_model_space).magnitude(), hitinfo.getNode());
                 }
 
                 if(m_models[0]->rayCast(v0_model_space, dir_model_space, hitinfo_model_space)) {
@@ -148,7 +148,7 @@ bool KRCollider::sphereCast(const KRVector3 &v0, const KRVector3 &v1, float radi
     if(layer_mask & m_layer_mask) { // Only test if layer masks have a common bit set
         loadModel();
         if(m_models.size()) {
-            KRAABB sphereCastBounds = KRAABB(
+            KRAABB sphereCastBounds = KRAABB( // TODO - Need to cache this; perhaps encasulate within a "spherecast" class to be passed through these functions
                 KRVector3(KRMIN(v0.x, v1.x) - radius, KRMIN(v0.y, v1.y) - radius, KRMIN(v0.z, v1.z) - radius),
                 KRVector3(KRMAX(v0.x, v1.x) + radius, KRMAX(v0.y, v1.y) + radius, KRMAX(v0.z, v1.z) + radius)
             );

KREngine/kraken/KRMesh.cpp

@@ -1065,9 +1065,7 @@ bool KRMesh::rayCast(const KRVector3 &start, const KRVector3 &dir, KRHitInfo &hi
 bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &v0, const KRVector3 &v1, float radius, KRHitInfo &hitinfo) const
 {
     m_pData->lock();
-    KRHitInfo new_hitinfo;
+
-    KRVector3 dir = KRVector3::Normalize(v1 - v0);
-    
     bool hit_found = false;
     for(int submesh_index=0; submesh_index < getSubmeshCount(); submesh_index++) {
         int vertex_count = getVertexCount(submesh_index);
@@ -1082,7 +1080,13 @@ bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &v0, const
                     
                     KRTriangle3 tri = KRTriangle3(getVertexPosition(tri_vert_index[0]), getVertexPosition(tri_vert_index[1]), getVertexPosition(tri_vert_index[2]));
                     
-                    if(sphereCast(model_to_world, v0, dir, radius, tri, getVertexNormal(tri_vert_index[0]), getVertexNormal(tri_vert_index[1]), getVertexNormal(tri_vert_index[2]), new_hitinfo)) hit_found = true;
+                    if(sphereCast(model_to_world, v0, v1, radius, tri, hitinfo)) hit_found = true;
+                    
+                    /*
+                    KRTriangle3 tri2 = KRTriangle3(getVertexPosition(tri_vert_index[1]), getVertexPosition(tri_vert_index[0]), getVertexPosition(tri_vert_index[2]));
+                    
+                    if(sphereCast(model_to_world, v0, v1, radius, tri2, new_hitinfo)) hit_found = true;
+                    */
                 }
                 break;
                 /*
@@ -1107,21 +1111,14 @@ bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &v0, const
     }
     m_pData->unlock();
     
-    
+    return hit_found;
-    if(hit_found) {
-        if(new_hitinfo.getDistance() <= (v1 - v0).magnitude()) {
-            // The hit was between v1 and v2
-            hitinfo = new_hitinfo;
-            return true;
-        }
-    }
-    return false; // Either no hit, or the hit was beyond v1
 }
 
-bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &start, const KRVector3 &dir, float radius, const KRTriangle3 &tri, const KRVector3 &tri_n0, const KRVector3 &tri_n1, const KRVector3 &tri_n2, KRHitInfo &hitinfo)
+bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &v0, const KRVector3 &v1, float radius, const KRTriangle3 &tri, KRHitInfo &hitinfo)
 {
     
-    // bool sphereCast(const KRVector3 &start, const KRVector3 &dir, float radius, KRVector3 &hit_point, float &hit_distance) const;
+    KRVector3 dir = KRVector3::Normalize(v1 - v0);
+    KRVector3 start = v0;
     
     KRVector3 new_hit_point;
     float new_hit_distance;
@@ -1129,8 +1126,9 @@ bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &start, co
     KRTriangle3 world_tri = KRTriangle3(KRMat4::Dot(model_to_world, tri[0]), KRMat4::Dot(model_to_world, tri[1]), KRMat4::Dot(model_to_world, tri[2]));
     
     if(world_tri.sphereCast(start, dir, radius, new_hit_point, new_hit_distance)) {
-        if(!hitinfo.didHit() || hitinfo.getDistance() > new_hit_distance) {
+        if((!hitinfo.didHit() || hitinfo.getDistance() > new_hit_distance) && new_hit_distance <= (v1 - v0).magnitude()) {
             
+            /*
             // Interpolate between the three vertex normals, performing a 3-way lerp of tri_n0, tri_n1, and tri_n2
             float distance_v0 = (tri[0] - new_hit_point).magnitude();
             float distance_v1 = (tri[1] - new_hit_point).magnitude();
@@ -1140,8 +1138,8 @@ bool KRMesh::sphereCast(const KRMat4 &model_to_world, const KRVector3 &start, co
             distance_v1 /= distance_total;
             distance_v2 /= distance_total;
             KRVector3 normal = KRVector3::Normalize(KRMat4::DotNoTranslate(model_to_world, (tri_n0 * (1.0 - distance_v0) + tri_n1 * (1.0 - distance_v1) + tri_n2 * (1.0 - distance_v2))));
-            
+            */
-            hitinfo = KRHitInfo(new_hit_point, normal, new_hit_distance);
+            hitinfo = KRHitInfo(new_hit_point, world_tri.calculateNormal(), new_hit_distance);
             return true;
         }
     }

KREngine/kraken/KRMesh.h

@@ -220,7 +220,7 @@ private:
     void getSubmeshes();
     
     static bool rayCast(const KRVector3 &start, const KRVector3 &dir, const KRTriangle3 &tri, const KRVector3 &tri_n0, const KRVector3 &tri_n1, const KRVector3 &tri_n2, KRHitInfo &hitinfo);
-    static bool sphereCast(const KRMat4 &model_to_world, const KRVector3 &start, const KRVector3 &dir, float radius, const KRTriangle3 &tri, const KRVector3 &tri_n0, const KRVector3 &tri_n1, const KRVector3 &tri_n2, KRHitInfo &hitinfo);
+    static bool sphereCast(const KRMat4 &model_to_world, const KRVector3 &v0, const KRVector3 &v1, float radius, const KRTriangle3 &tri, KRHitInfo &hitinfo);
     
     int m_lodCoverage; // This LOD level is activated when the bounding box of the model will cover less than this percent of the screen (100 = highest detail model)
     vector<KRMaterial *> m_materials;

KREngine/kraken/KROctreeNode.cpp

@@ -263,6 +263,7 @@ bool KROctreeNode::sphereCast(const KRVector3 &v0, const KRVector3 &v1, float ra
         hit_found = sphereCast(v0, hitinfo.getPosition(), radius, hitinfo, layer_mask);
     } else {
     */
+    
         KRAABB swept_bounds = KRAABB(KRVector3(KRMIN(v0.x, v1.x) - radius, KRMIN(v0.y, v1.y) - radius, KRMIN(v0.z, v1.z) - radius), KRVector3(KRMAX(v0.x, v1.x) + radius, KRMAX(v0.y, v1.y) + radius, KRMAX(v0.z, v1.z) + radius));
         // FINDME, TODO - Investigate AABB - swept sphere intersections or OBB - AABB intersections: "if(getBounds().intersectsSweptSphere(v0, v1, radius)) {"
         if(getBounds().intersects(swept_bounds)) {

KREngine/kraken/KRTriangle3.cpp

@@ -126,24 +126,33 @@ KRVector3 KRTriangle3::calculateNormal() const
     return KRVector3::Normalize(KRVector3::Cross(v1, v2));
 }
 
-bool _intersectSphere(const KRVector3 &rO, const KRVector3 &rV, const KRVector3 &sO, float sR, float &distance)
+bool _intersectSphere(const KRVector3 &start, const KRVector3 &dir, const KRVector3 &sphere_center, float sphere_radius, float &distance)
 {
+    // dir must be normalized
+
     // From: http://archive.gamedev.net/archive/reference/articles/article1026.html
     
     // TODO - Move to another class?
-    KRVector3 Q = sO - rO;
+    KRVector3 Q = sphere_center - start;
     float c = Q.magnitude();
-    float v = KRVector3::Dot(Q, rV);
+    float v = KRVector3::Dot(Q, dir);
-    float d = sR * sR - (c * c - v * v);
+    float d = sphere_radius * sphere_radius - (c * c - v * v);
     
-    // If there was no intersection, return -1
+
     
-    if(d < 0.0) return false;
+    if(d < 0.0) {
+        // No intersection
+        return false;
+    }
     
     // Return the distance to the [first] intersecting point
     
     distance = v - sqrt(d);
+    if(distance < 0.0f) {
+        return false;
+    }
     return true;
+
 }
 
 bool _sameSide(const KRVector3 &p1, const KRVector3 &p2, const KRVector3 &a, const KRVector3 &b)
@@ -196,9 +205,9 @@ KRVector3 KRTriangle3::closestPointOnTriangle(const KRVector3 &p) const
     if(sd_Rab < sd_Rbc && sd_Rab < sd_Rca) {
         return Rab;
     } else if(sd_Rbc < sd_Rab && sd_Rbc < sd_Rca) {
-        return sd_Rbc;
+        return Rbc;
     } else {
-        return sd_Rca;
+        return Rca;
     }
 }
 
@@ -216,6 +225,12 @@ bool KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float
     KRVector3 plane_intersect;
     float plane_intersect_distance;
     
+    float denom = KRVector3::Dot(tri_normal, dir);
+    
+    if(denom > -SMALL_NUM) {
+        return false; // dir is co-planar with the triangle or going in the direction of the normal; no intersection
+    }
+
     // Detect an embedded plane, caused by a sphere that is already intersecting the plane.
     if(cotangent_distance <= 0 && cotangent_distance >= -radius * 2.0f) {
         // Embedded plane - Sphere is already intersecting the plane.
@@ -225,17 +240,15 @@ bool KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float
     } else {
         // Sphere is not intersecting the plane
         // Determine the first point hit by the swept sphere on the triangle's plane
-
-        float denom = KRVector3::Dot(tri_normal, dir);
-        
-        if(fabs(denom) < SMALL_NUM) {
-            return false; // dir is co-planar with the triangle; no intersection
-        }
         
         plane_intersect_distance = -(cotangent_distance / denom);
         plane_intersect = start + dir * plane_intersect_distance;
     }
     
+    if(plane_intersect_distance < 0.0f) {
+        return false;
+    }
+    
     if(containsPoint(plane_intersect)) {
         // Triangle contains point
         hit_point = plane_intersect;
@@ -243,12 +256,12 @@ bool KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float
         return true;
     } else {
         // Triangle does not contain point, cast ray back to sphere from closest point on triangle edge or vertice
-        KRVector3 closest_point = closestPointOnTriangle(hit_point);
+        KRVector3 closest_point = closestPointOnTriangle(plane_intersect);
         float reverse_hit_distance;
         if(_intersectSphere(closest_point, -dir, start, radius, reverse_hit_distance)) {
             // Reverse cast hit sphere
             hit_distance = reverse_hit_distance;
-            hit_point = closest_point - dir * reverse_hit_distance;
+            hit_point = closest_point;
             return true;
         } else {
             // Reverse cast did not hit sphere
@@ -260,6 +273,7 @@ bool KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float
 
 bool KRTriangle3::containsPoint(const KRVector3 &p) const
 {
+    /*
     // From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
     
     const float SMALL_NUM = 0.00000001f;     // anything that avoids division overflow
@@ -272,6 +286,41 @@ bool KRTriangle3::containsPoint(const KRVector3 &p) const
     }
     
     return false;
+    */
+    
+    // From: http://blogs.msdn.com/b/rezanour/archive/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests.aspx
+    
+    KRVector3 A = m_c[0];
+    KRVector3 B = m_c[1];
+    KRVector3 C = m_c[2];
+    KRVector3 P = p;
+    
+    // Prepare our barycentric variables
+    KRVector3 u = B - A;
+    KRVector3 v = C - A;
+    KRVector3 w = P - A;
+    
+    KRVector3 vCrossW = KRVector3::Cross(v, w);
+    KRVector3 vCrossU = KRVector3::Cross(v, u);
+    
+    // Test sign of r
+    if (KRVector3::Dot(vCrossW, vCrossU) < 0)
+        return false;
+    
+    KRVector3 uCrossW = KRVector3::Cross(u, w);
+    KRVector3 uCrossV = KRVector3::Cross(u, v);
+    
+    // Test sign of t
+    if (KRVector3::Dot(uCrossW, uCrossV) < 0)
+        return false;
+    
+    // At this point, we know that r and t and both > 0.
+    // Therefore, as long as their sum is <= 1, each must be less <= 1
+    float denom = uCrossV.magnitude();
+    float r = vCrossW.magnitude() / denom;
+    float t = uCrossW.magnitude() / denom;
+    
+    return (r + t <= 1);
 }