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

#[cfg(feature = "dim2")]
use crate::math::Vector;
use crate::math::{Isometry, Real};
use crate::query::{sat, ContactManifold};
use crate::shape::{Cuboid, PolygonalFeature, Shape};

/// Computes the contact manifold between two cuboids represented as `Shape` trait-objects.
pub fn contact_manifold_cuboid_cuboid_shapes<ManifoldData, ContactData: Default + Copy>(
    pos12: &Isometry<Real>,
    g1: &dyn Shape,
    g2: &dyn Shape,
    prediction: Real,
    manifold: &mut ContactManifold<ManifoldData, ContactData>,
) {
    if let (Some(cuboid1), Some(cuboid2)) = (g1.as_cuboid(), g2.as_cuboid()) {
        contact_manifold_cuboid_cuboid(pos12, cuboid1, cuboid2, prediction, manifold);
    }
}

/// Computes the contact manifold between two cuboids.
pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + Copy>(
    pos12: &Isometry<Real>,
    cuboid1: &'a Cuboid,
    cuboid2: &'a Cuboid,
    prediction: Real,
    manifold: &mut ContactManifold<ManifoldData, ContactData>,
) {
    if manifold.try_update_contacts(&pos12) {
        return;
    }

    let pos21 = &pos12.inverse();
    let pos12 = &*pos12;

    /*
     *
     * Point-Face
     *
     */
    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, &pos12);
    if sep1.0 > prediction {
        manifold.clear();
        return;
    }

    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, &pos21);
    if sep2.0 > prediction {
        manifold.clear();
        return;
    }

    /*
     *
     * Edge-Edge cases
     *
     */
    #[cfg(feature = "dim2")]
    let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D.
    #[cfg(feature = "dim3")]
    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, &pos12);
    if sep3.0 > prediction {
        manifold.clear();
        return;
    }

    /*
     *
     * Select the best combination of features
     * and get the polygons to clip.
     *
     */
    let mut best_sep = sep1;

    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
        best_sep = (sep2.0, pos12 * -sep2.1);
    } else if sep3.0 > sep1.0 {
        best_sep = sep3;
    }

    // We do this clone to perform contact tracking and transfer impulses.
    // FIXME: find a more efficient way of doing this.
    let old_manifold_points = manifold.points.clone();
    manifold.clear();
    let local_n2 = pos21 * -best_sep.1;

    // Now the reference feature is from `cuboid1` and the best separation is `best_sep`.
    // Everything must be expressed in the local-space of `cuboid1` for contact clipping.
    let feature1 = cuboid1.support_feature(best_sep.1);
    let feature2 = cuboid2.support_feature(local_n2);

    PolygonalFeature::contacts(
        pos12,
        pos21,
        &best_sep.1,
        &local_n2,
        &feature1,
        &feature2,
        prediction,
        manifold,
        false,
    );

    manifold.local_n1 = best_sep.1;
    manifold.local_n2 = local_n2;

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