AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add collision detection routines for shape against sphere

Browse Source
This commit is contained in:
Irlan 2018-03-26 15:56:01 -03:00
parent be812ed897
commit 3e55b28956
10 changed files with 228 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/bounce/dynamics/shapes/capsule_shape.h
View File

@ -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;

4
include/bounce/dynamics/shapes/hull_shape.h
View File

@ -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;

4
include/bounce/dynamics/shapes/mesh_shape.h
View File

@ -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;

18
include/bounce/dynamics/shapes/shape.h
View File

@ -22,6 +22,7 @@
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/collision/collision.h>
#include <bounce/collision/shapes/sphere.h>
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;

4
include/bounce/dynamics/shapes/sphere_shape.h
View File

@ -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;

108
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@ -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;

51
src/bounce/dynamics/shapes/hull_shape.cpp
View File

@ -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;

12
src/bounce/dynamics/shapes/mesh_shape.cpp
View File

@ -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;
}

12
src/bounce/dynamics/shapes/shape.cpp
View File

@ -29,6 +29,18 @@
#include <bounce/collision/shapes/hull.h>
#include <bounce/collision/shapes/mesh.h>
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)

34
src/bounce/dynamics/shapes/sphere_shape.cpp
View File

@ -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)