mirror of https://github.com/mosra/magnum.git
Browse Source
Too deep nesting, too much typing. Colon cancer. Fully preserving backwards compatibility, except for the recently added cone/frustum intersection functions, which were not in master yet.pull/255/head
Vladimír Vondruš
8 years ago
18 changed files with 1090 additions and 915 deletions
@ -0,0 +1,276 @@ |
|||||||
|
#ifndef Magnum_Math_Distance_h |
||||||
|
#define Magnum_Math_Distance_h |
||||||
|
/*
|
||||||
|
This file is part of Magnum. |
||||||
|
|
||||||
|
Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 |
||||||
|
Vladimír Vondruš <mosra@centrum.cz> |
||||||
|
Copyright © 2016 Jonathan Hale <squareys@googlemail.com> |
||||||
|
|
||||||
|
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. |
||||||
|
*/ |
||||||
|
|
||||||
|
/** @file
|
||||||
|
* @brief Namespace @ref Magnum::Math::Distance |
||||||
|
*/ |
||||||
|
|
||||||
|
#include "Magnum/Math/Functions.h" |
||||||
|
#include "Magnum/Math/Vector3.h" |
||||||
|
#include "Magnum/Math/Vector4.h" |
||||||
|
|
||||||
|
namespace Magnum { namespace Math { namespace Distance { |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of line and point in 2D, squared |
||||||
|
@param a First point of the line |
||||||
|
@param b Second point of the line |
||||||
|
@param point Point |
||||||
|
|
||||||
|
More efficient than @ref linePoint(const Vector2<T>&, const Vector2<T>&, const Vector2<T>&) |
||||||
|
for comparing distance with other values, because it doesn't calculate the |
||||||
|
square root. |
||||||
|
*/ |
||||||
|
template<class T> inline T linePointSquared(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point) { |
||||||
|
const Vector2<T> bMinusA = b - a; |
||||||
|
return Math::pow<2>(cross(bMinusA, a - point))/bMinusA.dot(); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of line and point in 2D |
||||||
|
@param a First point of the line |
||||||
|
@param b Second point of the line |
||||||
|
@param point Point |
||||||
|
|
||||||
|
The distance @f$ d @f$ is calculated from point @f$ \boldsymbol{p} @f$ and line |
||||||
|
defined by @f$ \boldsymbol{a} @f$ and @f$ \boldsymbol{b} @f$ using |
||||||
|
@ref cross(const Vector2<T>&, const Vector2<T>&) "perp-dot product": @f[ |
||||||
|
d = \frac{|(\boldsymbol b - \boldsymbol a)_\bot \cdot (\boldsymbol a - \boldsymbol p)|}{|\boldsymbol b - \boldsymbol a|} |
||||||
|
@f] |
||||||
|
Source: http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
|
||||||
|
@see @ref linePointSquared(const Vector2<T>&, const Vector2<T>&, const Vector2<T>&) |
||||||
|
*/ |
||||||
|
template<class T> inline T linePoint(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point) { |
||||||
|
const Vector2<T> bMinusA = b - a; |
||||||
|
return std::abs(cross(bMinusA, a - point))/bMinusA.length(); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of line and point in 3D, squared |
||||||
|
|
||||||
|
More efficient than @ref linePoint(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&) |
||||||
|
for comparing distance with other values, because it doesn't calculate the |
||||||
|
square root. |
||||||
|
*/ |
||||||
|
template<class T> inline T linePointSquared(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point) { |
||||||
|
return cross(point - a, point - b).dot()/(b - a).dot(); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of line and point in 3D |
||||||
|
@param a First point of the line |
||||||
|
@param b Second point of the line |
||||||
|
@param point Point |
||||||
|
|
||||||
|
The distance @f$ d @f$ is calculated from point @f$ \boldsymbol{p} @f$ and line |
||||||
|
defined by @f$ \boldsymbol{a} @f$ and @f$ \boldsymbol{b} @f$ using |
||||||
|
@ref cross(const Vector3<T>&, const Vector3<T>&) "cross product": @f[ |
||||||
|
d = \frac{|(\boldsymbol p - \boldsymbol a) \times (\boldsymbol p - \boldsymbol b)|}{|\boldsymbol b - \boldsymbol a|} |
||||||
|
@f] |
||||||
|
Source: http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
|
||||||
|
@see @ref linePointSquared(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&) |
||||||
|
*/ |
||||||
|
template<class T> inline T linePoint(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point) { |
||||||
|
return std::sqrt(linePointSquared(a, b, point)); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of point from line segment in 2D, squared |
||||||
|
|
||||||
|
More efficient than @ref lineSegmentPoint() for comparing distance with other |
||||||
|
values, because it doesn't calculate the square root. |
||||||
|
*/ |
||||||
|
template<class T> T lineSegmentPointSquared(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Dístance of point from line segment in 2D |
||||||
|
@param a Starting point of the line |
||||||
|
@param b Ending point of the line |
||||||
|
@param point Point |
||||||
|
|
||||||
|
Returns distance of point from line segment or from its starting/ending point, |
||||||
|
depending on where the point lies. |
||||||
|
|
||||||
|
Determining whether the point lies next to line segment or outside is done |
||||||
|
using Pythagorean theorem. If the following equation applies, the point |
||||||
|
@f$ \boldsymbol{p} @f$ lies outside line segment closer to @f$ \boldsymbol{a} @f$: @f[ |
||||||
|
|\boldsymbol p - \boldsymbol b|^2 > |\boldsymbol b - \boldsymbol a|^2 + |\boldsymbol p - \boldsymbol a|^2 |
||||||
|
@f] |
||||||
|
On the other hand, if the following equation applies, the point lies outside |
||||||
|
line segment closer to @f$ \boldsymbol{b} @f$: @f[ |
||||||
|
|\boldsymbol p - \boldsymbol a|^2 > |\boldsymbol b - \boldsymbol a|^2 + |\boldsymbol p - \boldsymbol b|^2 |
||||||
|
@f] |
||||||
|
The last alternative is when the following equation applies. The point then |
||||||
|
lies between @f$ \boldsymbol{a} @f$ and @f$ \boldsymbol{b} @f$ and the distance |
||||||
|
is calculated the same way as in @ref linePoint(). @f[ |
||||||
|
|\boldsymbol b - \boldsymbol a|^2 > |\boldsymbol p - \boldsymbol a|^2 + |\boldsymbol p - \boldsymbol b|^2 |
||||||
|
@f] |
||||||
|
|
||||||
|
@see @ref lineSegmentPointSquared() |
||||||
|
*/ |
||||||
|
template<class T> T lineSegmentPoint(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of point from line segment in 3D, squared |
||||||
|
|
||||||
|
More efficient than @ref lineSegmentPoint(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&) |
||||||
|
for comparing distance with other values, because it doesn't calculate the |
||||||
|
square root. |
||||||
|
*/ |
||||||
|
template<class T> T lineSegmentPointSquared(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Dístance of point from line segment in 3D |
||||||
|
@param a Starting point of the line |
||||||
|
@param b Ending point of the line |
||||||
|
@param point Point |
||||||
|
|
||||||
|
Similar to 2D implementation @ref lineSegmentPoint(const Vector2<T>&, const Vector2<T>&, const Vector2<T>&). |
||||||
|
@see @ref lineSegmentPointSquared(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&) |
||||||
|
*/ |
||||||
|
template<class T> inline T lineSegmentPoint(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point) { |
||||||
|
return std::sqrt(lineSegmentPointSquared(a, b, point)); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of point from plane, scaled by the length of the planes normal |
||||||
|
|
||||||
|
The distance @f$ d @f$ is calculated from point @f$ \boldsymbol{p} @f$ and |
||||||
|
plane with normal @f$ \boldsymbol{n} @f$ and @f$ w @f$ using: @f[ |
||||||
|
d = \boldsymbol{p} \cdot \boldsymbol{n} + w |
||||||
|
@f] |
||||||
|
The distance is negative if the point lies behind the plane. |
||||||
|
|
||||||
|
More efficient than @ref pointPlane() when merely the sign of the distance is |
||||||
|
of interest, for example when testing on which half space of the plane the |
||||||
|
point lies. |
||||||
|
@see @ref pointPlaneNormalized() |
||||||
|
*/ |
||||||
|
template<class T> inline T pointPlaneScaled(const Vector3<T>& point, const Vector4<T>& plane) { |
||||||
|
return dot(plane.xyz(), point) + plane.w(); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of point from plane |
||||||
|
|
||||||
|
The distance @f$ d @f$ is calculated from point @f$ \boldsymbol{p} @f$ and |
||||||
|
plane with normal @f$ \boldsymbol{n} @f$ and @f$ w @f$ using: @f[ |
||||||
|
d = \frac{\boldsymbol{p} \cdot \boldsymbol{n} + w}{\left| \boldsymbol{n} \right|} |
||||||
|
@f] |
||||||
|
The distance is negative if the point lies behind the plane. |
||||||
|
|
||||||
|
In cases where the planes normal is a unit vector, @ref pointPlaneNormalized() |
||||||
|
is more efficient. If merely the sign of the distance is of interest, |
||||||
|
@ref pointPlaneScaled() is more efficient. |
||||||
|
*/ |
||||||
|
template<class T> inline T pointPlane(const Vector3<T>& point, const Vector4<T>& plane) { |
||||||
|
return pointPlaneScaled<T>(point, plane)/plane.xyz().length(); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Distance of point from plane with normalized normal |
||||||
|
|
||||||
|
The distance @f$ d @f$ is calculated from point @f$ \boldsymbol{p} @f$ and plane |
||||||
|
with normal @f$ \boldsymbol{n} @f$ and @f$ w @f$ using: @f[ |
||||||
|
d = \boldsymbol{p} \cdot \boldsymbol{n} + w |
||||||
|
@f] |
||||||
|
The distance is negative if the point lies behind the plane. Expects that |
||||||
|
@p plane normal is normalized. |
||||||
|
|
||||||
|
More efficient than @ref pointPlane() in cases where the plane's normal is |
||||||
|
normalized. Equivalent to @ref pointPlaneScaled() but with assertion added on |
||||||
|
top. |
||||||
|
*/ |
||||||
|
template<class T> inline T pointPlaneNormalized(const Vector3<T>& point, const Vector4<T>& plane) { |
||||||
|
CORRADE_ASSERT(plane.xyz().isNormalized(), |
||||||
|
"Math::Geometry::Distance::pointPlaneNormalized(): plane normal is not an unit vector", {}); |
||||||
|
return pointPlaneScaled<T>(point, plane); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> T lineSegmentPoint(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point) { |
||||||
|
const Vector2<T> pointMinusA = point - a; |
||||||
|
const Vector2<T> pointMinusB = point - b; |
||||||
|
const Vector2<T> bMinusA = b - a; |
||||||
|
const T pointDistanceA = pointMinusA.dot(); |
||||||
|
const T pointDistanceB = pointMinusB.dot(); |
||||||
|
const T bDistanceA = bMinusA.dot(); |
||||||
|
|
||||||
|
/* Point is before A */ |
||||||
|
if(pointDistanceB > bDistanceA + pointDistanceA) |
||||||
|
return std::sqrt(pointDistanceA); |
||||||
|
|
||||||
|
/* Point is after B */ |
||||||
|
if(pointDistanceA > bDistanceA + pointDistanceB) |
||||||
|
return std::sqrt(pointDistanceB); |
||||||
|
|
||||||
|
/* Between A and B */ |
||||||
|
return std::abs(cross(bMinusA, -pointMinusA))/std::sqrt(bDistanceA); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> T lineSegmentPointSquared(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point) { |
||||||
|
const Vector2<T> pointMinusA = point - a; |
||||||
|
const Vector2<T> pointMinusB = point - b; |
||||||
|
const Vector2<T> bMinusA = b - a; |
||||||
|
const T pointDistanceA = pointMinusA.dot(); |
||||||
|
const T pointDistanceB = pointMinusB.dot(); |
||||||
|
const T bDistanceA = bMinusA.dot(); |
||||||
|
|
||||||
|
/* Point is before A */ |
||||||
|
if(pointDistanceB > bDistanceA + pointDistanceA) |
||||||
|
return pointDistanceA; |
||||||
|
|
||||||
|
/* Point is after B */ |
||||||
|
if(pointDistanceA > bDistanceA + pointDistanceB) |
||||||
|
return pointDistanceB; |
||||||
|
|
||||||
|
/* Between A and B */ |
||||||
|
return Math::pow<2>(cross(bMinusA, -pointMinusA))/bDistanceA; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> T lineSegmentPointSquared(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point) { |
||||||
|
const Vector3<T> pointMinusA = point - a; |
||||||
|
const Vector3<T> pointMinusB = point - b; |
||||||
|
const T pointDistanceA = pointMinusA.dot(); |
||||||
|
const T pointDistanceB = pointMinusB.dot(); |
||||||
|
const T bDistanceA = (b - a).dot(); |
||||||
|
|
||||||
|
/* Point is before A */ |
||||||
|
if(pointDistanceB > bDistanceA + pointDistanceA) |
||||||
|
return pointDistanceA; |
||||||
|
|
||||||
|
/* Point is after B */ |
||||||
|
if(pointDistanceA > bDistanceA + pointDistanceB) |
||||||
|
return pointDistanceB; |
||||||
|
|
||||||
|
/* Between A and B */ |
||||||
|
return cross(pointMinusA, pointMinusB).dot()/bDistanceA; |
||||||
|
} |
||||||
|
|
||||||
|
}}} |
||||||
|
|
||||||
|
#endif |
||||||
@ -1,40 +0,0 @@ |
|||||||
# |
|
||||||
# This file is part of Magnum. |
|
||||||
# |
|
||||||
# Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 |
|
||||||
# Vladimír Vondruš <mosra@centrum.cz> |
|
||||||
# |
|
||||||
# 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. |
|
||||||
# |
|
||||||
|
|
||||||
corrade_add_test(MathGeometryDistanceTest DistanceTest.cpp LIBRARIES MagnumMathTestLib) |
|
||||||
target_compile_definitions(MathGeometryDistanceTest PRIVATE "CORRADE_GRACEFUL_ASSERT") |
|
||||||
|
|
||||||
corrade_add_test(MathGeometryIntersectionTest IntersectionTest.cpp LIBRARIES MagnumMathTestLib) |
|
||||||
corrade_add_test(MathGeometryIntersectionBenchmark IntersectionBenchmark.cpp LIBRARIES MagnumMathTestLib) |
|
||||||
|
|
||||||
set_target_properties( |
|
||||||
MathGeometryDistanceTest |
|
||||||
MathGeometryIntersectionTest |
|
||||||
MathGeometryIntersectionBenchmark |
|
||||||
PROPERTIES FOLDER "Magnum/Math/Geometry/Test") |
|
||||||
|
|
||||||
set_property(TARGET |
|
||||||
MathGeometryIntersectionTest |
|
||||||
APPEND PROPERTY COMPILE_DEFINITIONS "CORRADE_GRACEFUL_ASSERT") |
|
||||||
@ -0,0 +1,625 @@ |
|||||||
|
#ifndef Magnum_Math_Intersection_h |
||||||
|
#define Magnum_Math_Intersection_h |
||||||
|
/*
|
||||||
|
This file is part of Magnum. |
||||||
|
|
||||||
|
Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 |
||||||
|
Vladimír Vondruš <mosra@centrum.cz> |
||||||
|
Copyright © 2016, 2018 Jonathan Hale <squareys@googlemail.com> |
||||||
|
|
||||||
|
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. |
||||||
|
*/ |
||||||
|
|
||||||
|
/** @file
|
||||||
|
* @brief Namespace @ref Magnum::Math::Intersection |
||||||
|
*/ |
||||||
|
|
||||||
|
#include "Magnum/Math/Distance.h" |
||||||
|
#include "Magnum/Math/Frustum.h" |
||||||
|
#include "Magnum/Math/Range.h" |
||||||
|
#include "Magnum/Math/Vector2.h" |
||||||
|
#include "Magnum/Math/Vector3.h" |
||||||
|
#include "Magnum/Math/Matrix4.h" |
||||||
|
|
||||||
|
namespace Magnum { namespace Math { namespace Intersection { |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of two line segments in 2D |
||||||
|
@param p Starting point of first line segment |
||||||
|
@param r Direction of first line segment |
||||||
|
@param q Starting point of second line segment |
||||||
|
@param s Direction of second line segment |
||||||
|
|
||||||
|
Returns intersection point positions @f$ t @f$, @f$ u @f$ on both lines: |
||||||
|
|
||||||
|
- @f$ t, u = \mathrm{NaN} @f$ if the lines are collinear |
||||||
|
- @f$ t \in [ 0 ; 1 ] @f$ if the intersection is inside the line segment |
||||||
|
defined by @f$ \boldsymbol{p} @f$ and @f$ \boldsymbol{p} + \boldsymbol{r} @f$ |
||||||
|
- @f$ t \notin [ 0 ; 1 ] @f$ if the intersection is outside the line segment |
||||||
|
- @f$ u \in [ 0 ; 1 ] @f$ if the intersection is inside the line segment |
||||||
|
defined by @f$ \boldsymbol{q} @f$ and @f$ \boldsymbol{q} + \boldsymbol{s} @f$ |
||||||
|
- @f$ u \notin [ 0 ; 1 ] @f$ if the intersection is outside the line segment |
||||||
|
- @f$ t, u \in \{-\infty, \infty\} @f$ if the intersection doesn't exist (the |
||||||
|
2D lines are parallel) |
||||||
|
|
||||||
|
The two lines intersect if @f$ t @f$ and @f$ u @f$ exist such that: @f[ |
||||||
|
\boldsymbol p + t \boldsymbol r = \boldsymbol q + u \boldsymbol s |
||||||
|
@f] |
||||||
|
Crossing both sides with @f$ \boldsymbol{s} @f$, distributing the cross product |
||||||
|
and eliminating @f$ \boldsymbol s \times \boldsymbol s = 0 @f$, then solving |
||||||
|
for @f$ t @f$ and similarly for @f$ u @f$: @f[ |
||||||
|
\begin{array}{rcl} |
||||||
|
(\boldsymbol p + t \boldsymbol r) \times s & = & (\boldsymbol q + u \boldsymbol s) \times s \\
|
||||||
|
t (\boldsymbol r \times s) & = & (\boldsymbol q - \boldsymbol p) \times s \\
|
||||||
|
t & = & \cfrac{(\boldsymbol q - \boldsymbol p) \times s}{\boldsymbol r \times \boldsymbol s} \\
|
||||||
|
u & = & \cfrac{(\boldsymbol q - \boldsymbol p) \times r}{\boldsymbol r \times \boldsymbol s} |
||||||
|
\end{array} |
||||||
|
@f] |
||||||
|
|
||||||
|
See also @ref lineSegmentLine() which calculates only @f$ t @f$, useful if you |
||||||
|
don't need to test that the intersection lies inside line segment defined by |
||||||
|
@f$ \boldsymbol{q} @f$ and @f$ \boldsymbol{q} + \boldsymbol{s} @f$. |
||||||
|
|
||||||
|
@see @ref isInf(), @ref isNan() |
||||||
|
*/ |
||||||
|
template<class T> inline std::pair<T, T> lineSegmentLineSegment(const Vector2<T>& p, const Vector2<T>& r, const Vector2<T>& q, const Vector2<T>& s) { |
||||||
|
const Vector2<T> qp = q - p; |
||||||
|
const T rs = cross(r, s); |
||||||
|
return {cross(qp, s)/rs, cross(qp, r)/rs}; |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of line segment and line in 2D |
||||||
|
@param p Starting point of first line segment |
||||||
|
@param r Direction of first line segment |
||||||
|
@param q Starting point of second line |
||||||
|
@param s Direction of second line |
||||||
|
|
||||||
|
Returns intersection point position @f$ t @f$ on the first line: |
||||||
|
|
||||||
|
- @f$ t = \mathrm{NaN} @f$ if the lines are collinear |
||||||
|
- @f$ t \in [ 0 ; 1 ] @f$ if the intersection is inside the line segment |
||||||
|
defined by @f$ \boldsymbol{p} @f$ and @f$ \boldsymbol{p} + \boldsymbol{r} @f$ |
||||||
|
- @f$ t \notin [ 0 ; 1 ] @f$ if the intersection is outside the line segment |
||||||
|
- @f$ t \in \{-\infty, \infty\} @f$ if the intersection doesn't exist (the 2D |
||||||
|
lines are parallel) |
||||||
|
|
||||||
|
Unlike @ref lineSegmentLineSegment() calculates only @f$ t @f$. |
||||||
|
|
||||||
|
@see @ref isInf(), @ref isNan() |
||||||
|
*/ |
||||||
|
template<class T> inline T lineSegmentLine(const Vector2<T>& p, const Vector2<T>& r, const Vector2<T>& q, const Vector2<T>& s) { |
||||||
|
return cross(q - p, s)/cross(r, s); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a plane and line |
||||||
|
@param planePosition Plane position |
||||||
|
@param planeNormal Plane normal |
||||||
|
@param p Starting point of the line |
||||||
|
@param r Direction of the line |
||||||
|
|
||||||
|
Returns intersection point position @f$ t @f$ on the line: |
||||||
|
|
||||||
|
- @f$ t = \mathrm{NaN} @f$ if the line lies on the plane |
||||||
|
- @f$ t \in [ 0 ; 1 ] @f$ if the intersection is inside the line segment |
||||||
|
defined by @f$ \boldsymbol{p} @f$ and @f$ \boldsymbol{p} + \boldsymbol{r} @f$ |
||||||
|
- @f$ t \notin [ 0 ; 1 ] @f$ if the intersection is outside the line segment |
||||||
|
- @f$ t \in \{-\infty, \infty\} @f$ if the intersection doesn't exist |
||||||
|
|
||||||
|
First the parameter @f$ f @f$ of parametric equation of the plane is calculated |
||||||
|
from plane normal @f$ \boldsymbol{n} @f$ and plane position: @f[ |
||||||
|
\begin{pmatrix} n_0 \\ n_1 \\ n_2 \end{pmatrix} \cdot |
||||||
|
\begin{pmatrix} x \\ y \\ z \end{pmatrix} - f = 0 |
||||||
|
@f] |
||||||
|
Using plane normal @f$ \boldsymbol{n} @f$, parameter @f$ f @f$ and line defined |
||||||
|
by @f$ \boldsymbol{p} @f$ and @f$ \boldsymbol{r} @f$, value of @f$ t @f$ is |
||||||
|
calculated and returned. @f[ |
||||||
|
\begin{array}{rcl} |
||||||
|
f & = & \boldsymbol n \cdot (\boldsymbol p + t \boldsymbol r) \\
|
||||||
|
\Rightarrow t & = & \cfrac{f - \boldsymbol n \cdot \boldsymbol p}{\boldsymbol n \cdot \boldsymbol r} |
||||||
|
\end{array} |
||||||
|
@f] |
||||||
|
|
||||||
|
@see @ref isInf(), @ref isNan() |
||||||
|
*/ |
||||||
|
template<class T> inline T planeLine(const Vector3<T>& planePosition, const Vector3<T>& planeNormal, const Vector3<T>& p, const Vector3<T>& r) { |
||||||
|
const T f = dot(planePosition, planeNormal); |
||||||
|
return (f - dot(planeNormal, p))/dot(planeNormal, r); |
||||||
|
} |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a point and a frustum |
||||||
|
@param point Point |
||||||
|
@param frustum Frustum planes with normals pointing outwards |
||||||
|
@return @cpp true @ce if the point is on or inside the frustum, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Checks for each plane of the frustum whether the point is behind the plane (the |
||||||
|
points distance from the plane is negative) using @ref Distance::pointPlaneScaled(). |
||||||
|
*/ |
||||||
|
template<class T> bool pointFrustum(const Vector3<T>& point, const Frustum<T>& frustum); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a range and a frustum |
||||||
|
@param range Range |
||||||
|
@param frustum Frustum planes with normals pointing outwards |
||||||
|
@return @cpp true @ce if the box intersects with the frustum, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Uses the "p/n-vertex" approach: First converts the @ref Range3D into a |
||||||
|
representation using center and extent which allows using the following |
||||||
|
condition for whether the plane is intersecting the box: @f[ |
||||||
|
\begin{array}{rcl} |
||||||
|
d & = & \boldsymbol c \cdot \boldsymbol n \\
|
||||||
|
r & = & \boldsymbol c \cdot \text{abs}(\boldsymbol n) \\
|
||||||
|
d + r & < & -w |
||||||
|
\end{array} |
||||||
|
@f] |
||||||
|
|
||||||
|
for plane normal @f$ \boldsymbol n @f$ and determinant @f$ w @f$. |
||||||
|
|
||||||
|
@see @ref aabbFrustum() |
||||||
|
*/ |
||||||
|
template<class T> bool rangeFrustum(const Range3D<T>& range, const Frustum<T>& frustum); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of an axis-aligned box and a frustum |
||||||
|
@param aabbCenter Center of the AABB |
||||||
|
@param aabbExtents (Half-)extents of the AABB |
||||||
|
@param frustum Frustum planes with normals pointing outwards |
||||||
|
@return @cpp true @ce if the box intersects with the frustum, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Uses the same method as @ref rangeFrustum(), but does not need to convert to |
||||||
|
center/extents representation. |
||||||
|
*/ |
||||||
|
template<class T> bool aabbFrustum(const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, const Frustum<T>& frustum); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a sphere and a frustum |
||||||
|
@param sphereCenter Sphere center |
||||||
|
@param sphereRadius Sphere radius |
||||||
|
@param frustum Frustum planes with normals pointing outwards |
||||||
|
@return @cpp true @ce if the sphere intersects the frustum, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Checks for each plane of the frustum whether the sphere is behind the plane |
||||||
|
(the points distance larger than the sphere's radius) using |
||||||
|
@ref Distance::pointPlaneScaled(). |
||||||
|
*/ |
||||||
|
template<class T> bool sphereFrustum(const Vector3<T>& sphereCenter, T sphereRadius, const Frustum<T>& frustum); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a point and a cone |
||||||
|
@param point The point |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param coneAngle Apex angle of the cone |
||||||
|
@return @cpp true @ce if the point is inside the cone, @cpp false @ce otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref pointCone(const Vector3&, const Vector3&, const Vector3&, T). |
||||||
|
*/ |
||||||
|
template<class T> bool pointCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a point and a cone using precomputed values |
||||||
|
@param point The point |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation |
||||||
|
@return @cpp true @ce if the point is inside the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
The @p tanAngleSqPlusOne parameter can be calculated as: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
Math::pow<2>(Math::tan(angle*T(0.5))) + T(1) |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool pointCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, T tanAngleSqPlusOne); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a point and a double cone |
||||||
|
@param point The point |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param coneAngle Apex angle of the cone |
||||||
|
@return @cpp true @ce if the point is inside the double cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref pointDoubleCone(const Vector3&, const Vector3&, const Vector3&, T). |
||||||
|
*/ |
||||||
|
template<class T> bool pointDoubleCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a point and a double cone using precomputed values |
||||||
|
@param point The point |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation |
||||||
|
@return @cpp true @ce if the point is inside the double cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
The @p tanAngleSqPlusOne parameter can be precomputed like this: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
T tanAngleSqPlusOne = Math::pow<2>(Math::tan(angle*T(0.5))) + T(1); |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool pointDoubleCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, T tanAngleSqPlusOne); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a sphere and a cone view |
||||||
|
@param sphereCenter Center of the sphere |
||||||
|
@param sphereRadius Radius of the sphere |
||||||
|
@param coneView View matrix with translation and rotation of the cone |
||||||
|
@param coneAngle Apex angle of the cone (@f$ 0 < \Theta < \pi @f$) |
||||||
|
@return @cpp true @ce if the sphere intersects the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref sphereConeView(const Vector3<T>&, T, const Matrix4<T>&, T, T). |
||||||
|
*/ |
||||||
|
template<class T> bool sphereConeView(const Vector3<T>& sphereCenter, T sphereRadius, const Matrix4<T>& coneView, Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a sphere and a cone view |
||||||
|
@param sphereCenter Sphere center |
||||||
|
@param sphereRadius Sphere radius |
||||||
|
@param coneView View matrix with translation and rotation of the cone |
||||||
|
@param sinAngle Precomputed sine of half the cone's opening angle |
||||||
|
@param tanAngle Precomputed tangent of half the cone's opening angle |
||||||
|
@return @cpp true @ce if the sphere intersects the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Transforms the sphere center into cone space (using the cone view matrix) and |
||||||
|
performs sphere-cone intersection with the zero-origin -Z axis-aligned cone. |
||||||
|
The @p sinAngle, @p cosAngle, @p tanAngle can be precomputed like this: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
T sinAngle = Math::sin(angle*T(0.5)); |
||||||
|
T tanAngle = Math::tan(angle*T(0.5)); |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool sphereConeView(const Vector3<T>& sphereCenter, T sphereRadius, const Matrix4<T>& coneView, T sinAngle, T tanAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a sphere and a cone |
||||||
|
@param sphereCenter Sphere center |
||||||
|
@param sphereRadius Sphere radius |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param coneAngle Apex angle of the cone (@f$ 0 < \Theta < \pi @f$) |
||||||
|
@return @cpp true @ce if the sphere intersects with the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref sphereCone(const Vector3<T>& sphereCenter, T, const Vector3<T>&, const Vector3<T>&, T, T). |
||||||
|
*/ |
||||||
|
template<class T> bool sphereCone(const Vector3<T>& sphereCenter, T sphereRadius, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a sphere and a cone using precomputed values |
||||||
|
@param sphereCenter Sphere center |
||||||
|
@param sphereRadius Sphere radius |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param sinAngle Precomputed sine of half the cone's opening angle |
||||||
|
@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation |
||||||
|
@return @cpp true @ce if the sphere intersects with the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Offsets the cone plane by @f$ -r\sin{\frac{\Theta}{2}} \cdot \boldsymbol n @f$ |
||||||
|
(with @f$ \Theta @f$ being the cone apex angle) which separates two |
||||||
|
half-spaces: In front of the plane, in which the sphere cone intersection test |
||||||
|
is equivalent to testing the sphere's center against a similarly offset cone |
||||||
|
(which is equivalent the cone with surface expanded by @f$ r @f$ in surface |
||||||
|
normal direction), and behind the plane, where the test is equivalent to |
||||||
|
testing whether the origin of the original cone intersects the sphere. The |
||||||
|
@p sinAngle and @p tanAngleSqPlusOne parameters can be precomputed like this: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
T sinAngle = Math::sin(angle*T(0.5)); |
||||||
|
T tanAngleSqPlusOne = Math::pow<2>(Math::tan(angle*T(0.5))) + T(1); |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool sphereCone(const Vector3<T>& sphereCenter, T sphereRadius, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, T sinAngle, T tanAngleSqPlusOne); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of an axis aligned bounding box and a cone |
||||||
|
@param aabbCenter Center of the AABB |
||||||
|
@param aabbExtents (Half-)extents of the AABB |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param coneAngle Apex angle of the cone (@f$ 0 < \Theta < \pi @f$) |
||||||
|
@return @cpp true @ce if the box intersects the cone, @cpp false @ce otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref aabbCone(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&, const Vector3<T>&, T). |
||||||
|
*/ |
||||||
|
template<class T> bool aabbCone(const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of an axis aligned bounding box and a cone using precomputed values |
||||||
|
@param aabbCenter Center of the AABB |
||||||
|
@param aabbExtents (Half-)extents of the AABB |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation |
||||||
|
@return @cpp true @ce if the box intersects the cone, @cpp false @ce otherwise |
||||||
|
|
||||||
|
On each axis finds the intersection points of the cone's axis with infinite |
||||||
|
planes obtained by extending the two faces of the box that are perpendicular to |
||||||
|
that axis. The intersection points on the planes perpendicular to axis |
||||||
|
@f$ a \in \{ 0, 1, 2 \} @f$ are given by @f[ |
||||||
|
\boldsymbol i = \boldsymbol n \cdot \frac{(\boldsymbol c_a - \boldsymbol o_a) \pm \boldsymbol e_a}{\boldsymbol n_a} |
||||||
|
@f] |
||||||
|
|
||||||
|
with normal @f$ \boldsymbol n @f$, cone origin @f$ \boldsymbol o @f$, box |
||||||
|
center @f$ \boldsymbol c @f$ and box extents @f$ \boldsymbol e @f$. The points |
||||||
|
on the faces that are closest to this intersection point are the closest to the |
||||||
|
cone's axis and are tested for intersection with the cone using |
||||||
|
@ref pointCone(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&, const T) "pointCone()". As soon as an intersecting point is found, the function returns |
||||||
|
@cpp true @ce. If all points lie outside of the cone, it will return |
||||||
|
@cpp false @ce. |
||||||
|
|
||||||
|
The @p tanAngleSqPlusOne parameter can be precomputed like this: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
T tanAngleSqPlusOne = Math::pow<2>(Math::tan(angle*T(0.5))) + T(1); |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool aabbCone(const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, T tanAngleSqPlusOne); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a range and a cone |
||||||
|
@param range Range |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param coneAngle Apex angle of the cone (@f$ 0 < \Theta < \pi @f$) |
||||||
|
@return @cpp true @ce if the range intersects the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Precomputes a portion of the intersection equation from @p coneAngle and calls |
||||||
|
@ref rangeCone(const Range3D<T>&, const Vector3<T>&, const Vector3<T>&, T). |
||||||
|
*/ |
||||||
|
template<class T> bool rangeCone(const Range3D<T>& range, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle); |
||||||
|
|
||||||
|
/**
|
||||||
|
@brief Intersection of a range and a cone using precomputed values |
||||||
|
@param range Range |
||||||
|
@param coneOrigin Cone origin |
||||||
|
@param coneNormal Cone normal |
||||||
|
@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation |
||||||
|
@return @cpp true @ce if the range intersects the cone, @cpp false @ce |
||||||
|
otherwise |
||||||
|
|
||||||
|
Converts the range into center/extents representation and passes it on to |
||||||
|
@ref aabbCone(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&, const Vector3<T>&, T) "aabbCone()". The @p tanAngleSqPlusOne parameter can be precomputed like this: |
||||||
|
|
||||||
|
@code{.cpp} |
||||||
|
T tanAngleSqPlusOne = Math::pow<2>(Math::tan(angle*T(0.5))) + T(1); |
||||||
|
@endcode |
||||||
|
*/ |
||||||
|
template<class T> bool rangeCone(const Range3D<T>& range, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const T tanAngleSqPlusOne); |
||||||
|
|
||||||
|
template<class T> bool pointFrustum(const Vector3<T>& point, const Frustum<T>& frustum) { |
||||||
|
for(const Vector4<T>& plane: frustum.planes()) { |
||||||
|
/* The point is in front of one of the frustum planes (normals point
|
||||||
|
outwards) */ |
||||||
|
if(Distance::pointPlaneScaled<T>(point, plane) < T(0)) |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool rangeFrustum(const Range3D<T>& range, const Frustum<T>& frustum) { |
||||||
|
/* Convert to center/extent, avoiding division by 2 and instead comparing
|
||||||
|
to 2*-plane.w() later */ |
||||||
|
const Vector3<T> center = range.min() + range.max(); |
||||||
|
const Vector3<T> extent = range.max() - range.min(); |
||||||
|
|
||||||
|
for(const Vector4<T>& plane: frustum.planes()) { |
||||||
|
const Vector3<T> absPlaneNormal = Math::abs(plane.xyz()); |
||||||
|
|
||||||
|
const Float d = Math::dot(center, plane.xyz()); |
||||||
|
const Float r = Math::dot(extent, absPlaneNormal); |
||||||
|
if(d + r < -T(2)*plane.w()) return false; |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool aabbFrustum(const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, const Frustum<T>& frustum) { |
||||||
|
for(const Vector4<T>& plane: frustum.planes()) { |
||||||
|
const Vector3<T> absPlaneNormal = Math::abs(plane.xyz()); |
||||||
|
|
||||||
|
const Float d = Math::dot(aabbCenter, plane.xyz()); |
||||||
|
const Float r = Math::dot(aabbExtents, absPlaneNormal); |
||||||
|
if(d + r < -plane.w()) return false; |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool sphereFrustum(const Vector3<T>& sphereCenter, const T sphereRadius, const Frustum<T>& frustum) { |
||||||
|
const T radiusSq = sphereRadius*sphereRadius; |
||||||
|
|
||||||
|
for(const Vector4<T>& plane: frustum.planes()) { |
||||||
|
/* The sphere is in front of one of the frustum planes (normals point
|
||||||
|
outwards) */ |
||||||
|
if(Distance::pointPlaneScaled<T>(sphereCenter, plane) < -radiusSq) |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
template<class T> bool pointCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle) { |
||||||
|
const T tanAngleSqPlusOne = Math::pow<2>(Math::tan(coneAngle*T(0.5))) + T(1); |
||||||
|
return pointCone(point, coneOrigin, coneNormal, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool pointCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const T tanAngleSqPlusOne) { |
||||||
|
const Vector3<T> c = point - coneOrigin; |
||||||
|
const T lenA = dot(c, coneNormal); |
||||||
|
|
||||||
|
return lenA >= 0 && c.dot() <= lenA*lenA*tanAngleSqPlusOne; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool pointDoubleCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle) { |
||||||
|
const T tanAngleSqPlusOne = Math::pow<2>(Math::tan(coneAngle*T(0.5))) + T(1); |
||||||
|
return pointDoubleCone(point, coneOrigin, coneNormal, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool pointDoubleCone(const Vector3<T>& point, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const T tanAngleSqPlusOne) { |
||||||
|
const Vector3<T> c = point - coneOrigin; |
||||||
|
const T lenA = dot(c, coneNormal); |
||||||
|
|
||||||
|
return c.dot() <= lenA*lenA*tanAngleSqPlusOne; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool sphereConeView(const Vector3<T>& sphereCenter, const T sphereRadius, const Matrix4<T>& coneView, const Rad<T> coneAngle) { |
||||||
|
const Rad<T> halfAngle = coneAngle*T(0.5); |
||||||
|
const T sinAngle = Math::sin(halfAngle); |
||||||
|
const T tanAngle = Math::tan(halfAngle); |
||||||
|
|
||||||
|
return sphereConeView(sphereCenter, sphereRadius, coneView, sinAngle, tanAngle); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool sphereConeView(const Vector3<T>& sphereCenter, const T sphereRadius, const Matrix4<T>& coneView, const T sinAngle, const T tanAngle) { |
||||||
|
CORRADE_ASSERT(coneView.isRigidTransformation(), "Math::Geometry::Intersection::sphereConeView(): coneView does not represent a rigid transformation", false); |
||||||
|
|
||||||
|
/* Transform the sphere so that we can test against Z axis aligned origin
|
||||||
|
cone instead */ |
||||||
|
const Vector3<T> center = coneView.transformPoint(sphereCenter); |
||||||
|
|
||||||
|
/* Test against plane which determines whether to test against shifted cone
|
||||||
|
or center-sphere */ |
||||||
|
if (-center.z() > -sphereRadius*sinAngle) { |
||||||
|
/* Point - axis aligned cone test, shifted so that the cone's surface
|
||||||
|
is extended by the radius of the sphere */ |
||||||
|
const T coneRadius = tanAngle*(center.z() - sphereRadius/sinAngle); |
||||||
|
return center.xy().dot() <= coneRadius*coneRadius; |
||||||
|
} else { |
||||||
|
/* Simple sphere point check */ |
||||||
|
return center.dot() <= sphereRadius*sphereRadius; |
||||||
|
} |
||||||
|
|
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool sphereCone( |
||||||
|
const Vector3<T>& sphereCenter, const T sphereRadius, |
||||||
|
const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle) |
||||||
|
{ |
||||||
|
const Rad<T> halfAngle = coneAngle*T(0.5); |
||||||
|
const T sinAngle = Math::sin(halfAngle); |
||||||
|
const T tanAngleSqPlusOne = T(1) + Math::pow<T>(Math::tan<T>(halfAngle), T(2)); |
||||||
|
|
||||||
|
return sphereCone(sphereCenter, sphereRadius, coneOrigin, coneNormal, sinAngle, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool sphereCone( |
||||||
|
const Vector3<T>& sphereCenter, const T sphereRadius, |
||||||
|
const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, |
||||||
|
const T sinAngle, const T tanAngleSqPlusOne) |
||||||
|
{ |
||||||
|
const Vector3<T> diff = sphereCenter - coneOrigin; |
||||||
|
|
||||||
|
/* Point - cone test */ |
||||||
|
if(Math::dot(diff - sphereRadius*sinAngle*coneNormal, coneNormal) > T(0)) { |
||||||
|
const Vector3<T> c = sinAngle*diff + coneNormal*sphereRadius; |
||||||
|
const T lenA = Math::dot(c, coneNormal); |
||||||
|
|
||||||
|
return c.dot() <= lenA*lenA*tanAngleSqPlusOne; |
||||||
|
|
||||||
|
/* Simple sphere point check */ |
||||||
|
} else return diff.dot() <= sphereRadius*sphereRadius; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool aabbCone( |
||||||
|
const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, |
||||||
|
const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle) |
||||||
|
{ |
||||||
|
const T tanAngleSqPlusOne = Math::pow<T>(Math::tan<T>(coneAngle*T(0.5)), T(2)) + T(1); |
||||||
|
return aabbCone(aabbCenter, aabbExtents, coneOrigin, coneNormal, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool aabbCone( |
||||||
|
const Vector3<T>& aabbCenter, const Vector3<T>& aabbExtents, |
||||||
|
const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const T tanAngleSqPlusOne) |
||||||
|
{ |
||||||
|
const Vector3<T> c = aabbCenter - coneOrigin; |
||||||
|
|
||||||
|
for(const Int axis: {0, 1, 2}) { |
||||||
|
const Int z = axis; |
||||||
|
const Int x = (axis + 1) % 3; |
||||||
|
const Int y = (axis + 2) % 3; |
||||||
|
if(coneNormal[z] != T(0)) { |
||||||
|
const Float t0 = ((c[z] - aabbExtents[z])/coneNormal[z]); |
||||||
|
const Float t1 = ((c[z] + aabbExtents[z])/coneNormal[z]); |
||||||
|
|
||||||
|
const Vector3<T> i0 = coneNormal*t0; |
||||||
|
const Vector3<T> i1 = coneNormal*t1; |
||||||
|
|
||||||
|
for(const auto& i : {i0, i1}) { |
||||||
|
Vector3<T> closestPoint = i; |
||||||
|
|
||||||
|
if(i[x] - c[x] > aabbExtents[x]) { |
||||||
|
closestPoint[x] = c[x] + aabbExtents[x]; |
||||||
|
} else if(i[x] - c[x] < -aabbExtents[x]) { |
||||||
|
closestPoint[x] = c[x] - aabbExtents[x]; |
||||||
|
} /* Else: normal intersects within x bounds */ |
||||||
|
|
||||||
|
if(i[y] - c[y] > aabbExtents[y]) { |
||||||
|
closestPoint[y] = c[y] + aabbExtents[y]; |
||||||
|
} else if(i[y] - c[y] < -aabbExtents[y]) { |
||||||
|
closestPoint[y] = c[y] - aabbExtents[y]; |
||||||
|
} /* Else: normal intersects within Y bounds */ |
||||||
|
|
||||||
|
/* Found a point in cone and aabb */ |
||||||
|
if(pointCone<T>(closestPoint, {}, coneNormal, tanAngleSqPlusOne)) |
||||||
|
return true; |
||||||
|
} |
||||||
|
} /* Else: normal will intersect one of the other planes */ |
||||||
|
} |
||||||
|
|
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool rangeCone(const Range3D<T>& range, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const Rad<T> coneAngle) { |
||||||
|
const T tanAngleSqPlusOne = Math::pow<2>(Math::tan(coneAngle*T(0.5))) + T(1); |
||||||
|
return rangeCone(range, coneOrigin, coneNormal, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
template<class T> bool rangeCone(const Range3D<T>& range, const Vector3<T>& coneOrigin, const Vector3<T>& coneNormal, const T tanAngleSqPlusOne) { |
||||||
|
const Vector3<T> center = (range.min() + range.max())*T(0.5); |
||||||
|
const Vector3<T> extents = (range.max() - range.min())*T(0.5); |
||||||
|
return aabbCone(center, extents, coneOrigin, coneNormal, tanAngleSqPlusOne); |
||||||
|
} |
||||||
|
|
||||||
|
}}} |
||||||
|
|
||||||
|
#endif |
||||||
Loading…
Reference in new issue