0b7c3b34ec
* feat: migrate to glam whenever relevant + migrate testbed to kiss3d instead of bevy * chore: update changelog * Fix warnings and tests * Release v0.32.0
967 lines
36 KiB
Rust
967 lines
36 KiB
Rust
#![allow(clippy::unnecessary_cast, clippy::useless_conversion)] // Casts/conversions are needed for switching between f32/f64.
|
||
|
||
use crate::testbed::TestbedStateFlags;
|
||
use kiss3d::prelude::*;
|
||
use rand_pcg::Pcg32;
|
||
use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
|
||
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
|
||
use std::collections::HashMap;
|
||
|
||
#[cfg(feature = "dim2")]
|
||
pub use kiss3d::prelude::SceneNode2d as SceneNode;
|
||
#[cfg(feature = "dim3")]
|
||
pub use kiss3d::prelude::SceneNode3d as SceneNode;
|
||
|
||
/// Template key identifies a shape type that can be instanced together
|
||
#[derive(Hash, Eq, PartialEq, Clone, Debug)]
|
||
pub enum ShapeTemplateType {
|
||
Ball,
|
||
Cuboid,
|
||
// Capsule,
|
||
#[cfg(feature = "dim3")]
|
||
Cylinder,
|
||
#[cfg(feature = "dim3")]
|
||
Cone,
|
||
}
|
||
|
||
/// Info about an instanced collider
|
||
#[derive(Clone)]
|
||
pub struct InstancedCollider {
|
||
pub collider: ColliderHandle,
|
||
pub body: Option<RigidBodyHandle>,
|
||
pub delta: rapier::math::Pose,
|
||
pub half_extents: rapier::math::Vector,
|
||
pub color: Color,
|
||
pub tmp_color: Option<Color>,
|
||
}
|
||
|
||
/// A template node that can render multiple instances
|
||
pub struct ShapeTemplate {
|
||
pub node: SceneNode,
|
||
pub colliders: Vec<InstancedCollider>,
|
||
}
|
||
|
||
/// Non-instanced node for complex shapes
|
||
pub struct IndividualNode {
|
||
pub node: SceneNode,
|
||
pub collider: ColliderHandle,
|
||
pub delta: rapier::math::Pose,
|
||
pub color: Color,
|
||
pub tmp_color: Option<Color>,
|
||
}
|
||
|
||
/// Location of a collider in the graphics system
|
||
#[derive(Clone, Debug)]
|
||
pub enum NodeLocation {
|
||
Instanced {
|
||
template: ShapeTemplateType,
|
||
index: usize,
|
||
},
|
||
Individual {
|
||
index: usize,
|
||
},
|
||
}
|
||
|
||
pub struct GraphicsManager {
|
||
scene: SceneNode,
|
||
rand: Pcg32,
|
||
/// Template nodes for instanced primitives
|
||
templates: HashMap<ShapeTemplateType, ShapeTemplate>,
|
||
/// Individual nodes for complex shapes (trimesh, heightfield, etc.)
|
||
individual_nodes: Vec<IndividualNode>,
|
||
/// Map from collider to its render nodes
|
||
c2nodes: HashMap<ColliderHandle, NodeLocation>,
|
||
/// Colliders attached to a particular body, used to identify the
|
||
/// body’s nodes even if it has been removed from the RigidBodySet.
|
||
b2colliders: HashMap<RigidBodyHandle, Vec<ColliderHandle>>,
|
||
/// Colors per body
|
||
b2color: HashMap<RigidBodyHandle, Color>,
|
||
/// Colors per collider (overrides body color)
|
||
c2color: HashMap<ColliderHandle, Color>,
|
||
/// Wireframe mode per body
|
||
b2wireframe: HashMap<RigidBodyHandle, bool>,
|
||
/// Default color for fixed/ground bodies
|
||
ground_color: Color,
|
||
/// Graphics offset
|
||
pub gfx_shift: rapier::math::Vector,
|
||
/// Global wireframe mode
|
||
wireframe_mode: bool,
|
||
}
|
||
|
||
const GROUND_COLOR: Color = Color {
|
||
r: 0.58,
|
||
g: 0.54,
|
||
b: 0.50,
|
||
a: 1.0,
|
||
};
|
||
|
||
impl GraphicsManager {
|
||
pub fn new() -> GraphicsManager {
|
||
GraphicsManager {
|
||
scene: Default::default(),
|
||
rand: Pcg32::new(0, 1),
|
||
templates: HashMap::new(),
|
||
individual_nodes: Vec::new(),
|
||
c2nodes: HashMap::new(),
|
||
b2colliders: HashMap::new(),
|
||
b2color: HashMap::new(),
|
||
c2color: HashMap::new(),
|
||
b2wireframe: HashMap::new(),
|
||
ground_color: GROUND_COLOR,
|
||
gfx_shift: rapier::math::Vector::ZERO,
|
||
wireframe_mode: false,
|
||
}
|
||
}
|
||
|
||
pub fn clear(&mut self) {
|
||
self.scene = SceneNode::empty();
|
||
#[cfg(feature = "dim3")]
|
||
{
|
||
// Setup lights.
|
||
let mut light = self
|
||
.scene
|
||
.add_light(Light::directional(Vec3::new(-1.0, -1.0, -1.0)));
|
||
light.set_position(Vec3::new(100.0, 100.0, 100.0));
|
||
|
||
let mut light = self
|
||
.scene
|
||
.add_light(Light::point(10000.0).with_intensity(1.0));
|
||
light.set_position(Vec3::new(-100.0, 100.0, -100.0));
|
||
}
|
||
|
||
self.templates.clear();
|
||
self.individual_nodes.clear();
|
||
self.c2nodes.clear();
|
||
self.b2colliders.clear();
|
||
self.c2color.clear();
|
||
self.b2color.clear();
|
||
self.b2wireframe.clear();
|
||
self.rand = Pcg32::new(0, 1);
|
||
}
|
||
|
||
pub fn scene(&self) -> &SceneNode {
|
||
&self.scene
|
||
}
|
||
|
||
pub fn scene_mut(&mut self) -> &mut SceneNode {
|
||
&mut self.scene
|
||
}
|
||
|
||
/// Get or create a template node for a shape type
|
||
#[cfg(feature = "dim3")]
|
||
fn get_or_create_template(&mut self, template_type: ShapeTemplateType) -> &mut ShapeTemplate {
|
||
self.templates
|
||
.entry(template_type.clone())
|
||
.or_insert_with(|| {
|
||
// Create a unit-sized template node
|
||
let node = match template_type {
|
||
ShapeTemplateType::Ball => self.scene.add_sphere_with_subdiv(1.0, 20, 20),
|
||
ShapeTemplateType::Cuboid => self.scene.add_cube(2.0, 2.0, 2.0),
|
||
// ShapeTemplateType::Capsule => self.scene.add_capsule(1.0, 2.0),
|
||
ShapeTemplateType::Cylinder => self.scene.add_cylinder(1.0, 2.0),
|
||
ShapeTemplateType::Cone => self.scene.add_cone(1.0, 2.0),
|
||
};
|
||
ShapeTemplate {
|
||
node,
|
||
colliders: Vec::new(),
|
||
}
|
||
})
|
||
}
|
||
|
||
#[cfg(feature = "dim2")]
|
||
fn get_or_create_template(&mut self, template_type: ShapeTemplateType) -> &mut ShapeTemplate {
|
||
self.templates
|
||
.entry(template_type.clone())
|
||
.or_insert_with(|| {
|
||
// Create a unit-sized template node
|
||
let node = match template_type {
|
||
ShapeTemplateType::Ball => self.scene.add_circle(1.0),
|
||
ShapeTemplateType::Cuboid => self.scene.add_rectangle(2.0, 2.0),
|
||
// ShapeTemplateType::Capsule => {
|
||
// // Use proper 2D capsule with unit radius and unit half-height
|
||
// window.add_capsule_2d(1.0, 2.0)
|
||
// }
|
||
};
|
||
ShapeTemplate {
|
||
node,
|
||
colliders: Vec::new(),
|
||
}
|
||
})
|
||
}
|
||
|
||
/// Determine the template type for a shape, if it can be instanced
|
||
fn shape_template_type(shape: &dyn Shape) -> Option<ShapeTemplateType> {
|
||
match shape.shape_type() {
|
||
ShapeType::Ball => Some(ShapeTemplateType::Ball),
|
||
ShapeType::Cuboid | ShapeType::RoundCuboid => Some(ShapeTemplateType::Cuboid),
|
||
// ShapeType::Capsule => Some(ShapeTemplateType::Capsule),
|
||
#[cfg(feature = "dim3")]
|
||
ShapeType::Cylinder | ShapeType::RoundCylinder => Some(ShapeTemplateType::Cylinder),
|
||
#[cfg(feature = "dim3")]
|
||
ShapeType::Cone | ShapeType::RoundCone => Some(ShapeTemplateType::Cone),
|
||
_ => None,
|
||
}
|
||
}
|
||
|
||
/// Get half-extents for a shape (used for scaling instances)
|
||
#[cfg(feature = "dim3")]
|
||
fn shape_half_extents(shape: &dyn Shape) -> rapier::math::Vector {
|
||
match shape.shape_type() {
|
||
ShapeType::Ball => {
|
||
let r = shape.as_ball().unwrap().radius;
|
||
rapier::math::Vector::new(r, r, r)
|
||
}
|
||
ShapeType::Cuboid => shape.as_cuboid().unwrap().half_extents,
|
||
ShapeType::RoundCuboid => shape.as_round_cuboid().unwrap().inner_shape.half_extents,
|
||
ShapeType::Capsule => {
|
||
let c = shape.as_capsule().unwrap();
|
||
rapier::math::Vector::new(c.radius, c.half_height() + c.radius, c.radius)
|
||
}
|
||
ShapeType::Cylinder => {
|
||
let c = shape.as_cylinder().unwrap();
|
||
rapier::math::Vector::new(c.radius, c.half_height, c.radius)
|
||
}
|
||
ShapeType::RoundCylinder => {
|
||
let c = &shape.as_round_cylinder().unwrap().inner_shape;
|
||
rapier::math::Vector::new(c.radius, c.half_height, c.radius)
|
||
}
|
||
ShapeType::Cone => {
|
||
let c = shape.as_cone().unwrap();
|
||
rapier::math::Vector::new(c.radius, c.half_height, c.radius)
|
||
}
|
||
ShapeType::RoundCone => {
|
||
let c = &shape.as_round_cone().unwrap().inner_shape;
|
||
rapier::math::Vector::new(c.radius, c.half_height, c.radius)
|
||
}
|
||
_ => rapier::math::Vector::new(1.0, 1.0, 1.0),
|
||
}
|
||
}
|
||
|
||
#[cfg(feature = "dim2")]
|
||
fn shape_half_extents(shape: &dyn Shape) -> rapier::math::Vector {
|
||
match shape.shape_type() {
|
||
ShapeType::Ball => {
|
||
let r = shape.as_ball().unwrap().radius;
|
||
rapier::math::Vector::new(r, r)
|
||
}
|
||
ShapeType::Cuboid => shape.as_cuboid().unwrap().half_extents,
|
||
ShapeType::RoundCuboid => shape.as_round_cuboid().unwrap().inner_shape.half_extents,
|
||
ShapeType::Capsule => {
|
||
let c = shape.as_capsule().unwrap();
|
||
rapier::math::Vector::new(c.radius, c.half_height() + c.radius)
|
||
}
|
||
_ => rapier::math::Vector::new(1.0, 1.0),
|
||
}
|
||
}
|
||
|
||
fn remove_node(&mut self, location: NodeLocation) {
|
||
match location {
|
||
NodeLocation::Instanced { template, index } => {
|
||
if let Some(tmpl) = self.templates.get_mut(&template) {
|
||
// Remove by swapping with last (O(1) removal)
|
||
if index < tmpl.colliders.len() {
|
||
tmpl.colliders.swap_remove(index);
|
||
// Update the location of the swapped element
|
||
if index < tmpl.colliders.len() {
|
||
let swapped_collider = tmpl.colliders[index].collider;
|
||
self.c2nodes.insert(
|
||
swapped_collider,
|
||
NodeLocation::Instanced {
|
||
template: template.clone(),
|
||
index,
|
||
},
|
||
);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
NodeLocation::Individual { index } => {
|
||
if index < self.individual_nodes.len() {
|
||
self.individual_nodes[index].node.detach();
|
||
self.individual_nodes.swap_remove(index);
|
||
// Update the location of the swapped element
|
||
if index < self.individual_nodes.len() {
|
||
let swapped_collider = self.individual_nodes[index].collider;
|
||
self.c2nodes
|
||
.insert(swapped_collider, NodeLocation::Individual { index });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn remove_collider_nodes(&mut self, collider: ColliderHandle) {
|
||
if let Some(location) = self.c2nodes.remove(&collider) {
|
||
self.remove_node(location);
|
||
}
|
||
}
|
||
|
||
pub fn remove_body_nodes(&mut self, body: RigidBodyHandle) {
|
||
if let Some(colliders) = self.b2colliders.get(&body).cloned() {
|
||
for collider in colliders {
|
||
self.remove_collider_nodes(collider);
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Color, tmp_color: bool) {
|
||
if !tmp_color {
|
||
self.b2color.insert(b, color);
|
||
}
|
||
|
||
if let Some(colls) = self.b2colliders.get(&b) {
|
||
for co in colls.iter() {
|
||
match &self.c2nodes[co] {
|
||
NodeLocation::Individual { index } => {
|
||
if tmp_color {
|
||
self.individual_nodes[*index].tmp_color = Some(color);
|
||
} else {
|
||
self.individual_nodes[*index].color = color;
|
||
}
|
||
}
|
||
NodeLocation::Instanced { template, index } => {
|
||
let instances = &mut self.templates.get_mut(template).unwrap();
|
||
if tmp_color {
|
||
instances.colliders[*index].tmp_color = Some(color);
|
||
} else {
|
||
instances.colliders[*index].color = color;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn set_initial_body_color(&mut self, b: RigidBodyHandle, color: Color) {
|
||
self.b2color.insert(b, color);
|
||
}
|
||
|
||
pub fn set_initial_collider_color(&mut self, c: ColliderHandle, color: Color) {
|
||
self.c2color.insert(c, color);
|
||
}
|
||
|
||
pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) {
|
||
self.b2wireframe.insert(b, enabled);
|
||
}
|
||
|
||
pub fn toggle_wireframe_mode(&mut self, _colliders: &ColliderSet, enabled: bool) {
|
||
self.wireframe_mode = enabled;
|
||
|
||
// // Update template nodes
|
||
// for tmpl in self.templates.values_mut() {
|
||
// if enabled {
|
||
// tmpl.node.set_lines_width(1.0, false);
|
||
// tmpl.node.set_surface_rendering_activation(false);
|
||
// } else {
|
||
// tmpl.node.set_lines_width(0.0, false);
|
||
// tmpl.node.set_surface_rendering_activation(true);
|
||
// }
|
||
// }
|
||
//
|
||
// // Update individual nodes
|
||
// for node in &mut self.individual_nodes {
|
||
// if enabled {
|
||
// node.node.set_lines_width(1.0, false);
|
||
// node.node.set_surface_rendering_activation(false);
|
||
// } else {
|
||
// node.node.set_lines_width(0.0, false);
|
||
// node.node.set_surface_rendering_activation(true);
|
||
// }
|
||
// }
|
||
}
|
||
|
||
pub fn next_color(&mut self) -> Rgba<f32> {
|
||
Self::gen_color(&mut self.rand)
|
||
}
|
||
|
||
fn gen_color(rng: &mut Pcg32) -> Rgba<f32> {
|
||
use rand::Rng;
|
||
let [r, g, b]: [f32; 3] = rng.random();
|
||
[r, g, b, 1.0].into()
|
||
}
|
||
|
||
fn alloc_color(&mut self, handle: RigidBodyHandle, is_fixed: bool) -> Color {
|
||
let mut color = self.ground_color;
|
||
|
||
if !is_fixed {
|
||
match self.b2color.get(&handle).cloned() {
|
||
Some(c) => color = c,
|
||
None => color = Self::gen_color(&mut self.rand),
|
||
}
|
||
}
|
||
|
||
self.b2color.insert(handle, color);
|
||
color
|
||
}
|
||
|
||
pub fn add_body_colliders(
|
||
&mut self,
|
||
window: &mut Window,
|
||
handle: RigidBodyHandle,
|
||
bodies: &RigidBodySet,
|
||
colliders: &ColliderSet,
|
||
) {
|
||
let body = bodies.get(handle).unwrap();
|
||
|
||
let color = self
|
||
.b2color
|
||
.get(&handle)
|
||
.cloned()
|
||
.unwrap_or_else(|| self.alloc_color(handle, !body.is_dynamic()));
|
||
|
||
self.add_body_colliders_with_color(window, handle, bodies, colliders, color);
|
||
}
|
||
|
||
pub fn add_body_colliders_with_color(
|
||
&mut self,
|
||
window: &mut Window,
|
||
handle: RigidBodyHandle,
|
||
bodies: &RigidBodySet,
|
||
colliders: &ColliderSet,
|
||
color: Color,
|
||
) {
|
||
for collider_handle in bodies[handle].colliders() {
|
||
let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
|
||
let collider = &colliders[*collider_handle];
|
||
self.add_shape(
|
||
window,
|
||
*collider_handle,
|
||
Some(handle),
|
||
collider.shape(),
|
||
collider.is_sensor(),
|
||
rapier::math::Pose::IDENTITY,
|
||
color,
|
||
);
|
||
}
|
||
}
|
||
|
||
pub fn add_collider(
|
||
&mut self,
|
||
window: &mut Window,
|
||
handle: ColliderHandle,
|
||
colliders: &ColliderSet,
|
||
) {
|
||
let collider = &colliders[handle];
|
||
let collider_parent = collider.parent();
|
||
let color = collider_parent
|
||
.and_then(|p| self.b2color.get(&p).copied())
|
||
.unwrap_or(self.ground_color);
|
||
let color = self.c2color.get(&handle).copied().unwrap_or(color);
|
||
self.add_shape(
|
||
window,
|
||
handle,
|
||
collider_parent,
|
||
collider.shape(),
|
||
collider.is_sensor(),
|
||
rapier::math::Pose::IDENTITY,
|
||
color,
|
||
);
|
||
}
|
||
|
||
pub fn add_shape(
|
||
&mut self,
|
||
_window: &mut Window,
|
||
handle: ColliderHandle,
|
||
body: Option<RigidBodyHandle>,
|
||
shape: &dyn Shape,
|
||
sensor: bool,
|
||
delta: rapier::math::Pose,
|
||
color: Color,
|
||
) {
|
||
if let Some(body) = body {
|
||
let colls = self.b2colliders.entry(body).or_default();
|
||
if !colls.contains(&handle) {
|
||
colls.push(handle);
|
||
}
|
||
}
|
||
|
||
// Handle compound shapes recursively
|
||
if let Some(compound) = shape.as_compound() {
|
||
for (shape_pos, sub_shape) in compound.shapes() {
|
||
self.add_shape(
|
||
_window,
|
||
handle,
|
||
body,
|
||
&**sub_shape,
|
||
sensor,
|
||
*shape_pos * delta,
|
||
color,
|
||
);
|
||
}
|
||
return;
|
||
}
|
||
|
||
let opacity = if sensor { 0.5 } else { 1.0 };
|
||
|
||
// Try to use instancing for primitive shapes
|
||
if let Some(template_type) = Self::shape_template_type(shape) {
|
||
let half_extents = Self::shape_half_extents(shape);
|
||
let template = self.get_or_create_template(template_type.clone());
|
||
let index = template.colliders.len();
|
||
template.colliders.push(InstancedCollider {
|
||
collider: handle,
|
||
body,
|
||
delta,
|
||
half_extents,
|
||
color: color.with_alpha(opacity),
|
||
tmp_color: None,
|
||
});
|
||
self.c2nodes.insert(
|
||
handle,
|
||
NodeLocation::Instanced {
|
||
template: template_type,
|
||
index,
|
||
},
|
||
);
|
||
} else {
|
||
// Create individual node for complex shapes
|
||
if let Some(node) = Self::create_individual_node(&mut self.scene, shape, color, sensor)
|
||
{
|
||
let index = self.individual_nodes.len();
|
||
self.individual_nodes.push(IndividualNode {
|
||
node,
|
||
collider: handle,
|
||
delta,
|
||
color: color.with_alpha(opacity),
|
||
tmp_color: None,
|
||
});
|
||
self.c2nodes
|
||
.insert(handle, NodeLocation::Individual { index });
|
||
}
|
||
}
|
||
}
|
||
|
||
#[cfg(feature = "dim3")]
|
||
fn create_individual_node(
|
||
scene: &mut SceneNode,
|
||
shape: &dyn Shape,
|
||
color: Color,
|
||
sensor: bool,
|
||
) -> Option<SceneNode3d> {
|
||
use kiss3d::procedural::{IndexBuffer, RenderMesh};
|
||
|
||
fn to_render_mesh(trimesh: &rapier::geometry::TriMesh) -> RenderMesh {
|
||
let vtx = trimesh
|
||
.vertices()
|
||
.iter()
|
||
.map(|pt| Vec3::new(pt.x as f32, pt.y as f32, pt.z as f32))
|
||
.collect();
|
||
let idx = trimesh.indices().to_vec();
|
||
let mut mesh = RenderMesh::new(vtx, None, None, Some(IndexBuffer::Unified(idx)));
|
||
mesh.replicate_vertices();
|
||
mesh.recompute_normals();
|
||
mesh
|
||
}
|
||
|
||
let mut node = match shape.shape_type() {
|
||
ShapeType::TriMesh => {
|
||
let trimesh = shape.as_trimesh().unwrap();
|
||
Some(scene.add_render_mesh(to_render_mesh(trimesh), Vec3::ONE))
|
||
}
|
||
ShapeType::HeightField => {
|
||
let heightfield = shape.as_heightfield().unwrap();
|
||
let (vertices, indices) = heightfield.to_trimesh();
|
||
let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
|
||
Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
|
||
}
|
||
ShapeType::ConvexPolyhedron | ShapeType::RoundConvexPolyhedron => {
|
||
let poly = shape
|
||
.as_convex_polyhedron()
|
||
.or_else(|| shape.as_round_convex_polyhedron().map(|rp| &rp.inner_shape));
|
||
poly.map(|p| {
|
||
let (vertices, indices) = p.to_trimesh();
|
||
let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
|
||
scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE)
|
||
})
|
||
}
|
||
ShapeType::Capsule => {
|
||
let caps = shape.as_capsule().unwrap();
|
||
let pose = caps.canonical_transform();
|
||
let mut parent = scene.add_group();
|
||
let mut node =
|
||
parent.add_capsule(caps.radius as f32, caps.half_height() as f32 * 2.0);
|
||
node.set_pose(pose.into());
|
||
Some(parent)
|
||
}
|
||
ShapeType::Triangle => {
|
||
let tri = shape.as_triangle().unwrap();
|
||
let vertices = vec![tri.a, tri.b, tri.c];
|
||
let indices = vec![[0u32, 1, 2], [0u32, 2, 1]];
|
||
let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
|
||
Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
|
||
}
|
||
ShapeType::HalfSpace => {
|
||
let mut parent = scene.add_group();
|
||
parent.rotate(Quat::from_axis_angle(Vec3::X, -std::f32::consts::FRAC_PI_2));
|
||
let node = parent.add_quad(2000.0, 2000.0, 1, 1);
|
||
Some(node)
|
||
}
|
||
ShapeType::Voxels => {
|
||
let voxels = shape.as_voxels().unwrap();
|
||
let (vertices, indices) = voxels.to_trimesh();
|
||
let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
|
||
Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
|
||
}
|
||
_ => None,
|
||
};
|
||
|
||
if let Some(ref mut n) = node {
|
||
n.set_color(color);
|
||
if sensor {
|
||
n.set_surface_rendering_activation(false);
|
||
n.set_lines_width(1.0, false);
|
||
}
|
||
}
|
||
|
||
node
|
||
}
|
||
|
||
#[cfg(feature = "dim2")]
|
||
fn create_individual_node(
|
||
scene: &mut SceneNode,
|
||
shape: &dyn Shape,
|
||
color: Color,
|
||
_sensor: bool,
|
||
) -> Option<SceneNode2d> {
|
||
let mut node = match shape.shape_type() {
|
||
ShapeType::Triangle => {
|
||
let tri = shape.as_triangle().unwrap();
|
||
let vertices: Vec<Vec2> = vec![
|
||
Vec2::new(tri.a.x as f32, tri.a.y as f32),
|
||
Vec2::new(tri.b.x as f32, tri.b.y as f32),
|
||
Vec2::new(tri.c.x as f32, tri.c.y as f32),
|
||
];
|
||
Some(scene.add_convex_polygon(vertices, Vec2::ONE))
|
||
}
|
||
ShapeType::TriMesh => {
|
||
let trimesh = shape.as_trimesh().unwrap();
|
||
let vertices: Vec<Vec2> = trimesh
|
||
.vertices()
|
||
.iter()
|
||
.map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
|
||
.collect();
|
||
let render_mesh = GpuMesh2d::new(vertices, trimesh.indices().to_vec(), None, false);
|
||
Some(scene.add_mesh(Rc::new(RefCell::new(render_mesh)), Vec2::ONE))
|
||
}
|
||
ShapeType::ConvexPolygon | ShapeType::RoundConvexPolygon => {
|
||
let poly = shape
|
||
.as_convex_polygon()
|
||
.or_else(|| shape.as_round_convex_polygon().map(|rp| &rp.inner_shape));
|
||
poly.map(|p| {
|
||
let vertices: Vec<Vec2> = p
|
||
.points()
|
||
.iter()
|
||
.map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
|
||
.collect();
|
||
scene.add_convex_polygon(vertices, Vec2::ONE)
|
||
})
|
||
}
|
||
ShapeType::Capsule => {
|
||
let caps = shape.as_capsule().unwrap();
|
||
let pose = caps.canonical_transform();
|
||
let mut parent = scene.add_group();
|
||
let mut node =
|
||
parent.add_capsule(caps.radius as f32, caps.half_height() as f32 * 2.0);
|
||
node.set_pose(pose.into());
|
||
Some(parent)
|
||
}
|
||
ShapeType::Segment => {
|
||
// Render segment as a thin quad with some thickness
|
||
let seg = shape.as_segment().unwrap();
|
||
let vertices = vec![
|
||
Vec2::new(seg.a.x as f32, seg.a.y as f32),
|
||
Vec2::new(seg.b.x as f32, seg.b.y as f32),
|
||
];
|
||
Some(scene.add_polyline(vertices, None, 0.1))
|
||
}
|
||
ShapeType::Polyline => {
|
||
// Render polyline as connected thin quads
|
||
let polyline = shape.as_polyline().unwrap();
|
||
let vertices: Vec<Vec2> = polyline
|
||
.vertices()
|
||
.iter()
|
||
.map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
|
||
.collect();
|
||
Some(
|
||
scene
|
||
.add_polyline(vertices, Some(polyline.indices().to_vec()), 0.2)
|
||
.set_lines_color(Some(GROUND_COLOR)),
|
||
)
|
||
}
|
||
ShapeType::HeightField => {
|
||
let hf = shape.as_heightfield().unwrap();
|
||
let (polyline_verts, indices) = hf.to_polyline();
|
||
let vertices: Vec<Vec2> = polyline_verts
|
||
.iter()
|
||
.map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
|
||
.collect();
|
||
Some(
|
||
scene
|
||
.add_polyline(vertices, Some(indices), 0.2)
|
||
.set_lines_color(Some(GROUND_COLOR)),
|
||
)
|
||
}
|
||
ShapeType::Voxels => {
|
||
let voxels = shape.as_voxels().unwrap();
|
||
|
||
let mut vtx = vec![];
|
||
let mut idx = vec![];
|
||
let sz = voxels.voxel_size() / 2.0;
|
||
for vox in voxels.voxels() {
|
||
if !vox.state.is_empty() {
|
||
let bid = vtx.len() as u32;
|
||
let center = Vec2::new(vox.center.x as f32, vox.center.y as f32);
|
||
vtx.push(center + Vec2::new(sz.x as f32, sz.y as f32));
|
||
vtx.push(center + Vec2::new(-sz.x as f32, sz.y as f32));
|
||
vtx.push(center + Vec2::new(-sz.x as f32, -sz.y as f32));
|
||
vtx.push(center + Vec2::new(sz.x as f32, -sz.y as f32));
|
||
idx.push([bid, bid + 1, bid + 2]);
|
||
idx.push([bid + 2, bid + 3, bid]);
|
||
}
|
||
}
|
||
|
||
let mesh = GpuMesh2d::new(vtx, idx, None, false);
|
||
Some(scene.add_mesh(Rc::new(RefCell::new(mesh)), Vec2::ONE))
|
||
}
|
||
_ => None,
|
||
};
|
||
|
||
if let Some(ref mut n) = node {
|
||
n.set_color(color);
|
||
}
|
||
|
||
node
|
||
}
|
||
|
||
#[cfg(feature = "dim3")]
|
||
pub fn draw(
|
||
&mut self,
|
||
flags: TestbedStateFlags,
|
||
_bodies: &RigidBodySet,
|
||
colliders: &ColliderSet,
|
||
) {
|
||
let draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES);
|
||
|
||
// Update instance data for all templates
|
||
for (template_type, template) in &mut self.templates {
|
||
template
|
||
.node
|
||
.set_visible(draw_surfaces && !template.colliders.is_empty());
|
||
|
||
if template.colliders.is_empty() {
|
||
continue;
|
||
}
|
||
|
||
let instances: Vec<InstanceData3d> = template
|
||
.colliders
|
||
.iter_mut()
|
||
.filter_map(|ic| {
|
||
let co = colliders.get(ic.collider)?;
|
||
let co_pos = *co.position() * ic.delta;
|
||
|
||
// Get actual color (might have been updated)
|
||
let bcolor = ic
|
||
.body
|
||
.and_then(|b| self.b2color.get(&b).copied())
|
||
.unwrap_or(ic.color);
|
||
let ccolor = self
|
||
.c2color
|
||
.get(&ic.collider)
|
||
.copied()
|
||
.unwrap_or(bcolor)
|
||
.with_alpha(ic.color.a);
|
||
let color = ic.tmp_color.take().unwrap_or(ccolor);
|
||
|
||
// Build position
|
||
let pos = Vec3::new(
|
||
(co_pos.translation.x + self.gfx_shift.x) as f32,
|
||
(co_pos.translation.y + self.gfx_shift.y) as f32,
|
||
(co_pos.translation.z + self.gfx_shift.z) as f32,
|
||
);
|
||
|
||
// Build deformation matrix (rotation * scale)
|
||
let rot = Quat::from_xyzw(
|
||
co_pos.rotation.x as f32,
|
||
co_pos.rotation.y as f32,
|
||
co_pos.rotation.z as f32,
|
||
co_pos.rotation.w as f32,
|
||
);
|
||
let rot_mat = Mat3::from_quat(rot);
|
||
|
||
// Scale based on shape type
|
||
let scale = match template_type {
|
||
ShapeTemplateType::Ball => {
|
||
// Ball template is unit radius, scale uniformly
|
||
Vec3::splat(ic.half_extents.x as f32)
|
||
}
|
||
ShapeTemplateType::Cuboid => {
|
||
// Cuboid template is 2x2x2, scale by half_extents
|
||
Vec3::new(
|
||
ic.half_extents.x as f32,
|
||
ic.half_extents.y as f32,
|
||
ic.half_extents.z as f32,
|
||
)
|
||
}
|
||
// ShapeTemplateType::Capsule => {
|
||
// // Capsule template has radius 1, half_height 1
|
||
// // half_extents = (radius, half_height + radius, radius)
|
||
// let radius = ic.half_extents.x as f32;
|
||
// let half_height = ic.half_extents.y as f32 - radius;
|
||
// Vec3::new(radius, (half_height + radius).max(radius), radius)
|
||
// }
|
||
ShapeTemplateType::Cylinder => {
|
||
// Cylinder template has radius 1, half_height 1
|
||
Vec3::new(
|
||
ic.half_extents.x as f32,
|
||
ic.half_extents.y as f32,
|
||
ic.half_extents.z as f32,
|
||
)
|
||
}
|
||
ShapeTemplateType::Cone => {
|
||
// Cone template has radius 1, half_height 1
|
||
Vec3::new(
|
||
ic.half_extents.x as f32,
|
||
ic.half_extents.y as f32,
|
||
ic.half_extents.z as f32,
|
||
)
|
||
}
|
||
};
|
||
|
||
let scale_mat = Mat3::from_diagonal(scale);
|
||
let deformation = rot_mat * scale_mat;
|
||
|
||
Some(InstanceData3d {
|
||
position: pos,
|
||
deformation,
|
||
color,
|
||
lines_color: None,
|
||
lines_width: None,
|
||
points_color: None,
|
||
points_size: None,
|
||
})
|
||
})
|
||
.collect();
|
||
|
||
template.node.set_instances(&instances);
|
||
}
|
||
|
||
// Update individual nodes
|
||
for node in &mut self.individual_nodes {
|
||
node.node.set_visible(draw_surfaces);
|
||
|
||
if let Some(co) = colliders.get(node.collider) {
|
||
let co_pos = *co.position() * node.delta;
|
||
node.node
|
||
.set_pose(co_pos.append_translation(self.gfx_shift).into());
|
||
node.node
|
||
.set_color(node.tmp_color.take().unwrap_or(node.color));
|
||
}
|
||
}
|
||
}
|
||
|
||
#[cfg(feature = "dim2")]
|
||
pub fn draw(
|
||
&mut self,
|
||
flags: TestbedStateFlags,
|
||
_bodies: &RigidBodySet,
|
||
colliders: &ColliderSet,
|
||
) {
|
||
let draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES);
|
||
|
||
// Update instance data for all templates
|
||
for (template_type, template) in &mut self.templates {
|
||
template
|
||
.node
|
||
.set_visible(draw_surfaces && !template.colliders.is_empty());
|
||
|
||
if template.colliders.is_empty() {
|
||
continue;
|
||
}
|
||
|
||
let instances: Vec<InstanceData2d> = template
|
||
.colliders
|
||
.iter_mut()
|
||
.filter_map(|ic| {
|
||
let co = colliders.get(ic.collider)?;
|
||
let co_pos = *co.position() * ic.delta;
|
||
|
||
// Get actual color (might have been updated)
|
||
let bcolor = ic
|
||
.body
|
||
.and_then(|b| self.b2color.get(&b).copied())
|
||
.unwrap_or(ic.color);
|
||
let ccolor = self
|
||
.c2color
|
||
.get(&ic.collider)
|
||
.copied()
|
||
.unwrap_or(bcolor)
|
||
.with_alpha(ic.color.a);
|
||
let color = ic.tmp_color.take().unwrap_or(ccolor);
|
||
|
||
// Build position
|
||
let pos = co_pos.translation + self.gfx_shift;
|
||
let pos = Vec2::new(pos.x as f32, pos.y as f32);
|
||
|
||
// Build deformation matrix (rotation * scale)
|
||
let rot_mat = co_pos.rotation.to_mat();
|
||
let rot_mat = Mat2::from_cols_array(&[
|
||
rot_mat.x_axis.x as f32,
|
||
rot_mat.x_axis.y as f32,
|
||
rot_mat.y_axis.x as f32,
|
||
rot_mat.y_axis.y as f32,
|
||
]);
|
||
|
||
// Scale based on shape type
|
||
let scale = match template_type {
|
||
ShapeTemplateType::Ball => {
|
||
// Ball template is unit radius, scale uniformly
|
||
Vec2::splat(ic.half_extents.x as f32)
|
||
}
|
||
ShapeTemplateType::Cuboid => {
|
||
// Cuboid template is 2x2, scale by half_extents
|
||
Vec2::new(ic.half_extents.x as f32, ic.half_extents.y as f32)
|
||
}
|
||
};
|
||
|
||
let scale_mat = Mat2::from_diagonal(scale);
|
||
let deformation = rot_mat * scale_mat;
|
||
|
||
Some(InstanceData2d {
|
||
position: pos,
|
||
deformation,
|
||
color: color.into(),
|
||
lines_color: None,
|
||
lines_width: None,
|
||
points_color: None,
|
||
points_size: None,
|
||
})
|
||
})
|
||
.collect();
|
||
|
||
template.node.set_instances(&instances);
|
||
}
|
||
|
||
// Update individual nodes
|
||
for node in &mut self.individual_nodes {
|
||
node.node.set_visible(draw_surfaces);
|
||
|
||
if let Some(co) = colliders.get(node.collider) {
|
||
let co_pos = *co.position() * node.delta;
|
||
node.node.set_position(Vec2::new(
|
||
(co_pos.translation.x + self.gfx_shift.x) as f32,
|
||
(co_pos.translation.y + self.gfx_shift.y) as f32,
|
||
));
|
||
node.node.set_rotation(co_pos.rotation.angle() as f32);
|
||
node.node
|
||
.set_color(node.tmp_color.take().unwrap_or(node.color));
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
impl Default for GraphicsManager {
|
||
fn default() -> Self {
|
||
Self::new()
|
||
}
|
||
}
|