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