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 LineSegment.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the LineSegment geometry object. */
18 #include "Math/MathFunc.h"
19 #include "Geometry/AABB.h"
20 #include "Geometry/LineSegment.h"
21 #include "Geometry/Ray.h"
22 #include "Geometry/Line.h"
23 #include "Geometry/Plane.h"
24 #include "Geometry/Polygon.h"
25 #include "Geometry/Polyhedron.h"
26 #include "Geometry/Frustum.h"
27 #include "Math/float3x3.h"
28 #include "Math/float3x4.h"
29 #include "Math/float4x4.h"
30 #include "Geometry/OBB.h"
31 #include "Math/Quat.h"
32 #include "Geometry/Sphere.h"
33 #include "Geometry/Capsule.h"
34 #include "Geometry/Triangle.h"
35 #include "Geometry/Circle.h"
36
37 MATH_BEGIN_NAMESPACE
38
39 LineSegment::LineSegment(const float3 &a_, const float3 &b_)
40 :a(a_), b(b_)
41 {
42 }
43
44 LineSegment::LineSegment(const Ray &ray, float d)
45 :a(ray.pos), b(ray.GetPoint(d))
46 {
47 }
48
49 LineSegment::LineSegment(const Line &line, float d)
50 :a(line.pos), b(line.GetPoint(d))
51 {
52 }
53
54 float3 LineSegment::GetPoint(float d) const
55 {
56         return (1.f - d) * a + d * b;
57 }
58
59 float3 LineSegment::CenterPoint() const
60 {
61         return (a + b) * 0.5f;
62 }
63
64 void LineSegment::Reverse()
65 {
66         Swap(a, b);
67 }
68
69 float3 LineSegment::Dir() const
70 {
71         return (b - a).Normalized();
72 }
73
74 void LineSegment::Transform(const float3x3 &transform)
75 {
76         a = transform * a;
77         b = transform * b;
78 }
79
80 void LineSegment::Transform(const float3x4 &transform)
81 {
82         a = transform.MulPos(a);
83         b = transform.MulPos(b);
84 }
85
86 void LineSegment::Transform(const float4x4 &transform)
87 {
88         a = transform.MulPos(a);
89         b = transform.MulPos(b);
90 }
91
92 void LineSegment::Transform(const Quat &transform)
93 {
94         a = transform * a;
95         b = transform * b;
96 }
97
98 float LineSegment::Length() const
99 {
100         return a.Distance(b);
101 }
102
103 float LineSegment::LengthSq() const
104 {
105         return a.DistanceSq(b);
106 }
107
108 bool LineSegment::IsFinite() const
109 {
110         return a.IsFinite() && b.IsFinite();
111 }
112
113 bool LineSegment::Contains(const float3 &point, float distanceThreshold) const
114 {
115         return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
116 }
117
118 bool LineSegment::Contains(const LineSegment &rhs, float distanceThreshold) const
119 {
120         return Contains(rhs.a, distanceThreshold) && Contains(rhs.b, distanceThreshold);
121 }
122
123 bool LineSegment::Equals(const LineSegment &rhs, float e) const
124 {
125         return (a.Equals(rhs.a, e) && b.Equals(rhs.b, e)) || (a.Equals(rhs.b, e) && b.Equals(rhs.a, e));
126 }
127
128 float3 LineSegment::ClosestPoint(const float3 &point, float *d) const
129 {
130         float3 dir = b - a;
131         float u = Clamp01(Dot(point - a, dir) / dir.LengthSq());
132         if (d)
133                 *d = u;
134         return a + u * dir;
135 }
136
137 float3 LineSegment::ClosestPoint(const Ray &other, float *d, float *d2) const
138 {
139         float u, u2;
140         Line::ClosestPointLineLine(a, b, other.pos, other.pos + other.dir, &u, &u2);
141         u = Clamp01(u); // This is a line segment - cap both ends.
142         if (d)
143                 *d = u;
144         u2 = Max(0.f, u2); // The other primitive is a ray - cap negative side.
145         if (d2)
146                 *d2 = u2;
147         return GetPoint(u);
148 }
149
150 float3 LineSegment::ClosestPoint(const Line &other, float *d, float *d2) const
151 {
152         float u, u2;
153         Line::ClosestPointLineLine(a, b, other.pos, other.pos + other.dir, &u, &u2);
154         u = Clamp01(u); // This is a line segment - cap both ends.
155         if (d)
156                 *d = u;
157         if (d2)
158                 *d2 = u2;
159         return GetPoint(u);
160 }
161
162 float3 LineSegment::ClosestPoint(const LineSegment &other, float *d, float *d2) const
163 {
164         float u, u2;
165         Line::ClosestPointLineLine(a, b, other.a, other.b, &u, &u2);
166         u = Clamp01(u); // This is a line segment - cap both ends.
167         if (d)
168                 *d = u;
169         u2 = Clamp01(u2); // The other primitive is a line segment as well - cap both ends.
170         if (d2)
171                 *d2 = u2;
172         return GetPoint(u);
173 }
174
175 float LineSegment::Distance(const float3 &point, float *d) const
176 {
177         ///@todo This function could be slightly optimized.
178         /// See Christer Ericson's Real-Time Collision Detection, p.130.
179         float3 closestPoint = ClosestPoint(point, d);
180         return closestPoint.Distance(point);
181 }
182
183 float LineSegment::Distance(const Ray &other, float *d, float *d2) const
184 {
185         float u, u2;
186         ClosestPoint(other, &u, &u2);
187         if (d)
188                 *d = u;
189         if (d2)
190                 *d2 = u2;
191         return GetPoint(u).Distance(other.GetPoint(u2));
192 }
193
194 float LineSegment::Distance(const Line &other, float *d, float *d2) const
195 {
196         float u, u2;
197         ClosestPoint(other, &u, &u2);
198         if (d)
199                 *d = u;
200         if (d2)
201                 *d2 = u2;
202         return GetPoint(u).Distance(other.GetPoint(u2));
203 }
204
205 float LineSegment::Distance(const LineSegment &other, float *d, float *d2) const
206 {
207         float u, u2;
208         ClosestPoint(other, &u, &u2);
209         if (d)
210                 *d = u;
211         if (d2)
212                 *d2 = u2;
213         return GetPoint(u).Distance(other.GetPoint(u2));
214 }
215
216 float LineSegment::Distance(const Plane &other) const
217 {
218         float aDist = other.SignedDistance(a);
219         float bDist = other.SignedDistance(b);
220         if (aDist * bDist < 0.f)
221                 return 0.f; // There was an intersection, so the distance is zero.
222         return Min(Abs(aDist), Abs(bDist));
223 }
224
225 float LineSegment::Distance(const Sphere &other) const
226 {
227         return Max(0.f, Distance(other.pos) - other.r);
228 }
229
230 float LineSegment::Distance(const Capsule &other) const
231 {
232         return Max(0.f, Distance(other.l) - other.r);
233 }
234
235 bool LineSegment::Intersects(const Plane &plane) const
236 {
237         float d = plane.SignedDistance(a);
238         float d2 = plane.SignedDistance(b);
239         return d * d2 <= 0.f;
240 }
241
242 bool LineSegment::Intersects(const Capsule &capsule) const
243 {
244         return capsule.Intersects(*this);
245 }
246
247 bool LineSegment::Intersects(const Plane &plane, float *d) const
248 {
249         return plane.Intersects(*this, d);
250 }
251
252 bool LineSegment::Intersects(const Triangle &triangle, float *d, float3 *intersectionPoint) const
253 {
254         return triangle.Intersects(*this, d, intersectionPoint);
255 }
256
257 bool LineSegment::Intersects(const Sphere &s, float3 *intersectionPoint, float3 *intersectionNormal, float *d) const
258 {
259         return s.Intersects(*this, intersectionPoint, intersectionNormal, d);
260 }
261
262 bool LineSegment::Intersects(const AABB &aabb, float *dNear, float *dFar) const
263 {
264         return aabb.Intersects(*this, dNear, dFar);
265 }
266
267 bool LineSegment::Intersects(const OBB &obb, float *dNear, float *dFar) const
268 {
269         return obb.Intersects(*this, dNear, dFar);
270 }
271
272 bool LineSegment::Intersects(const LineSegment &lineSegment, float epsilon) const
273 {
274         return Distance(lineSegment) <= epsilon;
275 }
276
277 bool LineSegment::Intersects(const Polygon &polygon) const
278 {
279         return polygon.Intersects(*this);
280 }
281
282 bool LineSegment::Intersects(const Frustum &frustum) const
283 {
284         return frustum.Intersects(*this);
285 }
286
287 bool LineSegment::Intersects(const Polyhedron &polyhedron) const
288 {
289         return polyhedron.Intersects(*this);
290 }
291
292 bool LineSegment::IntersectsDisc(const Circle &disc) const
293 {
294         return disc.IntersectsDisc(*this);
295 }
296
297 Ray LineSegment::ToRay() const
298 {
299         return Ray(a, Dir());
300 }
301
302 Line LineSegment::ToLine() const
303 {
304         return Line(a, Dir());
305 }
306
307 LineSegment operator *(const float3x3 &transform, const LineSegment &l)
308 {
309         return LineSegment(transform * l.a, transform * l.b);
310 }
311
312 LineSegment operator *(const float3x4 &transform, const LineSegment &l)
313 {
314         return LineSegment(transform.MulPos(l.a), transform.MulPos(l.b));
315 }
316
317 LineSegment operator *(const float4x4 &transform, const LineSegment &l)
318 {
319         return LineSegment(transform.MulPos(l.a), transform.MulPos(l.b));
320 }
321
322 LineSegment operator *(const Quat &transform, const LineSegment &l)
323 {
324         return LineSegment(transform * l.a, transform * l.b);
325 }
326
327 #ifdef MATH_ENABLE_STL_SUPPORT
328 std::string LineSegment::ToString() const
329 {
330         char str[256];
331         sprintf(str, "LineSegment(a:(%.2f, %.2f, %.2f) b:(%.2f, %.2f, %.2f))", 
332                 a.x, a.y, a.z, b.x, b.y, b.z);
333         return str;
334 }
335 #endif
336
337 MATH_END_NAMESPACE