chore: import upstream snapshot with attribution
This commit is contained in:
wehub-resource-sync
@@ -0,0 +1,631 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use crate::Matrix4Ext;
|
||||
use nalgebra::{Matrix4, Vector2, Vector3, Vector4};
|
||||
use rectutils::{OptionRect, Rect};
|
||||
|
||||
#[derive(Copy, Clone, PartialEq, Debug)]
|
||||
pub struct AxisAlignedBoundingBox {
|
||||
pub min: Vector3<f32>,
|
||||
pub max: Vector3<f32>,
|
||||
}
|
||||
|
||||
impl Default for AxisAlignedBoundingBox {
|
||||
#[inline]
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min: Vector3::new(f32::MAX, f32::MAX, f32::MAX),
|
||||
max: Vector3::new(-f32::MAX, -f32::MAX, -f32::MAX),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl AxisAlignedBoundingBox {
|
||||
#[inline]
|
||||
pub const fn unit() -> Self {
|
||||
Self::from_min_max(Vector3::new(-0.5, -0.5, -0.5), Vector3::new(0.5, 0.5, 0.5))
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub const fn collapsed() -> Self {
|
||||
Self {
|
||||
min: Vector3::new(0.0, 0.0, 0.0),
|
||||
max: Vector3::new(0.0, 0.0, 0.0),
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn from_radius(radius: f32) -> Self {
|
||||
Self {
|
||||
min: Vector3::new(-radius, -radius, -radius),
|
||||
max: Vector3::new(radius, radius, radius),
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub const fn from_min_max(min: Vector3<f32>, max: Vector3<f32>) -> Self {
|
||||
Self { min, max }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn from_point(point: Vector3<f32>) -> Self {
|
||||
Self {
|
||||
min: point,
|
||||
max: point,
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn from_points(points: &[Vector3<f32>]) -> Self {
|
||||
let mut aabb = AxisAlignedBoundingBox::default();
|
||||
for pt in points {
|
||||
aabb.add_point(*pt);
|
||||
}
|
||||
aabb
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn add_point(&mut self, a: Vector3<f32>) {
|
||||
if a.x < self.min.x {
|
||||
self.min.x = a.x;
|
||||
}
|
||||
if a.y < self.min.y {
|
||||
self.min.y = a.y;
|
||||
}
|
||||
if a.z < self.min.z {
|
||||
self.min.z = a.z;
|
||||
}
|
||||
|
||||
if a.x > self.max.x {
|
||||
self.max.x = a.x;
|
||||
}
|
||||
if a.y > self.max.y {
|
||||
self.max.y = a.y;
|
||||
}
|
||||
if a.z > self.max.z {
|
||||
self.max.z = a.z;
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn inflate(&mut self, delta: Vector3<f32>) {
|
||||
self.min -= delta.scale(0.5);
|
||||
self.max += delta.scale(0.5);
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn scale(&mut self, scale: f32) {
|
||||
let center = self.center();
|
||||
self.min = (self.min - center) * scale + center;
|
||||
self.max = (self.max - center) * scale + center;
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn add_box(&mut self, other: Self) {
|
||||
self.add_point(other.min);
|
||||
self.add_point(other.max);
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn corners(&self) -> [Vector3<f32>; 8] {
|
||||
[
|
||||
Vector3::new(self.min.x, self.min.y, self.min.z),
|
||||
Vector3::new(self.min.x, self.min.y, self.max.z),
|
||||
Vector3::new(self.max.x, self.min.y, self.max.z),
|
||||
Vector3::new(self.max.x, self.min.y, self.min.z),
|
||||
Vector3::new(self.min.x, self.max.y, self.min.z),
|
||||
Vector3::new(self.min.x, self.max.y, self.max.z),
|
||||
Vector3::new(self.max.x, self.max.y, self.max.z),
|
||||
Vector3::new(self.max.x, self.max.y, self.min.z),
|
||||
]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn volume(&self) -> f32 {
|
||||
let size = self.max - self.min;
|
||||
size.x * size.y * size.z
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn offset(&mut self, v: Vector3<f32>) {
|
||||
self.min += v;
|
||||
self.max += v;
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn center(&self) -> Vector3<f32> {
|
||||
(self.max + self.min).scale(0.5)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn half_extents(&self) -> Vector3<f32> {
|
||||
(self.max - self.min).scale(0.5)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn invalidate(&mut self) {
|
||||
*self = Default::default();
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_valid(&self) -> bool {
|
||||
#[inline(always)]
|
||||
fn is_nan_or_inf(x: &Vector3<f32>) -> bool {
|
||||
x.iter().all(|e| e.is_nan() || e.is_infinite())
|
||||
}
|
||||
|
||||
self.max.x >= self.min.x
|
||||
&& self.max.y >= self.min.y
|
||||
&& self.max.z >= self.min.z
|
||||
&& !is_nan_or_inf(&self.min)
|
||||
&& !is_nan_or_inf(&self.max)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_degenerate(&self) -> bool {
|
||||
self.max == self.min
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_invalid_or_degenerate(&self) -> bool {
|
||||
!self.is_valid() || self.is_degenerate()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_contains_point(&self, point: Vector3<f32>) -> bool {
|
||||
point.x >= self.min.x
|
||||
&& point.x <= self.max.x
|
||||
&& point.y >= self.min.y
|
||||
&& point.y <= self.max.y
|
||||
&& point.z >= self.min.z
|
||||
&& point.z <= self.max.z
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_sphere(&self, position: Vector3<f32>, radius: f32) -> bool {
|
||||
let r2 = radius.powi(2);
|
||||
let mut dmin = 0.0;
|
||||
|
||||
if position.x < self.min.x {
|
||||
dmin += (position.x - self.min.x).powi(2);
|
||||
} else if position.x > self.max.x {
|
||||
dmin += (position.x - self.max.x).powi(2);
|
||||
}
|
||||
|
||||
if position.y < self.min.y {
|
||||
dmin += (position.y - self.min.y).powi(2);
|
||||
} else if position.y > self.max.y {
|
||||
dmin += (position.y - self.max.y).powi(2);
|
||||
}
|
||||
|
||||
if position.z < self.min.z {
|
||||
dmin += (position.z - self.min.z).powi(2);
|
||||
} else if position.z > self.max.z {
|
||||
dmin += (position.z - self.max.z).powi(2);
|
||||
}
|
||||
|
||||
dmin <= r2
|
||||
|| ((position.x >= self.min.x)
|
||||
&& (position.x <= self.max.x)
|
||||
&& (position.y >= self.min.y)
|
||||
&& (position.y <= self.max.y)
|
||||
&& (position.z >= self.min.z)
|
||||
&& (position.z <= self.max.z))
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_aabb(&self, other: &Self) -> bool {
|
||||
let self_center = self.center();
|
||||
let self_half_extents = self.half_extents();
|
||||
|
||||
let other_half_extents = other.half_extents();
|
||||
let other_center = other.center();
|
||||
|
||||
if (self_center.x - other_center.x).abs() > (self_half_extents.x + other_half_extents.x) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (self_center.y - other_center.y).abs() > (self_half_extents.y + other_half_extents.y) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (self_center.z - other_center.z).abs() > (self_half_extents.z + other_half_extents.z) {
|
||||
return false;
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
/// Transforms axis-aligned bounding box using given affine transformation matrix.
|
||||
///
|
||||
/// # References
|
||||
///
|
||||
/// Transforming Axis-Aligned Bounding Boxes by Jim Arvo, "Graphics Gems", Academic Press, 1990
|
||||
#[inline]
|
||||
#[must_use]
|
||||
pub fn transform(&self, m: &Matrix4<f32>) -> AxisAlignedBoundingBox {
|
||||
let basis = m.basis();
|
||||
|
||||
let mut transformed = Self {
|
||||
min: m.position(),
|
||||
max: m.position(),
|
||||
};
|
||||
|
||||
for i in 0..3 {
|
||||
for j in 0..3 {
|
||||
let a = basis[(i, j)] * self.min[j];
|
||||
let b = basis[(i, j)] * self.max[j];
|
||||
if a < b {
|
||||
transformed.min[i] += a;
|
||||
transformed.max[i] += b;
|
||||
} else {
|
||||
transformed.min[i] += b;
|
||||
transformed.max[i] += a;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
transformed
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn split(&self) -> [AxisAlignedBoundingBox; 8] {
|
||||
let center = self.center();
|
||||
let min = &self.min;
|
||||
let max = &self.max;
|
||||
[
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(min.x, min.y, min.z),
|
||||
Vector3::new(center.x, center.y, center.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(center.x, min.y, min.z),
|
||||
Vector3::new(max.x, center.y, center.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(min.x, min.y, center.z),
|
||||
Vector3::new(center.x, center.y, max.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(center.x, min.y, center.z),
|
||||
Vector3::new(max.x, center.y, max.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(min.x, center.y, min.z),
|
||||
Vector3::new(center.x, max.y, center.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(center.x, center.y, min.z),
|
||||
Vector3::new(max.x, max.y, center.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(min.x, center.y, center.z),
|
||||
Vector3::new(center.x, max.y, max.z),
|
||||
),
|
||||
AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(center.x, center.y, center.z),
|
||||
Vector3::new(max.x, max.y, max.z),
|
||||
),
|
||||
]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn project(&self, view_projection: &Matrix4<f32>, viewport: &Rect<i32>) -> Rect<f32> {
|
||||
let mut rect_builder = OptionRect::default();
|
||||
for corner in self.corners() {
|
||||
let clip_space = view_projection * Vector4::new(corner.x, corner.y, corner.z, 1.0);
|
||||
let ndc_space = clip_space.xyz() / clip_space.w.abs();
|
||||
let mut normalized_screen_space =
|
||||
Vector2::new((ndc_space.x + 1.0) / 2.0, (1.0 - ndc_space.y) / 2.0);
|
||||
normalized_screen_space.x = normalized_screen_space.x.clamp(0.0, 1.0);
|
||||
normalized_screen_space.y = normalized_screen_space.y.clamp(0.0, 1.0);
|
||||
let screen_space_corner = Vector2::new(
|
||||
(normalized_screen_space.x * viewport.size.x as f32) + viewport.position.x as f32,
|
||||
(normalized_screen_space.y * viewport.size.y as f32) + viewport.position.y as f32,
|
||||
);
|
||||
|
||||
rect_builder.push(screen_space_corner);
|
||||
}
|
||||
rect_builder.unwrap()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use crate::aabb::AxisAlignedBoundingBox;
|
||||
use nalgebra::{Matrix4, Point3, Vector2, Vector3};
|
||||
use rectutils::Rect;
|
||||
|
||||
#[test]
|
||||
fn test_aabb_transform() {
|
||||
let aabb = AxisAlignedBoundingBox {
|
||||
min: Vector3::new(0.0, 0.0, 0.0),
|
||||
max: Vector3::new(1.0, 1.0, 1.0),
|
||||
};
|
||||
|
||||
let transform = Matrix4::new_translation(&Vector3::new(1.0, 1.0, 1.0))
|
||||
* Matrix4::new_nonuniform_scaling(&Vector3::new(2.0, 2.0, 2.0));
|
||||
|
||||
let transformed_aabb = aabb.transform(&transform);
|
||||
|
||||
assert_eq!(transformed_aabb.min, Vector3::new(1.0, 1.0, 1.0));
|
||||
assert_eq!(transformed_aabb.max, Vector3::new(3.0, 3.0, 3.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_default() {
|
||||
let _box = AxisAlignedBoundingBox::default();
|
||||
assert_eq!(_box.min, Vector3::new(f32::MAX, f32::MAX, f32::MAX));
|
||||
assert_eq!(_box.max, Vector3::new(-f32::MAX, -f32::MAX, -f32::MAX));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_unit() {
|
||||
let _box = AxisAlignedBoundingBox::unit();
|
||||
assert_eq!(_box.min, Vector3::new(-0.5, -0.5, -0.5));
|
||||
assert_eq!(_box.max, Vector3::new(0.5, 0.5, 0.5));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_collapsed() {
|
||||
let _box = AxisAlignedBoundingBox::collapsed();
|
||||
assert_eq!(_box.min, Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(_box.max, Vector3::new(0.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_from_radius() {
|
||||
let _box = AxisAlignedBoundingBox::from_radius(1.0);
|
||||
assert_eq!(_box.min, Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(_box.max, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_from_point() {
|
||||
let _box = AxisAlignedBoundingBox::from_point(Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(_box.min, Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(_box.max, Vector3::new(0.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_from_points() {
|
||||
let _box = AxisAlignedBoundingBox::from_points(
|
||||
vec![
|
||||
Vector3::new(-1.0, -1.0, -1.0),
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 1.0),
|
||||
]
|
||||
.as_ref(),
|
||||
);
|
||||
assert_eq!(_box.min, Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(_box.max, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_add_point() {
|
||||
let mut _box = AxisAlignedBoundingBox::default();
|
||||
_box.add_point(Vector3::new(-1.0, -1.0, -1.0));
|
||||
_box.add_point(Vector3::new(1.0, 1.0, 1.0));
|
||||
assert_eq!(_box.min, Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(_box.max, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_inflate() {
|
||||
let mut _box = AxisAlignedBoundingBox::from_radius(1.0);
|
||||
_box.inflate(Vector3::new(5.0, 5.0, 5.0));
|
||||
assert_eq!(_box.min, Vector3::new(-3.5, -3.5, -3.5));
|
||||
assert_eq!(_box.max, Vector3::new(3.5, 3.5, 3.5));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_add_box() {
|
||||
let mut _box = AxisAlignedBoundingBox::collapsed();
|
||||
let _box2 = AxisAlignedBoundingBox::from_radius(1.0);
|
||||
_box.add_box(_box2);
|
||||
assert_eq!(_box.min, Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(_box.max, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_corners() {
|
||||
let _box = AxisAlignedBoundingBox::from_radius(1.0);
|
||||
assert_eq!(
|
||||
_box.corners(),
|
||||
[
|
||||
Vector3::new(-1.0, -1.0, -1.0),
|
||||
Vector3::new(-1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, -1.0, -1.0),
|
||||
Vector3::new(-1.0, 1.0, -1.0),
|
||||
Vector3::new(-1.0, 1.0, 1.0),
|
||||
Vector3::new(1.0, 1.0, 1.0),
|
||||
Vector3::new(1.0, 1.0, -1.0),
|
||||
]
|
||||
);
|
||||
assert_eq!(_box.volume(), 8.0);
|
||||
assert_eq!(_box.center(), Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(_box.half_extents(), Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_offset() {
|
||||
let mut _box = AxisAlignedBoundingBox::unit();
|
||||
_box.offset(Vector3::new(1.0, 1.0, 1.0));
|
||||
assert_eq!(_box.min, Vector3::new(0.5, 0.5, 0.5));
|
||||
assert_eq!(_box.max, Vector3::new(1.5, 1.5, 1.5));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_invalidate() {
|
||||
let mut _box = AxisAlignedBoundingBox::collapsed();
|
||||
_box.invalidate();
|
||||
assert_eq!(_box.min, Vector3::new(f32::MAX, f32::MAX, f32::MAX));
|
||||
assert_eq!(_box.max, Vector3::new(-f32::MAX, -f32::MAX, -f32::MAX));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_valid() {
|
||||
let mut _box = AxisAlignedBoundingBox::default();
|
||||
assert!(!_box.is_valid());
|
||||
|
||||
_box.add_point(Vector3::new(1.0, 1.0, 1.0));
|
||||
assert!(_box.is_valid());
|
||||
|
||||
_box.add_point(Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert!(_box.is_valid());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_degenerate() {
|
||||
let _box = AxisAlignedBoundingBox::unit();
|
||||
assert!(!_box.is_degenerate());
|
||||
|
||||
let _box = AxisAlignedBoundingBox::collapsed();
|
||||
assert!(_box.is_degenerate());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_invalid_or_degenerate() {
|
||||
let mut _box = AxisAlignedBoundingBox::collapsed();
|
||||
assert!(_box.is_invalid_or_degenerate());
|
||||
|
||||
_box.invalidate();
|
||||
assert!(_box.is_invalid_or_degenerate());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_contains_point() {
|
||||
let _box = AxisAlignedBoundingBox::unit();
|
||||
assert!(_box.is_contains_point(Vector3::new(0.0, 0.0, 0.0)));
|
||||
|
||||
for point in _box.corners() {
|
||||
assert!(_box.is_contains_point(point));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_intersects_sphere() {
|
||||
let _box = AxisAlignedBoundingBox::unit();
|
||||
assert!(_box.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 1.0));
|
||||
assert!(_box.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 0.5));
|
||||
assert!(_box.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 1.5));
|
||||
assert!(_box.is_intersects_sphere(Vector3::new(0.5, 0.5, 0.5), 1.0));
|
||||
assert!(_box.is_intersects_sphere(Vector3::new(0.25, 0.25, 0.25), 1.0));
|
||||
|
||||
assert!(!_box.is_intersects_sphere(Vector3::new(10.0, 10.0, 10.0), 1.0));
|
||||
assert!(!_box.is_intersects_sphere(Vector3::new(-10.0, -10.0, -10.0), 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_is_intersects_aabb() {
|
||||
let _box = AxisAlignedBoundingBox::unit();
|
||||
let mut _box2 = _box;
|
||||
assert!(_box.is_intersects_aabb(&_box2));
|
||||
|
||||
_box2.offset(Vector3::new(0.5, 0.0, 0.0));
|
||||
assert!(_box.is_intersects_aabb(&_box2));
|
||||
_box2.offset(Vector3::new(1.0, 0.0, 0.0));
|
||||
assert!(!_box.is_intersects_aabb(&_box2));
|
||||
|
||||
let mut _box2 = _box;
|
||||
_box2.offset(Vector3::new(0.0, 0.5, 0.0));
|
||||
assert!(_box.is_intersects_aabb(&_box2));
|
||||
_box2.offset(Vector3::new(0.0, 1.0, 0.0));
|
||||
assert!(!_box.is_intersects_aabb(&_box2));
|
||||
|
||||
let mut _box2 = _box;
|
||||
_box2.offset(Vector3::new(0.0, 0.0, 0.5));
|
||||
assert!(_box.is_intersects_aabb(&_box2));
|
||||
_box2.offset(Vector3::new(0.0, 0.0, 1.0));
|
||||
assert!(!_box.is_intersects_aabb(&_box2));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_split() {
|
||||
let _box = AxisAlignedBoundingBox::from_radius(1.0);
|
||||
let _boxes = _box.split();
|
||||
|
||||
assert_eq!(_boxes[0].min, Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(_boxes[0].max, Vector3::new(0.0, 0.0, 0.0));
|
||||
|
||||
assert_eq!(_boxes[1].min, Vector3::new(0.0, -1.0, -1.0));
|
||||
assert_eq!(_boxes[1].max, Vector3::new(1.0, 0.0, 0.0));
|
||||
|
||||
assert_eq!(_boxes[2].min, Vector3::new(-1.0, -1.0, 0.0));
|
||||
assert_eq!(_boxes[2].max, Vector3::new(0.0, 0.0, 1.0));
|
||||
|
||||
assert_eq!(_boxes[3].min, Vector3::new(0.0, -1.0, 0.0));
|
||||
assert_eq!(_boxes[3].max, Vector3::new(1.0, 0.0, 1.0));
|
||||
|
||||
assert_eq!(_boxes[4].min, Vector3::new(-1.0, 0.0, -1.0));
|
||||
assert_eq!(_boxes[4].max, Vector3::new(0.0, 1.0, 0.0));
|
||||
|
||||
assert_eq!(_boxes[5].min, Vector3::new(0.0, 0.0, -1.0));
|
||||
assert_eq!(_boxes[5].max, Vector3::new(1.0, 1.0, 0.0));
|
||||
|
||||
assert_eq!(_boxes[6].min, Vector3::new(-1.0, 0.0, 0.0));
|
||||
assert_eq!(_boxes[6].max, Vector3::new(0.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(_boxes[7].min, Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(_boxes[7].max, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_aabb_projection() {
|
||||
let viewport = Rect::new(0, 0, 1000, 1000);
|
||||
let view_projection = Matrix4::new_perspective(1.0, 90.0f32.to_radians(), 0.025, 1000.0)
|
||||
* Matrix4::look_at_rh(
|
||||
&Default::default(),
|
||||
&Point3::new(0.0, 0.0, 1.0),
|
||||
&Vector3::y_axis(),
|
||||
);
|
||||
|
||||
let aabb = AxisAlignedBoundingBox::unit();
|
||||
let rect = aabb.project(&view_projection, &viewport);
|
||||
assert_eq!(rect.position, Vector2::new(0.0, 0.0));
|
||||
assert_eq!(rect.size, Vector2::new(1000.0, 1000.0));
|
||||
|
||||
let aabb = AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(-0.5, 0.0, -0.5),
|
||||
Vector3::new(0.5, 0.5, 0.5),
|
||||
);
|
||||
let rect = aabb.project(&view_projection, &viewport);
|
||||
assert_eq!(rect.position, Vector2::new(0.0, 0.0));
|
||||
assert_eq!(rect.size, Vector2::new(1000.0, 500.0));
|
||||
|
||||
let aabb = AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(-0.5, 0.25, -0.5),
|
||||
Vector3::new(0.5, 0.5, 0.5),
|
||||
);
|
||||
let rect = aabb.project(&view_projection, &viewport);
|
||||
assert_eq!(rect.position, Vector2::new(0.0, 0.0));
|
||||
assert_eq!(rect.size, Vector2::new(1000.0, 250.0));
|
||||
|
||||
let aabb = AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(-10.0, -10.0, -20.0),
|
||||
Vector3::new(10.0, 10.0, 10.0),
|
||||
);
|
||||
let rect = aabb.project(&view_projection, &viewport);
|
||||
assert_eq!(rect.position, Vector2::new(0.0, 0.0));
|
||||
assert_eq!(rect.size, Vector2::new(1000.0, 1000.0));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,545 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use crate::{cubicf, inf_sup_cubicf, lerpf, Rect};
|
||||
use std::cmp::Ordering;
|
||||
use uuid::Uuid;
|
||||
|
||||
fn stepf(p0: f32, p1: f32, t: f32) -> f32 {
|
||||
if t.eq(&1.0) {
|
||||
p1
|
||||
} else {
|
||||
p0
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default, Clone, Debug, PartialEq)]
|
||||
pub enum CurveKeyKind {
|
||||
#[default]
|
||||
Constant,
|
||||
Linear,
|
||||
Cubic {
|
||||
/// A `tan(angle)` of left tangent.
|
||||
left_tangent: f32,
|
||||
/// A `tan(angle)` of right tangent.
|
||||
right_tangent: f32,
|
||||
},
|
||||
}
|
||||
|
||||
impl CurveKeyKind {
|
||||
#[inline]
|
||||
pub fn new_cubic(left_angle_radians: f32, right_angle_radians: f32) -> Self {
|
||||
Self::Cubic {
|
||||
left_tangent: left_angle_radians.tan(),
|
||||
right_tangent: right_angle_radians.tan(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Default, Debug, PartialEq)]
|
||||
pub struct CurveKey {
|
||||
pub id: Uuid,
|
||||
pub location: f32,
|
||||
pub value: f32,
|
||||
pub kind: CurveKeyKind,
|
||||
}
|
||||
|
||||
impl CurveKey {
|
||||
#[inline]
|
||||
pub fn new(location: f32, value: f32, kind: CurveKeyKind) -> Self {
|
||||
Self {
|
||||
id: Uuid::new_v4(),
|
||||
location,
|
||||
value,
|
||||
kind,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn short_path_angles(mut start: f32, mut end: f32) -> (f32, f32) {
|
||||
if (end - start).abs() > std::f32::consts::PI {
|
||||
if end > start {
|
||||
start += std::f32::consts::TAU;
|
||||
} else {
|
||||
end += std::f32::consts::TAU;
|
||||
}
|
||||
}
|
||||
(start, end)
|
||||
}
|
||||
|
||||
fn interpolate(
|
||||
left_value: f32,
|
||||
left_kind: &CurveKeyKind,
|
||||
right_value: f32,
|
||||
right_kind: &CurveKeyKind,
|
||||
t: f32,
|
||||
) -> f32 {
|
||||
match (left_kind, right_kind) {
|
||||
// Constant-to-any
|
||||
(CurveKeyKind::Constant, CurveKeyKind::Constant)
|
||||
| (CurveKeyKind::Constant, CurveKeyKind::Linear)
|
||||
| (CurveKeyKind::Constant, CurveKeyKind::Cubic { .. }) => stepf(left_value, right_value, t),
|
||||
|
||||
// Linear-to-any
|
||||
(CurveKeyKind::Linear, CurveKeyKind::Constant)
|
||||
| (CurveKeyKind::Linear, CurveKeyKind::Linear)
|
||||
| (CurveKeyKind::Linear, CurveKeyKind::Cubic { .. }) => lerpf(left_value, right_value, t),
|
||||
|
||||
// Cubic-to-constant or cubic-to-linear
|
||||
(
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Constant,
|
||||
)
|
||||
| (
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Linear,
|
||||
) => cubicf(left_value, right_value, t, *left_tangent, 0.0),
|
||||
|
||||
// Cubic-to-cubic
|
||||
(
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Cubic {
|
||||
left_tangent: right_tangent,
|
||||
..
|
||||
},
|
||||
) => cubicf(left_value, right_value, t, *left_tangent, *right_tangent),
|
||||
}
|
||||
}
|
||||
|
||||
impl CurveKey {
|
||||
#[inline]
|
||||
pub fn location(&self) -> f32 {
|
||||
self.location
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn interpolate(&self, other: &Self, t: f32) -> f32 {
|
||||
interpolate(self.value, &self.kind, other.value, &other.kind, t)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn interpolate_angles(&self, other: &Self, t: f32) -> f32 {
|
||||
let (left_value, right_value) = short_path_angles(self.value, other.value);
|
||||
interpolate(left_value, &self.kind, right_value, &other.kind, t)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, PartialEq)]
|
||||
pub struct Curve {
|
||||
pub id: Uuid,
|
||||
pub name: String,
|
||||
pub keys: Vec<CurveKey>,
|
||||
}
|
||||
|
||||
impl Default for Curve {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
id: Uuid::new_v4(),
|
||||
name: Default::default(),
|
||||
keys: Default::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn sort_keys(keys: &mut [CurveKey]) {
|
||||
keys.sort_by(|a, b| {
|
||||
if a.location > b.location {
|
||||
Ordering::Greater
|
||||
} else if a.location < b.location {
|
||||
Ordering::Less
|
||||
} else {
|
||||
Ordering::Equal
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
impl From<Vec<CurveKey>> for Curve {
|
||||
fn from(mut keys: Vec<CurveKey>) -> Self {
|
||||
sort_keys(&mut keys);
|
||||
Self {
|
||||
id: Uuid::new_v4(),
|
||||
name: Default::default(),
|
||||
keys,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Curve {
|
||||
#[inline]
|
||||
pub fn set_id(&mut self, id: Uuid) {
|
||||
self.id = id;
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn id(&self) -> Uuid {
|
||||
self.id
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn set_name<S: AsRef<str>>(&mut self, name: S) {
|
||||
name.as_ref().clone_into(&mut self.name);
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn name(&self) -> &str {
|
||||
&self.name
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn clear(&mut self) {
|
||||
self.keys.clear()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.keys.is_empty()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn keys(&self) -> &[CurveKey] {
|
||||
&self.keys
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn keys_values(&mut self) -> impl Iterator<Item = &mut f32> {
|
||||
self.keys.iter_mut().map(|k| &mut k.value)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn add_key(&mut self, new_key: CurveKey) {
|
||||
let pos = self.keys.partition_point(|k| k.location < new_key.location);
|
||||
self.keys.insert(pos, new_key);
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn move_key(&mut self, key_id: usize, location: f32) {
|
||||
if let Some(key) = self.keys.get_mut(key_id) {
|
||||
key.location = location;
|
||||
sort_keys(&mut self.keys);
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn max_location(&self) -> f32 {
|
||||
self.keys.last().map(|k| k.location).unwrap_or_default()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn fetch_at<I>(&self, location: f32, interpolator: I) -> f32
|
||||
where
|
||||
I: FnOnce(&CurveKey, &CurveKey, f32) -> f32,
|
||||
{
|
||||
if let (Some(first), Some(last)) = (self.keys.first(), self.keys.last()) {
|
||||
if location <= first.location {
|
||||
first.value
|
||||
} else if location >= last.location {
|
||||
last.value
|
||||
} else {
|
||||
// Use binary search for multiple spans.
|
||||
let pos = self.keys.partition_point(|k| k.location < location);
|
||||
let left = self.keys.get(pos.saturating_sub(1)).unwrap();
|
||||
let right = self.keys.get(pos).unwrap();
|
||||
let t = (location - left.location) / (right.location - left.location);
|
||||
interpolator(left, right, t)
|
||||
}
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn value_at(&self, location: f32) -> f32 {
|
||||
self.fetch_at(location, |a, b, t| a.interpolate(b, t))
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn angle_at(&self, location: f32) -> f32 {
|
||||
self.fetch_at(location, |a, b, t| a.interpolate_angles(b, t))
|
||||
}
|
||||
|
||||
pub fn bounds(&self) -> Rect<f32> {
|
||||
// Handle edge cases first.
|
||||
if self.keys.is_empty() {
|
||||
return Rect::new(0.0, 0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
if self.keys.len() == 1 {
|
||||
let first = self.keys().first().unwrap();
|
||||
return Rect::new(first.location, first.value, 0.0, 0.0);
|
||||
}
|
||||
|
||||
// If there's 2 or more keys, calculate bounds normally.
|
||||
let mut max_y = -f32::MAX;
|
||||
let mut min_y = f32::MAX;
|
||||
let mut max_x = -f32::MAX;
|
||||
let mut min_x = f32::MAX;
|
||||
|
||||
let mut push = |x: f32, y: f32| {
|
||||
if x > max_x {
|
||||
max_x = x;
|
||||
}
|
||||
if x < min_x {
|
||||
min_x = x;
|
||||
}
|
||||
if y > max_y {
|
||||
max_y = y;
|
||||
}
|
||||
if y < min_y {
|
||||
min_y = y;
|
||||
}
|
||||
};
|
||||
|
||||
for keys in self.keys.windows(2) {
|
||||
let left = &keys[0];
|
||||
let right = &keys[1];
|
||||
match (&left.kind, &right.kind) {
|
||||
// Cubic-to-constant and cubic-to-linear is depicted as Hermite spline with right tangent == 0.0.
|
||||
(
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Constant,
|
||||
)
|
||||
| (
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Linear,
|
||||
) => {
|
||||
let (y0, y1) = inf_sup_cubicf(left.value, right.value, *left_tangent, 0.0);
|
||||
push(left.location, y0);
|
||||
push(right.location, y1);
|
||||
}
|
||||
|
||||
// Cubic-to-cubic is depicted as Hermite spline.
|
||||
(
|
||||
CurveKeyKind::Cubic {
|
||||
right_tangent: left_tangent,
|
||||
..
|
||||
},
|
||||
CurveKeyKind::Cubic {
|
||||
left_tangent: right_tangent,
|
||||
..
|
||||
},
|
||||
) => {
|
||||
let (y0, y1) =
|
||||
inf_sup_cubicf(left.value, right.value, *left_tangent, *right_tangent);
|
||||
push(left.location, y0);
|
||||
push(right.location, y1);
|
||||
}
|
||||
_ => {
|
||||
push(left.location, left.value);
|
||||
push(right.location, right.value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Rect::new(min_x, min_y, max_x - min_x, max_y - min_y)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use uuid::Uuid;
|
||||
|
||||
use crate::curve::{Curve, CurveKey, CurveKeyKind};
|
||||
|
||||
#[test]
|
||||
fn test_curve_key_insertion_order() {
|
||||
let mut curve = Curve::default();
|
||||
|
||||
// Insert keys in arbitrary order with arbitrary location.
|
||||
curve.add_key(CurveKey::new(0.0, 0.0, CurveKeyKind::Constant));
|
||||
curve.add_key(CurveKey::new(-1.0, 0.0, CurveKeyKind::Constant));
|
||||
curve.add_key(CurveKey::new(3.0, 0.0, CurveKeyKind::Constant));
|
||||
curve.add_key(CurveKey::new(2.0, 0.0, CurveKeyKind::Constant));
|
||||
curve.add_key(CurveKey::new(-5.0, 0.0, CurveKeyKind::Constant));
|
||||
|
||||
// Ensure that keys are sorted by their location.
|
||||
assert_eq!(curve.keys[0].location, -5.0);
|
||||
assert_eq!(curve.keys[1].location, -1.0);
|
||||
assert_eq!(curve.keys[2].location, 0.0);
|
||||
assert_eq!(curve.keys[3].location, 2.0);
|
||||
assert_eq!(curve.keys[4].location, 3.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_curve() {
|
||||
let mut curve = Curve::default();
|
||||
|
||||
// Test fetching from empty curve.
|
||||
assert_eq!(curve.value_at(0.0), 0.0);
|
||||
|
||||
curve.add_key(CurveKey::new(0.0, 1.0, CurveKeyKind::Linear));
|
||||
|
||||
// One-key curves must always return its single key value.
|
||||
assert_eq!(curve.value_at(-1.0), 1.0);
|
||||
assert_eq!(curve.value_at(1.0), 1.0);
|
||||
assert_eq!(curve.value_at(0.0), 1.0);
|
||||
|
||||
curve.add_key(CurveKey::new(1.0, 0.0, CurveKeyKind::Linear));
|
||||
|
||||
// Two-key curves must always use interpolation.
|
||||
assert_eq!(curve.value_at(-1.0), 1.0);
|
||||
assert_eq!(curve.value_at(2.0), 0.0);
|
||||
assert_eq!(curve.value_at(0.5), 0.5);
|
||||
|
||||
// Add one more key and do more checks.
|
||||
curve.add_key(CurveKey::new(2.0, 1.0, CurveKeyKind::Linear));
|
||||
|
||||
// Check order of the keys.
|
||||
assert!(curve.keys[0].location <= curve.keys[1].location);
|
||||
assert!(curve.keys[1].location <= curve.keys[2].location);
|
||||
|
||||
// Check generic out-of-bounds fetching.
|
||||
assert_eq!(curve.value_at(-1.0), 1.0); // Left side oob
|
||||
assert_eq!(curve.value_at(3.0), 1.0); // Right side oob.
|
||||
|
||||
// Check edge cases.
|
||||
assert_eq!(curve.value_at(0.0), 1.0); // Left edge.
|
||||
assert_eq!(curve.value_at(2.0), 1.0); // Right edge.
|
||||
|
||||
// Check interpolation.
|
||||
assert_eq!(curve.value_at(0.5), 0.5);
|
||||
|
||||
// Check id.
|
||||
let id = Uuid::new_v4();
|
||||
curve.set_id(id);
|
||||
assert_eq!(curve.id(), id);
|
||||
|
||||
// Check name.
|
||||
let name = "name";
|
||||
curve.set_name(name);
|
||||
assert_eq!(curve.name(), name);
|
||||
|
||||
// Check keys capacity.
|
||||
assert!(!curve.is_empty());
|
||||
curve.clear();
|
||||
assert!(curve.is_empty());
|
||||
|
||||
// Check keys.
|
||||
let key = CurveKey::new(0.0, 5.0, CurveKeyKind::Constant);
|
||||
let key2 = CurveKey::new(1.0, 10.0, CurveKeyKind::Linear);
|
||||
curve.add_key(key.clone());
|
||||
curve.add_key(key2.clone());
|
||||
assert_eq!(curve.keys(), vec![key.clone(), key2.clone()]);
|
||||
|
||||
// Check keys values.
|
||||
let mut values = [5.0, 10.0];
|
||||
assert!(curve.keys_values().eq(values.iter_mut()));
|
||||
|
||||
// Check max location.
|
||||
assert_eq!(curve.max_location(), 1.0);
|
||||
|
||||
// Check key moving.
|
||||
let mut curve2 = curve.clone();
|
||||
let key3 = CurveKey::default();
|
||||
curve2.add_key(key3.clone());
|
||||
assert_eq!(curve2.keys(), vec![key3.clone(), key.clone(), key2.clone()]);
|
||||
curve2.move_key(key3.id.get_version_num(), 20.0);
|
||||
assert_eq!(
|
||||
curve2.keys(),
|
||||
vec![
|
||||
key.clone(),
|
||||
key2.clone(),
|
||||
CurveKey {
|
||||
location: 20.0,
|
||||
..Default::default()
|
||||
}
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_curve_key_kind() {
|
||||
assert_eq!(CurveKeyKind::default(), CurveKeyKind::Constant);
|
||||
assert_eq!(
|
||||
CurveKeyKind::new_cubic(0.0, 0.0),
|
||||
CurveKeyKind::Cubic {
|
||||
left_tangent: 0.0,
|
||||
right_tangent: 0.0
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_curve_key() {
|
||||
assert_eq!(
|
||||
CurveKey::default(),
|
||||
CurveKey {
|
||||
id: Uuid::default(),
|
||||
location: 0.0,
|
||||
value: 0.0,
|
||||
kind: CurveKeyKind::Constant,
|
||||
},
|
||||
);
|
||||
|
||||
let key = CurveKey::new(0.0, 5.0, CurveKeyKind::Constant);
|
||||
let key2 = CurveKey::new(1.0, 10.0, CurveKeyKind::Linear);
|
||||
let key3 = CurveKey::new(2.0, 20.0, CurveKeyKind::new_cubic(0.0, 0.0));
|
||||
let key4 = CurveKey::new(3.0, 30.0, CurveKeyKind::new_cubic(0.0, 0.0));
|
||||
|
||||
assert_eq!(key.location(), 0.0);
|
||||
|
||||
// Constant-to-any
|
||||
assert_eq!(key.interpolate(&key2, 1.0), 10.0);
|
||||
assert_eq!(key.interpolate(&key2, 0.0), 5.0);
|
||||
assert_eq!(key.interpolate(&key3, 1.0), 20.0);
|
||||
assert_eq!(key.interpolate(&key3, 0.0), 5.0);
|
||||
|
||||
// Linear-to-any
|
||||
assert_eq!(key2.interpolate(&key, 1.0), 5.0);
|
||||
assert_eq!(key2.interpolate(&key, 0.0), 10.0);
|
||||
assert_eq!(key2.interpolate(&key3, 1.0), 20.0);
|
||||
assert_eq!(key2.interpolate(&key3, 0.0), 10.0);
|
||||
|
||||
// Cubic-to-constant or cubic-to-linear
|
||||
assert_eq!(key3.interpolate(&key, 1.0), 5.0);
|
||||
assert_eq!(key3.interpolate(&key, 0.0), 20.0);
|
||||
assert_eq!(key3.interpolate(&key2, 1.0), 10.0);
|
||||
assert_eq!(key3.interpolate(&key2, 0.0), 20.0);
|
||||
|
||||
// Cubic-to-cubic
|
||||
assert_eq!(key3.interpolate(&key4, 1.0), 30.0);
|
||||
assert_eq!(key3.interpolate(&key4, 0.0), 20.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_curve_from_vec() {
|
||||
let key = CurveKey::new(-1.0, -1.0, CurveKeyKind::Constant);
|
||||
let key2 = CurveKey::new(0.0, 0.0, CurveKeyKind::Constant);
|
||||
let key3 = CurveKey::new(1.0, 1.0, CurveKeyKind::Constant);
|
||||
let key4 = key2.clone();
|
||||
let curve = Curve::from(vec![key2.clone(), key3.clone(), key.clone(), key4.clone()]);
|
||||
assert_eq!(curve.name(), "");
|
||||
assert_eq!(curve.keys(), vec![key, key2, key4, key3,]);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,553 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use crate::{aabb::AxisAlignedBoundingBox, plane::Plane};
|
||||
use nalgebra::Point3;
|
||||
use nalgebra::{Matrix4, Vector3};
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||
pub struct Frustum {
|
||||
/// 0 - left, 1 - right, 2 - top, 3 - bottom, 4 - far, 5 - near
|
||||
pub planes: [Plane; 6],
|
||||
pub corners: [Vector3<f32>; 8],
|
||||
}
|
||||
|
||||
impl Default for Frustum {
|
||||
#[inline]
|
||||
fn default() -> Self {
|
||||
Self::from_view_projection_matrix(Matrix4::new_perspective(
|
||||
1.0,
|
||||
std::f32::consts::FRAC_PI_2,
|
||||
0.01,
|
||||
1024.0,
|
||||
))
|
||||
.unwrap()
|
||||
}
|
||||
}
|
||||
|
||||
impl Frustum {
|
||||
pub const LEFT: usize = 0;
|
||||
pub const RIGHT: usize = 1;
|
||||
pub const TOP: usize = 2;
|
||||
pub const BOTTOM: usize = 3;
|
||||
pub const FAR: usize = 4;
|
||||
pub const NEAR: usize = 5;
|
||||
|
||||
#[inline]
|
||||
pub fn from_view_projection_matrix(m: Matrix4<f32>) -> Option<Self> {
|
||||
let planes = [
|
||||
// Left
|
||||
Plane::from_abcd(m[3] + m[0], m[7] + m[4], m[11] + m[8], m[15] + m[12])?,
|
||||
// Right
|
||||
Plane::from_abcd(m[3] - m[0], m[7] - m[4], m[11] - m[8], m[15] - m[12])?,
|
||||
// Top
|
||||
Plane::from_abcd(m[3] - m[1], m[7] - m[5], m[11] - m[9], m[15] - m[13])?,
|
||||
// Bottom
|
||||
Plane::from_abcd(m[3] + m[1], m[7] + m[5], m[11] + m[9], m[15] + m[13])?,
|
||||
// Far
|
||||
Plane::from_abcd(m[3] - m[2], m[7] - m[6], m[11] - m[10], m[15] - m[14])?,
|
||||
// Near
|
||||
Plane::from_abcd(m[3] + m[2], m[7] + m[6], m[11] + m[10], m[15] + m[14])?,
|
||||
];
|
||||
|
||||
let corners = [
|
||||
planes[Self::LEFT].intersection_point(&planes[Self::TOP], &planes[Self::FAR]),
|
||||
planes[Self::LEFT].intersection_point(&planes[Self::BOTTOM], &planes[Self::FAR]),
|
||||
planes[Self::RIGHT].intersection_point(&planes[Self::BOTTOM], &planes[Self::FAR]),
|
||||
planes[Self::RIGHT].intersection_point(&planes[Self::TOP], &planes[Self::FAR]),
|
||||
planes[Self::LEFT].intersection_point(&planes[Self::TOP], &planes[Self::NEAR]),
|
||||
planes[Self::LEFT].intersection_point(&planes[Self::BOTTOM], &planes[Self::NEAR]),
|
||||
planes[Self::RIGHT].intersection_point(&planes[Self::BOTTOM], &planes[Self::NEAR]),
|
||||
planes[Self::RIGHT].intersection_point(&planes[Self::TOP], &planes[Self::NEAR]),
|
||||
];
|
||||
|
||||
Some(Self { planes, corners })
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn left(&self) -> &Plane {
|
||||
self.planes.first().unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn right(&self) -> &Plane {
|
||||
self.planes.get(1).unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn top(&self) -> &Plane {
|
||||
self.planes.get(2).unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn bottom(&self) -> &Plane {
|
||||
self.planes.get(3).unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn far(&self) -> &Plane {
|
||||
self.planes.get(4).unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn near(&self) -> &Plane {
|
||||
self.planes.get(5).unwrap()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn planes(&self) -> &[Plane] {
|
||||
&self.planes
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn left_top_front_corner(&self) -> Vector3<f32> {
|
||||
self.corners[0]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn left_bottom_front_corner(&self) -> Vector3<f32> {
|
||||
self.corners[1]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn right_bottom_front_corner(&self) -> Vector3<f32> {
|
||||
self.corners[2]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn right_top_front_corner(&self) -> Vector3<f32> {
|
||||
self.corners[3]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn left_top_back_corner(&self) -> Vector3<f32> {
|
||||
self.corners[4]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn left_bottom_back_corner(&self) -> Vector3<f32> {
|
||||
self.corners[5]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn right_bottom_back_corner(&self) -> Vector3<f32> {
|
||||
self.corners[6]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn right_top_back_corner(&self) -> Vector3<f32> {
|
||||
self.corners[7]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn corners(&self) -> [Vector3<f32>; 8] {
|
||||
[
|
||||
self.left_top_front_corner(),
|
||||
self.left_bottom_front_corner(),
|
||||
self.right_bottom_front_corner(),
|
||||
self.right_top_front_corner(),
|
||||
self.left_top_back_corner(),
|
||||
self.left_bottom_back_corner(),
|
||||
self.right_bottom_back_corner(),
|
||||
self.right_top_back_corner(),
|
||||
]
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn near_plane_center(&self) -> Vector3<f32> {
|
||||
(self.left_top_front_corner()
|
||||
+ self.left_bottom_front_corner()
|
||||
+ self.right_bottom_front_corner()
|
||||
+ self.right_top_front_corner())
|
||||
.scale(1.0 / 4.0)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn far_plane_center(&self) -> Vector3<f32> {
|
||||
(self.left_top_back_corner()
|
||||
+ self.left_bottom_back_corner()
|
||||
+ self.right_bottom_back_corner()
|
||||
+ self.right_top_back_corner())
|
||||
.scale(1.0 / 4.0)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn view_direction(&self) -> Vector3<f32> {
|
||||
self.far_plane_center() - self.near_plane_center()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn center(&self) -> Vector3<f32> {
|
||||
self.corners()
|
||||
.iter()
|
||||
.fold(Vector3::default(), |acc, corner| acc + *corner)
|
||||
.scale(1.0 / 8.0)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_point_cloud(&self, points: &[Vector3<f32>]) -> bool {
|
||||
for plane in self.planes.iter() {
|
||||
let mut back_points = 0;
|
||||
for point in points {
|
||||
if plane.dot(point) <= 0.0 {
|
||||
back_points += 1;
|
||||
if back_points >= points.len() {
|
||||
// All points are behind current plane.
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_aabb(&self, aabb: &AxisAlignedBoundingBox) -> bool {
|
||||
let corners = [
|
||||
Vector3::new(aabb.min.x, aabb.min.y, aabb.min.z),
|
||||
Vector3::new(aabb.min.x, aabb.min.y, aabb.max.z),
|
||||
Vector3::new(aabb.max.x, aabb.min.y, aabb.max.z),
|
||||
Vector3::new(aabb.max.x, aabb.min.y, aabb.min.z),
|
||||
Vector3::new(aabb.min.x, aabb.max.y, aabb.min.z),
|
||||
Vector3::new(aabb.min.x, aabb.max.y, aabb.max.z),
|
||||
Vector3::new(aabb.max.x, aabb.max.y, aabb.max.z),
|
||||
Vector3::new(aabb.max.x, aabb.max.y, aabb.min.z),
|
||||
];
|
||||
|
||||
if self.is_intersects_point_cloud(&corners) {
|
||||
return true;
|
||||
}
|
||||
|
||||
for corner in self.corners.iter() {
|
||||
if aabb.is_contains_point(*corner) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
false
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_aabb_offset(
|
||||
&self,
|
||||
aabb: &AxisAlignedBoundingBox,
|
||||
offset: Vector3<f32>,
|
||||
) -> bool {
|
||||
let corners = [
|
||||
Vector3::new(aabb.min.x, aabb.min.y, aabb.min.z) + offset,
|
||||
Vector3::new(aabb.min.x, aabb.min.y, aabb.max.z) + offset,
|
||||
Vector3::new(aabb.max.x, aabb.min.y, aabb.max.z) + offset,
|
||||
Vector3::new(aabb.max.x, aabb.min.y, aabb.min.z) + offset,
|
||||
Vector3::new(aabb.min.x, aabb.max.y, aabb.min.z) + offset,
|
||||
Vector3::new(aabb.min.x, aabb.max.y, aabb.max.z) + offset,
|
||||
Vector3::new(aabb.max.x, aabb.max.y, aabb.max.z) + offset,
|
||||
Vector3::new(aabb.max.x, aabb.max.y, aabb.min.z) + offset,
|
||||
];
|
||||
|
||||
if self.is_intersects_point_cloud(&corners) {
|
||||
return true;
|
||||
}
|
||||
|
||||
for corner in self.corners.iter() {
|
||||
if aabb.is_contains_point(*corner) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
false
|
||||
}
|
||||
|
||||
#[deprecated(
|
||||
since = "0.29.0",
|
||||
note = "this method does not handle all cases and could give weird results"
|
||||
)]
|
||||
#[inline]
|
||||
pub fn is_intersects_aabb_transform(
|
||||
&self,
|
||||
aabb: &AxisAlignedBoundingBox,
|
||||
transform: &Matrix4<f32>,
|
||||
) -> bool {
|
||||
if self.is_contains_point(
|
||||
transform
|
||||
.transform_point(&Point3::from(aabb.center()))
|
||||
.coords,
|
||||
) {
|
||||
return true;
|
||||
}
|
||||
|
||||
let corners = [
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.min.x, aabb.min.y, aabb.min.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.min.x, aabb.min.y, aabb.max.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.max.x, aabb.min.y, aabb.max.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.max.x, aabb.min.y, aabb.min.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.min.x, aabb.max.y, aabb.min.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.min.x, aabb.max.y, aabb.max.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.max.x, aabb.max.y, aabb.max.z))
|
||||
.coords,
|
||||
transform
|
||||
.transform_point(&Point3::new(aabb.max.x, aabb.max.y, aabb.min.z))
|
||||
.coords,
|
||||
];
|
||||
|
||||
self.is_intersects_point_cloud(&corners)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_contains_point(&self, pt: Vector3<f32>) -> bool {
|
||||
for plane in self.planes.iter() {
|
||||
if plane.dot(&pt) <= 0.0 {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn is_intersects_sphere(&self, p: Vector3<f32>, r: f32) -> bool {
|
||||
for plane in self.planes.iter() {
|
||||
let d = plane.dot(&p);
|
||||
if d < -r {
|
||||
return false;
|
||||
}
|
||||
if d.abs() < r {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
true
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use crate::aabb::AxisAlignedBoundingBox;
|
||||
use crate::{frustum::Frustum, plane::Plane};
|
||||
use nalgebra::{Matrix4, Vector3};
|
||||
|
||||
#[test]
|
||||
fn test_default_for_frustum() {
|
||||
assert_eq!(
|
||||
Frustum::default(),
|
||||
Frustum::from_view_projection_matrix(Matrix4::new_perspective(
|
||||
1.0,
|
||||
std::f32::consts::FRAC_PI_2,
|
||||
0.01,
|
||||
1024.0
|
||||
))
|
||||
.unwrap()
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_from_view_projection_matrix() {
|
||||
assert_eq!(
|
||||
Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0
|
||||
)),
|
||||
Some(Frustum {
|
||||
planes: [
|
||||
Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap(),
|
||||
],
|
||||
corners: [
|
||||
Vector3::new(-1.0, 1.0, 1.0),
|
||||
Vector3::new(-1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, 1.0, 1.0),
|
||||
Vector3::new(-1.0, 1.0, -1.0),
|
||||
Vector3::new(-1.0, -1.0, -1.0),
|
||||
Vector3::new(1.0, -1.0, -1.0),
|
||||
Vector3::new(1.0, 1.0, -1.0),
|
||||
],
|
||||
})
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_planes_and_corners() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert_eq!(f.left(), &Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap());
|
||||
assert_eq!(f.right(), &Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap());
|
||||
assert_eq!(f.top(), &Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap());
|
||||
assert_eq!(f.bottom(), &Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap());
|
||||
assert_eq!(f.far(), &Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap());
|
||||
assert_eq!(f.near(), &Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap());
|
||||
|
||||
assert_eq!(
|
||||
f.planes(),
|
||||
[
|
||||
Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap(),
|
||||
Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap(),
|
||||
]
|
||||
);
|
||||
|
||||
assert_eq!(f.left_top_front_corner(), Vector3::new(-1.0, 1.0, 1.0));
|
||||
assert_eq!(f.left_bottom_front_corner(), Vector3::new(-1.0, -1.0, 1.0));
|
||||
assert_eq!(f.right_bottom_front_corner(), Vector3::new(1.0, -1.0, 1.0));
|
||||
assert_eq!(f.right_top_front_corner(), Vector3::new(1.0, 1.0, 1.0));
|
||||
assert_eq!(f.left_top_back_corner(), Vector3::new(-1.0, 1.0, -1.0));
|
||||
assert_eq!(f.left_bottom_back_corner(), Vector3::new(-1.0, -1.0, -1.0));
|
||||
assert_eq!(f.right_bottom_back_corner(), Vector3::new(1.0, -1.0, -1.0));
|
||||
assert_eq!(f.right_top_back_corner(), Vector3::new(1.0, 1.0, -1.0));
|
||||
|
||||
assert_eq!(
|
||||
f.corners(),
|
||||
[
|
||||
Vector3::new(-1.0, 1.0, 1.0),
|
||||
Vector3::new(-1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, -1.0, 1.0),
|
||||
Vector3::new(1.0, 1.0, 1.0),
|
||||
Vector3::new(-1.0, 1.0, -1.0),
|
||||
Vector3::new(-1.0, -1.0, -1.0),
|
||||
Vector3::new(1.0, -1.0, -1.0),
|
||||
Vector3::new(1.0, 1.0, -1.0),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_plane_centers() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert_eq!(f.near_plane_center(), Vector3::new(0.0, 0.0, 1.0));
|
||||
assert_eq!(f.far_plane_center(), Vector3::new(0.0, 0.0, -1.0));
|
||||
assert_eq!(f.view_direction(), Vector3::new(0.0, 0.0, -2.0));
|
||||
assert_eq!(f.center(), Vector3::new(0.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_is_intersects_point_cloud() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert!(f.is_intersects_point_cloud(&[
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 1.0),
|
||||
]));
|
||||
assert!(!f.is_intersects_point_cloud(&[Vector3::new(-1.0, -2.0, 1.0)]));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_is_intersects_aabb() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert!(f.is_intersects_aabb(&AxisAlignedBoundingBox::unit()));
|
||||
assert!(!f.is_intersects_aabb(&AxisAlignedBoundingBox::from_min_max(
|
||||
Vector3::new(5.0, 5.0, 5.0),
|
||||
Vector3::new(15.0, 15.0, 15.0)
|
||||
)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_is_intersects_aabb_offset() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert!(f.is_intersects_aabb_offset(
|
||||
&AxisAlignedBoundingBox::unit(),
|
||||
Vector3::new(1.0, 1.0, 1.0)
|
||||
));
|
||||
assert!(!f.is_intersects_aabb_offset(
|
||||
&AxisAlignedBoundingBox::unit(),
|
||||
Vector3::new(10.0, 10.0, 10.0)
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_is_contains_point() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert!(f.is_contains_point(Vector3::new(0.0, 0.0, 0.0)));
|
||||
assert!(!f.is_contains_point(Vector3::new(10.0, 10.0, 10.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frustum_is_intersects_sphere() {
|
||||
let f = Frustum::from_view_projection_matrix(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
))
|
||||
.unwrap();
|
||||
|
||||
assert!(f.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 1.0));
|
||||
assert!(f.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 2.0));
|
||||
assert!(!f.is_intersects_sphere(Vector3::new(10.0, 10.0, 10.0), 1.0));
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,387 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use crate::{aabb::AxisAlignedBoundingBox, ray::Ray};
|
||||
use arrayvec::ArrayVec;
|
||||
use nalgebra::Vector3;
|
||||
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum OctreeNode {
|
||||
Leaf {
|
||||
indices: Vec<u32>,
|
||||
bounds: AxisAlignedBoundingBox,
|
||||
},
|
||||
Branch {
|
||||
bounds: AxisAlignedBoundingBox,
|
||||
leaves: [usize; 8],
|
||||
},
|
||||
}
|
||||
|
||||
#[derive(Default, Clone, Debug)]
|
||||
pub struct Octree {
|
||||
nodes: Vec<OctreeNode>,
|
||||
root: usize,
|
||||
}
|
||||
|
||||
impl Octree {
|
||||
pub fn new(triangles: &[[Vector3<f32>; 3]], split_threshold: usize) -> Self {
|
||||
// Calculate bounds.
|
||||
let mut bounds = AxisAlignedBoundingBox::default();
|
||||
for triangle in triangles {
|
||||
for pt in triangle.iter() {
|
||||
bounds.add_point(*pt);
|
||||
}
|
||||
}
|
||||
|
||||
// Inflate initial bounds by very low value to fix floating-point calculation
|
||||
// issues when splitting and checking intersection later on.
|
||||
let inflation = 2.0 * f32::EPSILON;
|
||||
bounds.inflate(Vector3::new(inflation, inflation, inflation));
|
||||
|
||||
// Get initial list of indices.
|
||||
let mut indices = Vec::new();
|
||||
for i in 0..triangles.len() {
|
||||
indices.push(i as u32);
|
||||
}
|
||||
|
||||
let mut nodes = Vec::new();
|
||||
let root = build_recursive(&mut nodes, triangles, bounds, indices, split_threshold);
|
||||
|
||||
Self { nodes, root }
|
||||
}
|
||||
|
||||
pub fn sphere_query(&self, position: Vector3<f32>, radius: f32, buffer: &mut Vec<u32>) {
|
||||
buffer.clear();
|
||||
self.sphere_recursive_query(self.root, position, radius, buffer);
|
||||
}
|
||||
|
||||
fn sphere_recursive_query(
|
||||
&self,
|
||||
node: usize,
|
||||
position: Vector3<f32>,
|
||||
radius: f32,
|
||||
buffer: &mut Vec<u32>,
|
||||
) {
|
||||
match &self.nodes[node] {
|
||||
OctreeNode::Leaf { indices, bounds } => {
|
||||
if bounds.is_intersects_sphere(position, radius) {
|
||||
buffer.extend_from_slice(indices)
|
||||
}
|
||||
}
|
||||
OctreeNode::Branch { bounds, leaves } => {
|
||||
if bounds.is_intersects_sphere(position, radius) {
|
||||
for leaf in leaves {
|
||||
self.sphere_recursive_query(*leaf, position, radius, buffer)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn aabb_query(&self, aabb: &AxisAlignedBoundingBox, buffer: &mut Vec<u32>) {
|
||||
buffer.clear();
|
||||
self.aabb_recursive_query(self.root, aabb, buffer);
|
||||
}
|
||||
|
||||
fn aabb_recursive_query(
|
||||
&self,
|
||||
node: usize,
|
||||
aabb: &AxisAlignedBoundingBox,
|
||||
buffer: &mut Vec<u32>,
|
||||
) {
|
||||
match &self.nodes[node] {
|
||||
OctreeNode::Leaf { indices, bounds } => {
|
||||
if bounds.is_intersects_aabb(aabb) {
|
||||
buffer.extend_from_slice(indices)
|
||||
}
|
||||
}
|
||||
OctreeNode::Branch { bounds, leaves } => {
|
||||
if bounds.is_intersects_aabb(aabb) {
|
||||
for leaf in leaves {
|
||||
self.aabb_recursive_query(*leaf, aabb, buffer)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn ray_query(&self, ray: &Ray, buffer: &mut Vec<u32>) {
|
||||
buffer.clear();
|
||||
self.ray_recursive_query(self.root, ray, buffer);
|
||||
}
|
||||
|
||||
fn ray_recursive_query(&self, node: usize, ray: &Ray, buffer: &mut Vec<u32>) {
|
||||
match &self.nodes[node] {
|
||||
OctreeNode::Leaf { indices, bounds } => {
|
||||
if ray.box_intersection(&bounds.min, &bounds.max).is_some() {
|
||||
buffer.extend_from_slice(indices)
|
||||
}
|
||||
}
|
||||
OctreeNode::Branch { bounds, leaves } => {
|
||||
if ray.box_intersection(&bounds.min, &bounds.max).is_some() {
|
||||
for leaf in leaves {
|
||||
self.ray_recursive_query(*leaf, ray, buffer)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn node(&self, handle: usize) -> &OctreeNode {
|
||||
&self.nodes[handle]
|
||||
}
|
||||
|
||||
pub fn nodes(&self) -> &Vec<OctreeNode> {
|
||||
&self.nodes
|
||||
}
|
||||
|
||||
pub fn ray_query_static<const CAP: usize>(&self, ray: &Ray, buffer: &mut ArrayVec<usize, CAP>) {
|
||||
buffer.clear();
|
||||
self.ray_recursive_query_static(self.root, ray, buffer);
|
||||
}
|
||||
|
||||
fn ray_recursive_query_static<const CAP: usize>(
|
||||
&self,
|
||||
node: usize,
|
||||
ray: &Ray,
|
||||
buffer: &mut ArrayVec<usize, CAP>,
|
||||
) {
|
||||
match &self.nodes[node] {
|
||||
OctreeNode::Leaf { bounds, .. } => {
|
||||
if ray.box_intersection(&bounds.min, &bounds.max).is_some() {
|
||||
buffer.push(node);
|
||||
}
|
||||
}
|
||||
OctreeNode::Branch { bounds, leaves } => {
|
||||
if ray.box_intersection(&bounds.min, &bounds.max).is_some() {
|
||||
for leaf in leaves {
|
||||
self.ray_recursive_query_static(*leaf, ray, buffer)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn point_query<C>(&self, point: Vector3<f32>, mut callback: C)
|
||||
where
|
||||
C: FnMut(&[u32]),
|
||||
{
|
||||
self.point_recursive_query(self.root, point, &mut callback);
|
||||
}
|
||||
|
||||
fn point_recursive_query<C>(&self, node: usize, point: Vector3<f32>, callback: &mut C)
|
||||
where
|
||||
C: FnMut(&[u32]),
|
||||
{
|
||||
match &self.nodes[node] {
|
||||
OctreeNode::Leaf { indices, bounds } => {
|
||||
if bounds.is_contains_point(point) {
|
||||
(callback)(indices)
|
||||
}
|
||||
}
|
||||
OctreeNode::Branch { bounds, leaves } => {
|
||||
if bounds.is_contains_point(point) {
|
||||
for leaf in leaves {
|
||||
self.point_recursive_query(*leaf, point, callback)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn build_recursive(
|
||||
nodes: &mut Vec<OctreeNode>,
|
||||
triangles: &[[Vector3<f32>; 3]],
|
||||
bounds: AxisAlignedBoundingBox,
|
||||
indices: Vec<u32>,
|
||||
split_threshold: usize,
|
||||
) -> usize {
|
||||
if indices.len() <= split_threshold {
|
||||
let index = nodes.len();
|
||||
nodes.push(OctreeNode::Leaf { bounds, indices });
|
||||
index
|
||||
} else {
|
||||
let mut leaves = [0; 8];
|
||||
let leaf_bounds = bounds.split();
|
||||
|
||||
for i in 0..8 {
|
||||
let mut leaf_indices = Vec::new();
|
||||
|
||||
for index in indices.iter() {
|
||||
let index = *index;
|
||||
|
||||
let triangle_bounds =
|
||||
AxisAlignedBoundingBox::from_points(&triangles[index as usize]);
|
||||
|
||||
if triangle_bounds.is_intersects_aabb(&bounds) {
|
||||
leaf_indices.push(index);
|
||||
}
|
||||
}
|
||||
|
||||
leaves[i] = build_recursive(
|
||||
nodes,
|
||||
triangles,
|
||||
leaf_bounds[i],
|
||||
leaf_indices,
|
||||
split_threshold,
|
||||
);
|
||||
}
|
||||
|
||||
let index = nodes.len();
|
||||
nodes.push(OctreeNode::Branch { leaves, bounds });
|
||||
index
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use super::*;
|
||||
|
||||
fn get_six_triangles() -> [[Vector3<f32>; 3]; 6] {
|
||||
[
|
||||
[
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::new(1.0, 1.0, 0.0),
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 2.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::new(1.0, 2.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 2.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::new(0.0, 2.0, 0.0),
|
||||
Vector3::new(1.0, 2.0, 0.0),
|
||||
Vector3::new(0.0, 3.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::new(1.0, 3.0, 0.0),
|
||||
Vector3::new(1.0, 2.0, 0.0),
|
||||
Vector3::new(0.0, 3.0, 0.0),
|
||||
],
|
||||
]
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_new() {
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
|
||||
assert_eq!(tree.root, 72);
|
||||
assert_eq!(tree.nodes().len(), 73);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_for_octree() {
|
||||
let tree = Octree::default();
|
||||
assert_eq!(tree.root, 0);
|
||||
assert_eq!(tree.nodes.len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_point_query() {
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
let mut buffer = Vec::new();
|
||||
tree.point_query(Vector3::new(0.0, 0.0, 0.0), |triangles| {
|
||||
buffer.extend_from_slice(triangles)
|
||||
});
|
||||
|
||||
assert_eq!(buffer, [0, 1, 2, 3, 0, 1, 2, 3]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_sphere_query() {
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
let mut buffer = Vec::new();
|
||||
tree.sphere_query(Vector3::new(0.0, 0.0, 0.0), 1.0, &mut buffer);
|
||||
|
||||
assert_eq!(
|
||||
buffer,
|
||||
[
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_aabb_query() {
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
let mut buffer = Vec::new();
|
||||
tree.aabb_query(
|
||||
&AxisAlignedBoundingBox {
|
||||
min: Vector3::new(0.0, 0.0, 0.0),
|
||||
max: Vector3::new(0.5, 0.5, 0.5),
|
||||
},
|
||||
&mut buffer,
|
||||
);
|
||||
|
||||
assert_eq!(
|
||||
buffer,
|
||||
[
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
|
||||
0, 1, 2, 3, 0, 1, 2, 3
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_ray_query() {
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
let mut buffer = Vec::new();
|
||||
tree.ray_query(
|
||||
&Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 0.0)),
|
||||
&mut buffer,
|
||||
);
|
||||
|
||||
assert_eq!(
|
||||
buffer,
|
||||
[
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
|
||||
0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn octree_ray_query_static() {
|
||||
const CAP: usize = 10;
|
||||
let tree = Octree::new(&get_six_triangles(), 5);
|
||||
let mut buffer = ArrayVec::<usize, CAP>::new();
|
||||
tree.ray_query_static::<CAP>(
|
||||
&Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 0.0)),
|
||||
&mut buffer,
|
||||
);
|
||||
|
||||
assert_eq!(buffer.as_slice(), [2, 3, 11, 15, 16, 18, 19, 27, 31, 32,]);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,206 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use nalgebra::Vector3;
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||
pub struct Plane {
|
||||
pub normal: Vector3<f32>,
|
||||
pub d: f32,
|
||||
}
|
||||
|
||||
impl Default for Plane {
|
||||
#[inline]
|
||||
fn default() -> Self {
|
||||
Plane {
|
||||
normal: Vector3::new(0.0, 1.0, 0.0),
|
||||
d: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Plane {
|
||||
/// Creates plane from a point and normal vector at that point.
|
||||
/// May fail if normal is degenerated vector.
|
||||
#[inline]
|
||||
pub fn from_normal_and_point(normal: &Vector3<f32>, point: &Vector3<f32>) -> Option<Self> {
|
||||
normal
|
||||
.try_normalize(f32::EPSILON)
|
||||
.map(|normalized_normal| Self {
|
||||
normal: normalized_normal,
|
||||
d: -point.dot(&normalized_normal),
|
||||
})
|
||||
}
|
||||
|
||||
/// Tries to create a plane from three points (triangle). May fail if the triangle is degenerated
|
||||
/// (collapsed into a point or a line).
|
||||
#[inline]
|
||||
pub fn from_triangle(a: &Vector3<f32>, b: &Vector3<f32>, c: &Vector3<f32>) -> Option<Self> {
|
||||
let normal = (b - a).cross(&(c - a));
|
||||
Self::from_normal_and_point(&normal, a)
|
||||
}
|
||||
|
||||
/// Creates plane using coefficients of plane equation Ax + By + Cz + D = 0
|
||||
/// May fail if length of normal vector is zero (normal is degenerated vector).
|
||||
#[inline]
|
||||
pub fn from_abcd(a: f32, b: f32, c: f32, d: f32) -> Option<Self> {
|
||||
let normal = Vector3::new(a, b, c);
|
||||
let len = normal.norm();
|
||||
if len == 0.0 {
|
||||
None
|
||||
} else {
|
||||
let coeff = 1.0 / len;
|
||||
Some(Self {
|
||||
normal: normal.scale(coeff),
|
||||
d: d * coeff,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn dot(&self, point: &Vector3<f32>) -> f32 {
|
||||
self.normal.dot(point) + self.d
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn distance(&self, point: &Vector3<f32>) -> f32 {
|
||||
self.dot(point).abs()
|
||||
}
|
||||
|
||||
/// Projects the given point onto the plane along the normal vector of the plane.
|
||||
#[inline]
|
||||
pub fn project(&self, point: &Vector3<f32>) -> Vector3<f32> {
|
||||
point - self.normal.scale(self.normal.dot(point) + self.d)
|
||||
}
|
||||
|
||||
/// <http://geomalgorithms.com/a05-_intersect-1.html>
|
||||
pub fn intersection_point(&self, b: &Plane, c: &Plane) -> Vector3<f32> {
|
||||
let f = -1.0 / self.normal.dot(&b.normal.cross(&c.normal));
|
||||
|
||||
let v1 = b.normal.cross(&c.normal).scale(self.d);
|
||||
let v2 = c.normal.cross(&self.normal).scale(b.d);
|
||||
let v3 = self.normal.cross(&b.normal).scale(c.d);
|
||||
|
||||
(v1 + v2 + v3).scale(f)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use crate::plane::Plane;
|
||||
use nalgebra::Vector3;
|
||||
|
||||
#[test]
|
||||
fn plane_sanity_tests() {
|
||||
// Computation test
|
||||
let plane = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 10.0, 0.0),
|
||||
&Vector3::new(0.0, 3.0, 0.0),
|
||||
);
|
||||
assert!(plane.is_some());
|
||||
let plane = plane.unwrap();
|
||||
assert_eq!(plane.normal.x, 0.0);
|
||||
assert_eq!(plane.normal.y, 1.0);
|
||||
assert_eq!(plane.normal.z, 0.0);
|
||||
assert_eq!(plane.d, -3.0);
|
||||
|
||||
// Degenerated normal case
|
||||
let plane = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
assert!(plane.is_none());
|
||||
|
||||
let plane = Plane::from_abcd(0.0, 0.0, 0.0, 0.0);
|
||||
assert!(plane.is_none())
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_default_for_plane() {
|
||||
assert_eq!(
|
||||
Plane::default(),
|
||||
Plane {
|
||||
normal: Vector3::new(0.0, 1.0, 0.0),
|
||||
d: 0.0,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_plane_from_abcd() {
|
||||
assert_eq!(Plane::from_abcd(0.0, 0.0, 0.0, 0.0), None);
|
||||
assert_eq!(
|
||||
Plane::from_abcd(1.0, 1.0, 1.0, 0.0),
|
||||
Some(Plane {
|
||||
normal: Vector3::new(0.57735026, 0.57735026, 0.57735026),
|
||||
d: 0.0
|
||||
})
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_plane_dot() {
|
||||
let plane = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 0.0, 1.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
assert!(plane.is_some());
|
||||
assert_eq!(plane.unwrap().dot(&Vector3::new(1.0, 1.0, 1.0)), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_plane_distance() {
|
||||
let plane = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 0.0, 1.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
assert!(plane.is_some());
|
||||
assert_eq!(plane.unwrap().distance(&Vector3::new(0.0, 0.0, 0.0)), 0.0);
|
||||
assert_eq!(plane.unwrap().distance(&Vector3::new(1.0, 0.0, 0.0)), 0.0);
|
||||
assert_eq!(plane.unwrap().distance(&Vector3::new(0.0, 1.0, 0.0)), 0.0);
|
||||
assert_eq!(plane.unwrap().distance(&Vector3::new(0.0, 0.0, 1.0)), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_plane_intersection_point() {
|
||||
let plane = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 0.0, 1.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
let plane2 = Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 1.0, 0.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
let plane3 = Plane::from_normal_and_point(
|
||||
&Vector3::new(1.0, 0.0, 0.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
);
|
||||
assert!(plane.is_some());
|
||||
assert!(plane2.is_some());
|
||||
assert!(plane3.is_some());
|
||||
|
||||
assert_eq!(
|
||||
plane
|
||||
.unwrap()
|
||||
.intersection_point(&plane2.unwrap(), &plane3.unwrap()),
|
||||
Vector3::new(0.0, 0.0, 0.0)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,882 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
// Clippy complains about normal mathematical symbols like A, B, C for quadratic equation.
|
||||
#![allow(clippy::many_single_char_names)]
|
||||
|
||||
use crate::aabb::AxisAlignedBoundingBox;
|
||||
use crate::{is_point_inside_triangle, plane::Plane, solve_quadratic};
|
||||
use nalgebra::{Matrix4, Point3, Vector3};
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||
pub struct Ray {
|
||||
pub origin: Vector3<f32>,
|
||||
pub dir: Vector3<f32>,
|
||||
}
|
||||
|
||||
impl Default for Ray {
|
||||
#[inline]
|
||||
fn default() -> Self {
|
||||
Ray {
|
||||
origin: Vector3::new(0.0, 0.0, 0.0),
|
||||
dir: Vector3::new(0.0, 0.0, 1.0),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Pair of ray equation parameters.
|
||||
#[derive(Clone, Debug, Copy)]
|
||||
pub struct IntersectionResult {
|
||||
pub min: f32,
|
||||
pub max: f32,
|
||||
}
|
||||
|
||||
impl IntersectionResult {
|
||||
#[inline]
|
||||
pub fn from_slice(roots: &[f32]) -> Self {
|
||||
let mut min = f32::MAX;
|
||||
let mut max = -f32::MAX;
|
||||
for n in roots {
|
||||
min = min.min(*n);
|
||||
max = max.max(*n);
|
||||
}
|
||||
Self { min, max }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn from_set(results: &[Option<IntersectionResult>]) -> Option<Self> {
|
||||
let mut result = None;
|
||||
for v in results {
|
||||
match result {
|
||||
None => result = *v,
|
||||
Some(ref mut result) => {
|
||||
if let Some(v) = v {
|
||||
result.merge(v.min);
|
||||
result.merge(v.max);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
result
|
||||
}
|
||||
|
||||
/// Updates min and max ray equation parameters according to a new parameter -
|
||||
/// expands range if `param` was outside of that range.
|
||||
#[inline]
|
||||
pub fn merge(&mut self, param: f32) {
|
||||
if param < self.min {
|
||||
self.min = param;
|
||||
}
|
||||
if param > self.max {
|
||||
self.max = param;
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn merge_slice(&mut self, params: &[f32]) {
|
||||
for param in params {
|
||||
self.merge(*param)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub enum CylinderKind {
|
||||
Infinite,
|
||||
Finite,
|
||||
Capped,
|
||||
}
|
||||
|
||||
impl Ray {
|
||||
/// Creates ray from two points. May fail if begin == end.
|
||||
#[inline]
|
||||
pub fn from_two_points(begin: Vector3<f32>, end: Vector3<f32>) -> Self {
|
||||
Ray {
|
||||
origin: begin,
|
||||
dir: end - begin,
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn new(origin: Vector3<f32>, dir: Vector3<f32>) -> Self {
|
||||
Self { origin, dir }
|
||||
}
|
||||
|
||||
/// Checks intersection with sphere. Returns two intersection points or none
|
||||
/// if there was no intersection.
|
||||
#[inline]
|
||||
pub fn sphere_intersection_points(
|
||||
&self,
|
||||
position: &Vector3<f32>,
|
||||
radius: f32,
|
||||
) -> Option<[Vector3<f32>; 2]> {
|
||||
self.try_eval_points(self.sphere_intersection(position, radius))
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn sphere_intersection(
|
||||
&self,
|
||||
position: &Vector3<f32>,
|
||||
radius: f32,
|
||||
) -> Option<IntersectionResult> {
|
||||
let d = self.origin - *position;
|
||||
let a = self.dir.dot(&self.dir);
|
||||
let b = 2.0 * self.dir.dot(&d);
|
||||
let c = d.dot(&d) - radius * radius;
|
||||
solve_quadratic(a, b, c).map(|roots| IntersectionResult::from_slice(&roots))
|
||||
}
|
||||
|
||||
/// Checks intersection with sphere.
|
||||
#[inline]
|
||||
pub fn is_intersect_sphere(&self, position: &Vector3<f32>, radius: f32) -> bool {
|
||||
let d = self.origin - position;
|
||||
let a = self.dir.dot(&self.dir);
|
||||
let b = 2.0 * self.dir.dot(&d);
|
||||
let c = d.dot(&d) - radius * radius;
|
||||
let discriminant = b * b - 4.0 * a * c;
|
||||
discriminant >= 0.0
|
||||
}
|
||||
|
||||
/// Returns t factor (at pt=o+d*t equation) for projection of given point at ray
|
||||
#[inline]
|
||||
pub fn project_point(&self, point: &Vector3<f32>) -> f32 {
|
||||
(point - self.origin).dot(&self.dir) / self.dir.norm_squared()
|
||||
}
|
||||
|
||||
/// Returns point on ray which defined by pt=o+d*t equation.
|
||||
#[inline]
|
||||
pub fn get_point(&self, t: f32) -> Vector3<f32> {
|
||||
self.origin + self.dir.scale(t)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn box_intersection(
|
||||
&self,
|
||||
min: &Vector3<f32>,
|
||||
max: &Vector3<f32>,
|
||||
) -> Option<IntersectionResult> {
|
||||
let (mut tmin, mut tmax) = if self.dir.x >= 0.0 {
|
||||
(
|
||||
(min.x - self.origin.x) / self.dir.x,
|
||||
(max.x - self.origin.x) / self.dir.x,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
(max.x - self.origin.x) / self.dir.x,
|
||||
(min.x - self.origin.x) / self.dir.x,
|
||||
)
|
||||
};
|
||||
|
||||
let (tymin, tymax) = if self.dir.y >= 0.0 {
|
||||
(
|
||||
(min.y - self.origin.y) / self.dir.y,
|
||||
(max.y - self.origin.y) / self.dir.y,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
(max.y - self.origin.y) / self.dir.y,
|
||||
(min.y - self.origin.y) / self.dir.y,
|
||||
)
|
||||
};
|
||||
|
||||
if tmin > tymax || (tymin > tmax) {
|
||||
return None;
|
||||
}
|
||||
if tymin > tmin {
|
||||
tmin = tymin;
|
||||
}
|
||||
if tymax < tmax {
|
||||
tmax = tymax;
|
||||
}
|
||||
let (tzmin, tzmax) = if self.dir.z >= 0.0 {
|
||||
(
|
||||
(min.z - self.origin.z) / self.dir.z,
|
||||
(max.z - self.origin.z) / self.dir.z,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
(max.z - self.origin.z) / self.dir.z,
|
||||
(min.z - self.origin.z) / self.dir.z,
|
||||
)
|
||||
};
|
||||
|
||||
if (tmin > tzmax) || (tzmin > tmax) {
|
||||
return None;
|
||||
}
|
||||
if tzmin > tmin {
|
||||
tmin = tzmin;
|
||||
}
|
||||
if tzmax < tmax {
|
||||
tmax = tzmax;
|
||||
}
|
||||
if tmin <= 1.0 && tmax >= 0.0 {
|
||||
Some(IntersectionResult {
|
||||
min: tmin,
|
||||
max: tmax,
|
||||
})
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn box_intersection_points(
|
||||
&self,
|
||||
min: &Vector3<f32>,
|
||||
max: &Vector3<f32>,
|
||||
) -> Option<[Vector3<f32>; 2]> {
|
||||
self.try_eval_points(self.box_intersection(min, max))
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn aabb_intersection(&self, aabb: &AxisAlignedBoundingBox) -> Option<IntersectionResult> {
|
||||
self.box_intersection(&aabb.min, &aabb.max)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn aabb_intersection_points(
|
||||
&self,
|
||||
aabb: &AxisAlignedBoundingBox,
|
||||
) -> Option<[Vector3<f32>; 2]> {
|
||||
self.box_intersection_points(&aabb.min, &aabb.max)
|
||||
}
|
||||
|
||||
/// Solves plane equation in order to find ray equation parameter.
|
||||
/// There is no intersection if result < 0.
|
||||
#[inline]
|
||||
pub fn plane_intersection(&self, plane: &Plane) -> f32 {
|
||||
let u = -(self.origin.dot(&plane.normal) + plane.d);
|
||||
let v = self.dir.dot(&plane.normal);
|
||||
u / v
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn plane_intersection_point(&self, plane: &Plane) -> Option<Vector3<f32>> {
|
||||
let t = self.plane_intersection(plane);
|
||||
if !(0.0..=1.0).contains(&t) {
|
||||
None
|
||||
} else {
|
||||
Some(self.get_point(t))
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn triangle_intersection(
|
||||
&self,
|
||||
vertices: &[Vector3<f32>; 3],
|
||||
) -> Option<(f32, Vector3<f32>)> {
|
||||
let ba = vertices[1] - vertices[0];
|
||||
let ca = vertices[2] - vertices[0];
|
||||
let plane = Plane::from_normal_and_point(&ba.cross(&ca), &vertices[0])?;
|
||||
|
||||
let t = self.plane_intersection(&plane);
|
||||
if (0.0..=1.0).contains(&t) {
|
||||
let point = self.get_point(t);
|
||||
if is_point_inside_triangle(&point, vertices) {
|
||||
return Some((t, point));
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn triangle_intersection_point(
|
||||
&self,
|
||||
vertices: &[Vector3<f32>; 3],
|
||||
) -> Option<Vector3<f32>> {
|
||||
let ba = vertices[1] - vertices[0];
|
||||
let ca = vertices[2] - vertices[0];
|
||||
let plane = Plane::from_normal_and_point(&ba.cross(&ca), &vertices[0])?;
|
||||
|
||||
if let Some(point) = self.plane_intersection_point(&plane) {
|
||||
if is_point_inside_triangle(&point, vertices) {
|
||||
return Some(point);
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
|
||||
/// Generic ray-cylinder intersection test.
|
||||
///
|
||||
/// <https://mrl.nyu.edu/~dzorin/rend05/lecture2.pdf>
|
||||
///
|
||||
/// Infinite cylinder oriented along line pa + va * t:
|
||||
/// sqr_len(q - pa - dot(va, q - pa) * va) - r ^ 2 = 0
|
||||
/// where q - point on cylinder, substitute q with ray p + v * t:
|
||||
/// sqr_len(p - pa + vt - dot(va, p - pa + vt) * va) - r ^ 2 = 0
|
||||
/// reduce to A * t * t + B * t + C = 0 (quadratic equation), where:
|
||||
/// A = sqr_len(v - dot(v, va) * va)
|
||||
/// B = 2 * dot(v - dot(v, va) * va, dp - dot(dp, va) * va)
|
||||
/// C = sqr_len(dp - dot(dp, va) * va) - r ^ 2
|
||||
/// where dp = p - pa
|
||||
/// to find intersection points we have to solve quadratic equation
|
||||
/// to get root which will be t parameter of ray equation.
|
||||
#[inline]
|
||||
pub fn cylinder_intersection(
|
||||
&self,
|
||||
pa: &Vector3<f32>,
|
||||
pb: &Vector3<f32>,
|
||||
r: f32,
|
||||
kind: CylinderKind,
|
||||
) -> Option<IntersectionResult> {
|
||||
let va = (*pb - *pa)
|
||||
.try_normalize(f32::EPSILON)
|
||||
.unwrap_or_else(|| Vector3::new(0.0, 1.0, 0.0));
|
||||
let vl = self.dir - va.scale(self.dir.dot(&va));
|
||||
let dp = self.origin - *pa;
|
||||
let dpva = dp - va.scale(dp.dot(&va));
|
||||
|
||||
let a = vl.norm_squared();
|
||||
let b = 2.0 * vl.dot(&dpva);
|
||||
let c = dpva.norm_squared() - r * r;
|
||||
|
||||
// Get roots for cylinder surfaces
|
||||
if let Some(cylinder_roots) = solve_quadratic(a, b, c) {
|
||||
match kind {
|
||||
CylinderKind::Infinite => Some(IntersectionResult::from_slice(&cylinder_roots)),
|
||||
CylinderKind::Capped => {
|
||||
let mut result = IntersectionResult::from_slice(&cylinder_roots);
|
||||
// In case of cylinder with caps we have to check intersection with caps
|
||||
for (cap_center, cap_normal) in [(pa, -va), (pb, va)].iter() {
|
||||
let cap_plane =
|
||||
Plane::from_normal_and_point(cap_normal, cap_center).unwrap();
|
||||
let t = self.plane_intersection(&cap_plane);
|
||||
if t > 0.0 {
|
||||
let intersection = self.get_point(t);
|
||||
if (*cap_center - intersection).norm_squared() <= r * r {
|
||||
// Point inside cap bounds
|
||||
result.merge(t);
|
||||
}
|
||||
}
|
||||
}
|
||||
result.merge_slice(&cylinder_roots);
|
||||
Some(result)
|
||||
}
|
||||
CylinderKind::Finite => {
|
||||
// In case of finite cylinder without caps we have to check that intersection
|
||||
// points on cylinder surface are between two planes of caps.
|
||||
let mut result = None;
|
||||
for root in cylinder_roots.iter() {
|
||||
let int_point = self.get_point(*root);
|
||||
if (int_point - *pa).dot(&va) >= 0.0 && (*pb - int_point).dot(&va) >= 0.0 {
|
||||
match &mut result {
|
||||
None => {
|
||||
result = Some(IntersectionResult {
|
||||
min: *root,
|
||||
max: *root,
|
||||
})
|
||||
}
|
||||
Some(result) => result.merge(*root),
|
||||
}
|
||||
}
|
||||
}
|
||||
result
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// We have no roots, so no intersection.
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn try_eval_points(&self, result: Option<IntersectionResult>) -> Option<[Vector3<f32>; 2]> {
|
||||
match result {
|
||||
None => None,
|
||||
Some(result) => {
|
||||
let a = if result.min >= 0.0 && result.min <= 1.0 {
|
||||
Some(self.get_point(result.min))
|
||||
} else {
|
||||
None
|
||||
};
|
||||
|
||||
let b = if result.max >= 0.0 && result.max <= 1.0 {
|
||||
Some(self.get_point(result.max))
|
||||
} else {
|
||||
None
|
||||
};
|
||||
|
||||
match a {
|
||||
None => b.map(|b| [b, b]),
|
||||
Some(a) => match b {
|
||||
None => Some([a, a]),
|
||||
Some(b) => Some([a, b]),
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn capsule_intersection(
|
||||
&self,
|
||||
pa: &Vector3<f32>,
|
||||
pb: &Vector3<f32>,
|
||||
radius: f32,
|
||||
) -> Option<[Vector3<f32>; 2]> {
|
||||
// Dumb approach - check intersection with finite cylinder without caps,
|
||||
// then check two sphere caps.
|
||||
let cylinder = self.cylinder_intersection(pa, pb, radius, CylinderKind::Finite);
|
||||
let cap_a = self.sphere_intersection(pa, radius);
|
||||
let cap_b = self.sphere_intersection(pb, radius);
|
||||
self.try_eval_points(IntersectionResult::from_set(&[cylinder, cap_a, cap_b]))
|
||||
}
|
||||
|
||||
/// Transforms ray using given matrix. This method is useful when you need to
|
||||
/// transform ray into some object space to simplify calculations. For example
|
||||
/// you may have mesh with lots of triangles, and in one way you would take all
|
||||
/// vertices, transform them into world space by some matrix, then do intersection
|
||||
/// test in world space. This works, but too inefficient, much more faster would
|
||||
/// be to put ray into object space and do intersection test in object space. This
|
||||
/// removes vertex*matrix multiplication and significantly improves performance.
|
||||
#[must_use = "Method does not modify ray, instead it returns transformed copy"]
|
||||
#[inline]
|
||||
pub fn transform(&self, mat: Matrix4<f32>) -> Self {
|
||||
Self {
|
||||
origin: mat.transform_point(&Point3::from(self.origin)).coords,
|
||||
dir: mat.transform_vector(&self.dir),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use nalgebra::Matrix4;
|
||||
|
||||
use crate::{
|
||||
aabb::AxisAlignedBoundingBox,
|
||||
plane::Plane,
|
||||
ray::{CylinderKind, Ray},
|
||||
Vector3,
|
||||
};
|
||||
|
||||
use super::IntersectionResult;
|
||||
|
||||
#[test]
|
||||
fn intersection() {
|
||||
let triangle = [
|
||||
Vector3::new(0.0, 0.5, 0.0),
|
||||
Vector3::new(-0.5, -0.5, 0.0),
|
||||
Vector3::new(0.5, -0.5, 0.0),
|
||||
];
|
||||
let ray = Ray::from_two_points(Vector3::new(0.0, 0.0, -2.0), Vector3::new(0.0, 0.0, -1.0));
|
||||
assert!(ray.triangle_intersection_point(&triangle).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_for_ray() {
|
||||
let ray = Ray::default();
|
||||
assert_eq!(ray.origin, Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(ray.dir, Vector3::new(0.0, 0.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersection_result_from_slice() {
|
||||
let ir = IntersectionResult::from_slice(&[0.0, -1.0, 1.0]);
|
||||
assert_eq!(ir.min, -1.0);
|
||||
assert_eq!(ir.max, 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersection_result_from_set() {
|
||||
assert!(IntersectionResult::from_set(&[None, None]).is_none());
|
||||
|
||||
let ir = IntersectionResult::from_set(&[
|
||||
Some(IntersectionResult {
|
||||
min: -1.0,
|
||||
max: 0.0,
|
||||
}),
|
||||
Some(IntersectionResult { min: 0.0, max: 1.0 }),
|
||||
]);
|
||||
assert!(ir.is_some());
|
||||
assert_eq!(ir.unwrap().min, -1.0);
|
||||
assert_eq!(ir.unwrap().max, 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersection_result_merge() {
|
||||
let mut ir = IntersectionResult {
|
||||
min: -1.0,
|
||||
max: 1.0,
|
||||
};
|
||||
ir.merge(-10.0);
|
||||
ir.merge(10.0);
|
||||
|
||||
assert_eq!(ir.min, -10.0);
|
||||
assert_eq!(ir.max, 10.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersection_result_merge_slice() {
|
||||
let mut ir = IntersectionResult {
|
||||
min: -1.0,
|
||||
max: 1.0,
|
||||
};
|
||||
ir.merge_slice(&[-10.0, 0.0, 10.0]);
|
||||
|
||||
assert_eq!(ir.min, -10.0);
|
||||
assert_eq!(ir.max, 10.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_new() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(ray.origin, Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(ray.dir, Vector3::new(1.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_try_eval_points() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert!(ray.try_eval_points(None).is_none());
|
||||
|
||||
let ir = IntersectionResult { min: 0.0, max: 1.0 };
|
||||
assert_eq!(
|
||||
ray.try_eval_points(Some(ir)),
|
||||
Some([Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0)])
|
||||
);
|
||||
|
||||
let ir = IntersectionResult {
|
||||
min: -1.0,
|
||||
max: 1.0,
|
||||
};
|
||||
assert_eq!(
|
||||
ray.try_eval_points(Some(ir)),
|
||||
Some([Vector3::new(1.0, 1.0, 1.0), Vector3::new(1.0, 1.0, 1.0)])
|
||||
);
|
||||
|
||||
let ir = IntersectionResult {
|
||||
min: 0.0,
|
||||
max: 10.0,
|
||||
};
|
||||
assert_eq!(
|
||||
ray.try_eval_points(Some(ir)),
|
||||
Some([Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0)])
|
||||
);
|
||||
|
||||
let ir = IntersectionResult {
|
||||
min: -10.0,
|
||||
max: 10.0,
|
||||
};
|
||||
assert_eq!(ray.try_eval_points(Some(ir)), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_sphere_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0));
|
||||
|
||||
assert!(ray
|
||||
.sphere_intersection(&Vector3::new(-10.0, -10.0, -10.0), 1.0)
|
||||
.is_none());
|
||||
|
||||
let result = ray.sphere_intersection(&Vector3::new(0.0, 0.0, 0.0), 1.0);
|
||||
assert_eq!(result.unwrap().min, -1.0);
|
||||
assert_eq!(result.unwrap().max, 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_sphere_intersection_points() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0));
|
||||
|
||||
assert!(ray
|
||||
.sphere_intersection_points(&Vector3::new(-10.0, -10.0, -10.0), 1.0)
|
||||
.is_none());
|
||||
|
||||
assert_eq!(
|
||||
ray.sphere_intersection_points(&Vector3::new(0.0, 0.0, 0.0), 1.0),
|
||||
Some([Vector3::new(1.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0)])
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_is_intersect_sphere() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0));
|
||||
|
||||
assert!(!ray.is_intersect_sphere(&Vector3::new(-10.0, -10.0, -10.0), 1.0));
|
||||
assert!(ray.is_intersect_sphere(&Vector3::new(0.0, 0.0, 0.0), 1.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_project_point() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(ray.project_point(&Vector3::new(0.0, 0.0, 0.0)), 0.0);
|
||||
assert_eq!(ray.project_point(&Vector3::new(1.0, 0.0, 0.0)), 0.33333334);
|
||||
assert_eq!(ray.project_point(&Vector3::new(0.0, 1.0, 0.0)), 0.33333334);
|
||||
assert_eq!(ray.project_point(&Vector3::new(0.0, 0.0, 1.0)), 0.33333334);
|
||||
assert_eq!(ray.project_point(&Vector3::new(1.0, 1.0, 1.0)), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_get_point() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(ray.get_point(0.0), Vector3::new(0.0, 0.0, 0.0));
|
||||
assert_eq!(ray.get_point(10.0), Vector3::new(10.0, 10.0, 10.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_box_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
let ir = ray.box_intersection(
|
||||
&Vector3::new(1.0, 1.0, 1.0),
|
||||
&Vector3::new(10.0, 10.0, 10.0),
|
||||
);
|
||||
assert_eq!(ir.unwrap().min, 1.0);
|
||||
assert_eq!(ir.unwrap().max, 10.0);
|
||||
|
||||
assert!(ray
|
||||
.box_intersection(&Vector3::new(1.0, 1.0, 0.0), &Vector3::new(10.0, 10.0, 0.0))
|
||||
.is_none());
|
||||
assert!(ray
|
||||
.box_intersection(&Vector3::new(1.0, 0.0, 1.0), &Vector3::new(10.0, 0.0, 10.0))
|
||||
.is_none());
|
||||
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(-1.0, -1.0, -1.0));
|
||||
let ir = ray.box_intersection(
|
||||
&Vector3::new(-10.0, -10.0, -10.0),
|
||||
&Vector3::new(-1.0, -1.0, -1.0),
|
||||
);
|
||||
assert_eq!(ir.unwrap().min, 1.0);
|
||||
assert_eq!(ir.unwrap().max, 10.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_box_intersection_points() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert!(ray
|
||||
.box_intersection_points(&Vector3::new(1.0, 1.0, 0.0), &Vector3::new(10.0, 10.0, 0.0))
|
||||
.is_none());
|
||||
assert_eq!(
|
||||
ray.box_intersection_points(
|
||||
&Vector3::new(1.0, 1.0, 1.0),
|
||||
&Vector3::new(10.0, 10.0, 10.0)
|
||||
),
|
||||
Some([Vector3::new(1.0, 1.0, 1.0), Vector3::new(1.0, 1.0, 1.0)])
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_aabb_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert!(ray
|
||||
.aabb_intersection(&AxisAlignedBoundingBox {
|
||||
min: Vector3::new(1.0, 1.0, 0.0),
|
||||
max: Vector3::new(10.0, 10.0, 0.0)
|
||||
})
|
||||
.is_none());
|
||||
|
||||
let ir = ray.aabb_intersection(&AxisAlignedBoundingBox {
|
||||
min: Vector3::new(1.0, 1.0, 1.0),
|
||||
max: Vector3::new(10.0, 10.0, 10.0),
|
||||
});
|
||||
assert_eq!(ir.unwrap().min, 1.0);
|
||||
assert_eq!(ir.unwrap().max, 10.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_aabb_intersection_points() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert!(ray
|
||||
.aabb_intersection_points(&AxisAlignedBoundingBox {
|
||||
min: Vector3::new(1.0, 1.0, 0.0),
|
||||
max: Vector3::new(10.0, 10.0, 0.0)
|
||||
})
|
||||
.is_none());
|
||||
|
||||
assert_eq!(
|
||||
ray.aabb_intersection_points(&AxisAlignedBoundingBox {
|
||||
min: Vector3::new(1.0, 1.0, 1.0),
|
||||
max: Vector3::new(10.0, 10.0, 10.0),
|
||||
}),
|
||||
Some([Vector3::new(1.0, 1.0, 1.0), Vector3::new(1.0, 1.0, 1.0)])
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_plane_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.plane_intersection(
|
||||
&Plane::from_normal_and_point(
|
||||
&Vector3::new(1.0, 1.0, 1.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0)
|
||||
)
|
||||
.unwrap()
|
||||
),
|
||||
0.0
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_plane_intersection_point() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.plane_intersection_point(
|
||||
&Plane::from_normal_and_point(
|
||||
&Vector3::new(1.0, 1.0, 1.0),
|
||||
&Vector3::new(0.0, 0.0, 0.0)
|
||||
)
|
||||
.unwrap()
|
||||
),
|
||||
Some(Vector3::new(0.0, 0.0, 0.0))
|
||||
);
|
||||
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 0.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.plane_intersection_point(
|
||||
&Plane::from_normal_and_point(
|
||||
&Vector3::new(0.0, 0.0, 1.0),
|
||||
&Vector3::new(1.0, 1.0, 1.0),
|
||||
)
|
||||
.unwrap()
|
||||
),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_triangle_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.triangle_intersection(&[
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 0.0),
|
||||
]),
|
||||
Some((0.0, Vector3::new(0.0, 0.0, 0.0)))
|
||||
);
|
||||
|
||||
assert_eq!(
|
||||
ray.triangle_intersection(&[
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, -1.0, 0.0),
|
||||
Vector3::new(-1.0, -1.0, 0.0),
|
||||
]),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_triangle_intersection_point() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.triangle_intersection_point(&[
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 1.0, 0.0),
|
||||
]),
|
||||
Some(Vector3::new(0.0, 0.0, 0.0))
|
||||
);
|
||||
|
||||
assert_eq!(
|
||||
ray.triangle_intersection_point(&[
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, -1.0, 0.0),
|
||||
Vector3::new(-1.0, -1.0, 0.0),
|
||||
]),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_cylinder_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
// Infinite
|
||||
let ir = ray.cylinder_intersection(
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
&Vector3::new(1.0, 0.0, 0.0),
|
||||
1.0,
|
||||
CylinderKind::Infinite,
|
||||
);
|
||||
assert_eq!(ir.unwrap().min, -0.70710677);
|
||||
assert_eq!(ir.unwrap().max, 0.70710677);
|
||||
|
||||
// Finite
|
||||
let ir = ray.cylinder_intersection(
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
&Vector3::new(1.0, 0.0, 0.0),
|
||||
1.0,
|
||||
CylinderKind::Finite,
|
||||
);
|
||||
assert_eq!(ir.unwrap().min, 0.70710677);
|
||||
assert_eq!(ir.unwrap().max, 0.70710677);
|
||||
|
||||
// Capped
|
||||
let ir = ray.cylinder_intersection(
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
&Vector3::new(1.0, 0.0, 0.0),
|
||||
1.0,
|
||||
CylinderKind::Capped,
|
||||
);
|
||||
assert_eq!(ir.unwrap().min, -0.70710677);
|
||||
assert_eq!(ir.unwrap().max, 0.70710677);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_capsule_intersection() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
assert_eq!(
|
||||
ray.capsule_intersection(
|
||||
&Vector3::new(0.0, 0.0, 0.0),
|
||||
&Vector3::new(1.0, 0.0, 0.0),
|
||||
1.0,
|
||||
),
|
||||
Some([
|
||||
Vector3::new(0.70710677, 0.70710677, 0.70710677),
|
||||
Vector3::new(0.70710677, 0.70710677, 0.70710677)
|
||||
])
|
||||
);
|
||||
assert_eq!(
|
||||
ray.capsule_intersection(
|
||||
&Vector3::new(10.0, 0.0, 0.0),
|
||||
&Vector3::new(11.0, 0.0, 0.0),
|
||||
1.0,
|
||||
),
|
||||
None
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ray_transform() {
|
||||
let ray = Ray::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 1.0));
|
||||
|
||||
let new_ray = ray.transform(Matrix4::new(
|
||||
1.0, 0.0, 0.0, 0.0, //
|
||||
0.0, 1.0, 0.0, 0.0, //
|
||||
0.0, 0.0, 1.0, 0.0, //
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
));
|
||||
|
||||
assert_eq!(ray.origin, new_ray.origin);
|
||||
assert_eq!(ray.dir, new_ray.dir);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,263 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use nalgebra::{RealField, SVector, Scalar, Vector2};
|
||||
use num_traits::{One, Signed, Zero};
|
||||
use rectutils::{Number, Rect};
|
||||
|
||||
/// Line segment in two dimensions
|
||||
pub type LineSegment2<T> = LineSegment<T, 2>;
|
||||
/// Line segment in three dimensions
|
||||
pub type LineSegment3<T> = LineSegment<T, 3>;
|
||||
|
||||
/// Line segment in any number of dimensions
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct LineSegment<T, const D: usize> {
|
||||
/// One end of the line segment, the point returned when interpolating at t = 0.0
|
||||
pub start: SVector<T, D>,
|
||||
/// One end of the line segment, the point returned when interpolating at t = 1.0
|
||||
pub end: SVector<T, D>,
|
||||
}
|
||||
|
||||
impl<T, const D: usize> LineSegment<T, D>
|
||||
where
|
||||
T: Zero + One + Scalar + RealField,
|
||||
{
|
||||
/// Create a new line segment with the given points.
|
||||
pub fn new(start: &SVector<T, D>, end: &SVector<T, D>) -> Self {
|
||||
Self {
|
||||
start: start.clone_owned(),
|
||||
end: end.clone_owned(),
|
||||
}
|
||||
}
|
||||
/// Creates a reversed line segment by swapping `start` and `end`.
|
||||
pub fn swapped(&self) -> Self {
|
||||
Self::new(&self.end, &self.start)
|
||||
}
|
||||
/// The two end-points of the line segment are equal.
|
||||
pub fn is_degenerate(&self) -> bool {
|
||||
self.start == self.end
|
||||
}
|
||||
/// Create a point somewhere between `start` and `end`.
|
||||
/// When t = 0.0, `start` is returned.
|
||||
/// When t = 1.0, `end` is returned.
|
||||
/// The result is `(1.0 - t) * start + t * end`, which may produce points off the line segment,
|
||||
/// if t < 0.0 or t > 1.0.
|
||||
pub fn interpolate(&self, t: T) -> SVector<T, D> {
|
||||
self.start.lerp(&self.end, t)
|
||||
}
|
||||
/// Create a point somewhere between `start` and `end`.
|
||||
/// This is just like [LineSegment::interpolate] except that t is clamped to between 0.0 and 1.0,
|
||||
/// so points off the line segment can never be returned.
|
||||
pub fn interpolate_clamped(&self, t: T) -> SVector<T, D> {
|
||||
self.interpolate(t.clamp(<T as Zero>::zero(), <T as One>::one()))
|
||||
}
|
||||
/// The vector from `start` to `end`
|
||||
pub fn vector(&self) -> SVector<T, D> {
|
||||
self.end.clone() - self.start.clone()
|
||||
}
|
||||
/// The distance between `start` and `end`
|
||||
pub fn length(&self) -> T {
|
||||
self.vector().norm()
|
||||
}
|
||||
/// The square of the distance between `start` and `end`
|
||||
pub fn length_squared(&self) -> T {
|
||||
self.vector().norm_squared()
|
||||
}
|
||||
/// The interpolation parameter of the point on this segment that is closest to the given point.
|
||||
///
|
||||
/// [Stack Exchange question: Find a point on a line segment which is the closest to other point not on the line segment](https://math.stackexchange.com/questions/2193720/find-a-point-on-a-line-segment-which-is-the-closest-to-other-point-not-on-the-li)
|
||||
pub fn nearest_t(&self, point: &SVector<T, D>) -> T {
|
||||
let v = self.vector();
|
||||
let u = self.start.clone() - point;
|
||||
let n2 = v.norm_squared();
|
||||
if n2.is_zero() {
|
||||
return T::zero();
|
||||
}
|
||||
-v.dot(&u) / n2
|
||||
}
|
||||
/// The point on this segment that is closest to the given point.
|
||||
pub fn nearest_point(&self, point: &SVector<T, D>) -> SVector<T, D> {
|
||||
self.interpolate_clamped(self.nearest_t(point))
|
||||
}
|
||||
/// The squared distance between the given point and the nearest point on this line segment.
|
||||
pub fn distance_squared(&self, point: &SVector<T, D>) -> T {
|
||||
(point - self.nearest_point(point)).norm_squared()
|
||||
}
|
||||
/// The distance between the given point and the nearest point on this line segment.
|
||||
pub fn distance(&self, point: &SVector<T, D>) -> T {
|
||||
(point - self.nearest_point(point)).norm()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T> LineSegment2<T>
|
||||
where
|
||||
T: Zero + One + Scalar + RealField,
|
||||
{
|
||||
/// AABB for a 2D line segment
|
||||
pub fn bounds(&self) -> Rect<T>
|
||||
where
|
||||
T: Number,
|
||||
{
|
||||
Rect::from_points(self.start, self.end)
|
||||
}
|
||||
/// Test whether a point is collinear with this segment.
|
||||
/// * 0.0 means collinear. Near to 0.0 means near to collinear.
|
||||
/// * Negative means that the point is to the counter-clockwise of `end` as viewed from `start`.
|
||||
/// * Positive means that the point is to the clockwise of `end` as viewed from `start`.
|
||||
pub fn collinearity(&self, point: &Vector2<T>) -> T {
|
||||
let v = self.vector();
|
||||
let u = self.start.clone() - point;
|
||||
v.x.clone() * u.y.clone() - u.x.clone() * v.y.clone()
|
||||
}
|
||||
/// True if this segment intersects the given segment based on collinearity.
|
||||
pub fn intersects(&self, other: &LineSegment2<T>) -> bool {
|
||||
fn pos<T>(t: &T) -> bool
|
||||
where
|
||||
T: Zero + Signed,
|
||||
{
|
||||
t.is_positive() && !t.is_zero()
|
||||
}
|
||||
fn neg<T>(t: &T) -> bool
|
||||
where
|
||||
T: Zero + Signed,
|
||||
{
|
||||
t.is_negative() && !t.is_zero()
|
||||
}
|
||||
let o1 = self.collinearity(&other.start);
|
||||
let o2 = self.collinearity(&other.end);
|
||||
let s1 = other.collinearity(&self.start);
|
||||
let s2 = other.collinearity(&self.end);
|
||||
// If both points of self are left of `other` or both points are right of `other`...
|
||||
if neg(&s1) && neg(&s2) || pos(&s1) && pos(&s2) {
|
||||
return false;
|
||||
}
|
||||
// If both points of `other` are left of self or both points are right of self...
|
||||
if neg(&o1) && neg(&o2) || pos(&o1) && pos(&o2) {
|
||||
return false;
|
||||
}
|
||||
true
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use super::*;
|
||||
use nalgebra::Vector2;
|
||||
#[test]
|
||||
fn nearest_at_start() {
|
||||
let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
|
||||
assert_eq!(segment.nearest_t(&Vector2::new(-1.0, -1.0)).max(0.0), 0.0);
|
||||
assert_eq!(
|
||||
segment.nearest_point(&Vector2::new(-1.0, -1.0)),
|
||||
Vector2::new(0.0, 0.0)
|
||||
);
|
||||
assert_eq!(segment.distance_squared(&Vector2::new(-1.0, -1.0)), 2.0);
|
||||
assert_eq!(segment.distance(&Vector2::new(-1.0, 0.0)), 1.0);
|
||||
}
|
||||
#[test]
|
||||
fn nearest_at_end() {
|
||||
let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
|
||||
assert_eq!(segment.nearest_t(&Vector2::new(2.0, 2.0)).min(1.0), 1.0);
|
||||
assert_eq!(
|
||||
segment.nearest_point(&Vector2::new(2.0, 2.0)),
|
||||
Vector2::new(1.0, 2.0)
|
||||
);
|
||||
assert_eq!(segment.distance_squared(&Vector2::new(3.0, 2.0)), 4.0);
|
||||
assert_eq!(segment.distance(&Vector2::new(3.0, 2.0)), 2.0);
|
||||
}
|
||||
#[test]
|
||||
fn nearest_in_middle() {
|
||||
let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
|
||||
assert_eq!(segment.nearest_t(&Vector2::new(2.5, 0.0)), 0.5);
|
||||
assert_eq!(
|
||||
segment.nearest_point(&Vector2::new(2.5, 0.0)),
|
||||
Vector2::new(0.5, 1.0)
|
||||
);
|
||||
assert_eq!(segment.distance_squared(&Vector2::new(2.5, 0.0)), 5.0);
|
||||
}
|
||||
#[test]
|
||||
fn length() {
|
||||
let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(4.0, 3.0));
|
||||
assert_eq!(segment.length_squared(), 25.0);
|
||||
assert_eq!(segment.length(), 5.0);
|
||||
}
|
||||
#[test]
|
||||
fn degenerate() {
|
||||
let segment = LineSegment2::new(&Vector2::new(1.0, 2.0), &Vector2::new(1.0, 2.0));
|
||||
assert!(segment.is_degenerate());
|
||||
assert_eq!(segment.length_squared(), 0.0);
|
||||
assert_eq!(segment.length(), 0.0);
|
||||
}
|
||||
#[test]
|
||||
fn collinear() {
|
||||
let segment = LineSegment2::new(&Vector2::new(0.0, 0.0), &Vector2::new(1.0, 2.0));
|
||||
assert_eq!(segment.collinearity(&Vector2::new(2.0, 4.0)), 0.0);
|
||||
assert_eq!(segment.collinearity(&Vector2::new(0.0, 0.0)), 0.0);
|
||||
assert_eq!(segment.collinearity(&Vector2::new(1.0, 2.0)), 0.0);
|
||||
assert!(
|
||||
segment.collinearity(&Vector2::new(1.0, 5.0)) < 0.0,
|
||||
"{} >= 0.0",
|
||||
segment.collinearity(&Vector2::new(1.0, 5.0))
|
||||
);
|
||||
assert!(
|
||||
segment.collinearity(&Vector2::new(1.0, 3.0)) < 0.0,
|
||||
"{} >= 0.0",
|
||||
segment.collinearity(&Vector2::new(1.0, 3.0))
|
||||
);
|
||||
assert!(
|
||||
segment.collinearity(&Vector2::new(1.0, 1.0)) > 0.0,
|
||||
"{} <= 0.0",
|
||||
segment.collinearity(&Vector2::new(1.0, 1.0))
|
||||
);
|
||||
assert!(
|
||||
segment.collinearity(&Vector2::new(-1.0, -5.0)) > 0.0,
|
||||
"{} <= 0.0",
|
||||
segment.collinearity(&Vector2::new(-1.0, -5.0))
|
||||
);
|
||||
}
|
||||
#[test]
|
||||
fn intersects() {
|
||||
let a = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(3.0, 1.0));
|
||||
let b = LineSegment::new(&Vector2::new(2.0, 0.0), &Vector2::new(2.5, 3.0));
|
||||
let c = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(-3.0, 1.0));
|
||||
assert!(a.intersects(&b));
|
||||
assert!(a.intersects(&c));
|
||||
assert!(b.intersects(&a));
|
||||
assert!(c.intersects(&a));
|
||||
assert!(a.swapped().intersects(&b));
|
||||
assert!(a.swapped().intersects(&c));
|
||||
}
|
||||
#[test]
|
||||
fn not_intersects() {
|
||||
let a = LineSegment::new(&Vector2::new(1.0, 2.0), &Vector2::new(3.0, 1.0));
|
||||
let b = LineSegment::new(&Vector2::new(0.0, 0.0), &Vector2::new(-1.0, 6.0));
|
||||
let c = LineSegment::new(&Vector2::new(2.0, 0.0), &Vector2::new(2.0, -1.0));
|
||||
assert!(!a.intersects(&b));
|
||||
assert!(!b.intersects(&c));
|
||||
assert!(!c.intersects(&a));
|
||||
assert!(!b.intersects(&a));
|
||||
assert!(!c.intersects(&b));
|
||||
assert!(!a.intersects(&c));
|
||||
assert!(!a.swapped().intersects(&b));
|
||||
assert!(!b.swapped().intersects(&c));
|
||||
assert!(!c.swapped().intersects(&a));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,314 @@
|
||||
// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in all
|
||||
// copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
|
||||
use nalgebra::{Vector2, Vector3};
|
||||
use std::fmt;
|
||||
|
||||
///
|
||||
/// Polygon vertex
|
||||
///
|
||||
#[derive(Debug)]
|
||||
struct Vertex {
|
||||
position: Vector2<f32>,
|
||||
prev: usize,
|
||||
index: usize,
|
||||
next: usize,
|
||||
}
|
||||
|
||||
///
|
||||
/// Linked list of vertices
|
||||
///
|
||||
struct Polygon {
|
||||
vertices: Vec<Vertex>,
|
||||
head: usize,
|
||||
tail: usize,
|
||||
}
|
||||
|
||||
impl Polygon {
|
||||
///
|
||||
/// Excludes vertex from polygon. Does not remove it from vertices array!
|
||||
///
|
||||
#[inline]
|
||||
fn remove_vertex(&mut self, index: usize) {
|
||||
let next_index = self.vertices[index].next;
|
||||
let prev_index = self.vertices[index].prev;
|
||||
|
||||
let prev = &mut self.vertices[prev_index];
|
||||
prev.next = next_index;
|
||||
|
||||
let next = &mut self.vertices[next_index];
|
||||
next.prev = prev_index;
|
||||
|
||||
if index == self.head {
|
||||
self.head = next_index;
|
||||
}
|
||||
|
||||
if index == self.tail {
|
||||
self.tail = prev_index;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl fmt::Debug for Polygon {
|
||||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||
let mut i = self.head;
|
||||
loop {
|
||||
let vertex = &self.vertices[i];
|
||||
writeln!(
|
||||
f,
|
||||
"Vertex {:?}; {} {} {}",
|
||||
vertex.position, vertex.prev, vertex.index, vertex.next
|
||||
)?;
|
||||
i = self.vertices[i].next;
|
||||
if i == self.head {
|
||||
break;
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
fn is_ear(poly: &Polygon, prev: &Vertex, ear: &Vertex, next: &Vertex) -> bool {
|
||||
// Check if other points are inside triangle
|
||||
let mut i = poly.head;
|
||||
loop {
|
||||
let vertex = &poly.vertices[i];
|
||||
if i != prev.index
|
||||
&& i != ear.index
|
||||
&& i != next.index
|
||||
&& crate::is_point_inside_2d_triangle(
|
||||
vertex.position,
|
||||
prev.position,
|
||||
ear.position,
|
||||
next.position,
|
||||
)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
i = vertex.next;
|
||||
if i == poly.head {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
///
|
||||
/// Triangulates specified polygon.
|
||||
///
|
||||
pub fn triangulate(vertices: &[Vector3<f32>], out_triangles: &mut Vec<[usize; 3]>) {
|
||||
out_triangles.clear();
|
||||
if vertices.len() == 3 {
|
||||
// Triangulating a triangle?
|
||||
out_triangles.push([0, 1, 2]);
|
||||
} else if vertices.len() == 4 {
|
||||
// Special case for quadrilaterals (much faster than generic)
|
||||
let mut start_vertex = 0;
|
||||
for i in 0..4 {
|
||||
let v = vertices[i];
|
||||
let v0 = vertices[(i + 3) % 4];
|
||||
if let Some(left) = (v0 - v).try_normalize(f32::EPSILON) {
|
||||
let v1 = vertices[(i + 2) % 4];
|
||||
if let Some(diag) = (v1 - v).try_normalize(f32::EPSILON) {
|
||||
let v2 = vertices[(i + 1) % 4];
|
||||
if let Some(right) = (v2 - v).try_normalize(f32::EPSILON) {
|
||||
// Check for concave vertex
|
||||
let angle = left.dot(&diag).acos() + right.dot(&diag).acos();
|
||||
if angle > std::f32::consts::PI {
|
||||
start_vertex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
out_triangles.push([start_vertex, (start_vertex + 1) % 4, (start_vertex + 2) % 4]);
|
||||
out_triangles.push([start_vertex, (start_vertex + 2) % 4, (start_vertex + 3) % 4]);
|
||||
} else {
|
||||
// Ear-clipping for arbitrary polygon (requires one additional memory allocation, so
|
||||
// relatively slow)
|
||||
if let Ok(normal) = crate::get_polygon_normal(vertices) {
|
||||
let plane_class = crate::classify_plane(normal);
|
||||
let mut polygon = Polygon {
|
||||
vertices: vertices
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, point)| Vertex {
|
||||
position: crate::vec3_to_vec2_by_plane(plane_class, normal, *point),
|
||||
index: i,
|
||||
prev: if i == 0 { vertices.len() - 1 } else { i - 1 },
|
||||
next: if i == vertices.len() - 1 { 0 } else { i + 1 },
|
||||
})
|
||||
.collect(),
|
||||
head: 0,
|
||||
tail: vertices.len() - 1,
|
||||
};
|
||||
let mut ear_index = polygon.head;
|
||||
let mut vertices_left = polygon.vertices.len();
|
||||
while vertices_left > 3 {
|
||||
let ear = &polygon.vertices[ear_index];
|
||||
let prev = &polygon.vertices[ear.prev];
|
||||
let next = &polygon.vertices[ear.next];
|
||||
if is_ear(&polygon, prev, ear, next) {
|
||||
let prev_index = prev.index;
|
||||
out_triangles.push([prev_index, ear.index, next.index]);
|
||||
polygon.remove_vertex(ear_index);
|
||||
ear_index = prev_index;
|
||||
vertices_left -= 1;
|
||||
} else {
|
||||
ear_index = ear.next;
|
||||
}
|
||||
}
|
||||
// Append last triangle.
|
||||
if vertices_left > 0 {
|
||||
let a = &polygon.vertices[polygon.head];
|
||||
let b = &polygon.vertices[a.next];
|
||||
out_triangles.push([polygon.head, a.next, b.next]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use nalgebra::Vector2;
|
||||
|
||||
use crate::triangulator::triangulate;
|
||||
use nalgebra::{Point3, Unit, UnitQuaternion, Vector3};
|
||||
|
||||
use super::{Polygon, Vertex};
|
||||
|
||||
#[test]
|
||||
fn triangle_triangulation() {
|
||||
let polygon = vec![
|
||||
Vector3::new(0.0, 0.0, 0.0),
|
||||
Vector3::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
];
|
||||
|
||||
let mut ref_indices = Vec::new();
|
||||
triangulate(polygon.as_slice(), &mut ref_indices);
|
||||
assert_ne!(ref_indices.len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn quadrilaterals_triangulation_non_concave() {
|
||||
let polygon = vec![
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
Vector3::new(1.0, 2.0, 1.0),
|
||||
Vector3::new(2.0, 3.0, 1.0),
|
||||
Vector3::new(3.0, 2.0, 1.0),
|
||||
];
|
||||
|
||||
let mut ref_indices = Vec::new();
|
||||
triangulate(polygon.as_slice(), &mut ref_indices);
|
||||
assert_ne!(ref_indices.len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn quadrilaterals_triangulation_concave() {
|
||||
let polygon = vec![
|
||||
Vector3::new(0.0, 2.0, 1.0),
|
||||
Vector3::new(3.0, 3.0, 1.0),
|
||||
Vector3::new(2.0, 2.0, 1.0),
|
||||
Vector3::new(3.0, 1.0, 1.0),
|
||||
];
|
||||
|
||||
let mut ref_indices = Vec::new();
|
||||
triangulate(polygon.as_slice(), &mut ref_indices);
|
||||
assert_ne!(ref_indices.len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ear_clip_test() {
|
||||
let polygon = vec![
|
||||
Vector3::new(-22.760103, 29.051392, 1.377507),
|
||||
Vector3::new(-24.6454, 29.051392, 1.377507),
|
||||
Vector3::new(-24.640476, 24.873882, 1.377506),
|
||||
Vector3::new(-24.637342, 22.215763, 1.377506),
|
||||
Vector3::new(-22.760103, 22.215763, 1.377506),
|
||||
];
|
||||
|
||||
// First test flat case
|
||||
let mut ref_indices = Vec::new();
|
||||
triangulate(polygon.as_slice(), &mut ref_indices);
|
||||
assert_ne!(ref_indices.len(), 0);
|
||||
|
||||
// Then compare previous result with series of rotated versions of the polygon.
|
||||
for axis in &[
|
||||
Unit::new_normalize(Vector3::new(1.0, 0.0, 0.0)),
|
||||
Unit::new_normalize(Vector3::new(0.0, 1.0, 0.0)),
|
||||
Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)),
|
||||
Unit::new_normalize(Vector3::new(1.0, 1.0, 1.0)),
|
||||
] {
|
||||
let mut angle: f32 = 0.0;
|
||||
while angle <= 360.0 {
|
||||
let mrot =
|
||||
UnitQuaternion::from_axis_angle(axis, angle.to_radians()).to_homogeneous();
|
||||
let rotated: Vec<Vector3<f32>> = polygon
|
||||
.iter()
|
||||
.map(|v: &Vector3<f32>| mrot.transform_point(&Point3::from(*v)).coords)
|
||||
.collect();
|
||||
let mut new_indices = Vec::new();
|
||||
triangulate(rotated.as_slice(), &mut new_indices);
|
||||
// We just need to ensure that we have the same amount of triangles as reference triangulation.
|
||||
assert_eq!(new_indices.len(), ref_indices.len());
|
||||
angle += 36.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_debug_for_polygon() {
|
||||
let p = Polygon {
|
||||
vertices: vec![
|
||||
Vertex {
|
||||
prev: 2,
|
||||
index: 0,
|
||||
next: 1,
|
||||
position: Vector2::new(0.0, 0.0),
|
||||
},
|
||||
Vertex {
|
||||
prev: 0,
|
||||
index: 1,
|
||||
next: 2,
|
||||
position: Vector2::new(1.0, 0.0),
|
||||
},
|
||||
Vertex {
|
||||
prev: 1,
|
||||
index: 2,
|
||||
next: 0,
|
||||
position: Vector2::new(0.0, 1.0),
|
||||
},
|
||||
],
|
||||
head: 0,
|
||||
tail: 2,
|
||||
};
|
||||
|
||||
assert_eq!(
|
||||
format!("{p:?}"),
|
||||
r"Vertex [[0.0, 0.0]]; 2 0 1
|
||||
Vertex [[1.0, 0.0]]; 0 1 2
|
||||
Vertex [[0.0, 1.0]]; 1 2 0
|
||||
"
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user