Thea
LineN.hpp
1 //============================================================================
2 //
3 // This file is part of the Thea toolkit.
4 //
5 // This software is distributed under the BSD license, as detailed in the
6 // accompanying LICENSE.txt file. Portions are derived from other works:
7 // their respective licenses and copyright information are reproduced in
8 // LICENSE.txt and/or in the relevant source files.
9 //
10 // Author: Siddhartha Chaudhuri
11 // First version: 2011
12 //
13 //============================================================================
14
15 #ifndef __Thea_LineN_hpp__
16 #define __Thea_LineN_hpp__
17
18 #include "Common.hpp"
19 #include "Math.hpp"
20 #include "MatVec.hpp"
21
22 namespace Thea {
23
24 // Forward declarations
25 template <int N, typename T> class LineN;
26
27 namespace Internal {
28
35 template <int N, typename T>
36 class /* THEA_DLL_LOCAL */ LineNBase
37 {
38  public:
39  typedef LineN<N, T> LineT;
41
42  THEA_DECL_SMART_POINTERS(LineT)
43
44
48  static LineT fromPointAndDirection(VectorT const & point_, VectorT const & direction_, bool normalize = true)
49  {
50  if (Math::fuzzyEq(direction_.squaredNorm(), static_cast<T>(0)))
51  throw Error("LineN: Direction vector has zero (or nearly zero) length");
52
53  LineT line;
54  line.point = point_;
55  line.direction = direction_; if (normalize) line.direction.normalize();
56  return line;
57  }
58
60  static LineT fromTwoPoints(VectorT const & point1, VectorT const & point2)
61  {
62  return fromPointAndDirection(point1, point2 - point1);
63  }
64
66  template <typename U> LineN<N, U> cast() const
67  {
68  return LineN<N, U>::fromPointAndDirection(point.template cast<U>(), direction.template cast<U>(),
69  /* normalize = */ false);
70  }
71
73  VectorT const & getPoint() const { return point; }
74
76  VectorT const & getDirection() const { return direction; }
77
79  T distance(VectorT const & p) const
80  {
81  return std::sqrt(squaredDistance(p));
82  }
83
85  T squaredDistance(VectorT const & p) const
86  {
87  return (p - closestPoint(p)).squaredNorm();
88  }
89
91  VectorT closestPoint(VectorT const & p) const
92  {
93  T t = direction.dot(p - point);
94  return point + t * direction;
95  }
96
98  T distance(LineT const & other) const
99  {
100  return std::sqrt(squaredDistance(other));
101  }
102
107  T squaredDistance(LineT const & other, VectorT * this_pt = nullptr, VectorT * other_pt = nullptr) const
108  {
109  // Adapted from Christer Ericson, "Real-Time Collision Detection", Morgan-Kaufman, 2005.
110
111  VectorT r = point - other.point;
112  T b = direction.dot(other.direction);
113  T f = other.direction.dot(r);
114  T denom = 1 - b * b; // always nonnegative
115
116  // If segments not parallel, compute closest point on L1 to L2. Else pick arbitrary s (here 0).
117  T s = 0;
118  if (Math::fuzzyGt(denom, static_cast<T>(0)))
119  {
120  T c = direction.dot(r);
121  s = (b * f - c) / denom;
122  }
123
124  VectorT c1 = point + s * direction;
125  if (this_pt)
126  *this_pt = c1;
127
128  // Compute point on L2 closest to S1(s) using t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f)
129  T t = b * s + f;
130  VectorT c2 = other.point + t * direction;
131  if (other_pt)
132  *other_pt = c2;
133
134  return (c1 - c2).squaredNorm();
135  }
136
138  std::string toString() const
139  {
140  std::ostringstream oss;
141  oss << "[P: " << Thea::toString(point) << ", U: " << Thea::toString(direction) << ']';
142  return oss.str();
143  }
144
145  private:
146  VectorT point;
147  VectorT direction;
148
149 }; // class LineNBase
150
151 } // namespace Internal
152
154 template <int N, typename T = Real>
155 class /* THEA_API */ LineN : public Internal::LineNBase<N, T>
156 {
157 }; // class LineN
158
159 } // namespace Thea
160
161 #include "Line2.hpp"
162 #include "Line3.hpp"
163
164 #endif
Eigen::Matrix< T, Size, 1, Options|((Options &Eigen::DontAlign)==0 &&Size==Eigen::Dynamic?Eigen::AutoAlign:Eigen::DontAlign), MaxRowsAtCompileTime, 1 > Vector
General 1D dense column vector template, alias for Eigen::Matrix<T, Size, 1,...>, with a custom align...
Definition: MatVec.hpp:146
Vector< N, T > VectorT
N-dimensional vector.
Definition: LineN.hpp:40
VectorT const & getPoint() const
Get a point on the line.
Definition: LineN.hpp:73
std::string toString(char const *s)
Convert a C-string to a std::string object, returning an empty string if the input is a null pointer...
Root namespace for the Thea library.
VectorT closestPoint(VectorT const &p) const
Get the point on the line closest to a given point.
Definition: LineN.hpp:91
LineN< N, U > cast() const
Cast the line to a different scalar type.
Definition: LineN.hpp:66
std::string toString() const
Get a textual description of the line.
Definition: LineN.hpp:138
T squaredDistance(LineT const &other, VectorT *this_pt=nullptr, VectorT *other_pt=nullptr) const
Get the point on this line and the point on another line closest to each other, and return the square...
Definition: LineN.hpp:107
T squaredDistance(VectorT const &p) const
Get the square of the distance of the line from a given point.
Definition: LineN.hpp:85
static LineT fromPointAndDirection(VectorT const &point_, VectorT const &direction_, bool normalize=true)
Construct a line from a point on it, and the direction vector of the line (need not be a unit vector)...
Definition: LineN.hpp:48
static LineT fromTwoPoints(VectorT const &point1, VectorT const &point2)
Construct a line from two points on it.
Definition: LineN.hpp:60
T distance(LineT const &other) const
Get the distance of this line from another line.
Definition: LineN.hpp:98
bool fuzzyEq(T const &a, T const &b, T const &tol)
Check if two numbers are approximately equal, with a given tolerance.
Definition: Math.hpp:210
bool fuzzyGt(T const &a, T const &b, T const &tol)
Check if a is strictly greater than b by at least a minimum value, with default tolerance.
Definition: Math.hpp:225
T distance(VectorT const &p) const
Get the distance of the line from a given point.
Definition: LineN.hpp:79
A straight line in N-dimensional space, where N is any positive (non-zero) integer and T is a field...
Definition: LineN.hpp:25
VectorT const & getDirection() const
Get the unit direction vector of the line.
Definition: LineN.hpp:76
LineN< N, T > LineT
N-dimensional straight line.
Definition: LineN.hpp:39
std::runtime_error Error
An error class.
Definition: Error.hpp:27
[Internal] Base class for straight lines in N-dimensional space, where N is any positive (non-zero) i...
Definition: LineN.hpp:36