PeriDyno 1.2.1
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 interNum;
2324 }
2325 else if (iRay.inside(t1))
2326 {
2327 interSeg.v0 = origin + iRay.leftLimit() * direction;
2328 interSeg.v1 = interSeg.v0;
2329 return 2;
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 abs(t1 - t0) > EPSILON ? 2 : 1;
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 interSeg.v0 = v0;
2791 interSeg.v1 = v1;
2792 return 0;
2793 }
2794 }
2795
2796 template<typename Real>
2797 DYN_FUNC int TSegment3D<Real>::intersect(const TOrientedBox3D<Real>& obb, TSegment3D<Real>& interSeg) const
2798 {
2799 int num = TLine3D<Real>(v0, direction()).intersect(obb, interSeg);
2800 if (num == 0) {
2801 return 0;
2802 }
2803
2804 Real t0 = parameter(interSeg.startPoint());
2805 Real t1 = parameter(interSeg.endPoint());
2806
2807 Interval<Real> iSeg(0, 1);
2808
2809 Interval<Real> iInter = iSeg.intersect(Interval<Real>(t0, t1));
2810
2811 if (abs(iInter.size()) < EPSILON)
2812 {
2813 Coord3D p = v0 + iInter.leftLimit() * direction();
2814 Coord3D q = v0 + iInter.rightLimit() * direction();
2815
2816 interSeg = TSegment3D<Real>(p, q);
2817
2818 return 1;
2819 }
2820 else if (!iInter.isEmpty())
2821 {
2822 Coord3D p = v0 + iInter.leftLimit() * direction();
2823 Coord3D q = v0 + iInter.rightLimit() * direction();
2824
2825 interSeg = TSegment3D<Real>(p, q);
2826
2827 return 2;
2828 }
2829 else
2830 {
2831 return 0;
2832 }
2833 }
2834
2835 template<typename Real>
2836 DYN_FUNC Real TSegment3D<Real>::length() const
2837 {
2838 return (v1 - v0).norm();
2839 }
2840
2841 template<typename Real>
2842 DYN_FUNC Real TSegment3D<Real>::lengthSquared() const
2843 {
2844 return (v1 - v0).normSquared();
2845 }
2846
2847 template<typename Real>
2848 DYN_FUNC Real TSegment3D<Real>::parameter(const Coord3D& pos) const
2849 {
2850 Coord3D l = pos - v0;
2851 Coord3D dir = direction();
2852 Real d2 = dir.normSquared();
2853
2854 return d2 < REAL_EPSILON_SQUARED ? Real(0) : l.dot(dir) / d2;
2855 }
2856
2857 template<typename Real>
2858 DYN_FUNC TSegment3D<Real> TSegment3D<Real>::operator-(void) const
2859 {
2860 TSegment3D<Real> seg;
2861 seg.v0 = v1;
2862 seg.v1 = v0;
2863
2864 return seg;
2865 }
2866
2867 template<typename Real>
2868 DYN_FUNC bool TSegment3D<Real>::isValid() const
2869 {
2870 return lengthSquared() >= REAL_EPSILON_SQUARED;
2871 }
2872
2873 template<typename Real>
2874 DYN_FUNC TPlane3D<Real>::TPlane3D()
2875 {
2876 origin = Coord3D(0);
2877 normal = Coord3D(0, 1, 0);
2878 }
2879
2880 template<typename Real>
2881 DYN_FUNC TPlane3D<Real>::TPlane3D(const Coord3D& pos, const Coord3D n)
2882 {
2883 origin = pos;
2884 normal = n;
2885 }
2886
2887 template<typename Real>
2888 DYN_FUNC TPlane3D<Real>::TPlane3D(const TPlane3D& plane)
2889 {
2890 origin = plane.origin;
2891 normal = plane.normal;
2892 }
2893
2894 template<typename Real>
2895 DYN_FUNC bool TPlane3D<Real>::isValid() const
2896 {
2897 return normal.normSquared() >= REAL_EPSILON;
2898 }
2899
2900 template<typename Real>
2901 DYN_FUNC TTriangle3D<Real>::TTriangle3D()
2902 {
2903 v[0] = Coord3D(0);
2904 v[1] = Coord3D(1, 0, 0);
2905 v[2] = Coord3D(0, 0, 1);
2906 }
2907
2908 template<typename Real>
2909 DYN_FUNC TTriangle3D<Real>::TTriangle3D(const Coord3D& p0, const Coord3D& p1, const Coord3D& p2)
2910 {
2911 v[0] = p0;
2912 v[1] = p1;
2913 v[2] = p2;
2914 }
2915
2916 template<typename Real>
2917 DYN_FUNC TTriangle3D<Real>::TTriangle3D(const TTriangle3D& triangle)
2918 {
2919 v[0] = triangle.v[0];
2920 v[1] = triangle.v[1];
2921 v[2] = triangle.v[2];
2922 }
2923
2924 template<typename Real>
2925 DYN_FUNC Real TTriangle3D<Real>::area() const
2926 {
2927 return Real(0.5) * ((v[1] - v[0]).cross(v[2] - v[0])).norm();
2928 }
2929
2930 template<typename Real>
2931 DYN_FUNC typename TTriangle3D<Real>::Coord3D TTriangle3D<Real>::normal() const
2932 {
2933 Coord3D n = (v[1] - v[0]).cross(v[2] - v[0]);
2934 if (n.norm() > REAL_EPSILON_SQUARED)
2935 {
2936 n.normalize();
2937 }
2938 return n;
2939 //return ((v[1] - v[0]) / (v[1] - v[0]).norm()).cross((v[2] - v[0]) / ((v[2] - v[0])).norm());
2940 }
2941
2942 template<typename Real>
2943 DYN_FUNC bool TTriangle3D<Real>::computeBarycentrics(const Coord3D& p, Param& bary) const
2944 {
2945 if (!isValid())
2946 {
2947 bary.u = (Real)0;
2948 bary.v = (Real)0;
2949 bary.w = (Real)0;
2950
2951 return false;
2952 }
2953
2954 Coord3D q = TPoint3D<Real>(p).project(TPlane3D<Real>(v[0], normal())).origin;
2955
2956 Coord3D d0 = v[1] - v[0];
2957 Coord3D d1 = v[2] - v[0];
2958 Coord3D d2 = q - v[0];
2959 Real d00 = d0.dot(d0);
2960 Real d01 = d0.dot(d1);
2961 Real d11 = d1.dot(d1);
2962 Real d20 = d2.dot(d0);
2963 Real d21 = d2.dot(d1);
2964 Real denom = d00 * d11 - d01 * d01;
2965
2966 bary.v = (d11 * d20 - d01 * d21) / denom;
2967 bary.w = (d00 * d21 - d01 * d20) / denom;
2968 bary.u = Real(1) - bary.v - bary.w;
2969
2970 return true;
2971 }
2972
2973
2974 template<typename Real>
2975 DYN_FUNC typename TTriangle3D<Real>::Coord3D TTriangle3D<Real>::computeLocation(const Param& bary) const
2976 {
2977 Coord3D d0 = v[1] - v[0];
2978 Coord3D d1 = v[2] - v[0];
2979 return v[0] + bary.v * d0 + bary.w * d1;
2980 }
2981
2982
2983 template<typename Real>
2984 DYN_FUNC Real TTriangle3D<Real>::maximumEdgeLength() const
2985 {
2986 return maximum(maximum((v[1] - v[0]).norm(), (v[2] - v[0]).norm()), (v[2] - v[1]).norm());
2987 }
2988
2989 template<typename Real>
2990 DYN_FUNC bool TTriangle3D<Real>::isValid() const
2991 {
2992 return abs(area()) > REAL_EPSILON_SQUARED;
2993 }
2994
2995
2996 template<typename Real>
2997 DYN_FUNC TAlignedBox3D<Real> TTriangle3D<Real>::aabb()
2998 {
2999 TAlignedBox3D<Real> abox;
3000 abox.v0 = minimum(v[0], minimum(v[1], v[2]));
3001 abox.v1 = maximum(v[0], maximum(v[1], v[2]));
3002
3003 return abox;
3004 }
3005
3006
3007 template<typename Real>
3008 DYN_FUNC Real TTriangle3D<Real>::distanceSquared(const TTriangle3D<Real>& triangle) const
3009 {
3010 Vector<Real, 3> p[3];
3011 p[0] = v[0];
3012 p[1] = v[1];
3013 p[2] = v[2];
3014
3015 Vector<Real, 3> q[3];
3016 q[0] = triangle.v[0];
3017 q[1] = triangle.v[1];
3018 q[2] = triangle.v[2];
3019
3020 //VF
3021 Real minD = (q[0] - p[0]).normSquared();
3022
3023 //EF
3024 for (int st = 0; st < 3; st++)
3025 {
3026 int ind0 = st;
3027 int ind1 = (st + 1) % 3;
3028 minD = minimum(minD, TSegment3D<Real>(p[ind0], p[ind1]).distanceSquared(triangle));
3029 }
3030
3031 for (int st = 0; st < 3; st++)
3032 {
3033 int ind0 = st;
3034 int ind1 = (st + 1) % 3;
3035 minD = minimum(minD, TSegment3D<Real>(q[ind0], q[ind1]).distanceSquared(*this));
3036 }
3037
3038// for (int st = 0; st < 3; st++)
3039// {
3040// int ind0 = st;
3041// int ind1 = (st + 1) % 3;
3042// for (int ss = 0; ss < 3; ss++)
3043// {
3044// int ind2 = st;
3045// int ind3 = (st + 1) % 3;
3046//
3047// minD = minimum(minD, TSegment3D<Real>(p[ind0], p[ind1]).distanceSquared(TSegment3D<Real>(p[ind2], p[ind3])));
3048// }
3049// }
3050
3051 return minD;
3052 }
3053
3054 template<typename Real>
3055 DYN_FUNC Real TTriangle3D<Real>::distance(const TTriangle3D<Real>& triangle) const
3056 {
3057 return glm::sqrt(distanceSquared(triangle));
3058 }
3059
3060 template<typename Real>
3061 DYN_FUNC TRectangle3D<Real>::TRectangle3D()
3062 {
3063 center = Coord3D(0);
3064 axis[0] = Coord3D(1, 0, 0);
3065 axis[1] = Coord3D(0, 0, 1);
3066 extent = Coord2D(1, 1);
3067 }
3068
3069 template<typename Real>
3070 DYN_FUNC TRectangle3D<Real>::TRectangle3D(const Coord3D& c, const Coord3D& a0, const Coord3D& a1, const Coord2D& ext)
3071 {
3072 center = c;
3073 axis[0] = a0;
3074 axis[1] = a1;
3075 extent = ext.maximum(Coord2D(0));
3076 }
3077
3078 template<typename Real>
3079 DYN_FUNC TRectangle3D<Real>::TRectangle3D(const TRectangle3D<Real>& rectangle)
3080 {
3081 center = rectangle.center;
3082 axis[0] = rectangle.axis[0];
3083 axis[1] = rectangle.axis[1];
3084 extent = rectangle.extent;
3085 }
3086
3087 template<typename Real>
3088 DYN_FUNC TPoint3D<Real> TRectangle3D<Real>::vertex(const int i) const
3089 {
3090 int id = i % 4;
3091 Coord3D u = axis[0] * extent[0];
3092 Coord3D v = axis[1] * extent[1];
3093 switch (id)
3094 {
3095 case 0:
3096 return TPoint3D<Real>(center - u - v);
3097 break;
3098 case 1:
3099 return TPoint3D<Real>(center + u - v);
3100 break;
3101 case 2:
3102 return TPoint3D<Real>(center + u + v);
3103 break;
3104 default:
3105 return TPoint3D<Real>(center - u + v);
3106 break;
3107 }
3108 }
3109
3110 template<typename Real>
3111 DYN_FUNC TSegment3D<Real> TRectangle3D<Real>::edge(const int i) const
3112 {
3113 return vertex(i + 1) - vertex(i);
3114 }
3115
3116 template<typename Real>
3117 DYN_FUNC Real TRectangle3D<Real>::area() const
3118 {
3119 return Real(4) * extent[0] * extent[1];
3120 }
3121
3122 template<typename Real>
3123 DYN_FUNC typename TRectangle3D<Real>::Coord3D TRectangle3D<Real>::normal() const
3124 {
3125 return axis[0].cross(axis[1]);
3126 }
3127
3128 template<typename Real>
3129 DYN_FUNC bool TRectangle3D<Real>::computeParams(const Coord3D& p, Param& par) const
3130 {
3131 if (!isValid())
3132 {
3133 par.u = (Real)0;
3134 par.v = (Real)0;
3135
3136 return false;
3137 }
3138
3139 Coord3D offset = p - center;
3140 par.u = offset.dot(axis[0]);
3141 par.v = offset.dot(axis[1]);
3142
3143 return true;
3144 }
3145
3146 template<typename Real>
3147 DYN_FUNC bool TRectangle3D<Real>::isValid() const
3148 {
3149 bool bValid = true;
3150 bValid &= extent[0] >= REAL_EPSILON;
3151 bValid &= extent[1] >= REAL_EPSILON;
3152
3153 bValid &= abs(axis[0].normSquared() - Real(1)) < REAL_EPSILON;
3154 bValid &= abs(axis[1].normSquared() - Real(1)) < REAL_EPSILON;
3155
3156 bValid &= abs(axis[0].dot(axis[1])) < REAL_EPSILON;
3157
3158 return bValid;
3159 }
3160
3161 template<typename Real>
3162 DYN_FUNC TDisk3D<Real>::TDisk3D()
3163 {
3164 center = Coord3D(0);
3165 normal = Coord3D(0, 1, 0);
3166 radius = 1;
3167 }
3168
3169 template<typename Real>
3170 DYN_FUNC TDisk3D<Real>::TDisk3D(const Coord3D& c, const Coord3D& n, const Real& r)
3171 {
3172 center = c;
3173 normal = n.norm() < REAL_EPSILON ? Coord3D(1, 0, 0) : n;
3174 normal.normalize();
3175 radius = r < 0 ? 0 : r;
3176 }
3177
3178 template<typename Real>
3179 DYN_FUNC TDisk3D<Real>::TDisk3D(const TDisk3D& circle)
3180 {
3181 center = circle.center;
3182 normal = circle.normal;
3183 radius = circle.radius;
3184 }
3185
3186 template<typename Real>
3187 DYN_FUNC Real TDisk3D<Real>::area()
3188 {
3189 return Real(M_PI) * radius * radius;
3190 }
3191
3192 template<typename Real>
3193 DYN_FUNC bool TDisk3D<Real>::isValid()
3194 {
3195 return radius > REAL_EPSILON && normal.norm() > REAL_EPSILON;
3196 }
3197
3198 template<typename Real>
3199 DYN_FUNC TSphere3D<Real>::TSphere3D()
3200 {
3201 center = Coord3D(0);
3202 rotation = Quat<Real>();
3203 radius = 1;
3204 }
3205
3206 template<typename Real>
3207 DYN_FUNC TSphere3D<Real>::TSphere3D(const Coord3D& c, const Real& r)
3208 {
3209 center = c;
3210 rotation = Quat<Real>();
3211 radius = r < 0 ? 0 : r;
3212 }
3213
3214 template<typename Real>
3215 DYN_FUNC dyno::TSphere3D<Real>::TSphere3D(const Coord3D& c, const Quat<Real>& rot, const Real& r)
3216 {
3217 center = c;
3218 rotation = rot;
3219 radius = r < 0 ? 0 : r;
3220 }
3221
3222 template<typename Real>
3223 DYN_FUNC TSphere3D<Real>::TSphere3D(const TSphere3D& sphere)
3224 {
3225 center = sphere.center;
3226 rotation = sphere.rotation;
3227 radius = sphere.radius;
3228 }
3229
3230 template<typename Real>
3231 DYN_FUNC Real TSphere3D<Real>::volume()
3232 {
3233 return 4 * Real(M_PI) * radius * radius * radius / 3;
3234 }
3235
3236 template<typename Real>
3237 DYN_FUNC bool TSphere3D<Real>::isValid()
3238 {
3239 return radius >= REAL_EPSILON;
3240 }
3241
3242//*********************** Cylinder3D **************************
3243 template<typename Real>
3244 DYN_FUNC TCylinder3D<Real>::TCylinder3D()
3245 {
3246 center = Coord3D(0);
3247 height = Real(1);
3248 radius = Real(0.5);
3249
3250 rotation = Quat<Real>();
3251 scale = Coord3D(1);
3252 }
3253
3254 template<typename Real>
3255 DYN_FUNC TCylinder3D<Real>::TCylinder3D(const Coord3D& c, const Real& h, const Real& r, const Quat<Real>& rot, const Coord3D& s)
3256 {
3257 center = c;
3258 height = h;
3259 radius = r;
3260
3261 rotation = rot;
3262 scale = s;
3263 }
3264
3265 template<typename Real>
3266 DYN_FUNC TCylinder3D<Real>::TCylinder3D(const TCylinder3D<Real>& cylinder)
3267 {
3268 center = cylinder.center;
3269 height = cylinder.height;
3270 radius = cylinder.radius;
3271
3272 rotation = cylinder.rotation;
3273 scale = cylinder.scale;
3274 }
3275
3276 //*********************** Cone3D **************************
3277 template<typename Real>
3278 DYN_FUNC TCone3D<Real>::TCone3D()
3279 {
3280 center = Coord3D(0);
3281 height = Real(1);
3282 radius = Real(0.5);
3283
3284 rotation = Quat<Real>();
3285 scale = Coord3D(1);
3286 }
3287
3288
3289 template<typename Real>
3290 DYN_FUNC TCone3D<Real>::TCone3D(const Coord3D& c, const Real& h, const Real& r, const Quat<Real>& rot, const Coord3D& s)
3291 {
3292 center = c;
3293 height = h;
3294 radius = r;
3295
3296 rotation = rot;
3297 scale = s;
3298 }
3299
3300 template<typename Real>
3301 DYN_FUNC TCone3D<Real>::TCone3D(const TCone3D<Real>& cone)
3302 {
3303 center = cone.center;
3304 height = cone.height;
3305 radius = cone.radius;
3306
3307 rotation = cone.rotation;
3308 scale = cone.scale;
3309 }
3310
3311 template<typename Real>
3312 DYN_FUNC TAlignedBox3D<Real> TSphere3D<Real>::aabb()
3313 {
3314 TAlignedBox3D<Real> abox;
3315 abox.v0 = center - radius;
3316 abox.v1 = center + radius;
3317
3318 return abox;
3319 }
3320
3321
3322 template<typename Real>
3323 DYN_FUNC TCapsule3D<Real>::TCapsule3D()
3324 {
3325 center = Coord3D(0, 0, 0);
3326 rotation = Quat<Real>();
3327 radius = Real(1);
3328 halfLength = Real(1);
3329 }
3330
3331 template<typename Real>
3332 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const Coord3D& v0, const Coord3D& v1, const Real& r)
3333 {
3334 center = 0.5f * (v0 + v1);
3335 Real len = (v1 - v0).norm();
3336
3337 rotation = Quat<Real>(Coord3D(0, 1, 0), v1 - v0);
3338 radius = Real(r);
3339 halfLength = Real(len * 0.5f);
3340 }
3341
3342 template<typename Real>
3343 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const Coord3D& c, const Quat<Real>& q, const Real& r, const Real& hl)
3344 : center(c)
3345 , rotation(q)
3346 , radius(r)
3347 , halfLength(hl)
3348 {
3349 }
3350
3351 template<typename Real>
3352 DYN_FUNC TCapsule3D<Real>::TCapsule3D(const TCapsule3D& capsule)
3353 {
3354 center = capsule.center;
3355 rotation = capsule.rotation;
3356 radius = capsule.radius;
3357 halfLength = capsule.halfLength;
3358 }
3359
3360 template<typename Real>
3361 DYN_FUNC Real TCapsule3D<Real>::volume() const
3362 {
3363 Real r2 = radius * radius;
3364 return 2 * Real(M_PI) * r2 * halfLength + Real(4.0 * M_PI / 3.0) * r2 * radius;
3365 }
3366
3367 template<typename Real>
3368 DYN_FUNC bool TCapsule3D<Real>::isValid() const
3369 {
3370 return radius >= REAL_EPSILON && halfLength >= REAL_EPSILON;
3371 }
3372
3373 template<typename Real>
3374 DYN_FUNC TAlignedBox3D<Real> TCapsule3D<Real>::aabb() const
3375 {
3376 TAlignedBox3D<Real> abox;
3377 Coord3D v0 = startPoint();
3378 Coord3D v1 = endPoint();
3379 abox.v0 = minimum(v0, v1) - radius;
3380 abox.v1 = maximum(v0, v1) + radius;
3381 return abox;
3382 }
3383
3384 template<typename Real>
3385 DYN_FUNC TTet3D<Real>::TTet3D()
3386 {
3387 v[0] = Coord3D(0);
3388 v[1] = Coord3D(1, 0, 0);
3389 v[2] = Coord3D(0, 0, 1);
3390 v[3] = Coord3D(0, 1, 0);
3391 }
3392
3393 template<typename Real>
3394 DYN_FUNC TTet3D<Real>::TTet3D(const Coord3D& v0, const Coord3D& v1, const Coord3D& v2, const Coord3D& v3)
3395 {
3396 v[0] = v0;
3397 v[1] = v1;
3398 v[2] = v2;
3399 v[3] = v3;
3400 }
3401
3402 template<typename Real>
3403 DYN_FUNC TTet3D<Real>::TTet3D(const TTet3D& tet)
3404 {
3405 v[0] = tet.v[0];
3406 v[1] = tet.v[1];
3407 v[2] = tet.v[2];
3408 v[3] = tet.v[3];
3409 }
3410
3411 template<typename Real>
3412 DYN_FUNC TSegment3D<Real> TTet3D<Real>::edge(const int index) const
3413 {
3414 switch (index)
3415 {
3416 case 0:
3417 return TSegment3D<Real>(v[0], v[1]);
3418 break;
3419 case 1:
3420 return TSegment3D<Real>(v[0], v[2]);
3421 break;
3422 case 2:
3423 return TSegment3D<Real>(v[0], v[3]);
3424 break;
3425 case 3:
3426 return TSegment3D<Real>(v[1], v[2]);
3427 break;
3428 case 4:
3429 return TSegment3D<Real>(v[1], v[3]);
3430 break;
3431 case 5:
3432 return TSegment3D<Real>(v[2], v[3]);
3433 break;
3434 default:
3435 break;
3436 }
3437 }
3438
3439 template<typename Real>
3440 DYN_FUNC TTriangle3D<Real> TTet3D<Real>::face(const int index) const
3441 {
3442 switch (index)
3443 {
3444 case 0:
3445 return TTriangle3D<Real>(v[1], v[3], v[2]);
3446 break;
3447 case 1:
3448 return TTriangle3D<Real>(v[0], v[2], v[3]);
3449 break;
3450 case 2:
3451 return TTriangle3D<Real>(v[0], v[3], v[1]);
3452 break;
3453 case 3:
3454 return TTriangle3D<Real>(v[0], v[1], v[2]);
3455 break;
3456 default:
3457 break;
3458 }
3459
3460 //return an ill triangle in case index is out of range
3461 return TTriangle3D<Real>(Coord3D(0), Coord3D(0), Coord3D(0));
3462 }
3463
3464 //https://en.wikipedia.org/wiki/Solid_angle
3465 template<typename Real>
3466 DYN_FUNC Real TTet3D<Real>::solidAngle(const int index) const
3467 {
3468 Coord3D v0, v1, v2, v3;
3469 switch (index)
3470 {
3471 case 0:
3472 v0 = v[0]; v1 = v[1]; v2 = v[3]; v3 = v[2];
3473 break;
3474 case 1:
3475 v0 = v[1]; v1 = v[0]; v2 = v[2]; v3 = v[3];
3476 break;
3477 case 2:
3478 v0 = v[2]; v1 = v[0]; v2 = v[3]; v3 = v[1];
3479 break;
3480 case 3:
3481 v0 = v[3]; v1 = v[0]; v2 = v[1]; v3 = v[2];
3482 break;
3483 default:
3484 break;
3485 }
3486
3487 Coord3D A = v1 - v0;
3488 Coord3D B = v2 - v0;
3489 Coord3D C = v3 - v0;
3490
3491 Real a = A.norm();
3492 Real b = B.norm();
3493 Real c = C.norm();
3494
3495 Real dividend = A.dot(B.cross(C));
3496 Real divisor = a * b * c + A.dot(B) * c + A.dot(C) * b + B.dot(C) * a;
3497
3498 Real angle = 2 * glm::atan(glm::abs(dividend) / divisor);
3499
3500 if (dividend > 0 && divisor < 0) angle += M_PI;
3501
3502 return angle;
3503 }
3504
3505 template<typename Real>
3506 DYN_FUNC Real TTet3D<Real>::volume() const
3507 {
3508 Matrix3D M;
3509 M.setRow(0, v[1] - v[0]);
3510 M.setRow(1, v[2] - v[0]);
3511 M.setRow(2, v[3] - v[0]);
3512
3513 return M.determinant() / Real(6);
3514 }
3515
3516 template<typename Real>
3517 DYN_FUNC bool TTet3D<Real>::isValid() const
3518 {
3519 return volume() >= REAL_EPSILON;
3520 }
3521
3522
3523 template<typename Real>
3524 DYN_FUNC void TTet3D<Real>::expand(Real r)
3525 {
3526 Coord3D center;
3527 center = (v[0] + v[1] + v[2] + v[3]) / 4.0f;
3528 for (int idx = 0; idx < 4; idx++)
3529 {
3530 Coord3D dir = (v[idx] - center);
3531 Real rr = dir.norm();
3532 dir /= dir.norm();
3533 rr += r;
3534 v[idx] = dir * rr + center;
3535 }
3536 }
3537
3538 template<typename Real>
3539 DYN_FUNC bool TTet3D<Real>::intersect(const TTet3D<Real>& tet2, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2, int need_distance) const
3540 {
3541
3542
3543 // printf("VVVVVVVVVVVVVVVVVVVVVVv\n");
3544
3545 Coord3D mid = (tet2.v[0] + tet2.v[1] + tet2.v[2] + tet2.v[3]) / 4.0f;
3546 Coord3D v11 = mid + (tet2.v[0] - mid) / (tet2.v[0] - mid).norm() * ((tet2.v[0] - mid).norm());
3547 Coord3D v22 = mid + (tet2.v[1] - mid) / (tet2.v[1] - mid).norm() * ((tet2.v[1] - mid).norm());
3548 Coord3D v33 = mid + (tet2.v[2] - mid) / (tet2.v[2] - mid).norm() * ((tet2.v[2] - mid).norm());
3549 Coord3D v44 = mid + (tet2.v[3] - mid) / (tet2.v[3] - mid).norm() * ((tet2.v[3] - mid).norm());
3550 TTet3D<Real> tet(v11, v22, v33, v44);
3551
3552 Real inter_dist_0 = 0.0f;
3553 Coord3D p11, p22, interNorm_0;
3554
3555 TSegment3D<Real> s[18];
3556 s[0] = TSegment3D<Real>(v[0], v[1]);
3557 s[1] = TSegment3D<Real>(v[0], v[2]);
3558 s[2] = TSegment3D<Real>(v[0], v[3]);
3559 s[3] = TSegment3D<Real>(v[1], v[2]);
3560 s[4] = TSegment3D<Real>(v[1], v[3]);
3561 s[5] = TSegment3D<Real>(v[2], v[3]);
3562
3563 s[6] = TSegment3D<Real>(v[0], (v[1] + v[2]) / 2.0f);
3564 s[7] = TSegment3D<Real>(v[0], (v[1] + v[3]) / 2.0f);
3565 s[8] = TSegment3D<Real>(v[0], (v[3] + v[2]) / 2.0f);
3566 s[9] = TSegment3D<Real>(v[1], (v[3] + v[2]) / 2.0f);
3567 s[10] = TSegment3D<Real>(v[1], (v[0] + v[2]) / 2.0f);
3568 s[11] = TSegment3D<Real>(v[1], (v[0] + v[3]) / 2.0f);
3569
3570 s[12] = TSegment3D<Real>(v[2], (v[1] + v[0]) / 2.0f);
3571 s[13] = TSegment3D<Real>(v[2], (v[1] + v[3]) / 2.0f);
3572 s[14] = TSegment3D<Real>(v[2], (v[0] + v[3]) / 2.0f);
3573 s[15] = TSegment3D<Real>(v[3], (v[1] + v[2]) / 2.0f);
3574 s[16] = TSegment3D<Real>(v[3], (v[0] + v[2]) / 2.0f);
3575 s[17] = TSegment3D<Real>(v[3], (v[1] + v[0]) / 2.0f);
3576
3577
3578
3579 for (int i = 0; i < 18; i++)
3580 {
3581 if (i >= 6 && !(inter_dist_0 < -0.000f))
3582 break;
3583 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3584 TSegment3D<Real> tmp_seg;
3585
3586 if (l_i.intersect(tet, tmp_seg))
3587 {
3588
3589 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
3590 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
3591 if (right < left)
3592 {
3593 Real tmp = left;
3594 left = right;
3595 right = tmp;
3596 }
3597 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3598 if (right < 0 || left > maxx)
3599 continue;
3600 left = maximum(left, Real(0));
3601 right = minimum(right, maxx);
3602
3603 p1 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
3604 Bool tmp_bool;
3605 p2 = TPoint3D<Real>(p1).project(tet, tmp_bool).origin;//
3606 interDist = -(p1 - p2).norm();
3607
3608 if (need_distance)
3609 {
3610 Coord3D p11 = s[i].v0 + left * s[i].direction().normalize();
3611 Coord3D p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3612 if ((p11 - p22).norm() > abs(interDist))
3613 {
3614 p1 = p11; p2 = p22;
3615 interDist = -(p1 - p2).norm();
3616 }
3617
3618 p11 = s[i].v0 + right * s[i].direction().normalize();
3619 p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3620 if ((p11 - p22).norm() > abs(interDist))
3621 {
3622 p1 = p11; p2 = p22;
3623 interDist = -(p1 - p2).norm();
3624 }
3625 }
3626 Coord3D dir = (p1 - p2) / interDist;
3627 interNorm = dir;
3628 //dir *= -1;
3629
3630 if (interDist < inter_dist_0)
3631 {
3632 inter_dist_0 = interDist;
3633 p11 = p1;
3634 p22 = p2;
3635 interNorm_0 = interNorm;
3636 }
3637
3638 if (need_distance == 0)
3639 if (inter_dist_0 < -0.000f)
3640 {
3641 interDist = inter_dist_0;
3642 p1 = p11;
3643 p2 = p22;
3644 interNorm = interNorm_0;
3645 interDist += 0.0000f;
3646 return true;
3647 }
3648
3649 }
3650 }
3651
3652 if (inter_dist_0 < -0.000f)
3653 {
3654 interDist = inter_dist_0;
3655 p1 = p11;
3656 p2 = p22;
3657 interNorm = interNorm_0;
3658 interDist += 0.0000f;
3659 return true;
3660 }
3661 return false;
3662 }
3663
3664
3665 template<typename Real>
3666 DYN_FUNC bool TTet3D<Real>::intersect(const TTriangle3D<Real>& tri, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2, int need_distance) const
3667 {
3668
3669
3670 // printf("VVVVVVVVVVVVVVVVVVVVVVv\n");
3671
3672
3673 TTet3D<Real> tet(v[0], v[1], v[2], v[3]);
3674
3675 Real inter_dist_0 = 0.0f;
3676 Coord3D p11, p22, interNorm_0;
3677
3678 TSegment3D<Real> s[18];
3679
3680 s[0] = TSegment3D<Real>(tri.v[0], tri.v[1]);
3681 s[1] = TSegment3D<Real>(tri.v[0], tri.v[2]);
3682 s[2] = TSegment3D<Real>(tri.v[0], tri.v[2]);
3683
3684 for (int i = 0; i < 3; i++)
3685 {
3686
3687 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3688 TSegment3D<Real> tmp_seg;
3689
3690 if (l_i.intersect(tet, tmp_seg))
3691 {
3692
3693 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
3694 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
3695 if (right < left)
3696 {
3697 Real tmp = left;
3698 left = right;
3699 right = tmp;
3700 }
3701 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3702 if (right < 0 || left > maxx)
3703 continue;
3704 left = maximum(left, Real(0));
3705 right = minimum(right, maxx);
3706
3707 p2 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
3708 Bool tmp_bool;
3709 p1 = TPoint3D<Real>(p2).project(tet, tmp_bool).origin;//
3710 interDist = -(p1 - p2).norm();
3711
3712
3713 Coord3D p11 = s[i].v0 + left * s[i].direction().normalize();
3714 Coord3D p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3715 if ((p11 - p22).norm() > abs(interDist))
3716 {
3717 p2 = p11; p1 = p22;
3718 interDist = -(p1 - p2).norm();
3719 }
3720
3721 p11 = s[i].v0 + right * s[i].direction().normalize();
3722 p22 = TPoint3D<Real>(p11).project(tet, tmp_bool).origin;
3723 if ((p11 - p22).norm() > abs(interDist))
3724 {
3725 p2 = p11; p1 = p22;
3726 interDist = -(p1 - p2).norm();
3727 }
3728
3729 Coord3D dir = (p1 - p2) / interDist;
3730 interNorm = dir;
3731 //dir *= -1;
3732
3733 if (interDist < inter_dist_0)
3734 {
3735 inter_dist_0 = interDist;
3736 p11 = p1;
3737 p22 = p2;
3738 interNorm_0 = interNorm;
3739 }
3740
3741
3742
3743 }
3744 }
3745
3746 if (inter_dist_0 < -0.000f)
3747 {
3748 interDist = inter_dist_0;
3749 p1 = p11;
3750 p2 = p22;
3751 interNorm = interNorm_0;
3752 interDist += 0.0000f;
3753 return true;
3754 }
3755
3756 s[0] = TSegment3D<Real>(v[0], v[1]);
3757 s[1] = TSegment3D<Real>(v[0], v[2]);
3758 s[2] = TSegment3D<Real>(v[0], v[3]);
3759 s[3] = TSegment3D<Real>(v[1], v[2]);
3760 s[4] = TSegment3D<Real>(v[1], v[3]);
3761 s[5] = TSegment3D<Real>(v[2], v[3]);
3762
3763 s[6] = TSegment3D<Real>(v[0], (v[1] + v[2]) / 2.0f);
3764 s[7] = TSegment3D<Real>(v[0], (v[1] + v[3]) / 2.0f);
3765 s[8] = TSegment3D<Real>(v[0], (v[3] + v[2]) / 2.0f);
3766 s[9] = TSegment3D<Real>(v[1], (v[3] + v[2]) / 2.0f);
3767 s[10] = TSegment3D<Real>(v[1], (v[0] + v[2]) / 2.0f);
3768 s[11] = TSegment3D<Real>(v[1], (v[0] + v[3]) / 2.0f);
3769
3770 s[12] = TSegment3D<Real>(v[2], (v[1] + v[0]) / 2.0f);
3771 s[13] = TSegment3D<Real>(v[2], (v[1] + v[3]) / 2.0f);
3772 s[14] = TSegment3D<Real>(v[2], (v[0] + v[3]) / 2.0f);
3773 s[15] = TSegment3D<Real>(v[3], (v[1] + v[2]) / 2.0f);
3774 s[16] = TSegment3D<Real>(v[3], (v[0] + v[2]) / 2.0f);
3775 s[17] = TSegment3D<Real>(v[3], (v[1] + v[0]) / 2.0f);
3776
3777
3778
3779 for (int i = 0; i < 18; i++)
3780 {
3781
3782 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
3783 TPoint3D<Real> tmp_point;
3784
3785 if (l_i.intersect(tri, tmp_point))
3786 {
3787
3788 Real left = (tmp_point.origin - s[i].v0).dot(s[i].direction().normalize());
3789 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
3790
3791 if (left < 0 || left > maxx)
3792 continue;
3793
3794 if (left < maxx / 2.0f)
3795 p1 = s[i].v0;
3796 else
3797 p1 = s[i].v1;
3798 p2 = tmp_point.origin;//TPoint3D<Real>(p1).project(tet, tmp_bool).origin;
3799 interDist = -(p1 - p2).norm();
3800 Coord3D dir = (p1 - p2) / interDist;
3801 interNorm = dir;
3802
3803 }
3804 }
3805 if (inter_dist_0 < -0.000f)
3806 {
3807 interDist = inter_dist_0;
3808 p1 = p11;
3809 p2 = p22;
3810 interNorm = interNorm_0;
3811 interDist += 0.0000f;
3812 return true;
3813 }
3814
3815
3816
3817
3818 return false;
3819 }
3820 template<typename Real>
3821 DYN_FUNC TAlignedBox3D<Real> TTet3D<Real>::aabb()
3822 {
3823 TAlignedBox3D<Real> abox;
3824 abox.v0[0] = minimum(minimum(v[0][0], v[1][0]), minimum(v[2][0], v[3][0]));
3825 abox.v0[1] = minimum(minimum(v[0][1], v[1][1]), minimum(v[2][1], v[3][1]));
3826 abox.v0[2] = minimum(minimum(v[0][2], v[1][2]), minimum(v[2][2], v[3][2]));
3827
3828 abox.v1[0] = maximum(maximum(v[0][0], v[1][0]), maximum(v[2][0], v[3][0]));
3829 abox.v1[1] = maximum(maximum(v[0][1], v[1][1]), maximum(v[2][1], v[3][1]));
3830 abox.v1[2] = maximum(maximum(v[0][2], v[1][2]), maximum(v[2][2], v[3][2]));
3831
3832 return abox;
3833 }
3834
3835 template<typename Real>
3836 DYN_FUNC TPoint3D<Real> TTet3D<Real>::circumcenter() const
3837 {
3838 // Use coordinates relative to point `a' of the tetrahedron.
3839
3840 // ba = b - a
3841 Real ba_x = v[1][0] - v[0][0];
3842 Real ba_y = v[1][1] - v[0][1];
3843 Real ba_z = v[1][2] - v[0][2];
3844
3845 // ca = c - a
3846 Real ca_x = v[2][0] - v[0][0];
3847 Real ca_y = v[2][1] - v[0][1];
3848 Real ca_z = v[2][2] - v[0][2];
3849
3850 // da = d - a
3851 Real da_x = v[3][0] - v[0][0];
3852 Real da_y = v[3][1] - v[0][1];
3853 Real da_z = v[3][2] - v[0][2];
3854
3855 // Squares of lengths of the edges incident to `a'.
3856 Real len_ba = ba_x * ba_x + ba_y * ba_y + ba_z * ba_z;
3857 Real len_ca = ca_x * ca_x + ca_y * ca_y + ca_z * ca_z;
3858 Real len_da = da_x * da_x + da_y * da_y + da_z * da_z;
3859
3860 // Cross products of these edges.
3861
3862 // c cross d
3863 Real cross_cd_x = ca_y * da_z - da_y * ca_z;
3864 Real cross_cd_y = ca_z * da_x - da_z * ca_x;
3865 Real cross_cd_z = ca_x * da_y - da_x * ca_y;
3866
3867 // d cross b
3868 Real cross_db_x = da_y * ba_z - ba_y * da_z;
3869 Real cross_db_y = da_z * ba_x - ba_z * da_x;
3870 Real cross_db_z = da_x * ba_y - ba_x * da_y;
3871
3872 // b cross c
3873 Real cross_bc_x = ba_y * ca_z - ca_y * ba_z;
3874 Real cross_bc_y = ba_z * ca_x - ca_z * ba_x;
3875 Real cross_bc_z = ba_x * ca_y - ca_x * ba_y;
3876
3877 // Calculate the denominator of the formula.
3878 Real denominator = 0.5 / (ba_x * cross_cd_x + ba_y * cross_cd_y + ba_z * cross_cd_z);
3879
3880 // Calculate offset (from `a') of circumcenter.
3881 Real circ_x = (len_ba * cross_cd_x + len_ca * cross_db_x + len_da * cross_bc_x) * denominator;
3882 Real circ_y = (len_ba * cross_cd_y + len_ca * cross_db_y + len_da * cross_bc_y) * denominator;
3883 Real circ_z = (len_ba * cross_cd_z + len_ca * cross_db_z + len_da * cross_bc_z) * denominator;
3884
3885 return TPoint3D<Real>(circ_x + v[0][0], circ_y + v[0][1], circ_z + v[0][2]);
3886 }
3887
3888 template<typename Real>
3889 DYN_FUNC TPoint3D<Real> TTet3D<Real>::barycenter() const
3890 {
3891 return TPoint3D<Real>(Real(0.25) * (v[0] + v[1] + v[2] + v[3]));
3892 }
3893
3894 template<typename Real>
3895 DYN_FUNC bool TTet3D<Real>::contain(Coord3D p)
3896 {
3897 return TPoint3D<Real>(p).inside(*this);
3898 }
3899
3900 template<typename Real>
3901 DYN_FUNC Vector<Real, 4> TTet3D<Real>::computeBarycentricCoordinates(const Coord3D& p)
3902 {
3903 SquareMatrix<Real, 3> T(v[0].x - v[3].x, v[1].x - v[3].x, v[2].x - v[3].x,
3904 v[0].y - v[3].y, v[1].y - v[3].y, v[2].y - v[3].y,
3905 v[0].z - v[3].z, v[1].z - v[3].z, v[2].z - v[3].z);
3906
3907 Vector<Real, 3> b(p.x - v[3].x, p.y - v[3].y, p.z - v[3].z);
3908
3909 Vector<Real, 3> lambda = T.inverse() * b;
3910
3911 return Vector<Real, 4>(lambda.x, lambda.y, lambda.z, Real(1) - lambda.x - lambda.y - lambda.z);
3912 }
3913
3914 template<typename Real>
3915 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D()
3916 {
3917 v0 = Coord3D(Real(-1));
3918 v1 = Coord3D(Real(1));
3919 }
3920
3921 template<typename Real>
3922 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D(const Coord3D& p0, const Coord3D& p1)
3923 {
3924 v0 = p0;
3925 v1 = p1;
3926 }
3927
3928 template<typename Real>
3929 DYN_FUNC TAlignedBox3D<Real>::TAlignedBox3D(const TAlignedBox3D& box)
3930 {
3931 v0 = box.v0;
3932 v1 = box.v1;
3933 }
3934
3935 template<typename Real>
3936 DYN_FUNC bool TAlignedBox3D<Real>::intersect(const TAlignedBox3D& abox, TAlignedBox3D& interBox) const
3937 {
3938 for (int i = 0; i < 3; i++)
3939 {
3940 if (v1[i] <= abox.v0[i] || v0[i] >= abox.v1[i])
3941 {
3942 return false;
3943 }
3944 }
3945
3946 interBox.v0 = v0.maximum(abox.v0);
3947 interBox.v1 = v1.minimum(abox.v1);
3948
3949 for (int i = 0; i < 3; i++)
3950 {
3951 if (v1[i] <= abox.v1[i])
3952 {
3953 interBox.v1[i] = v1[i];
3954 }
3955 else
3956 {
3957 interBox.v1[i] = abox.v1[i];
3958 }
3959
3960 if (v0[i] <= abox.v0[i])
3961 {
3962 interBox.v0[i] = abox.v0[i];
3963 }
3964 else
3965 {
3966 interBox.v0[i] = v0[i];
3967 }
3968 }
3969
3970 return true;
3971 }
3972
3973
3974 template<typename Real>
3975 DYN_FUNC bool TAlignedBox3D<Real>::checkOverlap(const TAlignedBox3D<Real>& abox) const
3976 {
3977 for (int i = 0; i < 3; i++)
3978 {
3979 if (v1[i] <= abox.v0[i] || v0[i] >= abox.v1[i])
3980 {
3981 return false;
3982 }
3983 }
3984
3985 return true;
3986 }
3987
3988
3989 template<typename Real>
3990 DYN_FUNC TAlignedBox3D<Real> TAlignedBox3D<Real>::merge(const TAlignedBox3D<Real>& aabb) const
3991 {
3992 TAlignedBox3D<Real> ret;
3993 ret.v0 = v0.minimum(aabb.v0);
3994 ret.v1 = v1.maximum(aabb.v1);
3995
3996 return ret;
3997 }
3998
3999
4000 template<typename Real>
4001 DYN_FUNC bool TAlignedBox3D<Real>::meshInsert(const TTriangle3D<Real>& tri) const
4002 {
4003 //return true;
4004 TPoint3D<Real> p0 = TPoint3D<Real>(tri.v[0]);
4005 TPoint3D<Real> p1 = TPoint3D<Real>(tri.v[1]);
4006 TPoint3D<Real> p2 = TPoint3D<Real>(tri.v[2]);
4007 TAlignedBox3D<Real> AABB = TAlignedBox3D<Real>(v0, v1);
4008 if (p0.inside(AABB))return true;
4009 if (p1.inside(AABB))return true;
4010 if (p2.inside(AABB))return true;
4011
4012 TPoint3D<Real> pTmp = TPoint3D<Real>((v0 + v1) / 2);
4013 Real r = (pTmp.origin - v0).norm();
4014 // if (abs(pTmp.distance(tri)) < r) return true;
4015
4016 Coord3D P0 = v0;
4017 Coord3D P1 = Coord3D(v0[0], v0[1], v1[2]);
4018 Coord3D P2 = Coord3D(v0[0], v1[1], v0[2]);
4019 Coord3D P3 = Coord3D(v0[0], v1[1], v1[2]);
4020 Coord3D P4 = Coord3D(v1[0], v0[1], v0[2]);
4021 Coord3D P5 = Coord3D(v1[0], v0[1], v1[2]);
4022 Coord3D P6 = Coord3D(v1[0], v1[1], v0[2]);
4023 Coord3D P7 = v1;
4024
4025 TPoint3D<Real> interpt;
4026 Coord3D seg[12][2] = {
4027 P0,P1,
4028 P0,P2,
4029 P0,P4,
4030 P7,P6,
4031 P7,P5,
4032 P7,P3,
4033 P2,P3,
4034 P2,P6,
4035 P3,P1,
4036 P6,P4,
4037 P4,P5,
4038 P1,P5
4039 };
4040
4041 Coord3D m_triangle_index[12][3] = {
4042 P0,P1,P2,
4043 P0,P1,P4,
4044 P0,P2,P4,
4045 P7,P5,P6,
4046 P7,P3,P5,
4047 P7,P3,P6,
4048 P2,P4,P6,
4049 P1,P4,P5,
4050 P1,P2,P3,
4051 P3,P5,P1,
4052 P5,P6,P4,
4053 P3,P6,P2
4054 };
4055
4056 for (int i = 0; i < 12; i++)
4057 {
4058 TTriangle3D<Real> t_tmp = TTriangle3D<Real>(m_triangle_index[i][0], m_triangle_index[i][1], m_triangle_index[i][2]);
4059 TSegment3D<Real> s_tmp = TSegment3D<Real>(seg[i][0], seg[i][1]);
4060 TSegment3D<Real> s0, s1, s2;
4061 s0 = TSegment3D<Real>(tri.v[0], tri.v[1]);
4062 s1 = TSegment3D<Real>(tri.v[1], tri.v[2]);
4063 s2 = TSegment3D<Real>(tri.v[2], tri.v[0]);
4064
4065 if ((s0.intersect(t_tmp, interpt)) || (s1.intersect(t_tmp, interpt)) || (s2.intersect(t_tmp, interpt)) || (s_tmp.intersect(tri, interpt)))
4066 return true;
4067 }
4068 //printf("!!!!!!!!!!!!!!!!!!FALSE CASE:\n");
4069
4070 /*
4071 \nAABB:\n%.3lf %.3lf %.3lf\nTRI:\n%.3lf %.3lf %.3lf\n%.3lf %.3lf %.3lf\n%.3lf %.3lf %.3lf\nPPPPPPPPPPPPPPP\n",
4072 v0[0], v0[1], v0[2],
4073 v1[0], v1[1], v1[2],
4074 tri.v[0][0], tri.v[0][1], tri.v[0][2],
4075 tri.v[1][0], tri.v[1][1], tri.v[1][2],
4076 tri.v[2][0], tri.v[2][1], tri.v[2][2]
4077 );*/
4078 return false;
4079 }
4080
4081 template<typename Real>
4082 DYN_FUNC TOrientedBox3D<Real> TAlignedBox3D<Real>::rotate(const Matrix3D& mat)
4083 {
4084 TOrientedBox3D<Real> obox;
4085 obox.u = mat * Coord3D(1, 0, 0);
4086 obox.v = mat * Coord3D(0, 1, 0);
4087 obox.w = mat * Coord3D(0, 0, 1);
4088 obox.center = 0.5f * (v0 + v1);
4089 obox.extent = 0.5f * (v1 - v0);
4090
4091 return obox;
4092 }
4093
4094
4095 template<typename Real>
4096 DYN_FUNC bool TAlignedBox3D<Real>::isValid()
4097 {
4098 return v1[0] > v0[0] && v1[1] > v0[1] && v1[2] > v0[2];
4099 }
4100
4101 template<typename Real>
4102 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D()
4103 {
4104 center = Coord3D(0);
4105 u = Coord3D(1, 0, 0);
4106 v = Coord3D(0, 1, 0);
4107 w = Coord3D(0, 0, 1);
4108
4109 extent = Coord3D(1);
4110 }
4111
4112 template<typename Real>
4113 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const Coord3D c, const Coord3D u_t, const Coord3D v_t, const Coord3D w_t, const Coord3D ext)
4114 {
4115 center = c;
4116 u = u_t;
4117 v = v_t;
4118 w = w_t;
4119 extent = ext;
4120 }
4121
4122 template<typename Real>
4123 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const Coord3D c, const Quat<Real> rot, const Coord3D ext)
4124 {
4125 center = c;
4126 extent = ext;
4127
4128 auto mat = rot.toMatrix3x3();
4129 u = mat.col(0);
4130 v = mat.col(1);
4131 w = mat.col(2);
4132 }
4133
4134 template<typename Real>
4135 DYN_FUNC TOrientedBox3D<Real>::TOrientedBox3D(const TOrientedBox3D& obb)
4136 {
4137 center = obb.center;
4138 u = obb.u;
4139 v = obb.v;
4140 w = obb.w;
4141 extent = obb.extent;
4142 }
4143
4144 template<typename Real>
4145 DYN_FUNC TPoint3D<Real> TOrientedBox3D<Real>::vertex(const int i) const
4146 {
4147 int id = i % 8;
4148 Coord3D hu = u * extent[0];
4149 Coord3D hv = v * extent[1];
4150 Coord3D hw = w * extent[2];
4151
4152 return TPoint3D<Real>(center + (2 * (id & 1) - 1) * hu + (2 * ((id >> 1) & 1) - 1) * hv + (2 * ((id >> 2) & 1) - 1) * hw);
4153 }
4154
4155 template<typename Real>
4156 DYN_FUNC TSegment3D<Real> TOrientedBox3D<Real>::edge(const int i) const
4157 {
4158 int id = i % 12;
4159 Vec2u table[12] = { Vec2u(0, 1), Vec2u(1, 3), Vec2u(2, 3), Vec2u(0, 2),
4160 Vec2u(0, 4), Vec2u(1, 5), Vec2u(2, 6), Vec2u(3, 7),
4161 Vec2u(4, 5), Vec2u(5, 7), Vec2u(6, 7), Vec2u(4, 6)};
4162 return vertex(table[id][0]) - vertex(table[id][1]);
4163 }
4164
4165 template<typename Real>
4166 DYN_FUNC TRectangle3D<Real> TOrientedBox3D<Real>::face(const int index) const
4167 {
4168 int id = index % 6;
4169 Coord3D c = center;
4170 Coord3D Nx, Ny;
4171 Coord2D Ext = Coord2D(0.f);
4172 switch (id)
4173 {
4174 case 0:
4175 c -= v * extent[1];
4176 Nx = u; Ny = w;
4177 Ext = Coord2D(extent[0], extent[2]);
4178 break;
4179 case 1:
4180 c += v * extent[1];
4181 Nx = w; Ny = u;
4182 Ext = Coord2D(extent[2], extent[0]);
4183 break;
4184 case 2:
4185 c -= u * extent[0];
4186 Nx = w; Ny = v;
4187 Ext = Coord2D(extent[2], extent[1]);
4188 break;
4189 case 3:
4190 c += u * extent[0];
4191 Nx = v; Ny = w;
4192 Ext = Coord2D(extent[1], extent[2]);
4193 break;
4194 case 4:
4195 c -= w * extent[2];
4196 Nx = v; Ny = u;
4197 Ext = Coord2D(extent[1], extent[0]);
4198 break;
4199 default:
4200 c += w * extent[2];
4201 Nx = u; Ny = v;
4202 Ext = Coord2D(extent[0], extent[1]);
4203 break;
4204 }
4205 return TRectangle3D<Real>(c, Nx, Ny, Ext);
4206 }
4207
4208
4209 template<typename Real>
4210 DYN_FUNC Real TOrientedBox3D<Real>::volume()
4211 {
4212 return 8 * extent[0] * extent[1] * extent[2];
4213 }
4214
4215 template<typename Real>
4216 DYN_FUNC bool TOrientedBox3D<Real>::isValid()
4217 {
4218 bool bValid = true;
4219 bValid &= extent[0] >= REAL_EPSILON;
4220 bValid &= extent[1] >= REAL_EPSILON;
4221 bValid &= extent[2] >= REAL_EPSILON;
4222
4223 bValid &= abs(u.normSquared() - Real(1)) < REAL_EPSILON;
4224 bValid &= abs(v.normSquared() - Real(1)) < REAL_EPSILON;
4225 bValid &= abs(w.normSquared() - Real(1)) < REAL_EPSILON;
4226
4227 bValid &= abs(u.dot(v)) < REAL_EPSILON;
4228 bValid &= abs(v.dot(w)) < REAL_EPSILON;
4229 bValid &= abs(w.dot(u)) < REAL_EPSILON;
4230
4231 return bValid;
4232 }
4233
4234
4235 template<typename Real>
4236 DYN_FUNC TOrientedBox3D<Real> TOrientedBox3D<Real>::rotate(const Matrix3D& mat)
4237 {
4238 TOrientedBox3D<Real> obox;
4239 obox.center = center;
4240 obox.extent = extent;
4241 obox.u = mat * obox.u;
4242 obox.v = mat * obox.v;
4243 obox.w = mat * obox.w;
4244
4245 return obox;
4246 }
4247
4248
4249 template<typename Real>
4250 DYN_FUNC TAlignedBox3D<Real> TOrientedBox3D<Real>::aabb()
4251 {
4252 TAlignedBox3D<Real> abox;
4253
4254 Coord3D r_ext;
4255 r_ext[0] = abs(extent[0] * u[0]) + abs(extent[1] * v[0]) + abs(extent[2] * w[0]);
4256 r_ext[1] = abs(extent[0] * u[1]) + abs(extent[1] * v[1]) + abs(extent[2] * w[1]);
4257 r_ext[2] = abs(extent[0] * u[2]) + abs(extent[1] * v[2]) + abs(extent[2] * w[2]);
4258
4259 abox.v0 = center - r_ext;
4260 abox.v1 = center + r_ext;
4261
4262 return abox;
4263 }
4264
4265
4266
4267 template<typename Real>
4268 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TOrientedBox3D<Real>& OBB, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4269 {
4270 TPoint3D<Real> p[8];
4271 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4272 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4273 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4274 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4275 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4276 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4277 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4278 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4279
4280 TSegment3D<Real> s[12];
4281 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);
4282 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);
4283 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);
4284 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);
4285
4286 int min_idx1 = -1;
4287 int min_idx2 = -1;
4288 Real distance = 1.0f;
4289
4290 TPlane3D<Real> plane1, plane2;
4291 Coord3D axis;
4292 Real inter;
4293
4294 //check u axis
4295 Real min_u, max_u;
4296 for (int i = 0; i < 8; i++)
4297 {
4298 Coord3D offset_self = p[i].origin - OBB.center;
4299 Real tmp = offset_self.dot(OBB.u);
4300 if (i == 0) min_u = max_u = tmp;
4301 else
4302 {
4303 min_u = minimum(min_u, tmp);
4304 max_u = maximum(max_u, tmp);
4305 }
4306 }
4307 if (max_u < -OBB.extent[0] || min_u > OBB.extent[0]) return false;
4308 Real inter_u = minimum(minimum(max_u + OBB.extent[0], OBB.extent[0] - min_u), minimum(2 * OBB.extent[0], max_u - min_u));
4309 axis = OBB.u;
4310 inter = inter_u;
4311 plane1 = TPlane3D<Real>((OBB.center + OBB.u * OBB.extent[0]), OBB.u);
4312 plane2 = TPlane3D<Real>((OBB.center - OBB.u * OBB.extent[0]), -OBB.u);
4313
4314 //check v axis
4315 Real min_v, max_v;
4316 for (int i = 0; i < 8; i++)
4317 {
4318 Coord3D offset_self = p[i].origin - OBB.center;
4319 Real tmp = offset_self.dot(OBB.v);
4320 if (i == 0) min_v = max_v = tmp;
4321 else
4322 {
4323 min_v = minimum(min_v, tmp);
4324 max_v = maximum(max_v, tmp);
4325 }
4326 }
4327 if (max_v < -OBB.extent[1] || min_v > OBB.extent[1]) return false;
4328 Real inter_v = minimum(minimum(max_v + OBB.extent[1], OBB.extent[1] - min_v), minimum(2 * OBB.extent[1], max_v - min_v));
4329 if (inter_v < inter)
4330 {
4331 axis = OBB.v;
4332 inter = inter_v;
4333 plane1 = TPlane3D<Real>((OBB.center + OBB.v * OBB.extent[1]), OBB.v);
4334 plane2 = TPlane3D<Real>((OBB.center - OBB.v * OBB.extent[1]), -OBB.v);
4335 }
4336
4337 //check w axis
4338 Real min_w, max_w;
4339 for (int i = 0; i < 8; i++)
4340 {
4341 Coord3D offset_self = p[i].origin - OBB.center;
4342 Real tmp = offset_self.dot(OBB.w);
4343 if (i == 0) min_w = max_w = tmp;
4344 else
4345 {
4346 min_w = minimum(min_w, tmp);
4347 max_w = maximum(max_w, tmp);
4348 }
4349 }
4350 if (max_w < -OBB.extent[2] || min_w > OBB.extent[2]) { return false; }
4351 Real inter_w = minimum(minimum(max_w + OBB.extent[2], OBB.extent[2] - min_w), minimum(2 * OBB.extent[2], max_w - min_w));
4352 if (inter_w < inter)
4353 {
4354 axis = OBB.w;
4355 inter = inter_w;
4356 plane1 = TPlane3D<Real>((OBB.center + OBB.w * OBB.extent[2]), OBB.w);
4357 plane2 = TPlane3D<Real>((OBB.center - OBB.w * OBB.extent[2]), -OBB.w);
4358 }
4359
4360 //printf("$$$$$$$$$$$$$$$$$$$$ INSIDE CHECKPOINT\n");
4361
4362 for (int i = 0; i < 8; i++)
4363 {
4364 Real tmp = -minimum(abs(p[i].distance(plane1)), abs(p[i].distance(plane2)));
4365 if (tmp < distance && p[i].distance(OBB) < EPSILON)
4366 {
4367 distance = tmp;
4368 min_idx1 = i;
4369 }
4370 }
4371
4372 for (int i = 0; i < 12; i++)
4373 {
4374 TSegment3D<Real> tmp;
4375 if (s[i].intersect(OBB, tmp))
4376 {
4377 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4378 Real tmp_distance = -minimum(abs(inp.distance(plane1)), abs(inp.distance(plane2)));
4379 if (tmp_distance < distance)
4380 {
4381
4382 //printf(" ---------------------------------- interSegStart: %.3lf %.3lf %.3lf interSegEnd: %.3lf %.3lf %.3lf \n",
4383 // tmp.startPoint()[0], tmp.startPoint()[1], tmp.startPoint()[2],
4384 // tmp.endPoint()[0], tmp.endPoint()[1], tmp.endPoint()[2]);
4385 distance = tmp_distance;
4386 min_idx2 = i;
4387 min_idx1 = -1;
4388 }
4389
4390 }
4391 }
4392 //printf("distance = %.10lf\n", distance);
4393 if (distance > EPSILON) return false;
4394 interDist = distance;
4395
4396 TPoint3D<Real> pt;
4397 if (min_idx1 != -1)
4398 pt = p[min_idx1];
4399 else
4400 {
4401 TSegment3D<Real> tmp;
4402 s[min_idx2].intersect(OBB, tmp);
4403 pt = TPoint3D<Real>((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4404 }
4405
4406
4407 p1 = pt.origin;
4408
4409 if (abs(pt.distance(plane1)) < abs(pt.distance(plane2)))
4410 {
4411 p2 = pt.project(plane1).origin;
4412 interNorm = axis;
4413 }
4414 else
4415 {
4416 p2 = pt.project(plane2).origin;
4417 interNorm = -axis;
4418 }
4419
4420 return true;
4421 }
4422
4423 template<typename Real>
4424 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TTet3D<Real>& TET, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4425 {
4426 TPoint3D<Real> p[8];
4427 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4428 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4429 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4430 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4431 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4432 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4433 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4434 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4435
4436 TSegment3D<Real> s[12];
4437 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);
4438 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);
4439 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);
4440 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);
4441
4442 TSegment3D<Real> s_tet[18];
4443 s_tet[0] = TSegment3D<Real>(TET.v[0], TET.v[1]);
4444 s_tet[1] = TSegment3D<Real>(TET.v[0], TET.v[2]);
4445 s_tet[2] = TSegment3D<Real>(TET.v[0], TET.v[3]);
4446 s_tet[3] = TSegment3D<Real>(TET.v[1], TET.v[2]);
4447 s_tet[4] = TSegment3D<Real>(TET.v[1], TET.v[3]);
4448 s_tet[5] = TSegment3D<Real>(TET.v[2], TET.v[3]);
4449
4450 s_tet[6] = TSegment3D<Real>(TET.v[0], (TET.v[1] + TET.v[2]) / 2.0f);
4451 s_tet[7] = TSegment3D<Real>(TET.v[0], (TET.v[1] + TET.v[3]) / 2.0f);
4452 s_tet[8] = TSegment3D<Real>(TET.v[0], (TET.v[3] + TET.v[2]) / 2.0f);
4453 s_tet[9] = TSegment3D<Real>(TET.v[1], (TET.v[3] + TET.v[2]) / 2.0f);
4454 s_tet[10] = TSegment3D<Real>(TET.v[1], (TET.v[0] + TET.v[2]) / 2.0f);
4455 s_tet[11] = TSegment3D<Real>(TET.v[1], (TET.v[0] + TET.v[3]) / 2.0f);
4456
4457 s_tet[12] = TSegment3D<Real>(TET.v[2], (TET.v[1] + TET.v[0]) / 2.0f);
4458 s_tet[13] = TSegment3D<Real>(TET.v[2], (TET.v[1] + TET.v[3]) / 2.0f);
4459 s_tet[14] = TSegment3D<Real>(TET.v[2], (TET.v[0] + TET.v[3]) / 2.0f);
4460 s_tet[15] = TSegment3D<Real>(TET.v[3], (TET.v[1] + TET.v[2]) / 2.0f);
4461 s_tet[16] = TSegment3D<Real>(TET.v[3], (TET.v[0] + TET.v[2]) / 2.0f);
4462 s_tet[17] = TSegment3D<Real>(TET.v[3], (TET.v[1] + TET.v[0]) / 2.0f);
4463
4464
4465 interDist = 0.0f;
4466 //obb intersect tet
4467 for (int i = 0; i < 12; i++)
4468 {
4469 TLine3D<Real> l_i = TLine3D<Real>(s[i].v0, s[i].direction());
4470 TSegment3D<Real> tmp_seg;
4471
4472 if (l_i.intersect(TET, tmp_seg))
4473 {
4474
4475 Real left = (tmp_seg.v0 - s[i].v0).dot(s[i].direction().normalize());
4476 Real right = (tmp_seg.v1 - s[i].v0).dot(s[i].direction().normalize());
4477 if (right < left)
4478 {
4479 Real tmp = left;
4480 left = right;
4481 right = tmp;
4482 }
4483 Real maxx = (s[i].v1 - s[i].v0).dot(s[i].direction().normalize());
4484 if (right < 0 || left > maxx)
4485 continue;
4486 left = maximum(left, Real(0));
4487 right = minimum(right, maxx);
4488
4489 Bool tmp_bool;
4490
4491 Coord3D p11 = s[i].v0 + ((left + right) / 2.0f * s[i].direction().normalize());
4492 Coord3D p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;//
4493
4494
4495 if ((p11 - p22).norm() > abs(interDist))
4496 {
4497 interDist = -(p11 - p22).norm();
4498 p1 = p11;
4499 p2 = p22;
4500 }
4501 p11 = s[i].v0 + left * s[i].direction().normalize();
4502 p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;
4503 if ((p11 - p22).norm() > abs(interDist))
4504 {
4505 p1 = p11; p2 = p22;
4506 interDist = -(p1 - p2).norm();
4507 }
4508
4509 p11 = s[i].v0 + right * s[i].direction().normalize();
4510 p22 = TPoint3D<Real>(p11).project(TET, tmp_bool).origin;
4511 if ((p11 - p22).norm() > abs(interDist))
4512 {
4513 p1 = p11; p2 = p22;
4514 interDist = -(p1 - p2).norm();
4515 }
4516 //dir = -(p1 - p2) / interDist;
4517
4518 }
4519 }
4520 TOrientedBox3D<Real> OBB(center, u, v, w, extent);
4521 for (int i = 0; i < 18; i++)
4522 {
4523 TSegment3D<Real> tmp;
4524
4525 if (s_tet[i].intersect(OBB, tmp))
4526 {
4527 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4528 TPoint3D<Real> sp(tmp.startPoint());
4529 TPoint3D<Real> ep(tmp.endPoint());
4530 if (abs(inp.distance(OBB)) > abs(interDist))
4531 {
4532 p2 = inp.origin;
4533 p1 = inp.project(OBB).origin;
4534 interDist = -abs(inp.distance(OBB));
4535 }
4536 if (abs(sp.distance(OBB)) > abs(interDist))
4537 {
4538 p2 = sp.origin;
4539 p1 = sp.project(OBB).origin;
4540 interDist = -abs(sp.distance(OBB));
4541 }
4542 if (abs(ep.distance(OBB)) > abs(interDist))
4543 {
4544 p2 = ep.origin;
4545 p1 = ep.project(OBB).origin;
4546 interDist = -abs(ep.distance(OBB));
4547 }
4548 }
4549 }
4550 if (interDist > -EPSILON) return false;
4551 interNorm = (p1 - p2) / interDist;
4552 return true;
4553 }
4554
4555
4556 template<typename Real>
4557 DYN_FUNC bool TOrientedBox3D<Real>::point_intersect(const TTriangle3D<Real>& TRI, Coord3D& interNorm, Real& interDist, Coord3D& p1, Coord3D& p2) const
4558 {
4559
4560 TSegment3D<Real> s_tri[3];
4561 s_tri[0] = TSegment3D<Real>(TRI.v[0], TRI.v[1]);
4562 s_tri[1] = TSegment3D<Real>(TRI.v[0], TRI.v[2]);
4563 s_tri[2] = TSegment3D<Real>(TRI.v[2], TRI.v[1]);
4564
4565 interDist = 0.0f;
4566 //obb intersect tet
4567
4568 TPoint3D<Real> p[8];
4569 p[0] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] - w * extent[2]);
4570 p[1] = TPoint3D<Real>(center - u * extent[0] - v * extent[1] + w * extent[2]);
4571 p[2] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] - w * extent[2]);
4572 p[3] = TPoint3D<Real>(center - u * extent[0] + v * extent[1] + w * extent[2]);
4573 p[4] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] - w * extent[2]);
4574 p[5] = TPoint3D<Real>(center + u * extent[0] - v * extent[1] + w * extent[2]);
4575 p[6] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] - w * extent[2]);
4576 p[7] = TPoint3D<Real>(center + u * extent[0] + v * extent[1] + w * extent[2]);
4577
4578 TSegment3D<Real> s[12];
4579 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);
4580 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);
4581 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);
4582 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);
4583
4584
4585
4586 TOrientedBox3D<Real> OBB(center, u, v, w, extent);
4587 for (int i = 0; i < 3; i++)
4588 {
4589 TSegment3D<Real> tmp;
4590
4591 if (s_tri[i].intersect(OBB, tmp))
4592 {
4593 TPoint3D<Real> inp((tmp.startPoint() + tmp.endPoint()) / 2.0f);
4594 TPoint3D<Real> sp(tmp.startPoint());
4595 TPoint3D<Real> ep(tmp.endPoint());
4596 if (abs(inp.distance(OBB)) > abs(interDist))
4597 {
4598 p2 = inp.origin;
4599 p1 = inp.project(OBB).origin;
4600 interDist = -abs(inp.distance(OBB));
4601 }
4602 if (abs(sp.distance(OBB)) > abs(interDist))
4603 {
4604 p2 = sp.origin;
4605 p1 = sp.project(OBB).origin;
4606 interDist = -abs(sp.distance(OBB));
4607 }
4608 if (abs(ep.distance(OBB)) > abs(interDist))
4609 {
4610 p2 = ep.origin;
4611 p1 = ep.project(OBB).origin;
4612 interDist = -abs(ep.distance(OBB));
4613 }
4614 }
4615 }
4616 interNorm = (p1 - p2) / interDist;
4617 if (interDist < -EPSILON)
4618 return true;
4619
4620 for (int i = 0; i < 12; i++)
4621 {
4622 TPoint3D<Real> tmp_point;
4623 if (s[i].intersect(TRI, tmp_point))
4624 {
4625 TPoint3D<Real> tmp_p1;// = s[i].startPoint();
4626 TPoint3D<Real> tmp_p2;// = s[i].endPoint();
4627 if (tmp_point.distance(TPoint3D<Real>(s[i].startPoint())) < tmp_point.distance(TPoint3D<Real>(s[i].endPoint())))
4628 {
4629 tmp_p1 = s[i].endPoint();
4630 tmp_p2 = s[i].startPoint();
4631 }
4632 else
4633 {
4634 tmp_p1 = s[i].startPoint();
4635 tmp_p2 = s[i].endPoint();
4636 }
4637 if (abs(tmp_p2.distance(tmp_point)) > abs(interDist))
4638 {
4639 p2 = tmp_point.origin;
4640 p1 = tmp_p2.origin;
4641 interDist = -abs(tmp_p2.distance(tmp_point));
4642 }
4643 }
4644 }
4645
4646 interNorm = (p1 - p2) / interDist;
4647 if (interDist < -EPSILON)
4648 return true;
4649
4650 return false;
4651 }
4652
4653}
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::Interval::isEmpty
DYN_FUNC bool isEmpty() const
Definition Interval.inl:132
dyno::Interval::size
DYN_FUNC Real size() const
Definition Interval.inl:65
dyno::Interval::intersect
DYN_FUNC Interval< Real > intersect(const Interval< Real > &itv) const
Definition Interval.inl:119
dyno::Quat::toMatrix3x3
DYN_FUNC SquareMatrix< Real, 3 > toMatrix3x3() const
Definition Quat.inl:275
dyno::TAlignedBox3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:724
dyno::TAlignedBox3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:723
dyno::TAlignedBox3D::checkOverlap
DYN_FUNC bool checkOverlap(const TAlignedBox3D< Real > &abox) const
Definition Primitive3D.inl:3975
dyno::TAlignedBox3D::intersect
DYN_FUNC bool intersect(const TAlignedBox3D< Real > &abox, TAlignedBox3D< Real > &interBox) const
Definition Primitive3D.inl:3936
dyno::TAlignedBox3D::meshInsert
DYN_FUNC bool meshInsert(const TTriangle3D< Real > &tri) const
Definition Primitive3D.inl:4001
dyno::TAlignedBox3D::rotate
DYN_FUNC TOrientedBox3D< Real > rotate(const Matrix3D &mat)
Definition Primitive3D.inl:4082
dyno::TAlignedBox3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:4096
dyno::TAlignedBox3D::merge
DYN_FUNC TAlignedBox3D< Real > merge(const TAlignedBox3D< Real > &aabb) const
Definition Primitive3D.inl:3990
dyno::TCapsule3D::TCapsule3D
DYN_FUNC TCapsule3D()
Definition Primitive3D.inl:3323
dyno::TCapsule3D::volume
DYN_FUNC Real volume() const
Definition Primitive3D.inl:3361
dyno::TCapsule3D::centerline
DYN_FUNC TSegment3D< Real > centerline() const
Definition Primitive3D.h:664
dyno::TCapsule3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:647
dyno::TCapsule3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:667
dyno::TCapsule3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3368
dyno::TCapsule3D::endPoint
DYN_FUNC Coord3D endPoint() const
Definition Primitive3D.h:662
dyno::TCapsule3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb() const
Definition Primitive3D.inl:3374
dyno::TCapsule3D::startPoint
DYN_FUNC Coord3D startPoint() const
Definition Primitive3D.h:661
dyno::TCone3D::TCone3D
DYN_FUNC TCone3D()
Definition Primitive3D.inl:3278
dyno::TCone3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:638
dyno::TCone3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:624
dyno::TCylinder3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:604
dyno::TCylinder3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:617
dyno::TDisk3D::TDisk3D
DYN_FUNC TDisk3D()
Definition Primitive3D.inl:3162
dyno::TDisk3D::area
DYN_FUNC Real area()
Definition Primitive3D.inl:3187
dyno::TDisk3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:560
dyno::TDisk3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:3193
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:4210
dyno::TOrientedBox3D::Coord2D
Vector< Real, 2 > Coord2D
Definition Primitive3D.h:751
dyno::TOrientedBox3D::u
Coord3D u
three unit vectors u, v and w forming a right-handed orthornormal basis
Definition Primitive3D.h:803
dyno::TOrientedBox3D::vertex
DYN_FUNC TPoint3D< Real > vertex(const int i) const
Definition Primitive3D.inl:4145
dyno::TOrientedBox3D::edge
DYN_FUNC TSegment3D< Real > edge(const int i) const
Definition Primitive3D.inl:4156
dyno::TOrientedBox3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:752
dyno::TOrientedBox3D::face
DYN_FUNC TRectangle3D< Real > face(const int i) const
Definition Primitive3D.inl:4166
dyno::TOrientedBox3D::extent
Coord3D extent
half the dimension in each of the u, v, and w directions
Definition Primitive3D.h:808
dyno::TOrientedBox3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:4216
dyno::TOrientedBox3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:753
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:4268
dyno::TOrientedBox3D::center
Coord3D center
centerpoint
Definition Primitive3D.h:797
dyno::TOrientedBox3D::rotate
DYN_FUNC TOrientedBox3D< Real > rotate(const Matrix3D &mat)
Definition Primitive3D.inl:4236
dyno::TOrientedBox3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:4250
dyno::TPlane3D
2D geometric primitives in three-dimensional space
Definition Primitive3D.h:459
dyno::TPlane3D::TPlane3D
DYN_FUNC TPlane3D()
Definition Primitive3D.inl:2874
dyno::TPlane3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2895
dyno::TPlane3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:462
dyno::TPlane3D::normal
Coord3D normal
the plane will be treated as a single point if its normal is zero
Definition Primitive3D.h:476
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:551
dyno::TRectangle3D::Coord2D
Vector< Real, 2 > Coord2D
Definition Primitive3D.h:522
dyno::TRectangle3D::vertex
DYN_FUNC TPoint3D< Real > vertex(const int i) const
Definition Primitive3D.inl:3088
dyno::TRectangle3D::computeParams
DYN_FUNC bool computeParams(const Coord3D &p, Param &par) const
Definition Primitive3D.inl:3129
dyno::TRectangle3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:523
dyno::TRectangle3D::edge
DYN_FUNC TSegment3D< Real > edge(const int i) const
Definition Primitive3D.inl:3111
dyno::TRectangle3D::normal
DYN_FUNC Coord3D normal() const
Definition Primitive3D.inl:3123
dyno::TRectangle3D::area
DYN_FUNC Real area() const
Definition Primitive3D.inl:3117
dyno::TRectangle3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3147
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:442
dyno::TSegment3D::operator-
DYN_FUNC TSegment3D< Real > operator-(void) const
Definition Primitive3D.inl:2858
dyno::TSegment3D::parameter
DYN_FUNC Real parameter(const Coord3D &pos) const
Definition Primitive3D.inl:2848
dyno::TSegment3D::lengthSquared
DYN_FUNC Real lengthSquared() const
Definition Primitive3D.inl:2842
dyno::TSegment3D::direction
DYN_FUNC Coord3D direction() const
Definition Primitive3D.h:447
dyno::TSegment3D::proximity
DYN_FUNC TSegment3D< Real > proximity(const TSegment3D< Real > &segment) const
return a segment pointing from the input segment to the other primitive
Definition Primitive3D.inl:2374
dyno::TSegment3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2868
dyno::TSegment3D::length
DYN_FUNC Real length() const
Definition Primitive3D.inl:2836
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:441
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:578
dyno::TSphere3D::volume
DYN_FUNC Real volume()
Definition Primitive3D.inl:3231
dyno::TSphere3D::isValid
DYN_FUNC bool isValid()
Definition Primitive3D.inl:3237
dyno::TSphere3D::Coord3D
Vector< Real, 3 > Coord3D
Definition Primitive3D.h:581
dyno::TSphere3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:3312
dyno::TSphere3D::TSphere3D
DYN_FUNC TSphere3D()
Definition Primitive3D.inl:3199
dyno::TSphere3D::rotation
Quat< Real > rotation
Definition Primitive3D.h:596
dyno::TTet3D
vertices are ordered so that the normal vectors for the triangular faces point outwards 3 / | \ 0 - 2...
Definition Primitive3D.h:680
dyno::TTet3D::circumcenter
DYN_FUNC TPoint3D< Real > circumcenter() const
Definition Primitive3D.inl:3836
dyno::TTet3D::contain
DYN_FUNC bool contain(Coord3D p)
Definition Primitive3D.inl:3895
dyno::TTet3D::barycenter
DYN_FUNC TPoint3D< Real > barycenter() const
Definition Primitive3D.inl:3889
dyno::TTet3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:683
dyno::TTet3D::v
Coord3D v[4]
Definition Primitive3D.h:715
dyno::TTet3D::Matrix3D
SquareMatrix< Real, 3 > Matrix3D
Definition Primitive3D.h:684
dyno::TTet3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:3517
dyno::TTet3D::edge
DYN_FUNC TSegment3D< Real > edge(const int index) const
Definition Primitive3D.inl:3412
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:3539
dyno::TTet3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:3821
dyno::TTet3D::TTet3D
DYN_FUNC TTet3D()
Definition Primitive3D.inl:3385
dyno::TTet3D::computeBarycentricCoordinates
DYN_FUNC Vector< Real, 4 > computeBarycentricCoordinates(const Coord3D &p)
Definition Primitive3D.inl:3901
dyno::TTet3D::expand
DYN_FUNC void expand(Real r)
Definition Primitive3D.inl:3524
dyno::TTet3D::face
DYN_FUNC TTriangle3D< Real > face(const int index) const
Definition Primitive3D.inl:3440
dyno::TTet3D::solidAngle
DYN_FUNC Real solidAngle(const int index) const
Definition Primitive3D.inl:3466
dyno::TTet3D::volume
DYN_FUNC Real volume() const
Definition Primitive3D.inl:3506
dyno::TTriangle3D::distance
DYN_FUNC Real distance(const TTriangle3D< Real > &triangle) const
Definition Primitive3D.inl:3055
dyno::TTriangle3D::aabb
DYN_FUNC TAlignedBox3D< Real > aabb()
Definition Primitive3D.inl:2997
dyno::TTriangle3D::computeLocation
DYN_FUNC Coord3D computeLocation(const Param &bary) const
Definition Primitive3D.inl:2975
dyno::TTriangle3D< double >::Coord3D
Vector< double, 3 > Coord3D
Definition Primitive3D.h:485
dyno::TTriangle3D::isValid
DYN_FUNC bool isValid() const
Definition Primitive3D.inl:2990
dyno::TTriangle3D::maximumEdgeLength
DYN_FUNC Real maximumEdgeLength() const
Definition Primitive3D.inl:2984
dyno::TTriangle3D::computeBarycentrics
DYN_FUNC bool computeBarycentrics(const Coord3D &p, Param &bary) const
Definition Primitive3D.inl:2943
dyno::TTriangle3D::area
DYN_FUNC Real area() const
Definition Primitive3D.inl:2925
dyno::TTriangle3D::distanceSquared
DYN_FUNC Real distanceSquared(const TTriangle3D< Real > &triangle) const
Definition Primitive3D.inl:3008
dyno::TTriangle3D::normal
DYN_FUNC Coord3D normal() const
Definition Primitive3D.inl:2931
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:320
M
#define M(X, Y)
Definition vgMath.h:291
Generated by doxygen 1.13.2