diff --git a/include/bounce/dynamics/shapes/capsule_shape.h b/include/bounce/dynamics/shapes/capsule_shape.h index eb0f500..579e48f 100644 --- a/include/bounce/dynamics/shapes/capsule_shape.h +++ b/include/bounce/dynamics/shapes/capsule_shape.h @@ -33,7 +33,9 @@ public: void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const; - bool TestPoint(const b3Vec3& point, const b3Transform& xf) const; + bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const; + + bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const; bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const; diff --git a/include/bounce/dynamics/shapes/hull_shape.h b/include/bounce/dynamics/shapes/hull_shape.h index 9424541..193798f 100644 --- a/include/bounce/dynamics/shapes/hull_shape.h +++ b/include/bounce/dynamics/shapes/hull_shape.h @@ -35,7 +35,9 @@ public : void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const; - bool TestPoint(const b3Vec3& point, const b3Transform& xf) const; + bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const; + + bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const; bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const; diff --git a/include/bounce/dynamics/shapes/mesh_shape.h b/include/bounce/dynamics/shapes/mesh_shape.h index 8c67769..c8215e6 100644 --- a/include/bounce/dynamics/shapes/mesh_shape.h +++ b/include/bounce/dynamics/shapes/mesh_shape.h @@ -37,8 +37,10 @@ public: void ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 childIndex) const; - bool TestPoint(const b3Vec3& point, const b3Transform& xf) const; + bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const; + bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const; + bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const; bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf, u32 childIndex) const; diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index 29d89b2..a34360b 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -22,6 +22,7 @@ #include #include #include +#include struct b3ContactEdge; @@ -37,6 +38,12 @@ enum b3ShapeType e_maxShapes }; +struct b3TestSphereOutput +{ + float32 separation; + b3Vec3 normal; +}; + struct b3ShapeDef { b3ShapeDef() @@ -72,7 +79,7 @@ struct b3MassData class b3Shape { public: - b3Shape() { } + b3Shape(); virtual ~b3Shape() { } // Get the shape type. @@ -88,8 +95,13 @@ public: // Compute the shape world AABB. virtual void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const = 0; - // Test if a point is contained inside this shape. - virtual bool TestPoint(const b3Vec3& point, const b3Transform& xf) const = 0; + // Test if a sphere is contained inside this shape. + virtual bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const = 0; + + // Test if a sphere is contained inside this shape. + // If the sphere is inside this shape then return the minimum separation distance and normal. + // The direction of the normal points from this shape to the sphere. + virtual bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const = 0; // Compute the ray intersection point, normal of surface, and fraction. virtual bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const = 0; diff --git a/include/bounce/dynamics/shapes/sphere_shape.h b/include/bounce/dynamics/shapes/sphere_shape.h index 6960601..be5514b 100644 --- a/include/bounce/dynamics/shapes/sphere_shape.h +++ b/include/bounce/dynamics/shapes/sphere_shape.h @@ -33,8 +33,10 @@ public : void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const; - bool TestPoint(const b3Vec3& point, const b3Transform& xf) const; + bool TestSphere(const b3Sphere& sphere, const b3Transform& xf) const; + bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const; + bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const; b3Vec3 m_center; diff --git a/src/bounce/dynamics/shapes/capsule_shape.cpp b/src/bounce/dynamics/shapes/capsule_shape.cpp index 5373b11..ade563d 100644 --- a/src/bounce/dynamics/shapes/capsule_shape.cpp +++ b/src/bounce/dynamics/shapes/capsule_shape.cpp @@ -140,10 +140,10 @@ void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const aabb->m_upper = b3Max(c1, c2) + r; } -bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const +bool b3CapsuleShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const { // The point in the frame of the capsule - b3Vec3 Q = b3MulT(xf, point); + b3Vec3 Q = b3MulT(xf, sphere.vertex); b3Vec3 A = m_centers[0]; b3Vec3 B = m_centers[1]; @@ -153,10 +153,12 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const float32 u = b3Dot(B - Q, AB); float32 v = b3Dot(Q - A, AB); + float32 radius = m_radius + sphere.radius; + if (v <= 0.0f) { // A - if (b3DistanceSquared(A, Q) > m_radius * m_radius) + if (b3DistanceSquared(A, Q) > radius * radius) { return false; } @@ -166,7 +168,7 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const if (u <= 0.0f) { // B - if (b3DistanceSquared(B, Q) > m_radius * m_radius) + if (b3DistanceSquared(B, Q) > radius * radius) { return false; } @@ -177,7 +179,7 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const float32 s = b3Dot(AB, AB); B3_ASSERT(s > 0.0f); b3Vec3 P = (1.0f / s) * (u * A + v * B); - if (b3DistanceSquared(P, Q) > m_radius * m_radius) + if (b3DistanceSquared(P, Q) > radius * radius) { return false; } @@ -185,6 +187,102 @@ bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const return true; } +bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const +{ + b3Vec3 Q = sphere.vertex; + + b3Vec3 A = b3Mul(xf, m_centers[0]); + b3Vec3 B = b3Mul(xf, m_centers[1]); + b3Vec3 AB = B - A; + + // Barycentric coordinates for Q + float32 u = b3Dot(B - Q, AB); + float32 v = b3Dot(Q - A, AB); + + float32 radius = m_radius + sphere.radius; + + if (v <= 0.0f) + { + // A + b3Vec3 P = A; + b3Vec3 d = Q - P; + float32 dd = b3Dot(d, d); + if (dd > radius * radius) + { + return false; + } + + b3Vec3 n(0.0f, 1.0f, 0.0f); + float32 len = b3Sqrt(dd); + if (len > B3_EPSILON) + { + n = d / len; + } + + output->separation = len - radius; + output->normal = n; + + return true; + } + + if (u <= 0.0f) + { + // B + b3Vec3 P = B; + b3Vec3 d = Q - P; + float32 dd = b3Dot(d, d); + if (dd > radius * radius) + { + return false; + } + + b3Vec3 n(0.0f, 1.0f, 0.0f); + float32 len = b3Sqrt(dd); + if (len > B3_EPSILON) + { + n = d / len; + } + + output->separation = len - radius; + output->normal = n; + + return true; + } + + // AB + float32 s = b3Dot(AB, AB); + //B3_ASSERT(s > 0.0f); + b3Vec3 P; + if (s < B3_LINEAR_SLOP * B3_LINEAR_SLOP) + { + P = A; + } + else + { + P = (u * A + v * B) / s; + } + + b3Vec3 d = Q - P; + float32 dd = b3Dot(d, d); + if (dd > radius * radius) + { + return false; + } + + b3Vec3 QA = A - Q; + b3Vec3 e = b3Cross(AB, QA); + b3Vec3 n = b3Cross(AB, e); + if (b3Dot(n, QA) < 0.0f) + { + n = -n; + } + n.Normalize(); + + output->separation = b3Sqrt(dd) - radius; + output->normal = -n; + return true; +} + bool b3CapsuleShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const { b3Vec3 A = input.p1; diff --git a/src/bounce/dynamics/shapes/hull_shape.cpp b/src/bounce/dynamics/shapes/hull_shape.cpp index 4757b7b..5269eec 100644 --- a/src/bounce/dynamics/shapes/hull_shape.cpp +++ b/src/bounce/dynamics/shapes/hull_shape.cpp @@ -142,21 +142,62 @@ void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const aabb->Extend(m_radius); } -bool b3HullShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const +bool b3HullShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const { - // Put the point into the hull's frame of reference. - b3Vec3 p = b3MulT(xf, point); + b3Vec3 support = b3MulT(xf, sphere.vertex); + float32 radius = m_radius + sphere.radius; + for (u32 i = 0; i < m_hull->faceCount; ++i) { - float32 d = b3Distance(p, m_hull->planes[i]); - if (d > m_radius) + b3Plane plane = m_hull->GetPlane(i); + float32 separation = b3Distance(support, plane); + + if (separation > radius) { return false; } } + return true; } +bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const +{ + // Perform computations in the local space of the first hull. + b3Vec3 support = b3MulT(xf, sphere.vertex); + float32 radius = m_radius + sphere.radius; + + u32 maxIndex = ~0; + float32 maxSeparation = -B3_MAX_FLOAT; + + for (u32 i = 0; i < m_hull->faceCount; ++i) + { + b3Plane plane = m_hull->GetPlane(i); + float32 separation = b3Distance(support, plane); + + if (separation > radius) + { + return false; + } + + if (separation > maxSeparation) + { + maxIndex = i; + maxSeparation = separation; + } + } + + if (maxIndex != ~0) + { + output->separation = maxSeparation; + output->normal = b3Mul(xf.rotation, m_hull->GetPlane(maxIndex).normal); + return true; + } + + B3_ASSERT(false); + return false; +} + bool b3HullShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const { u32 planeCount = m_hull->faceCount; diff --git a/src/bounce/dynamics/shapes/mesh_shape.cpp b/src/bounce/dynamics/shapes/mesh_shape.cpp index 0e4786d..7d2fa9e 100644 --- a/src/bounce/dynamics/shapes/mesh_shape.cpp +++ b/src/bounce/dynamics/shapes/mesh_shape.cpp @@ -73,9 +73,17 @@ void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index) output->Extend(m_radius); } -bool b3MeshShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const +bool b3MeshShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const { - B3_NOT_USED(point); + B3_NOT_USED(sphere); + B3_NOT_USED(xf); + return false; +} + +bool b3MeshShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const +{ + B3_NOT_USED(output); + B3_NOT_USED(sphere); B3_NOT_USED(xf); return false; } diff --git a/src/bounce/dynamics/shapes/shape.cpp b/src/bounce/dynamics/shapes/shape.cpp index e4a7e3d..e849ccd 100644 --- a/src/bounce/dynamics/shapes/shape.cpp +++ b/src/bounce/dynamics/shapes/shape.cpp @@ -29,6 +29,18 @@ #include #include +b3Shape::b3Shape() +{ + m_density = 0.0f; + m_friction = 0.0f; + m_restitution = 0.0f; + + m_isSensor = false; + m_userData = nullptr; + + m_body = nullptr; +} + void b3Shape::SetSensor(bool flag) { if (flag != m_isSensor) diff --git a/src/bounce/dynamics/shapes/sphere_shape.cpp b/src/bounce/dynamics/shapes/sphere_shape.cpp index f36aa50..6fc8d24 100644 --- a/src/bounce/dynamics/shapes/sphere_shape.cpp +++ b/src/bounce/dynamics/shapes/sphere_shape.cpp @@ -58,16 +58,42 @@ void b3SphereShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const aabb->m_upper = center + r; } -bool b3SphereShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const +bool b3SphereShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) const { b3Vec3 center = b3Mul(xf, m_center); - float32 rr = m_radius * m_radius; - b3Vec3 d = point - center; + float32 radius = m_radius + sphere.radius; + float32 rr = radius * radius; + b3Vec3 d = sphere.vertex - center; float32 dd = b3Dot(d, d); return dd <= rr; } -bool b3SphereShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const +bool b3SphereShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const +{ + b3Vec3 center = b3Mul(xf, m_center); + float32 radius = m_radius + sphere.radius; + float32 rr = radius * radius; + b3Vec3 d = sphere.vertex - center; + float32 dd = b3Dot(d, d); + + if (dd <= rr) + { + float32 d_len = b3Sqrt(dd); + + output->separation = d_len - radius; + output->normal.Set(0.0f, 1.0, 0.0f); + if (d_len > B3_EPSILON) + { + output->normal = d / d_len; + } + + return true; + } + + return false; +} + +bool b3SphereShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const { // dot(x - c, x - c) - r^2 = 0 // S = p1 + t * (p2 - p1)