diff --git a/src/Math/Geometry/Distance.h b/src/Math/Geometry/Distance.h index b86b5aa71..9073520a8 100644 --- a/src/Math/Geometry/Distance.h +++ b/src/Math/Geometry/Distance.h @@ -19,6 +19,8 @@ * @brief Class Magnum::Math::Geometry::Distance */ +#include "Math/Math.h" +#include "Math/Matrix.h" #include "Math/Vector3.h" namespace Magnum { namespace Math { namespace Geometry { @@ -27,7 +29,39 @@ namespace Magnum { namespace Math { namespace Geometry { class Distance { public: /** - * @brief %Distance of line and point + * @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 *d* is computed from point **p** and line defined by **a** + * and **b** using @ref Matrix::determinant() "determinant": @f[ + * d = \frac{|det(b - a a - point)|} {|b - a|} + * @f] + * Source: http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html + * @see linePointSquared(const Vector2&, const Vector2&, const Vector2&) + */ + template inline static T linePoint(const Vector2& a, const Vector2& b, const Vector2& point) { + return std::abs(Matrix<2, T>::from(b - a, a - point).determinant())/(b - a).length(); + } + + /** + * @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 linePoint(const Vector2&, const Vector2&, const Vector2&) + * for comparing distance with other values, because it doesn't + * compute the square root. + */ + template inline static T linePointSquared(const Vector2& a, const Vector2& b, const Vector2& point) { + Vector2 bMinusA = b - a; + return Math::pow<2>(Matrix<2, T>::from(bMinusA, a - point).determinant())/bMinusA.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 @@ -37,25 +71,26 @@ class Distance { * d = \frac{|(\boldsymbol p - \boldsymbol a) \times (\boldsymbol p - \boldsymbol b)|} * {|\boldsymbol b - \boldsymbol a|} * @f] - * - * @see linePointSquared() + * Source: http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html + * @see linePointSquared(const Vector3&, const Vector3&, const Vector3&) */ template inline static T linePoint(const Vector3& a, const Vector3& b, const Vector3& point) { - return sqrt(linePointSquared(a, b, point)); + return std::sqrt(linePointSquared(a, b, point)); } /** - * @brief %Distance of line and point, squared + * @brief %Distance of line and point in 3D, squared * - * More efficient than linePoint() for comparing distance with other - * values, because it doesn't compute the square root. + * More efficient than linePoint(const Vector3&, const Vector3&, const Vector3&) + * for comparing distance with other values, because it doesn't + * compute the square root. */ template static T linePointSquared(const Vector3& a, const Vector3& b, const Vector3& point) { return Vector3::cross(point - a, point - b).dot()/(b - a).dot(); } /** - * @brief %Dístance of point from line segment + * @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 @@ -80,16 +115,73 @@ class Distance { * * @see lineSegmentPointSquared() */ - template inline static T lineSegmentPoint(const Vector3& a, const Vector3& b, const Vector3& point) { - return sqrt(lineSegmentPointSquared(a, b, point)); + template inline static T lineSegmentPoint(const Vector2& a, const Vector2& b, const Vector2& point) { + Vector2 pointMinusA = point - a; + Vector2 pointMinusB = point - b; + Vector2 bMinusA = b - a; + T pointDistanceA = pointMinusA.dot(); + T pointDistanceB = pointMinusB.dot(); + 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(Matrix<2, T>::from(bMinusA, -pointMinusA).determinant())/std::sqrt(bDistanceA); } /** - * @brief %Distance of point from line segment, squared + * @brief %Distance of point from line segment in 2D, squared * * More efficient than lineSegmentPoint() for comparing distance with * other values, because it doesn't compute the square root. */ + template static T lineSegmentPointSquared(const Vector2& a, const Vector2& b, const Vector2& point) { + Vector2 pointMinusA = point - a; + Vector2 pointMinusB = point - b; + Vector2 bMinusA = b - a; + T pointDistanceA = pointMinusA.dot(); + T pointDistanceB = pointMinusB.dot(); + 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>(Matrix<2, T>::from(bMinusA, -pointMinusA).determinant())/bDistanceA; + } + + /** + * @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 + * lineSegmentPoint(const Vector2&, const Vector2&, const Vector2&). + * + * @see lineSegmentPointSquared(const Vector3&, const Vector3&, const Vector3&) + */ + template inline static T lineSegmentPoint(const Vector3& a, const Vector3& b, const Vector3& point) { + return std::sqrt(lineSegmentPointSquared(a, b, point)); + } + + /** + * @brief %Distance of point from line segment in 3D, squared + * + * More efficient than lineSegmentPoint(const Vector3&, const Vector3&, const Vector3&) for comparing distance with + * other values, because it doesn't compute the square root. + */ template static T lineSegmentPointSquared(const Vector3& a, const Vector3& b, const Vector3& point) { Vector3 pointMinusA = point - a; Vector3 pointMinusB = point - b; diff --git a/src/Math/Geometry/Test/DistanceTest.cpp b/src/Math/Geometry/Test/DistanceTest.cpp index 240ad21eb..ace9c485a 100644 --- a/src/Math/Geometry/Test/DistanceTest.cpp +++ b/src/Math/Geometry/Test/DistanceTest.cpp @@ -26,14 +26,35 @@ using namespace std; namespace Magnum { namespace Math { namespace Geometry { namespace Test { +typedef Magnum::Math::Vector2 Vector2; typedef Magnum::Math::Vector3 Vector3; DistanceTest::DistanceTest() { - addTests(&DistanceTest::linePoint, - &DistanceTest::lineSegmentPoint); + addTests(&DistanceTest::linePoint2D, + &DistanceTest::linePoint3D, + &DistanceTest::lineSegmentPoint2D, + &DistanceTest::lineSegmentPoint3D); } -void DistanceTest::linePoint() { +void DistanceTest::linePoint2D() { + Vector2 a(0.0f); + Vector2 b(1.0f); + + /* Point on the line */ + CORRADE_COMPARE((Distance::linePoint(a, b, Vector2(0.25f))), 0.0f); + + /* The distance should be the same for all equidistant points */ + CORRADE_COMPARE((Distance::linePoint(a, b, Vector2(1.0f, 0.0f))), + 1.0f/Constants::sqrt2()); + CORRADE_COMPARE((Distance::linePoint(a, b, Vector2(1.0f, 0.0f)+Vector2(100.0f))), + 1.0f/Constants::sqrt2()); + + /* Be sure that *Squared() works the same, as it has slightly different implementation */ + CORRADE_COMPARE((Distance::linePointSquared(a, b, Vector2(1.0f, 0.0f))), + 0.5f); +} + +void DistanceTest::linePoint3D() { Vector3 a(0.0f); Vector3 b(1.0f); @@ -47,7 +68,38 @@ void DistanceTest::linePoint() { Constants::sqrt2()/Constants::sqrt3()); } -void DistanceTest::lineSegmentPoint() { +void DistanceTest::lineSegmentPoint2D() { + Vector2 a(0.0f); + Vector2 b(1.0f); + + /* Point on the line segment */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(0.25f))), 0.0f); + + /* Point on the line, outside the segment, closer to A */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(-1.0f))), Constants::sqrt2()); + /* Be sure that *Squared() works the same, as it has slightly different implementation */ + CORRADE_COMPARE((Distance::lineSegmentPointSquared(a, b, Vector2(-1.0f))), 2.0f); + + /* Point on the line, outside the segment, closer to B */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(1.0f+1.0f/Constants::sqrt2()))), 1.0f); + CORRADE_COMPARE((Distance::lineSegmentPointSquared(a, b, Vector2(1.0f+1.0f/Constants::sqrt2()))), 1.0f); + + /* Point next to the line segment */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(1.0f, 0.0f))), + 1.0f/Constants::sqrt2()); + CORRADE_COMPARE((Distance::lineSegmentPointSquared(a, b, Vector2(1.0f, 0.0f))), + 0.5f); + + /* Point outside the line segment, closer to A */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(1.0f, 0.0f)-Vector2(1.0f, 0.5f))), 0.5f); + CORRADE_COMPARE((Distance::lineSegmentPointSquared(a, b, Vector2(1.0f, 0.0f)-Vector2(1.0f, 0.5f))), 0.25f); + + /* Point outside the line segment, closer to B */ + CORRADE_COMPARE((Distance::lineSegmentPoint(a, b, Vector2(1.0f, 0.0f)+Vector2(0.5f, 1.0f))), 0.5f); + CORRADE_COMPARE((Distance::lineSegmentPointSquared(a, b, Vector2(1.0f, 0.0f)+Vector2(0.5f, 1.0f))), 0.25f); +} + +void DistanceTest::lineSegmentPoint3D() { Vector3 a(0.0f); Vector3 b(1.0f); diff --git a/src/Math/Geometry/Test/DistanceTest.h b/src/Math/Geometry/Test/DistanceTest.h index 0cae07036..dc7c7bc88 100644 --- a/src/Math/Geometry/Test/DistanceTest.h +++ b/src/Math/Geometry/Test/DistanceTest.h @@ -23,8 +23,10 @@ class DistanceTest: public Corrade::TestSuite::Tester { public: DistanceTest(); - void linePoint(); - void lineSegmentPoint(); + void linePoint2D(); + void linePoint3D(); + void lineSegmentPoint2D(); + void lineSegmentPoint3D(); }; }}}}