use std::any::Any;
use std::cell::{Cell, RefCell};
use std::ffi::c_void;
use std::panic::{catch_unwind, resume_unwind, AssertUnwindSafe};
use std::ptr::NonNull;
use std::sync::{Mutex, MutexGuard, OnceLock, PoisonError};
use oxijolt_sys::*;
use crate::body::with_read_locked_body;
use crate::limits;
use crate::owned::{JoltObject, Owned};
use crate::query::{offset_from, rotation_translation};
use crate::{BodyId, ObjectLayer, PhysicsWorld, Quat, QueryError, QueryFilter, RVec3, Real, Vec3};
const BROAD_PHASE_MARGIN: Real = 0.01;
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct DebugLineSettings {
center: RVec3,
radius: f32,
max_lines: usize,
}
impl DebugLineSettings {
pub fn new(center: RVec3, radius: f32) -> Self {
Self {
center,
radius,
max_lines: usize::MAX,
}
}
#[must_use]
pub fn max_lines(self, max_lines: usize) -> Self {
Self { max_lines, ..self }
}
fn validate(&self) -> Result<(), QueryError> {
if !limits::is_in_frame(self.center) {
return Err(QueryError::InvalidValue(
"debug line center must be finite and within limits::MAX_POSITION",
));
}
let radius = Real::from(self.radius);
if !(radius.is_finite() && (0.0..=2.0 * limits::MAX_POSITION).contains(&radius)) {
return Err(QueryError::InvalidValue(
"debug line radius must be between 0 and 2 * limits::MAX_POSITION",
));
}
Ok(())
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
#[non_exhaustive]
pub struct DebugLine {
pub from: RVec3,
pub to: RVec3,
pub body: BodyId,
pub child_user_data: Option<u32>,
}
#[derive(Debug, Default)]
pub struct DebugLines {
lines: Vec<DebugLine>,
truncated: bool,
}
impl DebugLines {
pub fn new() -> Self {
Self::default()
}
pub fn lines(&self) -> &[DebugLine] {
&self.lines
}
pub fn is_truncated(&self) -> bool {
self.truncated
}
pub fn release(&mut self) {
self.lines = Vec::new();
self.truncated = false;
}
}
impl PhysicsWorld {
pub fn debug_lines(
&self,
settings: &DebugLineSettings,
filter: &QueryFilter<'_>,
) -> Result<DebugLines, QueryError> {
let mut lines = DebugLines::new();
self.debug_lines_into(settings, filter, &mut lines)?;
Ok(lines)
}
pub fn debug_lines_into(
&self,
settings: &DebugLineSettings,
filter: &QueryFilter<'_>,
out: &mut DebugLines,
) -> Result<(), QueryError> {
settings.validate()?;
filter.validate(self)?;
out.lines.clear();
out.truncated = false;
let bodies = self.debug_bodies(settings, filter);
let guard = lock_draw();
install_procs();
let state = DrawState::new(&mut out.lines, settings.max_lines);
{
let renderer = create_renderer(&state)?;
for body in &bodies {
if state.truncated.get() {
break;
}
self.draw_body(&renderer, &state, body, settings, filter);
}
}
let truncated = state.truncated.get();
let panic = state.panic.take();
drop(state);
drop(guard);
if let Some(payload) = panic {
resume_unwind(payload);
}
out.truncated = truncated;
Ok(())
}
fn debug_bodies(
&self,
settings: &DebugLineSettings,
filter: &QueryFilter<'_>,
) -> Vec<DebugBody> {
let reach = Real::from(settings.radius) + BROAD_PHASE_MARGIN;
let center = settings.center;
#[allow(clippy::unnecessary_cast)]
let corner = |sign: Real| JPH_Vec3 {
x: (center.x + sign * reach) as f32,
y: (center.y + sign * reach) as f32,
z: (center.z + sign * reach) as f32,
};
let query = JPH_AABox {
min: corner(-1.0),
max: corner(1.0),
};
let radius_sq = Real::from(settings.radius) * Real::from(settings.radius);
let mut bodies = Vec::new();
for raw in self.broad_phase_bodies(&query) {
let id = BodyId::new(raw, self.tag);
if !filter.keeps_body(id) {
continue;
}
let read = with_read_locked_body(self.body_lock_interface, id, |body| {
let body = body.as_ptr();
let mut bounds = JPH_AABox {
min: Vec3::ZERO.to_jph(),
max: Vec3::ZERO.to_jph(),
};
let mut position = RVec3::ZERO.to_jph();
let mut rotation = Quat::IDENTITY.to_jph();
let (layer, shape) = unsafe {
JPH_Body_GetWorldSpaceBounds(body, &mut bounds);
JPH_Body_GetCenterOfMassPosition(body, &mut position);
JPH_Body_GetRotation(body, &mut rotation);
(JPH_Body_GetObjectLayer(body), JPH_Body_GetShape(body))
};
(
ObjectLayer::new(layer),
bounds,
RVec3::from_jph(position),
Quat::from_jph(rotation),
NonNull::new(shape.cast_mut()),
)
});
let Some((layer, bounds, position, rotation, Some(shape))) = read else {
continue;
};
if filter.keeps_object_layer(layer) && bounds_distance_sq(&bounds, center) <= radius_sq
{
bodies.push(DebugBody {
id,
shape,
position,
rotation,
});
}
}
bodies
}
fn draw_body(
&self,
renderer: &Owned<JPH_DebugRenderer>,
state: &DrawState<'_>,
body: &DebugBody,
settings: &DebugLineSettings,
filter: &QueryFilter<'_>,
) {
let root = body.shape.as_ptr();
let sub_type = unsafe { JPH_Shape_GetSubType(root) };
if sub_type != JPH_ShapeSubType_StaticCompound
&& sub_type != JPH_ShapeSubType_MutableCompound
{
state.current.set(Some((body.id, None)));
draw_shape(renderer, root, body.rotation, body.position);
return;
}
let compound: *const JPH_CompoundShape = root.cast();
let radius_sq = Real::from(settings.radius) * Real::from(settings.radius);
let count = unsafe { JPH_CompoundShape_GetNumSubShapes(compound) };
for index in 0..count {
if state.truncated.get() {
return;
}
let mut child: *const JPH_Shape = std::ptr::null();
let mut position_com = Vec3::ZERO.to_jph();
let mut rotation = Quat::IDENTITY.to_jph();
let mut user_data = 0;
unsafe {
JPH_CompoundShape_GetSubShape(
compound,
index,
&mut child,
&mut position_com,
&mut rotation,
&mut user_data,
);
}
if child.is_null() || !filter.keeps_group(user_data) {
continue;
}
let rotation = body.rotation.product(Quat::from_jph(rotation));
let offset = body.rotation.rotate(Vec3::from_jph(position_com));
let position = offset_from(body.position, offset.to_jph());
if bounds_distance_sq(&shape_bounds(child, rotation, position), settings.center)
> radius_sq
{
continue;
}
state.current.set(Some((body.id, Some(user_data))));
draw_shape(renderer, child, rotation, position);
}
}
}
struct DebugBody {
id: BodyId,
shape: NonNull<JPH_Shape>,
position: RVec3,
rotation: Quat,
}
fn shape_bounds(shape: *const JPH_Shape, rotation: Quat, position: RVec3) -> JPH_AABox {
let mut transform = rotation_translation(rotation, position);
let mut scale = Vec3::new(1.0, 1.0, 1.0).to_jph();
let mut bounds = JPH_AABox {
min: Vec3::ZERO.to_jph(),
max: Vec3::ZERO.to_jph(),
};
unsafe { JPH_Shape_GetWorldSpaceBounds(shape, &mut transform, &mut scale, &mut bounds) };
bounds
}
fn draw_shape(
renderer: &Owned<JPH_DebugRenderer>,
shape: *const JPH_Shape,
rotation: Quat,
position: RVec3,
) {
let transform = rotation_translation(rotation, position);
let scale = Vec3::new(1.0, 1.0, 1.0).to_jph();
unsafe {
JPH_Shape_Draw(
shape,
renderer.as_ptr(),
&transform,
&scale,
0xFFFF_FFFF,
false,
true,
);
}
}
fn bounds_distance_sq(bounds: &JPH_AABox, center: RVec3) -> Real {
let axis = |min: f32, max: f32, value: Real| {
let closest = value.clamp(Real::from(min), Real::from(max));
(value - closest) * (value - closest)
};
axis(bounds.min.x, bounds.max.x, center.x)
+ axis(bounds.min.y, bounds.max.y, center.y)
+ axis(bounds.min.z, bounds.max.z, center.z)
}
impl JoltObject for JPH_DebugRenderer {
unsafe fn destroy(ptr: *mut Self) {
unsafe { JPH_DebugRenderer_Destroy(ptr) };
}
}
static DEBUG_RENDERER_PROCS: JPH_DebugRenderer_Procs = JPH_DebugRenderer_Procs {
DrawLine: Some(draw_line),
DrawTriangle: None,
DrawText3D: None,
};
fn install_procs() {
static INSTALLED: OnceLock<()> = OnceLock::new();
INSTALLED.get_or_init(|| {
unsafe { JPH_DebugRenderer_SetProcs(&DEBUG_RENDERER_PROCS) };
});
}
fn lock_draw() -> MutexGuard<'static, ()> {
static DRAW: Mutex<()> = Mutex::new(());
DRAW.lock().unwrap_or_else(PoisonError::into_inner)
}
fn create_renderer(state: &DrawState<'_>) -> Result<Owned<JPH_DebugRenderer>, QueryError> {
let user_data = (state as *const DrawState<'_>).cast_mut().cast::<c_void>();
unsafe { Owned::from_raw(JPH_DebugRenderer_Create(user_data)) }
.ok_or(QueryError::AllocationFailed)
}
struct DrawState<'a> {
lines: RefCell<&'a mut Vec<DebugLine>>,
max_lines: usize,
truncated: Cell<bool>,
current: Cell<Option<(BodyId, Option<u32>)>>,
panic: Cell<Option<Box<dyn Any + Send>>>,
}
impl<'a> DrawState<'a> {
fn new(lines: &'a mut Vec<DebugLine>, max_lines: usize) -> Self {
Self {
lines: RefCell::new(lines),
max_lines,
truncated: Cell::new(false),
current: Cell::new(None),
panic: Cell::new(None),
}
}
fn has_panicked(&self) -> bool {
let payload = self.panic.take();
let panicked = payload.is_some();
self.panic.set(payload);
panicked
}
fn push(&self, from: RVec3, to: RVec3) {
if self.truncated.get() {
return;
}
let Some((body, child_user_data)) = self.current.get() else {
return;
};
let mut lines = self.lines.borrow_mut();
if lines.len() < self.max_lines {
lines.push(DebugLine {
from,
to,
body,
child_user_data,
});
} else {
self.truncated.set(true);
}
}
}
unsafe extern "C" fn draw_line(
user_data: *mut c_void,
from: *const JPH_RVec3,
to: *const JPH_RVec3,
_color: JPH_Color,
) {
let (state, from, to) = unsafe {
(
&*user_data.cast::<DrawState<'_>>(),
RVec3::from_jph(*from),
RVec3::from_jph(*to),
)
};
if state.has_panicked() {
return;
}
let pushed = catch_unwind(AssertUnwindSafe(|| {
#[cfg(test)]
tests::panic_if_injected();
state.push(from, to);
}));
if let Err(payload) = pushed {
state.panic.set(Some(payload));
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::{BodySettings, CompoundChild, Shape, WorldSettings};
thread_local! {
static INJECTED_PANIC: Cell<bool> = const { Cell::new(false) };
}
pub(super) fn panic_if_injected() {
if INJECTED_PANIC.get() {
panic!("injected draw_line panic");
}
}
fn unit_box() -> JPH_AABox {
JPH_AABox {
min: Vec3::new(-1.0, -1.0, -1.0).to_jph(),
max: Vec3::new(1.0, 1.0, 1.0).to_jph(),
}
}
#[test]
fn bounds_distance_is_zero_inside_and_on_faces() {
assert_eq!(bounds_distance_sq(&unit_box(), RVec3::ZERO), 0.0);
assert_eq!(
bounds_distance_sq(&unit_box(), RVec3::new(1.0, 0.5, -1.0)),
0.0
);
}
#[test]
fn bounds_distance_outside() {
assert_eq!(
bounds_distance_sq(&unit_box(), RVec3::new(4.0, 0.0, 0.0)),
9.0
);
assert_eq!(
bounds_distance_sq(&unit_box(), RVec3::new(-3.0, 3.0, 0.5)),
8.0
);
assert_eq!(
bounds_distance_sq(&unit_box(), RVec3::new(2.0, -3.0, 4.0)),
1.0 + 4.0 + 9.0
);
}
#[test]
fn release_frees_the_line_storage() {
let mut lines = DebugLines::new();
assert_eq!(lines.lines.capacity(), 0);
let world = PhysicsWorld::new(WorldSettings::default()).unwrap();
let line = DebugLine {
from: RVec3::ZERO,
to: RVec3::ZERO,
body: BodyId::new(0, world.tag),
child_user_data: None,
};
lines.lines.extend([line; 100]);
lines.truncated = true;
lines.release();
assert_eq!(lines.lines.capacity(), 0);
assert!(lines.lines().is_empty());
assert!(!lines.is_truncated());
}
#[test]
fn settings_validation() {
let valid = |settings: DebugLineSettings| settings.validate();
assert_eq!(valid(DebugLineSettings::new(RVec3::ZERO, 0.0)), Ok(()));
assert_eq!(valid(DebugLineSettings::new(RVec3::ZERO, 5.0)), Ok(()));
for radius in [f32::NAN, -1.0, f32::INFINITY] {
assert!(
matches!(
valid(DebugLineSettings::new(RVec3::ZERO, radius)),
Err(QueryError::InvalidValue(_))
),
"{radius}"
);
}
for center in [
RVec3::new(Real::NAN, 0.0, 0.0),
RVec3::new(0.0, Real::INFINITY, 0.0),
] {
assert!(matches!(
valid(DebugLineSettings::new(center, 1.0)),
Err(QueryError::InvalidValue(_))
));
}
}
#[test]
fn a_panic_in_draw_line_resumes_after_the_renderer_is_gone() {
let mut world = PhysicsWorld::new(WorldSettings::default()).unwrap();
let unit = Shape::new_box(Vec3::new(0.5, 0.5, 0.5)).unwrap();
let compound = Shape::new_compound(&[
CompoundChild {
shape: &unit,
position: Vec3::ZERO,
rotation: Quat::IDENTITY,
user_data: 1,
},
CompoundChild {
shape: &unit,
position: Vec3::new(2.0, 0.0, 0.0),
rotation: Quat::IDENTITY,
user_data: 2,
},
])
.unwrap();
world
.create_body(&compound, &BodySettings::new_static())
.unwrap();
world
.create_body(
&unit,
&BodySettings::new_static().position(RVec3::new(0.0, 3.0, 0.0)),
)
.unwrap();
let settings = DebugLineSettings::new(RVec3::ZERO, 10.0);
let filter = QueryFilter::new();
let mut clean = DebugLines::new();
world
.debug_lines_into(&settings, &filter, &mut clean)
.unwrap();
assert_eq!(clean.lines().len(), 3 * 36);
let mut lines = DebugLines::new();
INJECTED_PANIC.set(true);
let result = catch_unwind(AssertUnwindSafe(|| {
world.debug_lines_into(&settings, &filter, &mut lines)
}));
INJECTED_PANIC.set(false);
let payload = result.expect_err("joltc returned and the panic resumed");
assert_eq!(
payload.downcast_ref::<&str>(),
Some(&"injected draw_line panic")
);
world
.debug_lines_into(&settings, &filter, &mut lines)
.unwrap();
assert_eq!(lines.lines(), clean.lines());
assert!(!lines.is_truncated());
}
}