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 Capsule.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation for the Capsule geometry object. */
18 #include "Capsule.h"
19 #include "../Math/MathConstants.h"
20 #include "../Math/MathFunc.h"
21 #include "../Math/float3x3.h"
22 #include "../Math/float3x4.h"
23 #include "../Math/float4x4.h"
24 #include "../Math/Quat.h"
25 #include "AABB.h"
26 #include "OBB.h"
27 #include "Frustum.h"
28 #include "Plane.h"
29 #include "Ray.h"
30 #include "Line.h"
31 #include "LineSegment.h"
32 #include "Polygon.h"
33 #include "Polyhedron.h"
34 #include "Sphere.h"
35 #include "Circle.h"
36 #include "Triangle.h"
37 #include "../Algorithm/Random/LCG.h"
38 #include "../Algorithm/GJK.h"
39 #include "../Math/assume.h"
40
41 #ifdef MATH_ENABLE_STL_SUPPORT
42 #include <iostream>
43 #endif
44
45 MATH_BEGIN_NAMESPACE
46
47 Capsule::Capsule(const LineSegment &endPoints, float radius)
48 :l(endPoints), r(radius)
49 {
50 }
51
52 Capsule::Capsule(const vec &bottomPoint, const vec &topPoint, float radius)
53 :l(bottomPoint, topPoint), r(radius)
54 {
55 }
56
57 void Capsule::SetFrom(const Sphere &s)
58 {
59         l = LineSegment(s.pos, s.pos);
60         r = s.r;
61 }
62
63 void Capsule::SetDegenerate()
64 {
65         r = -1.f;
66 }
67
68 bool Capsule::IsDegenerate() const
69 {
70         return r <= 0.f;
71 }
72
73 float Capsule::LineLength() const
74 {
75         return l.Length();
76 }
77
78 float Capsule::Diameter() const
79 {
80         return 2.f * r;
81 }
82
83 vec Capsule::Bottom() const
84 {
85         return l.a - UpDirection() * r;
86 }
87
88 vec Capsule::Center() const
89 {
90         return l.CenterPoint();
91 }
92
93 vec Capsule::ExtremePoint(const vec &direction) const
94 {
95         float len = direction.Length();
96         assume(len > 0.f);
97         return (Dot(direction, l.b - l.a) >= 0.f ? l.b : l.a) + direction * (r / len);
98 }
99
100 vec Capsule::ExtremePoint(const vec &direction, float &projectionDistance) const
101 {
102         vec extremePoint = ExtremePoint(direction);
103         projectionDistance = extremePoint.Dot(direction);
104         return extremePoint;
105 }
106
107 void Capsule::ProjectToAxis(const vec &direction, float &outMin, float &outMax) const
108 {
109         outMin = Dot(direction, l.a);
110         outMax = Dot(direction, l.b);
111         if (outMax < outMin)
112                 Swap(outMin, outMax);
113
114         // The following requires that direction is normalized, otherwise we would have to sub/add 'r * direction.Length()', but
115         // don't want to do that for performance reasons.
116         assume(direction.IsNormalized());
117         outMin -= r;
118         outMax += r;
119 }
120
121 vec Capsule::Top() const
122 {
123         return l.b + UpDirection() * r;
124 }
125
126 vec Capsule::UpDirection() const
127 {
128         vec d = l.b - l.a;
129         d.Normalize(); // Will always result in a normalized vector, even if l.a == l.b.
130         return d;
131 }
132
133 float Capsule::Height() const
134 {
135         return LineLength() + Diameter();
136 }
137
138 float Capsule::Volume() const
139 {
140         return pi * r * r * LineLength() + 4.f * pi * r * r * r / 3.f;
141 }
142
143 float Capsule::SurfaceArea() const
144 {
145         return 2.f * pi * r * LineLength() + 4.f * pi * r * r;
146 }
147
148 Circle Capsule::CrossSection(float yPos) const
149 {
150         assume(yPos >= 0.f);
151         assume(yPos <= 1.f);
152         yPos *= Height();
153         vec up = UpDirection();
154         vec centerPos = Bottom() + up * yPos;
155         if (yPos < r) // First section, between Bottom() and lower point.
156                 return Circle(centerPos, up, Sqrt(r*r - (r-yPos)*(r-yPos)));
157         if (yPos < l.Length() + r) // Second section, between lower and upper points.
158                 return Circle(centerPos, up, r);
159         float d = yPos - r - l.Length(); // Third section, above upper point.
160         return Circle(centerPos, up, Sqrt(r*r - d*d));
161 }
162
163 LineSegment Capsule::HeightLineSegment() const
164 {
165         return LineSegment(Bottom(), Top());
166 }
167
168 bool Capsule::IsFinite() const
169 {
170         return l.IsFinite() && MATH_NS::IsFinite(r);
171 }
172
173 vec Capsule::PointInside(float l, float a, float d) const
174 {
175         Circle c = CrossSection(l);
176         return c.GetPoint(a*2.f*pi, d);
177 }
178
179 vec Capsule::UniformPointPerhapsInside(float l, float x, float y) const
180 {
181         return MinimalEnclosingOBB().PointInside(l, x, y);
182 }
183
184 Sphere Capsule::SphereA() const
185 {
186         return Sphere(l.a, r);
187 }
188
189 Sphere Capsule::SphereB() const
190 {
191         return Sphere(l.b, r);
192 }
193
194 AABB Capsule::MinimalEnclosingAABB() const
195 {
196         vec d = DIR_VEC_SCALAR(r);
197         AABB aabb(Min(l.a, l.b) - d, Max(l.a, l.b) + d);
198         return aabb;
199 }
200
201 OBB Capsule::MinimalEnclosingOBB() const
202 {
203         OBB obb;
204         obb.axis[0] = UpDirection();
205         obb.axis[0].PerpendicularBasis(obb.axis[1], obb.axis[2]);
206         obb.pos = Center();
207         obb.r[0] = Height() * 0.5f;
208         obb.r[1] = r;
209         obb.r[2] = r;
210         return obb;
211 }
212
213 vec Capsule::RandomPointInside(LCG &rng) const
214 {
215         assume(IsFinite());
216
217         OBB obb = MinimalEnclosingOBB();
218         for(int i = 0; i < 1000; ++i)
219         {
220                 vec pt = obb.RandomPointInside(rng);
221                 if (Contains(pt))
222                         return pt;
223         }
224         assume(false && "Warning: Capsule::RandomPointInside ran out of iterations to perform!");
225         return Center(); // Just return some point that is known to be inside.
226 }
227
228 vec Capsule::RandomPointOnSurface(LCG &rng) const
229 {
230         float f1 = rng.Float();
231         float f2 = rng.Float();
232         return PointInside(f1, f2, 1.f);
233 }
234
235 void Capsule::Translate(const vec &offset)
236 {
237         l.a += offset;
238         l.b += offset;
239 }
240
241 void Capsule::Scale(const vec &centerPoint, float scaleFactor)
242 {
243         float3x4 tm = float3x4::Scale(DIR_VEC_SCALAR(scaleFactor), centerPoint);
244         l.Transform(tm);
245         r *= scaleFactor;
246 }
247
248 void Capsule::Transform(const float3x3 &transform)
249 {
250         assume(transform.HasUniformScale());
251         assume(transform.IsColOrthogonal());
252         l.Transform(transform);
253         r *= transform.Col(0).Length(); // Scale the radius.
254 }
255
256 void Capsule::Transform(const float3x4 &transform)
257 {
258         assume(transform.HasUniformScale());
259         assume(transform.IsColOrthogonal());
260         l.Transform(transform);
261         r *= transform.Col(0).Length(); // Scale the radius.
262 }
263
264 void Capsule::Transform(const float4x4 &transform)
265 {
266         assume(transform.HasUniformScale());
267         assume(transform.IsColOrthogonal3());
268         l.Transform(transform);
269         r *= transform.Col3(0).Length(); // Scale the radius.
270 }
271
272 void Capsule::Transform(const Quat &transform)
273 {
274         l.Transform(transform);
275 }
276
277 vec Capsule::ClosestPoint(const vec &targetPoint) const
278 {
279         vec ptOnLine = l.ClosestPoint(targetPoint);
280         if (ptOnLine.DistanceSq(targetPoint) <= r*r)
281                 return targetPoint;
282         else
283                 return ptOnLine + (targetPoint - ptOnLine).ScaledToLength(r);
284 }
285
286 float Capsule::Distance(const vec &point) const
287 {
288         return Max(0.f, l.Distance(point) - r);
289 }
290
291 float Capsule::Distance(const Capsule &capsule) const
292 {
293         return Max(0.f, l.Distance(capsule.l) - r - capsule.r);
294 }
295
296 float Capsule::Distance(const Plane &plane) const
297 {
298         return plane.Distance(*this);
299 }
300
301 float Capsule::Distance(const Sphere &sphere) const
302 {
303         return Max(0.f, Distance(sphere.pos) - sphere.r);
304 }
305
306 float Capsule::Distance(const Ray &ray) const
307 {
308         return ray.Distance(*this);
309 }
310
311 float Capsule::Distance(const Line &line) const
312 {
313         return line.Distance(*this);
314 }
315
316 float Capsule::Distance(const LineSegment &lineSegment) const
317 {
318         return lineSegment.Distance(*this);
319 }
320
321 bool Capsule::Contains(const vec &point) const
322 {
323         return l.Distance(point) <= r;
324 }
325
326 bool Capsule::Contains(const LineSegment &lineSegment) const
327 {
328         return Contains(lineSegment.a) && Contains(lineSegment.b);
329 }
330
331 bool Capsule::Contains(const Triangle &triangle) const
332 {
333         return Contains(triangle.a) && Contains(triangle.b) && Contains(triangle.c);
334 }
335
336 bool Capsule::Contains(const Polygon &polygon) const
337 {
338         for(int i = 0; i < polygon.NumVertices(); ++i)
339                 if (!Contains(polygon.Vertex(i)))
340                         return false;
341         return true;
342 }
343
344 bool Capsule::Contains(const AABB &aabb) const
345 {
346         for(int i = 0; i < 8; ++i)
347                 if (!Contains(aabb.CornerPoint(i)))
348                         return false;
349
350         return true;
351 }
352
353 bool Capsule::Contains(const OBB &obb) const
354 {
355         for(int i = 0; i < 8; ++i)
356                 if (!Contains(obb.CornerPoint(i)))
357                         return false;
358
359         return true;
360 }
361
362 bool Capsule::Contains(const Frustum &frustum) const
363 {
364         for(int i = 0; i < 8; ++i)
365                 if (!Contains(frustum.CornerPoint(i)))
366                         return false;
367
368         return true;
369 }
370
371 bool Capsule::Contains(const Polyhedron &polyhedron) const
372 {
373         assume(polyhedron.IsClosed());
374         for(int i = 0; i < polyhedron.NumVertices(); ++i)
375                 if (!Contains(polyhedron.Vertex(i)))
376                         return false;
377
378         return true;
379 }
380
381 bool Capsule::Intersects(const Ray &ray) const
382 {
383         return l.Distance(ray) <= r;
384 }
385
386 bool Capsule::Intersects(const Line &line) const
387 {
388         return l.Distance(line) <= r;
389 }
390
391 bool Capsule::Intersects(const LineSegment &lineSegment) const
392 {
393         return l.Distance(lineSegment) <= r;
394 }
395
396 bool Capsule::Intersects(const Plane &plane) const
397 {
398         return l.Distance(plane) <= r;
399 }
400
401 bool Capsule::Intersects(const AABB &aabb) const
402 {
403         return GJKIntersect(*this, aabb);
404 }
405
406 bool Capsule::Intersects(const OBB &obb) const
407 {
408         return GJKIntersect(*this, obb);
409 }
410
411 /// [groupSyntax]
412 bool Capsule::Intersects(const Sphere &sphere) const
413 {
414         float R = r + sphere.r;
415         return l.DistanceSq(sphere.pos) <= R*R;
416 }
417
418 /// [groupSyntax]
419 bool Capsule::Intersects(const Capsule &capsule) const
420 {
421         float R = r + capsule.r;
422         return l.DistanceSq(capsule.l) <= R*R;
423 }
424
425 bool Capsule::Intersects(const Triangle &triangle) const
426 {
427         vec thisPoint;
428         vec trianglePoint = triangle.ClosestPoint(l, &thisPoint);
429         return thisPoint.DistanceSq(trianglePoint) <= r*r;
430 }
431
432 bool Capsule::Intersects(const Polygon &polygon) const
433 {
434         return polygon.Intersects(*this);
435 }
436
437 bool Capsule::Intersects(const Frustum &frustum) const
438 {
439         return frustum.Intersects(*this);
440 }
441
442 bool Capsule::Intersects(const Polyhedron &polyhedron) const
443 {
444         return polyhedron.Intersects(*this);
445 }
446
447 #ifdef MATH_ENABLE_STL_SUPPORT
448 std::string Capsule::ToString() const
449 {
450         char str[256];
451         sprintf(str, "Capsule(a:(%.2f, %.2f, %.2f) b:(%.2f, %.2f, %.2f), r:%.2f)", l.a.x, l.a.y, l.a.z, l.b.x, l.b.y, l.b.z, r);
452         return str;
453 }
454
455 std::string Capsule::SerializeToString() const
456 {
457         char str[256];
458         char *s = SerializeFloat(l.a.x, str); *s = ','; ++s;
459         s = SerializeFloat(l.a.y, s); *s = ','; ++s;
460         s = SerializeFloat(l.a.z, s); *s = ','; ++s;
461         s = SerializeFloat(l.b.x, s); *s = ','; ++s;
462         s = SerializeFloat(l.b.y, s); *s = ','; ++s;
463         s = SerializeFloat(l.b.z, s); *s = ','; ++s;
464         s = SerializeFloat(r, s);
465         assert(s+1 - str < 256);
466         MARK_UNUSED(s);
467         return str;
468 }
469
470 std::string Capsule::SerializeToCodeString() const
471 {
472         char str[256];
473         sprintf(str, "%.9g", r);
474         return "Capsule(" + l.SerializeToCodeString() + "," + str + ")";
475 }
476
477 std::ostream &operator <<(std::ostream &o, const Capsule &capsule)
478 {
479         o << capsule.ToString();
480         return o;
481 }
482
483 #endif
484
485 Capsule Capsule::FromString(const char *str, const char **outEndStr)
486 {
487         assume(str);
488         if (!str)
489                 return Capsule(vec::nan, vec::nan, FLOAT_NAN);
490         Capsule c;
491         MATH_SKIP_WORD(str, "Capsule(");
492         MATH_SKIP_WORD(str, "a:(");
493         MATH_SKIP_WORD(str, "LineSegment(");
494         c.l.a = PointVecFromString(str, &str);
495         MATH_SKIP_WORD(str, " b:");
496         c.l.b = PointVecFromString(str, &str);
497         MATH_SKIP_WORD(str, ")");
498         MATH_SKIP_WORD(str, ",");
499         MATH_SKIP_WORD(str, " r:");
500         c.r = DeserializeFloat(str, &str);
501         if (outEndStr)
502                 *outEndStr = str;
503         return c;
504 }
505
506 bool Capsule::BitEquals(const Capsule &other) const
507 {
508         return l.BitEquals(other.l) && ReinterpretAsU32(r) == ReinterpretAsU32(other.r);
509 }
510
511 Capsule operator *(const float3x3 &transform, const Capsule &capsule)
512 {
513         Capsule c(capsule);
514         c.Transform(transform);
515         return c;
516 }
517
518 Capsule operator *(const float3x4 &transform, const Capsule &capsule)
519 {
520         Capsule c(capsule);
521         c.Transform(transform);
522         return c;
523 }
524
525 Capsule operator *(const float4x4 &transform, const Capsule &capsule)
526 {
527         Capsule c(capsule);
528         c.Transform(transform);
529         return c;
530 }
531
532 Capsule operator *(const Quat &transform, const Capsule &capsule)
533 {
534         Capsule c(capsule);
535         c.Transform(transform);
536         return c;
537 }
538
539 MATH_END_NAMESPACE