diff --git a/include/bounce/collision/collision.h b/include/bounce/collision/collision.h index 9eda6de..4201dc3 100644 --- a/include/bounce/collision/collision.h +++ b/include/bounce/collision/collision.h @@ -37,4 +37,8 @@ struct b3RayCastOutput b3Vec3 normal; // surface normal of intersection }; +// Perform a ray-cast on a triangle. +bool b3RayCast(b3RayCastOutput* output, const b3RayCastInput* input, + const b3Vec3& v1, const b3Vec3& v2, const b3Vec3& v3); + #endif \ No newline at end of file diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 2866748..90cd13b 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -396,92 +396,19 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 b3Vec3 v2 = m_vertexParticles[triangle->v2]->m_position; b3Vec3 v3 = m_vertexParticles[triangle->v3]->m_position; - b3Vec3 p1 = input->p1; - b3Vec3 p2 = input->p2; - float32 maxFraction = input->maxFraction; - b3Vec3 d = p2 - p1; - B3_ASSERT(b3LengthSquared(d) > B3_EPSILON * B3_EPSILON); - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - float32 len = b3Length(n); - if (len <= B3_EPSILON) - { - return false; - } - - n /= len; - - float32 numerator = b3Dot(n, v1 - p1); - float32 denominator = b3Dot(n, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 fraction = numerator / denominator; - - // Is the intersection not on the segment? - if (fraction < 0.0f || maxFraction < fraction) - { - return false; - } - - b3Vec3 Q = p1 + fraction * d; - - b3Vec3 A = v1; - b3Vec3 B = v2; - b3Vec3 C = v3; - - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - // Barycentric coordinates for Q - float32 u = b3Dot(QB_x_QC, AB_x_AC); - float32 v = b3Dot(QC_x_QA, AB_x_AC); - float32 w = b3Dot(QA_x_QB, AB_x_AC); - - // Is the intersection on the triangle? - if (u >= 0.0f && v >= 0.0f && w >= 0.0f) - { - output->fraction = fraction; - - // Does the ray start from below or above the triangle? - if (numerator > 0.0f) - { - output->normal = -n; - } - else - { - output->normal = n; - } - - return true; - } - - return false; + return b3RayCast(output, input, v1, v2, v3); } void b3Cloth::UpdateBodyContacts() { + B3_PROFILE("Cloth Update Body Contacts"); + // Is there a world attached to this cloth? if (m_world == nullptr) { return; } - B3_PROFILE("Cloth Update Body Contacts"); - // Create contacts for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { @@ -690,11 +617,6 @@ void b3Cloth::UpdateContacts() { // Update body contacts UpdateBodyContacts(); - -#if 0 - // Update particle contacts - UpdateParticleContacts(); -#endif } void b3Cloth::Step(float32 dt) diff --git a/src/bounce/collision/collision.cpp b/src/bounce/collision/collision.cpp index 30fbc81..9246b4f 100644 --- a/src/bounce/collision/collision.cpp +++ b/src/bounce/collision/collision.cpp @@ -16,6 +16,8 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#include + #include #include #include @@ -24,4 +26,90 @@ const b3Sphere b3Sphere_identity(b3Vec3_zero, 1.0f); const b3Capsule b3Capsule_identity(b3Vec3(0.0f, -0.5f, 0.0f), b3Vec3(0.0f, 0.5f, 0.0f), 1.0f); -const b3BoxHull b3BoxHull_identity(1.0f, 1.0f, 1.0f); \ No newline at end of file +const b3BoxHull b3BoxHull_identity(1.0f, 1.0f, 1.0f); + +bool b3RayCast(b3RayCastOutput* output, + const b3RayCastInput* input, + const b3Vec3& v1, const b3Vec3& v2, const b3Vec3& v3) +{ + b3Vec3 p1 = input->p1; + b3Vec3 p2 = input->p2; + float32 maxFraction = input->maxFraction; + + b3Vec3 d = p2 - p1; + + if (b3LengthSquared(d) < B3_EPSILON * B3_EPSILON) + { + return false; + } + + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); + float32 len = b3Length(n); + + if (len == 0.0f) + { + return false; + } + + n /= len; + + float32 num = b3Dot(n, v1 - p1); + float32 den = b3Dot(n, d); + + if (den == 0.0f) + { + return false; + } + + float32 fraction = num / den; + + // Is the intersection not on the segment? + if (fraction < 0.0f || maxFraction < fraction) + { + return false; + } + + b3Vec3 Q = p1 + fraction * d; + + b3Vec3 A = v1; + b3Vec3 B = v2; + b3Vec3 C = v3; + + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + b3Vec3 QC = C - Q; + + b3Vec3 QB_x_QC = b3Cross(QB, QC); + b3Vec3 QC_x_QA = b3Cross(QC, QA); + b3Vec3 QA_x_QB = b3Cross(QA, QB); + + b3Vec3 AB_x_AC = b3Cross(AB, AC); + + // Barycentric coordinates for Q + float32 u = b3Dot(QB_x_QC, AB_x_AC); + float32 v = b3Dot(QC_x_QA, AB_x_AC); + float32 w = b3Dot(QA_x_QB, AB_x_AC); + + // Is the intersection on the triangle? + if (u >= 0.0f && v >= 0.0f && w >= 0.0f) + { + output->fraction = fraction; + + // Does the ray start from below or above the triangle? + if (num > 0.0f) + { + output->normal = -n; + } + else + { + output->normal = n; + } + + return true; + } + + return false; +} diff --git a/src/bounce/dynamics/shapes/mesh_shape.cpp b/src/bounce/dynamics/shapes/mesh_shape.cpp index 4331b1d..99af62f 100644 --- a/src/bounce/dynamics/shapes/mesh_shape.cpp +++ b/src/bounce/dynamics/shapes/mesh_shape.cpp @@ -112,6 +112,7 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, { B3_ASSERT(index < m_mesh->triangleCount); b3Triangle* triangle = m_mesh->triangles + index; + b3Vec3 v1 = m_mesh->vertices[triangle->v1]; b3Vec3 v2 = m_mesh->vertices[triangle->v2]; b3Vec3 v3 = m_mesh->vertices[triangle->v3]; @@ -119,71 +120,17 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, // Put the ray into the mesh's frame of reference. b3Vec3 p1 = b3MulT(xf, input.p1); b3Vec3 p2 = b3MulT(xf, input.p2); - b3Vec3 d = p2 - p1; - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - n.Normalize(); - float32 numerator = b3Dot(n, v1 - p1); - float32 denominator = b3Dot(n, d); + b3RayCastInput subInput; + subInput.p1 = p1; + subInput.p2 = p2; + subInput.maxFraction = input.maxFraction; - if (denominator == 0.0f) + b3RayCastOutput subOutput; + if (b3RayCast(&subOutput, &subInput, v1, v2, v3)) { - return false; - } - - float32 t = numerator / denominator; - - // Is the intersection not on the segment? - if (t < 0.0f || input.maxFraction < t) - { - return false; - } - - b3Vec3 q = p1 + t * d; - - // Barycentric coordinates for q - b3Vec3 Q = q; - b3Vec3 A = v1; - b3Vec3 B = v2; - b3Vec3 C = v3; - - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - float32 u = b3Dot(QB_x_QC, AB_x_AC); - float32 v = b3Dot(QC_x_QA, AB_x_AC); - float32 w = b3Dot(QA_x_QB, AB_x_AC); - - // This tolerance helps intersections lying on - // shared edges to not be missed. - const float32 kTol = -0.005f; - - // Is the intersection on the triangle? - if (u > kTol && v > kTol && w > kTol) - { - output->fraction = t; - - // Does the ray start from below or above the triangle? - if (numerator > 0.0f) - { - output->normal = -b3Mul(xf.rotation, n); - } - else - { - output->normal = b3Mul(xf.rotation, n); - } - + output->fraction = subOutput.fraction; + output->normal = xf.rotation * subOutput.normal; return true; }