use crate::ecs::lines::components::Line;
use crate::ecs::world::components::Name;
use crate::ecs::world::{
GLOBAL_TRANSFORM, LINES, LOCAL_TRANSFORM, LOCAL_TRANSFORM_DIRTY, NAME, VISIBILITY, World,
};
use nalgebra_glm::{Vec3, Vec4, vec3};
use rapier3d::na::UnitQuaternion;
use std::f32::consts::PI;
const COLLIDER_COLOR: Vec4 = Vec4::new(0.0, 1.0, 0.0, 1.0);
const DYNAMIC_BODY_COLOR: Vec4 = Vec4::new(0.0, 0.5, 1.0, 1.0);
const KINEMATIC_BODY_COLOR: Vec4 = Vec4::new(1.0, 0.5, 0.0, 1.0);
const STATIC_BODY_COLOR: Vec4 = Vec4::new(0.5, 0.5, 0.5, 1.0);
const JOINT_COLOR: Vec4 = Vec4::new(1.0, 1.0, 0.0, 1.0);
const JOINT_ANCHOR_COLOR: Vec4 = Vec4::new(1.0, 0.2, 0.8, 1.0);
const CIRCLE_SEGMENTS: usize = 24;
pub fn physics_debug_draw_system(world: &mut World) {
if !world.resources.physics.debug_draw {
if let Some(entity) = world.resources.physics.debug_entity
&& let Some(visibility) = world.core.get_visibility_mut(entity)
{
visibility.visible = false;
}
return;
}
let debug_entity = match world.resources.physics.debug_entity {
Some(entity) => entity,
None => {
let entity = world.spawn_entities(
LINES
| VISIBILITY
| LOCAL_TRANSFORM
| GLOBAL_TRANSFORM
| LOCAL_TRANSFORM_DIRTY
| NAME,
1,
)[0];
world
.core
.set_name(entity, Name("Physics Debug Gizmo".to_string()));
world.resources.physics.debug_entity = Some(entity);
entity
}
};
if let Some(visibility) = world.core.get_visibility_mut(debug_entity) {
visibility.visible = true;
}
let current_version = world.resources.physics.debug_version;
let version_changed = current_version != world.resources.physics.debug_last_version;
if version_changed {
world.resources.physics.debug_static_lines_cache.clear();
world.resources.physics.debug_last_version = current_version;
}
let mut has_dynamic_or_kinematic = false;
let mut all_static_cached = !version_changed;
if all_static_cached {
for (collider_handle, collider) in world.resources.physics.collider_set.iter() {
let parent_handle = collider.parent();
let body_type = parent_handle
.and_then(|h| world.resources.physics.rigid_body_set.get(h))
.map(|rb| rb.body_type());
let is_static = matches!(body_type, Some(rapier3d::prelude::RigidBodyType::Fixed));
if !is_static {
has_dynamic_or_kinematic = true;
break;
}
if !world
.resources
.physics
.debug_static_lines_cache
.contains_key(&collider_handle)
{
all_static_cached = false;
break;
}
}
} else {
for (_collider_handle, collider) in world.resources.physics.collider_set.iter() {
let parent_handle = collider.parent();
let body_type = parent_handle
.and_then(|h| world.resources.physics.rigid_body_set.get(h))
.map(|rb| rb.body_type());
if !matches!(body_type, Some(rapier3d::prelude::RigidBodyType::Fixed)) {
has_dynamic_or_kinematic = true;
break;
}
}
}
if all_static_cached && !has_dynamic_or_kinematic {
return;
}
let mut lines = Vec::new();
let mut static_colliders_to_cache = Vec::new();
for (collider_handle, collider) in world.resources.physics.collider_set.iter() {
let parent_handle = collider.parent();
let body_type = parent_handle
.and_then(|h| world.resources.physics.rigid_body_set.get(h))
.map(|rb| rb.body_type());
let is_static = matches!(body_type, Some(rapier3d::prelude::RigidBodyType::Fixed));
if is_static
&& let Some(cached_lines) = world
.resources
.physics
.debug_static_lines_cache
.get(&collider_handle)
{
lines.extend_from_slice(cached_lines);
continue;
}
let color = match body_type {
Some(rapier3d::prelude::RigidBodyType::Dynamic) => DYNAMIC_BODY_COLOR,
Some(rapier3d::prelude::RigidBodyType::KinematicPositionBased)
| Some(rapier3d::prelude::RigidBodyType::KinematicVelocityBased) => {
KINEMATIC_BODY_COLOR
}
Some(rapier3d::prelude::RigidBodyType::Fixed) => STATIC_BODY_COLOR,
None => COLLIDER_COLOR,
};
let position = collider.position();
let translation = vec3(
position.translation.x,
position.translation.y,
position.translation.z,
);
let rotation = position.rotation;
let start_index = lines.len();
generate_collider_lines(&mut lines, collider.shape(), translation, rotation, color);
if is_static {
let collider_lines: Vec<Line> = lines[start_index..].to_vec();
static_colliders_to_cache.push((collider_handle, collider_lines));
}
}
for (handle, collider_lines) in static_colliders_to_cache {
world
.resources
.physics
.debug_static_lines_cache
.insert(handle, collider_lines);
}
generate_joint_lines(&mut lines, world);
if let Some(lines_component) = world.core.get_lines_mut(debug_entity) {
lines_component.lines = lines;
lines_component.mark_dirty();
}
}
fn generate_joint_lines(lines: &mut Vec<Line>, world: &World) {
for (_handle, joint) in world.resources.physics.impulse_joint_set.iter() {
let body1_handle = joint.body1;
let body2_handle = joint.body2;
let Some(body1) = world.resources.physics.rigid_body_set.get(body1_handle) else {
continue;
};
let Some(body2) = world.resources.physics.rigid_body_set.get(body2_handle) else {
continue;
};
let body1_pos = body1.position();
let body2_pos = body2.position();
let anchor1_local = joint.data.local_anchor1();
let anchor2_local = joint.data.local_anchor2();
let anchor1_world = body1_pos * anchor1_local;
let anchor2_world = body2_pos * anchor2_local;
let anchor1 = vec3(anchor1_world.x, anchor1_world.y, anchor1_world.z);
let anchor2 = vec3(anchor2_world.x, anchor2_world.y, anchor2_world.z);
lines.push(Line {
start: anchor1,
end: anchor2,
color: JOINT_COLOR,
});
let body1_center = vec3(
body1_pos.translation.x,
body1_pos.translation.y,
body1_pos.translation.z,
);
let body2_center = vec3(
body2_pos.translation.x,
body2_pos.translation.y,
body2_pos.translation.z,
);
lines.push(Line {
start: body1_center,
end: anchor1,
color: JOINT_ANCHOR_COLOR,
});
lines.push(Line {
start: body2_center,
end: anchor2,
color: JOINT_ANCHOR_COLOR,
});
generate_joint_anchor_marker(lines, anchor1, JOINT_ANCHOR_COLOR);
generate_joint_anchor_marker(lines, anchor2, JOINT_ANCHOR_COLOR);
}
}
fn generate_joint_anchor_marker(lines: &mut Vec<Line>, position: Vec3, color: Vec4) {
let size = 0.05;
lines.push(Line {
start: position - vec3(size, 0.0, 0.0),
end: position + vec3(size, 0.0, 0.0),
color,
});
lines.push(Line {
start: position - vec3(0.0, size, 0.0),
end: position + vec3(0.0, size, 0.0),
color,
});
lines.push(Line {
start: position - vec3(0.0, 0.0, size),
end: position + vec3(0.0, 0.0, size),
color,
});
}
fn generate_collider_lines(
lines: &mut Vec<Line>,
shape: &dyn rapier3d::prelude::Shape,
translation: Vec3,
rotation: UnitQuaternion<f32>,
color: Vec4,
) {
if let Some(ball) = shape.as_ball() {
generate_sphere_lines(lines, translation, rotation, ball.radius, color);
} else if let Some(cuboid) = shape.as_cuboid() {
generate_cuboid_lines(lines, translation, rotation, cuboid.half_extents, color);
} else if let Some(capsule) = shape.as_capsule() {
generate_capsule_lines(
lines,
translation,
rotation,
capsule.half_height(),
capsule.radius,
color,
);
} else if let Some(cylinder) = shape.as_cylinder() {
generate_cylinder_lines(
lines,
translation,
rotation,
cylinder.half_height,
cylinder.radius,
color,
);
} else if let Some(cone) = shape.as_cone() {
generate_cone_lines(
lines,
translation,
rotation,
cone.half_height,
cone.radius,
color,
);
} else if let Some(trimesh) = shape.as_trimesh() {
generate_trimesh_lines(lines, translation, rotation, trimesh, color);
} else if let Some(convex) = shape.as_convex_polyhedron() {
generate_convex_lines(lines, translation, rotation, convex, color);
} else if let Some(heightfield) = shape.as_heightfield() {
generate_heightfield_lines(lines, translation, rotation, heightfield, color);
}
}
fn rotate_point(point: Vec3, rotation: UnitQuaternion<f32>) -> Vec3 {
let rotated = rotation * rapier3d::prelude::Point::new(point.x, point.y, point.z);
vec3(rotated.x, rotated.y, rotated.z)
}
fn transform_point(point: Vec3, translation: Vec3, rotation: UnitQuaternion<f32>) -> Vec3 {
rotate_point(point, rotation) + translation
}
fn generate_sphere_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
radius: f32,
color: Vec4,
) {
generate_circle_lines(
lines,
translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 1.0, 0.0),
color,
);
generate_circle_lines(
lines,
translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
generate_circle_lines(
lines,
translation,
rotation,
radius,
vec3(0.0, 1.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
}
fn generate_circle_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
radius: f32,
axis1: Vec3,
axis2: Vec3,
color: Vec4,
) {
for segment_index in 0..CIRCLE_SEGMENTS {
let angle1 = (segment_index as f32 / CIRCLE_SEGMENTS as f32) * 2.0 * PI;
let angle2 = ((segment_index + 1) as f32 / CIRCLE_SEGMENTS as f32) * 2.0 * PI;
let local_point1 = axis1 * angle1.cos() * radius + axis2 * angle1.sin() * radius;
let local_point2 = axis1 * angle2.cos() * radius + axis2 * angle2.sin() * radius;
let point1 = transform_point(local_point1, translation, rotation);
let point2 = transform_point(local_point2, translation, rotation);
lines.push(Line {
start: point1,
end: point2,
color,
});
}
}
fn generate_cuboid_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
half_extents: rapier3d::prelude::Vector<f32>,
color: Vec4,
) {
let hx = half_extents.x;
let hy = half_extents.y;
let hz = half_extents.z;
let corners = [
vec3(-hx, -hy, -hz),
vec3(hx, -hy, -hz),
vec3(hx, hy, -hz),
vec3(-hx, hy, -hz),
vec3(-hx, -hy, hz),
vec3(hx, -hy, hz),
vec3(hx, hy, hz),
vec3(-hx, hy, hz),
];
let edges = [
(0, 1),
(1, 2),
(2, 3),
(3, 0),
(4, 5),
(5, 6),
(6, 7),
(7, 4),
(0, 4),
(1, 5),
(2, 6),
(3, 7),
];
for (start_index, end_index) in edges {
let start = transform_point(corners[start_index], translation, rotation);
let end = transform_point(corners[end_index], translation, rotation);
lines.push(Line { start, end, color });
}
}
fn generate_capsule_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
half_height: f32,
radius: f32,
color: Vec4,
) {
let top_center = vec3(0.0, half_height, 0.0);
let bottom_center = vec3(0.0, -half_height, 0.0);
let top_translation = translation + rotate_point(top_center, rotation);
let bottom_translation = translation + rotate_point(bottom_center, rotation);
generate_circle_lines(
lines,
top_translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
generate_circle_lines(
lines,
bottom_translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
for segment_index in 0..4 {
let angle = (segment_index as f32 / 4.0) * 2.0 * PI;
let offset = vec3(angle.cos() * radius, 0.0, angle.sin() * radius);
let top = transform_point(top_center + offset, translation, rotation);
let bottom = transform_point(bottom_center + offset, translation, rotation);
lines.push(Line {
start: top,
end: bottom,
color,
});
}
generate_hemisphere_lines(lines, top_translation, rotation, radius, true, color);
generate_hemisphere_lines(lines, bottom_translation, rotation, radius, false, color);
}
fn generate_hemisphere_lines(
lines: &mut Vec<Line>,
center: Vec3,
rotation: UnitQuaternion<f32>,
radius: f32,
top: bool,
color: Vec4,
) {
let arc_segments = CIRCLE_SEGMENTS / 4;
let direction = if top { 1.0 } else { -1.0 };
for arc_index in 0..2 {
let base_angle = arc_index as f32 * PI / 2.0;
for segment_index in 0..arc_segments {
let phi1 = (segment_index as f32 / arc_segments as f32) * (PI / 2.0);
let phi2 = ((segment_index + 1) as f32 / arc_segments as f32) * (PI / 2.0);
let local_point1 = vec3(
phi1.cos() * base_angle.cos() * radius,
phi1.sin() * radius * direction,
phi1.cos() * base_angle.sin() * radius,
);
let local_point2 = vec3(
phi2.cos() * base_angle.cos() * radius,
phi2.sin() * radius * direction,
phi2.cos() * base_angle.sin() * radius,
);
let point1 = center + rotate_point(local_point1, rotation);
let point2 = center + rotate_point(local_point2, rotation);
lines.push(Line {
start: point1,
end: point2,
color,
});
}
}
}
fn generate_cylinder_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
half_height: f32,
radius: f32,
color: Vec4,
) {
let top_center = vec3(0.0, half_height, 0.0);
let bottom_center = vec3(0.0, -half_height, 0.0);
let top_translation = translation + rotate_point(top_center, rotation);
let bottom_translation = translation + rotate_point(bottom_center, rotation);
generate_circle_lines(
lines,
top_translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
generate_circle_lines(
lines,
bottom_translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
for segment_index in 0..4 {
let angle = (segment_index as f32 / 4.0) * 2.0 * PI;
let offset = vec3(angle.cos() * radius, 0.0, angle.sin() * radius);
let top = transform_point(top_center + offset, translation, rotation);
let bottom = transform_point(bottom_center + offset, translation, rotation);
lines.push(Line {
start: top,
end: bottom,
color,
});
}
}
fn generate_cone_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
half_height: f32,
radius: f32,
color: Vec4,
) {
let apex = vec3(0.0, half_height, 0.0);
let base_center = vec3(0.0, -half_height, 0.0);
let base_translation = translation + rotate_point(base_center, rotation);
generate_circle_lines(
lines,
base_translation,
rotation,
radius,
vec3(1.0, 0.0, 0.0),
vec3(0.0, 0.0, 1.0),
color,
);
let apex_world = transform_point(apex, translation, rotation);
for segment_index in 0..4 {
let angle = (segment_index as f32 / 4.0) * 2.0 * PI;
let base_point = vec3(angle.cos() * radius, -half_height, angle.sin() * radius);
let base_world = transform_point(base_point, translation, rotation);
lines.push(Line {
start: apex_world,
end: base_world,
color,
});
}
}
fn generate_trimesh_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
trimesh: &rapier3d::prelude::TriMesh,
color: Vec4,
) {
for triangle_index in 0..trimesh.num_triangles() {
let triangle = trimesh.triangle(triangle_index as u32);
let vertex_a = transform_point(
vec3(triangle.a.x, triangle.a.y, triangle.a.z),
translation,
rotation,
);
let vertex_b = transform_point(
vec3(triangle.b.x, triangle.b.y, triangle.b.z),
translation,
rotation,
);
let vertex_c = transform_point(
vec3(triangle.c.x, triangle.c.y, triangle.c.z),
translation,
rotation,
);
lines.push(Line {
start: vertex_a,
end: vertex_b,
color,
});
lines.push(Line {
start: vertex_b,
end: vertex_c,
color,
});
lines.push(Line {
start: vertex_c,
end: vertex_a,
color,
});
}
}
fn generate_convex_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
convex: &rapier3d::prelude::ConvexPolyhedron,
color: Vec4,
) {
let vertices = convex.points();
for face_index in 0..convex.faces().len() {
let face = &convex.faces()[face_index];
let first_vertex_index = face.first_vertex_or_edge as usize;
let vertex_count = face.num_vertices_or_edges as usize;
for local_vertex_index in 0..vertex_count {
let current_index =
convex.vertices_adj_to_face()[first_vertex_index + local_vertex_index] as usize;
let next_index = convex.vertices_adj_to_face()
[first_vertex_index + (local_vertex_index + 1) % vertex_count]
as usize;
let current_vertex = &vertices[current_index];
let next_vertex = &vertices[next_index];
let start = transform_point(
vec3(current_vertex.x, current_vertex.y, current_vertex.z),
translation,
rotation,
);
let end = transform_point(
vec3(next_vertex.x, next_vertex.y, next_vertex.z),
translation,
rotation,
);
lines.push(Line { start, end, color });
}
}
}
fn generate_heightfield_lines(
lines: &mut Vec<Line>,
translation: Vec3,
rotation: UnitQuaternion<f32>,
heightfield: &rapier3d::prelude::HeightField,
color: Vec4,
) {
let scale = heightfield.scale();
let (rows, cols) = (heightfield.nrows(), heightfield.ncols());
let heights = heightfield.heights();
for row_index in 0..rows {
for col_index in 0..cols {
let height = heights[(row_index, col_index)];
let x = (col_index as f32 - cols as f32 / 2.0) * scale.x / cols as f32;
let z = (row_index as f32 - rows as f32 / 2.0) * scale.z / rows as f32;
let y = height * scale.y;
let current_point = transform_point(vec3(x, y, z), translation, rotation);
if col_index + 1 < cols {
let next_height = heights[(row_index, col_index + 1)];
let next_x = ((col_index + 1) as f32 - cols as f32 / 2.0) * scale.x / cols as f32;
let next_y = next_height * scale.y;
let next_point = transform_point(vec3(next_x, next_y, z), translation, rotation);
lines.push(Line {
start: current_point,
end: next_point,
color,
});
}
if row_index + 1 < rows {
let next_height = heights[(row_index + 1, col_index)];
let next_z = ((row_index + 1) as f32 - rows as f32 / 2.0) * scale.z / rows as f32;
let next_y = next_height * scale.y;
let next_point = transform_point(vec3(x, next_y, next_z), translation, rotation);
lines.push(Line {
start: current_point,
end: next_point,
color,
});
}
}
}
}