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
|
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,
});
}
}
}
}
|
