Mathter
A configurable 3D math library for game developers.
Geometry.hpp
Go to the documentation of this file.
1 //==============================================================================
2 // This software is distributed under The Unlicense.
3 // For more information, please refer to <http://unlicense.org/>
4 //==============================================================================
5 
6 #pragma once
7 
8 #include <cmath>
9 #include <limits>
10 #include <array>
11 #include "Vector.hpp"
12 
13 
14 namespace mathter {
15 
16 
17 //------------------------------------------------------------------------------
18 // Shapes
19 //------------------------------------------------------------------------------
20 
21 template <class T, int Dim>
22 class Hyperplane;
23 
24 template <class T, int Dim>
25 class Line {
26  using VectorT = Vector<T, Dim>;
27 public:
29  Line() = default;
30 
34  Line(const VectorT& base, const VectorT& direction) : direction(direction), base(base) {
35  assert(IsNormalized(direction));
36  }
37 
41  static Line Through(const VectorT& point1, const VectorT& point2) {
42  return Line(point1, SafeNormalize(point2 - point1));
43  }
44 
46  Line(const Hyperplane<T, 2>& plane);
47 
49  VectorT Direction() const { return direction; }
50 
52  VectorT Base() const { return base; }
53 
55  VectorT PointAt(T param) const { return base + param*direction; }
56 public:
58 };
59 
60 
61 
62 template <class T, int Dim>
63 class LineSegment {
64  using VectorT = Vector<T, Dim>;
65 public:
66  LineSegment() : point1(0), point2(0) {
67  point2(0) = 1;
68  }
69  LineSegment(const VectorT& base, const VectorT& direction, T length) {
70  point1 = base;
71  point2 = base + direction*length;
72  }
73  LineSegment(const VectorT& point1, const VectorT& point2) : point1(point1), point2(point2) {}
74 
75  T Length() const { return mathter::Length(point2 - point1); }
76  VectorT Direction() const { return Normalize(point2 - point1); }
77  VectorT Start() const { return point1; }
78  VectorT End() const { return point2; }
79  VectorT Interpol(T t) const { return t*point2 + (T(1) - t)*point1; }
80 
82  return mathter::Line<T, Dim>{point1, Direction()};
83  }
84 public:
85  VectorT point1, point2;
86 };
87 
88 
89 template <class T, int Dim>
90 class Ray : protected Line<T, Dim> {
91 public:
92  // Inhreitance is protected to deny casting.
93  // Casting is bad 'cause we don't want to implicit cast a Ray to a Line and Intersect() it with a plane.
94  using Line<T, Dim>::Line;
97  using Line<T, Dim>::Base;
100  using Line<T, Dim>::base;
102  return static_cast<mathter::Line<T, Dim>>(*this);
103  }
104 };
105 
106 
107 
108 template <class T, int Dim>
109 class Hyperplane {
110  using VectorT = Vector<T, Dim>;
111 public:
112  Hyperplane() : normal(0), scalar(0) { normal(0) = 1; }
113  Hyperplane(const VectorT& base, const VectorT& normal) : normal(normal) {
114  assert(IsNormalized(normal));
115  scalar = Dot(normal, base);
116  }
117  Hyperplane(const VectorT& normal, T scalar) : normal(normal), scalar(scalar) {
118  assert(IsNormalized(normal));
119  }
120  Hyperplane(const Line<T, 2>& line) {
121  static_assert(Dim == 2, "Plane dimension must be two, which is a line.");
122  normal = { -line.Direction()(1), line.Direction()(0) };
123  scalar = Dot(normal, line.Base());
124  }
125 
126  const VectorT& Normal() const { return normal; }
127  T Scalar() const { return scalar; }
128 
129  template <bool Packed>
131  return Dot(point, normal) - scalar;
132  }
133 private:
134  VectorT normal;
135  T scalar;
136 };
137 
138 
139 template <class T, int Dim>
141  static_assert(Dim == 2, "Line dimension must be two, since it a plane in 2 dimensional space.");
142 
143  // Intersect plane's line with line through origo perpendicular to plane to find suitable base
144  T a = plane.Normal()(0);
145  T b = plane.Normal()(1);
146  T d = plane.Scalar();
147  T div = (a*a + b*b);
148  base = { a*d / div, b*d / div };
149  direction = { b, -a };
150 }
151 
152 
153 template <class T>
154 class Triangle3D {
155 public:
156  Triangle3D() = default;
158  a(a), b(b), c(c) {}
159  Vector<T, 3, false> a, b, c; // Corners of the traingle.
160 };
161 
162 
163 //------------------------------------------------------------------------------
164 // Intersections
165 //------------------------------------------------------------------------------
166 
167 
168 template <class T, class U>
170 
171 
172 
173 template <class T, class U>
174 auto Intersect(const T& t, const U& u) {
175  return Intersection<T, U>(t, u);
176 }
177 
178 
179 // Plane-line intersection
180 template <class T, int Dim>
181 class Intersection<Hyperplane<T, Dim>, Line<T, Dim>> {
182 protected:
183  using PlaneT = Hyperplane<T, Dim>;
184  using LineT = Line<T, Dim>;
185  using VectorT = Vector<T, Dim>;
186 public:
187  Intersection(const PlaneT& plane, const LineT& line);
188 
189  bool Intersecting() const { return !std::isinf(param); }
190  VectorT Point() const { return line.PointAt(param); }
191  T LineParameter() const { return param; }
192 private:
193  LineT line;
194  T param;
195 };
196 
197 template <class T, int Dim>
198 class Intersection<Line<T, Dim>, Hyperplane<T, Dim>> : public Intersection<Hyperplane<T, Dim>, Line<T, Dim>> {
199  using PlaneT = Hyperplane<T, Dim>;
200  using LineT = Line<T, Dim>;
201 public:
202  Intersection(const LineT& line, const PlaneT& plane) : Intersection<Hyperplane<T, Dim>, Line<T, Dim>>(plane, line) {}
203 };
204 
205 
206 
207 // Plane-line segment intersection
208 template <class T, int Dim>
209 class Intersection<Hyperplane<T, Dim>, LineSegment<T, Dim>> {
210  using PlaneT = Hyperplane<T, Dim>;
211  using LineT = LineSegment<T, Dim>;
212  using VectorT = Vector<T, Dim>;
213 public:
214  Intersection(const PlaneT& plane, const LineT& line) {
215  lineSegment = line;
216  auto intersection = Intersect(plane, line.Line());
217  param = intersection.LineParameter() / line.Length();
218  }
219 
220  bool Intersecting() const { return T(0) <= param && param <= T(1); }
221  VectorT Point() const { return lineSegment.Interpol(param); }
222  T InterpolParameter() const { return param; }
223  T LineParameter() const { return param * lineSegment.Length(); }
224 private:
225  LineT lineSegment;
226  T param;
227 };
228 
229 template <class T, int Dim>
230 class Intersection<LineSegment<T, Dim>, Hyperplane<T, Dim>> : public Intersection<Hyperplane<T, Dim>, LineSegment<T, Dim>> {
231  using PlaneT = Hyperplane<T, Dim>;
232  using LineT = LineSegment<T, Dim>;
233 public:
234  Intersection(const LineT& line, const PlaneT& plane) : Intersection<Hyperplane<T, Dim>, LineSegment<T, Dim>>(plane, line) {}
235 };
236 
237 
238 
239 template <class T, int Dim>
241  this->line = line;
242 
243  // We have to solve the system of linear equations for x,y,z,t
244  // |d | |a b c 0| |x|
245  // |px| |1 0 0 p| |y|
246  // |py| = |0 1 0 q| * |z|
247  // |pz| |0 0 1 r| |t|
248  // b = A * x
249  // where [px,py,pz] + t*[p,q,r] = [x,y,z] is the line's equation
250  // and ax + by + cz + d = 0 is the plane's equation
251 
253  Vector<T, Dim + 1> A_inv_t;
254 
255  // Fill up 'b'
256  b = -plane.Scalar() | line.Base();
257 
258  // Fill up 'A_inv_t', which is the last line of A^-1, used to calculate 't'
259  A_inv_t = 1 | plane.Normal();
260 
261  // Compute result of the equation
262  T scaler = Dot(line.Direction(), plane.Normal());
263  T x_t = Dot(A_inv_t, b);
264  T t = x_t / scaler;
265  param = -t;
266 }
267 
268 
269 
270 // 2D line intersection
271 // with lines
272 template <class T>
273 class Intersection<Line<T, 2>, Line<T, 2>> {
274  using LineT = Line<T, 2>;
275 public:
276  Intersection(const LineT& l1, const LineT& l2) {
277  line2 = l2;
278  auto intersection = Intersect(Hyperplane<T, 2>(l1), l2);
279  param2 = intersection.LineParameter();
280  param1 = std::isinf(param2) ? std::numeric_limits<T>::infinity() : Length(intersection.Point() - l1.Base());
281  }
282 
283  bool Intersecting() const { return !std::isinf(param1); }
284  T LineParameter1() const { return param1; }
285  T LineParameter2() const { return param2; }
286  Vector<T, 2> Point() const { return line2.PointAt(param2); }
287 private:
288  T param1, param2;
289  LineT line2;
290 };
291 
292 // with hyperplanes
293 template <class T>
294 class Intersection<Hyperplane<T, 2>, Hyperplane<T, 2>> : public Intersection<Line<T, 2>, Line<T, 2>> {
295 public:
296  Intersection(const Hyperplane<T, 2>& p1, const Hyperplane<T, 2>& p2) : Intersection<Line<T, 2>, Line<T, 2>>(p1, p2) {}
297 };
298 
299 
300 // line segments
301 template <class T>
302 class Intersection<LineSegment<T, 2>, LineSegment<T, 2>> {
303 public:
305  lineSegment1 = l1;
306  lineSegment2 = l2;
307  auto intersection = Intersect(l1.Line(), l2.Line());
308  if (intersection.Intersecting()) {
309  param1 = intersection.LineParameter1() / l1.Length();
310  param2 = intersection.LineParameter2() / l2.Length();
311  }
312  else {
313  param1 = param2 = std::numeric_limits<T>::infinity();
314  }
315  }
316 
317  bool Intersecting() const { return T(0) <= param1 && param2 <= T(1); }
318  Vector<T, 2> Point() const { return lineSegment1.Interpol(param1); }
319  T InterpolParameter1() const { return param1; }
320  T InterpolParameter2() const { return param2; }
321  T LineParameter1() const { return param1 * lineSegment1.Length(); }
322  T LineParameter2() const { return param2 * lineSegment2.Length(); }
323 private:
324  T param1;
325  T param2;
326  LineSegment<T, 2> lineSegment1;
327  LineSegment<T, 2> lineSegment2;
328 };
329 
330 
331 // line segment vs line2d
332 template <class T>
333 class Intersection<LineSegment<T, 2>, Line<T, 2>> {
334 public:
335  Intersection(const LineSegment<T, 2>& line1, const Line<T, 2>& line2) {
336  auto inter = Intersect(line1.Line(), line2);
337  if (inter.Intersecting() && inter.LineParameter1() < line1.Length()) {
338  param1 = inter.LineParameter1();
339  param2 = inter.LineParameter2();
340  }
341  else {
342  param1 = param2 = std::numeric_limits<T>::infinity();
343  }
344  }
345 
346  bool Intersecting() const { return !isinf(param1); }
347  Vector<T, 2> Point() const { return line1.Line().PointAt(param1); }
348  T LineParameter1() const { return param1; }
349  T InterpolParameter1() const { return param1 / line1.Length(); }
350  T LineParameter2() const { return param2; }
351 private:
352  T param1;
353  T param2;
354  LineSegment<T, 2> line1;
355 };
356 
357 template <class T>
358 class Intersection<Line<T, 2>, LineSegment<T, 2>> : private Intersection<LineSegment<T, 2>, Line<T, 2>> {
359 public:
360  Intersection(const Line<T, 2>& line1, const LineSegment<T, 2>& line2)
361  : Intersection<LineSegment<T, 2>, Line<T, 2>>(line2, line1) {}
362 
363  using Intersection<LineSegment<T, 2>, Line<T, 2>>::Intersecting;
365 
366  T LineParameter1() const { return Intersection<LineSegment<T, 2>, Line<T, 2>>::LineParameter2(); }
367  T InterpolParameter2() const { return Intersection<LineSegment<T, 2>, Line<T, 2>>::InterpolParameter1(); }
368  T LineParameter2() const { return Intersection<LineSegment<T, 2>, Line<T, 2>>::LineParameter1(); }
369 };
370 
371 
372 // Ray-triangle intersection (M�ller-Trumbore algorithm)
373 template <class T>
374 class Intersection<Ray<T, 3>, Triangle3D<T>> {
376 public:
377  Intersection(const Ray<T, 3>& ray, const Triangle3D<T>& triangle);
378 
379  bool IsIntersecting() const { return intersecting; }
380  VectorT Point() const { return point; }
381 
382  template <class U>
383  U Interpolate(const U& a, const U& b, const U& c) const;
384 
385  T GetT() const { return t; }
386  T GetU() const { return u; }
387  T GetV() const { return v; }
388 
389 private:
390  T t, u, v;
391  bool intersecting;
392  VectorT point;
393 };
394 
395 template <class T>
397  constexpr T EPSILON = T(0.00000001);
398 
399  VectorT edge1 = triangle.b - triangle.a;
400  VectorT edge2 = triangle.c - triangle.a;
401 
402  VectorT h = Cross(ray.Direction(), edge2);
403  T a = Dot(edge1, h);
404 
405  if (std::abs(a) < EPSILON) {
406  intersecting = false;
407  return;
408  }
409 
410  T f = T(1)/a;
411  VectorT s = ray.Base() - triangle.a;
412  u = f* Dot(s, h);
413 
414  if (u < T(0) || u > T(1)) {
415  intersecting = false;
416  return;
417  }
418 
419  VectorT q = Cross(s, edge1);
420  v = f * Dot(ray.Direction(), q);
421 
422  if (v < 0.0 || u + v > 1.0) {
423  intersecting = false;
424  return;
425  }
426 
427  t = f * Dot(edge2, q);
428  intersecting = t > EPSILON;
429  if (intersecting) {
430  point = ray.PointAt(t);
431  }
432 }
433 
434 
435 template <class T>
436 template <class U>
437 U Intersection<Ray<T, 3>, Triangle3D<T>>::Interpolate(const U& a, const U& b, const U& c) const {
438  T w = T(1) - u - v;
439  return u*b + v*c + w*a;
440 }
441 
442 
443 template <class T, int Dim, int Order>
444 class BezierCurve {
445  static_assert(Order >= 1, "Bezier curve must have order n>=1.");
446 public:
448 
449  VectorT operator()(T t) const {
450  return EvalInterpolRecurse(t);
451  }
452 
453 protected:
454  VectorT EvalInterpolRecurse(T t) const;
455 
456 public:
457  std::array<VectorT, Order+1> p;
458 };
459 
460 
461 template <class T, int Dim, int Order>
463  std::array<VectorT, Order+1> reduction = p;
464 
465  T u = T(1) - t;
466 
467  for (int i=Order; i>=1; --i) {
468  for (int j=1; j<=i; ++j) {
469  reduction[j-1] = u*reduction[j-1] + t*reduction[j];
470  }
471  }
472 
473  return reduction[0];
474 }
475 
476 
477 
478 } // namespace mathter
Vector< T, 2 > Point() const
Definition: Geometry.hpp:318
bool Intersecting() const
Definition: Geometry.hpp:346
VectorT operator()(T t) const
Definition: Geometry.hpp:449
Intersection(const LineSegment< T, 2 > &l1, const LineSegment< T, 2 > &l2)
Definition: Geometry.hpp:304
bool Intersecting() const
Definition: Geometry.hpp:189
T Distance(const Vector< T, Dim, Packed > &point)
Definition: Geometry.hpp:130
VectorT base
Definition: Geometry.hpp:57
Intersection(const LineT &line, const PlaneT &plane)
Definition: Geometry.hpp:202
Definition: Geometry.hpp:22
Vector< T, 3, false > c
Definition: Geometry.hpp:159
mathter::Line< T, Dim > Line() const
Definition: Geometry.hpp:81
Hyperplane(const VectorT &base, const VectorT &normal)
Definition: Geometry.hpp:113
VectorT Start() const
Definition: Geometry.hpp:77
bool Intersecting() const
Definition: Geometry.hpp:283
Definition: Geometry.hpp:169
T Length() const
Definition: Geometry.hpp:75
std::array< VectorT, Order+1 > p
Definition: Geometry.hpp:457
T LineParameter2() const
Definition: Geometry.hpp:350
Quaternion< T, Packed > Normalize(const Quaternion< T, Packed > &q)
Returns the unit quaternion of the same direction. Does not change this object.
Definition: QuaternionFunction.hpp:66
Definition: Geometry.hpp:63
Definition: Geometry.hpp:154
Vector< T, 2 > Point() const
Definition: Geometry.hpp:347
T LineParameter2() const
Definition: Geometry.hpp:368
bool IsIntersecting() const
Definition: Geometry.hpp:379
VectorT point2
Definition: Geometry.hpp:85
Intersection(const PlaneT &plane, const LineT &line)
Definition: Geometry.hpp:214
T Dot(const Vector< T, Dim, Packed > &lhs, const Vector< T, Dim, Packed > &rhs)
Calculates the scalar product (dot product) of the two arguments.
Definition: VectorFunction.hpp:111
static Line Through(const VectorT &point1, const VectorT &point2)
Constructs a line through both points.
Definition: Geometry.hpp:41
Definition: Geometry.hpp:25
LineSegment()
Definition: Geometry.hpp:66
Intersection(const Line< T, 2 > &line1, const LineSegment< T, 2 > &line2)
Definition: Geometry.hpp:360
Intersection(const LineT &l1, const LineT &l2)
Definition: Geometry.hpp:276
Vector< T, 2 > Point() const
Definition: Geometry.hpp:286
T GetU() const
Definition: Geometry.hpp:386
VectorT direction
Definition: Geometry.hpp:57
T LineParameter2() const
Definition: Geometry.hpp:285
Definition: Approx.hpp:11
T GetV() const
Definition: Geometry.hpp:387
T LineParameter1() const
Definition: Geometry.hpp:366
T LineParameter1() const
Definition: Geometry.hpp:348
Definition: Geometry.hpp:90
Hyperplane(const VectorT &normal, T scalar)
Definition: Geometry.hpp:117
T InterpolParameter1() const
Definition: Geometry.hpp:349
auto Intersect(const T &t, const U &u)
Definition: Geometry.hpp:174
Line(const VectorT &base, const VectorT &direction)
Construct a line through base in given direction .
Definition: Geometry.hpp:34
Hyperplane(const Line< T, 2 > &line)
Definition: Geometry.hpp:120
VectorT Interpol(T t) const
Definition: Geometry.hpp:79
VectorT Direction() const
Definition: Geometry.hpp:76
VectorT End() const
Definition: Geometry.hpp:78
Hyperplane()
Definition: Geometry.hpp:112
const VectorT & Normal() const
Definition: Geometry.hpp:126
LineSegment(const VectorT &base, const VectorT &direction, T length)
Definition: Geometry.hpp:69
bool Intersecting() const
Definition: Geometry.hpp:317
Vector< T, 3, false > a
Definition: Geometry.hpp:159
Triangle3D(const Vector< T, 3, false > &a, const Vector< T, 3, false > &b, const Vector< T, 3, false > &c)
Definition: Geometry.hpp:157
VectorT PointAt(T param) const
Returns the point at param distance from the base point along direction.
Definition: Geometry.hpp:55
VectorT Point() const
Definition: Geometry.hpp:190
Intersection(const Hyperplane< T, 2 > &p1, const Hyperplane< T, 2 > &p2)
Definition: Geometry.hpp:296
Line()=default
Does not zero-initialize members.
VectorT Base() const
Returns the base point or point1 as given in constructor.
Definition: Geometry.hpp:52
T Scalar() const
Definition: Geometry.hpp:127
Vector< T, 3, false > b
Definition: Geometry.hpp:159
Intersection(const LineT &line, const PlaneT &plane)
Definition: Geometry.hpp:234
auto Cross(const Vector< T, Dim, Packed > &head, Args &&... args) -> Vector< T, Dim, Packed >
Returns the generalized cross-product in N dimensions.
Definition: VectorFunction.hpp:223
T LineParameter() const
Definition: Geometry.hpp:191
T GetT() const
Definition: Geometry.hpp:385
T InterpolParameter2() const
Definition: Geometry.hpp:367
Intersection(const LineSegment< T, 2 > &line1, const Line< T, 2 > &line2)
Definition: Geometry.hpp:335
mathter::Line< T, Dim > Line() const
Definition: Geometry.hpp:101
VectorT Point() const
Definition: Geometry.hpp:380
LineSegment(const VectorT &point1, const VectorT &point2)
Definition: Geometry.hpp:73
VectorT Direction() const
Return the signed direction of the line (as given in constructor).
Definition: Geometry.hpp:49
T Length(const Quaternion< T, Packed > &q)
Returns the absolute value of the quaternion.
Definition: QuaternionFunction.hpp:60
Definition: Geometry.hpp:444
Vector< T, Dim, Packed > SafeNormalize(const Vector< T, Dim, Packed > &v)
Makes a unit vector, but keeps direction. Leans towards (1,0,0...) for nullvectors, costs more.
Definition: VectorFunction.hpp:76
T LineParameter1() const
Definition: Geometry.hpp:284
bool IsNormalized(const Quaternion< T, Packed > &q)
Check if the quaternion is a unit quaternion, with some tolerance for floats.
Definition: QuaternionFunction.hpp:78