1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file LineSegment.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the LineSegment geometry object. */
18 #include "LineSegment.h"
19 #include "../Math/MathFunc.h"
20 #include "AABB.h"
21 #include "Ray.h"
22 #include "Line.h"
23 #include "Plane.h"
24 #include "Polygon.h"
25 #include "Polyhedron.h"
26 #include "Frustum.h"
27 #include "../Math/float3x3.h"
28 #include "../Math/float3x4.h"
29 #include "../Math/float4x4.h"
30 #include "OBB.h"
31 #include "../Math/Quat.h"
32 #include "Sphere.h"
33 #include "Capsule.h"
34 #include "Triangle.h"
35 #include "Circle.h"
36
37 #ifdef MATH_ENABLE_STL_SUPPORT
38 #include <iostream>
39 #endif
40
41 #ifdef MATH_GRAPHICSENGINE_INTEROP
42 #include "VertexBuffer.h"
43 #endif
44
45 MATH_BEGIN_NAMESPACE
46
47 LineSegment::LineSegment(const vec &a_, const vec &b_)
48 :a(a_), b(b_)
49 {
50 }
51
52 LineSegment::LineSegment(const Ray &ray, float d)
53 :a(ray.pos), b(ray.GetPoint(d))
54 {
55 }
56
57 LineSegment::LineSegment(const Line &line, float d)
58 :a(line.pos), b(line.GetPoint(d))
59 {
60 }
61
62 vec LineSegment::GetPoint(float d) const
63 {
64         return (1.f - d) * a + d * b;
65 }
66
67 vec LineSegment::CenterPoint() const
68 {
69         return (a + b) * 0.5f;
70 }
71
72 void LineSegment::Reverse()
73 {
74         Swap(a, b);
75 }
76
77 vec LineSegment::Dir() const
78 {
79         return (b - a).Normalized();
80 }
81
82 vec LineSegment::ExtremePoint(const vec &direction) const
83 {
84         return Dot(direction, b-a) >= 0.f ? b : a;
85 }
86
87 vec LineSegment::ExtremePoint(const vec &direction, float &projectionDistance) const
88 {
89         vec extremePoint = ExtremePoint(direction);
90         projectionDistance = extremePoint.Dot(direction);
91         return extremePoint;
92 }
93
94 void LineSegment::Translate(const vec &offset)
95 {
96         a += offset;
97         b += offset;
98 }
99
100 void LineSegment::Transform(const float3x3 &transform)
101 {
102         a = transform * a;
103         b = transform * b;
104 }
105
106 void LineSegment::Transform(const float3x4 &transform)
107 {
108         a = transform.MulPos(a);
109         b = transform.MulPos(b);
110 }
111
112 void LineSegment::Transform(const float4x4 &transform)
113 {
114         a = transform.MulPos(a);
115         b = transform.MulPos(b);
116 }
117
118 void LineSegment::Transform(const Quat &transform)
119 {
120         a = transform * a;
121         b = transform * b;
122 }
123
124 float LineSegment::Length() const
125 {
126         return a.Distance(b);
127 }
128
129 float LineSegment::LengthSq() const
130 {
131         return a.DistanceSq(b);
132 }
133
134 bool LineSegment::IsFinite() const
135 {
136         return a.IsFinite() && b.IsFinite();
137 }
138
139 bool LineSegment::Contains(const vec &point, float distanceThreshold) const
140 {
141         return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
142 }
143
144 bool LineSegment::Contains(const LineSegment &rhs, float distanceThreshold) const
145 {
146         return Contains(rhs.a, distanceThreshold) && Contains(rhs.b, distanceThreshold);
147 }
148
149 bool LineSegment::Equals(const LineSegment &rhs, float e) const
150 {
151         return (a.Equals(rhs.a, e) && b.Equals(rhs.b, e)) || (a.Equals(rhs.b, e) && b.Equals(rhs.a, e));
152 }
153
154 vec LineSegment::ClosestPoint(const vec &point, float &d) const
155 {
156         vec dir = b - a;
157         d = Clamp01(Dot(point - a, dir) / dir.LengthSq());
158         return a + d * dir;
159 }
160
161 vec LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const
162 {
163         other.ClosestPoint(*this, d2, d);
164         return GetPoint(d);
165 }
166
167 vec LineSegment::ClosestPoint(const Line &other, float &d, float &d2) const
168 {
169         Line::ClosestPointLineLine(other.pos, other.dir, a, b - a, d2, d);
170         if (d < 0.f)
171         {
172                 d = 0.f;
173                 other.ClosestPoint(a, d2);
174                 return a;
175         }
176         else if (d > 1.f)
177         {
178                 d = 1.f;
179                 other.ClosestPoint(b, d2);
180                 return b;
181         }
182         else
183                 return GetPoint(d);
184 }
185
186 vec LineSegment::ClosestPoint(const LineSegment &other, float &d, float &d2) const
187 {
188         vec dir = b - a;
189         Line::ClosestPointLineLine(a, b - a, other.a, other.b - other.a, d, d2);
190         if (d >= 0.f && d <= 1.f && d2 >= 0.f && d2 <= 1.f)
191                 return a + d * dir;
192         else if (d >= 0.f && d <= 1.f) // Only d2 is out of bounds.
193         {
194                 vec p;
195                 if (d2 < 0.f)
196                 {
197                         d2 = 0.f;
198                         p = other.a;
199                 }
200                 else
201                 {
202                         d2 = 1.f;
203                         p = other.b;
204                 }
205                 return ClosestPoint(p, d);
206         }
207         else if (d2 >= 0.f && d2 <= 1.f) // Only d is out of bounds.
208         {
209                 vec p;
210                 if (d < 0.f)
211                 {
212                         d = 0.f;
213                         p = a;
214                 }
215                 else
216                 {
217                         d = 1.f;
218                         p = b;
219                 }
220
221                 other.ClosestPoint(p, d2);
222                 return p;
223         }
224         else // Both u and u2 are out of bounds.
225         {
226                 vec p;
227                 if (d < 0.f)
228                 {
229                         p = a;
230                         d = 0.f;
231                 }
232                 else
233                 {
234                         p = b;
235                         d = 1.f;
236                 }
237
238                 vec p2;
239                 if (d2 < 0.f)
240                 {
241                         p2 = other.a;
242                         d2 = 0.f;
243                 }
244                 else
245                 {
246                         p2 = other.b;
247                         d2 = 1.f;
248                 }
249
250                 float T, T2;
251                 vec closestPoint = ClosestPoint(p2, T);
252                 vec closestPoint2 = other.ClosestPoint(p, T2);
253
254                 if (closestPoint.DistanceSq(p2) <= closestPoint2.DistanceSq(p))
255                 {
256                         d = T;
257                         return closestPoint;
258                 }
259                 else
260                 {
261                         d2 = T2;
262                         return p;
263                 }
264         }
265 }
266
267 float LineSegment::Distance(const vec &point, float &d) const
268 {
269         /// See Christer Ericson's Real-Time Collision Detection, p.130.
270         vec closestPoint = ClosestPoint(point, d);
271         return closestPoint.Distance(point);
272 }
273
274 float LineSegment::DistanceSq(const vec &point) const
275 {
276         float d;
277         /// See Christer Ericson's Real-Time Collision Detection, p.130.
278         vec closestPoint = ClosestPoint(point, d);
279         return closestPoint.DistanceSq(point);
280 }
281
282 float LineSegment::Distance(const Ray &other, float &d, float &d2) const
283 {
284         ClosestPoint(other, d, d2);
285         return GetPoint(d).Distance(other.GetPoint(d2));
286 }
287
288 float LineSegment::Distance(const Line &other, float &d, float &d2) const
289 {
290         vec closestPoint2 = other.ClosestPoint(*this, d, d2);
291         vec closestPoint = GetPoint(d2);
292         return closestPoint.Distance(closestPoint2);
293 }
294
295 float LineSegment::Distance(const LineSegment &other, float &d, float &d2) const
296 {
297         ClosestPoint(other, d, d2);
298         return GetPoint(d).Distance(other.GetPoint(d2));
299 }
300
301 float LineSegment::DistanceSq(const LineSegment &other) const
302 {
303         float d, d2;
304         ClosestPoint(other, d, d2);
305         return GetPoint(d).DistanceSq(other.GetPoint(d2));
306 }
307
308 float LineSegment::Distance(const Plane &other) const
309 {
310         float aDist = other.SignedDistance(a);
311         float bDist = other.SignedDistance(b);
312         if (aDist * bDist < 0.f)
313                 return 0.f; // There was an intersection, so the distance is zero.
314         return Min(Abs(aDist), Abs(bDist));
315 }
316
317 float LineSegment::Distance(const Sphere &other) const
318 {
319         return Max(0.f, Distance(other.pos) - other.r);
320 }
321
322 float LineSegment::Distance(const Capsule &other) const
323 {
324         return Max(0.f, Distance(other.l) - other.r);
325 }
326
327 bool LineSegment::Intersects(const Plane &plane) const
328 {
329         float d = plane.SignedDistance(a);
330         float d2 = plane.SignedDistance(b);
331         return d * d2 <= 0.f;
332 }
333
334 bool LineSegment::Intersects(const Capsule &capsule) const
335 {
336         return capsule.Intersects(*this);
337 }
338
339 bool LineSegment::Intersects(const Plane &plane, float *d) const
340 {
341         return plane.Intersects(*this, d);
342 }
343
344 bool LineSegment::Intersects(const Triangle &triangle, float *d, vec *intersectionPoint) const
345 {
346         return triangle.Intersects(*this, d, intersectionPoint);
347 }
348
349 bool LineSegment::Intersects(const Sphere &s, vec *intersectionPoint, vec *intersectionNormal, float *d) const
350 {
351         return s.Intersects(*this, intersectionPoint, intersectionNormal, d) > 0;
352 }
353
354 bool LineSegment::Intersects(const AABB &aabb) const
355 {
356         return aabb.Intersects(*this);
357 }
358
359 bool LineSegment::Intersects(const AABB &aabb, float &dNear, float &dFar) const
360 {
361         return aabb.Intersects(*this, dNear, dFar);
362 }
363
364 bool LineSegment::Intersects(const OBB &obb) const
365 {
366         return obb.Intersects(*this);
367 }
368
369 bool LineSegment::Intersects(const OBB &obb, float &dNear, float &dFar) const
370 {
371         return obb.Intersects(*this, dNear, dFar);
372 }
373
374 bool LineSegment::Intersects(const LineSegment &lineSegment, float epsilon) const
375 {
376         return Distance(lineSegment) <= epsilon;
377 }
378
379 bool LineSegment::Intersects(const Polygon &polygon) const
380 {
381         return polygon.Intersects(*this);
382 }
383
384 bool LineSegment::Intersects(const Frustum &frustum) const
385 {
386         return frustum.Intersects(*this);
387 }
388
389 bool LineSegment::Intersects(const Polyhedron &polyhedron) const
390 {
391         return polyhedron.Intersects(*this);
392 }
393
394 bool LineSegment::IntersectsDisc(const Circle &disc) const
395 {
396         return disc.IntersectsDisc(*this);
397 }
398
399 Ray LineSegment::ToRay() const
400 {
401         return Ray(a, Dir());
402 }
403
404 Line LineSegment::ToLine() const
405 {
406         return Line(a, Dir());
407 }
408
409 void LineSegment::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
410 {
411         outMin = Dot(direction, a);
412         outMax = Dot(direction, b);
413         if (outMax < outMin)
414                 Swap(outMin, outMax);
415 }
416
417 LineSegment operator *(const float3x3 &transform, const LineSegment &l)
418 {
419         return LineSegment(transform * l.a, transform * l.b);
420 }
421
422 LineSegment operator *(const float3x4 &transform, const LineSegment &l)
423 {
424         return LineSegment(transform.MulPos(l.a), transform.MulPos(l.b));
425 }
426
427 LineSegment operator *(const float4x4 &transform, const LineSegment &l)
428 {
429         return LineSegment(transform.MulPos(l.a), transform.MulPos(l.b));
430 }
431
432 LineSegment operator *(const Quat &transform, const LineSegment &l)
433 {
434         return LineSegment(transform * l.a, transform * l.b);
435 }
436
437 #ifdef MATH_ENABLE_STL_SUPPORT
438 std::string LineSegment::ToString() const
439 {
440         char str[256];
441         sprintf(str, "LineSegment(a:(%.2f, %.2f, %.2f) b:(%.2f, %.2f, %.2f))",
442                 a.x, a.y, a.z, b.x, b.y, b.z);
443         return str;
444 }
445
446 std::string LineSegment::SerializeToString() const
447 {
448         char str[256];
449         char *s = SerializeFloat(a.x, str); *s = ','; ++s;
450         s = SerializeFloat(a.y, s); *s = ','; ++s;
451         s = SerializeFloat(a.z, s); *s = ','; ++s;
452         s = SerializeFloat(b.x, s); *s = ','; ++s;
453         s = SerializeFloat(b.y, s); *s = ','; ++s;
454         s = SerializeFloat(b.z, s);
455         assert(s+1 - str < 256);
456         MARK_UNUSED(s);
457         return str;
458 }
459
460 std::string LineSegment::SerializeToCodeString() const
461 {
462         return "LineSegment(" + a.SerializeToCodeString() + "," + b.SerializeToCodeString() + ")";
463 }
464
465 std::ostream &operator <<(std::ostream &o, const LineSegment &lineSegment)
466 {
467         o << lineSegment.ToString();
468         return o;
469 }
470
471 #endif
472
473 LineSegment LineSegment::FromString(const char *str, const char **outEndStr)
474 {
475         assume(str);
476         if (!str)
477                 return LineSegment(vec::nan, vec::nan);
478         LineSegment l;
479         MATH_SKIP_WORD(str, "LineSegment(");
480         MATH_SKIP_WORD(str, "a:(");
481         l.a = PointVecFromString(str, &str);
482         MATH_SKIP_WORD(str, " b:(");
483         l.b = PointVecFromString(str, &str);
484         if (outEndStr)
485                 *outEndStr = str;
486         return l;
487 }
488
489 #ifdef MATH_GRAPHICSENGINE_INTEROP
490
491 void LineSegment::ToLineList(VertexBuffer &vb) const
492 {
493         int startIndex = vb.AppendVertices(2);
494         vb.Set(startIndex, VDPosition, POINT_TO_FLOAT4(a));
495         vb.Set(startIndex+1, VDPosition, POINT_TO_FLOAT4(b));
496 }
497
498 #endif
499
500 MATH_END_NAMESPACE