avian3d/debug_render/
gizmos.rs

1#![allow(clippy::unnecessary_cast)]
2
3use crate::prelude::*;
4use bevy::prelude::*;
5#[cfg(all(
6    feature = "default-collider",
7    any(feature = "parry-f32", feature = "parry-f64")
8))]
9use parry::shape::{SharedShape, TypedShape};
10
11/// An extension trait for `Gizmos<PhysicsGizmo>`.
12pub trait PhysicsGizmoExt {
13    /// Draws a line from `a` to `b`.
14    fn draw_line(&mut self, a: RVector, b: RVector, color: Color);
15
16    /// Draws lines between a list of points.
17    fn draw_line_strip(
18        &mut self,
19        points: Vec<RVector>,
20        position: RVector,
21        rotation: impl Into<Rot>,
22        closed: bool,
23        color: Color,
24    );
25
26    /// Draws a polyline based on the given vertex and index buffers.
27    fn draw_polyline(
28        &mut self,
29        vertices: &[RVector],
30        indices: &[[u32; 2]],
31        position: RVector,
32        rotation: impl Into<Rot>,
33        color: Color,
34    );
35
36    /// Draws an arrow from `a` to `b` with an arrowhead that has a length of `head_length`.
37    fn draw_arrow(&mut self, a: RVector, b: RVector, head_length: f32, color: Color);
38
39    /// Draws a [`Collider`] shape.
40    #[cfg(all(
41        feature = "default-collider",
42        any(feature = "parry-f32", feature = "parry-f64")
43    ))]
44    fn draw_collider(
45        &mut self,
46        collider: &Collider,
47        position: RVector,
48        rotation: impl Into<Rot>,
49        color: Color,
50    );
51
52    /// Draws the results of a [raycast](SpatialQuery#raycasting).
53    #[allow(clippy::too_many_arguments)]
54    fn draw_raycast(
55        &mut self,
56        origin: RVector,
57        direction: Dir,
58        max_distance: f32,
59        hits: &[RayHitData],
60        ray_color: Color,
61        point_color: Color,
62        normal_color: Color,
63        length_unit: f32,
64    );
65
66    /// Draws the results of a [shapecast](SpatialQuery#shapecasting).
67    #[allow(clippy::too_many_arguments)]
68    #[cfg(all(
69        feature = "default-collider",
70        any(feature = "parry-f32", feature = "parry-f64")
71    ))]
72    fn draw_shapecast(
73        &mut self,
74        shape: &Collider,
75        origin: RVector,
76        shape_rotation: impl Into<Rot>,
77        direction: Dir,
78        max_distance: f32,
79        hits: &[ShapeHitData],
80        ray_color: Color,
81        shape_color: Color,
82        point_color: Color,
83        normal_color: Color,
84        length_unit: f32,
85    );
86}
87
88impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> {
89    /// Draws a line from `a` to `b`.
90    fn draw_line(&mut self, a: RVector, b: RVector, color: Color) {
91        #[cfg(feature = "2d")]
92        self.line_2d(a.f32(), b.f32(), color);
93        #[cfg(feature = "3d")]
94        self.line(a.f32(), b.f32(), color);
95    }
96
97    /// Draws lines between a list of points.
98    fn draw_line_strip(
99        &mut self,
100        points: Vec<RVector>,
101        position: RVector,
102        rotation: impl Into<Rot>,
103        closed: bool,
104        color: Color,
105    ) {
106        let rotation = rotation.into();
107
108        let pos = position.f32();
109        #[cfg(feature = "2d")]
110        self.linestrip_2d(points.iter().map(|p| pos + rotation * p.f32()), color);
111        #[cfg(feature = "3d")]
112        self.linestrip(points.iter().map(|p| pos + rotation * p.f32()), color);
113
114        if closed && points.len() > 2 {
115            let a = position + rotation.real() * points[0];
116            let b = position + rotation.real() * *points.last().unwrap();
117            self.draw_line(a, b, color);
118        }
119    }
120
121    /// Draws a polyline based on the given vertex and index buffers.
122    fn draw_polyline(
123        &mut self,
124        vertices: &[RVector],
125        indices: &[[u32; 2]],
126        position: RVector,
127        rotation: impl Into<Rot>,
128        color: Color,
129    ) {
130        let rotation = rotation.into().real();
131
132        for [i1, i2] in indices {
133            let a = position + rotation * vertices[*i1 as usize];
134            let b = position + rotation * vertices[*i2 as usize];
135            self.draw_line(a, b, color);
136        }
137    }
138
139    /// Draws an arrow from `a` to `b` with an arrowhead that has a length of `head_length`
140    /// and a width of `head_width`.
141    fn draw_arrow(&mut self, a: RVector, b: RVector, head_length: f32, color: Color) {
142        #[cfg(feature = "2d")]
143        {
144            self.arrow_2d(a.f32(), b.f32(), color)
145                .with_tip_length(head_length as f32);
146        }
147
148        #[cfg(feature = "3d")]
149        {
150            self.arrow(a.f32(), b.f32(), color)
151                .with_tip_length(head_length as f32);
152        }
153    }
154
155    /// Draws a collider shape with a given position and rotation.
156    #[cfg(all(
157        feature = "default-collider",
158        any(feature = "parry-f32", feature = "parry-f64")
159    ))]
160    fn draw_collider(
161        &mut self,
162        collider: &Collider,
163        position: RVector,
164        rotation: impl Into<Rot>,
165        color: Color,
166    ) {
167        let rotation: Rot = rotation.into();
168
169        match collider.shape_scaled().as_typed_shape() {
170            #[cfg(feature = "2d")]
171            TypedShape::Ball(s) => {
172                self.circle_2d(position.f32(), s.radius as f32, color);
173            }
174            #[cfg(feature = "3d")]
175            TypedShape::Ball(s) => {
176                self.sphere(
177                    Isometry3d::new(position.f32(), rotation),
178                    s.radius as f32,
179                    color,
180                );
181            }
182            #[cfg(feature = "2d")]
183            TypedShape::Cuboid(s) => {
184                self.rect_2d(
185                    Isometry2d::new(position.f32(), rotation),
186                    2.0 * s.half_extents.f32(),
187                    color,
188                );
189            }
190            #[cfg(feature = "3d")]
191            TypedShape::Cuboid(s) => {
192                use bevy_shape::Aabb3d;
193
194                self.aabb_3d(
195                    Aabb3d::new(Vec3A::ZERO, s.half_extents.f32()),
196                    Transform::from_translation(position.f32()).with_rotation(rotation),
197                    color,
198                );
199            }
200            #[cfg(feature = "2d")]
201            TypedShape::Capsule(s) => {
202                self.draw_line_strip(s.to_polyline(32), position, rotation, true, color);
203            }
204            #[cfg(feature = "3d")]
205            TypedShape::Capsule(s) => {
206                let (vertices, indices) = s.to_outline(32);
207                self.draw_polyline(&vertices, &indices, position, rotation, color);
208            }
209            TypedShape::Segment(s) => {
210                self.draw_line_strip(vec![s.a, s.b], position, rotation, false, color)
211            }
212            TypedShape::Triangle(s) => {
213                self.draw_line_strip(vec![s.a, s.b, s.c], position, rotation, true, color)
214            }
215            TypedShape::TriMesh(s) => {
216                for tri in s.triangles() {
217                    self.draw_collider(
218                        &Collider::from(SharedShape::new(tri)),
219                        position,
220                        rotation,
221                        color,
222                    );
223                }
224            }
225            TypedShape::Polyline(s) => {
226                self.draw_polyline(s.vertices(), s.indices(), position, rotation, color)
227            }
228            #[cfg(feature = "2d")]
229            TypedShape::HalfSpace(s) => {
230                let basis = RVector::new(-s.normal.y, s.normal.x);
231                let a = basis * 10_000.0;
232                let b = basis * -10_000.0;
233                self.draw_line_strip(vec![a, b], position, rotation, false, color);
234            }
235            #[cfg(feature = "3d")]
236            TypedShape::HalfSpace(s) => {
237                let n = s.normal;
238                let sign = n.z.signum();
239                let a = -1.0 / (sign + n.z);
240                let b = n.x * n.y * a;
241                let basis1 = RVector::new(1.0 + sign * n.x * n.x * a, sign * b, -sign * n.x);
242                let basis2 = RVector::new(b, sign + n.y * n.y * a, -n.y);
243                let a = basis1 * 10_000.0;
244                let b = basis1 * -10_000.0;
245                let c = basis2 * 10_000.0;
246                let d = basis2 * -10_000.0;
247                self.draw_polyline(&[a, b, c, d], &[[0, 1], [2, 3]], position, rotation, color);
248            }
249            TypedShape::Voxels(v) => {
250                #[cfg(feature = "2d")]
251                let (vertices, indices) = v.to_polyline();
252                #[cfg(feature = "3d")]
253                let (vertices, indices) = v.to_outline();
254                self.draw_polyline(&vertices, &indices, position, rotation, color);
255            }
256            TypedShape::HeightField(s) => {
257                #[cfg(feature = "2d")]
258                for segment in s.segments() {
259                    self.draw_collider(
260                        &Collider::from(SharedShape::new(segment)),
261                        position,
262                        rotation,
263                        color,
264                    );
265                }
266                #[cfg(feature = "3d")]
267                for triangle in s.triangles() {
268                    self.draw_collider(
269                        &Collider::from(SharedShape::new(triangle)),
270                        position,
271                        rotation,
272                        color,
273                    );
274                }
275            }
276            TypedShape::Compound(s) => {
277                for (sub_pos, shape) in s.shapes() {
278                    let pos = position + rotation.real() * sub_pos.translation;
279                    #[cfg(feature = "2d")]
280                    let rot = rotation
281                        * Rot2::from_sin_cos(
282                            sub_pos.rotation.sin().f32(),
283                            sub_pos.rotation.cos().f32(),
284                        );
285                    #[cfg(feature = "3d")]
286                    let rot = rotation.mul_quat(sub_pos.rotation.f32()).normalize();
287                    self.draw_collider(&Collider::from(shape.to_owned()), pos, rot, color);
288                }
289            }
290            #[cfg(feature = "2d")]
291            TypedShape::ConvexPolygon(s) => {
292                self.draw_line_strip(s.points().to_vec(), position, rotation, true, color);
293            }
294            #[cfg(feature = "3d")]
295            TypedShape::ConvexPolyhedron(s) => {
296                let indices = s
297                    .edges()
298                    .iter()
299                    .map(|e| [e.vertices[0], e.vertices[1]])
300                    .collect::<Vec<_>>();
301                self.draw_polyline(s.points(), &indices, position, rotation, color);
302            }
303            #[cfg(feature = "3d")]
304            TypedShape::Cylinder(s) => {
305                let (vertices, indices) = s.to_outline(32);
306                self.draw_polyline(&vertices, &indices, position, rotation, color);
307            }
308            #[cfg(feature = "3d")]
309            TypedShape::Cone(s) => {
310                let (vertices, indices) = s.to_outline(32);
311                self.draw_polyline(&vertices, &indices, position, rotation, color);
312            }
313            // ------------
314            // Round shapes
315            // ------------
316            #[cfg(feature = "2d")]
317            TypedShape::RoundCuboid(s) => {
318                self.draw_line_strip(s.to_polyline(32), position, rotation, true, color);
319            }
320            #[cfg(feature = "3d")]
321            TypedShape::RoundCuboid(s) => {
322                let (vertices, indices) = s.to_outline(32);
323                self.draw_polyline(&vertices, &indices, position, rotation, color);
324            }
325            TypedShape::RoundTriangle(s) => {
326                // Parry doesn't have a method for the rounded outline, so we have to just use a normal triangle
327                // (or compute the outline manually based on the border radius)
328                self.draw_collider(
329                    &Collider::from(SharedShape::new(s.inner_shape)),
330                    position,
331                    rotation,
332                    color,
333                );
334            }
335            #[cfg(feature = "2d")]
336            TypedShape::RoundConvexPolygon(s) => {
337                self.draw_line_strip(s.to_polyline(32), position, rotation, true, color);
338            }
339            #[cfg(feature = "3d")]
340            TypedShape::RoundConvexPolyhedron(s) => {
341                let (vertices, indices) = s.to_outline(32);
342                self.draw_polyline(&vertices, &indices, position, rotation, color);
343            }
344            #[cfg(feature = "3d")]
345            TypedShape::RoundCylinder(s) => {
346                let (vertices, indices) = s.to_outline(32, 32);
347                self.draw_polyline(&vertices, &indices, position, rotation, color);
348            }
349            #[cfg(feature = "3d")]
350            TypedShape::RoundCone(s) => {
351                let (vertices, indices) = s.to_outline(32, 32);
352                self.draw_polyline(&vertices, &indices, position, rotation, color);
353            }
354            TypedShape::Custom(_id) => {
355                #[cfg(feature = "2d")]
356                {
357                    use crate::collision::collider::{
358                        EllipseColliderShape, RegularPolygonColliderShape,
359                    };
360
361                    if let Some(ellipse) =
362                        collider.shape_scaled().as_shape::<EllipseColliderShape>()
363                    {
364                        let isometry = Isometry2d::new(position.f32(), rotation);
365                        self.primitive_2d(&ellipse.0, isometry, color);
366                    } else if let Some(polygon) = collider
367                        .shape_scaled()
368                        .as_shape::<RegularPolygonColliderShape>()
369                    {
370                        let isometry = Isometry2d::new(position.f32(), rotation);
371                        self.primitive_2d(&polygon.0, isometry, color);
372                    }
373                }
374            }
375        }
376    }
377
378    /// Draws the results of a [raycast](SpatialQuery#raycasting).
379    #[allow(clippy::too_many_arguments)]
380    fn draw_raycast(
381        &mut self,
382        origin: RVector,
383        direction: Dir,
384        max_distance: f32,
385        hits: &[RayHitData],
386        ray_color: Color,
387        point_color: Color,
388        normal_color: Color,
389        length_unit: f32,
390    ) {
391        let max_distance = hits
392            .iter()
393            .max_by(|a, b| a.distance.total_cmp(&b.distance))
394            .map_or(max_distance, |hit| hit.distance.f32());
395
396        // Draw ray as arrow
397        self.draw_arrow(
398            origin,
399            origin + (direction * max_distance).real(),
400            0.1 * length_unit,
401            ray_color,
402        );
403
404        // Draw all hit points and normals
405        for hit in hits {
406            let point = origin + (direction * hit.distance).real();
407
408            // Draw hit point
409            #[cfg(feature = "2d")]
410            self.circle_2d(point.f32(), 0.1 * length_unit as f32, point_color);
411            #[cfg(feature = "3d")]
412            self.sphere(point.f32(), 0.1 * length_unit as f32, point_color);
413
414            // Draw hit normal as arrow
415            self.draw_arrow(
416                point,
417                point + (hit.normal * 0.5 * length_unit).real(),
418                0.1 * length_unit,
419                normal_color,
420            );
421        }
422    }
423
424    /// Draws the results of a [shapecast](SpatialQuery#shapecasting).
425    #[cfg(all(
426        feature = "default-collider",
427        any(feature = "parry-f32", feature = "parry-f64")
428    ))]
429    #[allow(clippy::too_many_arguments)]
430    fn draw_shapecast(
431        &mut self,
432        shape: &Collider,
433        origin: RVector,
434        shape_rotation: impl Into<Rot>,
435        direction: Dir,
436        max_distance: f32,
437        hits: &[ShapeHitData],
438        ray_color: Color,
439        shape_color: Color,
440        point_color: Color,
441        normal_color: Color,
442        length_unit: f32,
443    ) {
444        let shape_rotation = shape_rotation.into();
445        #[cfg(feature = "3d")]
446        let shape_rotation = shape_rotation.normalize();
447
448        let max_distance = hits
449            .iter()
450            .max_by(|a, b| a.distance.total_cmp(&b.distance))
451            .map_or(max_distance, |hit| hit.distance);
452
453        // Draw collider at origin
454        self.draw_collider(shape, origin, shape_rotation, shape_color);
455
456        // Draw arrow from origin to position of shape at final hit
457        // TODO: We could render the swept collider outline instead
458        self.draw_arrow(
459            origin,
460            origin + (max_distance * direction).real(),
461            0.1 * length_unit,
462            ray_color,
463        );
464
465        // Draw all hit points, normals and the shape at the hit points
466        for hit in hits {
467            // Draw hit point
468            #[cfg(feature = "2d")]
469            self.circle_2d(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
470            #[cfg(feature = "3d")]
471            self.sphere(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
472
473            // Draw hit normal as arrow
474            self.draw_arrow(
475                hit.point1,
476                hit.point1 + (hit.normal1 * 0.5 * length_unit).real(),
477                0.1 * length_unit,
478                normal_color,
479            );
480
481            // Draw collider at hit point
482            self.draw_collider(
483                shape,
484                origin + (hit.distance * direction).real(),
485                shape_rotation,
486                shape_color.with_alpha(0.3),
487            );
488        }
489    }
490}
avian3d/debug_render/gizmos.rs

avian3d/debug_render/
gizmos.rs
