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

avian3d/debug_render/
gizmos.rs
