1use crate::math::{Isometry, Point, Vector};
4use crate::shape::Segment;
5use crate::shape::SupportMap;
6#[cfg(feature = "dim3")]
7use crate::shape::{ConvexPolygonalFeature, ConvexPolyhedron, FeatureId};
8use na::RealField;
9use na::{self, Unit};
10#[cfg(feature = "dim3")]
11use std::f64;
12use std::mem;
13
14#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
16#[repr(C)]
17#[derive(PartialEq, Debug, Copy, Clone)]
18pub struct Triangle<N: RealField + Copy> {
19 pub a: Point<N>,
21 pub b: Point<N>,
23 pub c: Point<N>,
25}
26
27#[derive(Copy, Clone, Debug)]
29pub enum TrianglePointLocation<N: RealField + Copy> {
30 OnVertex(usize),
32 OnEdge(usize, [N; 2]),
40 OnFace(usize, [N; 3]),
46 OnSolid,
48}
49
50impl<N: RealField + Copy> TrianglePointLocation<N> {
51 pub fn barycentric_coordinates(&self) -> Option<[N; 3]> {
55 let mut bcoords = [N::zero(); 3];
56
57 match self {
58 TrianglePointLocation::OnVertex(i) => bcoords[*i] = N::one(),
59 TrianglePointLocation::OnEdge(i, uv) => {
60 let idx = match i {
61 0 => (0, 1),
62 1 => (1, 2),
63 2 => (0, 2),
64 _ => unreachable!(),
65 };
66
67 bcoords[idx.0] = uv[0];
68 bcoords[idx.1] = uv[1];
69 }
70 TrianglePointLocation::OnFace(_, uvw) => {
71 bcoords[0] = uvw[0];
72 bcoords[1] = uvw[1];
73 bcoords[2] = uvw[2];
74 }
75 TrianglePointLocation::OnSolid => {
76 return None;
77 }
78 }
79
80 Some(bcoords)
81 }
82
83 pub fn is_on_face(&self) -> bool {
85 if let TrianglePointLocation::OnFace(..) = *self {
86 true
87 } else {
88 false
89 }
90 }
91}
92
93impl<N: RealField + Copy> Triangle<N> {
94 #[inline]
96 pub fn new(a: Point<N>, b: Point<N>, c: Point<N>) -> Triangle<N> {
97 Triangle { a, b, c }
98 }
99
100 pub fn from_array(arr: &[Point<N>; 3]) -> &Triangle<N> {
102 unsafe { mem::transmute(arr) }
103 }
104
105 #[inline]
107 #[deprecated(note = "use the `self.a` public field directly.")]
108 pub fn a(&self) -> &Point<N> {
109 &self.a
110 }
111
112 #[inline]
114 #[deprecated(note = "use the `self.b` public field directly.")]
115 pub fn b(&self) -> &Point<N> {
116 &self.b
117 }
118
119 #[inline]
121 #[deprecated(note = "use the `self.c` public field directly.")]
122 pub fn c(&self) -> &Point<N> {
123 &self.c
124 }
125
126 #[inline]
128 pub fn vertices(&self) -> &[Point<N>; 3] {
129 unsafe { mem::transmute(self) }
130 }
131
132 #[inline]
137 pub fn normal(&self) -> Option<Unit<Vector<N>>> {
138 Unit::try_new(self.scaled_normal(), N::default_epsilon())
139 }
140
141 #[inline]
143 pub fn edges(&self) -> [Segment<N>; 3] {
144 [
145 Segment::new(self.a, self.b),
146 Segment::new(self.b, self.c),
147 Segment::new(self.c, self.a),
148 ]
149 }
150
151 #[inline]
153 pub fn transformed(&self, m: &Isometry<N>) -> Self {
154 Triangle::new(m * self.a, m * self.b, m * self.c)
155 }
156 #[inline]
158 pub fn edges_scaled_directions(&self) -> [Vector<N>; 3] {
159 [self.b - self.a, self.c - self.b, self.a - self.c]
160 }
161
162 #[inline]
167 pub fn scaled_normal(&self) -> Vector<N> {
168 let ab = self.b - self.a;
169 let ac = self.c - self.a;
170 ab.cross(&ac)
171 }
172
173 #[inline]
178 pub fn extents_on_dir(&self, dir: &Unit<Vector<N>>) -> (N, N) {
179 let a = self.a.coords.dot(dir);
180 let b = self.b.coords.dot(dir);
181 let c = self.c.coords.dot(dir);
182
183 if a > b {
184 if b > c {
185 (c, a)
186 } else if a > c {
187 (b, a)
188 } else {
189 (b, c)
190 }
191 } else {
192 if a > c {
194 (c, b)
195 } else if b > c {
196 (a, b)
197 } else {
198 (a, c)
199 }
200 }
201 }
202
203 #[cfg(feature = "dim3")]
205 #[inline]
206 pub fn tangent_cone_contains_dir(
207 &self,
208 feature: FeatureId,
209 m: &Isometry<N>,
210 dir: &Unit<Vector<N>>,
211 ) -> bool {
212 let ls_dir = m.inverse_transform_vector(dir);
213
214 if let Some(normal) = self.normal() {
215 match feature {
216 FeatureId::Vertex(_) => {
217 false
220 }
221 FeatureId::Edge(_) => {
222 false
225 }
226 FeatureId::Face(0) => ls_dir.dot(&normal) <= N::zero(),
227 FeatureId::Face(1) => ls_dir.dot(&normal) >= N::zero(),
228 _ => panic!("Invalid feature ID."),
229 }
230 } else {
231 false
232 }
233 }
234
235 #[cfg(feature = "dim3")]
236 fn support_feature_id_toward(&self, local_dir: &Unit<Vector<N>>, eps: N) -> FeatureId {
237 if let Some(normal) = self.normal() {
238 let (seps, ceps) = eps.sin_cos();
239
240 let normal_dot = local_dir.dot(&*normal);
241 if normal_dot >= ceps {
242 FeatureId::Face(0)
243 } else if normal_dot <= -ceps {
244 FeatureId::Face(1)
245 } else {
246 let edges = self.edges();
247 let mut dots = [N::zero(); 3];
248
249 let dir1 = edges[0].direction();
250 if let Some(dir1) = dir1 {
251 dots[0] = dir1.dot(local_dir);
252
253 if dots[0].abs() < seps {
254 return FeatureId::Edge(0);
255 }
256 }
257
258 let dir2 = edges[1].direction();
259 if let Some(dir2) = dir2 {
260 dots[1] = dir2.dot(local_dir);
261
262 if dots[1].abs() < seps {
263 return FeatureId::Edge(1);
264 }
265 }
266
267 let dir3 = edges[2].direction();
268 if let Some(dir3) = dir3 {
269 dots[2] = dir3.dot(local_dir);
270
271 if dots[2].abs() < seps {
272 return FeatureId::Edge(2);
273 }
274 }
275
276 if dots[0] > N::zero() && dots[1] < N::zero() {
277 FeatureId::Vertex(1)
278 } else if dots[1] > N::zero() && dots[2] < N::zero() {
279 FeatureId::Vertex(2)
280 } else {
281 FeatureId::Vertex(0)
282 }
283 }
284 } else {
285 FeatureId::Vertex(0)
286 }
287 }
288}
289
290impl<N: RealField + Copy> SupportMap<N> for Triangle<N> {
291 #[inline]
292 fn local_support_point(&self, dir: &Vector<N>) -> Point<N> {
293 let d1 = self.a.coords.dot(dir);
294 let d2 = self.b.coords.dot(dir);
295 let d3 = self.c.coords.dot(dir);
296
297 if d1 > d2 {
298 if d1 > d3 {
299 self.a
300 } else {
301 self.c
302 }
303 } else {
304 if d2 > d3 {
305 self.b
306 } else {
307 self.c
308 }
309 }
310 }
311}
312
313#[cfg(feature = "dim3")]
314impl<N: RealField + Copy> ConvexPolyhedron<N> for Triangle<N> {
315 fn vertex(&self, id: FeatureId) -> Point<N> {
316 match id.unwrap_vertex() {
317 0 => self.a,
318 1 => self.b,
319 2 => self.c,
320 _ => panic!("Triangle vertex index out of bounds."),
321 }
322 }
323 fn edge(&self, id: FeatureId) -> (Point<N>, Point<N>, FeatureId, FeatureId) {
324 match id.unwrap_edge() {
325 0 => (self.a, self.b, FeatureId::Vertex(0), FeatureId::Vertex(1)),
326 1 => (self.b, self.c, FeatureId::Vertex(1), FeatureId::Vertex(2)),
327 2 => (self.c, self.a, FeatureId::Vertex(2), FeatureId::Vertex(0)),
328 _ => panic!("Triangle edge index out of bounds."),
329 }
330 }
331
332 fn face(&self, id: FeatureId, face: &mut ConvexPolygonalFeature<N>) {
333 face.clear();
334
335 if let Some(normal) = self.normal() {
336 face.set_feature_id(id);
337
338 match id.unwrap_face() {
339 0 => {
340 face.push(self.a, FeatureId::Vertex(0));
341 face.push(self.b, FeatureId::Vertex(1));
342 face.push(self.c, FeatureId::Vertex(2));
343 face.push_edge_feature_id(FeatureId::Edge(0));
344 face.push_edge_feature_id(FeatureId::Edge(1));
345 face.push_edge_feature_id(FeatureId::Edge(2));
346 face.set_normal(normal);
347 }
348 1 => {
349 face.push(self.a, FeatureId::Vertex(0));
350 face.push(self.c, FeatureId::Vertex(2));
351 face.push(self.b, FeatureId::Vertex(1));
352 face.push_edge_feature_id(FeatureId::Edge(2));
353 face.push_edge_feature_id(FeatureId::Edge(1));
354 face.push_edge_feature_id(FeatureId::Edge(0));
355 face.set_normal(-normal);
356 }
357 _ => unreachable!(),
358 }
359
360 face.recompute_edge_normals();
361 } else {
362 face.push(self.a, FeatureId::Vertex(0));
363 face.set_feature_id(FeatureId::Vertex(0));
364 }
365 }
366
367 fn feature_normal(&self, _: FeatureId) -> Unit<Vector<N>> {
368 if let Some(normal) = self.normal() {
369 normal
371 } else {
372 Vector::y_axis()
373 }
374 }
375
376 fn support_face_toward(
377 &self,
378 m: &Isometry<N>,
379 dir: &Unit<Vector<N>>,
380 face: &mut ConvexPolygonalFeature<N>,
381 ) {
382 let normal = self.scaled_normal();
383
384 if normal.dot(&*dir) >= na::zero() {
385 ConvexPolyhedron::<N>::face(self, FeatureId::Face(0), face);
386 } else {
387 ConvexPolyhedron::<N>::face(self, FeatureId::Face(1), face);
388 }
389 face.transform_by(m)
390 }
391
392 fn support_feature_toward(
393 &self,
394 transform: &Isometry<N>,
395 dir: &Unit<Vector<N>>,
396 eps: N,
397 out: &mut ConvexPolygonalFeature<N>,
398 ) {
399 out.clear();
400 let tri = self.transformed(transform);
401 let feature = tri.support_feature_id_toward(dir, eps);
402
403 match feature {
404 FeatureId::Vertex(_) => {
405 let v = tri.vertex(feature);
406 out.push(v, feature);
407 out.set_feature_id(feature);
408 }
409 FeatureId::Edge(_) => {
410 let (a, b, fa, fb) = tri.edge(feature);
411 out.push(a, fa);
412 out.push(b, fb);
413 out.push_edge_feature_id(feature);
414 out.set_feature_id(feature);
415 }
416 FeatureId::Face(_) => tri.face(feature, out),
417 _ => unreachable!(),
418 }
419 }
420
421 fn support_feature_id_toward(&self, local_dir: &Unit<Vector<N>>) -> FeatureId {
422 self.support_feature_id_toward(local_dir, na::convert(f64::consts::PI / 180.0))
423 }
424}