use super::*;
impl Gjk {
pub fn support_point(
shape: &ColliderShape,
pos: Vec3,
rot: gizmo_math::Quat,
dir: Vec3,
) -> Vec3 {
let local_dir = rot.inverse() * dir;
let local_support = match shape {
ColliderShape::Sphere(s) => Self::sphere_support(s, local_dir),
ColliderShape::Box(b) => Self::box_support(b, local_dir),
ColliderShape::Capsule(c) => Self::capsule_support(c, local_dir),
ColliderShape::Plane(_) => {
debug_assert!(false, "Plane shapes must use separate collision detection");
Vec3::ZERO
}
ColliderShape::TriMesh(tm) => {
let mut best_dot = f32::NEG_INFINITY;
let mut best_pt = Vec3::ZERO;
if !tm.bvh.nodes.is_empty() {
let mut stack = Vec::with_capacity(64);
stack.push(0);
let abs_dir = gizmo_math::Vec3A::new(
local_dir.x.abs(),
local_dir.y.abs(),
local_dir.z.abs(),
);
let dir_a = gizmo_math::Vec3A::new(local_dir.x, local_dir.y, local_dir.z);
while let Some(node_idx) = stack.pop() {
let node = &tm.bvh.nodes[node_idx];
let center = node.aabb.center();
let half_extents = node.aabb.half_extents();
let max_node_dot = center.dot(dir_a)
+ half_extents.x * abs_dir.x
+ half_extents.y * abs_dir.y
+ half_extents.z * abs_dir.z;
if max_node_dot < best_dot {
continue;
}
if node.is_leaf() {
let start = (node.first_tri_index * 3) as usize;
let end = start + (node.tri_count * 3) as usize;
for i in start..end {
let v = tm.vertices[tm.indices[i] as usize];
let d = v.dot(local_dir);
if d > best_dot {
best_dot = d;
best_pt = v;
}
}
} else {
if node.left_child >= 0 {
stack.push(node.left_child as usize);
}
if node.right_child >= 0 {
stack.push(node.right_child as usize);
}
}
}
} else {
for v in tm.vertices.iter() {
let d = v.dot(local_dir);
if d > best_dot {
best_dot = d;
best_pt = *v;
}
}
}
best_pt
}
ColliderShape::ConvexHull(ch) => {
let mut best_dot = f32::NEG_INFINITY;
let mut best_pt = Vec3::ZERO;
for v in ch.vertices.iter() {
let d = v.dot(local_dir);
if d > best_dot {
best_dot = d;
best_pt = *v;
}
}
best_pt
}
crate::components::ColliderShape::Compound(_) => {
debug_assert!(
false,
"Compound shapes must use separate collision detection"
);
Vec3::ZERO
}
};
pos + rot * local_support
}
fn sphere_support(sphere: &SphereShape, dir: Vec3) -> Vec3 {
dir.try_normalize().unwrap_or(Vec3::X) * sphere.radius
}
fn box_support(box_shape: &BoxShape, dir: Vec3) -> Vec3 {
Vec3::new(
if dir.x > 0.0 {
box_shape.half_extents.x
} else {
-box_shape.half_extents.x
},
if dir.y > 0.0 {
box_shape.half_extents.y
} else {
-box_shape.half_extents.y
},
if dir.z > 0.0 {
box_shape.half_extents.z
} else {
-box_shape.half_extents.z
},
)
}
fn capsule_support(capsule: &CapsuleShape, dir: Vec3) -> Vec3 {
let dir_normalized = dir.try_normalize().unwrap_or(Vec3::X);
let sphere_center = if dir_normalized.y > 0.0 {
Vec3::new(0.0, capsule.half_height, 0.0)
} else {
Vec3::new(0.0, -capsule.half_height, 0.0)
};
sphere_center + dir_normalized * capsule.radius
}
}