From be804fc3dec708dfaf05d0499e4353c78c6c1565 Mon Sep 17 00:00:00 2001 From: Kearwood Gilbert Date: Mon, 17 Feb 2014 23:36:54 -0800 Subject: [PATCH] SphereCast bug fixes, appears to be working now. More testing needed --HG-- branch : nfb --- KREngine/kraken/KRCollider.cpp | 4 +- KREngine/kraken/KRMesh.cpp | 34 +++++++------- KREngine/kraken/KRMesh.h | 2 +- KREngine/kraken/KROctreeNode.cpp | 1 + KREngine/kraken/KRTriangle3.cpp | 81 +++++++++++++++++++++++++------- 5 files changed, 85 insertions(+), 37 deletions(-) diff --git a/KREngine/kraken/KRCollider.cpp b/KREngine/kraken/KRCollider.cpp index e6ee63f..df0f9a8 100644 --- a/KREngine/kraken/KRCollider.cpp +++ b/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) ); diff --git a/KREngine/kraken/KRMesh.cpp b/KREngine/kraken/KRMesh.cpp index 99e4bcd..4be1af1 100644 --- a/KREngine/kraken/KRMesh.cpp +++ b/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(); - - 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 + return hit_found; } -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; } } diff --git a/KREngine/kraken/KRMesh.h b/KREngine/kraken/KRMesh.h index 745ed3b..b5217a8 100644 --- a/KREngine/kraken/KRMesh.h +++ b/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 m_materials; diff --git a/KREngine/kraken/KROctreeNode.cpp b/KREngine/kraken/KROctreeNode.cpp index 929289f..2b62c8a 100644 --- a/KREngine/kraken/KROctreeNode.cpp +++ b/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)) { diff --git a/KREngine/kraken/KRTriangle3.cpp b/KREngine/kraken/KRTriangle3.cpp index d4d4419..b140802 100644 --- a/KREngine/kraken/KRTriangle3.cpp +++ b/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 d = sR * sR - (c * c - v * v); + float v = KRVector3::Dot(Q, dir); + 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); }