rapier_testbed3d_f64/
graphics.rs

1#![allow(clippy::unnecessary_cast, clippy::useless_conversion)] // Casts/conversions are needed for switching between f32/f64.
2
3use crate::testbed::TestbedStateFlags;
4use kiss3d::prelude::*;
5use rand_pcg::Pcg32;
6use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
7use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
8use std::collections::HashMap;
9
10#[cfg(feature = "dim2")]
11pub use kiss3d::prelude::SceneNode2d as SceneNode;
12#[cfg(feature = "dim3")]
13pub use kiss3d::prelude::SceneNode3d as SceneNode;
14
15/// Template key identifies a shape type that can be instanced together
16#[derive(Hash, Eq, PartialEq, Clone, Debug)]
17pub enum ShapeTemplateType {
18    Ball,
19    Cuboid,
20    // Capsule,
21    #[cfg(feature = "dim3")]
22    Cylinder,
23    #[cfg(feature = "dim3")]
24    Cone,
25}
26
27/// Info about an instanced collider
28#[derive(Clone)]
29pub struct InstancedCollider {
30    pub collider: ColliderHandle,
31    pub body: Option<RigidBodyHandle>,
32    pub delta: rapier::math::Pose,
33    pub half_extents: rapier::math::Vector,
34    pub color: Color,
35    pub tmp_color: Option<Color>,
36}
37
38/// A template node that can render multiple instances
39pub struct ShapeTemplate {
40    pub node: SceneNode,
41    pub colliders: Vec<InstancedCollider>,
42}
43
44/// Non-instanced node for complex shapes
45pub struct IndividualNode {
46    pub node: SceneNode,
47    pub collider: ColliderHandle,
48    pub delta: rapier::math::Pose,
49    pub color: Color,
50    pub tmp_color: Option<Color>,
51}
52
53/// Location of a collider in the graphics system
54#[derive(Clone, Debug)]
55pub enum NodeLocation {
56    Instanced {
57        template: ShapeTemplateType,
58        index: usize,
59    },
60    Individual {
61        index: usize,
62    },
63}
64
65pub struct GraphicsManager {
66    scene: SceneNode,
67    rand: Pcg32,
68    /// Template nodes for instanced primitives
69    templates: HashMap<ShapeTemplateType, ShapeTemplate>,
70    /// Individual nodes for complex shapes (trimesh, heightfield, etc.)
71    individual_nodes: Vec<IndividualNode>,
72    /// Map from collider to its render nodes
73    c2nodes: HashMap<ColliderHandle, NodeLocation>,
74    /// Colliders attached to a particular body, used to identify the
75    /// body’s nodes even if it has been removed from the RigidBodySet.
76    b2colliders: HashMap<RigidBodyHandle, Vec<ColliderHandle>>,
77    /// Colors per body
78    b2color: HashMap<RigidBodyHandle, Color>,
79    /// Colors per collider (overrides body color)
80    c2color: HashMap<ColliderHandle, Color>,
81    /// Wireframe mode per body
82    b2wireframe: HashMap<RigidBodyHandle, bool>,
83    /// Default color for fixed/ground bodies
84    ground_color: Color,
85    /// Graphics offset
86    pub gfx_shift: rapier::math::Vector,
87    /// Global wireframe mode
88    wireframe_mode: bool,
89}
90
91const GROUND_COLOR: Color = Color {
92    r: 0.58,
93    g: 0.54,
94    b: 0.50,
95    a: 1.0,
96};
97
98impl GraphicsManager {
99    pub fn new() -> GraphicsManager {
100        GraphicsManager {
101            scene: Default::default(),
102            rand: Pcg32::new(0, 1),
103            templates: HashMap::new(),
104            individual_nodes: Vec::new(),
105            c2nodes: HashMap::new(),
106            b2colliders: HashMap::new(),
107            b2color: HashMap::new(),
108            c2color: HashMap::new(),
109            b2wireframe: HashMap::new(),
110            ground_color: GROUND_COLOR,
111            gfx_shift: rapier::math::Vector::ZERO,
112            wireframe_mode: false,
113        }
114    }
115
116    pub fn clear(&mut self) {
117        self.scene = SceneNode::empty();
118        #[cfg(feature = "dim3")]
119        {
120            // Setup lights.
121            let mut light = self
122                .scene
123                .add_light(Light::directional(Vec3::new(-1.0, -1.0, -1.0)));
124            light.set_position(Vec3::new(100.0, 100.0, 100.0));
125
126            let mut light = self
127                .scene
128                .add_light(Light::point(10000.0).with_intensity(1.0));
129            light.set_position(Vec3::new(-100.0, 100.0, -100.0));
130        }
131
132        self.templates.clear();
133        self.individual_nodes.clear();
134        self.c2nodes.clear();
135        self.b2colliders.clear();
136        self.c2color.clear();
137        self.b2color.clear();
138        self.b2wireframe.clear();
139        self.rand = Pcg32::new(0, 1);
140    }
141
142    pub fn scene(&self) -> &SceneNode {
143        &self.scene
144    }
145
146    pub fn scene_mut(&mut self) -> &mut SceneNode {
147        &mut self.scene
148    }
149
150    /// Get or create a template node for a shape type
151    #[cfg(feature = "dim3")]
152    fn get_or_create_template(&mut self, template_type: ShapeTemplateType) -> &mut ShapeTemplate {
153        self.templates
154            .entry(template_type.clone())
155            .or_insert_with(|| {
156                // Create a unit-sized template node
157                let node = match template_type {
158                    ShapeTemplateType::Ball => self.scene.add_sphere_with_subdiv(1.0, 20, 20),
159                    ShapeTemplateType::Cuboid => self.scene.add_cube(2.0, 2.0, 2.0),
160                    // ShapeTemplateType::Capsule => self.scene.add_capsule(1.0, 2.0),
161                    ShapeTemplateType::Cylinder => self.scene.add_cylinder(1.0, 2.0),
162                    ShapeTemplateType::Cone => self.scene.add_cone(1.0, 2.0),
163                };
164                ShapeTemplate {
165                    node,
166                    colliders: Vec::new(),
167                }
168            })
169    }
170
171    #[cfg(feature = "dim2")]
172    fn get_or_create_template(&mut self, template_type: ShapeTemplateType) -> &mut ShapeTemplate {
173        self.templates
174            .entry(template_type.clone())
175            .or_insert_with(|| {
176                // Create a unit-sized template node
177                let node = match template_type {
178                    ShapeTemplateType::Ball => self.scene.add_circle(1.0),
179                    ShapeTemplateType::Cuboid => self.scene.add_rectangle(2.0, 2.0),
180                    // ShapeTemplateType::Capsule => {
181                    //     // Use proper 2D capsule with unit radius and unit half-height
182                    //     window.add_capsule_2d(1.0, 2.0)
183                    // }
184                };
185                ShapeTemplate {
186                    node,
187                    colliders: Vec::new(),
188                }
189            })
190    }
191
192    /// Determine the template type for a shape, if it can be instanced
193    fn shape_template_type(shape: &dyn Shape) -> Option<ShapeTemplateType> {
194        match shape.shape_type() {
195            ShapeType::Ball => Some(ShapeTemplateType::Ball),
196            ShapeType::Cuboid | ShapeType::RoundCuboid => Some(ShapeTemplateType::Cuboid),
197            // ShapeType::Capsule => Some(ShapeTemplateType::Capsule),
198            #[cfg(feature = "dim3")]
199            ShapeType::Cylinder | ShapeType::RoundCylinder => Some(ShapeTemplateType::Cylinder),
200            #[cfg(feature = "dim3")]
201            ShapeType::Cone | ShapeType::RoundCone => Some(ShapeTemplateType::Cone),
202            _ => None,
203        }
204    }
205
206    /// Get half-extents for a shape (used for scaling instances)
207    #[cfg(feature = "dim3")]
208    fn shape_half_extents(shape: &dyn Shape) -> rapier::math::Vector {
209        match shape.shape_type() {
210            ShapeType::Ball => {
211                let r = shape.as_ball().unwrap().radius;
212                rapier::math::Vector::new(r, r, r)
213            }
214            ShapeType::Cuboid => shape.as_cuboid().unwrap().half_extents,
215            ShapeType::RoundCuboid => shape.as_round_cuboid().unwrap().inner_shape.half_extents,
216            ShapeType::Capsule => {
217                let c = shape.as_capsule().unwrap();
218                rapier::math::Vector::new(c.radius, c.half_height() + c.radius, c.radius)
219            }
220            ShapeType::Cylinder => {
221                let c = shape.as_cylinder().unwrap();
222                rapier::math::Vector::new(c.radius, c.half_height, c.radius)
223            }
224            ShapeType::RoundCylinder => {
225                let c = &shape.as_round_cylinder().unwrap().inner_shape;
226                rapier::math::Vector::new(c.radius, c.half_height, c.radius)
227            }
228            ShapeType::Cone => {
229                let c = shape.as_cone().unwrap();
230                rapier::math::Vector::new(c.radius, c.half_height, c.radius)
231            }
232            ShapeType::RoundCone => {
233                let c = &shape.as_round_cone().unwrap().inner_shape;
234                rapier::math::Vector::new(c.radius, c.half_height, c.radius)
235            }
236            _ => rapier::math::Vector::new(1.0, 1.0, 1.0),
237        }
238    }
239
240    #[cfg(feature = "dim2")]
241    fn shape_half_extents(shape: &dyn Shape) -> rapier::math::Vector {
242        match shape.shape_type() {
243            ShapeType::Ball => {
244                let r = shape.as_ball().unwrap().radius;
245                rapier::math::Vector::new(r, r)
246            }
247            ShapeType::Cuboid => shape.as_cuboid().unwrap().half_extents,
248            ShapeType::RoundCuboid => shape.as_round_cuboid().unwrap().inner_shape.half_extents,
249            ShapeType::Capsule => {
250                let c = shape.as_capsule().unwrap();
251                rapier::math::Vector::new(c.radius, c.half_height() + c.radius)
252            }
253            _ => rapier::math::Vector::new(1.0, 1.0),
254        }
255    }
256
257    fn remove_node(&mut self, location: NodeLocation) {
258        match location {
259            NodeLocation::Instanced { template, index } => {
260                if let Some(tmpl) = self.templates.get_mut(&template) {
261                    // Remove by swapping with last (O(1) removal)
262                    if index < tmpl.colliders.len() {
263                        tmpl.colliders.swap_remove(index);
264                        // Update the location of the swapped element
265                        if index < tmpl.colliders.len() {
266                            let swapped_collider = tmpl.colliders[index].collider;
267                            self.c2nodes.insert(
268                                swapped_collider,
269                                NodeLocation::Instanced {
270                                    template: template.clone(),
271                                    index,
272                                },
273                            );
274                        }
275                    }
276                }
277            }
278            NodeLocation::Individual { index } => {
279                if index < self.individual_nodes.len() {
280                    self.individual_nodes[index].node.detach();
281                    self.individual_nodes.swap_remove(index);
282                    // Update the location of the swapped element
283                    if index < self.individual_nodes.len() {
284                        let swapped_collider = self.individual_nodes[index].collider;
285                        self.c2nodes
286                            .insert(swapped_collider, NodeLocation::Individual { index });
287                    }
288                }
289            }
290        }
291    }
292
293    pub fn remove_collider_nodes(&mut self, collider: ColliderHandle) {
294        if let Some(location) = self.c2nodes.remove(&collider) {
295            self.remove_node(location);
296        }
297    }
298
299    pub fn remove_body_nodes(&mut self, body: RigidBodyHandle) {
300        if let Some(colliders) = self.b2colliders.get(&body).cloned() {
301            for collider in colliders {
302                self.remove_collider_nodes(collider);
303            }
304        }
305    }
306
307    pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Color, tmp_color: bool) {
308        if !tmp_color {
309            self.b2color.insert(b, color);
310        }
311
312        if let Some(colls) = self.b2colliders.get(&b) {
313            for co in colls.iter() {
314                match &self.c2nodes[co] {
315                    NodeLocation::Individual { index } => {
316                        if tmp_color {
317                            self.individual_nodes[*index].tmp_color = Some(color);
318                        } else {
319                            self.individual_nodes[*index].color = color;
320                        }
321                    }
322                    NodeLocation::Instanced { template, index } => {
323                        let instances = &mut self.templates.get_mut(template).unwrap();
324                        if tmp_color {
325                            instances.colliders[*index].tmp_color = Some(color);
326                        } else {
327                            instances.colliders[*index].color = color;
328                        }
329                    }
330                }
331            }
332        }
333    }
334
335    pub fn set_initial_body_color(&mut self, b: RigidBodyHandle, color: Color) {
336        self.b2color.insert(b, color);
337    }
338
339    pub fn set_initial_collider_color(&mut self, c: ColliderHandle, color: Color) {
340        self.c2color.insert(c, color);
341    }
342
343    pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) {
344        self.b2wireframe.insert(b, enabled);
345    }
346
347    pub fn toggle_wireframe_mode(&mut self, _colliders: &ColliderSet, enabled: bool) {
348        self.wireframe_mode = enabled;
349
350        // // Update template nodes
351        // for tmpl in self.templates.values_mut() {
352        //     if enabled {
353        //         tmpl.node.set_lines_width(1.0, false);
354        //         tmpl.node.set_surface_rendering_activation(false);
355        //     } else {
356        //         tmpl.node.set_lines_width(0.0, false);
357        //         tmpl.node.set_surface_rendering_activation(true);
358        //     }
359        // }
360        //
361        // // Update individual nodes
362        // for node in &mut self.individual_nodes {
363        //     if enabled {
364        //         node.node.set_lines_width(1.0, false);
365        //         node.node.set_surface_rendering_activation(false);
366        //     } else {
367        //         node.node.set_lines_width(0.0, false);
368        //         node.node.set_surface_rendering_activation(true);
369        //     }
370        // }
371    }
372
373    pub fn next_color(&mut self) -> Rgba<f32> {
374        Self::gen_color(&mut self.rand)
375    }
376
377    fn gen_color(rng: &mut Pcg32) -> Rgba<f32> {
378        use rand::Rng;
379        let [r, g, b]: [f32; 3] = rng.random();
380        [r, g, b, 1.0].into()
381    }
382
383    fn alloc_color(&mut self, handle: RigidBodyHandle, is_fixed: bool) -> Color {
384        let mut color = self.ground_color;
385
386        if !is_fixed {
387            match self.b2color.get(&handle).cloned() {
388                Some(c) => color = c,
389                None => color = Self::gen_color(&mut self.rand),
390            }
391        }
392
393        self.b2color.insert(handle, color);
394        color
395    }
396
397    pub fn add_body_colliders(
398        &mut self,
399        window: &mut Window,
400        handle: RigidBodyHandle,
401        bodies: &RigidBodySet,
402        colliders: &ColliderSet,
403    ) {
404        let body = bodies.get(handle).unwrap();
405
406        let color = self
407            .b2color
408            .get(&handle)
409            .cloned()
410            .unwrap_or_else(|| self.alloc_color(handle, !body.is_dynamic()));
411
412        self.add_body_colliders_with_color(window, handle, bodies, colliders, color);
413    }
414
415    pub fn add_body_colliders_with_color(
416        &mut self,
417        window: &mut Window,
418        handle: RigidBodyHandle,
419        bodies: &RigidBodySet,
420        colliders: &ColliderSet,
421        color: Color,
422    ) {
423        for collider_handle in bodies[handle].colliders() {
424            let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
425            let collider = &colliders[*collider_handle];
426            self.add_shape(
427                window,
428                *collider_handle,
429                Some(handle),
430                collider.shape(),
431                collider.is_sensor(),
432                rapier::math::Pose::IDENTITY,
433                color,
434            );
435        }
436    }
437
438    pub fn add_collider(
439        &mut self,
440        window: &mut Window,
441        handle: ColliderHandle,
442        colliders: &ColliderSet,
443    ) {
444        let collider = &colliders[handle];
445        let collider_parent = collider.parent();
446        let color = collider_parent
447            .and_then(|p| self.b2color.get(&p).copied())
448            .unwrap_or(self.ground_color);
449        let color = self.c2color.get(&handle).copied().unwrap_or(color);
450        self.add_shape(
451            window,
452            handle,
453            collider_parent,
454            collider.shape(),
455            collider.is_sensor(),
456            rapier::math::Pose::IDENTITY,
457            color,
458        );
459    }
460
461    pub fn add_shape(
462        &mut self,
463        _window: &mut Window,
464        handle: ColliderHandle,
465        body: Option<RigidBodyHandle>,
466        shape: &dyn Shape,
467        sensor: bool,
468        delta: rapier::math::Pose,
469        color: Color,
470    ) {
471        if let Some(body) = body {
472            let colls = self.b2colliders.entry(body).or_default();
473            if !colls.contains(&handle) {
474                colls.push(handle);
475            }
476        }
477
478        // Handle compound shapes recursively
479        if let Some(compound) = shape.as_compound() {
480            for (shape_pos, sub_shape) in compound.shapes() {
481                self.add_shape(
482                    _window,
483                    handle,
484                    body,
485                    &**sub_shape,
486                    sensor,
487                    *shape_pos * delta,
488                    color,
489                );
490            }
491            return;
492        }
493
494        let opacity = if sensor { 0.5 } else { 1.0 };
495
496        // Try to use instancing for primitive shapes
497        if let Some(template_type) = Self::shape_template_type(shape) {
498            let half_extents = Self::shape_half_extents(shape);
499            let template = self.get_or_create_template(template_type.clone());
500            let index = template.colliders.len();
501            template.colliders.push(InstancedCollider {
502                collider: handle,
503                body,
504                delta,
505                half_extents,
506                color: color.with_alpha(opacity),
507                tmp_color: None,
508            });
509            self.c2nodes.insert(
510                handle,
511                NodeLocation::Instanced {
512                    template: template_type,
513                    index,
514                },
515            );
516        } else {
517            // Create individual node for complex shapes
518            if let Some(node) = Self::create_individual_node(&mut self.scene, shape, color, sensor)
519            {
520                let index = self.individual_nodes.len();
521                self.individual_nodes.push(IndividualNode {
522                    node,
523                    collider: handle,
524                    delta,
525                    color: color.with_alpha(opacity),
526                    tmp_color: None,
527                });
528                self.c2nodes
529                    .insert(handle, NodeLocation::Individual { index });
530            }
531        }
532    }
533
534    #[cfg(feature = "dim3")]
535    fn create_individual_node(
536        scene: &mut SceneNode,
537        shape: &dyn Shape,
538        color: Color,
539        sensor: bool,
540    ) -> Option<SceneNode3d> {
541        use kiss3d::procedural::{IndexBuffer, RenderMesh};
542
543        fn to_render_mesh(trimesh: &rapier::geometry::TriMesh) -> RenderMesh {
544            let vtx = trimesh
545                .vertices()
546                .iter()
547                .map(|pt| Vec3::new(pt.x as f32, pt.y as f32, pt.z as f32))
548                .collect();
549            let idx = trimesh.indices().to_vec();
550            let mut mesh = RenderMesh::new(vtx, None, None, Some(IndexBuffer::Unified(idx)));
551            mesh.replicate_vertices();
552            mesh.recompute_normals();
553            mesh
554        }
555
556        let mut node = match shape.shape_type() {
557            ShapeType::TriMesh => {
558                let trimesh = shape.as_trimesh().unwrap();
559                Some(scene.add_render_mesh(to_render_mesh(trimesh), Vec3::ONE))
560            }
561            ShapeType::HeightField => {
562                let heightfield = shape.as_heightfield().unwrap();
563                let (vertices, indices) = heightfield.to_trimesh();
564                let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
565                Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
566            }
567            ShapeType::ConvexPolyhedron | ShapeType::RoundConvexPolyhedron => {
568                let poly = shape
569                    .as_convex_polyhedron()
570                    .or_else(|| shape.as_round_convex_polyhedron().map(|rp| &rp.inner_shape));
571                poly.map(|p| {
572                    let (vertices, indices) = p.to_trimesh();
573                    let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
574                    scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE)
575                })
576            }
577            ShapeType::Capsule => {
578                let caps = shape.as_capsule().unwrap();
579                let pose = caps.canonical_transform();
580                let mut parent = scene.add_group();
581                let mut node =
582                    parent.add_capsule(caps.radius as f32, caps.half_height() as f32 * 2.0);
583                node.set_pose(pose.into());
584                Some(parent)
585            }
586            ShapeType::Triangle => {
587                let tri = shape.as_triangle().unwrap();
588                let vertices = vec![tri.a, tri.b, tri.c];
589                let indices = vec![[0u32, 1, 2], [0u32, 2, 1]];
590                let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
591                Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
592            }
593            ShapeType::HalfSpace => {
594                let mut parent = scene.add_group();
595                parent.rotate(Quat::from_axis_angle(Vec3::X, -std::f32::consts::FRAC_PI_2));
596                let node = parent.add_quad(2000.0, 2000.0, 1, 1);
597                Some(node)
598            }
599            ShapeType::Voxels => {
600                let voxels = shape.as_voxels().unwrap();
601                let (vertices, indices) = voxels.to_trimesh();
602                let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap();
603                Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE))
604            }
605            _ => None,
606        };
607
608        if let Some(ref mut n) = node {
609            n.set_color(color);
610            if sensor {
611                n.set_surface_rendering_activation(false);
612                n.set_lines_width(1.0, false);
613            }
614        }
615
616        node
617    }
618
619    #[cfg(feature = "dim2")]
620    fn create_individual_node(
621        scene: &mut SceneNode,
622        shape: &dyn Shape,
623        color: Color,
624        _sensor: bool,
625    ) -> Option<SceneNode2d> {
626        let mut node = match shape.shape_type() {
627            ShapeType::Triangle => {
628                let tri = shape.as_triangle().unwrap();
629                let vertices: Vec<Vec2> = vec![
630                    Vec2::new(tri.a.x as f32, tri.a.y as f32),
631                    Vec2::new(tri.b.x as f32, tri.b.y as f32),
632                    Vec2::new(tri.c.x as f32, tri.c.y as f32),
633                ];
634                Some(scene.add_convex_polygon(vertices, Vec2::ONE))
635            }
636            ShapeType::TriMesh => {
637                let trimesh = shape.as_trimesh().unwrap();
638                let vertices: Vec<Vec2> = trimesh
639                    .vertices()
640                    .iter()
641                    .map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
642                    .collect();
643                let render_mesh = GpuMesh2d::new(vertices, trimesh.indices().to_vec(), None, false);
644                Some(scene.add_mesh(Rc::new(RefCell::new(render_mesh)), Vec2::ONE))
645            }
646            ShapeType::ConvexPolygon | ShapeType::RoundConvexPolygon => {
647                let poly = shape
648                    .as_convex_polygon()
649                    .or_else(|| shape.as_round_convex_polygon().map(|rp| &rp.inner_shape));
650                poly.map(|p| {
651                    let vertices: Vec<Vec2> = p
652                        .points()
653                        .iter()
654                        .map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
655                        .collect();
656                    scene.add_convex_polygon(vertices, Vec2::ONE)
657                })
658            }
659            ShapeType::Capsule => {
660                let caps = shape.as_capsule().unwrap();
661                let pose = caps.canonical_transform();
662                let mut parent = scene.add_group();
663                let mut node =
664                    parent.add_capsule(caps.radius as f32, caps.half_height() as f32 * 2.0);
665                node.set_pose(pose.into());
666                Some(parent)
667            }
668            ShapeType::Segment => {
669                // Render segment as a thin quad with some thickness
670                let seg = shape.as_segment().unwrap();
671                let vertices = vec![
672                    Vec2::new(seg.a.x as f32, seg.a.y as f32),
673                    Vec2::new(seg.b.x as f32, seg.b.y as f32),
674                ];
675                Some(scene.add_polyline(vertices, None, 0.1))
676            }
677            ShapeType::Polyline => {
678                // Render polyline as connected thin quads
679                let polyline = shape.as_polyline().unwrap();
680                let vertices: Vec<Vec2> = polyline
681                    .vertices()
682                    .iter()
683                    .map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
684                    .collect();
685                Some(
686                    scene
687                        .add_polyline(vertices, Some(polyline.indices().to_vec()), 0.2)
688                        .set_lines_color(Some(GROUND_COLOR)),
689                )
690            }
691            ShapeType::HeightField => {
692                let hf = shape.as_heightfield().unwrap();
693                let (polyline_verts, indices) = hf.to_polyline();
694                let vertices: Vec<Vec2> = polyline_verts
695                    .iter()
696                    .map(|pt| Vec2::new(pt.x as f32, pt.y as f32))
697                    .collect();
698                Some(
699                    scene
700                        .add_polyline(vertices, Some(indices), 0.2)
701                        .set_lines_color(Some(GROUND_COLOR)),
702                )
703            }
704            ShapeType::Voxels => {
705                let voxels = shape.as_voxels().unwrap();
706
707                let mut vtx = vec![];
708                let mut idx = vec![];
709                let sz = voxels.voxel_size() / 2.0;
710                for vox in voxels.voxels() {
711                    if !vox.state.is_empty() {
712                        let bid = vtx.len() as u32;
713                        let center = Vec2::new(vox.center.x as f32, vox.center.y as f32);
714                        vtx.push(center + Vec2::new(sz.x as f32, sz.y as f32));
715                        vtx.push(center + Vec2::new(-sz.x as f32, sz.y as f32));
716                        vtx.push(center + Vec2::new(-sz.x as f32, -sz.y as f32));
717                        vtx.push(center + Vec2::new(sz.x as f32, -sz.y as f32));
718                        idx.push([bid, bid + 1, bid + 2]);
719                        idx.push([bid + 2, bid + 3, bid]);
720                    }
721                }
722
723                let mesh = GpuMesh2d::new(vtx, idx, None, false);
724                Some(scene.add_mesh(Rc::new(RefCell::new(mesh)), Vec2::ONE))
725            }
726            _ => None,
727        };
728
729        if let Some(ref mut n) = node {
730            n.set_color(color);
731        }
732
733        node
734    }
735
736    #[cfg(feature = "dim3")]
737    pub fn draw(
738        &mut self,
739        flags: TestbedStateFlags,
740        _bodies: &RigidBodySet,
741        colliders: &ColliderSet,
742    ) {
743        let draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES);
744
745        // Update instance data for all templates
746        for (template_type, template) in &mut self.templates {
747            template
748                .node
749                .set_visible(draw_surfaces && !template.colliders.is_empty());
750
751            if template.colliders.is_empty() {
752                continue;
753            }
754
755            let instances: Vec<InstanceData3d> = template
756                .colliders
757                .iter_mut()
758                .filter_map(|ic| {
759                    let co = colliders.get(ic.collider)?;
760                    let co_pos = *co.position() * ic.delta;
761
762                    // Get actual color (might have been updated)
763                    let bcolor = ic
764                        .body
765                        .and_then(|b| self.b2color.get(&b).copied())
766                        .unwrap_or(ic.color);
767                    let ccolor = self
768                        .c2color
769                        .get(&ic.collider)
770                        .copied()
771                        .unwrap_or(bcolor)
772                        .with_alpha(ic.color.a);
773                    let color = ic.tmp_color.take().unwrap_or(ccolor);
774
775                    // Build position
776                    let pos = Vec3::new(
777                        (co_pos.translation.x + self.gfx_shift.x) as f32,
778                        (co_pos.translation.y + self.gfx_shift.y) as f32,
779                        (co_pos.translation.z + self.gfx_shift.z) as f32,
780                    );
781
782                    // Build deformation matrix (rotation * scale)
783                    let rot = Quat::from_xyzw(
784                        co_pos.rotation.x as f32,
785                        co_pos.rotation.y as f32,
786                        co_pos.rotation.z as f32,
787                        co_pos.rotation.w as f32,
788                    );
789                    let rot_mat = Mat3::from_quat(rot);
790
791                    // Scale based on shape type
792                    let scale = match template_type {
793                        ShapeTemplateType::Ball => {
794                            // Ball template is unit radius, scale uniformly
795                            Vec3::splat(ic.half_extents.x as f32)
796                        }
797                        ShapeTemplateType::Cuboid => {
798                            // Cuboid template is 2x2x2, scale by half_extents
799                            Vec3::new(
800                                ic.half_extents.x as f32,
801                                ic.half_extents.y as f32,
802                                ic.half_extents.z as f32,
803                            )
804                        }
805                        // ShapeTemplateType::Capsule => {
806                        //     // Capsule template has radius 1, half_height 1
807                        //     // half_extents = (radius, half_height + radius, radius)
808                        //     let radius = ic.half_extents.x as f32;
809                        //     let half_height = ic.half_extents.y as f32 - radius;
810                        //     Vec3::new(radius, (half_height + radius).max(radius), radius)
811                        // }
812                        ShapeTemplateType::Cylinder => {
813                            // Cylinder template has radius 1, half_height 1
814                            Vec3::new(
815                                ic.half_extents.x as f32,
816                                ic.half_extents.y as f32,
817                                ic.half_extents.z as f32,
818                            )
819                        }
820                        ShapeTemplateType::Cone => {
821                            // Cone template has radius 1, half_height 1
822                            Vec3::new(
823                                ic.half_extents.x as f32,
824                                ic.half_extents.y as f32,
825                                ic.half_extents.z as f32,
826                            )
827                        }
828                    };
829
830                    let scale_mat = Mat3::from_diagonal(scale);
831                    let deformation = rot_mat * scale_mat;
832
833                    Some(InstanceData3d {
834                        position: pos,
835                        deformation,
836                        color,
837                        lines_color: None,
838                        lines_width: None,
839                        points_color: None,
840                        points_size: None,
841                    })
842                })
843                .collect();
844
845            template.node.set_instances(&instances);
846        }
847
848        // Update individual nodes
849        for node in &mut self.individual_nodes {
850            node.node.set_visible(draw_surfaces);
851
852            if let Some(co) = colliders.get(node.collider) {
853                let co_pos = *co.position() * node.delta;
854                node.node
855                    .set_pose(co_pos.append_translation(self.gfx_shift).into());
856                node.node
857                    .set_color(node.tmp_color.take().unwrap_or(node.color));
858            }
859        }
860    }
861
862    #[cfg(feature = "dim2")]
863    pub fn draw(
864        &mut self,
865        flags: TestbedStateFlags,
866        _bodies: &RigidBodySet,
867        colliders: &ColliderSet,
868    ) {
869        let draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES);
870
871        // Update instance data for all templates
872        for (template_type, template) in &mut self.templates {
873            template
874                .node
875                .set_visible(draw_surfaces && !template.colliders.is_empty());
876
877            if template.colliders.is_empty() {
878                continue;
879            }
880
881            let instances: Vec<InstanceData2d> = template
882                .colliders
883                .iter_mut()
884                .filter_map(|ic| {
885                    let co = colliders.get(ic.collider)?;
886                    let co_pos = *co.position() * ic.delta;
887
888                    // Get actual color (might have been updated)
889                    let bcolor = ic
890                        .body
891                        .and_then(|b| self.b2color.get(&b).copied())
892                        .unwrap_or(ic.color);
893                    let ccolor = self
894                        .c2color
895                        .get(&ic.collider)
896                        .copied()
897                        .unwrap_or(bcolor)
898                        .with_alpha(ic.color.a);
899                    let color = ic.tmp_color.take().unwrap_or(ccolor);
900
901                    // Build position
902                    let pos = co_pos.translation + self.gfx_shift;
903                    let pos = Vec2::new(pos.x as f32, pos.y as f32);
904
905                    // Build deformation matrix (rotation * scale)
906                    let rot_mat = co_pos.rotation.to_mat();
907                    let rot_mat = Mat2::from_cols_array(&[
908                        rot_mat.x_axis.x as f32,
909                        rot_mat.x_axis.y as f32,
910                        rot_mat.y_axis.x as f32,
911                        rot_mat.y_axis.y as f32,
912                    ]);
913
914                    // Scale based on shape type
915                    let scale = match template_type {
916                        ShapeTemplateType::Ball => {
917                            // Ball template is unit radius, scale uniformly
918                            Vec2::splat(ic.half_extents.x as f32)
919                        }
920                        ShapeTemplateType::Cuboid => {
921                            // Cuboid template is 2x2, scale by half_extents
922                            Vec2::new(ic.half_extents.x as f32, ic.half_extents.y as f32)
923                        }
924                    };
925
926                    let scale_mat = Mat2::from_diagonal(scale);
927                    let deformation = rot_mat * scale_mat;
928
929                    Some(InstanceData2d {
930                        position: pos,
931                        deformation,
932                        color: color.into(),
933                        lines_color: None,
934                        lines_width: None,
935                        points_color: None,
936                        points_size: None,
937                    })
938                })
939                .collect();
940
941            template.node.set_instances(&instances);
942        }
943
944        // Update individual nodes
945        for node in &mut self.individual_nodes {
946            node.node.set_visible(draw_surfaces);
947
948            if let Some(co) = colliders.get(node.collider) {
949                let co_pos = *co.position() * node.delta;
950                node.node.set_position(Vec2::new(
951                    (co_pos.translation.x + self.gfx_shift.x) as f32,
952                    (co_pos.translation.y + self.gfx_shift.y) as f32,
953                ));
954                node.node.set_rotation(co_pos.rotation.angle() as f32);
955                node.node
956                    .set_color(node.tmp_color.take().unwrap_or(node.color));
957            }
958        }
959    }
960}
961
962impl Default for GraphicsManager {
963    fn default() -> Self {
964        Self::new()
965    }
966}
rapier_testbed3d_f64/graphics.rs

rapier_testbed3d_f64/
graphics.rs
