PeriDyno 1.0.0
Loading...
Searching...
No Matches
Primitive3D.inl
Go to the documentation of this file.
1//#include "Primitive3D.h"
2#include "Math/SimpleMath.h"
3#include "Interval.h"
4#include <glm/glm.hpp>
5
6namespace dyno
7{
8 template<typename Real>
9 DYN_FUNC TPoint3D<Real>::TPoint3D()
10 {
11 origin = Coord3D(0);
12 }
13
14 template<typename Real>
15 DYN_FUNC TPoint3D<Real>::TPoint3D(const Real& val)
16 {
17 origin = Coord3D(val);
18 }
19
20 template<typename Real>
21 DYN_FUNC TPoint3D<Real>::TPoint3D(const Real& c0, const Real& c1, const Real& c2)
22 {
23 origin = Coord3D(c0, c1, c2);
24 }
25
26 template<typename Real>
27 DYN_FUNC TPoint3D<Real>::TPoint3D(const Coord3D& pos)
28 {
29 origin = pos;
30 }
31
32 template<typename Real>
33 DYN_FUNC TPoint3D<Real>::TPoint3D(const TPoint3D& pt)
34 {
35 origin = pt.origin;
36 }
37
38 template<typename Real>
39 DYN_FUNC TPoint3D<Real>& TPoint3D<Real>::operator=(const Coord3D& p)
40 {
41 origin = p;
42 return *this;
43 }
44
45 template<typename Real>
46 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TLine3D<Real>& line) const
47 {
48 Coord3D u = origin - line.origin;
49 Real tNum = u.dot(line.direction);
50 Real a = line.direction.normSquared();
51 Real t = a < REAL_EPSILON_SQUARED ? 0 : tNum / a;
52
53 return TPoint3D(line.origin + t * line.direction);
54 }
55
56 template<typename Real>
57 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TRay3D<Real>& ray) const
58 {
59 Coord3D u = origin - ray.origin;
60
61 Real tNum = u.dot(ray.direction);
62 Real a = ray.direction.normSquared();
63 Real t = a < REAL_EPSILON_SQUARED ? 0 : tNum / a;
64
65 t = t < 0 ? 0 : t;
66
67 return TPoint3D<Real>(ray.origin + t * ray.direction);
68 }
69
70 template<typename Real>
71 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TSegment3D<Real>& segment) const
72 {
73 Coord3D l = origin - segment.v0;
74 Coord3D dir = segment.v1 - segment.v0;
75 if (dir.normSquared() < REAL_EPSILON_SQUARED)
76 {
77 return TPoint3D<Real>(segment.v0);
78 }
79
80 Real t = l.dot(dir) / dir.normSquared();
81
82 Coord3D q = segment.v0 + t * dir;
83 q = t < 0 ? segment.v0 : q;
84 q = t > 1 ? segment.v1 : q;
85 //printf("T: %.3lf\n", t);
86 return TPoint3D<Real>(q);
87 }
88
89 template<typename Real>
90 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TPlane3D<Real>& plane) const
91 {
92 Real t = (origin - plane.origin).dot(plane.normal);
93
94 Real n2 = plane.normal.normSquared();
95
96 return n2 < REAL_EPSILON ? TPoint3D<Real>(plane.origin) : TPoint3D<Real>(origin - t / n2 * plane.normal);
97 }
98
99 template<typename Real>
100 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TTriangle3D<Real>& triangle) const
101 {
102 Coord3D dir = triangle.v[0] - origin;
103 Coord3D e0 = triangle.v[1] - triangle.v[0];
104 Coord3D e1 = triangle.v[2] - triangle.v[0];
105 Real a = e0.dot(e0);
106 Real b = e0.dot(e1);
107 Real c = e1.dot(e1);
108 Real d = e0.dot(dir);
109 Real e = e1.dot(dir);
110 Real f = dir.dot(dir);
111
112 Real det = a * c - b * b;
113 Real s = b * e - c * d;
114 Real t = b * d - a * e;
115
116 Real maxL = triangle.maximumEdgeLength();
117 //handle degenerate triangles
118 if (det < REAL_EPSILON * maxL * maxL)
119 {
120 Real g = (triangle.v[2] - triangle.v[1]).normSquared();
121
122 Real l_max = a;
123 Coord3D p0 = triangle.v[0];
124 Coord3D p1 = triangle.v[1];
125
126 if (c > l_max)
127 {
128 p0 = triangle.v[0];
129 p1 = triangle.v[2];
130
131 l_max = c;
132 }
133
134 if (g > l_max)
135 {
136 p0 = triangle.v[1];
137 p1 = triangle.v[2];
138 }
139
140 return project(TSegment3D<Real>(p0, p1));
141 }
142
143 if (s + t <= det)
144 {
145 if (s < 0)
146 {
147 if (t < 0)
148 {
149 //region 4
150 s = 0;
151 t = 0;
152 }
153 else
154 {
155 // region 3
156 // F(t) = Q(0, t) = ct^2 + 2et + f
157 // F'(t)/2 = ct + e
158 // F'(t) = 0 when t = -e/c
159
160 s = 0;
161 t = (e >= 0 ? 0 : (-e >= c ? 1 : -e / c));
162 }
163 }
164 else
165 {
166 if (t < 0)
167 {
168 //region 5
169 // F(s) = Q(s, 0) = as^2 + 2ds + f
170 // F'(s)/2 = as + d
171 // F'(s) = 0 when t = -d/a
172
173 s = (d >= 0 ? 0 : (-d >= a ? 1 : -d / a));
174 t = 0;
175 }
176 else
177 {
178 //region 0
179 Real invDet = 1 / det;
180 s *= invDet;
181 t *= invDet;
182 }
183 }
184 }
185 else
186 {
187 if (s < 0)
188 {
189 //region 2
190 s = 0;
191 t = 1;
192 }
193 else if (t < 0)
194 {
195 //region 6
196 s = 1;
197 t = 0;
198 }
199 else
200 {
201 //region 1
202 // F(s) = Q(s, 1 - s) = (a - 2b + c)s^2 + 2(b - c + d - e)s + (c + 2e + f)
203 // F'(s)/2 = (a - 2b + c)s + (b - c + d - e)
204 // F'(s} = 0 when s = (c + e - b - d)/(a - 2b + c)
205 // a - 2b + c = |e0 - e1|^2 > 0,
206 // so only the sign of c + e - b - d need be considered
207
208 Real numer = c + e - b - d;
209 if (numer <= 0) {
210 s = 0;
211 }
212 else {
213 Real denom = a - 2 * b + c; // positive quantity
214 s = (numer >= denom ? 1 : numer / denom);
215 }
216 t = 1 - s;
217 }
218 }
219 return TPoint3D<Real>(triangle.v[0] + s * e0 + t * e1);
220 }
221
222 template<typename Real>
223 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TRectangle3D<Real>& rectangle) const
224 {
225 Coord3D diff = origin - rectangle.center;
226 Real b0 = diff.dot(rectangle.axis[0]);
227 Real b1 = diff.dot(rectangle.axis[1]);
228 Coord2D uv(b0, b1);
229
230 uv = clamp(uv, -rectangle.extent, rectangle.extent);
231
232 return TPoint3D<Real>(rectangle.center + uv[0] * rectangle.axis[0] + uv[1] * rectangle.axis[1]);
233 }
234
235 template<typename Real>
236 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TDisk3D<Real>& disk) const
237 {
238 Coord3D cp = origin - disk.center;
239 Coord3D cq = cp - cp.dot(disk.normal) * disk.normal;
240
241 Coord3D q;
242 q = disk.center + cq;
243 if (cq.normSquared() > disk.radius * disk.radius)
244 {
245 q = disk.center + disk.radius * cq.normalize();
246 }
247 return TPoint3D<Real>(q);
248 }
249
250 template<typename Real>
251 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TSphere3D<Real>& sphere) const
252 {
253 Coord3D cp = origin - sphere.center;
254 Coord3D q = sphere.center + sphere.radius * cp.normalize();
255
256 return TPoint3D<Real>(q);
257 }
258
259 template<typename Real>
260 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TCapsule3D<Real>& capsule) const
261 {
262 Coord3D coordQ = project(capsule.centerline()).origin;
263 Coord3D dir = origin - coordQ;
264
265 return TPoint3D<Real>(coordQ + capsule.radius * dir.normalize());
266 }
267
268 template<typename Real>
269 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TTet3D<Real>& tet) const
270 {
271 TPoint3D<Real> closestPt;
272 Real minDist = REAL_MAX;
273 for (int i = 0; i < 4; i++)
274 {
275 TTriangle3D<Real> face = tet.face(i);
276 TPoint3D<Real> q = project(tet.face(i));
277 Real d = (origin - q.origin).normSquared();
278 if (d < minDist)
279 {
280 minDist = d;
281 closestPt = q;
282 }
283 }
284
285 return closestPt;
286 }
287
288 template<typename Real>
289 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TAlignedBox3D<Real>& abox) const
290 {
291 bool bInside = inside(abox);
292
293 if (!bInside)
294 {
295 return TPoint3D<Real>(clamp(origin, abox.v0, abox.v1));
296 }
297
298 //compute the distance to six faces
299 Coord3D q;
300 Real minDist = REAL_MAX;
301 Coord3D offset0 = abs(origin - abox.v0);
302 if (offset0[0] < minDist)
303 {
304 q = Coord3D(abox.v0[0], origin[1], origin[2]);
305 minDist = offset0[0];
306 }
307
308 if (offset0[1] < minDist)
309 {
310 q = Coord3D(origin[0], abox.v0[1], origin[2]);
311 minDist = offset0[1];
312 }
313
314 if (offset0[2] < minDist)
315 {
316 q = Coord3D(origin[0], origin[1], abox.v0[2]);
317 minDist = offset0[2];
318 }
319
320
321 Coord3D offset1 = abs(origin - abox.v1);
322 if (offset1[0] < minDist)
323 {
324 q = Coord3D(abox.v1[0], origin[1], origin[2]);
325 minDist = offset1[0];
326 }
327
328 if (offset1[1] < minDist)
329 {
330 q = Coord3D(origin[0], abox.v1[1], origin[2]);
331 minDist = offset1[1];
332 }
333
334 if (offset1[2] < minDist)
335 {
336 q = Coord3D(origin[0], origin[1], abox.v1[2]);
337 minDist = offset1[2];
338 }
339
340 return TPoint3D<Real>(q);
341 }
342
343 template<typename Real>
344 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TOrientedBox3D<Real>& obb) const
345 {
346 Coord3D offset = origin - obb.center;
347 Coord3D pPrime(offset.dot(obb.u), offset.dot(obb.v), offset.dot(obb.w));
348
349 Coord3D qPrime = TPoint3D<Real>(pPrime).project(TAlignedBox3D<Real>(-obb.extent, obb.extent)).origin;
350
351 Coord3D q = obb.center + qPrime[0] * obb.u + qPrime[1] * obb.v + qPrime[2] * obb.w;
352
353 return TPoint3D<Real>(q);
354 }
355
356 template<typename Real>
357 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TSphere3D<Real>& sphere, Bool& bInside) const
358 {
359 Coord3D cp = origin - sphere.center;
360 Coord3D q = sphere.center + sphere.radius * cp.normalize();
361
362 bInside = cp.normSquared() >= sphere.radius * sphere.radius ? false : true;
363
364 return TPoint3D<Real>(q);
365 }
366
367 template<typename Real>
368 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TCapsule3D<Real>& capsule, Bool& bInside /*= Bool(false)*/) const
369 {
370 Coord3D coordQ = project(capsule.centerline()).origin;
371 Coord3D dir = origin - coordQ;
372
373 bInside = dir.normSquared() < capsule.radius * capsule.radius ? true : false;
374 return TPoint3D<Real>(coordQ + capsule.radius * dir.normalize());
375 }
376
377 template<typename Real>
378 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TTet3D<Real>& tet, Bool& bInside) const
379 {
380 bInside = inside(tet);
381
382 TPoint3D<Real> closestPt;
383 Real minDist = REAL_MAX;
384 for (int i = 0; i < 4; i++)
385 {
386 TPoint3D<Real> q = project(tet.face(i));
387 Real d = (origin - q.origin).normSquared();
388 if (d < minDist)
389 {
390 minDist = d;
391 closestPt = q;
392 }
393 }
394
395 return closestPt;
396 }
397
398 template<typename Real>
399 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TTet3D<Real>& tet, Bool& bInside, int* idx) const
400 {
401 bInside = true;
402
403 int idx2;
404 TPoint3D<Real> closestPt;
405 Real minDist = REAL_MAX;
406 for (int i = 0; i < 4; i++)
407 {
408 TTriangle3D<Real> face = tet.face(i);
409 TPoint3D<Real> q = project(tet.face(i));
410 Real d = (origin - q.origin).normSquared();
411 if (d < minDist)
412 {
413 minDist = d;
414 closestPt = q;
415 if (i == 0 || i == 3)
416 idx2 = 3 - i;
417 else
418 idx2 = i;
419 }
420 bInside &= (origin - face.v[0]).dot(face.normal()) < Real(0);
421 }
422
423 if (idx != NULL)
424 {
425 *idx = idx2;
426 }
427
428 return closestPt;
429 }
430
431 template<typename Real>
432 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TAlignedBox3D<Real>& abox, Bool& bInside) const
433 {
434 bInside = inside(abox);
435
436 if (!bInside)
437 {
438 return TPoint3D<Real>(clamp(origin, abox.v0, abox.v1));
439 }
440
441 //compute the distance to six faces
442 Coord3D q;
443 Real minDist = REAL_MAX;
444 Coord3D offset0 = abs(origin - abox.v0);
445 if (offset0[0] < minDist)
446 {
447 q = Coord3D(abox.v0[0], origin[1], origin[2]);
448 minDist = offset0[0];
449 }
450
451 if (offset0[1] < minDist)
452 {
453 q = Coord3D(origin[0], abox.v0[1], origin[2]);
454 minDist = offset0[1];
455 }
456
457 if (offset0[2] < minDist)
458 {
459 q = Coord3D(origin[0], origin[1], abox.v0[2]);
460 minDist = offset0[2];
461 }
462
463
464 Coord3D offset1 = abs(origin - abox.v1);
465 if (offset1[0] < minDist)
466 {
467 q = Coord3D(abox.v1[0], origin[1], origin[2]);
468 minDist = offset1[0];
469 }
470
471 if (offset1[1] < minDist)
472 {
473 q = Coord3D(origin[0], abox.v1[1], origin[2]);
474 minDist = offset1[1];
475 }
476
477 if (offset1[2] < minDist)
478 {
479 q = Coord3D(origin[0], origin[1], abox.v1[2]);
480 minDist = offset1[2];
481 }
482
483 return TPoint3D<Real>(q);
484 }
485
486 template<typename Real>
487 DYN_FUNC TPoint3D<Real> TPoint3D<Real>::project(const TOrientedBox3D<Real>& obb, Bool& bInside) const
488 {
489 Coord3D offset = origin - obb.center;
490 Coord3D pPrime(offset.dot(obb.u), offset.dot(obb.v), offset.dot(obb.w));
491
492 Coord3D qPrime = TPoint3D<Real>(pPrime).project(TAlignedBox3D<Real>(-obb.extent, obb.extent), bInside).origin;
493
494 Coord3D q = obb.center + qPrime[0] * obb.u + qPrime[1] * obb.v + qPrime[2] * obb.w;
495
496 return TPoint3D<Real>(q);
497 }
498
499 template<typename Real>
500 DYN_FUNC Real TPoint3D<Real>::distance(const TPoint3D<Real>& pt) const
501 {
502 return (origin - pt.origin).norm();
503 }
504
505 template<typename Real>
506 DYN_FUNC Real TPoint3D<Real>::distance(const TLine3D<Real>& line) const
507 {
508 return (origin - project(line).origin).norm();
509 }
510
511 template<typename Real>
512 DYN_FUNC Real TPoint3D<Real>::distance(const TRay3D<Real>& ray) const
513 {
514 return (origin - project(ray).origin).norm();
515 }
516
517 template<typename Real>
518 DYN_FUNC Real TPoint3D<Real>::distance(const TSegment3D<Real>& segment) const
519 {
520 return (origin - project(segment).origin).norm();
521 }
522
523 template<typename Real>
524 DYN_FUNC Real TPoint3D<Real>::distance(const TPlane3D<Real>& plane) const
525 {
526 Coord3D q = project(plane).origin;
527 Real sign = (origin - q).dot(plane.normal) < Real(0) ? Real(-1) : Real(1);
528
529 return sign * (origin - q).norm();
530 }
531
532 template<typename Real>
533 DYN_FUNC Real TPoint3D<Real>::distance(const TTriangle3D<Real>& triangle) const
534 {
535 Coord3D q = project(triangle).origin;
536 Real sign;
537 Real sign0 = (origin - q).dot(triangle.normal());
538 if (abs(sign0) < Real(REAL_EPSILON) && (origin - q).norm() > Real(REAL_EPSILON))
539 sign = Real(1);
540 else
541 sign = sign0 < Real(0) ? Real(-1) : Real(1);
542 //Real sign = (origin - q).dot(triangle.normal()) < Real(0) ? Real(-1) : Real(1);
543
544 return sign * (origin - q).norm();
545 }
546
547 template<typename Real>
548 DYN_FUNC Real TPoint3D<Real>::distance(const TRectangle3D<Real>& rectangle) const
549 {
550 Coord3D q = project(rectangle).origin;
551 Real sign = (origin - q).dot(rectangle.normal()) < Real(0) ? Real(-1) : Real(1);
552
553 return (origin - q).norm();
554 }
555
556 template<typename Real>
557 DYN_FUNC Real TPoint3D<Real>::distance(const TDisk3D<Real>& disk) const
558 {
559 Coord3D q = project(disk).origin;
560 Real sign = (origin - q).dot(disk.normal) < Real(0) ? Real(-1) : Real(1);
561
562 return (origin - q).norm();
563 }
564
565 template<typename Real>
566 DYN_FUNC Real TPoint3D<Real>::distance(const TSphere3D<Real>& sphere) const
567 {
568 return (origin - sphere.center).norm() - sphere.radius;
569 }
570
571 template<typename Real>
572 DYN_FUNC Real TPoint3D<Real>::distance(const TTet3D<Real>& tet) const
573 {
574 Bool bInside;
575 Real d = (origin - project(tet, bInside).origin).norm();
576 return bInside == true ? -d : d;
577 }
578
579 template<typename Real>
580 DYN_FUNC Real TPoint3D<Real>::distance(const TAlignedBox3D<Real>& abox) const
581 {
582 Bool bInside;
583 Real d = (origin - project(abox, bInside).origin).norm();
584 return bInside == true ? -d : d;
585 }
586
587 template<typename Real>
588 DYN_FUNC Real TPoint3D<Real>::distance(const TOrientedBox3D<Real>& obb) const
589 {
590 Bool bInside;
591 Real d = (origin - project(obb, bInside).origin).norm();
592 return bInside == true ? -d : d;
593 }
594
595 template<typename Real>
596 DYN_FUNC Real TPoint3D<Real>::distance(const TCapsule3D<Real>& capsule) const
597 {
598 Bool bInside;
599 Real d = (origin - project(capsule, bInside).origin).norm();
600 return bInside == true ? -d : d;
601 }
602
603 template<typename Real>
604 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TPoint3D& pt) const
605 {
606 return (origin - pt.origin).normSquared();
607 }
608
609 template<typename Real>
610 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TLine3D<Real>& line) const
611 {
612 return (origin - project(line).origin).normSquared();
613 }
614
615 template<typename Real>
616 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TRay3D<Real>& ray) const
617 {
618 return (origin - project(ray).origin).normSquared();
619 }
620
621 template<typename Real>
622 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TSegment3D<Real>& segment) const
623 {
624 return (origin - project(segment).origin).normSquared();
625 }
626
627 template<typename Real>
628 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TPlane3D<Real>& plane) const
629 {
630 return (origin - project(plane).origin).normSquared();
631 }
632
633 template<typename Real>
634 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TTriangle3D<Real>& triangle) const
635 {
636 return (origin - project(triangle).origin).normSquared();
637 }
638
639 template<typename Real>
640 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TRectangle3D<Real>& rectangle) const
641 {
642 return (origin - project(rectangle).origin).normSquared();
643 }
644
645 template<typename Real>
646 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TDisk3D<Real>& disk) const
647 {
648 return (origin - project(disk).origin).normSquared();
649 }
650
651 template<typename Real>
652 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TSphere3D<Real>& sphere) const
653 {
654 return (origin - project(sphere).origin).normSquared();
655 }
656
657 template<typename Real>
658 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TAlignedBox3D<Real>& abox) const
659 {
660 return (origin - project(abox).origin).normSquared();
661 }
662
663 template<typename Real>
664 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TOrientedBox3D<Real>& obb) const
665 {
666 return (origin - project(obb).origin).normSquared();
667 }
668
669 template<typename Real>
670 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TTet3D<Real>& tet) const
671 {
672 return (origin - project(tet).origin).normSquared();
673 }
674
675 template<typename Real>
676 DYN_FUNC Real TPoint3D<Real>::distanceSquared(const TCapsule3D<Real>& capsule) const
677 {
678 return (origin - project(capsule).origin).normSquared();
679 }
680
681 template<typename Real>
682 DYN_FUNC bool TPoint3D<Real>::inside(const TLine3D<Real>& line) const
683 {
684 if (!line.isValid())
685 {
686 return false;
687 }
688
689 return (origin - line.origin).cross(line.direction).normSquared() < REAL_EPSILON_SQUARED;
690 }
691
692 template<typename Real>
693 DYN_FUNC bool TPoint3D<Real>::inside(const TRay3D<Real>& ray) const
694 {
695 if (!inside(TLine3D<Real>(ray.origin, ray.direction)))
696 {
697 return false;
698 }
699
700 Coord3D offset = origin - ray.origin;
701 Real t = offset.dot(ray.direction);
702
703 return t > Real(0);
704 }
705
706 template<typename Real>
707 DYN_FUNC bool TPoint3D<Real>::inside(const TSegment3D<Real>& segment) const
708 {
709 Coord3D dir = segment.direction();
710 if (!inside(TLine3D<Real>(segment.startPoint(), dir)))
711 {
712 return false;
713 }
714
715 Coord3D offset = origin - segment.startPoint();
716 Real t = offset.dot(dir) / dir.normSquared();
717
718 return t > Real(0) && t < Real(1);
719 }
720
721 template<typename Real>
722 DYN_FUNC bool TPoint3D<Real>::inside(const TPlane3D<Real>& plane) const
723 {
724 if (!plane.isValid())
725 {
726 return false;
727 }
728
729 return abs((origin - plane.origin).dot(plane.normal)) < REAL_EPSILON;
730 }
731
732 template<typename Real>
733 DYN_FUNC bool TPoint3D<Real>::inside(const TTriangle3D<Real>& triangle) const
734 {
735 TPlane3D<Real> plane(triangle.v[0], triangle.normal());
736 if (!inside(plane))
737 {
738 return false;
739 }
740
741 typename TTriangle3D<Real>::Param tParam;
742 bool bValid = triangle.computeBarycentrics(origin, tParam);
743 if (bValid)
744 {
745 return tParam.u > Real(0) && tParam.u < Real(1) && tParam.v > Real(0) && tParam.v < Real(1) && tParam.w > Real(0) && tParam.w < Real(1);
746 }
747 else
748 {
749 return false;
750 }
751 }
752
753 template<typename Real>
754 DYN_FUNC bool TPoint3D<Real>::inside(const TRectangle3D<Real>& rectangle) const
755 {
756 TPlane3D<Real> plane(rectangle.center, rectangle.normal());
757 if (!inside(plane))
758 {
759 return false;
760 }
761
762 typename TRectangle3D<Real>::Param recParam;
763 bool bValid = rectangle.computeParams(origin, recParam);
764 if (bValid)
765 {
766 return recParam.u < rectangle.extent[0] && recParam.u > -rectangle.extent[0] && recParam.v < rectangle.extent[1] && recParam.v > -rectangle.extent[1];
767 }
768 else
769 {
770 return false;
771 }
772 }
773
774 template<typename Real>
775 DYN_FUNC bool TPoint3D<Real>::inside(const TDisk3D<Real>& disk) const
776 {
777 TPlane3D<Real> plane(disk.center, disk.normal);
778 if (!inside(plane))
779 {
780 return false;
781 }
782
783 return (origin - disk.center).normSquared() < disk.radius * disk.radius;
784 }
785
786 template<typename Real>
787 DYN_FUNC bool TPoint3D<Real>::inside(const TSphere3D<Real>& sphere) const
788 {
789 return (origin - sphere.center).normSquared() < sphere.radius * sphere.radius;
790 }
791
792 template<typename Real>
793 DYN_FUNC bool TPoint3D<Real>::inside(const TCapsule3D<Real>& capsule) const
794 {
795 return distanceSquared(capsule.centerline()) < capsule.radius * capsule.radius;
796 }
797
798 template<typename Real>
799 DYN_FUNC bool TPoint3D<Real>::inside(const TTet3D<Real>& tet) const
800 {
801 bool bInside = true;
802
803 TTriangle3D<Real> face;
804 Coord3D normal;
805 face = tet.face(0);
806 normal = face.normal();
807 bInside &= (origin - face.v[0]).dot(normal) * (tet.v[0] - face.v[0]).dot(normal) > 0;
808
809 face = tet.face(1);
810 normal = face.normal();
811 bInside &= (origin - face.v[0]).dot(normal) * (tet.v[1] - face.v[0]).dot(normal) > 0;
812
813 face = tet.face(2);
814 normal = face.normal();
815 bInside &= (origin - face.v[0]).dot(normal) * (tet.v[2] - face.v[0]).dot(normal) > 0;
816
817 face = tet.face(3);
818 normal = face.normal();
819 bInside &= (origin - face.v[0]).dot(normal) * (tet.v[3] - face.v[0]).dot(normal) > 0;
820
821 return bInside;
822 }
823
824 template<typename Real>
825 DYN_FUNC bool TPoint3D<Real>::inside(const TOrientedBox3D<Real>& obb) const
826 {
827 Coord3D offset = origin - obb.center;
828 Coord3D pPrime(offset.dot(obb.u), offset.dot(obb.v), offset.dot(obb.w));
829
830 bool bInside = true;
831 bInside &= pPrime[0] < obb.extent[0] && pPrime[0] > -obb.extent[0];
832 bInside &= pPrime[1] < obb.extent[1] && pPrime[1] > -obb.extent[1];
833 bInside &= pPrime[2] < obb.extent[2] && pPrime[2] > -obb.extent[2];
834 return bInside;
835 }
836
837 template<typename Real>
838 DYN_FUNC bool TPoint3D<Real>::inside(const TAlignedBox3D<Real>& box) const
839 {
840 Coord3D offset = origin - box.v0;
841 Coord3D extent = box.v1 - box.v0;
842
843 bool bInside = true;
844 bInside &= offset[0] < extent[0] && offset[0] > Real(0);
845 bInside &= offset[1] < extent[1] && offset[1] > Real(0);
846 bInside &= offset[2] < extent[2] && offset[2] > Real(0);
847
848 return bInside;
849 }
850
851 template<typename Real>
852 DYN_FUNC const TSegment3D<Real> TPoint3D<Real>::operator-(const TPoint3D<Real>& pt) const
853 {
854 return TSegment3D<Real>(pt.origin, origin);
855 }
856
857 template<typename Real>
858 DYN_FUNC TLine3D<Real>::TLine3D()
859 {
860 origin = Coord3D(0);
861 direction = Coord3D(1, 0, 0);
862 }
863
864 template<typename Real>
865 DYN_FUNC TLine3D<Real>::TLine3D(const Coord3D& pos, const Coord3D& dir)
866 {
867 origin = pos;
868 direction = dir;
869 }
870
871 template<typename Real>
872 DYN_FUNC TLine3D<Real>::TLine3D(const TLine3D<Real>& line)
873 {
874 origin = line.origin;
875 direction = line.direction;
876 }
877
878 template<typename Real>
879 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TLine3D<Real>& line) const
880 {
881 Coord3D u = origin - line.origin;
882 Real a = direction.normSquared();
883 Real b = direction.dot(line.direction);
884 Real c = line.direction.normSquared();
885 Real d = u.dot(direction);
886 Real e = u.dot(line.direction);
887 Real f = u.normSquared();
888 Real det = a * c - b * b;
889
890 if (det < REAL_EPSILON)
891 {
892 TPoint3D<Real> p = TPoint3D<Real>(line.origin);
893 return c < REAL_EPSILON ? p - p.project(*this) : TSegment3D<Real>(origin, line.origin + e / c * line.direction);
894 }
895 else
896 {
897 Real invDet = 1 / det;
898 Real s = (b * e - c * d) * invDet;
899 Real t = (a * e - b * d) * invDet;
900 return TSegment3D<Real>(origin + s * direction, line.origin + t * line.direction);
901 }
902 }
903
904 template<typename Real>
905 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TRay3D<Real>& ray) const
906 {
907 Coord3D u = origin - ray.origin;
908 Real a = direction.normSquared();
909 Real b = direction.dot(ray.direction);
910 Real c = ray.direction.normSquared();
911 Real d = u.dot(direction);
912 Real e = u.dot(ray.direction);
913 Real det = a * c - b * b;
914
915 if (det < REAL_EPSILON)
916 {
917 TPoint3D<Real> p0(origin);
918 TPoint3D<Real> p1(ray.origin);
919
920 return a < REAL_EPSILON ? p0.project(*this) - p0 : p1 - p1.project(*this);
921 }
922
923 Real sNum = b * e - c * d;
924 Real tNum = a * e - b * d;
925
926 Real sDenom = det;
927 Real tDenom = det;
928
929 if (tNum < 0) {
930 tNum = 0;
931 sNum = -d;
932 sDenom = a;
933 }
934 // Parameters of nearest points on restricted domain
935 Real s = sNum / sDenom;
936 Real t = tNum / tDenom;
937
938 return TSegment3D<Real>(origin + (s * direction), ray.origin + (t * ray.direction));
939 }
940
941 template<typename Real>
942 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TSegment3D<Real>& segment) const
943 {
944 Coord3D u = origin - segment.startPoint();
945 Coord3D dir1 = segment.endPoint() - segment.startPoint();
946 Real a = direction.normSquared();
947 Real b = direction.dot(dir1);
948 Real c = dir1.dot(dir1);
949 Real d = u.dot(direction);
950 Real e = u.dot(dir1);
951 Real det = a * c - b * b;
952
953 if (det < REAL_EPSILON)
954 {
955 TPoint3D<Real> p0(origin);
956 TPoint3D<Real> p1(segment.startPoint());
957
958 return a < REAL_EPSILON ? p0.project(*this) - p0 : p1 - p1.project(*this);
959 }
960
961 Real sNum = b * e - c * d;
962 Real tNum = a * e - b * d;
963
964 Real sDenom = det;
965 Real tDenom = det;
966
967 // Check t
968 if (tNum < 0) {
969 tNum = 0;
970 sNum = -d;
971 sDenom = a;
972 }
973 else if (tNum > tDenom) {
974 tNum = tDenom;
975 sNum = -d + b;
976 sDenom = a;
977 }
978 // Parameters of nearest points on restricted domain
979 Real s = sNum / sDenom;
980 Real t = tNum / tDenom;
981
982 return TSegment3D<Real>(origin + (s * direction), segment.startPoint() + (t * dir1));
983 }
984
985 template<typename Real>
986 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TTriangle3D<Real>& triangle) const
987 {
988 Coord3D e0 = triangle.v[1] - triangle.v[0];
989 Coord3D e1 = triangle.v[2] - triangle.v[0];
990 Coord3D normal = e0.cross(e1);
991 Real NdD = normal.dot(direction);
992 if (abs(NdD) > REAL_EPSILON)
993 {
994 // The line and triangle are not parallel, so the line
995 // intersects/ the plane of the triangle.
996 Coord3D diff = origin - triangle.v[0];
997
998 //compute a right - handed orthonormal basis
999 Coord3D W = direction;
1000 W.normalize();
1001 Coord3D U = W[0] > W[1] ? Coord3D(-W[2], (Real)0, W[0]) : Coord3D((Real)0, W[2], -W[1]);
1002 Coord3D V = W.cross(U);
1003
1004 Real UdE0 = U.dot(e0);
1005 Real UdE1 = U.dot(e1);
1006 Real UdDiff = U.dot(diff);
1007 Real VdE0 = V.dot(e0);
1008 Real VdE1 = V.dot(e1);
1009 Real VdDiff = V.dot(diff);
1010 Real invDet = ((Real)1) / (UdE0 * VdE1 - UdE1 * VdE0);
1011
1012 // Barycentric coordinates for the point of intersection.
1013 Real u = (VdE1 * UdDiff - UdE1 * VdDiff) * invDet;
1014 Real v = (UdE0 * VdDiff - VdE0 * UdDiff) * invDet;
1015 Real b0 = (Real)1 - u - v;
1016
1017 if (b0 >= (Real)0 && u >= (Real)0 && v >= (Real)0)
1018 {
1019 // Line parameter for the point of intersection.
1020 Real DdE0 = W.dot(e0);
1021 Real DdE1 = W.dot(e1);
1022 Real DdDiff = W.dot(diff);
1023 Real t = u * DdE0 + v * DdE1 - DdDiff;
1024
1025 return TSegment3D<Real>(origin + t * W, triangle.v[0] + u * e0 + v * e1);
1026 }
1027 }
1028
1029 if (direction.normSquared() < EPSILON)
1030 {
1031 TPoint3D<Real> p(origin);
1032 return p.project(triangle) - p;
1033 }
1034 else
1035 {
1036 // Treat line and triangle as parallel. Compute
1037 // closest points by computing distance from
1038 // line to each triangle edge and taking minimum.
1039 TSegment3D<Real> e0(triangle.v[0], triangle.v[1]);
1040 TSegment3D<Real> minPQ = proximity(e0);
1041 Real minDS = minPQ.lengthSquared();
1042
1043 TSegment3D<Real> e1(triangle.v[0], triangle.v[2]);
1044 TSegment3D<Real> pq1 = proximity(e1);
1045 if (pq1.lengthSquared() < minDS)
1046 {
1047 minPQ = pq1;
1048 minDS = pq1.lengthSquared();
1049 }
1050
1051 TSegment3D<Real> e2(triangle.v[1], triangle.v[2]);
1052 TSegment3D<Real> pq2 = proximity(e2);
1053 if (pq2.lengthSquared() < minDS)
1054 {
1055 minPQ = pq2;
1056 }
1057
1058 return minPQ;
1059 }
1060 }
1061
1062 template<typename Real>
1063 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TRectangle3D<Real>& rectangle) const
1064 {
1065 Coord3D N = rectangle.axis[0].cross(rectangle.axis[1]);
1066 Real NdD = N.dot(direction);
1067 if (abs(NdD) > REAL_EPSILON)
1068 {
1069 // The line and rectangle are not parallel, so the line
1070 // intersects the plane of the rectangle.
1071 Coord3D diff = origin - rectangle.center;
1072 Coord3D W = direction;
1073 W.normalize();
1074 Coord3D U = W[0] > W[1] ? Coord3D(-W[2], (Real)0, W[0]) : Coord3D((Real)0, W[2], -W[1]);
1075 Coord3D V = W.cross(U);
1076 Real UdD0 = U.dot(rectangle.axis[0]);
1077 Real UdD1 = U.dot(rectangle.axis[1]);
1078 Real UdPmC = U.dot(diff);
1079 Real VdD0 = V.dot(rectangle.axis[0]);
1080 Real VdD1 = V.dot(rectangle.axis[1]);
1081 Real VdPmC = V.dot(diff);
1082 Real invDet = ((Real)1) / (UdD0 * VdD1 - UdD1 * VdD0);
1083
1084 // Rectangle coordinates for the point of intersection.
1085 Real s0 = (VdD1 * UdPmC - UdD1 * VdPmC) * invDet;
1086 Real s1 = (UdD0 * VdPmC - VdD0 * UdPmC) * invDet;
1087
1088 if (abs(s0) <= rectangle.extent[0] && abs(s1) <= rectangle.extent[1])
1089 {
1090 // Line parameter for the point of intersection.
1091 Real DdD0 = direction.dot(rectangle.axis[0]);
1092 Real DdD1 = direction.dot(rectangle.axis[1]);
1093 Real DdDiff = direction.dot(diff);
1094 Real t = s0 * DdD0 + s1 * DdD1 - DdDiff;
1095
1096 return TSegment3D<Real>(origin + t * direction, rectangle.center + s0 * rectangle.axis[0] + s1 * rectangle.axis[1]);
1097 }
1098 }
1099
1100 Real minDS = REAL_MAX;
1101 TSegment3D<Real> minPQ;
1102
1103 for (int i = 0; i < 4; i++)
1104 {
1105 TSegment3D<Real> pq = proximity(rectangle.edge(i));
1106 if (pq.lengthSquared() < minDS)
1107 {
1108 minDS = pq.lengthSquared();
1109 minPQ = pq;
1110 }
1111 }
1112
1113 return minPQ;
1114 }
1115
1116 template<typename Real>
1117 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TSphere3D<Real>& sphere) const
1118 {
1119 Coord3D offset = sphere.center - origin;
1120 Real d2 = direction.normSquared();
1121 if (d2 < REAL_EPSILON)
1122 {
1123 return TPoint3D<Real>(origin).project(sphere) - TPoint3D<Real>(origin);
1124 }
1125
1126 Coord3D p = origin + offset.dot(direction) / d2 * direction;
1127
1128 return TPoint3D<Real>(p).project(sphere) - TPoint3D<Real>(p);
1129 }
1130
1131 template<typename Real>
1132 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TAlignedBox3D<Real>& box) const
1133 {
1134 Coord3D boxCenter, boxExtent;
1135 boxCenter = 0.5 * (box.v0 + box.v1);
1136 boxExtent = 0.5 * (box.v1 - box.v0);
1137 Coord3D point = origin - boxCenter;
1138 Coord3D lineDir = direction;
1139
1140 Real t = Real(0);
1141
1142 auto case00 = [](Coord3D& pt, Real& t, const Coord3D dir, const Coord3D extent, const int axis) {
1143 t = (extent[axis] - pt[axis]) / dir[axis];
1144 if (t < Real(0) || t > Real(1))
1145 {
1146 t = (-extent[axis] - pt[axis]) / dir[axis];
1147 pt[axis] = -extent[axis];
1148 }
1149 else
1150 pt[axis] = extent[axis];
1151
1152 pt = clamp(pt, -extent, extent);
1153 };
1154
1155 auto case0 = [](Coord3D& pt, Real& t, const Coord3D dir, const Coord3D extent, const int i0, const int i1, const int i2) {
1156 Real PmE0 = pt[i0] - extent[i0];
1157 Real PmE1 = pt[i1] - extent[i1];
1158 Real prod0 = dir[i1] * PmE0;
1159 Real prod1 = dir[i0] * PmE1;
1160
1161 if (prod0 >= prod1)
1162 {
1163 // line intersects P[i0] = e[i0]
1164 pt[i0] = extent[i0];
1165
1166 Real PpE1 = pt[i1] + extent[i1];
1167 Real delta = prod0 - dir[i0] * PpE1;
1168 if (delta >= (Real)0)
1169 {
1170 Real invLSqr = ((Real)1) / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
1171 pt[i1] = -extent[i1];
1172 t = -(dir[i0] * PmE0 + dir[i1] * PpE1) * invLSqr;
1173 }
1174 else
1175 {
1176 Real inv = ((Real)1) / dir[i0];
1177 pt[i1] -= prod0 * inv;
1178 t = -PmE0 * inv;
1179 }
1180 }
1181 else
1182 {
1183 // line intersects P[i1] = e[i1]
1184 pt[i1] = extent[i1];
1185
1186 Real PpE0 = pt[i0] + extent[i0];
1187 Real delta = prod1 - dir[i1] * PpE0;
1188 if (delta >= (Real)0)
1189 {
1190 Real invLSqr = ((Real)1) / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
1191 pt[i0] = -extent[i0];
1192 t = -(dir[i0] * PpE0 + dir[i1] * PmE1) * invLSqr;
1193 }
1194 else
1195 {
1196 Real inv = ((Real)1) / dir[i1];
1197 pt[i0] -= prod1 * inv;
1198 t = -PmE1 * inv;
1199 }
1200 }
1201
1202 if (pt[i2] < -extent[i2])
1203 {
1204 pt[i2] = -extent[i2];
1205 }
1206 else if (pt[i2] > extent[i2])
1207 {
1208 pt[i2] = extent[i2];
1209 }
1210 };
1211
1212 auto face = [](Coord3D& pnt, Real& final_t,
1213 const Coord3D& dir, const Coord3D& PmE, const Coord3D& boxExtent,
1214 int i0, int i1, int i2)
1215 {
1216 Coord3D PpE;
1217 Real lenSqr, inv, tmp, param, t, delta;
1218
1219 PpE[i1] = pnt[i1] + boxExtent[i1];
1220 PpE[i2] = pnt[i2] + boxExtent[i2];
1221 if (dir[i0] * PpE[i1] >= dir[i1] * PmE[i0])
1222 {
1223 if (dir[i0] * PpE[i2] >= dir[i2] * PmE[i0])
1224 {
1225 // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
1226 pnt[i0] = boxExtent[i0];
1227 inv = ((Real)1) / dir[i0];
1228 pnt[i1] -= dir[i1] * PmE[i0] * inv;
1229 pnt[i2] -= dir[i2] * PmE[i0] * inv;
1230 final_t = -PmE[i0] * inv;
1231 }
1232 else
1233 {
1234 // v[i1] >= -e[i1], v[i2] < -e[i2]
1235 lenSqr = dir[i0] * dir[i0] + dir[i2] * dir[i2];
1236 tmp = lenSqr * PpE[i1] - dir[i1] * (dir[i0] * PmE[i0] +
1237 dir[i2] * PpE[i2]);
1238 if (tmp <= ((Real)2) * lenSqr * boxExtent[i1])
1239 {
1240 t = tmp / lenSqr;
1241 lenSqr += dir[i1] * dir[i1];
1242 tmp = PpE[i1] - t;
1243 delta = dir[i0] * PmE[i0] + dir[i1] * tmp + dir[i2] * PpE[i2];
1244 param = -delta / lenSqr;
1245
1246 final_t = param;
1247 pnt[i0] = boxExtent[i0];
1248 pnt[i1] = t - boxExtent[i1];
1249 pnt[i2] = -boxExtent[i2];
1250 }
1251 else
1252 {
1253 lenSqr += dir[i1] * dir[i1];
1254 delta = dir[i0] * PmE[i0] + dir[i1] * PmE[i1] + dir[i2] * PpE[i2];
1255 param = -delta / lenSqr;
1256
1257 final_t = param;
1258 pnt[i0] = boxExtent[i0];
1259 pnt[i1] = boxExtent[i1];
1260 pnt[i2] = -boxExtent[i2];
1261 }
1262 }
1263 }
1264 else
1265 {
1266 if (dir[i0] * PpE[i2] >= dir[i2] * PmE[i0])
1267 {
1268 // v[i1] < -e[i1], v[i2] >= -e[i2]
1269 lenSqr = dir[i0] * dir[i0] + dir[i1] * dir[i1];
1270 tmp = lenSqr * PpE[i2] - dir[i2] * (dir[i0] * PmE[i0] +
1271 dir[i1] * PpE[i1]);
1272 if (tmp <= ((Real)2) * lenSqr * boxExtent[i2])
1273 {
1274 t = tmp / lenSqr;
1275 lenSqr += dir[i2] * dir[i2];
1276 tmp = PpE[i2] - t;
1277 delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * tmp;
1278 param = -delta / lenSqr;
1279
1280 final_t = param;
1281 pnt[i0] = boxExtent[i0];
1282 pnt[i1] = -boxExtent[i1];
1283 pnt[i2] = t - boxExtent[i2];
1284 }
1285 else
1286 {
1287 lenSqr += dir[i2] * dir[i2];
1288 delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * PmE[i2];
1289 param = -delta / lenSqr;
1290
1291 final_t = param;
1292 pnt[i0] = boxExtent[i0];
1293 pnt[i1] = -boxExtent[i1];
1294 pnt[i2] = boxExtent[i2];
1295 }
1296 }
1297 else
1298 {
1299 // v[i1] < -e[i1], v[i2] < -e[i2]
1300 lenSqr = dir[i0] * dir[i0] + dir[i2] * dir[i2];
1301 tmp = lenSqr * PpE[i1] - dir[i1] * (dir[i0] * PmE[i0] +
1302 dir[i2] * PpE[i2]);
1303 if (tmp >= (Real)0)
1304 {
1305 // v[i1]-edge is closest
1306 if (tmp <= ((Real)2) * lenSqr * boxExtent[i1])
1307 {
1308 t = tmp / lenSqr;
1309 lenSqr += dir[i1] * dir[i1];
1310 tmp = PpE[i1] - t;
1311 delta = dir[i0] * PmE[i0] + dir[i1] * tmp + dir[i2] * PpE[i2];
1312 param = -delta / lenSqr;
1313
1314 final_t = param;
1315 pnt[i0] = boxExtent[i0];
1316 pnt[i1] = t - boxExtent[i1];
1317 pnt[i2] = -boxExtent[i2];
1318 }
1319 else
1320 {
1321 lenSqr += dir[i1] * dir[i1];
1322 delta = dir[i0] * PmE[i0] + dir[i1] * PmE[i1]
1323 + dir[i2] * PpE[i2];
1324 param = -delta / lenSqr;
1325
1326 final_t = param;
1327 pnt[i0] = boxExtent[i0];
1328 pnt[i1] = boxExtent[i1];
1329 pnt[i2] = -boxExtent[i2];
1330 }
1331 return;
1332 }
1333
1334 lenSqr = dir[i0] * dir[i0] + dir[i1] * dir[i1];
1335 tmp = lenSqr * PpE[i2] - dir[i2] * (dir[i0] * PmE[i0] +
1336 dir[i1] * PpE[i1]);
1337 if (tmp >= (Real)0)
1338 {
1339 // v[i2]-edge is closest
1340 if (tmp <= ((Real)2) * lenSqr * boxExtent[i2])
1341 {
1342 t = tmp / lenSqr;
1343 lenSqr += dir[i2] * dir[i2];
1344 tmp = PpE[i2] - t;
1345 delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * tmp;
1346 param = -delta / lenSqr;
1347
1348 final_t = param;
1349 pnt[i0] = boxExtent[i0];
1350 pnt[i1] = -boxExtent[i1];
1351 pnt[i2] = t - boxExtent[i2];
1352 }
1353 else
1354 {
1355 lenSqr += dir[i2] * dir[i2];
1356 delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1]
1357 + dir[i2] * PmE[i2];
1358 param = -delta / lenSqr;
1359
1360 final_t = param;
1361 pnt[i0] = boxExtent[i0];
1362 pnt[i1] = -boxExtent[i1];
1363 pnt[i2] = boxExtent[i2];
1364 }
1365 return;
1366 }
1367
1368 // (v[i1],v[i2])-corner is closest
1369 lenSqr += dir[i2] * dir[i2];
1370 delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * PpE[i2];
1371 param = -delta / lenSqr;
1372
1373 final_t = param;
1374 pnt[i0] = boxExtent[i0];
1375 pnt[i1] = -boxExtent[i1];
1376 pnt[i2] = -boxExtent[i2];
1377 }
1378 }
1379 };
1380
1381
1382 // Apply reflections so that direction vector has nonnegative
1383 // components.
1384 bool reflect[3];
1385 for (int i = 0; i < 3; ++i)
1386 {
1387 if (lineDir[i] < (Real)0)
1388 {
1389 point[i] = -point[i];
1390 lineDir[i] = -lineDir[i];
1391 reflect[i] = true;
1392 }
1393 else
1394 {
1395 reflect[i] = false;
1396 }
1397 }
1398
1399 if (lineDir[0] > REAL_EPSILON)
1400 {
1401 if (lineDir[1] > REAL_EPSILON)
1402 {
1403 if (lineDir[2] > REAL_EPSILON) // (+,+,+)
1404 {
1405 //CaseNoZeros(point, dir, boxExtent, t);
1406 Coord3D PmE = point - boxExtent;
1407 Real prodDxPy = lineDir[0] * PmE[1];
1408 Real prodDyPx = lineDir[1] * PmE[0];
1409 Real prodDzPx, prodDxPz, prodDzPy, prodDyPz;
1410
1411 if (prodDyPx >= prodDxPy)
1412 {
1413 prodDzPx = lineDir[2] * PmE[0];
1414 prodDxPz = lineDir[0] * PmE[2];
1415 if (prodDzPx >= prodDxPz)
1416 {
1417 // line intersects x = e0
1418 face(point, t, lineDir, PmE, boxExtent, 0, 1, 2);
1419 }
1420 else
1421 {
1422 // line intersects z = e2
1423 face(point, t, lineDir, PmE, boxExtent, 2, 0, 1);
1424 }
1425 }
1426 else
1427 {
1428 prodDzPy = lineDir[2] * PmE[1];
1429 prodDyPz = lineDir[1] * PmE[2];
1430 if (prodDzPy >= prodDyPz)
1431 {
1432 // line intersects y = e1
1433 face(point, t, lineDir, PmE, boxExtent, 1, 2, 0);
1434 }
1435 else
1436 {
1437 // line intersects z = e2
1438 face(point, t, lineDir, PmE, boxExtent, 2, 0, 1);
1439 }
1440 }
1441 }
1442 else // (+,+,0)
1443 {
1444 //Case0(0, 1, 2, point, dir, boxExtent, t);
1445 case0(point, t, lineDir, boxExtent, 0, 1, 2);
1446 }
1447 }
1448 else
1449 {
1450 if (lineDir[2] > REAL_EPSILON) // (+,0,+)
1451 {
1452 //Case0(0, 2, 1, point, dir, boxExtent, t);
1453 case0(point, t, lineDir, boxExtent, 0, 2, 1);
1454 }
1455 else // (+,0,0)
1456 {
1457 //Case00(0, 1, 2, point, dir, boxExtent, t);
1458 case00(point, t, lineDir, boxExtent, 0);
1459 }
1460 }
1461 }
1462 else
1463 {
1464 if (lineDir[1] > REAL_EPSILON)
1465 {
1466 if (lineDir[2] > REAL_EPSILON) // (0,+,+)
1467 {
1468 //Case0(1, 2, 0, point, dir, boxExtent, t);
1469 case0(point, t, lineDir, boxExtent, 1, 2, 0);
1470 }
1471 else // (0,+,0)
1472 {
1473 //Case00(1, 0, 2, point, dir, boxExtent, t);
1474 case00(point, t, lineDir, boxExtent, 1);
1475 }
1476 }
1477 else
1478 {
1479 if (lineDir[2] > REAL_EPSILON) // (0,0,+)
1480 {
1481 //Case00(2, 0, 1, point, dir, boxExtent, t);
1482 case00(point, t, lineDir, boxExtent, 2);
1483 }
1484 else // (0,0,0)
1485 {
1486 point = clamp(point, -boxExtent, boxExtent);
1487 //Case000(point, boxExtent);
1488 }
1489 }
1490 }
1491
1492 // Undo the reflections applied previously.
1493 for (int i = 0; i < 3; ++i)
1494 {
1495 if (reflect[i])
1496 {
1497 point[i] = -point[i];
1498 }
1499 }
1500
1501 return TSegment3D<Real>(origin + t * direction, boxCenter + point);
1502 }
1503
1504 template<typename Real>
1505 DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TOrientedBox3D<Real>& obb) const
1506 {
1507 //transform to the local coordinate system of obb
1508 Coord3D diff = origin - obb.center;
1509 Coord3D originPrime = Coord3D(diff.dot(obb.u), diff.dot(obb.v), diff.dot(obb.w)) + Coord3D(obb.center);
1510 Coord3D dirPrime = Coord3D(direction.dot(obb.u), direction.dot(obb.v), direction.dot(obb.w));
1511
1512 TSegment3D<Real> pqPrime = TLine3D<Real>(originPrime, dirPrime).proximity(TAlignedBox3D<Real>(obb.center - obb.extent, obb.center + obb.extent));
1513
1514 Coord3D pPrime = pqPrime.startPoint();
1515 Coord3D qPrime = pqPrime.endPoint();
1516
1517 //transform back to the global coordinate system
1518 Coord3D p = pPrime[0] * obb.u + pPrime[1] * obb.v + pPrime[2] * obb.w;
1519 Coord3D q = qPrime[0] * obb.u + qPrime[1] * obb.v + qPrime[2] * obb.w;
1520
1521 return TSegment3D<Real>(p, q);
1522 }
1523
1524 // DYN_FUNC TSegment3D<Real> TLine3D<Real>::proximity(const TSegment3D<Real>& segment) const
1525// {
1526//
1527// }
1528
1529 template<typename Real>
1530 DYN_FUNC Real TLine3D<Real>::distance(const TPoint3D<Real>& pt) const
1531 {
1532 return pt.distance(*this);
1533 }
1534
1535 template<typename Real>
1536 DYN_FUNC Real TLine3D<Real>::distance(const TLine3D<Real>& line) const
1537 {
1538 return proximity(line).length();
1539 }
1540
1541 template<typename Real>
1542 DYN_FUNC Real TLine3D<Real>::distance(const TRay3D<Real>& ray) const
1543 {
1544 return proximity(ray).length();
1545 }
1546
1547 template<typename Real>
1548 DYN_FUNC Real TLine3D<Real>::distance(const TSegment3D<Real>& segment) const
1549 {
1550 return proximity(segment).length();
1551 }
1552
1553 template<typename Real>
1554 DYN_FUNC Real TLine3D<Real>::distance(const TAlignedBox3D<Real>& box) const
1555 {
1556 return proximity(box).length();
1557 }
1558
1559 template<typename Real>
1560 DYN_FUNC Real TLine3D<Real>::distance(const TOrientedBox3D<Real>& obb) const
1561 {
1562 return proximity(obb).length();
1563 }
1564
1565 template<typename Real>
1566 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TPoint3D<Real>& pt) const
1567 {
1568 return pt.distanceSquared(*this);
1569 }
1570
1571 template<typename Real>
1572 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TLine3D<Real>& line) const
1573 {
1574 return proximity(line).lengthSquared();
1575 // Coord3D u = origin - line.origin;
1576 // Real a = direction.normSquared();
1577 // Real b = direction.dot(line.direction);
1578 // Real c = line.direction.normSquared();
1579 // Real d = u.dot(direction);
1580 // Real e = u.dot(line.direction);
1581 // Real f = u.normSquared();
1582 // Real det = a * c - b * b;
1583 //
1584 // if (det < REAL_EPSILON)
1585 // {
1586 // return f - e * e;
1587 // }
1588 // else
1589 // {
1590 // Real invDet = 1 / det;
1591 // Real s = (b * e - c * d) * invDet;
1592 // Real t = (a * e - b * d) * invDet;
1593 // return s * (a * s - b * t + 2*d) - t * (b * s - c * t + 2*e) + f;
1594 // }
1595 }
1596
1597 template<typename Real>
1598 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TRay3D<Real>& ray) const
1599 {
1600 return proximity(ray).lengthSquared();
1601 }
1602
1603 template<typename Real>
1604 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TSegment3D<Real>& segment) const
1605 {
1606 return proximity(segment).lengthSquared();
1607 }
1608
1609 template<typename Real>
1610 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TAlignedBox3D<Real>& box) const
1611 {
1612 return proximity(box).lengthSquared();
1613 }
1614
1615 template<typename Real>
1616 DYN_FUNC Real TLine3D<Real>::distanceSquared(const TOrientedBox3D<Real>& obb) const
1617 {
1618 return proximity(obb).lengthSquared();
1619 }
1620
1621 template<typename Real>
1622 DYN_FUNC int TLine3D<Real>::intersect(const TPlane3D<Real>& plane, TPoint3D<Real>& interPt) const
1623 {
1624 Real DdN = direction.dot(plane.normal);
1625 if (abs(DdN) < REAL_EPSILON)
1626 {
1627 return 0;
1628 }
1629
1630 Coord3D offset = origin - plane.origin;
1631 Real t = -offset.dot(plane.normal) / DdN;
1632
1633 interPt.origin = origin + t * direction;
1634 return 1;
1635 }
1636
1637 template<typename Real>
1638 DYN_FUNC int TLine3D<Real>::intersect(const TTriangle3D<Real>& triangle, TPoint3D<Real>& interPt) const
1639 {
1640 Coord3D diff = origin - triangle.v[0];
1641 Coord3D e0 = triangle.v[1] - triangle.v[0];
1642 Coord3D e1 = triangle.v[2] - triangle.v[0];
1643 Coord3D normal = e0.cross(e1);
1644
1645 Real DdN = direction.dot(normal);
1646 Real sign;
1647 if (DdN >= REAL_EPSILON)
1648 {
1649 sign = Real(1);
1650 }
1651 else if (DdN <= -REAL_EPSILON)
1652 {
1653 sign = (Real)-1;
1654 DdN = -DdN;
1655 }
1656 else
1657 {
1658 return 0;
1659 }
1660
1661 Real DdQxE1 = sign * direction.dot(diff.cross(e1));
1662 if (DdQxE1 >= (Real)0)
1663 {
1664 Real DdE0xQ = sign * direction.dot(e0.cross(diff));
1665 if (DdE0xQ >= (Real)0)
1666 {
1667 if (DdQxE1 + DdE0xQ <= DdN)
1668 {
1669 // Line intersects triangle.
1670 Real QdN = -sign * diff.dot(normal);
1671 Real inv = (Real)1 / DdN;
1672
1673 Real t = QdN * inv;
1674 interPt.origin = origin + t * direction;
1675 return 1;
1676 }
1677 // else: b1+b2 > 1, no intersection
1678 }
1679 // else: b2 < 0, no intersection
1680 }
1681 // else: b1 < 0, no intersection
1682
1683 return 0;
1684 }
1685
1686 template<typename Real>
1687 DYN_FUNC int TLine3D<Real>::intersect(const TSphere3D<Real>& sphere, TSegment3D<Real>& interSeg) const
1688 {
1689 Coord3D diff = origin - sphere.center;
1690 Real a0 = diff.dot(diff) - sphere.radius * sphere.radius;
1691 Real a1 = direction.dot(diff);
1692
1693 // Intersection occurs when Q(t) has real roots.
1694 Real discr = a1 * a1 - a0;
1695 if (discr > (Real)0)
1696 {
1697 Real root = glm::sqrt(discr);
1698 interSeg.startPoint() = origin + (-a1 - root) * direction;
1699 interSeg.endPoint() = origin + (-a1 + root) * direction;
1700 return 2;
1701 }
1702 else if (discr < (Real)0)
1703 {
1704 return 0;
1705 }
1706 else
1707 {
1708 interSeg.startPoint() = origin - a1 * direction;
1709 return 1;
1710 }
1711 }
1712
1713 //TODO:
1714 template<typename Real>
1715 DYN_FUNC int TLine3D<Real>::intersect(const TTet3D<Real>& tet, TSegment3D<Real>& interSeg) const
1716 {
1717 TPoint3D<Real> p1, p2;
1718
1719 bool tmp1 = false;
1720 for (int i = 0; i < 4; i++)
1721 if (tmp1 == false)
1722 {
1723 if (intersect(tet.face(i), p1))
1724 tmp1 = true;
1725 }
1726 else if (intersect(tet.face(i), p2))
1727 {
1728
1729 if (p2.distance(p1) > EPSILON)
1730 {
1731 interSeg = TSegment3D<Real>(p1.origin, p2.origin);
1732 return true;
1733 }
1734 }
1735
1736 return 0;
1737 }
1738
1739 template<typename Real>
1740 DYN_FUNC int TLine3D<Real>::intersect(const TAlignedBox3D<Real>& abox, TSegment3D<Real>& interSeg) const
1741 {
1742 if (!isValid())
1743 {
1744 return 0;
1745 }
1746
1747 Real t0 = -REAL_MAX;
1748 Real t1 = REAL_MAX;
1749
1750 auto clip = [](Real denom, Real numer, Real& t0, Real& t1) -> bool
1751 {
1752 if (denom > REAL_EPSILON)
1753 {
1754 if (numer > denom * t1)
1755 {
1756 return false;
1757 }
1758 if (numer > denom * t0)
1759 {
1760 t0 = numer / denom;
1761 }
1762 return true;
1763 }
1764 else if (denom < -REAL_EPSILON)
1765 {
1766 if (numer > denom * t0)
1767 {
1768 return false;
1769 }
1770 if (numer > denom * t1)
1771 {
1772 t1 = numer / denom;
1773 }
1774 return true;
1775 }
1776 else
1777 {
1778 return numer <= -REAL_EPSILON;
1779 }
1780 };
1781
1782 Coord3D boxCenter = 0.5 * (abox.v0 + abox.v1);
1783 Coord3D boxExtent = 0.5 * (abox.v1 - abox.v0);
1784
1785 Coord3D offset = origin - boxCenter;
1786 Coord3D lineDir = direction;
1787 lineDir.normalize();
1788
1789 if (clip(+lineDir[0], -offset[0] - boxExtent[0], t0, t1) &&
1790 clip(-lineDir[0], +offset[0] - boxExtent[0], t0, t1) &&
1791 clip(+lineDir[1], -offset[1] - boxExtent[1], t0, t1) &&
1792 clip(-lineDir[1], +offset[1] - boxExtent[1], t0, t1) &&
1793 clip(+lineDir[2], -offset[2] - boxExtent[2], t0, t1) &&
1794 clip(-lineDir[2], +offset[2] - boxExtent[2], t0, t1))
1795 {
1796 if (t1 > t0)
1797 {
1798 interSeg.v0 = origin + t0 * lineDir;
1799 interSeg.v1 = origin + t1 * lineDir;
1800 return 2;
1801 }
1802 else
1803 {
1804 interSeg.v0 = origin + t0 * lineDir;
1805 interSeg.v1 = interSeg.v0;
1806 return 1;
1807 }
1808 }
1809
1810 return 0;
1811 }
1812
1813 template<typename Real>
1814 DYN_FUNC int TLine3D<Real>::intersect(const TOrientedBox3D<Real>& obb, TSegment3D<Real>& interSeg) const
1815 {
1816 if (!isValid())
1817 {
1818 return 0;
1819 }
1820
1821 Real t0 = -REAL_MAX;
1822 Real t1 = REAL_MAX;
1823
1824 auto clip = [](Real denom, Real numer, Real& t0, Real& t1) -> bool
1825 {
1826 if (denom > REAL_EPSILON)
1827 {
1828 if (numer > denom * t1)
1829 {
1830 return false;
1831 }
1832 if (numer > denom * t0)
1833 {
1834 t0 = numer / denom;
1835 }
1836 return true;
1837 }
1838 else if (denom < -REAL_EPSILON)
1839 {
1840 if (numer > denom * t0)
1841 {
1842 return false;
1843 }
1844 if (numer > denom * t1)
1845 {
1846 t1 = numer / denom;
1847 }
1848 return true;
1849 }
1850 else
1851 {
1852 return numer <= -REAL_EPSILON;
1853 }
1854 };
1855
1856 Coord3D boxCenter = obb.center;
1857 Coord3D boxExtent = obb.extent;
1858
1859 Coord3D offset = origin - boxCenter;
1860 Coord3D lineDir = direction;
1861 lineDir.normalize();
1862
1863 Real du = lineDir.dot(obb.u);
1864 Real dv = lineDir.dot(obb.v);
1865 Real dw = lineDir.dot(obb.w);
1866
1867 Real offset_u = offset.dot(obb.u);
1868 Real offset_v = offset.dot(obb.v);
1869 Real offset_w = offset.dot(obb.w);
1870
1871 if (clip(+du, -offset_u - boxExtent[0], t0, t1) &&
1872 clip(-du, +offset_u - boxExtent[0], t0, t1) &&
1873 clip(+dv, -offset_v - boxExtent[1], t0, t1) &&
1874 clip(-dv, +offset_v - boxExtent[1], t0, t1) &&
1875 clip(+dw, -offset_w - boxExtent[2], t0, t1) &&
1876 clip(-dw, +offset_w - boxExtent[2], t0, t1))
1877 {
1878 if (t1 > t0)
1879 {
1880 interSeg.v0 = origin + t0 * lineDir;
1881 interSeg.v1 = origin + t1 * lineDir;
1882 return 2;
1883 }
1884 else
1885 {
1886 interSeg.v0 = origin + t0 * lineDir;
1887 interSeg.v1 = interSeg.v0;
1888 return 1;
1889 }
1890 }
1891
1892 return 0;
1893 }
1894
1895 template<typename Real>
1896 DYN_FUNC Real TLine3D<Real>::parameter(const Coord3D& pos) const
1897 {
1898 Coord3D l = pos - origin;
1899 Real d2 = direction.normSquared();
1900
1901 return d2 < REAL_EPSILON_SQUARED ? Real(0) : l.dot(direction) / d2;
1902 }
1903
1904 template<typename Real>
1905 DYN_FUNC bool TLine3D<Real>::isValid() const
1906 {
1907 return direction.normSquared() > REAL_EPSILON_SQUARED;
1908 }
1909
1910 template<typename Real>
1911 DYN_FUNC TRay3D<Real>::TRay3D()
1912 {
1913 origin = Coord3D(0);
1914 direction = Coord3D(1, 0, 0);
1915 }
1916
1917 template<typename Real>
1918 DYN_FUNC TRay3D<Real>::TRay3D(const Coord3D& pos, const Coord3D& dir)
1919 {
1920 origin = pos;
1921 direction = dir;
1922 direction.normalize();
1923 }
1924
1925 template<typename Real>
1926 DYN_FUNC TRay3D<Real>::TRay3D(const TRay3D<Real>& ray)
1927 {
1928 origin = ray.origin;
1929 direction = ray.direction;
1930 }
1931
1932 template<typename Real>
1933 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TRay3D<Real>& ray) const
1934 {
1935 Coord3D u = origin - ray.origin;
1936 Real a = direction.normSquared();
1937 Real b = direction.dot(ray.direction);
1938 Real c = ray.direction.normSquared();
1939 Real d = u.dot(direction);
1940 Real e = u.dot(ray.direction);
1941 Real det = a * c - b * b;
1942
1943 if (det < REAL_EPSILON)
1944 {
1945 TPoint3D<Real> p0(origin);
1946 TPoint3D<Real> p1(ray.origin);
1947
1948 TPoint3D<Real> q0 = p0.project(ray);
1949 TPoint3D<Real> q1 = p1.project(*this);
1950
1951 return (q0 - p0).lengthSquared() < (q1 - p1).lengthSquared() ? q0 - p0 : p1 - q1;
1952 }
1953
1954 Real sNum = b * e - c * d;
1955 Real tNum = a * e - b * d;
1956
1957 Real sDenom = det;
1958 Real tDenom = det;
1959
1960 if (sNum < 0) {
1961 sNum = 0;
1962 tNum = e;
1963 tDenom = c;
1964 }
1965 // Check t
1966 if (tNum < 0) {
1967 tNum = 0;
1968 if (-d < 0) {
1969 sNum = 0;
1970 }
1971 else {
1972 sNum = -d;
1973 sDenom = a;
1974 }
1975 }
1976 // Parameters of nearest points on restricted domain
1977 Real s = sNum / sDenom;
1978 Real t = tNum / tDenom;
1979
1980 return TSegment3D<Real>(origin + (s * direction), ray.origin + (t * direction));
1981 }
1982
1983 template<typename Real>
1984 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TSegment3D<Real>& segment) const
1985 {
1986 Coord3D u = origin - segment.startPoint();
1987 Real a = direction.normSquared();
1988 Real b = direction.dot(segment.direction());
1989 Real c = segment.lengthSquared();
1990 Real d = u.dot(direction);
1991 Real e = u.dot(segment.direction());
1992 Real det = a * c - b * b;
1993
1994 if (det < REAL_EPSILON)
1995 {
1996 if (a < REAL_EPSILON_SQUARED)
1997 {
1998 TPoint3D<Real> p0(origin);
1999 return p0.project(segment) - p0;
2000 }
2001 else
2002 {
2003 TPoint3D<Real> p1(segment.startPoint());
2004 TPoint3D<Real> p2(segment.endPoint());
2005
2006 TPoint3D<Real> q1 = p1.project(*this);
2007 TPoint3D<Real> q2 = p2.project(*this);
2008
2009 return (p1 - q1).lengthSquared() < (p2 - q2).lengthSquared() ? (p1 - q1) : (p2 - q2);
2010 }
2011 }
2012
2013 Real sNum = b * e - c * d;
2014 Real tNum = a * e - b * d;
2015
2016 Real sDenom = det;
2017 Real tDenom = det;
2018
2019 if (sNum < 0) {
2020 sNum = 0;
2021 tNum = e;
2022 tDenom = c;
2023 }
2024
2025 // Check t
2026 if (tNum < 0) {
2027 tNum = 0;
2028 if (-d < 0) {
2029 sNum = 0;
2030 }
2031 else {
2032 sNum = -d;
2033 sDenom = a;
2034 }
2035 }
2036 else if (tNum > tDenom) {
2037 tNum = tDenom;
2038 if ((-d + b) < 0) {
2039 sNum = 0;
2040 }
2041 else {
2042 sNum = -d + b;
2043 sDenom = a;
2044 }
2045 }
2046
2047 Real s = sNum / sDenom;
2048 Real t = tNum / tDenom;
2049
2050 return TSegment3D<Real>(origin + (s * direction), segment.startPoint() + (t * segment.direction()));
2051 }
2052
2053 template<typename Real>
2054 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TTriangle3D<Real>& triangle) const
2055 {
2056 TLine3D<Real> line(origin, direction);
2057 TSegment3D<Real> pq = line.proximity(triangle);
2058
2059 Real t = parameter(pq.startPoint());
2060
2061 if (t < Real(0))
2062 {
2063 return TPoint3D<Real>(origin).project(triangle) - TPoint3D<Real>(origin);
2064 }
2065
2066 return pq;
2067 }
2068
2069 template<typename Real>
2070 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TRectangle3D<Real>& rectangle) const
2071 {
2072 TLine3D<Real> line(origin, direction);
2073 TSegment3D<Real> pq = line.proximity(rectangle);
2074
2075 Real t = parameter(pq.startPoint());
2076
2077 if (t < Real(0))
2078 {
2079 return TPoint3D<Real>(origin).project(rectangle) - TPoint3D<Real>(origin);
2080 }
2081
2082 return pq;
2083 }
2084
2085 template<typename Real>
2086 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TAlignedBox3D<Real>& box) const
2087 {
2088 TLine3D<Real> line(origin, direction);
2089 TSegment3D<Real> pq = line.proximity(box);
2090
2091 Real t = parameter(pq.startPoint());
2092
2093 if (t < Real(0))
2094 {
2095 return TPoint3D<Real>(origin).project(box) - TPoint3D<Real>(origin);
2096 }
2097
2098 return pq;
2099 }
2100
2101 template<typename Real>
2102 DYN_FUNC TSegment3D<Real> TRay3D<Real>::proximity(const TOrientedBox3D<Real>& obb) const
2103 {
2104 TLine3D<Real> line(origin, direction);
2105 TSegment3D<Real> pq = line.proximity(obb);
2106
2107 Real t = parameter(pq.startPoint());
2108
2109 if (t < Real(0))
2110 {
2111 return TPoint3D<Real>(origin).project(obb) - TPoint3D<Real>(origin);
2112 }
2113
2114 return pq;
2115 }
2116
2117 template<typename Real>
2118 DYN_FUNC Real TRay3D<Real>::distance(const TPoint3D<Real>& pt) const
2119 {
2120 return pt.distance(*this);
2121 }
2122
2123 template<typename Real>
2124 DYN_FUNC Real TRay3D<Real>::distance(const TSegment3D<Real>& segment) const
2125 {
2126 return proximity(segment).length();
2127 }
2128
2129 template<typename Real>
2130 DYN_FUNC Real TRay3D<Real>::distance(const TTriangle3D<Real>& triangle) const
2131 {
2132 return proximity(triangle).length();
2133 }
2134
2135 template<typename Real>
2136 DYN_FUNC Real TRay3D<Real>::distanceSquared(const TPoint3D<Real>& pt) const
2137 {
2138 return pt.distanceSquared(*this);
2139 }
2140
2141 template<typename Real>
2142 DYN_FUNC Real TRay3D<Real>::distanceSquared(const TSegment3D<Real>& segment) const
2143 {
2144 return proximity(segment).lengthSquared();
2145 }
2146
2147 template<typename Real>
2148 DYN_FUNC Real TRay3D<Real>::distanceSquared(const TTriangle3D<Real>& triangle) const
2149 {
2150 return proximity(triangle).lengthSquared();
2151 }
2152
2153 template<typename Real>
2154 DYN_FUNC int TRay3D<Real>::intersect(const TPlane3D<Real>& plane, TPoint3D<Real>& interPt) const
2155 {
2156 Real DdN = direction.dot(plane.normal);
2157 if (abs(DdN) < REAL_EPSILON)
2158 {
2159 return 0;
2160 }
2161
2162 Coord3D offset = origin - plane.origin;
2163 Real t = -offset.dot(plane.normal) / DdN;
2164
2165 if (t < Real(0))
2166 {
2167 return 0;
2168 }
2169
2170 interPt.origin = origin + t * direction;
2171
2172 return 1;
2173 }
2174
2175 template<typename Real>
2176 DYN_FUNC int TRay3D<Real>::intersect(const TTriangle3D<Real>& triangle, TPoint3D<Real>& interPt) const
2177 {
2178 Coord3D diff = origin - triangle.v[0];
2179 Coord3D e0 = triangle.v[1] - triangle.v[0];
2180 Coord3D e1 = triangle.v[2] - triangle.v[0];
2181 Coord3D normal = e0.cross(e1);
2182
2183 Real DdN = direction.dot(normal);
2184 Real sign;
2185 if (DdN >= REAL_EPSILON)
2186 {
2187 sign = Real(1);
2188 }
2189 else if (DdN <= -REAL_EPSILON)
2190 {
2191 sign = (Real)-1;
2192 DdN = -DdN;
2193 }
2194 else
2195 {
2196 return 0;
2197 }
2198
2199 Real DdQxE1 = sign * direction.dot(diff.cross(e1));
2200 if (DdQxE1 >= (Real)0)
2201 {
2202 Real DdE0xQ = sign * direction.dot(e0.cross(diff));
2203 if (DdE0xQ >= (Real)0)
2204 {
2205 if (DdQxE1 + DdE0xQ <= DdN)
2206 {
2207 // Line intersects triangle.
2208 Real QdN = -sign * diff.dot(normal);
2209 Real inv = (Real)1 / DdN;
2210
2211 Real t = QdN * inv;
2212
2213 if (t < Real(0))
2214 {
2215 return 0;
2216 }
2217
2218 interPt.origin = origin + t * direction;
2219 return 1;
2220 }
2221 // else: b1+b2 > 1, no intersection
2222 }
2223 // else: b2 < 0, no intersection
2224 }
2225 // else: b1 < 0, no intersection
2226
2227 return 0;
2228 }
2229
2230 template<typename Real>
2231 DYN_FUNC int TRay3D<Real>::intersect(const TSphere3D<Real>& sphere, TSegment3D<Real>& interSeg) const
2232 {
2233 Coord3D diff = origin - sphere.center;
2234 Real a0 = diff.dot(diff) - sphere.radius * sphere.radius;
2235 Real a1 = direction.dot(diff);
2236
2237 // Intersection occurs when Q(t) has real roots.
2238 Real discr = a1 * a1 - a0;
2239 if (discr > (Real)0)
2240 {
2241 Real root = glm::sqrt(discr);
2242
2243 if (-a1 + root < Real(0))
2244 {
2245 return 0;
2246 }
2247 else if (-a1 + root < Real(0))
2248 {
2249 interSeg.startPoint() = origin + (-a1 + root) * direction;
2250 return 1;
2251 }
2252 else
2253 {
2254 interSeg.startPoint() = origin + (-a1 - root) * direction;
2255 interSeg.endPoint() = origin + (-a1 + root) * direction;
2256 return 2;
2257 }
2258 }
2259 else if (discr < Real(0))
2260 {
2261 return 0;
2262 }
2263 else
2264 {
2265 if (a1 > Real(0))
2266 {
2267 return 0;
2268 }
2269 interSeg.startPoint() = origin - a1 * direction;
2270 return 1;
2271 }
2272 }
2273
2274 template<typename Real>
2275 DYN_FUNC int TRay3D<Real>::intersect(const TAlignedBox3D<Real>& abox, TSegment3D<Real>& interSeg) const
2276 {
2277 int interNum = TLine3D<Real>(origin, direction).intersect(abox, interSeg);
2278 if (interNum == 0)
2279 {
2280 return 0;
2281 }
2282
2283 Real t0 = parameter(interSeg.startPoint());
2284 Real t1 = parameter(interSeg.endPoint());
2285
2286 Interval<Real> iRay(0, REAL_MAX, false, true);
2287
2288 if (iRay.inside(t0))
2289 {
2290 interSeg.v0 = origin + iRay.leftLimit() * direction;
2291 interSeg.v1 = origin + iRay.rightLimit() * direction;
2292 return 2;
2293 }
2294 else if (iRay.inside(t1))
2295 {
2296 interSeg.v0 = origin + iRay.leftLimit() * direction;
2297 interSeg.v1 = interSeg.v0;
2298 return 1;
2299 }
2300 else
2301 {
2302 return 0;
2303 }
2304 }
2305
2306 template<typename Real>
2307 DYN_FUNC int TRay3D<Real>::intersect(const TOrientedBox3D<Real>& obb, TSegment3D<Real>& interSeg) const
2308 {
2309 int interNum = TLine3D<Real>(origin, direction).intersect(obb, interSeg);
2310 if (interNum == 0) {
2311 return 0;
2312 }
2313
2314 Real t0 = parameter(interSeg.startPoint());
2315 Real t1 = parameter(interSeg.endPoint());
2316
2317 Interval<Real> iRay(0, REAL_MAX, false, true);
2318
2319 if (iRay.inside(t0))
2320 {
2321 interSeg.v0 = origin + iRay.leftLimit() * direction;
2322 interSeg.v1 = origin + iRay.rightLimit() * direction;
2323 return 2;
2324 }
2325 else if (iRay.inside(t1))
2326 {
2327 interSeg.v0 = origin + iRay.leftLimit() * direction;
2328 interSeg.v1 = interSeg.v0;
2329 return 1;
2330 }
2331 else
2332 {
2333 return 0;
2334 }
2335 }
2336
2337 template<typename Real>
2338 DYN_FUNC Real TRay3D<Real>::parameter(const Coord3D& pos) const
2339 {
2340 Coord3D l = pos - origin;
2341 Real d2 = direction.normSquared();
2342
2343 return d2 < REAL_EPSILON_SQUARED ? Real(0) : l.dot(direction) / d2;
2344 }
2345
2346 template<typename Real>
2347 DYN_FUNC bool TRay3D<Real>::isValid() const
2348 {
2349 return direction.normSquared() > REAL_EPSILON_SQUARED;
2350 }
2351
2352 template<typename Real>
2353 DYN_FUNC TSegment3D<Real>::TSegment3D()
2354 {
2355 v0 = Coord3D(0, 0, 0);
2356 v1 = Coord3D(1, 0, 0);
2357 }
2358
2359 template<typename Real>
2360 DYN_FUNC TSegment3D<Real>::TSegment3D(const Coord3D& p0, const Coord3D& p1)
2361 {
2362 v0 = p0;
2363 v1 = p1;
2364 }
2365
2366 template<typename Real>
2367 DYN_FUNC TSegment3D<Real>::TSegment3D(const TSegment3D<Real>& segment)
2368 {
2369 v0 = segment.v0;
2370 v1 = segment.v1;
2371 }
2372
2373 template<typename Real>
2374 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TSegment3D<Real>& segment) const
2375 {
2376 Coord3D u = v0 - segment.v0;
2377 Coord3D dir0 = v1 - v0;
2378 Coord3D dir1 = segment.v1 - segment.v0;
2379 Real a = dir0.normSquared();
2380 Real b = dir0.dot(dir1);
2381 Real c = dir1.normSquared();
2382 Real d = u.dot(dir0);
2383 Real e = u.dot(dir1);
2384 Real det = a * c - b * b;
2385
2386 // Check for (near) parallelism
2387 if (det < REAL_EPSILON) {
2388 // Arbitrary choice
2389 Real l0 = lengthSquared();
2390 Real l1 = segment.lengthSquared();
2391 TPoint3D<Real> p0 = l0 < l1 ? TPoint3D<Real>(v0) : TPoint3D<Real>(segment.v0);
2392 TPoint3D<Real> p1 = l0 < l1 ? TPoint3D<Real>(v1) : TPoint3D<Real>(segment.v1);
2393 TSegment3D<Real> longerSeg = l0 < l1 ? segment : *this;
2394 bool bOpposite = l0 < l1 ? false : true;
2395 TPoint3D<Real> q0 = p0.project(longerSeg);
2396 TPoint3D<Real> q1 = p1.project(longerSeg);
2397 TSegment3D<Real> ret = p0.distance(q0) < p1.distance(q1) ? (q0 - p0) : (q1 - p1);
2398 return bOpposite ? -ret : ret;
2399 }
2400
2401 // Find parameter values of closest points
2402 // on each segment's infinite line. Denominator
2403 // assumed at this point to be ����det����,
2404 // which is always positive. We can check
2405 // value of numerators to see if we are outside
2406 // the [0, 1] x [0, 1] domain.
2407 Real sNum = b * e - c * d;
2408 Real tNum = a * e - b * d;
2409
2410 Real sDenom = det;
2411 Real tDenom = det;
2412
2413 if (sNum < 0) {
2414 sNum = 0;
2415 tNum = e;
2416 tDenom = c;
2417 }
2418 else if (sNum > det) {
2419 sNum = det;
2420 tNum = e + b;
2421 tDenom = c;
2422 }
2423
2424 // Check t
2425 if (tNum < 0) {
2426 tNum = 0;
2427 if (-d < 0) {
2428 sNum = 0;
2429 }
2430 else if (-d > a) {
2431 sNum = sDenom;
2432 }
2433 else {
2434 sNum = -d;
2435 sDenom = a;
2436 }
2437 }
2438 else if (tNum > tDenom) {
2439 tNum = tDenom;
2440 if ((-d + b) < 0) {
2441 sNum = 0;
2442 }
2443 else if ((-d + b) > a) {
2444 sNum = sDenom;
2445 }
2446 else {
2447 sNum = -d + b;
2448 sDenom = a;
2449 }
2450 }
2451
2452 Real s = sNum / sDenom;
2453 Real t = tNum / tDenom;
2454
2455 return TSegment3D<Real>(v0 + (s * dir0), segment.v0 + (t * dir1));
2456 }
2457
2458 template<typename Real>
2459 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TPlane3D<Real>& plane) const
2460 {
2461 TPoint3D<Real> p0(v0);
2462 TPoint3D<Real> p1(v1);
2463 TPoint3D<Real> q0 = p0.project(plane);
2464 TPoint3D<Real> q1 = p1.project(plane);
2465
2466 TSegment3D<Real> pq0 = q0 - p0;
2467 TSegment3D<Real> pq1 = q1 - p1;
2468
2469 return pq0.lengthSquared() < pq1.lengthSquared() ? pq0 : pq1;
2470 }
2471
2472 template<typename Real>
2473 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TTriangle3D<Real>& triangle) const
2474 {
2475 TLine3D<Real> line(startPoint(), direction());
2476 TSegment3D<Real> pq = line.proximity(triangle);
2477
2478 Real t = parameter(pq.startPoint());
2479
2480 if (t < Real(0))
2481 return TPoint3D<Real>(startPoint()).project(triangle) - TPoint3D<Real>(startPoint());
2482
2483 if (t > Real(1))
2484 return TPoint3D<Real>(endPoint()).project(triangle) - TPoint3D<Real>(endPoint());
2485
2486 return pq;
2487 }
2488
2489 template<typename Real>
2490 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TRectangle3D<Real>& rectangle) const
2491 {
2492 Coord3D N = rectangle.axis[0].cross(rectangle.axis[1]);
2493 Coord3D D = v1 - v0;
2494 Real maxT = D.norm();
2495 D.normalize();
2496
2497 Coord3D Q = v0 - rectangle.center;
2498 Real DdN = D.dot(N);
2499 Real absDdN = abs(DdN);
2500 if (absDdN > REAL_EPSILON)
2501 {
2502 // The Seg and rectangle are not parallel, so the seg
2503 // intersects the plane of the rectangle.
2504 // Solve Q + t*D = s0*W0 + s1*W1 (Q = diff, D = line direction(unit),
2505 // W0 = edge 0 direction, W1 = edge 1 direction, N = Cross(W0,W1))
2506 // by
2507 // s0 = Dot(W1,Cross(D,Q)) / Dot(D,N)
2508 // s1 = -Dot(W0,Cross(D,Q)) / Dot(D,N)
2509 // t = -Dot(Q,N) / Dot(D,N)
2510 Coord3D DxQ = D.cross(Q);
2511 Real W1dDxQ = rectangle.axis[1].dot(DxQ);
2512 if (abs(W1dDxQ) <= rectangle.extent[0] * absDdN)
2513 {
2514 Real W0dDxQ = rectangle.axis[0].dot(DxQ);
2515 if (abs(W0dDxQ) <= rectangle.extent[1] * absDdN)
2516 {
2517 Real t = -Q.dot(N) / DdN;
2518 if (t >= 0.f && t <= maxT)
2519 {
2520 Real s0 = W1dDxQ / DdN;
2521 Real s1 = -W0dDxQ / DdN;
2522 printf("inter\n");
2523 return TSegment3D<Real>(v0 + t * D, rectangle.center + s0 * rectangle.axis[0] + s1 * rectangle.axis[1]);
2524 }
2525 }
2526 }
2527 }
2528 printf("prox\n");
2529 Real minDS = REAL_MAX;
2530 TSegment3D<Real> minPQ;
2531 // Edge -Edge
2532 for (int i = 0; i < 4; i++)
2533 {
2534 TSegment3D<Real> pq = proximity(rectangle.edge(i));
2535 if (pq.lengthSquared() < minDS)
2536 {
2537 minDS = pq.lengthSquared();
2538 minPQ = pq;
2539 }
2540 }
2541
2542 // Point - Face
2543 for (int i = 0; i < 2; i++)
2544 {
2545 Coord3D v = (i == 0)? v0 : v1;
2546 TPoint3D<Real> p(v);
2547 TPoint3D<Real> q = TPoint3D<Real>(p).project(rectangle);
2548 TSegment3D<Real> pq = q - p;
2549 if (pq.lengthSquared() < minDS)
2550 {
2551 minDS = pq.lengthSquared();
2552 minPQ = pq;
2553 }
2554 }
2555 return minPQ;
2556 }
2557
2558 template<typename Real>
2559 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TAlignedBox3D<Real>& box) const
2560 {
2561 TLine3D<Real> line(startPoint(), direction());
2562 TSegment3D<Real> pq = line.proximity(box);
2563
2564 Real t = parameter(pq.startPoint());
2565
2566 if (t < Real(0))
2567 return TPoint3D<Real>(startPoint()).project(box) - TPoint3D<Real>(startPoint());
2568
2569 if (t > Real(1))
2570 return TPoint3D<Real>(endPoint()).project(box) - TPoint3D<Real>(endPoint());
2571
2572 return pq;
2573 }
2574
2575 template<typename Real>
2576 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TOrientedBox3D<Real>& obb) const
2577 {
2578 TLine3D<Real> line(startPoint(), direction());
2579 TSegment3D<Real> pq = line.proximity(obb);
2580
2581 Real t = parameter(pq.startPoint());
2582
2583 if (t < Real(0))
2584 return TPoint3D<Real>(startPoint()).project(obb) - TPoint3D<Real>(startPoint());
2585
2586 if (t > Real(1))
2587 return TPoint3D<Real>(endPoint()).project(obb) - TPoint3D<Real>(endPoint());
2588
2589 return pq;
2590 }
2591
2592 template<typename Real>
2593 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::proximity(const TTet3D<Real>& tet) const
2594 {
2595 TSegment3D<Real> pq = proximity(tet.face(0));
2596
2597 for (int i = 1; i < 4; i++)
2598 {
2599 TSegment3D<Real> tmp = proximity(tet.face(i));
2600 if (tmp.length() < pq.length())
2601 pq = tmp;
2602 }
2603 return pq;
2604 }
2605
2606 template<typename Real>
2607 DYN_FUNC Real TSegment3D<Real>::distance(const TSegment3D<Real>& segment) const
2608 {
2609 return proximity(segment).length();
2610 }
2611
2612 template<typename Real>
2613 DYN_FUNC Real TSegment3D<Real>::distance(const TTriangle3D<Real>& triangle) const
2614 {
2615 return proximity(triangle).length();
2616 }
2617
2618 template<typename Real>
2619 DYN_FUNC Real TSegment3D<Real>::distanceSquared(const TSegment3D<Real>& segment) const
2620 {
2621 return proximity(segment).lengthSquared();
2622 }
2623
2624 template<typename Real>
2625 DYN_FUNC Real TSegment3D<Real>::distanceSquared(const TTriangle3D<Real>& triangle) const
2626 {
2627 return proximity(triangle).lengthSquared();
2628 }
2629
2630 template<typename Real>
2631 DYN_FUNC bool TSegment3D<Real>::intersect(const TPlane3D<Real>& plane, TPoint3D<Real>& interPt) const
2632 {
2633 Coord3D dir = direction();
2634 Real DdN = dir.dot(plane.normal);
2635 if (abs(DdN) < REAL_EPSILON)
2636 {
2637 return false;
2638 }
2639
2640 Coord3D offset = v0 - plane.origin;
2641 Real t = -offset.dot(plane.normal) / DdN;
2642
2643 if (t < Real(0) || t > Real(1))
2644 {
2645 return false;
2646 }
2647
2648 interPt.origin = v0 + t * dir;
2649 return true;
2650 }
2651
2652 template<typename Real>
2653 DYN_FUNC bool TSegment3D<Real>::intersect(const TTriangle3D<Real>& triangle, TPoint3D<Real>& interPt) const
2654 {
2655 Coord3D dir = direction();
2656 Coord3D diff = v0 - triangle.v[0];
2657 Coord3D e0 = triangle.v[1] - triangle.v[0];
2658 Coord3D e1 = triangle.v[2] - triangle.v[0];
2659 Coord3D normal = e0.cross(e1);
2660
2661 Real DdN = dir.dot(normal);
2662 Real sign;
2663 if (DdN >= REAL_EPSILON)
2664 {
2665 sign = Real(1);
2666 }
2667 else if (DdN <= -REAL_EPSILON)
2668 {
2669 sign = (Real)-1;
2670 DdN = -DdN;
2671 }
2672 else
2673 {
2674 return false;
2675 }
2676
2677 Real DdQxE1 = sign * dir.dot(diff.cross(e1));
2678 if (DdQxE1 >= (Real)0)
2679 {
2680 Real DdE0xQ = sign * dir.dot(e0.cross(diff));
2681 if (DdE0xQ >= (Real)0)
2682 {
2683 if (DdQxE1 + DdE0xQ <= DdN)
2684 {
2685 // Line intersects triangle.
2686 Real QdN = -sign * diff.dot(normal);
2687 Real inv = (Real)1 / DdN;
2688
2689 Real t = QdN * inv;
2690
2691 if (t < Real(0) || t > Real(1))
2692 {
2693 return false;
2694 }
2695
2696 interPt.origin = v0 + t * dir;
2697 return true;
2698 }
2699 // else: b1+b2 > 1, no intersection
2700 }
2701 // else: b2 < 0, no intersection
2702 }
2703 // else: b1 < 0, no intersection
2704
2705 return false;
2706 }
2707
2708 template<typename Real>
2709 DYN_FUNC int TSegment3D<Real>::intersect(const TSphere3D<Real>& sphere, TSegment3D<Real>& interSeg) const
2710 {
2711 Coord3D diff = v0 - sphere.center;
2712 Coord3D dir = direction();
2713 Real a0 = diff.dot(diff) - sphere.radius * sphere.radius;
2714 Real a1 = dir.dot(diff);
2715
2716 // Intersection occurs when Q(t) has real roots.
2717 Real discr = a1 * a1 - a0;
2718 if (discr > (Real)0)
2719 {
2720 Real root = glm::sqrt(discr);
2721 Real t1 = maximum(-a1 - root, Real(0));
2722 Real t2 = minimum(-a1 + root, Real(1));
2723 if (t1 < t2)
2724 {
2725 interSeg.startPoint() = v0 + t1 * dir;
2726 interSeg.endPoint() = v0 + t2 * dir;
2727 return 2;
2728 }
2729 else if (t1 > t2)
2730 {
2731 return 0;
2732 }
2733 else
2734 {
2735 interSeg.startPoint() = v0 + t1 * dir;
2736 return 1;
2737 }
2738 }
2739 else if (discr < (Real)0)
2740 {
2741 return 0;
2742 }
2743 else
2744 {
2745 Real t = -a1;
2746 if (t >= Real(0) && t <= Real(1))
2747 {
2748 interSeg.startPoint() = v0 - a1 * dir;
2749 return 1;
2750 }
2751 return 0;
2752 }
2753 }
2754
2755 template<typename Real>
2756 DYN_FUNC int TSegment3D<Real>::intersect(const TAlignedBox3D<Real>& abox, TSegment3D<Real>& interSeg) const
2757 {
2758 Coord3D lineDir = direction();
2759 int interNum = TLine3D<Real>(v0, lineDir).intersect(abox, interSeg);
2760 if (interNum == 0)
2761 {
2762 return 0;
2763 }
2764
2765 Real t0 = parameter(interSeg.startPoint());
2766 Real t1 = parameter(interSeg.endPoint());
2767
2768 //Interval<Real> iSeg(0, 1, false, false);
2769
2770 if ((0.0f <= t0 && t0 <= 1.0f) && (0.0f <= t1 && t1 <= 1.0f))
2771 {
2772 interSeg.v0 = v0 + t0 * lineDir;
2773 interSeg.v1 = v0 + t1 * lineDir;
2774 return 2;
2775 }
2776 else if ((0.0f <= t1 && t1 <= 1.0f))
2777 {
2778 interSeg.v0 = v0 + t1 * lineDir;
2779 interSeg.v1 = interSeg.v0;
2780 return 1;
2781 }
2782 else if ((0.0f <= t0 && t0 <= 1.0f))
2783 {
2784 interSeg.v0 = v0 + t0 * lineDir;
2785 interSeg.v1 = interSeg.v0;
2786 return 1;
2787 }
2788 else
2789 {
2790 return 0;
2791 }
2792 }
2793
2794 template<typename Real>
2795 DYN_FUNC int TSegment3D<Real>::intersect(const TOrientedBox3D<Real>& obb, TSegment3D<Real>& interSeg) const
2796 {
2797 //transform to the local coordinate system of obb
2798 Coord3D diff0 = v0 - obb.center;
2799 Coord3D diff1 = v1 - obb.center;
2800 Coord3D v0Prime = Coord3D(diff0.dot(obb.u), diff0.dot(obb.v), diff0.dot(obb.w));
2801 Coord3D v1Prime = Coord3D(diff1.dot(obb.u), diff1.dot(obb.v), diff1.dot(obb.w));
2802
2803 TSegment3D<Real> tmp = TSegment3D<Real>(v0Prime, v1Prime);
2804
2805 int ret = tmp.intersect(TAlignedBox3D<Real>(-obb.extent, obb.extent), interSeg);
2806
2807 Coord3D pPrime = interSeg.startPoint();
2808 Coord3D qPrime = interSeg.endPoint();
2809
2810 //transform back to the global coordinate system
2811 Coord3D p = pPrime[0] * obb.u + pPrime[1] * obb.v + pPrime[2] * obb.w + obb.center;
2812 Coord3D q = qPrime[0] * obb.u + qPrime[1] * obb.v + qPrime[2] * obb.w + obb.center;
2813
2814 interSeg = TSegment3D<Real>(p, q);
2815
2816 return ret;
2817 }
2818
2819 template<typename Real>
2820 DYN_FUNC Real TSegment3D<Real>::length() const
2821 {
2822 return (v1 - v0).norm();
2823 }
2824
2825 template<typename Real>
2826 DYN_FUNC Real TSegment3D<Real>::lengthSquared() const
2827 {
2828 return (v1 - v0).normSquared();
2829 }
2830
2831 template<typename Real>
2832 DYN_FUNC Real TSegment3D<Real>::parameter(const Coord3D& pos) const
2833 {
2834 Coord3D l = pos - v0;
2835 Coord3D dir = direction();
2836 Real d2 = dir.normSquared();
2837
2838 return d2 < REAL_EPSILON_SQUARED ? Real(0) : l.dot(dir) / d2;
2839 }
2840
2841 template<typename Real>
2842 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::operator-(void) const
2843 {
2844 TSegment3D<Real> seg;
2845 seg.v0 = v1;
2846 seg.v1 = v0;
2847
2848 return seg;
2849 }
2850
2851 template<typename Real>
2852 DYN_FUNC bool TSegment3D<Real>::isValid() const
2853 {
2854 return lengthSquared() >= REAL_EPSILON_SQUARED;
2855 }
2856
2857 template<typename Real>
2858 DYN_FUNC TPlane3D<Real>::TPlane3D()
2859 {
2860 origin = Coord3D(0);
2861 normal = Coord3D(0, 1, 0);
2862 }
2863
2864 template<typename Real>
2865 DYN_FUNC TPlane3D<Real>::TPlane3D(const Coord3D& pos, const Coord3D n)
2866 {
2867 origin = pos;
2868 normal = n;
2869 }
2870
2871 template<typename Real>
2872 DYN_FUNC TPlane3D<Real>::TPlane3D(const TPlane3D& plane)
2873 {
2874 origin = plane.origin;
2875 normal = plane.normal;
2876 }
2877
2878 template<typename Real>
2879 DYN_FUNC bool TPlane3D<Real>::isValid() const
2880 {
2881 return normal.normSquared() >= REAL_EPSILON;
2882 }
2883
2884 template<typename Real>
2885 DYN_FUNC TTriangle3D<Real>::TTriangle3D()
2886 {
2887 v[0] = Coord3D(0);
2888 v[1] = Coord3D(1, 0, 0);
2889 v[2] = Coord3D(0, 0, 1);
2890 }
2891
2892 template<typename Real>
2893 DYN_FUNC TTriangle3D<Real>::TTriangle3D(const Coord3D& p0, const Coord3D& p1, const Coord3D& p2)
2894 {
2895 v[0] = p0;
2896 v[1] = p1;
2897 v[2] = p2;
2898 }
2899
2900 template<typename Real>
2901 DYN_FUNC TTriangle3D<Real>::TTriangle3D(const TTriangle3D& triangle)
2902 {
2903 v[0] = triangle.v[0];
2904 v[1] = triangle.v[1];
2905 v[2] = triangle.v[2];
2906 }
2907
2908 template<typename Real>
2909 DYN_FUNC Real TTriangle3D<Real>::area() const
2910 {
2911 return Real(0.5) * ((v[1] - v[0]).cross(v[2] - v[0])).norm();
2912 }
2913
2914 template<typename Real>
2915 DYN_FUNC typename TTriangle3D<Real>::Coord3D TTriangle3D<Real>::normal() const
2916 {
2917 Coord3D n = (v[1] - v[0]).cross(v[2] - v[0]);
2918 if (n.norm() > REAL_EPSILON_SQUARED)
2919 {
2920 n.normalize();
2921 }
2922 return n;
2923 //return ((v[1] - v[0]) / (v[1] - v[0]).norm()).cross((v[2] - v[0]) / ((v[2] - v[0])).norm());
2924 }
2925
2926 template<typename Real>
2927 DYN_FUNC bool TTriangle3D<Real>::computeBarycentrics(const Coord3D& p, Param& bary) const
2928 {
2929 if (!isValid())
2930 {
2931 bary.u = (Real)0;
2932 bary.v = (Real)0;
2933 bary.w = (Real)0;
2934
2935 return false;
2936 }
2937
2938 Coord3D q = TPoint3D<Real>(p).project(TPlane3D<Real>(v[0], normal())).origin;
2939
2940 Coord3D d0 = v[1] - v[0];
2941 Coord3D d1 = v[2] - v[0];
2942 Coord3D d2 = q - v[0];
2943 Real d00 = d0.dot(d0);
2944 Real d01 = d0.dot(d1);
2945 Real d11 = d1.dot(d1);
2946 Real d20 = d2.dot(d0);
2947 Real d21 = d2.dot(d1);
2948 Real denom = d00 * d11 - d01 * d01;
2949
2950 bary.v = (d11 * d20 - d01 * d21) / denom;
2951 bary.w = (d00 * d21 - d01 * d20) / denom;
2952 bary.u = Real(1) - bary.v - bary.w;
2953
2954 return true;
2955 }
2956
2957
2958 template<typename Real>
2959 DYN_FUNC typename TTriangle3D<Real>::Coord3D TTriangle3D<Real>::computeLocation(const Param& bary) const
2960 {
2961 Coord3D d0 = v[1] - v[0];
2962 Coord3D d1 = v[2] - v[0];
2963 return v[0] + bary.v * d0 + bary.w * d1;
2964 }
2965
2966
2967 template<typename Real>
2968 DYN_FUNC Real TTriangle3D<Real>::maximumEdgeLength() const
2969 {
2970 return maximum(maximum((v[1] - v[0]).norm(), (v[2] - v[0]).norm()), (v[2] - v[1]).norm());
2971 }
2972
2973 template<typename Real>
2974 DYN_FUNC bool TTriangle3D<Real>::isValid() const
2975 {
2976 return abs(area()) > REAL_EPSILON_SQUARED;
2977 }
2978
2979
2980 template<typename Real>
2981 DYN_FUNC TAlignedBox3D<Real> TTriangle3D<Real>::aabb()
2982 {
2983 TAlignedBox3D<Real> abox;
2984 abox.v0 = minimum(v[0], minimum(v[1], v[2]));
2985 abox.v1 = maximum(v[0], maximum(v[1], v[2]));
2986
2987 return abox;
2988 }
2989
2990
2991 template<typename Real>
2992 DYN_FUNC Real TTriangle3D<Real>::distanceSquared(const TTriangle3D<Real>& triangle) const
2993 {
2994 Vector<Real, 3> p[3];
2995 p[0] = v[0];
2996 p[1] = v[1];
2997 p[2] = v[2];
2998
2999 Vector<Real, 3> q[3];
3000 q[0] = triangle.v[0];
3001 q[1] = triangle.v[1];
3002 q[2] = triangle.v[2];
3003
3004 //VF
3005 Real minD = (q[0] - p[0]).normSquared();
3006
3007 //EF
3008 for (int st = 0; st < 3; st++)
3009 {
3010 int ind0 = st;
3011 int ind1 = (st + 1) % 3;
3012 minD = minimum(minD, TSegment3D<Real>(p[ind0], p[ind1]).distanceSquared(triangle));
3013 }
3014
3015 for (int st = 0; st < 3; st++)
3016 {
3017 int ind0 = st;
3018 int ind1 = (st + 1) % 3;
3019 minD = minimum(minD, TSegment3D<Real>(q[ind0], q[ind1]).distanceSquared(*this));
3020 }
3021
3022// for (int st = 0; st < 3; st++)
3023// {
3024// int ind0 = st;
3025// int ind1 = (st + 1) % 3;
3026// for (int ss = 0; ss < 3; ss++)
3027// {
3028// int ind2 = st;
3029// int ind3 = (st + 1) % 3;
3030//
3031// minD = minimum(minD, TSegment3D<Real>(p[ind0], p[ind1]).distanceSquared(TSegment3D<Real>(p[ind2], p[ind3])));
3032// }
3033// }
3034
3035 return minD;
3036 }
3037
3038 template<typename Real>
3039 DYN_FUNC Real TTriangle3D<Real>::distance(const TTriangle3D<Real>& triangle) const
3040 {
3041 return glm::sqrt(distanceSquared(triangle));
3042 }
3043
3044 template<typename Real>
3045 DYN_FUNC TRectangle3D<Real>::TRectangle3D()
3046 {
3047 center = Coord3D(0);
3048 axis[0] = Coord3D(1, 0, 0);
3049 axis[1] = Coord3D(0, 0, 1);
3050 extent = Coord2D(1, 1);
3051 }
3052
3053 template<typename Real>
3054 DYN_FUNC TRectangle3D<Real>::TRectangle3D(const Coord3D& c, const Coord3D& a0, const Coord3D& a1, const Coord2D& ext)
3055 {
3056 center = c;
3057 axis[0] = a0;
3058 axis[1] = a1;
3059 extent = ext.maximum(Coord2D(0));
3060 }
3061
3062 template<typename Real>
3063 DYN_FUNC TRectangle3D<Real>::TRectangle3D(const TRectangle3D<Real>& rectangle)
3064 {
3065 center = rectangle.center;
3066 axis[0] = rectangle.axis[0];
3067 axis[1] = rectangle.axis[1];
3068 extent = rectangle.extent;
3069 }
3070
3071 template<typename Real>
3072 DYN_FUNC TPoint3D<Real> TRectangle3D<Real>::vertex(const int i) const
3073 {
3074 int id = i % 4;
3075 Coord3D u = axis[0] * extent[0];
3076 Coord3D v = axis[1] * extent[1];
3077 switch (id)
3078 {
3079 case 0:
3080 return TPoint3D<Real>(center - u - v);
3081 break;
3082 case 1:
3083 return TPoint3D<Real>(center + u - v);
3084 break;
3085 case 2:
3086 return TPoint3D<Real>(center + u + v);
3087 break;
3088 default:
3089 return TPoint3D<Real>(center - u + v);
3090 break;
3091 }
3092 }
3093
3094 template<typename Real>
3095 DYN_FUNC TSegment3D<Real> TRectangle3D<Real>::edge(const int i) const
3096 {
3097 return vertex(i + 1) - vertex(i);
3098 }
3099
3100 template<typename Real>
3101 DYN_FUNC Real TRectangle3D<Real>::area() const
3102 {
3103 return Real(4) * extent[0] * extent[1];
3104 }
3105
3106 template<typename Real>
3107 DYN_FUNC typename TRectangle3D<Real>::Coord3D TRectangle3D<Real>::normal() const
3108 {
3109 return axis[0].cross(axis[1]);
3110 }
3111
3112 template<typename Real>
3113 DYN_FUNC bool TRectangle3D<Real>::computeParams(const Coord3D& p, Param& par) const
3114 {
3115 if (!isValid())
3116 {
3117 par.u = (Real)0;
3118 par.v = (Real)0;
3119
3120 return false;
3121 }
3122
3123 Coord3D offset = p - center;
3124 par.u = offset.dot(axis[0]);
3125 par.v = offset.dot(axis[1]);
3126
3127 return true;
3128 }
3129
3130 template<typename Real>
3131 DYN_FUNC bool TRectangle3D<Real>::isValid() const
3132 {
3133 bool bValid = true;
3134 bValid &= extent[0] >= REAL_EPSILON;
3135 bValid &= extent[1] >= REAL_EPSILON;
3136
3137 bValid &= abs(axis[0].normSquared() - Real(1)) < REAL_EPSILON;
3138 bValid &= abs(axis[1].normSquared() - Real(1)) < REAL_EPSILON;
3139
3140 bValid &= abs(axis[0].dot(axis[1])) < REAL_EPSILON;
3141
3142 return bValid;
3143 }
3144
3145 template<typename Real>
3146 DYN_FUNC TDisk3D<Real>::TDisk3D()
3147 {
3148 center = Coord3D(0);
3149 normal = Coord3D(0, 1, 0);
3150 radius = 1;
3151 }
3152
3153 template<typename Real>
3154 DYN_FUNC TDisk3D<Real>::TDisk3D(const Coord3D& c, const Coord3D& n, const Real& r)
3155 {
3156 center = c;
3157 normal = n.norm() < REAL_EPSILON ? Coord3D(1, 0, 0) : n;
3158 normal.normalize();
3159 radius = r < 0 ? 0 : r;
3160 }
3161
3162 template<typename Real>
3163 DYN_FUNC TDisk3D<Real>::TDisk3D(const TDisk3D& circle)
3164 {
3165 center = circle.center;
3166 normal = circle.normal;
3167 radius = circle.radius;
3168 }
3169
3170 template<typename Real>
3171 DYN_FUNC Real TDisk3D<Real>::area()
3172 {
3173 return Real(M_PI) * radius * radius;
3174 }
3175
3176 template<typename Real>
3177 DYN_FUNC bool TDisk3D<Real>::isValid()
3178 {
3179 return radius > REAL_EPSILON && normal.norm() > REAL_EPSILON;
3180 }
3181
3182 template<typename Real>
3183 DYN_FUNC TSphere3D<Real>::TSphere3D()
3184 {
3185 center = Coord3D(0);
3186 rotation = Quat<Real>();
3187 radius = 1;
3188 }
3189
3190 template<typename Real>
3191 DYN_FUNC TSphere3D<Real>::TSphere3D(const Coord3D& c, const Real& r)
3192 {
3193 center = c;
3194 rotation = Quat<Real>();
3195 radius = r < 0 ? 0 : r;
3196 }
3197
3198 template<typename Real>
3199 DYN_FUNC dyno::TSphere3D<Real>::TSphere3D(const Coord3D& c, const Quat<Real>& rot, const Real& r)
3200 {
3201 center = c;
3202 rotation = rot;
3203 radius = r < 0 ? 0 : r;
3204 }
3205
3206 template<typename Real>
3207 DYN_FUNC TSphere3D<Real>::TSphere3D(const TSphere3D& sphere)
3208 {
3209 center = sphere.center;
3210 rotation = sphere.rotation;
3211 radius = sphere.radius;
3212 }
3213
3214 template<typename Real>
3215 DYN_FUNC Real TSphere3D<Real>::volume()
3216 {
3217 return 4 * Real(M_PI) * radius * radius * radius / 3;
3218 }
3219
3220 template<typename Real>
3221 DYN_FUNC bool TSphere3D<Real>::isValid()
3222 {
3223 return radius >= REAL_EPSILON;
3224 }
3225
3226//*********************** Cylinder3D **************************
3227 template<typename Real>
3228 DYN_FUNC TCylinder3D<Real>::TCylinder3D()
3229 {
3230 center = Coord3D(0);
3231 height = Real(1);
3232 radius = Real(0.5);
3233
3234 rotation = Quat<Real>();
3235 scale = Coord3D(1);
3236 }
3237
3238 template<typename Real>
3239 DYN_FUNC TCylinder3D<Real>::TCylinder3D(const Coord3D& c, const Real& h, const Real& r, const Quat<Real>& rot, const Coord3D& s)
3240 {
3241 center = c;
3242 height = h;
3243 radius = r;
3244
3245 rotation = rot;
3246 scale = s;
3247 }
3248
3249 template<typename Real>
3250 DYN_FUNC TCylinder3D<Real>::TCylinder3D(const TCylinder3D<Real>& cylinder)
3251 {
3252 center = cylinder.center;
3253 height = cylinder.height;
3254 radius = cylinder.radius;
3255
3256 rotation = cylinder.rotation;
3257 scale = cylinder.scale;
3258 }
3259
3260 //*********************** Cone3D **************************
3261 template<typename Real>
3262 DYN_FUNC TCone3D<Real>::TCone3D()
3263 {
3264 center = Coord3D(0);
3265 height = Real(1);
3266 radius = Real(0.5);
3267
3268 rotation = Quat<Real>();
3269 scale = Coord3D(1);
3270 }
3271
3272
3273 template<typename Real>
3274 DYN_FUNC TCone3D<Real>::TCone3D(const Coord3D& c, const Real& h, const Real& r, const Quat<Real>& rot, const Coord3D& s)
3275 {
3276 center = c;
3277 height = h;
3278 radius = r;
3279
3280 rotation = rot;
3281 scale = s;
3282 }
3283
3284 template<typename Real>
3285 DYN_FUNC TCone3D<Real>::TCone3D(const TCone3D<Real>& cone)
3286 {
3287 center = cone.center;
3288 height = cone.height;
3289 radius = cone.radius;
3290
3291 rotation = cone.rotation;
3292 scale = cone.scale;
3293 }
3294
3295 template<typename Real>
3296 DYN_FUNC TAlignedBox3D<Real> TSphere3D<Real>::aabb()
3297 {
3298 TAlignedBox3D<Real> abox;
3299 abox.v0 = center - radius;
3300 abox.v1 = center + radius;
3301
3302 return abox;
3303 }
3304
3305
3306 template<typename Real>
3307 DYN_FUNC TCapsule3D<Real>::TCapsule3D()
3308 {
3309 center = Coord3D(0, 0, 0);
3310 rotation = Quat<Real>();
3311 radius = Real(1);
3312 halfLength = Real(1);
3313 }
3314
3315 template<typename Real>
3316 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const Coord3D& v0, const Coord3D& v1, const Real& r)
3317 {
3318 center = 0.5f * (v0 + v1);
3319 Real len = (v1 - v0).norm();
3320
3321 rotation = Quat<Real>(Coord3D(0, 1, 0), v1 - v0);
3322 radius = Real(r);
3323 halfLength = Real(len * 0.5f);
3324 }
3325
3326 template<typename Real>
3327 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const Coord3D& c, const Quat<Real>& q, const Real& r, const Real& hl)
3328 : center(c)
3329 , rotation(q)
3330 , radius(r)
3331 , halfLength(hl)
3332 {
3333 }
3334
3335 template<typename Real>
3336 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const TCapsule3D& capsule)
3337 {
3338 center = capsule.center;
3339 rotation = capsule.rotation;
3340 radius = capsule.radius;
3341 halfLength = capsule.halfLength;
3342 }
3343
3344 template<typename Real>
3345 DYN_FUNC Real TCapsule3D<Real>::volume() const
3346 {
3347 Real r2 = radius * radius;
3348 return 2 * Real(M_PI) * r2 * halfLength + Real(4.0 * M_PI / 3.0) * r2 * radius;
3349 }
3350
3351 template<typename Real>
3352 DYN_FUNC bool TCapsule3D<Real>::isValid() const
3353 {
3354 return radius >= REAL_EPSILON && halfLength >= REAL_EPSILON;
3355 }
3356
3357 template<typename Real>
3358 DYN_FUNC TAlignedBox3D<Real> TCapsule3D<Real>::aabb() const
3359 {
3360 TAlignedBox3D<Real> abox;
3361 Coord3D v0 = startPoint();
3362 Coord3D v1 = endPoint();
3363 abox.v0 = minimum(v0, v1) - radius;
3364 abox.v1 = maximum(v0, v1) + radius;
3365 return abox;
3366 }
3367
3368 template<typename Real>
3369 DYN_FUNC TTet3D<Real>::TTet3D()
3370 {
3371 v[0] = Coord3D(0);
3372 v[1] = Coord3D(1, 0, 0);
3373 v[2] = Coord3D(0, 0, 1);
3374 v[3] = Coord3D(0, 1, 0);
3375 }
3376
3377 template<typename Real>
3378 DYN_FUNC TTet3D<Real>::TTet3D(const Coord3D& v0, const Coord3D& v1, const Coord3D& v2, const Coord3D& v3)
3379 {
3380 v[0] = v0;
3381 v[1] = v1;
3382 v[2] = v2;
3383 v[3] = v3;
3384 }
3385
3386 template<typename Real>
3387 DYN_FUNC TTet3D<Real>::TTet3D(const TTet3D& tet)
3388 {
3389 v[0] = tet.v[0];
3390 v[1] = tet.v[1];
3391 v[2] = tet.v[2];
3392 v[3] = tet.v[3];
3393 }
3394
3395 template<typename Real>
3396 DYN_FUNC TSegment3D<Real> TTet3D<Real>::edge(const int index) const
3397 {
3398 switch (index)
3399 {
3400 case 0:
3401 return TSegment3D<Real>(v[0], v[1]);
3402 break;
3403 case 1:
3404 return TSegment3D<Real>(v[0], v[2]);
3405 break;
3406 case 2:
3407 return TSegment3D<Real>(v[0], v[3]);
3408 break;
3409 case 3:
3410 return TSegment3D<Real>(v[1], v[2]);
3411 break;
3412 case 4:
3413 return TSegment3D<Real>(v[1], v[3]);
3414 break;
3415 case 5:
3416 return TSegment3D<Real>(v[2], v[3]);
3417 break;
3418 default:
3419 break;
3420 }
3421 }
3422
3423 template<typename Real>
3424 DYN_FUNC TTriangle3D<Real> TTet3D<Real>::face(const int index) const
3425 {
3426 switch (index)
3427 {
3428 case 0:
3429 return TTriangle3D<Real>(v[1], v[3], v[2]);
3430 break;
3431 case 1:
3432 return TTriangle3D<Real>(v[0], v[2], v[3]);
3433 break;
3434 case 2:
3435 return TTriangle3D<Real>(v[0], v[3], v[1]);
3436 break;
3437 case 3:
3438 return TTriangle3D<Real>(v[0], v[1], v[2]);
3439 break;
3440 default:
3441 break;
3442 }
3443
3444 //return an ill triangle in case index is out of range
3445 return TTriangle3D<Real>(Coord3D(0), Coord3D(0), Coord3D(0));
3446 }
3447
3448 //https://en.wikipedia.org/wiki/Solid_angle
3449 template<typename Real>
3450 DYN_FUNC Real TTet3D<Real>::solidAngle(const int index) const
3451 {
3452 Coord3D v0, v1, v2, v3;
3453 switch (index)
3454 {
3455 case 0:
3456 v0 = v[0]; v1 = v[1]; v2 = v[3]; v3 = v[2];
3457 break;
3458 case 1:
3459 v0 = v[1]; v1 = v[0]; v2 = v[2]; v3 = v[3];
3460 break;
3461 case 2:
3462 v0 = v[2]; v1 = v[0]; v2 = v[3]; v3 = v[1];
3463 break;
3464 case 3:
3465 v0 = v[3]; v1 = v[0]; v2 = v[1]; v3 = v[2];
3466 break;
3467 default:
3468 break;
3469 }
3470
3471 Coord3D A = v1 - v0;
3472 Coord3D B = v2 - v0;
3473 Coord3D C = v3 - v0;
3474
3475 Real a = A.norm();
3476 Real b = B.norm();
3477 Real c = C.norm();
3478
3479 Real dividend = A.dot(B.cross(C));
3480 Real divisor = a * b * c + A.dot(B) * c + A.dot(C) * b + B.dot(C) * a;
3481
3482 Real angle = 2 * glm::atan(glm::abs(dividend) / divisor);
3483
3484 if (dividend > 0 && divisor < 0) angle += M_PI;
3485
3486 return angle;
3487 }
3488
3489 template<typename Real>
3490 DYN_FUNC Real TTet3D<Real>::volume() const
3491 {
3492 Matrix3D M;
3493 M.setRow(0, v[1] - v[0]);
3494 M.setRow(1, v[2] - v[0]);
3495 M.setRow(2, v[3] - v[0]);
3496
3497 return M.determinant() / Real(6);
3498 }
3499
3500 template<typename Real>
3501 DYN_FUNC bool TTet3D<Real>::isValid() const
3502 {
3503 return volume() >= REAL_EPSILON;
3504 }
3505
3506
3507 template<typename Real>
3508 DYN_FUNC void TTet3D<Real>::expand(Real r)
3509 {
3510 Coord3D center;
3511 center = (v[0] + v[1] + v[2] + v[3]) / 4.0f;
3512 for (int idx = 0; idx < 4; idx++)
3513 {
3514 Coord3D dir = (v[idx] - center);
3515 Real rr = dir.norm();
3516 dir /= dir.norm();
3517 rr += r;
3518 v[idx] = dir * rr + center;
3519 }
3520 }
3521
3522 template<typename Real>
3523 DYN_FUNC bool TTet3D<Real>::intersect(const TTet3D<Real>& tet2, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2, int need_distance) const
3524 {
3525
3526
3527 // printf("VVVVVVVVVVVVVVVVVVVVVVv\n");
3528
3529 Coord3D mid = (tet2.v[0] + tet2.v[1] + tet2.v[2] + tet2.v[3]) / 4.0f;
3530 Coord3D v11 = mid + (tet2.v[0] - mid) / (tet2.v[0] - mid).norm() * ((tet2.v[0] - mid).norm());
3531 Coord3D v22 = mid + (tet2.v[1] - mid) / (tet2.v[1] - mid).norm() * ((tet2.v[1] - mid).norm());
3532 Coord3D v33 = mid + (tet2.v[2] - mid) / (tet2.v[2] - mid).norm() * ((tet2.v[2] - mid).norm());
3533 Coord3D v44 = mid + (tet2.v[3] - mid) / (tet2.v[3] - mid).norm() * ((tet2.v[3] - mid).norm());
3534 TTet3D<Real> tet(v11, v22, v33, v44);
3535
3536 Real inter_dist_0 = 0.0f;
3537 Coord3D p11, p22, interNorm_0;
3538
3539 TSegment3D<Real> s[18];
3540 s[0] = TSegment3D<Real>(v[0], v[1]);
3541 s[1] = TSegment3D<Real>(v[0], v[2]);
3542 s[2] = TSegment3D<Real>(v[0], v[3]);
3543 s[3] = TSegment3D<Real>(v[1], v[2]);
3544 s[4] = TSegment3D<Real>(v[1], v[3]);
3545 s[5] = TSegment3D<Real>(v[2], v[3]);
3546
3547 s[6] = TSegment3D<Real>(v[0], (v[1] + v[2]) / 2.0f);
3548 s[7] = TSegment3D<Real>(v[0], (v[1] + v[3]) / 2.0f);
3549 s[8] = TSegment3D<Real>(v[0], (v[3] + v[2]) / 2.0f);
3550 s[9] = TSegment3D<Real>(v[1], (v[3] + v[2]) / 2.0f);
3551 s[10] = TSegment3D<Real>(v[1], (v[0] + v[2]) / 2.0f);
3552 s[11] = TSegment3D<Real>(v[1], (v[0] + v[3]) / 2.0f);
3553
3554 s[12] = TSegment3D<Real>(v[2], (v[1] + v[0]) / 2.0f);
3555 s[13] = TSegment3D<Real>(v[2], (v[1] + v[3]) / 2.0f);
3556 s[14] = TSegment3D<Real>(v[2], (v[0] + v[3]) / 2.0f);
3557 s[15] = TSegment3D<Real>(v[3], (v[1] + v[2]) / 2.0f);
3558 s[16] = TSegment3D<Real>(v[3], (v[0] + v[2]) / 2.0f);
3559 s[17] = TSegment3D<Real>(v[3], (v[1] + v[0]) / 2.0f);
3560
3561
3562
3563 for (int i = 0; i < 18; i++)
3564 {
3565 if (i >= 6 && !(inter_dist_0 < -0.000f))
3566 break;
3567 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3568 TSegment3D<Real> tmp_seg;
3569
3570 if (l_i.intersect(tet, tmp_seg))
3571 {
3572
3573 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
3574 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
3575 if (right < left)
3576 {
3577 Real tmp = left;
3578 left = right;
3579 right = tmp;
3580 }
3581 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3582 if (right < 0 || left > maxx)
3583 continue;
3584 left = maximum(left, Real(0));
3585 right = minimum(right, maxx);
3586
3587 p1 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
3588 Bool tmp_bool;
3589 p2 = TPoint3D<Real>(p1).project(tet, tmp_bool).origin;//
3590 interDist = -(p1 - p2).norm();
3591
3592 if (need_distance)
3593 {
3594 Coord3D p11 = s[i].v0 + left * s[i].direction().normalize();
3595 Coord3D p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3596 if ((p11 - p22).norm() > abs(interDist))
3597 {
3598 p1 = p11; p2 = p22;
3599 interDist = -(p1 - p2).norm();
3600 }
3601
3602 p11 = s[i].v0 + right * s[i].direction().normalize();
3603 p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3604 if ((p11 - p22).norm() > abs(interDist))
3605 {
3606 p1 = p11; p2 = p22;
3607 interDist = -(p1 - p2).norm();
3608 }
3609 }
3610 Coord3D dir = (p1 - p2) / interDist;
3611 interNorm = dir;
3612 //dir *= -1;
3613
3614 if (interDist < inter_dist_0)
3615 {
3616 inter_dist_0 = interDist;
3617 p11 = p1;
3618 p22 = p2;
3619 interNorm_0 = interNorm;
3620 }
3621
3622 if (need_distance == 0)
3623 if (inter_dist_0 < -0.000f)
3624 {
3625 interDist = inter_dist_0;
3626 p1 = p11;
3627 p2 = p22;
3628 interNorm = interNorm_0;
3629 interDist += 0.0000f;
3630 return true;
3631 }
3632
3633 }
3634 }
3635
3636 if (inter_dist_0 < -0.000f)
3637 {
3638 interDist = inter_dist_0;
3639 p1 = p11;
3640 p2 = p22;
3641 interNorm = interNorm_0;
3642 interDist += 0.0000f;
3643 return true;
3644 }
3645 return false;
3646 }
3647
3648
3649 template<typename Real>
3650 DYN_FUNC bool TTet3D<Real>::intersect(const TTriangle3D<Real>& tri, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2, int need_distance) const
3651 {
3652
3653
3654 // printf("VVVVVVVVVVVVVVVVVVVVVVv\n");
3655
3656
3657 TTet3D<Real> tet(v[0], v[1], v[2], v[3]);
3658
3659 Real inter_dist_0 = 0.0f;
3660 Coord3D p11, p22, interNorm_0;
3661
3662 TSegment3D<Real> s[18];
3663
3664 s[0] = TSegment3D<Real>(tri.v[0], tri.v[1]);
3665 s[1] = TSegment3D<Real>(tri.v[0], tri.v[2]);
3666 s[2] = TSegment3D<Real>(tri.v[0], tri.v[2]);
3667
3668 for (int i = 0; i < 3; i++)
3669 {
3670
3671 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3672 TSegment3D<Real> tmp_seg;
3673
3674 if (l_i.intersect(tet, tmp_seg))
3675 {
3676
3677 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
3678 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
3679 if (right < left)
3680 {
3681 Real tmp = left;
3682 left = right;
3683 right = tmp;
3684 }
3685 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3686 if (right < 0 || left > maxx)
3687 continue;
3688 left = maximum(left, Real(0));
3689 right = minimum(right, maxx);
3690
3691 p2 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
3692 Bool tmp_bool;
3693 p1 = TPoint3D<Real>(p2).project(tet, tmp_bool).origin;//
3694 interDist = -(p1 - p2).norm();
3695
3696
3697 Coord3D p11 = s[i].v0 + left * s[i].direction().normalize();
3698 Coord3D p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3699 if ((p11 - p22).norm() > abs(interDist))
3700 {
3701 p2 = p11; p1 = p22;
3702 interDist = -(p1 - p2).norm();
3703 }
3704
3705 p11 = s[i].v0 + right * s[i].direction().normalize();
3706 p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3707 if ((p11 - p22).norm() > abs(interDist))
3708 {
3709 p2 = p11; p1 = p22;
3710 interDist = -(p1 - p2).norm();
3711 }
3712
3713 Coord3D dir = (p1 - p2) / interDist;
3714 interNorm = dir;
3715 //dir *= -1;
3716
3717 if (interDist < inter_dist_0)
3718 {
3719 inter_dist_0 = interDist;
3720 p11 = p1;
3721 p22 = p2;
3722 interNorm_0 = interNorm;
3723 }
3724
3725
3726
3727 }
3728 }
3729
3730 if (inter_dist_0 < -0.000f)
3731 {
3732 interDist = inter_dist_0;
3733 p1 = p11;
3734 p2 = p22;
3735 interNorm = interNorm_0;
3736 interDist += 0.0000f;
3737 return true;
3738 }
3739
3740 s[0] = TSegment3D<Real>(v[0], v[1]);
3741 s[1] = TSegment3D<Real>(v[0], v[2]);
3742 s[2] = TSegment3D<Real>(v[0], v[3]);
3743 s[3] = TSegment3D<Real>(v[1], v[2]);
3744 s[4] = TSegment3D<Real>(v[1], v[3]);
3745 s[5] = TSegment3D<Real>(v[2], v[3]);
3746
3747 s[6] = TSegment3D<Real>(v[0], (v[1] + v[2]) / 2.0f);
3748 s[7] = TSegment3D<Real>(v[0], (v[1] + v[3]) / 2.0f);
3749 s[8] = TSegment3D<Real>(v[0], (v[3] + v[2]) / 2.0f);
3750 s[9] = TSegment3D<Real>(v[1], (v[3] + v[2]) / 2.0f);
3751 s[10] = TSegment3D<Real>(v[1], (v[0] + v[2]) / 2.0f);
3752 s[11] = TSegment3D<Real>(v[1], (v[0] + v[3]) / 2.0f);
3753
3754 s[12] = TSegment3D<Real>(v[2], (v[1] + v[0]) / 2.0f);
3755 s[13] = TSegment3D<Real>(v[2], (v[1] + v[3]) / 2.0f);
3756 s[14] = TSegment3D<Real>(v[2], (v[0] + v[3]) / 2.0f);
3757 s[15] = TSegment3D<Real>(v[3], (v[1] + v[2]) / 2.0f);
3758 s[16] = TSegment3D<Real>(v[3], (v[0] + v[2]) / 2.0f);
3759 s[17] = TSegment3D<Real>(v[3], (v[1] + v[0]) / 2.0f);
3760
3761
3762
3763 for (int i = 0; i < 18; i++)
3764 {
3765
3766 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3767 TPoint3D<Real> tmp_point;
3768
3769 if (l_i.intersect(tri, tmp_point))
3770 {
3771
3772 Real left = (tmp_point.origin - s[i].v0).dot(s[i].direction().normalize());
3773 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3774
3775 if (left < 0 || left > maxx)
3776 continue;
3777
3778 if (left < maxx / 2.0f)
3779 p1 = s[i].v0;
3780 else
3781 p1 = s[i].v1;
3782 p2 = tmp_point.origin;//TPoint3D<Real>(p1).project(tet, tmp_bool).origin;
3783 interDist = -(p1 - p2).norm();
3784 Coord3D dir = (p1 - p2) / interDist;
3785 interNorm = dir;
3786
3787 }
3788 }
3789 if (inter_dist_0 < -0.000f)
3790 {
3791 interDist = inter_dist_0;
3792 p1 = p11;
3793 p2 = p22;
3794 interNorm = interNorm_0;
3795 interDist += 0.0000f;
3796 return true;
3797 }
3798
3799
3800
3801
3802 return false;
3803 }
3804 template<typename Real>
3805 DYN_FUNC TAlignedBox3D<Real> TTet3D<Real>::aabb()
3806 {
3807 TAlignedBox3D<Real> abox;
3808 abox.v0[0] = minimum(minimum(v[0][0], v[1][0]), minimum(v[2][0], v[3][0]));
3809 abox.v0[1] = minimum(minimum(v[0][1], v[1][1]), minimum(v[2][1], v[3][1]));
3810 abox.v0[2] = minimum(minimum(v[0][2], v[1][2]), minimum(v[2][2], v[3][2]));
3811
3812 abox.v1[0] = maximum(maximum(v[0][0], v[1][0]), maximum(v[2][0], v[3][0]));
3813 abox.v1[1] = maximum(maximum(v[0][1], v[1][1]), maximum(v[2][1], v[3][1]));
3814 abox.v1[2] = maximum(maximum(v[0][2], v[1][2]), maximum(v[2][2], v[3][2]));
3815
3816 return abox;
3817 }
3818
3819 template<typename Real>
3820 DYN_FUNC TPoint3D<Real> TTet3D<Real>::circumcenter() const
3821 {
3822 // Use coordinates relative to point `a' of the tetrahedron.
3823
3824 // ba = b - a
3825 Real ba_x = v[1][0] - v[0][0];
3826 Real ba_y = v[1][1] - v[0][1];
3827 Real ba_z = v[1][2] - v[0][2];
3828
3829 // ca = c - a
3830 Real ca_x = v[2][0] - v[0][0];
3831 Real ca_y = v[2][1] - v[0][1];
3832 Real ca_z = v[2][2] - v[0][2];
3833
3834 // da = d - a
3835 Real da_x = v[3][0] - v[0][0];
3836 Real da_y = v[3][1] - v[0][1];
3837 Real da_z = v[3][2] - v[0][2];
3838
3839 // Squares of lengths of the edges incident to `a'.
3840 Real len_ba = ba_x * ba_x + ba_y * ba_y + ba_z * ba_z;
3841 Real len_ca = ca_x * ca_x + ca_y * ca_y + ca_z * ca_z;
3842 Real len_da = da_x * da_x + da_y * da_y + da_z * da_z;
3843
3844 // Cross products of these edges.
3845
3846 // c cross d
3847 Real cross_cd_x = ca_y * da_z - da_y * ca_z;
3848 Real cross_cd_y = ca_z * da_x - da_z * ca_x;
3849 Real cross_cd_z = ca_x * da_y - da_x * ca_y;
3850
3851 // d cross b
3852 Real cross_db_x = da_y * ba_z - ba_y * da_z;
3853 Real cross_db_y = da_z * ba_x - ba_z * da_x;
3854 Real cross_db_z = da_x * ba_y - ba_x * da_y;
3855
3856 // b cross c
3857 Real cross_bc_x = ba_y * ca_z - ca_y * ba_z;
3858 Real cross_bc_y = ba_z * ca_x - ca_z * ba_x;
3859 Real cross_bc_z = ba_x * ca_y - ca_x * ba_y;
3860
3861 // Calculate the denominator of the formula.
3862 Real denominator = 0.5 / (ba_x * cross_cd_x + ba_y * cross_cd_y + ba_z * cross_cd_z);
3863
3864 // Calculate offset (from `a') of circumcenter.
3865 Real circ_x = (len_ba * cross_cd_x + len_ca * cross_db_x + len_da * cross_bc_x) * denominator;
3866 Real circ_y = (len_ba * cross_cd_y + len_ca * cross_db_y + len_da * cross_bc_y) * denominator;
3867 Real circ_z = (len_ba * cross_cd_z + len_ca * cross_db_z + len_da * cross_bc_z) * denominator;
3868
3869 return TPoint3D<Real>(circ_x + v[0][0], circ_y + v[0][1], circ_z + v[0][2]);
3870 }
3871
3872 template<typename Real>
3873 DYN_FUNC TPoint3D<Real> TTet3D<Real>::barycenter() const
3874 {
3875 return TPoint3D<Real>(Real(0.25) * (v[0] + v[1] + v[2] + v[3]));
3876 }
3877
3878 template<typename Real>
3879 DYN_FUNC bool TTet3D<Real>::contain(Coord3D p)
3880 {
3881 return TPoint3D<Real>(p).inside(*this);
3882 }
3883
3884 template<typename Real>
3885 DYN_FUNC Vector<Real, 4> TTet3D<Real>::computeBarycentricCoordinates(const Coord3D& p)
3886 {
3887 SquareMatrix<Real, 3> T(v[0].x - v[3].x, v[1].x - v[3].x, v[2].x - v[3].x,
3888 v[0].y - v[3].y, v[1].y - v[3].y, v[2].y - v[3].y,
3889 v[0].z - v[3].z, v[1].z - v[3].z, v[2].z - v[3].z);
3890
3891 Vector<Real, 3> b(p.x - v[3].x, p.y - v[3].y, p.z - v[3].z);
3892
3893 Vector<Real, 3> lambda = T.inverse() * b;
3894
3895 return Vector<Real, 4>(lambda.x, lambda.y, lambda.z, Real(1) - lambda.x - lambda.y - lambda.z);
3896 }
3897
3898 template<typename Real>
3899 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D()
3900 {
3901 v0 = Coord3D(Real(-1));
3902 v1 = Coord3D(Real(1));
3903 }
3904
3905 template<typename Real>
3906 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D(const Coord3D& p0, const Coord3D& p1)
3907 {
3908 v0 = p0;
3909 v1 = p1;
3910 }
3911
3912 template<typename Real>
3913 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D(const TAlignedBox3D& box)
3914 {
3915 v0 = box.v0;
3916 v1 = box.v1;
3917 }
3918
3919 template<typename Real>
3920 DYN_FUNC bool TAlignedBox3D<Real>::intersect(const TAlignedBox3D& abox, TAlignedBox3D& interBox) const
3921 {
3922 for (int i = 0; i < 3; i++)
3923 {
3924 if (v1[i] <= abox.v0[i] || v0[i] >= abox.v1[i])
3925 {
3926 return false;
3927 }
3928 }
3929
3930 interBox.v0 = v0.maximum(abox.v0);
3931 interBox.v1 = v1.minimum(abox.v1);
3932
3933 for (int i = 0; i < 3; i++)
3934 {
3935 if (v1[i] <= abox.v1[i])
3936 {
3937 interBox.v1[i] = v1[i];
3938 }
3939 else
3940 {
3941 interBox.v1[i] = abox.v1[i];
3942 }
3943
3944 if (v0[i] <= abox.v0[i])
3945 {
3946 interBox.v0[i] = abox.v0[i];
3947 }
3948 else
3949 {
3950 interBox.v0[i] = v0[i];
3951 }
3952 }
3953
3954 return true;
3955 }
3956
3957
3958 template<typename Real>
3959 DYN_FUNC bool TAlignedBox3D<Real>::checkOverlap(const TAlignedBox3D<Real>& abox) const
3960 {
3961 for (int i = 0; i < 3; i++)
3962 {
3963 if (v1[i] <= abox.v0[i] || v0[i] >= abox.v1[i])
3964 {
3965 return false;
3966 }
3967 }
3968
3969 return true;
3970 }
3971
3972
3973 template<typename Real>
3974 DYN_FUNC TAlignedBox3D<Real> TAlignedBox3D<Real>::merge(const TAlignedBox3D<Real>& aabb) const
3975 {
3976 TAlignedBox3D<Real> ret;
3977 ret.v0 = v0.minimum(aabb.v0);
3978 ret.v1 = v1.maximum(aabb.v1);
3979
3980 return ret;
3981 }
3982
3983
3984 template<typename Real>
3985 DYN_FUNC bool TAlignedBox3D<Real>::meshInsert(const TTriangle3D<Real>& tri) const
3986 {
3987 //return true;
3988 TPoint3D<Real> p0 = TPoint3D<Real>(tri.v[0]);
3989 TPoint3D<Real> p1 = TPoint3D<Real>(tri.v[1]);
3990 TPoint3D<Real> p2 = TPoint3D<Real>(tri.v[2]);
3991 TAlignedBox3D<Real> AABB = TAlignedBox3D<Real>(v0, v1);
3992 if (p0.inside(AABB))return true;
3993 if (p1.inside(AABB))return true;
3994 if (p2.inside(AABB))return true;
3995
3996 TPoint3D<Real> pTmp = TPoint3D<Real>((v0 + v1) / 2);
3997 Real r = (pTmp.origin - v0).norm();
3998 // if (abs(pTmp.distance(tri)) < r) return true;
3999
4000 Coord3D P0 = v0;
4001 Coord3D P1 = Coord3D(v0[0], v0[1], v1[2]);
4002 Coord3D P2 = Coord3D(v0[0], v1[1], v0[2]);
4003 Coord3D P3 = Coord3D(v0[0], v1[1], v1[2]);
4004 Coord3D P4 = Coord3D(v1[0], v0[1], v0[2]);
4005 Coord3D P5 = Coord3D(v1[0], v0[1], v1[2]);
4006 Coord3D P6 = Coord3D(v1[0], v1[1], v0[2]);
4007 Coord3D P7 = v1;
4008
4009 TPoint3D<Real> interpt;
4010 Coord3D seg[12][2] = {
4011 P0,P1,
4012 P0,P2,
4013 P0,P4,
4014 P7,P6,
4015 P7,P5,
4016 P7,P3,
4017 P2,P3,
4018 P2,P6,
4019 P3,P1,
4020 P6,P4,
4021 P4,P5,
4022 P1,P5
4023 };
4024
4025 Coord3D m_triangle_index[12][3] = {
4026 P0,P1,P2,
4027 P0,P1,P4,
4028 P0,P2,P4,
4029 P7,P5,P6,
4030 P7,P3,P5,
4031 P7,P3,P6,
4032 P2,P4,P6,
4033 P1,P4,P5,
4034 P1,P2,P3,
4035 P3,P5,P1,
4036 P5,P6,P4,
4037 P3,P6,P2
4038 };
4039
4040 for (int i = 0; i < 12; i++)
4041 {
4042 TTriangle3D<Real> t_tmp = TTriangle3D<Real>(m_triangle_index[i][0], m_triangle_index[i][1], m_triangle_index[i][2]);
4043 TSegment3D<Real> s_tmp = TSegment3D<Real>(seg[i][0], seg[i][1]);
4044 TSegment3D<Real> s0, s1, s2;
4045 s0 = TSegment3D<Real>(tri.v[0], tri.v[1]);
4046 s1 = TSegment3D<Real>(tri.v[1], tri.v[2]);
4047 s2 = TSegment3D<Real>(tri.v[2], tri.v[0]);
4048
4049 if ((s0.intersect(t_tmp, interpt)) || (s1.intersect(t_tmp, interpt)) || (s2.intersect(t_tmp, interpt)) || (s_tmp.intersect(tri, interpt)))
4050 return true;
4051 }
4052 //printf("!!!!!!!!!!!!!!!!!!FALSE CASE:\n");
4053
4054 /*
4055 \nAABB:\n%.3lf %.3lf %.3lf\nTRI:\n%.3lf %.3lf %.3lf\n%.3lf %.3lf %.3lf\n%.3lf %.3lf %.3lf\nPPPPPPPPPPPPPPP\n",
4056 v0[0], v0[1], v0[2],
4057 v1[0], v1[1], v1[2],
4058 tri.v[0][0], tri.v[0][1], tri.v[0][2],
4059 tri.v[1][0], tri.v[1][1], tri.v[1][2],
4060 tri.v[2][0], tri.v[2][1], tri.v[2][2]
4061 );*/
4062 return false;
4063 }
4064
4065 template<typename Real>
4066 DYN_FUNC TOrientedBox3D<Real> TAlignedBox3D<Real>::rotate(const Matrix3D& mat)
4067 {
4068 TOrientedBox3D<Real> obox;
4069 obox.u = mat * Coord3D(1, 0, 0);
4070 obox.v = mat * Coord3D(0, 1, 0);
4071 obox.w = mat * Coord3D(0, 0, 1);
4072 obox.center = 0.5f * (v0 + v1);
4073 obox.extent = 0.5f * (v1 - v0);
4074
4075 return obox;
4076 }
4077
4078
4079 template<typename Real>
4080 DYN_FUNC bool TAlignedBox3D<Real>::isValid()
4081 {
4082 return v1[0] > v0[0] && v1[1] > v0[1] && v1[2] > v0[2];
4083 }
4084
4085 template<typename Real>
4086 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D()
4087 {
4088 center = Coord3D(0);
4089 u = Coord3D(1, 0, 0);
4090 v = Coord3D(0, 1, 0);
4091 w = Coord3D(0, 0, 1);
4092
4093 extent = Coord3D(1);
4094 }
4095
4096 template<typename Real>
4097 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const Coord3D c, const Coord3D u_t, const Coord3D v_t, const Coord3D w_t, const Coord3D ext)
4098 {
4099 center = c;
4100 u = u_t;
4101 v = v_t;
4102 w = w_t;
4103 extent = ext;
4104 }
4105
4106 template<typename Real>
4107 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const Coord3D c, const Quat<Real> rot, const Coord3D ext)
4108 {
4109 center = c;
4110 extent = ext;
4111
4112 auto mat = rot.toMatrix3x3();
4113 u = mat.col(0);
4114 v = mat.col(1);
4115 w = mat.col(2);
4116 }
4117
4118 template<typename Real>
4119 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const TOrientedBox3D& obb)
4120 {
4121 center = obb.center;
4122 u = obb.u;
4123 v = obb.v;
4124 w = obb.w;
4125 extent = obb.extent;
4126 }
4127
4128 template<typename Real>
4129 DYN_FUNC TPoint3D<Real> TOrientedBox3D<Real>::vertex(const int i) const
4130 {
4131 int id = i % 8;
4132 Coord3D hu = u * extent[0];
4133 Coord3D hv = v * extent[1];
4134 Coord3D hw = w * extent[2];
4135
4136 return TPoint3D<Real>(center + (2 * (id & 1) - 1) * hu + (2 * ((id >> 1) & 1) - 1) * hv + (2 * ((id >> 2) & 1) - 1) * hw);
4137 }
4138
4139 template<typename Real>
4140 DYN_FUNC TSegment3D<Real> TOrientedBox3D<Real>::edge(const int i) const
4141 {
4142 int id = i % 12;
4143 Vec2u table[12] = { Vec2u(0, 1), Vec2u(1, 3), Vec2u(2, 3), Vec2u(0, 2),
4144 Vec2u(0, 4), Vec2u(1, 5), Vec2u(2, 6), Vec2u(3, 7),
4145 Vec2u(4, 5), Vec2u(5, 7), Vec2u(6, 7), Vec2u(4, 6)};
4146 return vertex(table[id][0]) - vertex(table[id][1]);
4147 }
4148
4149 template<typename Real>
4150 DYN_FUNC TRectangle3D<Real> TOrientedBox3D<Real>::face(const int index) const
4151 {
4152 int id = index % 6;
4153 Coord3D c = center;
4154 Coord3D Nx, Ny;
4155 Coord2D Ext = Coord2D(0.f);
4156 switch (id)
4157 {
4158 case 0:
4159 c -= v * extent[1];
4160 Nx = u; Ny = w;
4161 Ext = Coord2D(extent[0], extent[2]);
4162 break;
4163 case 1:
4164 c += v * extent[1];
4165 Nx = w; Ny = u;
4166 Ext = Coord2D(extent[2], extent[0]);
4167 break;
4168 case 2:
4169 c -= u * extent[0];
4170 Nx = w; Ny = v;
4171 Ext = Coord2D(extent[2], extent[1]);
4172 break;
4173 case 3:
4174 c += u * extent[0];
4175 Nx = v; Ny = w;
4176 Ext = Coord2D(extent[1], extent[2]);
4177 break;
4178 case 4:
4179 c -= w * extent[2];
4180 Nx = v; Ny = u;
4181 Ext = Coord2D(extent[1], extent[0]);
4182 break;
4183 default:
4184 c += w * extent[2];
4185 Nx = u; Ny = v;
4186 Ext = Coord2D(extent[0], extent[1]);
4187 break;
4188 }
4189 return TRectangle3D<Real>(c, Nx, Ny, Ext);
4190 }
4191
4192
4193 template<typename Real>
4194 DYN_FUNC Real TOrientedBox3D<Real>::volume()
4195 {
4196 return 8 * extent[0] * extent[1] * extent[2];
4197 }
4198
4199 template<typename Real>
4200 DYN_FUNC bool TOrientedBox3D<Real>::isValid()
4201 {
4202 bool bValid = true;
4203 bValid &= extent[0] >= REAL_EPSILON;
4204 bValid &= extent[1] >= REAL_EPSILON;
4205 bValid &= extent[2] >= REAL_EPSILON;
4206
4207 bValid &= abs(u.normSquared() - Real(1)) < REAL_EPSILON;
4208 bValid &= abs(v.normSquared() - Real(1)) < REAL_EPSILON;
4209 bValid &= abs(w.normSquared() - Real(1)) < REAL_EPSILON;
4210
4211 bValid &= abs(u.dot(v)) < REAL_EPSILON;
4212 bValid &= abs(v.dot(w)) < REAL_EPSILON;
4213 bValid &= abs(w.dot(u)) < REAL_EPSILON;
4214
4215 return bValid;
4216 }
4217
4218
4219 template<typename Real>
4220 DYN_FUNC TOrientedBox3D<Real> TOrientedBox3D<Real>::rotate(const Matrix3D& mat)
4221 {
4222 TOrientedBox3D<Real> obox;
4223 obox.center = center;
4224 obox.extent = extent;
4225 obox.u = mat * obox.u;
4226 obox.v = mat * obox.v;
4227 obox.w = mat * obox.w;
4228
4229 return obox;
4230 }
4231
4232
4233 template<typename Real>
4234 DYN_FUNC TAlignedBox3D<Real> TOrientedBox3D<Real>::aabb()
4235 {
4236 TAlignedBox3D<Real> abox;
4237
4238 Coord3D r_ext;
4239 r_ext[0] = abs(extent[0] * u[0]) + abs(extent[1] * v[0]) + abs(extent[2] * w[0]);
4240 r_ext[1] = abs(extent[0] * u[1]) + abs(extent[1] * v[1]) + abs(extent[2] * w[1]);
4241 r_ext[2] = abs(extent[0] * u[2]) + abs(extent[1] * v[2]) + abs(extent[2] * w[2]);
4242
4243 abox.v0 = center - r_ext;
4244 abox.v1 = center + r_ext;
4245
4246 return abox;
4247 }
4248
4249
4250
4251 template<typename Real>
4252 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TOrientedBox3D<Real>& OBB, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4253 {
4254 TPoint3D<Real> p[8];
4255 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4256 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4257 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4258 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4259 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4260 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4261 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4262 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4263
4264 TSegment3D<Real> s[12];
4265 s[0] = TSegment3D<Real>(p[0].origin, p[1].origin); s[1] = TSegment3D<Real>(p[0].origin, p[2].origin); s[2] = TSegment3D<Real>(p[0].origin, p[4].origin);
4266 s[3] = TSegment3D<Real>(p[3].origin, p[1].origin); s[4] = TSegment3D<Real>(p[3].origin, p[2].origin); s[5] = TSegment3D<Real>(p[3].origin, p[7].origin);
4267 s[6] = TSegment3D<Real>(p[6].origin, p[7].origin); s[7] = TSegment3D<Real>(p[6].origin, p[2].origin); s[8] = TSegment3D<Real>(p[6].origin, p[4].origin);
4268 s[9] = TSegment3D<Real>(p[5].origin, p[7].origin); s[10] = TSegment3D<Real>(p[5].origin, p[1].origin); s[11] = TSegment3D<Real>(p[5].origin, p[4].origin);
4269
4270 int min_idx1 = -1;
4271 int min_idx2 = -1;
4272 Real distance = 1.0f;
4273
4274 TPlane3D<Real> plane1, plane2;
4275 Coord3D axis;
4276 Real inter;
4277
4278 //check u axis
4279 Real min_u, max_u;
4280 for (int i = 0; i < 8; i++)
4281 {
4282 Coord3D offset_self = p[i].origin - OBB.center;
4283 Real tmp = offset_self.dot(OBB.u);
4284 if (i == 0) min_u = max_u = tmp;
4285 else
4286 {
4287 min_u = minimum(min_u, tmp);
4288 max_u = maximum(max_u, tmp);
4289 }
4290 }
4291 if (max_u < -OBB.extent[0] || min_u > OBB.extent[0]) return false;
4292 Real inter_u = minimum(minimum(max_u + OBB.extent[0], OBB.extent[0] - min_u), minimum(2 * OBB.extent[0], max_u - min_u));
4293 axis = OBB.u;
4294 inter = inter_u;
4295 plane1 = TPlane3D<Real>((OBB.center + OBB.u * OBB.extent[0]), OBB.u);
4296 plane2 = TPlane3D<Real>((OBB.center - OBB.u * OBB.extent[0]), -OBB.u);
4297
4298 //check v axis
4299 Real min_v, max_v;
4300 for (int i = 0; i < 8; i++)
4301 {
4302 Coord3D offset_self = p[i].origin - OBB.center;
4303 Real tmp = offset_self.dot(OBB.v);
4304 if (i == 0) min_v = max_v = tmp;
4305 else
4306 {
4307 min_v = minimum(min_v, tmp);
4308 max_v = maximum(max_v, tmp);
4309 }
4310 }
4311 if (max_v < -OBB.extent[1] || min_v > OBB.extent[1]) return false;
4312 Real inter_v = minimum(minimum(max_v + OBB.extent[1], OBB.extent[1] - min_v), minimum(2 * OBB.extent[1], max_v - min_v));
4313 if (inter_v < inter)
4314 {
4315 axis = OBB.v;
4316 inter = inter_v;
4317 plane1 = TPlane3D<Real>((OBB.center + OBB.v * OBB.extent[1]), OBB.v);
4318 plane2 = TPlane3D<Real>((OBB.center - OBB.v * OBB.extent[1]), -OBB.v);
4319 }
4320
4321 //check w axis
4322 Real min_w, max_w;
4323 for (int i = 0; i < 8; i++)
4324 {
4325 Coord3D offset_self = p[i].origin - OBB.center;
4326 Real tmp = offset_self.dot(OBB.w);
4327 if (i == 0) min_w = max_w = tmp;
4328 else
4329 {
4330 min_w = minimum(min_w, tmp);
4331 max_w = maximum(max_w, tmp);
4332 }
4333 }
4334 if (max_w < -OBB.extent[2] || min_w > OBB.extent[2]) { return false; }
4335 Real inter_w = minimum(minimum(max_w + OBB.extent[2], OBB.extent[2] - min_w), minimum(2 * OBB.extent[2], max_w - min_w));
4336 if (inter_w < inter)
4337 {
4338 axis = OBB.w;
4339 inter = inter_w;
4340 plane1 = TPlane3D<Real>((OBB.center + OBB.w * OBB.extent[2]), OBB.w);
4341 plane2 = TPlane3D<Real>((OBB.center - OBB.w * OBB.extent[2]), -OBB.w);
4342 }
4343
4344 //printf("$$$$$$$$$$$$$$$$$$$$ INSIDE CHECKPOINT\n");
4345
4346 for (int i = 0; i < 8; i++)
4347 {
4348 Real tmp = -minimum(abs(p[i].distance(plane1)), abs(p[i].distance(plane2)));
4349 if (tmp < distance && p[i].distance(OBB) < EPSILON)
4350 {
4351 distance = tmp;
4352 min_idx1 = i;
4353 }
4354 }
4355
4356 for (int i = 0; i < 12; i++)
4357 {
4358 TSegment3D<Real> tmp;
4359 if (s[i].intersect(OBB, tmp))
4360 {
4361 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4362 Real tmp_distance = -minimum(abs(inp.distance(plane1)), abs(inp.distance(plane2)));
4363 if (tmp_distance < distance)
4364 {
4365
4366 //printf(" ---------------------------------- interSegStart: %.3lf %.3lf %.3lf interSegEnd: %.3lf %.3lf %.3lf \n",
4367 // tmp.startPoint()[0], tmp.startPoint()[1], tmp.startPoint()[2],
4368 // tmp.endPoint()[0], tmp.endPoint()[1], tmp.endPoint()[2]);
4369 distance = tmp_distance;
4370 min_idx2 = i;
4371 min_idx1 = -1;
4372 }
4373
4374 }
4375 }
4376 //printf("distance = %.10lf\n", distance);
4377 if (distance > EPSILON) return false;
4378 interDist = distance;
4379
4380 TPoint3D<Real> pt;
4381 if (min_idx1 != -1)
4382 pt = p[min_idx1];
4383 else
4384 {
4385 TSegment3D<Real> tmp;
4386 s[min_idx2].intersect(OBB, tmp);
4387 pt = TPoint3D<Real>((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4388 }
4389
4390
4391 p1 = pt.origin;
4392
4393 if (abs(pt.distance(plane1)) < abs(pt.distance(plane2)))
4394 {
4395 p2 = pt.project(plane1).origin;
4396 interNorm = axis;
4397 }
4398 else
4399 {
4400 p2 = pt.project(plane2).origin;
4401 interNorm = -axis;
4402 }
4403
4404 return true;
4405 }
4406
4407 template<typename Real>
4408 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TTet3D<Real>& TET, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4409 {
4410 TPoint3D<Real> p[8];
4411 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4412 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4413 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4414 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4415 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4416 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4417 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4418 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4419
4420 TSegment3D<Real> s[12];
4421 s[0] = TSegment3D<Real>(p[0].origin, p[1].origin); s[1] = TSegment3D<Real>(p[0].origin, p[2].origin); s[2] = TSegment3D<Real>(p[0].origin, p[4].origin);
4422 s[3] = TSegment3D<Real>(p[3].origin, p[1].origin); s[4] = TSegment3D<Real>(p[3].origin, p[2].origin); s[5] = TSegment3D<Real>(p[3].origin, p[7].origin);
4423 s[6] = TSegment3D<Real>(p[6].origin, p[7].origin); s[7] = TSegment3D<Real>(p[6].origin, p[2].origin); s[8] = TSegment3D<Real>(p[6].origin, p[4].origin);
4424 s[9] = TSegment3D<Real>(p[5].origin, p[7].origin); s[10] = TSegment3D<Real>(p[5].origin, p[1].origin); s[11] = TSegment3D<Real>(p[5].origin, p[4].origin);
4425
4426 TSegment3D<Real> s_tet[18];
4427 s_tet[0] = TSegment3D<Real>(TET.v[0], TET.v[1]);
4428 s_tet[1] = TSegment3D<Real>(TET.v[0], TET.v[2]);
4429 s_tet[2] = TSegment3D<Real>(TET.v[0], TET.v[3]);
4430 s_tet[3] = TSegment3D<Real>(TET.v[1], TET.v[2]);
4431 s_tet[4] = TSegment3D<Real>(TET.v[1], TET.v[3]);
4432 s_tet[5] = TSegment3D<Real>(TET.v[2], TET.v[3]);
4433
4434 s_tet[6] = TSegment3D<Real>(TET.v[0], (TET.v[1] + TET.v[2]) / 2.0f);
4435 s_tet[7] = TSegment3D<Real>(TET.v[0], (TET.v[1] + TET.v[3]) / 2.0f);
4436 s_tet[8] = TSegment3D<Real>(TET.v[0], (TET.v[3] + TET.v[2]) / 2.0f);
4437 s_tet[9] = TSegment3D<Real>(TET.v[1], (TET.v[3] + TET.v[2]) / 2.0f);
4438 s_tet[10] = TSegment3D<Real>(TET.v[1], (TET.v[0] + TET.v[2]) / 2.0f);
4439 s_tet[11] = TSegment3D<Real>(TET.v[1], (TET.v[0] + TET.v[3]) / 2.0f);
4440
4441 s_tet[12] = TSegment3D<Real>(TET.v[2], (TET.v[1] + TET.v[0]) / 2.0f);
4442 s_tet[13] = TSegment3D<Real>(TET.v[2], (TET.v[1] + TET.v[3]) / 2.0f);
4443 s_tet[14] = TSegment3D<Real>(TET.v[2], (TET.v[0] + TET.v[3]) / 2.0f);
4444 s_tet[15] = TSegment3D<Real>(TET.v[3], (TET.v[1] + TET.v[2]) / 2.0f);
4445 s_tet[16] = TSegment3D<Real>(TET.v[3], (TET.v[0] + TET.v[2]) / 2.0f);
4446 s_tet[17] = TSegment3D<Real>(TET.v[3], (TET.v[1] + TET.v[0]) / 2.0f);
4447
4448
4449 interDist = 0.0f;
4450 //obb intersect tet
4451 for (int i = 0; i < 12; i++)
4452 {
4453 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
4454 TSegment3D<Real> tmp_seg;
4455
4456 if (l_i.intersect(TET, tmp_seg))
4457 {
4458
4459 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
4460 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
4461 if (right < left)
4462 {
4463 Real tmp = left;
4464 left = right;
4465 right = tmp;
4466 }
4467 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
4468 if (right < 0 || left > maxx)
4469 continue;
4470 left = maximum(left, Real(0));
4471 right = minimum(right, maxx);
4472
4473 Bool tmp_bool;
4474
4475 Coord3D p11 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
4476 Coord3D p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;//
4477
4478
4479 if ((p11 - p22).norm() > abs(interDist))
4480 {
4481 interDist = -(p11 - p22).norm();
4482 p1 = p11;
4483 p2 = p22;
4484 }
4485 p11 = s[i].v0 + left * s[i].direction().normalize();
4486 p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;
4487 if ((p11 - p22).norm() > abs(interDist))
4488 {
4489 p1 = p11; p2 = p22;
4490 interDist = -(p1 - p2).norm();
4491 }
4492
4493 p11 = s[i].v0 + right * s[i].direction().normalize();
4494 p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;
4495 if ((p11 - p22).norm() > abs(interDist))
4496 {
4497 p1 = p11; p2 = p22;
4498 interDist = -(p1 - p2).norm();
4499 }
4500 //dir = -(p1 - p2) / interDist;
4501
4502 }
4503 }
4504 TOrientedBox3D<Real> OBB(center, u, v, w, extent);
4505 for (int i = 0; i < 18; i++)
4506 {
4507 TSegment3D<Real> tmp;
4508
4509 if (s_tet[i].intersect(OBB, tmp))
4510 {
4511 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4512 TPoint3D<Real> sp(tmp.startPoint());
4513 TPoint3D<Real> ep(tmp.endPoint());
4514 if (abs(inp.distance(OBB)) > abs(interDist))
4515 {
4516 p2 = inp.origin;
4517 p1 = inp.project(OBB).origin;
4518 interDist = -abs(inp.distance(OBB));
4519 }
4520 if (abs(sp.distance(OBB)) > abs(interDist))
4521 {
4522 p2 = sp.origin;
4523 p1 = sp.project(OBB).origin;
4524 interDist = -abs(sp.distance(OBB));
4525 }
4526 if (abs(ep.distance(OBB)) > abs(interDist))
4527 {
4528 p2 = ep.origin;
4529 p1 = ep.project(OBB).origin;
4530 interDist = -abs(ep.distance(OBB));
4531 }
4532 }
4533 }
4534 if (interDist > -EPSILON) return false;
4535 interNorm = (p1 - p2) / interDist;
4536 return true;
4537 }
4538
4539
4540 template<typename Real>
4541 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TTriangle3D<Real>& TRI, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4542 {
4543
4544 TSegment3D<Real> s_tri[3];
4545 s_tri[0] = TSegment3D<Real>(TRI.v[0], TRI.v[1]);
4546 s_tri[1] = TSegment3D<Real>(TRI.v[0], TRI.v[2]);
4547 s_tri[2] = TSegment3D<Real>(TRI.v[2], TRI.v[1]);
4548
4549 interDist = 0.0f;
4550 //obb intersect tet
4551
4552 TPoint3D<Real> p[8];
4553 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4554 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4555 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4556 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4557 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4558 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4559 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4560 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4561
4562 TSegment3D<Real> s[12];
4563 s[0] = TSegment3D<Real>(p[0].origin, p[1].origin); s[1] = TSegment3D<Real>(p[0].origin, p[2].origin); s[2] = TSegment3D<Real>(p[0].origin, p[4].origin);
4564 s[3] = TSegment3D<Real>(p[3].origin, p[1].origin); s[4] = TSegment3D<Real>(p[3].origin, p[2].origin); s[5] = TSegment3D<Real>(p[3].origin, p[7].origin);
4565 s[6] = TSegment3D<Real>(p[6].origin, p[7].origin); s[7] = TSegment3D<Real>(p[6].origin, p[2].origin); s[8] = TSegment3D<Real>(p[6].origin, p[4].origin);
4566 s[9] = TSegment3D<Real>(p[5].origin, p[7].origin); s[10] = TSegment3D<Real>(p[5].origin, p[1].origin); s[11] = TSegment3D<Real>(p[5].origin, p[4].origin);
4567
4568
4569
4570 TOrientedBox3D<Real> OBB(center, u, v, w, extent);
4571 for (int i = 0; i < 3; i++)
4572 {
4573 TSegment3D<Real> tmp;
4574
4575 if (s_tri[i].intersect(OBB, tmp))
4576 {
4577 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4578 TPoint3D<Real> sp(tmp.startPoint());
4579 TPoint3D<Real> ep(tmp.endPoint());
4580 if (abs(inp.distance(OBB)) > abs(interDist))
4581 {
4582 p2 = inp.origin;
4583 p1 = inp.project(OBB).origin;
4584 interDist = -abs(inp.distance(OBB));
4585 }
4586 if (abs(sp.distance(OBB)) > abs(interDist))
4587 {
4588 p2 = sp.origin;
4589 p1 = sp.project(OBB).origin;
4590 interDist = -abs(sp.distance(OBB));
4591 }
4592 if (abs(ep.distance(OBB)) > abs(interDist))
4593 {
4594 p2 = ep.origin;
4595 p1 = ep.project(OBB).origin;
4596 interDist = -abs(ep.distance(OBB));
4597 }
4598 }
4599 }
4600 interNorm = (p1 - p2) / interDist;
4601 if (interDist < -EPSILON)
4602 return true;
4603
4604 for (int i = 0; i < 12; i++)
4605 {
4606 TPoint3D<Real> tmp_point;
4607 if (s[i].intersect(TRI, tmp_point))
4608 {
4609 TPoint3D<Real> tmp_p1;// = s[i].startPoint();
4610 TPoint3D<Real> tmp_p2;// = s[i].endPoint();
4611 if (tmp_point.distance(TPoint3D<Real>(s[i].startPoint())) < tmp_point.distance(TPoint3D<Real>(s[i].endPoint())))
4612 {
4613 tmp_p1 = s[i].endPoint();
4614 tmp_p2 = s[i].startPoint();
4615 }
4616 else
4617 {
4618 tmp_p1 = s[i].startPoint();
4619 tmp_p2 = s[i].endPoint();
4620 }
4621 if (abs(tmp_p2.distance(tmp_point)) > abs(interDist))
4622 {
4623 p2 = tmp_point.origin;
4624 p1 = tmp_p2.origin;
4625 interDist = -abs(tmp_p2.distance(tmp_point));
4626 }
4627 }
4628 }
4629
4630 interNorm = (p1 - p2) / interDist;
4631 if (interDist < -EPSILON)
4632 return true;
4633
4634 return false;
4635 }
4636
4637}
Real
double Real
Definition Typedef.inl:23
M_PI
#define M_PI
Definition Typedef.inl:36
dyno::Interval::inside
DYN_FUNC bool inside(Real val) const
Definition Interval.inl:97
dyno::Interval::rightLimit
DYN_FUNC Real rightLimit() const
Definition Interval.h:25
dyno::Interval::leftLimit
DYN_FUNC Real leftLimit() const
Definition Interval.h:24
dyno::Quat::toMatrix3x3
DYN_FUNC SquareMatrix< Real, 3 > toMatrix3x3() const
Definition Quat.inl:275
dyno::TAlignedBox3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:721
dyno::TAlignedBox3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:720
dyno::TAlignedBox3D::checkOverlap
DYN_FUNC bool checkOverlap(const TAlignedBox3D< Real > &abox) const
Definition Primitive3D.inl:3959
dyno::TAlignedBox3D::intersect
DYN_FUNC bool intersect(const TAlignedBox3D< Real > &abox, TAlignedBox3D< Real > &interBox) const
Definition Primitive3D.inl:3920
dyno::TAlignedBox3D::meshInsert
DYN_FUNC bool meshInsert(const TTriangle3D< Real > &tri) const
Definition Primitive3D.inl:3985
dyno::TAlignedBox3D::rotate
DYN_FUNC TOrientedBox3D< Real > rotate(const Matrix3D &mat)
Definition Primitive3D.inl:4066
dyno::TAlignedBox3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:4080
dyno::TAlignedBox3D::merge
DYN_FUNC TAlignedBox3D< Real > merge(const TAlignedBox3D< Real > &aabb) const
Definition Primitive3D.inl:3974
dyno::TCapsule3D::TCapsule3D
DYN_FUNC TCapsule3D()
Definition Primitive3D.inl:3307
dyno::TCapsule3D::volume
DYN_FUNC Real volume() const
Definition Primitive3D.inl:3345
dyno::TCapsule3D::centerline
DYN_FUNC TSegment3D< Real > centerline() const
Definition Primitive3D.h:661
dyno::TCapsule3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:644
dyno::TCapsule3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:664
dyno::TCapsule3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3352
dyno::TCapsule3D::endPoint
DYN_FUNC Coord3D endPoint() const
Definition Primitive3D.h:659
dyno::TCapsule3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb() const
Definition Primitive3D.inl:3358
dyno::TCapsule3D::startPoint
DYN_FUNC Coord3D startPoint() const
Definition Primitive3D.h:658
dyno::TCone3D::TCone3D
DYN_FUNC TCone3D()
Definition Primitive3D.inl:3262
dyno::TCone3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:635
dyno::TCone3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:621
dyno::TCylinder3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:601
dyno::TCylinder3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:614
dyno::TDisk3D::TDisk3D
DYN_FUNC TDisk3D()
Definition Primitive3D.inl:3146
dyno::TDisk3D::area
DYN_FUNC Real area()
Definition Primitive3D.inl:3171
dyno::TDisk3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:557
dyno::TDisk3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:3177
dyno::TLine3D
1D geometric primitives in three-dimensional space
Definition Primitive3D.h:259
dyno::TLine3D::proximity
DYN_FUNC TSegment3D< Real > proximity(const TLine3D< Real > &line) const
Definition Primitive3D.inl:879
dyno::TLine3D::parameter
DYN_FUNC Real parameter(const Coord3D &pos) const
Definition Primitive3D.inl:1896
dyno::TLine3D::intersect
DYN_FUNC int intersect(const TPlane3D< Real > &plane, TPoint3D< Real > &interPt) const
intersection tests
Definition Primitive3D.inl:1622
dyno::TLine3D::TLine3D
DYN_FUNC TLine3D()
Definition Primitive3D.inl:858
dyno::TLine3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:1905
dyno::TLine3D::distanceSquared
DYN_FUNC Real distanceSquared(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:1566
dyno::TLine3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:262
dyno::TLine3D::distance
DYN_FUNC Real distance(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:1530
dyno::TLine3D::direction
Coord3D direction
Definition Primitive3D.h:333
dyno::TOrientedBox3D::volume
DYN_FUNC Real volume()
Definition Primitive3D.inl:4194
dyno::TOrientedBox3D::Coord2D
Vector< Real, 2 > Coord2D
Definition Primitive3D.h:748
dyno::TOrientedBox3D::u
Coord3D u
three unit vectors u, v and w forming a right-handed orthornormal basis
Definition Primitive3D.h:800
dyno::TOrientedBox3D::vertex
DYN_FUNC TPoint3D< Real > vertex(const int i) const
Definition Primitive3D.inl:4129
dyno::TOrientedBox3D::edge
DYN_FUNC TSegment3D< Real > edge(const int i) const
Definition Primitive3D.inl:4140
dyno::TOrientedBox3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:749
dyno::TOrientedBox3D::face
DYN_FUNC TRectangle3D< Real > face(const int i) const
Definition Primitive3D.inl:4150
dyno::TOrientedBox3D::extent
Coord3D extent
half the dimension in each of the u, v, and w directions
Definition Primitive3D.h:805
dyno::TOrientedBox3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:4200
dyno::TOrientedBox3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:750
dyno::TOrientedBox3D::point_intersect
DYN_FUNC bool point_intersect(const TOrientedBox3D< Real > &OBB, Coord3D &interNorm, Real &interDist, Coord3D &p1, Coord3D &p2) const
Definition Primitive3D.inl:4252
dyno::TOrientedBox3D::center
Coord3D center
centerpoint
Definition Primitive3D.h:794
dyno::TOrientedBox3D::rotate
DYN_FUNC TOrientedBox3D< Real > rotate(const Matrix3D &mat)
Definition Primitive3D.inl:4220
dyno::TOrientedBox3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:4234
dyno::TPlane3D
2D geometric primitives in three-dimensional space
Definition Primitive3D.h:456
dyno::TPlane3D::TPlane3D
DYN_FUNC TPlane3D()
Definition Primitive3D.inl:2858
dyno::TPlane3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2879
dyno::TPlane3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:459
dyno::TPlane3D::normal
Coord3D normal
the plane will be treated as a single point if its normal is zero
Definition Primitive3D.h:473
dyno::TPoint3D
0D geometric primitive in three-dimensional space
Definition Primitive3D.h:109
dyno::TPoint3D::TPoint3D
DYN_FUNC TPoint3D()
Definition Primitive3D.inl:9
dyno::TPoint3D::distanceSquared
DYN_FUNC Real distanceSquared(const TPoint3D< Real > &pt) const
dyno::TPoint3D::operator=
DYN_FUNC TPoint3D & operator=(const Coord3D &p)
Definition Primitive3D.inl:39
dyno::TPoint3D::project
DYN_FUNC TPoint3D< Real > project(const TPlane3D< Real > &plane) const
project a point onto planar components – planes, triangles and disks
Definition Primitive3D.inl:90
dyno::TPoint3D< double >::Coord2D
Vector< double, 2 > Coord2D
Definition Primitive3D.h:111
dyno::TPoint3D::project
DYN_FUNC TPoint3D< Real > project(const TLine3D< Real > &line) const
project a point onto linear components – lines, rays and segments
Definition Primitive3D.inl:46
dyno::TPoint3D::operator-
DYN_FUNC const TSegment3D< Real > operator-(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:852
dyno::TPoint3D::inside
DYN_FUNC bool inside(const TLine3D< Real > &line) const
check whether a point strictly lies inside (excluding boundary) a 1D geometric primitive
Definition Primitive3D.inl:682
dyno::TPoint3D::distance
DYN_FUNC Real distance(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:500
dyno::TPoint3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:112
dyno::TRay3D::distanceSquared
DYN_FUNC Real distanceSquared(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:2136
dyno::TRay3D::parameter
DYN_FUNC Real parameter(const Coord3D &pos) const
Definition Primitive3D.inl:2338
dyno::TRay3D::origin
Coord3D origin
Definition Primitive3D.h:390
dyno::TRay3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2347
dyno::TRay3D::intersect
DYN_FUNC int intersect(const TPlane3D< Real > &plane, TPoint3D< Real > &interPt) const
Definition Primitive3D.inl:2154
dyno::TRay3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:341
dyno::TRay3D::proximity
DYN_FUNC TSegment3D< Real > proximity(const TRay3D< Real > &ray) const
Definition Primitive3D.inl:1933
dyno::TRay3D::distance
DYN_FUNC Real distance(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:2118
dyno::TRay3D::direction
Coord3D direction
Definition Primitive3D.h:393
dyno::TRay3D::TRay3D
DYN_FUNC TRay3D()
Definition Primitive3D.inl:1911
dyno::TRectangle3D::axis
Coord3D axis[2]
two orthonormal unit axis
Definition Primitive3D.h:548
dyno::TRectangle3D::Coord2D
Vector< Real, 2 > Coord2D
Definition Primitive3D.h:519
dyno::TRectangle3D::vertex
DYN_FUNC TPoint3D< Real > vertex(const int i) const
Definition Primitive3D.inl:3072
dyno::TRectangle3D::computeParams
DYN_FUNC bool computeParams(const Coord3D &p, Param &par) const
Definition Primitive3D.inl:3113
dyno::TRectangle3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:520
dyno::TRectangle3D::edge
DYN_FUNC TSegment3D< Real > edge(const int i) const
Definition Primitive3D.inl:3095
dyno::TRectangle3D::normal
DYN_FUNC Coord3D normal() const
Definition Primitive3D.inl:3107
dyno::TRectangle3D::area
DYN_FUNC Real area() const
Definition Primitive3D.inl:3101
dyno::TRectangle3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3131
dyno::TSegment3D::distance
DYN_FUNC Real distance(const TSegment3D< Real > &segment) const
Definition Primitive3D.inl:2607
dyno::TSegment3D::endPoint
DYN_FUNC Coord3D & endPoint()
Definition Primitive3D.h:439
dyno::TSegment3D::operator-
DYN_FUNC TSegment3D< Real > operator-(void) const
Definition Primitive3D.inl:2842
dyno::TSegment3D::parameter
DYN_FUNC Real parameter(const Coord3D &pos) const
Definition Primitive3D.inl:2832
dyno::TSegment3D::lengthSquared
DYN_FUNC Real lengthSquared() const
Definition Primitive3D.inl:2826
dyno::TSegment3D::direction
DYN_FUNC Coord3D direction() const
Definition Primitive3D.h:444
dyno::TSegment3D::proximity
DYN_FUNC TSegment3D< Real > proximity(const TSegment3D< Real > &segment) const
Definition Primitive3D.inl:2374
dyno::TSegment3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2852
dyno::TSegment3D::length
DYN_FUNC Real length() const
Definition Primitive3D.inl:2820
dyno::TSegment3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:401
dyno::TSegment3D::TSegment3D
DYN_FUNC TSegment3D()
Definition Primitive3D.inl:2353
dyno::TSegment3D::intersect
DYN_FUNC bool intersect(const TPlane3D< Real > &plane, TPoint3D< Real > &interPt) const
Definition Primitive3D.inl:2631
dyno::TSegment3D::startPoint
DYN_FUNC Coord3D & startPoint()
Definition Primitive3D.h:438
dyno::TSegment3D::distanceSquared
DYN_FUNC Real distanceSquared(const TSegment3D< Real > &segment) const
Definition Primitive3D.inl:2619
dyno::TSphere3D
3D geometric primitives in three-dimensional space
Definition Primitive3D.h:575
dyno::TSphere3D::volume
DYN_FUNC Real volume()
Definition Primitive3D.inl:3215
dyno::TSphere3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:3221
dyno::TSphere3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:578
dyno::TSphere3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:3296
dyno::TSphere3D::TSphere3D
DYN_FUNC TSphere3D()
Definition Primitive3D.inl:3183
dyno::TSphere3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:593
dyno::TTet3D
vertices are ordered so that the normal vectors for the triangular faces point outwards 3 / | \ 0 - 2...
Definition Primitive3D.h:677
dyno::TTet3D::circumcenter
DYN_FUNC TPoint3D< Real > circumcenter() const
Definition Primitive3D.inl:3820
dyno::TTet3D::contain
DYN_FUNC bool contain(Coord3D p)
Definition Primitive3D.inl:3879
dyno::TTet3D::barycenter
DYN_FUNC TPoint3D< Real > barycenter() const
Definition Primitive3D.inl:3873
dyno::TTet3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:680
dyno::TTet3D::v
Coord3D v[4]
Definition Primitive3D.h:712
dyno::TTet3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:681
dyno::TTet3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3501
dyno::TTet3D::edge
DYN_FUNC TSegment3D< Real > edge(const int index) const
Definition Primitive3D.inl:3396
dyno::TTet3D::intersect
DYN_FUNC bool intersect(const TTet3D< Real > &tet, Coord3D &interNorm, Real &interDist, Coord3D &p1, Coord3D &p2, int need_distance=1) const
Definition Primitive3D.inl:3523
dyno::TTet3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:3805
dyno::TTet3D::TTet3D
DYN_FUNC TTet3D()
Definition Primitive3D.inl:3369
dyno::TTet3D::computeBarycentricCoordinates
DYN_FUNC Vector< Real, 4 > computeBarycentricCoordinates(const Coord3D &p)
Definition Primitive3D.inl:3885
dyno::TTet3D::expand
DYN_FUNC void expand(Real r)
Definition Primitive3D.inl:3508
dyno::TTet3D::face
DYN_FUNC TTriangle3D< Real > face(const int index) const
Definition Primitive3D.inl:3424
dyno::TTet3D::solidAngle
DYN_FUNC Real solidAngle(const int index) const
Definition Primitive3D.inl:3450
dyno::TTet3D::volume
DYN_FUNC Real volume() const
Definition Primitive3D.inl:3490
dyno::TTriangle3D::distance
DYN_FUNC Real distance(const TTriangle3D< Real > &triangle) const
Definition Primitive3D.inl:3039
dyno::TTriangle3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:2981
dyno::TTriangle3D::computeLocation
DYN_FUNC Coord3D computeLocation(const Param &bary) const
Definition Primitive3D.inl:2959
dyno::TTriangle3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:482
dyno::TTriangle3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2974
dyno::TTriangle3D::maximumEdgeLength
DYN_FUNC Real maximumEdgeLength() const
Definition Primitive3D.inl:2968
dyno::TTriangle3D::computeBarycentrics
DYN_FUNC bool computeBarycentrics(const Coord3D &p, Param &bary) const
Definition Primitive3D.inl:2927
dyno::TTriangle3D::area
DYN_FUNC Real area() const
Definition Primitive3D.inl:2909
dyno::TTriangle3D::distanceSquared
DYN_FUNC Real distanceSquared(const TTriangle3D< Real > &triangle) const
Definition Primitive3D.inl:2992
dyno::TTriangle3D::normal
DYN_FUNC Coord3D normal() const
Definition Primitive3D.inl:2915
V
#define V(a, b, c)
T
#define T(t)
N
#define N(x, y, z)
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::sign
DYN_FUNC int sign(T const &a, T const EPS=EPSILON)
Definition SimpleMath.h:39
dyno::REAL_EPSILON_SQUARED
constexpr Real REAL_EPSILON_SQUARED
Definition Typedef.inl:44
dyno::REAL_MAX
constexpr Real REAL_MAX
Definition Typedef.inl:45
dyno::cross
DYN_FUNC Vector< T, 3 > cross(Vector< T, 3 > const &U, Vector< T, 3 > const &V)
Definition SimpleMath.h:211
dyno::dot
DYN_FUNC T dot(Vector< T, 2 > const &U, Vector< T, 2 > const &V)
Definition SimpleMath.h:199
dyno::abs
DYN_FUNC T abs(const T &v)
Definition SimpleMath.h:81
dyno::Vec2u
Vector< uint32_t, 2 > Vec2u
Definition Vector2D.h:83
dyno::EPSILON
constexpr Real EPSILON
Definition Typedef.inl:42
dyno::minimum
DYN_FUNC T minimum(const T &v0, const T &v1)
Definition SimpleMath.h:120
dyno::AABB
::dyno::TAlignedBox3D< Real > AABB
Definition CollisionDetectionBroadPhase.h:11
dyno::REAL_EPSILON
constexpr Real REAL_EPSILON
Definition Typedef.inl:43
dyno::clamp
DYN_FUNC T clamp(const T &v, const T &lo, const T &hi)
Definition SimpleMath.h:42
dyno::maximum
DYN_FUNC T maximum(const T &v0, const T &v1)
Definition SimpleMath.h:160
dyno::clip
DYN_FUNC int clip(ClipVertex *outVerts, float *outDepths, const Vector< Real, 3 > &rPos, const Vector< Real, 3 > &e, unsigned char *clipEdges, const SquareMatrix< Real, 3 > &basis, ClipVertex *incident)
Definition CollisionDetectionAlgorithm.inl:312
M
#define M(X, Y)
Definition vgMath.h:291
Generated by doxygen 1.13.2