1 /* Copyright 2011 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 "Geometry/Line.h"
19 #include "Geometry/Ray.h"
20 #include "Geometry/LineSegment.h"
21 #include "Math/float3x3.h"
22 #include "Math/float3x4.h"
23 #include "Math/float4x4.h"
24 #include "Geometry/OBB.h"
25 #include "Math/Quat.h"
26 #include "Geometry/Frustum.h"
27 #include "Geometry/Triangle.h"
28 #include "Geometry/Plane.h"
29 #include "Geometry/Polygon.h"
30 #include "Geometry/Polyhedron.h"
31 #include "Geometry/Sphere.h"
32 #include "Geometry/AABB.h"
33 #include "Geometry/Capsule.h"
34 #include "Geometry/Circle.h"
35 #include "Math/MathFunc.h"
36
37 MATH_BEGIN_NAMESPACE
38
39 /// A helper function to compute the line-line closest point.
40 /** This code is adapted from http://paulbourke.net/geometry/lineline3d/ .
41         dmnop = (xm - xn)(xo - xp) + (ym - yn)(yo - yp) + (zm - zn)(zo - zp).
42         @param v An array of four floats: [0]: line 0 start. [1]: line 0 end. [2]: line 1 start. [3]: line 1 end. */
43 float Dmnop(const float3 *v, int m, int n, int o, int p)
44 {
45         return (v[m].x - v[n].x) * (v[o].x - v[p].x) + (v[m].y - v[n].y) * (v[o].y - v[p].y) + (v[m].z - v[n].z) * (v[o].z - v[p].z);
46 }
47
48 /// Computes the closest point pair on two lines.
49 /** The first line is specified by two points start0 and end0. The second line is specified by
50         two points start1 and end1.
51         The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ .
52         @param d [out] If specified, receives the normalized distance of the closest point along the first line.
53                 This pointer may be left null.
54         @param d2 [out] If specified, receives the normalized distance of the closest point along the second line.
55                 This pointer may be left null.
56         @return Returns the closest point on line start0<->end0 to the second line.
57         @note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead.
58         @see ClosestPoint(), Distance(). */
59 float3 Line::ClosestPointLineLine(float3 start0, float3 end0, float3 start1, float3 end1, float *d, float *d2)
60 {
61         const float3 v[4] = { start0, end0, start1, end1 };
62
63         float d0232 = Dmnop(v,0,2,3,2);
64         float d3210 = Dmnop(v,3,2,1,0);
65         float d3232 = Dmnop(v,3,2,3,2);
66         float mu = (d0232 * d3210 - Dmnop(v,0,2,1,0)*d3232) / (Dmnop(v,1,0,1,0)*Dmnop(v,3,2,3,2) - Dmnop(v,3,2,1,0)*Dmnop(v,3,2,1,0));
67         if (d)
68                 *d = mu;
69
70         if (d2)
71                 *d2 = (d0232 + mu * d3210) / d3232;
72
73         return start0 + mu * (end0 - start0);
74 }
75
76 Line::Line(const float3 &pos_, const float3 &dir_)
77 :pos(pos_), dir(dir_)
78 {
79         assume(dir.IsNormalized());
80 }
81
82 Line::Line(const Ray &ray)
83 :pos(ray.pos), dir(ray.dir)
84 {
85         assume(dir.IsNormalized());
86 }
87
88 Line::Line(const LineSegment &lineSegment)
89 :pos(lineSegment.a), dir(lineSegment.Dir())
90 {
91 }
92
93 float3 Line::GetPoint(float d) const
94 {
95         assert(dir.IsNormalized());
96         return pos + d * dir;
97 }
98
99 void Line::Transform(const float3x3 &transform)
100 {
101         pos = transform.Transform(pos);
102         dir = transform.Transform(dir);
103 }
104
105 void Line::Transform(const float3x4 &transform)
106 {
107         pos = transform.TransformPos(pos);
108         dir = transform.TransformDir(dir);
109 }
110
111 void Line::Transform(const float4x4 &transform)
112 {
113         pos = transform.TransformPos(pos);
114         dir = transform.TransformDir(dir);
115 }
116
117 void Line::Transform(const Quat &transform)
118 {
119         pos = transform.Transform(pos);
120         dir = transform.Transform(dir);
121 }
122
123 bool Line::Contains(const float3 &point, float distanceThreshold) const
124 {
125         return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
126 }
127
128 bool Line::Contains(const Ray &ray, float epsilon) const
129 {
130         return Contains(ray.pos, epsilon) && dir.Equals(ray.dir, epsilon);
131 }
132
133 bool Line::Contains(const LineSegment &lineSegment, float epsilon) const
134 {
135         return Contains(lineSegment.a, epsilon) && Contains(lineSegment.b, epsilon);
136 }
137
138 bool Line::Equals(const Line &line, float epsilon) const
139 {
140         assume(dir.IsNormalized());
141         assume(line.dir.IsNormalized());
142         // If the point of the other line is on this line, and the two lines point to the same, or exactly reverse directions,
143         // they must be equal.
144         return Contains(line.pos, epsilon) && EqualAbs(Abs(dir.Dot(line.dir)), 1.f, epsilon);
145 }
146
147 float Line::Distance(const float3 &point, float *d) const
148 {
149         return ClosestPoint(point, d).Distance(point);
150 }
151
152 float Line::Distance(const Ray &other, float *d, float *d2) const
153 {
154         float u2;
155         float3 c = ClosestPoint(other, d, &u2);
156         if (d2) *d2 = u2;
157         return c.Distance(other.GetPoint(u2));
158 }
159
160 float Line::Distance(const Ray &other) const
161 {
162         return Distance(other, 0, 0);
163 }
164
165 float Line::Distance(const Line &other, float *d, float *d2) const
166 {
167         float u2;
168         float3 c = ClosestPoint(other, d, &u2);
169         if (d2) *d2 = u2;
170         return c.Distance(other.GetPoint(u2));
171 }
172
173 float Line::Distance(const Line &other) const
174 {
175         return Distance(other, 0, 0);
176 }
177
178 float Line::Distance(const LineSegment &other, float *d, float *d2) const
179 {
180         float u2;
181         float3 c = ClosestPoint(other, d, &u2);
182         if (d2) *d2 = u2;
183         return c.Distance(other.GetPoint(u2));
184 }
185
186 float Line::Distance(const LineSegment &other) const
187 {
188         return Distance(other, 0, 0);
189 }
190
191 float Line::Distance(const Sphere &other) const
192 {
193         return Max(0.f, Distance(other.pos) - other.r);
194 }
195
196 float Line::Distance(const Capsule &other) const
197 {
198         return Max(0.f, Distance(other.l) - other.r);
199 }
200
201 bool Line::Intersects(const Triangle &triangle, float *d, float3 *intersectionPoint) const
202 {
203         return triangle.Intersects(*this, d, intersectionPoint);
204 }
205
206 bool Line::Intersects(const Plane &plane, float *d) const
207 {
208         return plane.Intersects(*this, d);
209 }
210
211 bool Line::Intersects(const Sphere &s, float3 *intersectionPoint, float3 *intersectionNormal, float *d) const
212 {
213         return s.Intersects(*this, intersectionPoint, intersectionNormal, d);
214 }
215
216 bool Line::Intersects(const AABB &aabb, float *dNear, float *dFar) const
217 {
218         return aabb.Intersects(*this, dNear, dFar);
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 float3 Line::ClosestPoint(const float3 &targetPoint, float *d) const
252 {
253         float u = Dot(targetPoint - pos, dir);
254         if (d)
255                 *d = u;
256         return GetPoint(u);
257 }
258
259 float3 Line::ClosestPoint(const Ray &other, float *d, float *d2) const
260 {
261         ///\bug Properly cap d2.
262         return ClosestPointLineLine(pos, pos + dir, other.pos, other.pos + other.dir, d, d2);
263 }
264
265 float3 Line::ClosestPoint(const Line &other, float *d, float *d2) const
266 {
267         return ClosestPointLineLine(pos, pos + dir, other.pos, other.pos + other.dir, d, d2);
268 }
269
270 float3 Line::ClosestPoint(const LineSegment &other, float *d, float *d2) const
271 {
272         ///\bug Properly cap d2.
273         return ClosestPointLineLine(pos, pos + dir, other.a, other.b, d, d2);
274 }
275
276 float3 Line::ClosestPoint(const Triangle &triangle, float *outU, float *outV, float *outD) const
277 {
278         float d;
279         if (!outD)
280                 outD = &d;
281         triangle.ClosestPoint(*this, outU, outV, outD);
282         return GetPoint(*outD);
283 }
284
285 bool Line::AreCollinear(const float3 &p1, const float3 &p2, const float3 &p3, float epsilon)
286 {
287         ///@todo Improve this check to be distance length -invariant.
288         return Abs((p2-p1).Dot(p3-p1)) <= epsilon;
289 }
290
291 Ray Line::ToRay() const
292 {
293         return Ray(pos, dir);
294 }
295
296 LineSegment Line::ToLineSegment(float d) const
297 {
298         return LineSegment(pos, GetPoint(d));
299 }
300
301 Line operator *(const float3x3 &transform, const Line &l)
302 {
303         return Line(transform * l.pos, transform * l.dir);
304 }
305
306 Line operator *(const float3x4 &transform, const Line &l)
307 {
308         return Line(transform.MulPos(l.pos), transform.MulDir(l.dir));
309 }
310
311 Line operator *(const float4x4 &transform, const Line &l)
312 {
313         return Line(transform.MulPos(l.pos), transform.MulDir(l.dir));
314 }
315
316 Line operator *(const Quat &transform, const Line &l)
317 {
318         return Line(transform * l.pos, transform * l.dir);
319 }
320
321 #ifdef MATH_ENABLE_STL_SUPPORT
322 std::string Line::ToString() const
323 {
324         char str[256];
325         sprintf(str, "Line(pos:(%.2f, %.2f, %.2f) dir:(%.2f, %.2f, %.2f))", 
326                 pos.x, pos.y, pos.z, dir.x, dir.y, dir.z);
327         return str;
328 }
329 #endif
330
331 MATH_END_NAMESPACE