129 lines
3.8 KiB
Rust
129 lines
3.8 KiB
Rust
use crate::geometry::{self, Contact, ContactManifold};
|
|
use crate::math::{Isometry, Point, Vector};
|
|
use ncollide::shape::Segment;
|
|
|
|
#[derive(Debug)]
|
|
#[allow(dead_code)]
|
|
pub enum CuboidFeature {
|
|
Face(CuboidFeatureFace),
|
|
Vertex(CuboidFeatureVertex),
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct CuboidFeatureVertex {
|
|
pub vertex: Point<f32>,
|
|
pub vid: u8,
|
|
}
|
|
|
|
impl CuboidFeatureVertex {
|
|
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
|
|
self.vertex = iso * self.vertex;
|
|
}
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct CuboidFeatureFace {
|
|
pub vertices: [Point<f32>; 2],
|
|
pub vids: [u8; 2],
|
|
pub fid: u8,
|
|
}
|
|
|
|
impl From<Segment<f32>> for CuboidFeatureFace {
|
|
fn from(seg: Segment<f32>) -> Self {
|
|
CuboidFeatureFace {
|
|
vertices: [seg.a, seg.b],
|
|
vids: [0, 2],
|
|
fid: 1,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl CuboidFeatureFace {
|
|
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
|
|
self.vertices[0] = iso * self.vertices[0];
|
|
self.vertices[1] = iso * self.vertices[1];
|
|
}
|
|
}
|
|
|
|
impl CuboidFeature {
|
|
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
|
|
match self {
|
|
CuboidFeature::Face(face) => face.transform_by(iso),
|
|
CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
|
|
}
|
|
}
|
|
|
|
/// Compute contacts points between a face and a vertex.
|
|
///
|
|
/// This method assume we already know that at least one contact exists.
|
|
pub fn face_vertex_contacts(
|
|
face1: &CuboidFeatureFace,
|
|
sep_axis1: &Vector<f32>,
|
|
vertex2: &CuboidFeatureVertex,
|
|
pos21: &Isometry<f32>,
|
|
manifold: &mut ContactManifold,
|
|
) {
|
|
let tangent1 = face1.vertices[1] - face1.vertices[0];
|
|
let normal1 = Vector::new(-tangent1.y, tangent1.x);
|
|
let denom = -normal1.dot(&sep_axis1);
|
|
let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
|
|
let local_p2 = vertex2.vertex;
|
|
let local_p1 = vertex2.vertex - dist * sep_axis1;
|
|
|
|
manifold.points.push(Contact {
|
|
local_p1,
|
|
local_p2: pos21 * local_p2,
|
|
impulse: 0.0,
|
|
tangent_impulse: Contact::zero_tangent_impulse(),
|
|
fid1: face1.fid,
|
|
fid2: vertex2.vid,
|
|
dist,
|
|
});
|
|
}
|
|
|
|
pub fn face_face_contacts(
|
|
_prediction_distance: f32,
|
|
face1: &CuboidFeatureFace,
|
|
normal1: &Vector<f32>,
|
|
face2: &CuboidFeatureFace,
|
|
pos21: &Isometry<f32>,
|
|
manifold: &mut ContactManifold,
|
|
) {
|
|
if let Some((clip_a, clip_b)) = geometry::clip_segments(
|
|
(face1.vertices[0], face1.vertices[1]),
|
|
(face2.vertices[0], face2.vertices[1]),
|
|
) {
|
|
let fids1 = [face1.vids[0], face1.fid, face1.vids[1]];
|
|
let fids2 = [face2.vids[0], face2.fid, face2.vids[1]];
|
|
|
|
let dist = (clip_a.1 - clip_a.0).dot(normal1);
|
|
if true {
|
|
// dist < prediction_distance {
|
|
manifold.points.push(Contact {
|
|
local_p1: clip_a.0,
|
|
local_p2: pos21 * clip_a.1,
|
|
impulse: 0.0,
|
|
tangent_impulse: Contact::zero_tangent_impulse(),
|
|
fid1: fids1[clip_a.2],
|
|
fid2: fids2[clip_a.3],
|
|
dist,
|
|
});
|
|
}
|
|
|
|
let dist = (clip_b.1 - clip_b.0).dot(normal1);
|
|
if true {
|
|
// dist < prediction_distance {
|
|
manifold.points.push(Contact {
|
|
local_p1: clip_b.0,
|
|
local_p2: pos21 * clip_b.1,
|
|
impulse: 0.0,
|
|
tangent_impulse: Contact::zero_tangent_impulse(),
|
|
fid1: fids1[clip_b.2],
|
|
fid2: fids2[clip_b.3],
|
|
dist,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|