feat(physics): add ConvexHull collider with GJK support function
Add ConvexHull struct storing vertices with a support function that returns the farthest point in a given direction, enabling GJK/EPA collision detection. Update all Collider match arms across the physics crate (collision, raycast, integrator, solver) to handle the new variant. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -1,10 +1,37 @@
|
|||||||
use voltex_math::{Vec3, AABB};
|
use voltex_math::{Vec3, AABB};
|
||||||
|
|
||||||
#[derive(Debug, Clone, Copy)]
|
/// A convex hull collider defined by a set of vertices.
|
||||||
|
#[derive(Debug, Clone)]
|
||||||
|
pub struct ConvexHull {
|
||||||
|
pub vertices: Vec<Vec3>,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl ConvexHull {
|
||||||
|
pub fn new(vertices: Vec<Vec3>) -> Self {
|
||||||
|
ConvexHull { vertices }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Support function for GJK: returns the vertex farthest in the given direction.
|
||||||
|
pub fn support(&self, direction: Vec3) -> Vec3 {
|
||||||
|
let mut best = self.vertices[0];
|
||||||
|
let mut best_dot = best.dot(direction);
|
||||||
|
for &v in &self.vertices[1..] {
|
||||||
|
let d = v.dot(direction);
|
||||||
|
if d > best_dot {
|
||||||
|
best_dot = d;
|
||||||
|
best = v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
best
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Debug, Clone)]
|
||||||
pub enum Collider {
|
pub enum Collider {
|
||||||
Sphere { radius: f32 },
|
Sphere { radius: f32 },
|
||||||
Box { half_extents: Vec3 },
|
Box { half_extents: Vec3 },
|
||||||
Capsule { radius: f32, half_height: f32 },
|
Capsule { radius: f32, half_height: f32 },
|
||||||
|
ConvexHull(ConvexHull),
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Collider {
|
impl Collider {
|
||||||
@@ -21,6 +48,16 @@ impl Collider {
|
|||||||
let r = Vec3::new(*radius, *half_height + *radius, *radius);
|
let r = Vec3::new(*radius, *half_height + *radius, *radius);
|
||||||
AABB::new(position - r, position + r)
|
AABB::new(position - r, position + r)
|
||||||
}
|
}
|
||||||
|
Collider::ConvexHull(hull) => {
|
||||||
|
let mut min = position + hull.vertices[0];
|
||||||
|
let mut max = min;
|
||||||
|
for &v in &hull.vertices[1..] {
|
||||||
|
let p = position + v;
|
||||||
|
min = Vec3::new(min.x.min(p.x), min.y.min(p.y), min.z.min(p.z));
|
||||||
|
max = Vec3::new(max.x.max(p.x), max.y.max(p.y), max.z.max(p.z));
|
||||||
|
}
|
||||||
|
AABB::new(min, max)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -52,4 +89,45 @@ mod tests {
|
|||||||
assert_eq!(aabb.min, Vec3::new(0.5, 0.5, 2.5));
|
assert_eq!(aabb.min, Vec3::new(0.5, 0.5, 2.5));
|
||||||
assert_eq!(aabb.max, Vec3::new(1.5, 3.5, 3.5));
|
assert_eq!(aabb.max, Vec3::new(1.5, 3.5, 3.5));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_convex_hull_support() {
|
||||||
|
let hull = ConvexHull::new(vec![
|
||||||
|
Vec3::new(-1.0, -1.0, -1.0),
|
||||||
|
Vec3::new(1.0, -1.0, -1.0),
|
||||||
|
Vec3::new(0.0, 1.0, 0.0),
|
||||||
|
Vec3::new(0.0, -1.0, 1.0),
|
||||||
|
]);
|
||||||
|
// Support in +Y direction should return the top vertex
|
||||||
|
let s = hull.support(Vec3::new(0.0, 1.0, 0.0));
|
||||||
|
assert!((s.y - 1.0).abs() < 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_convex_hull_support_negative() {
|
||||||
|
let hull = ConvexHull::new(vec![
|
||||||
|
Vec3::new(0.0, 0.0, 0.0),
|
||||||
|
Vec3::new(1.0, 0.0, 0.0),
|
||||||
|
Vec3::new(0.0, 1.0, 0.0),
|
||||||
|
]);
|
||||||
|
let s = hull.support(Vec3::new(-1.0, 0.0, 0.0));
|
||||||
|
assert!((s.x - 0.0).abs() < 1e-6); // origin is farthest in -X
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_convex_hull_in_collider() {
|
||||||
|
// Test that ConvexHull variant works with existing collision system
|
||||||
|
let hull = ConvexHull::new(vec![
|
||||||
|
Vec3::new(-1.0, -1.0, -1.0),
|
||||||
|
Vec3::new(1.0, -1.0, -1.0),
|
||||||
|
Vec3::new(1.0, 1.0, -1.0),
|
||||||
|
Vec3::new(-1.0, 1.0, -1.0),
|
||||||
|
Vec3::new(-1.0, -1.0, 1.0),
|
||||||
|
Vec3::new(1.0, -1.0, 1.0),
|
||||||
|
Vec3::new(1.0, 1.0, 1.0),
|
||||||
|
Vec3::new(-1.0, 1.0, 1.0),
|
||||||
|
]);
|
||||||
|
// Just verify construction works
|
||||||
|
assert_eq!(hull.vertices.len(), 8);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -13,7 +13,7 @@ pub fn detect_collisions(world: &World) -> Vec<ContactPoint> {
|
|||||||
let pairs_data: Vec<(Entity, Vec3, Collider)> = world
|
let pairs_data: Vec<(Entity, Vec3, Collider)> = world
|
||||||
.query2::<Transform, Collider>()
|
.query2::<Transform, Collider>()
|
||||||
.into_iter()
|
.into_iter()
|
||||||
.map(|(e, t, c)| (e, t.position, *c))
|
.map(|(e, t, c)| (e, t.position, c.clone()))
|
||||||
.collect();
|
.collect();
|
||||||
|
|
||||||
if pairs_data.len() < 2 {
|
if pairs_data.len() < 2 {
|
||||||
@@ -34,7 +34,7 @@ pub fn detect_collisions(world: &World) -> Vec<ContactPoint> {
|
|||||||
let mut contacts = Vec::new();
|
let mut contacts = Vec::new();
|
||||||
|
|
||||||
let lookup = |entity: Entity| -> Option<(Vec3, Collider)> {
|
let lookup = |entity: Entity| -> Option<(Vec3, Collider)> {
|
||||||
pairs_data.iter().find(|(e, _, _)| *e == entity).map(|(_, p, c)| (*p, *c))
|
pairs_data.iter().find(|(e, _, _)| *e == entity).map(|(_, p, c)| (*p, c.clone()))
|
||||||
};
|
};
|
||||||
|
|
||||||
for (ea, eb) in broad_pairs {
|
for (ea, eb) in broad_pairs {
|
||||||
@@ -55,8 +55,9 @@ pub fn detect_collisions(world: &World) -> Vec<ContactPoint> {
|
|||||||
(Collider::Box { half_extents: ha }, Collider::Box { half_extents: hb }) => {
|
(Collider::Box { half_extents: ha }, Collider::Box { half_extents: hb }) => {
|
||||||
narrow::box_vs_box(pos_a, *ha, pos_b, *hb)
|
narrow::box_vs_box(pos_a, *ha, pos_b, *hb)
|
||||||
}
|
}
|
||||||
// Any combination involving Capsule uses GJK/EPA
|
// Any combination involving Capsule or ConvexHull uses GJK/EPA
|
||||||
(Collider::Capsule { .. }, _) | (_, Collider::Capsule { .. }) => {
|
(Collider::Capsule { .. }, _) | (_, Collider::Capsule { .. })
|
||||||
|
| (Collider::ConvexHull(_), _) | (_, Collider::ConvexHull(_)) => {
|
||||||
gjk::gjk_epa(&col_a, pos_a, &col_b, pos_b)
|
gjk::gjk_epa(&col_a, pos_a, &col_b, pos_b)
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -30,6 +30,9 @@ fn support(collider: &Collider, position: Vec3, direction: Vec3) -> Vec3 {
|
|||||||
}
|
}
|
||||||
base + direction * (*radius / len)
|
base + direction * (*radius / len)
|
||||||
}
|
}
|
||||||
|
Collider::ConvexHull(hull) => {
|
||||||
|
position + hull.support(direction)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -32,6 +32,25 @@ pub fn inertia_tensor(collider: &Collider, mass: f32) -> Vec3 {
|
|||||||
let iz = ix;
|
let iz = ix;
|
||||||
Vec3::new(ix, iy, iz)
|
Vec3::new(ix, iy, iz)
|
||||||
}
|
}
|
||||||
|
Collider::ConvexHull(hull) => {
|
||||||
|
// Approximate using AABB of the hull vertices
|
||||||
|
let mut min = hull.vertices[0];
|
||||||
|
let mut max = min;
|
||||||
|
for &v in &hull.vertices[1..] {
|
||||||
|
min = Vec3::new(min.x.min(v.x), min.y.min(v.y), min.z.min(v.z));
|
||||||
|
max = Vec3::new(max.x.max(v.x), max.y.max(v.y), max.z.max(v.z));
|
||||||
|
}
|
||||||
|
let size = max - min;
|
||||||
|
let w = size.x;
|
||||||
|
let h = size.y;
|
||||||
|
let d = size.z;
|
||||||
|
let factor = mass / 12.0;
|
||||||
|
Vec3::new(
|
||||||
|
factor * (h * h + d * d),
|
||||||
|
factor * (w * w + d * d),
|
||||||
|
factor * (w * w + h * h),
|
||||||
|
)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ pub fn raycast(world: &World, ray: &Ray, max_dist: f32) -> Option<RayHit> {
|
|||||||
let entities: Vec<(Entity, Vec3, Collider)> = world
|
let entities: Vec<(Entity, Vec3, Collider)> = world
|
||||||
.query2::<Transform, Collider>()
|
.query2::<Transform, Collider>()
|
||||||
.into_iter()
|
.into_iter()
|
||||||
.map(|(e, t, c)| (e, t.position, *c))
|
.map(|(e, t, c)| (e, t.position, c.clone()))
|
||||||
.collect();
|
.collect();
|
||||||
|
|
||||||
if entities.is_empty() {
|
if entities.is_empty() {
|
||||||
@@ -53,6 +53,11 @@ pub fn raycast(world: &World, ray: &Ray, max_dist: f32) -> Option<RayHit> {
|
|||||||
Collider::Capsule { radius, half_height } => {
|
Collider::Capsule { radius, half_height } => {
|
||||||
ray_tests::ray_vs_capsule(ray, *pos, *radius, *half_height)
|
ray_tests::ray_vs_capsule(ray, *pos, *radius, *half_height)
|
||||||
}
|
}
|
||||||
|
Collider::ConvexHull(_) => {
|
||||||
|
// Use AABB test as approximation for convex hull raycasting
|
||||||
|
let aabb = collider.aabb(*pos);
|
||||||
|
ray_tests::ray_vs_aabb(ray, &aabb).map(|t| (t, Vec3::Y))
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
if let Some((t, normal)) = result {
|
if let Some((t, normal)) = result {
|
||||||
@@ -77,7 +82,7 @@ pub fn raycast_all(world: &World, ray: &Ray, max_dist: f32) -> Vec<RayHit> {
|
|||||||
let entities: Vec<(Entity, Vec3, Collider)> = world
|
let entities: Vec<(Entity, Vec3, Collider)> = world
|
||||||
.query2::<Transform, Collider>()
|
.query2::<Transform, Collider>()
|
||||||
.into_iter()
|
.into_iter()
|
||||||
.map(|(e, t, c)| (e, t.position, *c))
|
.map(|(e, t, c)| (e, t.position, c.clone()))
|
||||||
.collect();
|
.collect();
|
||||||
|
|
||||||
if entities.is_empty() {
|
if entities.is_empty() {
|
||||||
@@ -106,6 +111,11 @@ pub fn raycast_all(world: &World, ray: &Ray, max_dist: f32) -> Vec<RayHit> {
|
|||||||
Collider::Capsule { radius, half_height } => {
|
Collider::Capsule { radius, half_height } => {
|
||||||
ray_tests::ray_vs_capsule(ray, *pos, *radius, *half_height)
|
ray_tests::ray_vs_capsule(ray, *pos, *radius, *half_height)
|
||||||
}
|
}
|
||||||
|
Collider::ConvexHull(_) => {
|
||||||
|
// Use AABB test as approximation for convex hull raycasting
|
||||||
|
let aabb = collider.aabb(*pos);
|
||||||
|
ray_tests::ray_vs_aabb(ray, &aabb).map(|t| (t, Vec3::Y))
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
if let Some((t, normal)) = result {
|
if let Some((t, normal)) = result {
|
||||||
|
|||||||
@@ -23,8 +23,8 @@ pub fn resolve_collisions(world: &mut World, contacts: &[ContactPoint], iteratio
|
|||||||
for contact in contacts {
|
for contact in contacts {
|
||||||
let rb_a = world.get::<RigidBody>(contact.entity_a).copied();
|
let rb_a = world.get::<RigidBody>(contact.entity_a).copied();
|
||||||
let rb_b = world.get::<RigidBody>(contact.entity_b).copied();
|
let rb_b = world.get::<RigidBody>(contact.entity_b).copied();
|
||||||
let col_a = world.get::<Collider>(contact.entity_a).copied();
|
let col_a = world.get::<Collider>(contact.entity_a).cloned();
|
||||||
let col_b = world.get::<Collider>(contact.entity_b).copied();
|
let col_b = world.get::<Collider>(contact.entity_b).cloned();
|
||||||
let pos_a = world.get::<Transform>(contact.entity_a).map(|t| t.position);
|
let pos_a = world.get::<Transform>(contact.entity_a).map(|t| t.position);
|
||||||
let pos_b = world.get::<Transform>(contact.entity_b).map(|t| t.position);
|
let pos_b = world.get::<Transform>(contact.entity_b).map(|t| t.position);
|
||||||
|
|
||||||
@@ -205,7 +205,7 @@ fn apply_ccd(world: &mut World, config: &PhysicsConfig) {
|
|||||||
.query3::<Transform, RigidBody, Collider>()
|
.query3::<Transform, RigidBody, Collider>()
|
||||||
.into_iter()
|
.into_iter()
|
||||||
.filter(|(_, _, rb, _)| !rb.is_static() && !rb.is_sleeping)
|
.filter(|(_, _, rb, _)| !rb.is_static() && !rb.is_sleeping)
|
||||||
.map(|(e, t, rb, c)| (e, t.position, rb.velocity, *c))
|
.map(|(e, t, rb, c)| (e, t.position, rb.velocity, c.clone()))
|
||||||
.collect();
|
.collect();
|
||||||
|
|
||||||
let all_colliders: Vec<(Entity, voltex_math::AABB)> = world
|
let all_colliders: Vec<(Entity, voltex_math::AABB)> = world
|
||||||
@@ -222,6 +222,10 @@ fn apply_ccd(world: &mut World, config: &PhysicsConfig) {
|
|||||||
Collider::Sphere { radius } => *radius,
|
Collider::Sphere { radius } => *radius,
|
||||||
Collider::Box { half_extents } => half_extents.x.min(half_extents.y).min(half_extents.z),
|
Collider::Box { half_extents } => half_extents.x.min(half_extents.y).min(half_extents.z),
|
||||||
Collider::Capsule { radius, .. } => *radius,
|
Collider::Capsule { radius, .. } => *radius,
|
||||||
|
Collider::ConvexHull(hull) => {
|
||||||
|
// Use minimum distance from origin to any vertex as approximate radius
|
||||||
|
hull.vertices.iter().map(|v| v.length()).fold(f32::MAX, f32::min)
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
// Only apply CCD if displacement > collider radius
|
// Only apply CCD if displacement > collider radius
|
||||||
|
|||||||
Reference in New Issue
Block a user