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 Line.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Line geometry object. */
18 #include "Line.h"
19 #include "Ray.h"
20 #include "LineSegment.h"
21 #include "../Math/float3x3.h"
22 #include "../Math/float3x4.h"
23 #include "../Math/float4x4.h"
24 #include "OBB.h"
25 #include "../Math/Quat.h"
26 #include "Frustum.h"
27 #include "Triangle.h"
28 #include "Plane.h"
29 #include "Polygon.h"
30 #include "Polyhedron.h"
31 #include "Sphere.h"
32 #include "AABB.h"
33 #include "Capsule.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 /// Computes the closest point pair on two lines.
44 /** The first line is specified by two points start0 and end0. The second line is specified by
45         two points start1 and end1.
46         The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ .
47         @param v0 The starting point of the first line.
48         @param v10 The direction vector of the first line. This can be unnormalized.
49         @param v2 The starting point of the second line.
50         @param v32 The direction vector of the second line. This can be unnormalized.
51         @param d [out] Receives the normalized distance of the closest point along the first line.
52         @param d2 [out] Receives the normalized distance of the closest point along the second line.
53         @return Returns the closest point on line start0<->end0 to the second line.
54         @note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead.
55         @see ClosestPoint(), Distance(). */
56 void Line::ClosestPointLineLine(const vec &v0, const vec &v10, const vec &v2, const vec &v32, float &d, float &d2)
57 {
58         assume(!v10.IsZero());
59         assume(!v32.IsZero());
60         vec v02 = v0 - v2;
61         float d0232 = v02.Dot(v32);
62         float d3210 = v32.Dot(v10);
63         float d3232 = v32.Dot(v32);
64         assume(d3232 != 0.f); // Don't call with a zero direction vector.
65         float d0210 = v02.Dot(v10);
66         float d1010 = v10.Dot(v10);
67         float denom = d1010*d3232 - d3210*d3210;
68         if (denom != 0.f)
69                 d = (d0232*d3210 - d0210*d3232) / denom;
70         else
71                 d = 0.f;
72         d2 = (d0232 + d * d3210) / d3232;
73 }
74
75 Line::Line(const vec &pos_, const vec &dir_)
76 :pos(pos_), dir(dir_)
77 {
78         assume2(dir.IsNormalized(), dir, dir.LengthSq());
79 }
80
81 Line::Line(const Ray &ray)
82 :pos(ray.pos), dir(ray.dir)
83 {
84         assume2(dir.IsNormalized(), dir, dir.LengthSq());
85 }
86
87 Line::Line(const LineSegment &lineSegment)
88 :pos(lineSegment.a), dir(lineSegment.Dir())
89 {
90 }
91
92 bool Line::IsFinite() const
93 {
94         return pos.IsFinite() && dir.IsFinite();
95 }
96
97 vec Line::GetPoint(float d) const
98 {
99         assume2(dir.IsNormalized(), dir, dir.LengthSq());
100         return pos + d * dir;
101 }
102
103 void Line::Translate(const vec &offset)
104 {
105         pos += offset;
106 }
107
108 void Line::Transform(const float3x3 &transform)
109 {
110         pos = transform.Transform(pos);
111         dir = transform.Transform(dir);
112 }
113
114 void Line::Transform(const float3x4 &transform)
115 {
116         pos = transform.MulPos(pos);
117         dir = transform.MulDir(dir);
118 }
119
120 void Line::Transform(const float4x4 &transform)
121 {
122         pos = transform.MulPos(pos);
123         dir = transform.MulDir(dir);
124 }
125
126 void Line::Transform(const Quat &transform)
127 {
128         pos = transform.Transform(pos);
129         dir = transform.Transform(dir);
130 }
131
132 bool Line::Contains(const vec &point, float distanceThreshold) const
133 {
134         return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
135 }
136
137 bool Line::Contains(const Ray &ray, float epsilon) const
138 {
139         return Contains(ray.pos, epsilon) && dir.Equals(ray.dir, epsilon);
140 }
141
142 bool Line::Contains(const LineSegment &lineSegment, float epsilon) const
143 {
144         return Contains(lineSegment.a, epsilon) && Contains(lineSegment.b, epsilon);
145 }
146
147 bool Line::Equals(const Line &line, float epsilon) const
148 {
149         assume2(dir.IsNormalized(), dir, dir.LengthSq());
150         assume2(line.dir.IsNormalized(), line.dir, line.dir.LengthSq());
151         // If the point of the other line is on this line, and the two lines point to the same, or exactly reverse directions,
152         // they must be equal.
153         return Contains(line.pos, epsilon) && EqualAbs(Abs(dir.Dot(line.dir)), 1.f, epsilon);
154 }
155
156 float Line::Distance(const vec &point, float &d) const
157 {
158         return ClosestPoint(point, d).Distance(point);
159 }
160
161 float Line::Distance(const Ray &other, float &d, float &d2) const
162 {
163         vec c = ClosestPoint(other, d, d2);
164         return c.Distance(other.GetPoint(d2));
165 }
166
167 float Line::Distance(const Line &other, float &d, float &d2) const
168 {
169         vec c = ClosestPoint(other, d, d2);
170         return c.Distance(other.GetPoint(d2));
171 }
172
173 float Line::Distance(const LineSegment &other, float &d, float &d2) const
174 {
175         vec c = ClosestPoint(other, d, d2);
176         mathassert(d2 >= 0.f);
177         mathassert(d2 <= 1.f);
178         return c.Distance(other.GetPoint(d2));
179 }
180
181 float Line::Distance(const Sphere &other) const
182 {
183         return Max(0.f, Distance(other.pos) - other.r);
184 }
185
186 float Line::Distance(const Capsule &other) const
187 {
188         return Max(0.f, Distance(other.l) - other.r);
189 }
190
191 bool Line::Intersects(const Triangle &triangle, float *d, vec *intersectionPoint) const
192 {
193         return triangle.Intersects(*this, d, intersectionPoint);
194 }
195
196 bool Line::Intersects(const Plane &plane, float *d) const
197 {
198         return plane.Intersects(*this, d);
199 }
200
201 bool Line::Intersects(const Sphere &s, vec *intersectionPoint, vec *intersectionNormal, float *d) const
202 {
203         return s.Intersects(*this, intersectionPoint, intersectionNormal, d) > 0;
204 }
205
206 bool Line::Intersects(const AABB &aabb) const
207 {
208         return aabb.Intersects(*this);
209 }
210
211 bool Line::Intersects(const AABB &aabb, float &dNear, float &dFar) const
212 {
213         return aabb.Intersects(*this, dNear, dFar);
214 }
215
216 bool Line::Intersects(const OBB &obb) const
217 {
218         return obb.Intersects(*this);
219 }
220
221 bool Line::Intersects(const OBB &obb, float &dNear, float &dFar) const
222 {
223         return obb.Intersects(*this, dNear, dFar);
224 }
225
226 bool Line::Intersects(const Capsule &capsule) const
227 {
228         return capsule.Intersects(*this);
229 }
230
231 bool Line::Intersects(const Polygon &polygon) const
232 {
233         return polygon.Intersects(*this);
234 }
235
236 bool Line::Intersects(const Frustum &frustum) const
237 {
238         return frustum.Intersects(*this);
239 }
240
241 bool Line::Intersects(const Polyhedron &polyhedron) const
242 {
243         return polyhedron.Intersects(*this);
244 }
245
246 bool Line::IntersectsDisc(const Circle &disc) const
247 {
248         return disc.IntersectsDisc(*this);
249 }
250
251 vec Line::ClosestPoint(const vec &targetPoint, float &d) const
252 {
253         d = Dot(targetPoint - pos, dir);
254         return GetPoint(d);
255 }
256
257 vec Line::ClosestPoint(const Ray &other, float &d, float &d2) const
258 {
259         ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
260         if (d2 >= 0.f)
261                 return GetPoint(d);
262         else
263         {
264                 d2 = 0.f;
265                 return ClosestPoint(other.pos, d);
266         }
267 }
268
269 vec Line::ClosestPoint(const Line &other, float &d, float &d2) const
270 {
271         ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
272         return GetPoint(d);
273 }
274
275 vec Line::ClosestPoint(const LineSegment &other, float &d, float &d2) const
276 {
277         ClosestPointLineLine(pos, dir, other.a, other.b - other.a, d, d2);
278         if (d2 < 0.f)
279         {
280                 d2 = 0.f;
281                 return ClosestPoint(other.a, d);
282         }
283         else if (d2 > 1.f)
284         {
285                 d2 = 1.f;
286                 return ClosestPoint(other.b, d);
287         }
288         else
289                 return GetPoint(d);
290 }
291
292 vec Line::ClosestPoint(const Triangle &triangle, float &d) const
293 {
294         vec closestPointTriangle = triangle.ClosestPoint(*this);
295         return ClosestPoint(closestPointTriangle, d);
296 }
297
298 vec Line::ClosestPoint(const Triangle &triangle, float &d, float2 &outBarycentricUV) const
299 {
300         vec closestPointTriangle = triangle.ClosestPoint(*this);
301         outBarycentricUV = triangle.BarycentricUV(closestPointTriangle);
302         return ClosestPoint(closestPointTriangle, d);
303 }
304
305 bool Line::AreCollinear(const vec &p1, const vec &p2, const vec &p3, float epsilon)
306 {
307         return vec::AreCollinear(p1, p2, p3, epsilon);
308 }
309
310 Ray Line::ToRay() const
311 {
312         return Ray(pos, dir);
313 }
314
315 LineSegment Line::ToLineSegment(float d) const
316 {
317         return LineSegment(pos, GetPoint(d));
318 }
319
320 void Line::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
321 {
322         // Most of the time, the projection of a line spans the whole 1D axis.
323         // As a special case, if the line is perpendicular to the direction vector in question,
324         // then the projection interval of this line is a single point.
325         if (dir.IsPerpendicular(direction))
326                 outMin = outMax = Dot(direction, pos);
327         else
328         {
329                 outMin = -FLOAT_INF;
330                 outMax = FLOAT_INF;
331         }
332 }
333
334 LineSegment Line::ToLineSegment(float dStart, float dEnd) const
335 {
336         return LineSegment(GetPoint(dStart), GetPoint(dEnd));
337 }
338
339 Line operator *(const float3x3 &transform, const Line &l)
340 {
341         return Line(transform * l.pos, transform * l.dir);
342 }
343
344 Line operator *(const float3x4 &transform, const Line &l)
345 {
346         return Line(transform.MulPos(l.pos), transform.MulDir(l.dir));
347 }
348
349 Line operator *(const float4x4 &transform, const Line &l)
350 {
351         return Line(transform.MulPos(l.pos), transform.MulDir(l.dir));
352 }
353
354 Line operator *(const Quat &transform, const Line &l)
355 {
356         return Line(transform * l.pos, transform * l.dir);
357 }
358
359 #ifdef MATH_ENABLE_STL_SUPPORT
360 std::string Line::ToString() const
361 {
362         char str[256];
363         sprintf(str, "Line(Pos:(%.2f, %.2f, %.2f) Dir:(%.3f, %.3f, %.3f))", pos.x, pos.y, pos.z, dir.x, dir.y, dir.z);
364         return str;
365 }
366
367 std::string Line::SerializeToString() const
368 {
369         char str[256];
370         char *s = SerializeFloat(pos.x, str); *s = ','; ++s;
371         s = SerializeFloat(pos.y, s); *s = ','; ++s;
372         s = SerializeFloat(pos.z, s); *s = ','; ++s;
373         s = SerializeFloat(dir.x, s); *s = ','; ++s;
374         s = SerializeFloat(dir.y, s); *s = ','; ++s;
375         s = SerializeFloat(dir.z, s);
376         assert(s+1 - str < 256);
377         MARK_UNUSED(s);
378         return str;
379 }
380
381 std::string Line::SerializeToCodeString() const
382 {
383         return "Line(" + pos.SerializeToCodeString() + "," + dir.SerializeToCodeString() + ")";
384 }
385
386 std::ostream &operator <<(std::ostream &o, const Line &line)
387 {
388         o << line.ToString();
389         return o;
390 }
391
392 #endif
393
394 Line Line::FromString(const char *str, const char **outEndStr)
395 {
396         assume(str);
397         if (!str)
398                 return Line(vec::nan, vec::nan);
399         Line l;
400         MATH_SKIP_WORD(str, "Line(");
401         MATH_SKIP_WORD(str, "Pos:(");
402         l.pos = PointVecFromString(str, &str);
403         MATH_SKIP_WORD(str, " Dir:(");
404         l.dir = DirVecFromString(str, &str);
405         if (outEndStr)
406                 *outEndStr = str;
407         return l;
408 }
409
410 MATH_END_NAMESPACE