1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

use crate::math::{Isometry, Real};
use crate::query::{
    self,
    gjk::{GJKResult, VoronoiSimplex},
    ContactManifold, TrackedContact,
};
use crate::shape::{PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape};
use na::Unit;

/// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap`
/// trait, both represented as `Shape` trait-objects.
pub fn contact_manifold_pfm_pfm_shapes<ManifoldData, ContactData>(
    pos12: &Isometry<Real>,
    shape1: &dyn Shape,
    shape2: &dyn Shape,
    prediction: Real,
    manifold: &mut ContactManifold<ManifoldData, ContactData>,
) where
    ManifoldData: Default,
    ContactData: Default + Copy,
{
    if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
        shape1.as_polygonal_feature_map(),
        shape2.as_polygonal_feature_map(),
    ) {
        contact_manifold_pfm_pfm(
            pos12,
            pfm1,
            border_radius1,
            pfm2,
            border_radius2,
            prediction,
            manifold,
        );
    }
}

/// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap` trait.
pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>(
    pos12: &Isometry<Real>,
    pfm1: &'a S1,
    border_radius1: Real,
    pfm2: &'a S2,
    border_radius2: Real,
    prediction: Real,
    manifold: &mut ContactManifold<ManifoldData, ContactData>,
) where
    S1: ?Sized + PolygonalFeatureMap,
    S2: ?Sized + PolygonalFeatureMap,
    ManifoldData: Default,
    ContactData: Default + Copy,
{
    // We use very small thresholds for the manifold update because something to high would
    // cause numerical drifts with the effect of introducing bumps in
    // what should have been smooth rolling motions.
    if manifold.try_update_contacts_eps(pos12, crate::utils::COS_1_DEGREES, 1.0e-6) {
        return;
    }

    let init_dir = Unit::try_new(manifold.local_n1, crate::math::DEFAULT_EPSILON);
    let total_prediction = prediction + border_radius1 + border_radius2;
    let contact = query::details::contact_support_map_support_map_with_params(
        pos12,
        pfm1,
        pfm2,
        total_prediction,
        &mut VoronoiSimplex::new(),
        init_dir,
    );

    let old_manifold_points = manifold.points.clone();
    manifold.clear();

    match contact {
        GJKResult::ClosestPoints(p1, p2_1, dir) => {
            let local_n1 = dir;
            let local_n2 = pos12.inverse_transform_unit_vector(&-dir);
            let mut feature1 = PolygonalFeature::default();
            let mut feature2 = PolygonalFeature::default();
            pfm1.local_support_feature(&local_n1, &mut feature1);
            pfm2.local_support_feature(&local_n2, &mut feature2);

            PolygonalFeature::contacts(
                pos12,
                &pos12.inverse(),
                &local_n1,
                &local_n2,
                &feature1,
                &feature2,
                total_prediction,
                manifold,
                false,
            );

            if (cfg!(feature = "dim2") && manifold.points.is_empty())
                // TODO: this test is a workaround until we figure-out a way to
                //       determine the feature ids for the GJK/EPA contact.
                || pfm1.is_convex_polyhedron()
                || pfm2.is_convex_polyhedron()
            {
                let contact = TrackedContact::new(
                    p1,
                    pos12.inverse_transform_point(&p2_1),
                    PackedFeatureId::UNKNOWN, // TODO: We don't know what features are involved.
                    PackedFeatureId::UNKNOWN,
                    (p2_1 - p1).dot(&dir),
                );
                manifold.points.push(contact);
            }

            // Adjust points to take the radius into account.
            if border_radius1 != 0.0 || border_radius2 != 0.0 {
                for contact in &mut manifold.points {
                    contact.local_p1 += *local_n1 * border_radius1;
                    contact.local_p2 += *local_n2 * border_radius2;
                    contact.dist -= border_radius1 + border_radius2;
                }
            }

            manifold.local_n1 = *local_n1;
            manifold.local_n2 = *local_n2;
        }
        GJKResult::NoIntersection(dir) => {
            // Use the manifold normal as a cache.
            manifold.local_n1 = *dir;
        }
        _ => {
            // Reset the cached direction.
            manifold.local_n1.fill(0.0);
        }
    }

    // Transfer impulses.
    manifold.match_contacts(&old_manifold_points);
}