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 Ray.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Ray geometry object. */
18 #include "Ray.h"
19 #include "AABB.h"
20 #include "Line.h"
21 #include "LineSegment.h"
22 #include "../Math/float3x3.h"
23 #include "../Math/float3x4.h"
24 #include "../Math/float4x4.h"
25 #include "OBB.h"
26 #include "Plane.h"
27 #include "Polygon.h"
28 #include "Polyhedron.h"
29 #include "Frustum.h"
30 #include "../Math/Quat.h"
31 #include "Sphere.h"
32 #include "Capsule.h"
33 #include "Triangle.h"
34 #include "Circle.h"
35 #include "../Math/MathFunc.h"
36
37 #ifdef MATH_ENABLE_STL_SUPPORT
38 #include <iostream>
39 #endif
40
41 MATH_BEGIN_NAMESPACE
42
43 Ray::Ray(const vec &pos_, const vec &dir_)
44 :pos(pos_), dir(dir_)
45 {
46         assume2(dir.IsNormalized(), dir, dir.LengthSq());
47 }
48
49 Ray::Ray(const Line &line)
50 :pos(line.pos), dir(line.dir)
51 {
52         assume2(dir.IsNormalized(), dir, dir.LengthSq());
53 }
54
55 Ray::Ray(const LineSegment &lineSegment)
56 :pos(lineSegment.a), dir(lineSegment.Dir())
57 {
58 }
59
60 bool Ray::IsFinite() const
61 {
62         return pos.IsFinite() && dir.IsFinite();
63 }
64
65 vec Ray::GetPoint(float d) const
66 {
67         assume2(dir.IsNormalized(), dir, dir.LengthSq());
68         return pos + d * dir;
69 }
70
71 void Ray::Translate(const vec &offset)
72 {
73         pos += offset;
74 }
75
76 void Ray::Transform(const float3x3 &transform)
77 {
78         pos = transform.Transform(pos);
79         dir = transform.Transform(dir);
80 }
81
82 void Ray::Transform(const float3x4 &transform)
83 {
84         pos = transform.MulPos(pos);
85         dir = transform.MulDir(dir);
86 }
87
88 void Ray::Transform(const float4x4 &transform)
89 {
90         pos = transform.MulPos(pos);
91         dir = transform.MulDir(dir);
92 }
93
94 void Ray::Transform(const Quat &transform)
95 {
96         pos = transform.Transform(pos);
97         dir = transform.Transform(dir);
98 }
99
100 bool Ray::Contains(const vec &point, float distanceThreshold) const
101 {
102         return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
103 }
104
105 bool Ray::Contains(const LineSegment &lineSegment, float distanceThreshold) const
106 {
107         return Contains(lineSegment.a, distanceThreshold) && Contains(lineSegment.b, distanceThreshold);
108 }
109
110 bool Ray::Equals(const Ray &rhs, float epsilon) const
111 {
112         return pos.Equals(rhs.pos, epsilon) && dir.Equals(rhs.dir, epsilon);
113 }
114
115 float Ray::Distance(const vec &point, float &d) const
116 {
117         return ClosestPoint(point, d).Distance(point);
118 }
119
120 float Ray::Distance(const Ray &other, float &d, float &d2) const
121 {
122         vec c = ClosestPoint(other, d, d2);
123         return c.Distance(other.GetPoint(d2));
124 }
125
126 float Ray::Distance(const Line &other, float &d, float &d2) const
127 {
128         vec c = ClosestPoint(other, d, d2);
129         return c.Distance(other.GetPoint(d2));
130 }
131
132 float Ray::Distance(const LineSegment &other, float &d, float &d2) const
133 {
134         vec c = ClosestPoint(other, d, d2);
135         return c.Distance(other.GetPoint(d2));
136 }
137
138 float Ray::Distance(const Sphere &sphere) const
139 {
140         return Max(0.f, Distance(sphere.pos) - sphere.r);
141 }
142
143 float Ray::Distance(const Capsule &capsule) const
144 {
145         return Max(0.f, Distance(capsule.l) - capsule.r);
146 }
147
148 vec Ray::ClosestPoint(const vec &targetPoint, float &d) const
149 {
150         d = Max(0.f, Dot(targetPoint - pos, dir));
151         return GetPoint(d);
152 }
153
154 vec Ray::ClosestPoint(const Ray &other, float &d, float &d2) const
155 {
156         Line::ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
157         if (d < 0.f && d2 < 0.f)
158         {
159                 vec closestPoint = ClosestPoint(other.pos, d);
160                 vec closestPoint2 = other.ClosestPoint(pos, d2);
161                 if (closestPoint.DistanceSq(other.pos) <= closestPoint2.DistanceSq(pos))
162                 {
163                         d2 = 0.f;
164                         return closestPoint;
165                 }
166                 else
167                 {
168                         d = 0.f;
169                         return pos;
170                 }
171         }
172         else if (d < 0.f)
173         {
174                 d = 0.f;
175                 other.ClosestPoint(pos, d2);
176                 d2 = Max(0.f, d2);
177                 return pos;
178         }
179         else if (d2 < 0.f)
180         {
181                 vec pt = ClosestPoint(other.pos, d);
182                 d = Max(0.f, d);
183                 d2 = 0.f;
184                 return pt;
185         }
186         else
187         {
188                 return GetPoint(d);
189         }
190 }
191
192 vec Ray::ClosestPoint(const Line &other, float &d, float &d2) const
193 {
194         Line::ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
195         if (d < 0.f)
196         {
197                 d = 0.f;
198                 other.ClosestPoint(pos, d2);
199                 return pos;
200         }
201         else
202                 return GetPoint(d);
203 }
204
205 vec Ray::ClosestPoint(const LineSegment &other, float &d, float &d2) const
206 {
207         Line::ClosestPointLineLine(pos, dir, other.a, other.b - other.a, d, d2);
208         if (d < 0.f)
209         {
210                 d = 0.f;
211                 if (d2 >= 0.f && d2 <= 1.f)
212                 {
213                         other.ClosestPoint(pos, d2);
214                         return pos;
215                 }
216
217                 vec p;
218                 float t2;
219
220                 if (d2 < 0.f)
221                 {
222                         p = other.a;
223                         t2 = 0.f;
224                 }
225                 else // u2 > 1.f
226                 {
227                         p = other.b;
228                         t2 = 1.f;
229                 }
230
231                 vec closestPoint = ClosestPoint(p, d);
232                 vec closestPoint2 = other.ClosestPoint(pos, d2);
233                 if (closestPoint.DistanceSq(p) <= closestPoint2.DistanceSq(pos))
234                 {
235                         d2 = t2;
236                         return closestPoint;
237                 }
238                 else
239                 {
240                         d = 0.f;
241                         return pos;
242                 }
243         }
244         else if (d2 < 0.f)
245         {
246                 d2 = 0.f;
247                 return ClosestPoint(other.a, d);
248         }
249         else if (d2 > 1.f)
250         {
251                 d2 = 1.f;
252                 return ClosestPoint(other.b, d);
253         }
254         else
255                 return GetPoint(d);
256 }
257
258 bool Ray::Intersects(const Triangle &triangle, float *d, vec *intersectionPoint) const
259 {
260         return triangle.Intersects(*this, d, intersectionPoint);
261 }
262
263 bool Ray::Intersects(const Triangle &triangle) const
264 {
265         float u, v;
266         float t = Triangle::IntersectLineTri(pos, dir, triangle.a, triangle.b, triangle.c, u, v);
267         if (t < 0.f || t == FLOAT_INF)
268                 return false;
269         return true;
270 }
271
272 bool Ray::Intersects(const Plane &plane, float *d) const
273 {
274         return plane.Intersects(*this, d);
275 }
276
277 bool Ray::Intersects(const Plane &plane) const
278 {
279         return plane.Intersects(*this, 0);
280 }
281
282 bool Ray::Intersects(const Sphere &sphere, vec *intersectionPoint, vec *intersectionNormal, float *d) const
283 {
284         return sphere.Intersects(*this, intersectionPoint, intersectionNormal, d) > 0;
285 }
286
287 bool Ray::Intersects(const Sphere &sphere) const
288 {
289         return sphere.Intersects(*this, 0, 0, 0) > 0;
290 }
291
292 bool Ray::Intersects(const AABB &aabb) const
293 {
294         return aabb.Intersects(*this);
295 }
296
297 bool Ray::Intersects(const AABB &aabb, float &dNear, float &dFar) const
298 {
299         return aabb.Intersects(*this, dNear, dFar);
300 }
301
302 bool Ray::Intersects(const OBB &obb, float &dNear, float &dFar) const
303 {
304         return obb.Intersects(*this, dNear, dFar);
305 }
306
307 bool Ray::Intersects(const OBB &obb) const
308 {
309         return obb.Intersects(*this);
310 }
311
312 bool Ray::Intersects(const Capsule &capsule) const
313 {
314         return capsule.Intersects(*this);
315 }
316
317 bool Ray::Intersects(const Polygon &polygon) const
318 {
319         return polygon.Intersects(*this);
320 }
321
322 bool Ray::Intersects(const Frustum &frustum) const
323 {
324         return frustum.Intersects(*this);
325 }
326
327 bool Ray::Intersects(const Polyhedron &polyhedron) const
328 {
329         return polyhedron.Intersects(*this);
330 }
331
332 bool Ray::IntersectsDisc(const Circle &disc) const
333 {
334         return disc.IntersectsDisc(*this);
335 }
336
337 Line Ray::ToLine() const
338 {
339         return Line(pos, dir);
340 }
341
342 LineSegment Ray::ToLineSegment(float d) const
343 {
344         return LineSegment(pos, GetPoint(d));
345 }
346
347 LineSegment Ray::ToLineSegment(float dStart, float dEnd) const
348 {
349         return LineSegment(GetPoint(dStart), GetPoint(dEnd));
350 }
351
352 void Ray::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
353 {
354         outMin = outMax = Dot(direction, pos);
355         float d = Dot(direction, dir);
356
357         // Most of the time, the projection interval will be a half-infinite range, extending to either -inf or +inf.
358         if (d > 1e-4f)
359                 outMax = FLOAT_INF;
360         else if (d < -1e4f)
361                 outMin = -FLOAT_INF;
362 }
363
364 #ifdef MATH_ENABLE_STL_SUPPORT
365 std::string Ray::ToString() const
366 {
367         char str[256];
368         sprintf(str, "Ray(Pos:(%.2f, %.2f, %.2f) Dir:(%.3f, %.3f, %.3f))", pos.x, pos.y, pos.z, dir.x, dir.y, dir.z);
369         return str;
370 }
371
372 std::string Ray::SerializeToString() const
373 {
374         char str[256];
375         char *s = SerializeFloat(pos.x, str); *s = ','; ++s;
376         s = SerializeFloat(pos.y, s); *s = ','; ++s;
377         s = SerializeFloat(pos.z, s); *s = ','; ++s;
378         s = SerializeFloat(dir.x, s); *s = ','; ++s;
379         s = SerializeFloat(dir.y, s); *s = ','; ++s;
380         s = SerializeFloat(dir.z, s);
381         assert(s+1 - str < 256);
382         MARK_UNUSED(s);
383         return str;
384 }
385
386 std::string Ray::SerializeToCodeString() const
387 {
388         return "Ray(" + pos.SerializeToCodeString() + "," + dir.SerializeToCodeString() + ")";
389 }
390
391 std::ostream &operator <<(std::ostream &o, const Ray &ray)
392 {
393         o << ray.ToString();
394         return o;
395 }
396
397 #endif
398
399 Ray Ray::FromString(const char *str, const char **outEndStr)
400 {
401         assume(str);
402         if (!str)
403                 return Ray(vec::nan, vec::nan);
404         Ray r;
405         MATH_SKIP_WORD(str, "Ray(");
406         MATH_SKIP_WORD(str, "Pos:(");
407         r.pos = PointVecFromString(str, &str);
408         MATH_SKIP_WORD(str, " Dir:(");
409         r.dir = DirVecFromString(str, &str);
410         if (outEndStr)
411                 *outEndStr = str;
412         return r;
413 }
414
415 Ray operator *(const float3x3 &transform, const Ray &ray)
416 {
417         assume(transform.IsInvertible());
418         return Ray(transform * ray.pos, (transform * ray.dir).Normalized());
419 }
420
421 Ray operator *(const float3x4 &transform, const Ray &ray)
422 {
423         assume(transform.IsInvertible());
424         return Ray(transform.MulPos(ray.pos), transform.MulDir(ray.dir).Normalized());
425 }
426
427 Ray operator *(const float4x4 &transform, const Ray &ray)
428 {
429         assume(transform.IsInvertible());
430         return Ray(transform.MulPos(ray.pos), transform.MulDir(ray.dir).Normalized());
431 }
432
433 Ray operator *(const Quat &transform, const Ray &ray)
434 {
435         return Ray(transform * ray.pos, transform * ray.dir);
436 }
437
438 MATH_END_NAMESPACE