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
156
157
158
159
160
161
|
use super::{LineSegment, Vec2};
use alga::general::{ClosedMul, ClosedSub};
use nalgebra::{RealField, Scalar};
use num_traits::Zero;
/// Represents a triangle
pub struct Triangle<T: Scalar + Copy> {
/// The three corners of the triangle. Internally, it is made sure that the corners are always
/// ordered in a counterclockwise manner, to make operations like contains simpler.
corners: [Vec2<T>; 3],
}
impl<T: Scalar + Copy> Triangle<T> {
/// Create a new Triangle as defined by its three corner points
pub fn new(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> Self
where
T: ClosedSub + ClosedMul + PartialOrd + Zero,
{
// Make sure the three points are in counterclockwise order.
match triplet_orientation(a, b, c) {
TripletOrientation::Counterclockwise => Self { corners: [a, b, c] },
TripletOrientation::Clockwise => Self { corners: [a, c, b] },
TripletOrientation::Collinear => {
warn!(
"Creating triangle without any area: [{:?}, {:?}, {:?}]",
a, b, c
);
Self { corners: [a, b, c] }
}
}
}
/// Create a new Triangle from a three-point slice, instead of the three points one after
/// another.
pub fn from_slice(corners: [Vec2<T>; 3]) -> Self
where
T: ClosedSub + ClosedMul + PartialOrd + Zero,
{
Self::new(corners[0], corners[1], corners[2])
}
/// Check if the triangle contains a given point. If the point is right on an edge, it still
/// counts as inside it.
pub fn contains_point(&self, point: Vec2<T>) -> bool
where
T: ClosedSub + ClosedMul + PartialOrd + Zero,
{
// Since the points are ordered counterclockwise, check if the point is to the left of all
// edges (or on an edge) from one point to the next. When the point is to the left of all
// edges, it must be inside the triangle.
for i in 0..3 {
if triplet_orientation(self.corners[i], self.corners[(i + 1) % 3], point)
== TripletOrientation::Clockwise
{
return false;
}
}
true
}
}
#[derive(PartialEq, Eq)]
pub(crate) enum TripletOrientation {
Clockwise,
Counterclockwise,
Collinear,
}
/// Helper function to determine which direction one would turn to traverse from the first point
/// over the second to the third point. The third option is collinear, in which case the three points
/// are on the same line.
pub(crate) fn triplet_orientation<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> TripletOrientation
where
T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero,
{
/* Check the slopes of the vector from a to b and b to c. If the slope of ab is greater than
* that of bc, the rotation is clockwise. If ab is smaller than bc it's counterclockwise. If
* they are the same it follows that the three points are collinear.
*/
match (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y) {
q if q > T::zero() => TripletOrientation::Counterclockwise,
q if q < T::zero() => TripletOrientation::Clockwise,
_ => TripletOrientation::Collinear,
}
}
/// Calculate the angle between three points. Guaranteed to have 0 <= angle < 2Pi. The angle is
/// calculated as the angle between a line from a to b and then from b to c, increasing
/// counterclockwise.
///
/// # Panics
/// If the length from a to b or the length from b to c is zero.
pub(crate) fn triplet_angle<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> T
where
T: Scalar + Copy + ClosedSub + RealField + Zero,
{
assert!(a != b);
assert!(b != c);
// Handle the extreme 0 and 180 degree cases
let orientation = triplet_orientation(a, b, c);
if orientation == TripletOrientation::Collinear {
if LineSegment::new(a, b).contains_collinear(c)
|| LineSegment::new(b, c).contains_collinear(a)
{
return T::zero();
} else {
return T::pi();
}
}
// Calculate the vectors out of the points
let ba = a - b;
let bc = c - b;
// Calculate the angle between 0 and Pi.
let angle = ((ba * bc) / (ba.length() * bc.length())).acos();
// Make angle into a full circle angle by looking at the orientation of the triplet.
let angle = match orientation {
TripletOrientation::Counterclockwise => T::pi() + (T::pi() - angle),
_ => angle,
};
angle
}
#[cfg(test)]
mod test {
use super::*;
use std::f64::consts::PI;
#[test]
fn triplet_angle() {
assert_eq!(
super::triplet_angle(Vec2::new(0., 0.), Vec2::new(0., -1.), Vec2::new(-1., -1.)),
1.5 * PI
);
assert_eq!(
super::triplet_angle(Vec2::new(2., 2.), Vec2::new(0., 0.), Vec2::new(-1., -1.)),
PI
);
assert_eq!(
super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(6., 2.)),
0.
);
assert_eq!(
super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(-6., -2.)),
PI
);
assert_eq!(
super::triplet_angle(Vec2::new(0., 1.), Vec2::new(0., 2.), Vec2::new(-3., 2.)),
0.5 * PI
);
assert_eq!(
super::triplet_angle(Vec2::new(3., 1.), Vec2::new(2., 2.), Vec2::new(0., 2.)),
0.75 * PI
);
}
}
|
