fyrox_math/
frustum.rs

1// Copyright (c) 2019-present Dmitry Stepanov and Fyrox Engine contributors.
2//
3// Permission is hereby granted, free of charge, to any person obtaining a copy
4// of this software and associated documentation files (the "Software"), to deal
5// in the Software without restriction, including without limitation the rights
6// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7// copies of the Software, and to permit persons to whom the Software is
8// furnished to do so, subject to the following conditions:
9//
10// The above copyright notice and this permission notice shall be included in all
11// copies or substantial portions of the Software.
12//
13// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19// SOFTWARE.
20
21use crate::{aabb::AxisAlignedBoundingBox, plane::Plane};
22use nalgebra::Point3;
23use nalgebra::{Matrix4, Vector3};
24
25#[derive(Copy, Clone, Debug, PartialEq)]
26pub struct Frustum {
27    /// 0 - left, 1 - right, 2 - top, 3 - bottom, 4 - far, 5 - near
28    pub planes: [Plane; 6],
29    pub corners: [Vector3<f32>; 8],
30}
31
32impl Default for Frustum {
33    #[inline]
34    fn default() -> Self {
35        Self::from_view_projection_matrix(Matrix4::new_perspective(
36            1.0,
37            std::f32::consts::FRAC_PI_2,
38            0.01,
39            1024.0,
40        ))
41        .unwrap()
42    }
43}
44
45impl Frustum {
46    pub const LEFT: usize = 0;
47    pub const RIGHT: usize = 1;
48    pub const TOP: usize = 2;
49    pub const BOTTOM: usize = 3;
50    pub const FAR: usize = 4;
51    pub const NEAR: usize = 5;
52
53    #[inline]
54    pub fn from_view_projection_matrix(m: Matrix4<f32>) -> Option<Self> {
55        let planes = [
56            // Left
57            Plane::from_abcd(m[3] + m[0], m[7] + m[4], m[11] + m[8], m[15] + m[12])?,
58            // Right
59            Plane::from_abcd(m[3] - m[0], m[7] - m[4], m[11] - m[8], m[15] - m[12])?,
60            // Top
61            Plane::from_abcd(m[3] - m[1], m[7] - m[5], m[11] - m[9], m[15] - m[13])?,
62            // Bottom
63            Plane::from_abcd(m[3] + m[1], m[7] + m[5], m[11] + m[9], m[15] + m[13])?,
64            // Far
65            Plane::from_abcd(m[3] - m[2], m[7] - m[6], m[11] - m[10], m[15] - m[14])?,
66            // Near
67            Plane::from_abcd(m[3] + m[2], m[7] + m[6], m[11] + m[10], m[15] + m[14])?,
68        ];
69
70        let corners = [
71            planes[Self::LEFT].intersection_point(&planes[Self::TOP], &planes[Self::FAR]),
72            planes[Self::LEFT].intersection_point(&planes[Self::BOTTOM], &planes[Self::FAR]),
73            planes[Self::RIGHT].intersection_point(&planes[Self::BOTTOM], &planes[Self::FAR]),
74            planes[Self::RIGHT].intersection_point(&planes[Self::TOP], &planes[Self::FAR]),
75            planes[Self::LEFT].intersection_point(&planes[Self::TOP], &planes[Self::NEAR]),
76            planes[Self::LEFT].intersection_point(&planes[Self::BOTTOM], &planes[Self::NEAR]),
77            planes[Self::RIGHT].intersection_point(&planes[Self::BOTTOM], &planes[Self::NEAR]),
78            planes[Self::RIGHT].intersection_point(&planes[Self::TOP], &planes[Self::NEAR]),
79        ];
80
81        Some(Self { planes, corners })
82    }
83
84    #[inline]
85    pub fn left(&self) -> &Plane {
86        self.planes.first().unwrap()
87    }
88
89    #[inline]
90    pub fn right(&self) -> &Plane {
91        self.planes.get(1).unwrap()
92    }
93
94    #[inline]
95    pub fn top(&self) -> &Plane {
96        self.planes.get(2).unwrap()
97    }
98
99    #[inline]
100    pub fn bottom(&self) -> &Plane {
101        self.planes.get(3).unwrap()
102    }
103
104    #[inline]
105    pub fn far(&self) -> &Plane {
106        self.planes.get(4).unwrap()
107    }
108
109    #[inline]
110    pub fn near(&self) -> &Plane {
111        self.planes.get(5).unwrap()
112    }
113
114    #[inline]
115    pub fn planes(&self) -> &[Plane] {
116        &self.planes
117    }
118
119    #[inline]
120    pub fn left_top_front_corner(&self) -> Vector3<f32> {
121        self.corners[0]
122    }
123
124    #[inline]
125    pub fn left_bottom_front_corner(&self) -> Vector3<f32> {
126        self.corners[1]
127    }
128
129    #[inline]
130    pub fn right_bottom_front_corner(&self) -> Vector3<f32> {
131        self.corners[2]
132    }
133
134    #[inline]
135    pub fn right_top_front_corner(&self) -> Vector3<f32> {
136        self.corners[3]
137    }
138
139    #[inline]
140    pub fn left_top_back_corner(&self) -> Vector3<f32> {
141        self.corners[4]
142    }
143
144    #[inline]
145    pub fn left_bottom_back_corner(&self) -> Vector3<f32> {
146        self.corners[5]
147    }
148
149    #[inline]
150    pub fn right_bottom_back_corner(&self) -> Vector3<f32> {
151        self.corners[6]
152    }
153
154    #[inline]
155    pub fn right_top_back_corner(&self) -> Vector3<f32> {
156        self.corners[7]
157    }
158
159    #[inline]
160    pub fn corners(&self) -> [Vector3<f32>; 8] {
161        [
162            self.left_top_front_corner(),
163            self.left_bottom_front_corner(),
164            self.right_bottom_front_corner(),
165            self.right_top_front_corner(),
166            self.left_top_back_corner(),
167            self.left_bottom_back_corner(),
168            self.right_bottom_back_corner(),
169            self.right_top_back_corner(),
170        ]
171    }
172
173    #[inline]
174    pub fn near_plane_center(&self) -> Vector3<f32> {
175        (self.left_top_front_corner()
176            + self.left_bottom_front_corner()
177            + self.right_bottom_front_corner()
178            + self.right_top_front_corner())
179        .scale(1.0 / 4.0)
180    }
181
182    #[inline]
183    pub fn far_plane_center(&self) -> Vector3<f32> {
184        (self.left_top_back_corner()
185            + self.left_bottom_back_corner()
186            + self.right_bottom_back_corner()
187            + self.right_top_back_corner())
188        .scale(1.0 / 4.0)
189    }
190
191    #[inline]
192    pub fn view_direction(&self) -> Vector3<f32> {
193        self.far_plane_center() - self.near_plane_center()
194    }
195
196    #[inline]
197    pub fn center(&self) -> Vector3<f32> {
198        self.corners()
199            .iter()
200            .fold(Vector3::default(), |acc, corner| acc + *corner)
201            .scale(1.0 / 8.0)
202    }
203
204    #[inline]
205    pub fn is_intersects_point_cloud(&self, points: &[Vector3<f32>]) -> bool {
206        for plane in self.planes.iter() {
207            let mut back_points = 0;
208            for point in points {
209                if plane.dot(point) <= 0.0 {
210                    back_points += 1;
211                    if back_points >= points.len() {
212                        // All points are behind current plane.
213                        return false;
214                    }
215                }
216            }
217        }
218        true
219    }
220
221    #[inline]
222    pub fn is_intersects_aabb(&self, aabb: &AxisAlignedBoundingBox) -> bool {
223        let corners = [
224            Vector3::new(aabb.min.x, aabb.min.y, aabb.min.z),
225            Vector3::new(aabb.min.x, aabb.min.y, aabb.max.z),
226            Vector3::new(aabb.max.x, aabb.min.y, aabb.max.z),
227            Vector3::new(aabb.max.x, aabb.min.y, aabb.min.z),
228            Vector3::new(aabb.min.x, aabb.max.y, aabb.min.z),
229            Vector3::new(aabb.min.x, aabb.max.y, aabb.max.z),
230            Vector3::new(aabb.max.x, aabb.max.y, aabb.max.z),
231            Vector3::new(aabb.max.x, aabb.max.y, aabb.min.z),
232        ];
233
234        if self.is_intersects_point_cloud(&corners) {
235            return true;
236        }
237
238        for corner in self.corners.iter() {
239            if aabb.is_contains_point(*corner) {
240                return true;
241            }
242        }
243
244        false
245    }
246
247    #[inline]
248    pub fn is_intersects_aabb_offset(
249        &self,
250        aabb: &AxisAlignedBoundingBox,
251        offset: Vector3<f32>,
252    ) -> bool {
253        let corners = [
254            Vector3::new(aabb.min.x, aabb.min.y, aabb.min.z) + offset,
255            Vector3::new(aabb.min.x, aabb.min.y, aabb.max.z) + offset,
256            Vector3::new(aabb.max.x, aabb.min.y, aabb.max.z) + offset,
257            Vector3::new(aabb.max.x, aabb.min.y, aabb.min.z) + offset,
258            Vector3::new(aabb.min.x, aabb.max.y, aabb.min.z) + offset,
259            Vector3::new(aabb.min.x, aabb.max.y, aabb.max.z) + offset,
260            Vector3::new(aabb.max.x, aabb.max.y, aabb.max.z) + offset,
261            Vector3::new(aabb.max.x, aabb.max.y, aabb.min.z) + offset,
262        ];
263
264        if self.is_intersects_point_cloud(&corners) {
265            return true;
266        }
267
268        for corner in self.corners.iter() {
269            if aabb.is_contains_point(*corner) {
270                return true;
271            }
272        }
273
274        false
275    }
276
277    #[deprecated(
278        since = "0.29.0",
279        note = "this method does not handle all cases and could give weird results"
280    )]
281    #[inline]
282    pub fn is_intersects_aabb_transform(
283        &self,
284        aabb: &AxisAlignedBoundingBox,
285        transform: &Matrix4<f32>,
286    ) -> bool {
287        if self.is_contains_point(
288            transform
289                .transform_point(&Point3::from(aabb.center()))
290                .coords,
291        ) {
292            return true;
293        }
294
295        let corners = [
296            transform
297                .transform_point(&Point3::new(aabb.min.x, aabb.min.y, aabb.min.z))
298                .coords,
299            transform
300                .transform_point(&Point3::new(aabb.min.x, aabb.min.y, aabb.max.z))
301                .coords,
302            transform
303                .transform_point(&Point3::new(aabb.max.x, aabb.min.y, aabb.max.z))
304                .coords,
305            transform
306                .transform_point(&Point3::new(aabb.max.x, aabb.min.y, aabb.min.z))
307                .coords,
308            transform
309                .transform_point(&Point3::new(aabb.min.x, aabb.max.y, aabb.min.z))
310                .coords,
311            transform
312                .transform_point(&Point3::new(aabb.min.x, aabb.max.y, aabb.max.z))
313                .coords,
314            transform
315                .transform_point(&Point3::new(aabb.max.x, aabb.max.y, aabb.max.z))
316                .coords,
317            transform
318                .transform_point(&Point3::new(aabb.max.x, aabb.max.y, aabb.min.z))
319                .coords,
320        ];
321
322        self.is_intersects_point_cloud(&corners)
323    }
324
325    #[inline]
326    pub fn is_contains_point(&self, pt: Vector3<f32>) -> bool {
327        for plane in self.planes.iter() {
328            if plane.dot(&pt) <= 0.0 {
329                return false;
330            }
331        }
332        true
333    }
334
335    #[inline]
336    pub fn is_intersects_sphere(&self, p: Vector3<f32>, r: f32) -> bool {
337        for plane in self.planes.iter() {
338            let d = plane.dot(&p);
339            if d < -r {
340                return false;
341            }
342            if d.abs() < r {
343                return true;
344            }
345        }
346        true
347    }
348}
349
350#[cfg(test)]
351mod test {
352    use crate::aabb::AxisAlignedBoundingBox;
353    use crate::{frustum::Frustum, plane::Plane};
354    use nalgebra::{Matrix4, Vector3};
355
356    #[test]
357    fn test_default_for_frustum() {
358        assert_eq!(
359            Frustum::default(),
360            Frustum::from_view_projection_matrix(Matrix4::new_perspective(
361                1.0,
362                std::f32::consts::FRAC_PI_2,
363                0.01,
364                1024.0
365            ))
366            .unwrap()
367        );
368    }
369
370    #[test]
371    fn test_frustum_from_view_projection_matrix() {
372        assert_eq!(
373            Frustum::from_view_projection_matrix(Matrix4::new(
374                1.0, 0.0, 0.0, 0.0, //
375                0.0, 1.0, 0.0, 0.0, //
376                0.0, 0.0, 1.0, 0.0, //
377                0.0, 0.0, 0.0, 1.0
378            )),
379            Some(Frustum {
380                planes: [
381                    Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap(),
382                    Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap(),
383                    Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap(),
384                    Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap(),
385                    Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap(),
386                    Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap(),
387                ],
388                corners: [
389                    Vector3::new(-1.0, 1.0, 1.0),
390                    Vector3::new(-1.0, -1.0, 1.0),
391                    Vector3::new(1.0, -1.0, 1.0),
392                    Vector3::new(1.0, 1.0, 1.0),
393                    Vector3::new(-1.0, 1.0, -1.0),
394                    Vector3::new(-1.0, -1.0, -1.0),
395                    Vector3::new(1.0, -1.0, -1.0),
396                    Vector3::new(1.0, 1.0, -1.0),
397                ],
398            })
399        );
400    }
401
402    #[test]
403    fn test_frustum_planes_and_corners() {
404        let f = Frustum::from_view_projection_matrix(Matrix4::new(
405            1.0, 0.0, 0.0, 0.0, //
406            0.0, 1.0, 0.0, 0.0, //
407            0.0, 0.0, 1.0, 0.0, //
408            0.0, 0.0, 0.0, 1.0,
409        ))
410        .unwrap();
411
412        assert_eq!(f.left(), &Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap());
413        assert_eq!(f.right(), &Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap());
414        assert_eq!(f.top(), &Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap());
415        assert_eq!(f.bottom(), &Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap());
416        assert_eq!(f.far(), &Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap());
417        assert_eq!(f.near(), &Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap());
418
419        assert_eq!(
420            f.planes(),
421            [
422                Plane::from_abcd(1.0, 0.0, 0.0, 1.0).unwrap(),
423                Plane::from_abcd(-1.0, 0.0, 0.0, 1.0).unwrap(),
424                Plane::from_abcd(0.0, -1.0, 0.0, 1.0).unwrap(),
425                Plane::from_abcd(0.0, 1.0, 0.0, 1.0).unwrap(),
426                Plane::from_abcd(0.0, 0.0, -1.0, 1.0).unwrap(),
427                Plane::from_abcd(0.0, 0.0, 1.0, 1.0).unwrap(),
428            ]
429        );
430
431        assert_eq!(f.left_top_front_corner(), Vector3::new(-1.0, 1.0, 1.0));
432        assert_eq!(f.left_bottom_front_corner(), Vector3::new(-1.0, -1.0, 1.0));
433        assert_eq!(f.right_bottom_front_corner(), Vector3::new(1.0, -1.0, 1.0));
434        assert_eq!(f.right_top_front_corner(), Vector3::new(1.0, 1.0, 1.0));
435        assert_eq!(f.left_top_back_corner(), Vector3::new(-1.0, 1.0, -1.0));
436        assert_eq!(f.left_bottom_back_corner(), Vector3::new(-1.0, -1.0, -1.0));
437        assert_eq!(f.right_bottom_back_corner(), Vector3::new(1.0, -1.0, -1.0));
438        assert_eq!(f.right_top_back_corner(), Vector3::new(1.0, 1.0, -1.0));
439
440        assert_eq!(
441            f.corners(),
442            [
443                Vector3::new(-1.0, 1.0, 1.0),
444                Vector3::new(-1.0, -1.0, 1.0),
445                Vector3::new(1.0, -1.0, 1.0),
446                Vector3::new(1.0, 1.0, 1.0),
447                Vector3::new(-1.0, 1.0, -1.0),
448                Vector3::new(-1.0, -1.0, -1.0),
449                Vector3::new(1.0, -1.0, -1.0),
450                Vector3::new(1.0, 1.0, -1.0),
451            ]
452        );
453    }
454
455    #[test]
456    fn test_frustum_plane_centers() {
457        let f = Frustum::from_view_projection_matrix(Matrix4::new(
458            1.0, 0.0, 0.0, 0.0, //
459            0.0, 1.0, 0.0, 0.0, //
460            0.0, 0.0, 1.0, 0.0, //
461            0.0, 0.0, 0.0, 1.0,
462        ))
463        .unwrap();
464
465        assert_eq!(f.near_plane_center(), Vector3::new(0.0, 0.0, 1.0));
466        assert_eq!(f.far_plane_center(), Vector3::new(0.0, 0.0, -1.0));
467        assert_eq!(f.view_direction(), Vector3::new(0.0, 0.0, -2.0));
468        assert_eq!(f.center(), Vector3::new(0.0, 0.0, 0.0));
469    }
470
471    #[test]
472    fn test_frustum_is_intersects_point_cloud() {
473        let f = Frustum::from_view_projection_matrix(Matrix4::new(
474            1.0, 0.0, 0.0, 0.0, //
475            0.0, 1.0, 0.0, 0.0, //
476            0.0, 0.0, 1.0, 0.0, //
477            0.0, 0.0, 0.0, 1.0,
478        ))
479        .unwrap();
480
481        assert!(f.is_intersects_point_cloud(&[
482            Vector3::new(0.0, 0.0, 0.0),
483            Vector3::new(1.0, 1.0, 1.0),
484        ]));
485        assert!(!f.is_intersects_point_cloud(&[Vector3::new(-1.0, -2.0, 1.0)]));
486    }
487
488    #[test]
489    fn test_frustum_is_intersects_aabb() {
490        let f = Frustum::from_view_projection_matrix(Matrix4::new(
491            1.0, 0.0, 0.0, 0.0, //
492            0.0, 1.0, 0.0, 0.0, //
493            0.0, 0.0, 1.0, 0.0, //
494            0.0, 0.0, 0.0, 1.0,
495        ))
496        .unwrap();
497
498        assert!(f.is_intersects_aabb(&AxisAlignedBoundingBox::unit()));
499        assert!(!f.is_intersects_aabb(&AxisAlignedBoundingBox::from_min_max(
500            Vector3::new(5.0, 5.0, 5.0),
501            Vector3::new(15.0, 15.0, 15.0)
502        )));
503    }
504
505    #[test]
506    fn test_frustum_is_intersects_aabb_offset() {
507        let f = Frustum::from_view_projection_matrix(Matrix4::new(
508            1.0, 0.0, 0.0, 0.0, //
509            0.0, 1.0, 0.0, 0.0, //
510            0.0, 0.0, 1.0, 0.0, //
511            0.0, 0.0, 0.0, 1.0,
512        ))
513        .unwrap();
514
515        assert!(f.is_intersects_aabb_offset(
516            &AxisAlignedBoundingBox::unit(),
517            Vector3::new(1.0, 1.0, 1.0)
518        ));
519        assert!(!f.is_intersects_aabb_offset(
520            &AxisAlignedBoundingBox::unit(),
521            Vector3::new(10.0, 10.0, 10.0)
522        ));
523    }
524
525    #[test]
526    fn test_frustum_is_contains_point() {
527        let f = Frustum::from_view_projection_matrix(Matrix4::new(
528            1.0, 0.0, 0.0, 0.0, //
529            0.0, 1.0, 0.0, 0.0, //
530            0.0, 0.0, 1.0, 0.0, //
531            0.0, 0.0, 0.0, 1.0,
532        ))
533        .unwrap();
534
535        assert!(f.is_contains_point(Vector3::new(0.0, 0.0, 0.0)));
536        assert!(!f.is_contains_point(Vector3::new(10.0, 10.0, 10.0)));
537    }
538
539    #[test]
540    fn test_frustum_is_intersects_sphere() {
541        let f = Frustum::from_view_projection_matrix(Matrix4::new(
542            1.0, 0.0, 0.0, 0.0, //
543            0.0, 1.0, 0.0, 0.0, //
544            0.0, 0.0, 1.0, 0.0, //
545            0.0, 0.0, 0.0, 1.0,
546        ))
547        .unwrap();
548
549        assert!(f.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 1.0));
550        assert!(f.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 2.0));
551        assert!(!f.is_intersects_sphere(Vector3::new(10.0, 10.0, 10.0), 1.0));
552    }
553}
fyrox_math/frustum.rs

fyrox_math/
frustum.rs
