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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155

use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::math::Vector;
use ncollide::shape::Cuboid;

pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) {
    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
        generate_contacts(
            ctxt.prediction_distance,
            cube1,
            ctxt.position1,
            cube2,
            ctxt.position2,
            ctxt.manifold,
        );
    } else {
        unreachable!()
    }

    ctxt.manifold.update_warmstart_multiplier();
    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}

pub fn generate_contacts<'a>(
    prediction_distance: f32,
    mut cube1: &'a Cuboid<f32>,
    mut pos1: &'a Isometry<f32>,
    mut cube2: &'a Cuboid<f32>,
    mut pos2: &'a Isometry<f32>,
    manifold: &mut ContactManifold,
) {
    let mut pos12 = pos1.inverse() * pos2;
    let mut pos21 = pos12.inverse();

    if manifold.try_update_contacts(&pos12) {
        return;
    }

    /*
     *
     * Point-Face
     *
     */
    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21);
    if sep1.0 > prediction_distance {
        manifold.points.clear();
        return;
    }

    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12);
    if sep2.0 > prediction_distance {
        manifold.points.clear();
        return;
    }

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

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

    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
        // The reference shape will be the second shape.
        std::mem::swap(&mut cube1, &mut cube2);
        std::mem::swap(&mut pos1, &mut pos2);
        std::mem::swap(&mut pos12, &mut pos21);
        manifold.swap_identifiers();
        best_sep = sep2;
        swapped = true;
    } 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.points.clear();

    // Now the reference feature is from `cube1` and the best separation is `best_sep`.
    // Everything must be expressed in the local-space of `cube1` for contact clipping.
    let feature1 = cuboid::support_feature(cube1, best_sep.1);
    let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1);
    feature2.transform_by(&pos12);

    match (&feature1, &feature2) {
        (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => {
            CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold)
        }
        #[cfg(feature = "dim3")]
        (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts(
            prediction_distance,
            f1,
            &best_sep.1,
            e2,
            &pos21,
            manifold,
            false,
        ),
        (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts(
            prediction_distance,
            f1,
            &best_sep.1,
            f2,
            &pos21,
            manifold,
        ),
        #[cfg(feature = "dim3")]
        (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => {
            CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold)
        }
        #[cfg(feature = "dim3")]
        (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => {
            // Since f2 is also expressed in the local-space of the first
            // feature, the position we provide here is pos21.
            CuboidFeature::face_edge_contacts(
                prediction_distance,
                f2,
                &-best_sep.1,
                e1,
                &pos21,
                manifold,
                true,
            )
        }
        _ => unreachable!(), // The other cases are not possible.
    }

    manifold.local_n1 = best_sep.1;
    manifold.local_n2 = pos21 * -best_sep.1;
    manifold.kinematics.category = KinematicsCategory::PlanePoint;
    manifold.kinematics.radius1 = 0.0;
    manifold.kinematics.radius2 = 0.0;

    // Transfer impulses.
    super::match_contacts(manifold, &old_manifold_points, swapped);
}