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

avian3d/debug_render/
gizmos.rs
