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
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
/*
* // Copyright (c) 2021 Feng Yang
* //
* // I am making my contributions/submissions to this project solely in my
* // personal capacity and am not conveying any rights to any intellectual
* // property of any third parties.
*/
use crate::vector2::Vector2D;
use crate::ray2::Ray2D;
use crate::bounding_box2::BoundingBox2D;
///
/// # Represents 2-D rigid body transform.
///
pub struct Transform2 {
_translation: Vector2D,
_orientation: f64,
_cos_angle: f64,
_sin_angle: f64,
}
impl Transform2 {
/// Constructs identity transform.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// let t1 = Transform2::new_default();
///
/// assert_eq!(Vector2D::new_default(), t1.translation());
/// assert_eq!(0.0, t1.orientation());
/// ```
pub fn new_default() -> Transform2 {
return Transform2 {
_translation: Vector2D::new_default(),
_orientation: 0.0,
_cos_angle: 1.0,
_sin_angle: 0.0,
};
}
/// Constructs a transform with translation and orientation.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::constants::K_QUARTER_PI_D;
/// let t2 = Transform2::new(Vector2D::new_lst([2.0, -5.0]), K_QUARTER_PI_D);
///
/// assert_eq!(Vector2D::new(2.0, -5.0), t2.translation());
/// assert_eq!(K_QUARTER_PI_D, t2.orientation());
/// ```
pub fn new(translation: Vector2D, orientation: f64) -> Transform2 {
return Transform2 {
_translation: translation,
_orientation: orientation,
_cos_angle: orientation.cos(),
_sin_angle: orientation.sin(),
};
}
}
impl Transform2 {
/// Returns the translation.
pub fn translation(&self) -> Vector2D {
return self._translation;
}
/// Sets the translation.
pub fn set_translation(&mut self, translation: Vector2D) {
self._translation = translation;
}
/// Returns the orientation in radians.
pub fn orientation(&self) -> f64 {
return self._orientation;
}
/// Sets the orientation in radians.
pub fn set_orientation(&mut self, orientation: f64) {
self._orientation = orientation;
self._cos_angle = f64::cos(orientation);
self._sin_angle = f64::sin(orientation);
}
/// Transforms a point in world coordinate to the local frame.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let r1 = t.to_world_vec(&Vector2D::new(4.0, 1.0));
/// let r2 = t.to_local_vec(&r1);
/// assert_eq!(4.0, r2.x);
/// assert_eq!(1.0, r2.y);
/// ```
pub fn to_local_vec(&self, point_in_world: &Vector2D) -> Vector2D {
// Convert to the local frame
let xmt = *point_in_world - self._translation;
return Vector2D::new(
self._cos_angle * xmt.x + self._sin_angle * xmt.y,
-self._sin_angle * xmt.x + self._cos_angle * xmt.y);
}
/// Transforms a direction in world coordinate to the local frame.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let r3 = t.to_world_direction(&Vector2D::new(4.0, 1.0));
/// let r4 = t.to_local_direction(&r3);
/// assert_eq!(4.0, r4.x);
/// assert_eq!(1.0, r4.y);
/// ```
pub fn to_local_direction(&self, dir_in_world: &Vector2D) -> Vector2D {
// Convert to the local frame
return Vector2D::new(
self._cos_angle * dir_in_world.x + self._sin_angle * dir_in_world.y,
-self._sin_angle * dir_in_world.x + self._cos_angle * dir_in_world.y);
}
/// Transforms a ray in world coordinate to the local frame.
pub fn to_local_ray(&self, ray_in_world: &Ray2D) -> Ray2D {
return Ray2D::new(
self.to_local_vec(&ray_in_world.origin),
self.to_local_direction(&ray_in_world.direction));
}
/// Transforms a bounding box in world coordinate to the local frame.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::bounding_box2::BoundingBox2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let bbox = BoundingBox2D::new(Vector2D::new(-2.0, -1.0), Vector2D::new(2.0, 1.0));
/// let r5 = t.to_world_aabb(&bbox);
/// let r6 = t.to_local_aabb(&r5);
/// assert_eq!(bbox.lower_corner.is_similar(&r6.lower_corner, Some(10.0*f64::EPSILON)), true);
/// assert_eq!(bbox.upper_corner.is_similar(&r6.upper_corner, Some(10.0*f64::EPSILON)), true);
/// ```
pub fn to_local_aabb(&self, bbox_in_world: &BoundingBox2D) -> BoundingBox2D {
let mut bbox_in_local = BoundingBox2D::new_default();
for i in 0..4 {
let corner_in_local = self.to_local_vec(&bbox_in_world.corner(i));
bbox_in_local.lower_corner
= crate::vector2::min(&bbox_in_local.lower_corner, &corner_in_local);
bbox_in_local.upper_corner
= crate::vector2::max(&bbox_in_local.upper_corner, &corner_in_local);
}
return bbox_in_local;
}
/// Transforms a point in local space to the world coordinate.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let r1 = t.to_world_vec(&Vector2D::new(4.0, 1.0));
/// assert_delta!(1.0, r1.x, f64::EPSILON);
/// assert_eq!(-1.0, r1.y);
/// ```
pub fn to_world_vec(&self, point_in_local: &Vector2D) -> Vector2D {
// Convert to the world frame
return Vector2D::new(
self._cos_angle * point_in_local.x - self._sin_angle * point_in_local.y
+ self._translation.x,
self._sin_angle * point_in_local.x + self._cos_angle * point_in_local.y
+ self._translation.y);
}
/// Transforms a direction in local space to the world coordinate.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let r3 = t.to_world_direction(&Vector2D::new(4.0, 1.0));
/// assert_delta!(-1.0, r3.x, f64::EPSILON);
/// assert_eq!(4.0, r3.y);
/// ```
pub fn to_world_direction(&self, dir_in_local: &Vector2D) -> Vector2D {
// Convert to the world frame
return Vector2D::new(
self._cos_angle * dir_in_local.x - self._sin_angle * dir_in_local.y,
self._sin_angle * dir_in_local.x + self._cos_angle * dir_in_local.y);
}
/// Transforms a ray in local space to the world coordinate.
pub fn to_world_ray(&self, ray_in_local: Ray2D) -> Ray2D {
return Ray2D::new(
self.to_world_vec(&ray_in_local.origin),
self.to_world_direction(&ray_in_local.direction));
}
/// Transforms a bounding box in local space to the world coordinate.
/// ```
/// use vox_geometry_rust::transform2::Transform2;
/// use vox_geometry_rust::vector2::Vector2D;
/// use vox_geometry_rust::bounding_box2::BoundingBox2D;
/// use vox_geometry_rust::constants::K_HALF_PI_D;
/// use vox_geometry_rust::assert_delta;
/// let t = Transform2::new(Vector2D::new(2.0, -5.0), K_HALF_PI_D);
///
/// let bbox = BoundingBox2D::new(Vector2D::new(-2.0, -1.0), Vector2D::new(2.0, 1.0));
/// let r5 = t.to_world_aabb(&bbox);
/// let result = BoundingBox2D::new(Vector2D::new(1.0, -7.0), Vector2D::new(3.0, -3.0));
/// assert_eq!(result.lower_corner.is_similar(&r5.lower_corner, Some(10.0*f64::EPSILON)), true);
/// assert_eq!(result.upper_corner, r5.upper_corner);
/// ```
pub fn to_world_aabb(&self, bbox_in_local: &BoundingBox2D) -> BoundingBox2D {
let mut bbox_in_world = BoundingBox2D::new_default();
for i in 0..4 {
let corner_in_world = self.to_world_vec(&bbox_in_local.corner(i));
bbox_in_world.lower_corner
= crate::vector2::min(&bbox_in_world.lower_corner, &corner_in_world);
bbox_in_world.upper_corner
= crate::vector2::max(&bbox_in_world.upper_corner, &corner_in_world);
}
return bbox_in_world;
}
}
impl Clone for Transform2 {
fn clone(&self) -> Self {
return Transform2 {
_translation: self._translation,
_orientation: self._orientation,
_cos_angle: self._cos_angle,
_sin_angle: self._sin_angle,
};
}
}