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

avian3d/debug_render/
gizmos.rs
