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        let nalgebra_to_glam =
173            |points: &[_]| points.iter().map(|p| Vector::from(*p)).collect::<Vec<_>>();
174        match collider.shape_scaled().as_typed_shape() {
175            #[cfg(feature = "2d")]
176            TypedShape::Ball(s) => {
177                self.circle_2d(position.f32(), s.radius as f32, color);
178            }
179            #[cfg(feature = "3d")]
180            TypedShape::Ball(s) => {
181                self.sphere(
182                    Isometry3d::new(position.f32(), rotation.f32()),
183                    s.radius as f32,
184                    color,
185                );
186            }
187            #[cfg(feature = "2d")]
188            TypedShape::Cuboid(s) => {
189                self.cuboid(
190                    Transform::from_scale(Vector::from(s.half_extents).extend(0.0).f32() * 2.0)
191                        .with_translation(position.extend(0.0).f32())
192                        .with_rotation(Quaternion::from(rotation).f32()),
193                    color,
194                );
195            }
196            #[cfg(feature = "3d")]
197            TypedShape::Cuboid(s) => {
198                self.cuboid(
199                    Transform::from_scale(Vector::from(s.half_extents).f32() * 2.0)
200                        .with_translation(position.f32())
201                        .with_rotation(rotation.f32()),
202                    color,
203                );
204            }
205            #[cfg(feature = "2d")]
206            TypedShape::Capsule(s) => {
207                self.draw_line_strip(
208                    nalgebra_to_glam(&s.to_polyline(32)),
209                    position,
210                    rotation,
211                    true,
212                    color,
213                );
214            }
215            #[cfg(feature = "3d")]
216            TypedShape::Capsule(s) => {
217                let (vertices, indices) = s.to_outline(32);
218                self.draw_polyline(
219                    &nalgebra_to_glam(&vertices),
220                    &indices,
221                    position,
222                    rotation,
223                    color,
224                );
225            }
226            TypedShape::Segment(s) => self.draw_line_strip(
227                vec![s.a.into(), s.b.into()],
228                position,
229                rotation,
230                false,
231                color,
232            ),
233            TypedShape::Triangle(s) => self.draw_line_strip(
234                vec![s.a.into(), s.b.into(), s.c.into()],
235                position,
236                rotation,
237                true,
238                color,
239            ),
240            TypedShape::TriMesh(s) => {
241                for tri in s.triangles() {
242                    self.draw_collider(
243                        &Collider::from(SharedShape::new(tri)),
244                        position,
245                        rotation,
246                        color,
247                    );
248                }
249            }
250            TypedShape::Polyline(s) => self.draw_polyline(
251                &nalgebra_to_glam(s.vertices()),
252                s.indices(),
253                position,
254                rotation,
255                color,
256            ),
257            #[cfg(feature = "2d")]
258            TypedShape::HalfSpace(s) => {
259                let basis = Vector::new(-s.normal.y, s.normal.x);
260                let a = basis * 10_000.0;
261                let b = basis * -10_000.0;
262                self.draw_line_strip(vec![a, b], position, rotation, false, color);
263            }
264            #[cfg(feature = "3d")]
265            TypedShape::HalfSpace(s) => {
266                let n = s.normal;
267                let sign = n.z.signum();
268                let a = -1.0 / (sign + n.z);
269                let b = n.x * n.y * a;
270                let basis1 = Vector::new(1.0 + sign * n.x * n.x * a, sign * b, -sign * n.x);
271                let basis2 = Vector::new(b, sign + n.y * n.y * a, -n.y);
272                let a = basis1 * 10_000.0;
273                let b = basis1 * -10_000.0;
274                let c = basis2 * 10_000.0;
275                let d = basis2 * -10_000.0;
276                self.draw_polyline(&[a, b, c, d], &[[0, 1], [2, 3]], position, rotation, color);
277            }
278            TypedShape::HeightField(s) => {
279                #[cfg(feature = "2d")]
280                for segment in s.segments() {
281                    self.draw_collider(
282                        &Collider::from(SharedShape::new(segment)),
283                        position,
284                        rotation,
285                        color,
286                    );
287                }
288                #[cfg(feature = "3d")]
289                for triangle in s.triangles() {
290                    self.draw_collider(
291                        &Collider::from(SharedShape::new(triangle)),
292                        position,
293                        rotation,
294                        color,
295                    );
296                }
297            }
298            TypedShape::Compound(s) => {
299                for (sub_pos, shape) in s.shapes() {
300                    let pos = Position(position.0 + rotation * Vector::from(sub_pos.translation));
301                    #[cfg(feature = "2d")]
302                    let rot = rotation * Rotation::radians(sub_pos.rotation.angle());
303                    #[cfg(feature = "3d")]
304                    let rot = Rotation((rotation.mul_quat(sub_pos.rotation.into())).normalize());
305                    self.draw_collider(&Collider::from(shape.to_owned()), pos, rot, color);
306                }
307            }
308            #[cfg(feature = "2d")]
309            TypedShape::ConvexPolygon(s) => {
310                self.draw_line_strip(
311                    nalgebra_to_glam(s.points()),
312                    position,
313                    rotation,
314                    true,
315                    color,
316                );
317            }
318            #[cfg(feature = "3d")]
319            TypedShape::ConvexPolyhedron(s) => {
320                let indices = s
321                    .edges()
322                    .iter()
323                    .map(|e| [e.vertices.x, e.vertices.y])
324                    .collect::<Vec<_>>();
325                self.draw_polyline(
326                    &nalgebra_to_glam(s.points()),
327                    &indices,
328                    position,
329                    rotation,
330                    color,
331                );
332            }
333            #[cfg(feature = "3d")]
334            TypedShape::Cylinder(s) => {
335                let (vertices, indices) = s.to_outline(32);
336                self.draw_polyline(
337                    &nalgebra_to_glam(&vertices),
338                    &indices,
339                    position,
340                    rotation,
341                    color,
342                );
343            }
344            #[cfg(feature = "3d")]
345            TypedShape::Cone(s) => {
346                let (vertices, indices) = s.to_outline(32);
347                self.draw_polyline(
348                    &nalgebra_to_glam(&vertices),
349                    &indices,
350                    position,
351                    rotation,
352                    color,
353                );
354            }
355            // ------------
356            // Round shapes
357            // ------------
358            #[cfg(feature = "2d")]
359            TypedShape::RoundCuboid(s) => {
360                self.draw_line_strip(
361                    nalgebra_to_glam(&s.to_polyline(32)),
362                    position,
363                    rotation,
364                    true,
365                    color,
366                );
367            }
368            #[cfg(feature = "3d")]
369            TypedShape::RoundCuboid(s) => {
370                let (vertices, indices) = s.to_outline(32);
371                self.draw_polyline(
372                    &nalgebra_to_glam(&vertices),
373                    &indices,
374                    position,
375                    rotation,
376                    color,
377                );
378            }
379            TypedShape::RoundTriangle(s) => {
380                // Parry doesn't have a method for the rounded outline, so we have to just use a normal triangle
381                // (or compute the outline manually based on the border radius)
382                self.draw_collider(
383                    &Collider::from(SharedShape::new(s.inner_shape)),
384                    position,
385                    rotation,
386                    color,
387                );
388            }
389            #[cfg(feature = "2d")]
390            TypedShape::RoundConvexPolygon(s) => {
391                self.draw_line_strip(
392                    nalgebra_to_glam(&s.to_polyline(32)),
393                    position,
394                    rotation,
395                    true,
396                    color,
397                );
398            }
399            #[cfg(feature = "3d")]
400            TypedShape::RoundConvexPolyhedron(s) => {
401                let (vertices, indices) = s.to_outline(32);
402                self.draw_polyline(
403                    &nalgebra_to_glam(&vertices),
404                    &indices,
405                    position,
406                    rotation,
407                    color,
408                );
409            }
410            #[cfg(feature = "3d")]
411            TypedShape::RoundCylinder(s) => {
412                let (vertices, indices) = s.to_outline(32, 32);
413                self.draw_polyline(
414                    &nalgebra_to_glam(&vertices),
415                    &indices,
416                    position,
417                    rotation,
418                    color,
419                );
420            }
421            #[cfg(feature = "3d")]
422            TypedShape::RoundCone(s) => {
423                let (vertices, indices) = s.to_outline(32, 32);
424                self.draw_polyline(
425                    &nalgebra_to_glam(&vertices),
426                    &indices,
427                    position,
428                    rotation,
429                    color,
430                );
431            }
432            TypedShape::Custom(_id) => {
433                #[cfg(feature = "2d")]
434                {
435                    use crate::collision::collider::{
436                        EllipseColliderShape, RegularPolygonColliderShape,
437                    };
438
439                    if let Some(ellipse) =
440                        collider.shape_scaled().as_shape::<EllipseColliderShape>()
441                    {
442                        let isometry = Isometry2d::new(
443                            position.f32(),
444                            Rot2::from_sin_cos(rotation.sin as f32, rotation.cos as f32),
445                        );
446                        self.primitive_2d(&ellipse.0, isometry, color);
447                    } else if let Some(polygon) = collider
448                        .shape_scaled()
449                        .as_shape::<RegularPolygonColliderShape>()
450                    {
451                        let isometry = Isometry2d::new(
452                            position.f32(),
453                            Rot2::from_sin_cos(rotation.sin as f32, rotation.cos as f32),
454                        );
455                        self.primitive_2d(&polygon.0, isometry, color);
456                    }
457                }
458            }
459        }
460    }
461
462    /// Draws the results of a [raycast](SpatialQuery#raycasting).
463    #[allow(clippy::too_many_arguments)]
464    fn draw_raycast(
465        &mut self,
466        origin: Vector,
467        direction: Dir,
468        max_distance: Scalar,
469        hits: &[RayHitData],
470        ray_color: Color,
471        point_color: Color,
472        normal_color: Color,
473        length_unit: Scalar,
474    ) {
475        let max_distance = hits
476            .iter()
477            .max_by(|a, b| a.distance.total_cmp(&b.distance))
478            .map_or(max_distance, |hit| hit.distance);
479
480        // Draw ray as arrow
481        self.draw_arrow(
482            origin,
483            origin + direction.adjust_precision() * max_distance,
484            0.1 * length_unit,
485            ray_color,
486        );
487
488        // Draw all hit points and normals
489        for hit in hits {
490            let point = origin + direction.adjust_precision() * hit.distance;
491
492            // Draw hit point
493            #[cfg(feature = "2d")]
494            self.circle_2d(point.f32(), 0.1 * length_unit as f32, point_color);
495            #[cfg(feature = "3d")]
496            self.sphere(point.f32(), 0.1 * length_unit as f32, point_color);
497
498            // Draw hit normal as arrow
499            self.draw_arrow(
500                point,
501                point + hit.normal * 0.5 * length_unit,
502                0.1 * length_unit,
503                normal_color,
504            );
505        }
506    }
507
508    /// Draws the results of a [shapecast](SpatialQuery#shapecasting).
509    #[cfg(all(
510        feature = "default-collider",
511        any(feature = "parry-f32", feature = "parry-f64")
512    ))]
513    #[allow(clippy::too_many_arguments)]
514    fn draw_shapecast(
515        &mut self,
516        shape: &Collider,
517        origin: Vector,
518        shape_rotation: impl Into<Rotation>,
519        direction: Dir,
520        max_distance: Scalar,
521        hits: &[ShapeHitData],
522        ray_color: Color,
523        shape_color: Color,
524        point_color: Color,
525        normal_color: Color,
526        length_unit: Scalar,
527    ) {
528        let shape_rotation = shape_rotation.into();
529        #[cfg(feature = "3d")]
530        let shape_rotation = Rotation(shape_rotation.normalize());
531
532        let max_distance = hits
533            .iter()
534            .max_by(|a, b| a.distance.total_cmp(&b.distance))
535            .map_or(max_distance, |hit| hit.distance);
536
537        // Draw collider at origin
538        self.draw_collider(shape, origin, shape_rotation, shape_color);
539
540        // Draw arrow from origin to position of shape at final hit
541        // TODO: We could render the swept collider outline instead
542        self.draw_arrow(
543            origin,
544            origin + max_distance * direction.adjust_precision(),
545            0.1 * length_unit,
546            ray_color,
547        );
548
549        // Draw all hit points, normals and the shape at the hit points
550        for hit in hits {
551            // Draw hit point
552            #[cfg(feature = "2d")]
553            self.circle_2d(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
554            #[cfg(feature = "3d")]
555            self.sphere(hit.point1.f32(), 0.1 * length_unit as f32, point_color);
556
557            // Draw hit normal as arrow
558            self.draw_arrow(
559                hit.point1,
560                hit.point1 + hit.normal1 * 0.5 * length_unit,
561                0.1 * length_unit,
562                normal_color,
563            );
564
565            // Draw collider at hit point
566            self.draw_collider(
567                shape,
568                origin + hit.distance * direction.adjust_precision(),
569                shape_rotation,
570                shape_color.with_alpha(0.3),
571            );
572        }
573    }
574}
avian3d/debug_render/gizmos.rs

avian3d/debug_render/
gizmos.rs
