BALL 1.5.0
Loading...
Searching...
No Matches
analyticalGeometry.h
Go to the documentation of this file.
1// -*- Mode: C++; tab-width: 2; -*-
2// vi: set ts=2:
3//
4
5#ifndef BALL_MATHS_ANALYTICALGEOMETRY_H
6#define BALL_MATHS_ANALYTICALGEOMETRY_H
7
8#ifndef BALL_COMMON_EXCEPTION_H
10#endif
11
12#ifndef BALL_MATHS_ANGLE_H
13# include <BALL/MATHS/angle.h>
14#endif
15
16#ifndef BALL_MATHS_CIRCLE3_H
17# include <BALL/MATHS/circle3.h>
18#endif
19
20#ifndef BALL_MATHS_LINE3_H
21# include <BALL/MATHS/line3.h>
22#endif
23
24#ifndef BALL_MATHS_PLANE3_H
25# include <BALL/MATHS/plane3.h>
26#endif
27
28#ifndef BALL_MATHS_SPHERE3_H
29# include <BALL/MATHS/sphere3.h>
30#endif
31
32#ifndef BALL_MATHS_VECTOR3_H
33# include <BALL/MATHS/vector3.h>
34#endif
35
36#define BALL_MATRIX_CELL(m, dim, row, col) *((m) + (row) * (dim) + (col))
37#define BALL_CELL(x, y) *((m) + (y) * (dim) + (x))
38
39namespace BALL
40{
41
48
55 template <typename T>
57 T getDeterminant_(const T* m, Size dim)
58 {
59 T determinant = 0;
60 Index dim1 = dim - 1;
61
62 if (dim > 1)
63 {
64 T* submatrix = new T[dim1 * dim1];
65
66 for (Index i = 0; i < (Index)dim; ++i)
67 {
68 for (Index j = 0; j < dim1; ++j)
69 {
70 for (Index k = 0; k < dim1; ++k)
71 {
72 *(submatrix + j * dim1 + k) = *(m + (j + 1) * dim + (k < i ? k : k + 1));
73 }
74 }
75 determinant += *(m + i) * (i / 2.0 == i / 2 ? 1 : -1) * getDeterminant_(submatrix, dim1);
76 }
77
78 delete [] submatrix;
79 }
80 else
81 {
82 determinant = *m;
83 }
84
85 return determinant;
86 }
87
92 template <typename T>
93 T getDeterminant(const T* m, Size dim)
94 {
95 if (dim == 2)
96 {
97 return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0));
98 }
99 else if (dim == 3)
100 {
101 return ( BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2)
102 + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0)
103 + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1)
104 - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0)
105 - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1)
106 - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2));
107 }
108 else
109 {
110 return getDeterminant_(m, dim);
111 }
112 }
113
117 template <typename T>
119 T getDeterminant2(const T* m)
120 {
121 Size dim = 2;
122 return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0));
123 }
124
131 template <typename T>
133 T getDeterminant2(const T& m00, const T& m01, const T& m10, const T& m11)
134 {
135 return (m00 * m11 - m01 * m10);
136 }
137
141 template <typename T>
143 T getDeterminant3(const T *m)
144 {
145 Size dim = 3;
146 return ( BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2)
147 + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0)
148 + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1)
149 - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0)
150 - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1)
151 - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2));
152 }
153
157 template <typename T>
158 BALL_INLINE T
159 getDeterminant3(const T& m00, const T& m01, const T& m02, const T& m10, const T& m11, const T& m12, const T& m20, const T& m21, const T& m22)
160 {
161 return ( m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m02 * m11 * m20 - m00 * m12 * m21 - m01 * m10 * m22);
162 }
163
190 template <typename T>
191 bool SolveSystem(const T* m, T* x, const Size dim)
192 {
193 T pivot;
194 Index i, j, k, p;
195 // the column dimension of the matrix
196 const Size col_dim = dim + 1;
197 T* matrix = new T[dim * (dim + 1)];
198 const T* source = m;
199 T* target = (T*)matrix;
200 T* end = (T*)&BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim);
201
202 while (target <= end)
203 {
204 *target++ = *source++;
205 }
206
207 for (i = 0; i < (Index)dim; ++i)
208 {
209 pivot = BALL_MATRIX_CELL(matrix, col_dim, i, i);
210 p = i;
211 for (j = i + 1; j < (Index)dim; ++j)
212 {
213 if (Maths::isLess(pivot, BALL_MATRIX_CELL(matrix, col_dim, j, i)))
214 {
215 pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i);
216 p = j;
217 }
218 }
219
220 if (p != i)
221 {
222 T tmp;
223
224 for (k = i; k < (Index)dim + 1; ++k)
225 {
226 tmp = BALL_MATRIX_CELL(matrix, dim, i, k);
227 BALL_MATRIX_CELL(matrix, col_dim, i, k) = BALL_MATRIX_CELL(matrix, col_dim, p, k);
228 BALL_MATRIX_CELL(matrix, col_dim, p, k) = tmp;
229 }
230 }
231 else if (Maths::isZero(pivot) || Maths::isNan(pivot))
232 {
233 // invariant: matrix m is singular
234 delete [] matrix;
235
236 return false;
237 }
238
239 for (j = dim; j >= i; --j)
240 {
241 BALL_MATRIX_CELL(matrix, col_dim, i, j) /= pivot;
242 }
243
244 for (j = i + 1; j < (Index)dim; ++j)
245 {
246 pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i);
247
248 for (k = dim; k>= i; --k)
249 {
250 BALL_MATRIX_CELL(matrix, col_dim, j, k) -= pivot * BALL_MATRIX_CELL(matrix, col_dim, i, k);
251 }
252 }
253 }
254
255 x[dim - 1] = BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim);
256
257 for (i = dim - 2; i >= 0; --i)
258 {
259 x[i] = BALL_MATRIX_CELL(matrix, col_dim, i, dim);
260
261 for (j = i + 1; j < (Index)dim; ++j)
262 {
263 x[i] -= BALL_MATRIX_CELL(matrix, col_dim, i, j) * x[j];
264 }
265 }
266
267 delete [] matrix;
268
269 return true;
270 }
271
272#undef BALL_CELL
273#undef BALL_MATRIX_CELL
274
283 template <typename T>
285 bool SolveSystem2(const T& a1, const T& b1, const T& c1, const T& a2, const T& b2, const T& c2, T& x1, T& x2)
286 {
287 T quot = (a1 * b2 - a2 * b1);
288
289 if (Maths::isZero(quot))
290 {
291 return false;
292 }
293
294 x1 = (c1 * b2 - c2 * b1) / quot;
295 x2 = (a1 * c2 - a2 * c1) / quot;
296
297 return true;
298 }
299
309 template <typename T>
310 short SolveQuadraticEquation(const T& a, const T& b, const T &c, T &x1, T &x2)
311 {
312 if (a == 0)
313 {
314 if (b == 0)
315 {
316 return 0;
317 }
318 x1 = x2 = c / b;
319 return 1;
320 }
321 T discriminant = b * b - 4 * a * c;
322
323 if (Maths::isLess(discriminant, 0))
324 {
325 return 0;
326 }
327
328 T sqrt_discriminant = sqrt(discriminant);
329 if (Maths::isZero(sqrt_discriminant))
330 {
331 x1 = x2 = -b / (2 * a);
332
333 return 1;
334 }
335 else
336 {
337 x1 = (-b + sqrt_discriminant) / (2 * a);
338 x2 = (-b - sqrt_discriminant) / (2 * a);
339
340 return 2;
341 }
342 }
343
349 template <typename T>
352 {
353 return TVector3<T>((b.x + a.x) / 2, (b.y + a.y) / 2, (b.z + a.z) / 2);
354 }
355
365 template <typename T>
367 TVector3<T> GetPartition(const TVector3<T>& a, const TVector3<T>& b, const T& r, const T& s)
368 {
369 T sum = r + s;
370 if (sum == (T)0)
371 {
372 throw Exception::DivisionByZero(__FILE__, __LINE__);
373 }
374 return TVector3<T>
375 ((s * a.x + r * b.x) / sum,
376 (s * a.y + r * b.y) / sum,
377 (s * a.z + r * b.z) / sum);
378 }
379
385 template <typename T>
387 T GetDistance(const TVector3<T>& a, const TVector3<T>& b)
388 {
389 T dx = a.x - b.x;
390 T dy = a.y - b.y;
391 T dz = a.z - b.z;
392
393 return sqrt(dx * dx + dy * dy + dz * dz);
394 }
395
402 template <typename T>
404 T GetDistance(const TLine3<T>& line, const TVector3<T>& point)
405 {
406 if (line.d.getLength() == (T)0)
407 {
408 throw Exception::DivisionByZero(__FILE__, __LINE__);
409 }
410 return ((line.d % (point - line.p)).getLength() / line.d.getLength());
411 }
412
419 template <typename T>
421 T GetDistance(const TVector3<T>& point, const TLine3<T>& line)
422 {
423 return GetDistance(line, point);
424 }
425
432 template <typename T>
433 T GetDistance(const TLine3<T>& a, const TLine3<T>& b)
434 {
435 T cross_product_length = (a.d % b.d).getLength();
436
437 if (Maths::isZero(cross_product_length))
438 { // invariant: parallel lines
439 if (a.d.getLength() == (T)0)
440 {
441 throw Exception::DivisionByZero(__FILE__, __LINE__);
442 }
443 return ((a.d % (b.p - a.p)).getLength() / a.d.getLength());
444 }
445 else
446 {
447 T spat_product = TVector3<T>::getTripleProduct(a.d, b.d, b.p - a.p);
448
449 if (Maths::isNotZero(spat_product))
450 { // invariant: windschiefe lines
451
452 return (Maths::abs(spat_product) / cross_product_length);
453 }
454 else
455 {
456 // invariant: intersecting lines
457 return 0;
458 }
459 }
460 }
461
468 template <typename T>
470 T GetDistance(const TVector3<T>& point, const TPlane3<T>& plane)
471 {
472 T length = plane.n.getLength();
473
474 if (length == (T)0)
475 {
476 throw Exception::DivisionByZero(__FILE__, __LINE__);
477 }
478 return (Maths::abs(plane.n * (point - plane.p)) / length);
479 }
480
487 template <typename T>
489 T GetDistance(const TPlane3<T>& plane, const TVector3<T>& point)
490 {
491 return GetDistance(point, plane);
492 }
493
500 template <typename T>
502 T GetDistance(const TLine3<T>& line, const TPlane3<T>& plane)
503 {
504 T length = plane.n.getLength();
505 if (length == (T)0)
506 {
507 throw Exception::DivisionByZero(__FILE__, __LINE__);
508 }
509 return (Maths::abs(plane.n * (line.p - plane.p)) / length);
510 }
511
518 template <typename T>
520 T GetDistance(const TPlane3<T>& plane, const TLine3<T>& line)
521 {
522 return GetDistance(line, plane);
523 }
524
531 template <typename T>
533 T GetDistance(const TPlane3<T>& a, const TPlane3<T>& b)
534 {
535 T length = a.n.getLength();
536 if (length == (T)0)
537 {
538 throw Exception::DivisionByZero(__FILE__, __LINE__);
539 }
540 return (Maths::abs(a.n * (a.p - b.p)) / length);
541 }
542
549 template <typename T>
551 bool GetAngle(const TVector3<T>& a, const TVector3<T>& b, TAngle<T> &intersection_angle)
552 {
553 T length_product = a.getSquareLength() * b.getSquareLength();
554 if(Maths::isZero(length_product))
555 {
556 return false;
557 }
558 intersection_angle = a.getAngle(b);
559 return true;
560 }
561
568 template <typename T>
570 bool GetAngle(const TLine3<T>& a, const TLine3<T>& b, TAngle<T>& intersection_angle)
571 {
572 T length_product = a.d.getSquareLength() * b.d.getSquareLength();
573
574 if(Maths::isZero(length_product))
575 {
576 return false;
577 }
578 intersection_angle = acos(Maths::abs(a.d * b.d) / sqrt(length_product));
579 return true;
580 }
581
588 template <typename T>
590 bool GetAngle(const TPlane3<T>& plane, const TVector3<T>& vector, TAngle<T>& intersection_angle)
591 {
592 T length_product = plane.n.getSquareLength() * vector.getSquareLength();
593
594 if (Maths::isZero(length_product))
595 {
596 return false;
597 }
598 else
599 {
600 intersection_angle = asin(Maths::abs(plane.n * vector) / sqrt(length_product));
601 return true;
602 }
603 }
604
611 template <typename T>
613 bool GetAngle(const TVector3<T>& vector ,const TPlane3<T>& plane, TAngle<T> &intersection_angle)
614 {
615 return GetAngle(plane, vector, intersection_angle);
616 }
617
624 template <typename T>
626 bool GetAngle(const TPlane3<T>& plane,const TLine3<T>& line, TAngle<T>& intersection_angle)
627 {
628 T length_product = plane.n.getSquareLength() * line.d.getSquareLength();
629
630 if (Maths::isZero(length_product))
631 {
632 return false;
633 }
634
635 intersection_angle = asin(Maths::abs(plane.n * line.d) / sqrt(length_product));
636 return true;
637 }
638
645 template <typename T>
647 bool GetAngle(const TLine3<T>& line, const TPlane3<T>& plane, TAngle<T>& intersection_angle)
648 {
649 return GetAngle(plane, line, intersection_angle);
650 }
651
652
659 template <typename T>
661 bool GetAngle(const TPlane3<T>& a, const TPlane3<T>& b, TAngle<T>& intersection_angle)
662 {
663 T length_product = a.n.getSquareLength() * b.n.getSquareLength();
664
665 if(Maths::isZero(length_product))
666 {
667 return false;
668 }
669
670 intersection_angle = acos(Maths::abs(a.n * b.n) / sqrt(length_product));
671 return true;
672 }
673
680 template <typename T>
681 bool GetIntersection(const TLine3<T>& a, const TLine3<T>& b, TVector3<T>& point)
682 {
683 T c1, c2;
684 if ((SolveSystem2(a.d.x, -b.d.x, b.p.x - a.p.x, a.d.y, -b.d.y, b.p.y - a.p.y, c1, c2) == true && Maths::isEqual(a.p.z + a.d.z * c1, b.p.z + b.d.z * c2)) || (SolveSystem2(a.d.x, -b.d.x, b.p.x - a.p.x, a.d.z, -b.d.z, b.p.z - a.p.z, c1, c2) == true && Maths::isEqual(a.p.y + a.d.y * c1, b.p.y + b.d.y * c2)) || (SolveSystem2(a.d.y, -b.d.y, b.p.y - a.p.y, a.d.z, -b.d.z, b.p.z - a.p.z, c1, c2) == true && Maths::isEqual(a.p.x + a.d.x * c1, b.p.x + b.d.x * c2)))
685 {
686 point.set(a.p.x + a.d.x * c1, a.p.y + a.d.y * c1, a.p.z + a.d.z * c1);
687 return true;
688 }
689
690 return false;
691 }
692
699 template <typename T>
701 bool GetIntersection(const TPlane3<T>& plane, const TLine3<T>& line, TVector3<T>& intersection_point)
702 {
703 T dot_product = plane.n * line.d;
704 if (Maths::isZero(dot_product))
705 {
706 return false;
707 }
708 intersection_point.set(line.p + (plane.n * (plane.p - line.p)) * line.d / dot_product);
709 return true;
710 }
711
718 template <typename T>
720 bool GetIntersection(const TLine3<T>& line, const TPlane3<T>& plane, TVector3<T>& intersection_point)
721 {
722 return GetIntersection(plane, line, intersection_point);
723 }
724
731 template <typename T>
732 bool GetIntersection(const TPlane3<T>& plane1, const TPlane3<T>& plane2, TLine3<T>& line)
733 {
734 T u = plane1.p*plane1.n;
735 T v = plane2.p*plane2.n;
736 T det = plane1.n.x*plane2.n.y-plane1.n.y*plane2.n.x;
737 if (Maths::isZero(det))
738 {
739 det = plane1.n.x*plane2.n.z-plane1.n.z*plane2.n.x;
740 if (Maths::isZero(det))
741 {
742 det = plane1.n.y*plane2.n.z-plane1.n.z*plane2.n.y;
743 if (Maths::isZero(det))
744 {
745 return false;
746 }
747 else
748 {
749 T a = plane2.n.z/det;
750 T b = -plane1.n.z/det;
751 T c = -plane2.n.y/det;
752 T d = plane1.n.y/det;
753 line.p.x = 0;
754 line.p.y = a*u+b*v;
755 line.p.z = c*u+d*v;
756 line.d.x = -1;
757 line.d.y = a*plane1.n.x+b*plane2.n.x;
758 line.d.z = c*plane1.n.x+d*plane2.n.x;
759 }
760 }
761 else
762 {
763 T a = plane2.n.z/det;
764 T b = -plane1.n.z/det;
765 T c = -plane2.n.x/det;
766 T d = plane1.n.x/det;
767 line.p.x = a*u+b*v;
768 line.p.y = 0;
769 line.p.z = c*u+d*v;
770 line.d.x = a*plane1.n.y+b*plane2.n.y;
771 line.d.y = -1;
772 line.d.z = c*plane1.n.y+d*plane2.n.y;
773 }
774 }
775 else
776 {
777 T a = plane2.n.y/det;
778 T b = -plane1.n.y/det;
779 T c = -plane2.n.x/det;
780 T d = plane1.n.x/det;
781 line.p.x = a*u+b*v;
782 line.p.y = c*u+d*v;
783 line.p.z = 0;
784 line.d.x = a*plane1.n.z+b*plane2.n.z;
785 line.d.y = c*plane1.n.z+d*plane2.n.z;
786 line.d.z = -1;
787 }
788 return true;
789 }
790
798 template <typename T>
799 bool GetIntersection(const TSphere3<T>& sphere, const TLine3<T>& line, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2)
800 {
801 T x1, x2;
802 short number_of_solutions = SolveQuadraticEquation (line.d * line.d, (line.p - sphere.p) * line.d * 2, (line.p - sphere.p) * (line.p - sphere.p) - sphere.radius * sphere.radius, x1, x2);
803
804 if (number_of_solutions == 0)
805 {
806 return false;
807 }
808
809 intersection_point1 = line.p + x1 * line.d;
810 intersection_point2 = line.p + x2 * line.d;
811
812 return true;
813 }
814
822 template <typename T>
824 bool GetIntersection(const TLine3<T>& line, const TSphere3<T>& sphere, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2)
825 {
826 return GetIntersection(sphere, line, intersection_point1, intersection_point2);
827 }
828
835 template <typename T>
836 bool GetIntersection(const TSphere3<T>& sphere, const TPlane3<T>& plane, TCircle3<T>& intersection_circle)
837 {
838 T distance = GetDistance(sphere.p, plane);
839
840 if (Maths::isGreater(distance, sphere.radius))
841 {
842 return false;
843 }
844
845 TVector3<T> Vector3(plane.n);
847
848 if (Maths::isEqual(distance, sphere.radius))
849 {
850 intersection_circle.set(sphere.p + sphere.radius * Vector3, plane.n, 0);
851 }
852 else
853 {
854 intersection_circle.set
855 (sphere.p + distance * Vector3, plane.n,
856 sqrt(sphere.radius * sphere.radius - distance * distance));
857 }
858
859 return true;
860 }
861
868 template <typename T>
869 BALL_INLINE bool
870 GetIntersection(const TPlane3<T>& plane, const TSphere3<T>& sphere, TCircle3<T>& intersection_circle)
871 {
872 return GetIntersection(sphere, plane, intersection_circle);
873 }
874
883 template <typename T>
884 bool GetIntersection(const TSphere3<T>& a, const TSphere3<T>& b, TCircle3<T>& intersection_circle)
885 {
886 TVector3<T> norm = b.p - a.p;
887 T square_dist = norm * norm;
888 if (Maths::isZero(square_dist))
889 {
890 return false;
891 }
892 T dist = sqrt(square_dist);
893 if (Maths::isLess(a.radius + b.radius, dist))
894 {
895 return false;
896 }
898 {
899 return false;
900 }
901
902 T radius1_square = a.radius * a.radius;
903 T radius2_square = b.radius * b.radius;
904 T u = radius1_square - radius2_square + square_dist;
905 T length = u / (2 * square_dist);
906 T square_radius = radius1_square - u * length / 2;
907 if (square_radius < 0)
908 {
909 return false;
910 }
911
912 intersection_circle.p = a.p + (norm * length);
913 intersection_circle.radius = sqrt(square_radius);
914 intersection_circle.n = norm / dist;
915
916 return true;
917 }
918
928 template <class T>
929 bool GetIntersection(const TSphere3<T>& s1, const TSphere3<T>& s2, const TSphere3<T>& s3, TVector3<T>& p1, TVector3<T>& p2, bool test = true)
930 {
931 T r1_square = s1.radius*s1.radius;
932 T r2_square = s2.radius*s2.radius;
933 T r3_square = s3.radius*s3.radius;
934 T p1_square_length = s1.p*s1.p;
935 T p2_square_length = s2.p*s2.p;
936 T p3_square_length = s3.p*s3.p;
937 T u = (r2_square-r1_square-p2_square_length+p1_square_length)/2;
938 T v = (r3_square-r1_square-p3_square_length+p1_square_length)/2;
939 TPlane3<T> plane1;
940 TPlane3<T> plane2;
941 try
942 {
943 plane1 = TPlane3<T>(s2.p.x-s1.p.x,s2.p.y-s1.p.y,s2.p.z-s1.p.z,u);
944 plane2 = TPlane3<T>(s3.p.x-s1.p.x,s3.p.y-s1.p.y,s3.p.z-s1.p.z,v);
945 }
947 {
948 return false;
949 }
950 TLine3<T> line;
951 if (GetIntersection(plane1,plane2,line))
952 {
953 TVector3<T> diff(s1.p-line.p);
954 T x1 = 0;
955 T x2 = 0;
956 if (SolveQuadraticEquation(line.d*line.d, -diff*line.d*2, diff*diff-r1_square, x1,x2) > 0)
957 {
958 p1 = line.p+x1*line.d;
959 p2 = line.p+x2*line.d;
960 if (test)
961 {
962 TVector3<T> test = s1.p-p1;
963 if (Maths::isNotEqual(test*test,r1_square))
964 {
965 return false;
966 }
967 test = s1.p-p2;
968 if (Maths::isNotEqual(test*test,r1_square))
969 {
970 return false;
971 }
972 test = s2.p-p1;
973 if (Maths::isNotEqual(test*test,r2_square))
974 {
975 return false;
976 }
977 test = s2.p-p2;
978 if (Maths::isNotEqual(test*test,r2_square))
979 {
980 return false;
981 }
982 test = s3.p-p1;
983 if (Maths::isNotEqual(test*test,r3_square))
984 {
985 return false;
986 }
987 test = s3.p-p2;
988 if (Maths::isNotEqual(test*test,r3_square))
989 {
990 return false;
991 }
992 }
993 return true;
994 }
995 }
996 return false;
997 }
998
999
1005 template <typename T>
1007 bool isCollinear(const TVector3<T>& a, const TVector3<T>& b)
1008 {
1009 return (a % b).isZero();
1010 }
1011
1018 template <typename T>
1020 bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c)
1021 {
1023 }
1024
1032 template <typename T>
1034 bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c, const TVector3<T>& d)
1035 {
1036 return isComplanar(a - b, a - c, a - d);
1037 }
1038
1044 template <typename T>
1046 bool isOrthogonal(const TVector3<T>& a, const TVector3<T>& b)
1047 {
1048 return Maths::isZero(a * b);
1049 }
1050
1056 template <typename T>
1058 bool isOrthogonal(const TVector3<T>& vector, const TLine3<T>& line)
1059 {
1060 return Maths::isZero(vector * line.d);
1061 }
1062
1068 template <typename T>
1070 bool isOrthogonal(const TLine3<T>& line, const TVector3<T>& vector)
1071 {
1072 return isOrthogonal(vector, line);
1073 }
1074
1080 template <typename T>
1082 bool isOrthogonal(const TLine3<T>& a, const TLine3<T>& b)
1083 {
1084 return Maths::isZero(a.d * b.d);
1085 }
1086
1092 template <typename T>
1094 bool isOrthogonal(const TVector3<T>& vector, const TPlane3<T>& plane)
1095 {
1096 return isCollinear(vector, plane.n);
1097 }
1098
1104 template <typename T>
1106 bool isOrthogonal(const TPlane3<T>& plane, const TVector3<T>& vector)
1107 {
1108 return isOrthogonal(vector, plane);
1109 }
1110
1116 template <typename T>
1118 bool isOrthogonal(const TPlane3<T>& a, const TPlane3<T>& b)
1119 {
1120 return Maths::isZero(a.n * b.n);
1121 }
1122
1128 template <typename T>
1130 bool isIntersecting(const TVector3<T>& point, const TLine3<T>& line)
1131 {
1132 return Maths::isZero(GetDistance(point, line));
1133 }
1134
1140 template <typename T>
1142 bool isIntersecting(const TLine3<T>& line, const TVector3<T>& point)
1143 {
1144 return isIntersecting(point, line);
1145 }
1146
1152 template <typename T>
1154 bool isIntersecting(const TLine3<T>& a, const TLine3<T>& b)
1155 {
1156 return Maths::isZero(GetDistance(a, b));
1157 }
1158
1164 template <typename T>
1166 bool isIntersecting(const TVector3<T>& point, const TPlane3<T>& plane)
1167 {
1168 return Maths::isZero(GetDistance(point, plane));
1169 }
1170
1176 template <typename T>
1178 bool isIntersecting(const TPlane3<T>& plane, const TVector3<T>& point)
1179 {
1180 return isIntersecting(point, plane);
1181 }
1182
1188 template <typename T>
1190 bool isIntersecting(const TLine3<T>& line, const TPlane3<T>& plane)
1191 {
1192 return Maths::isZero(GetDistance(line, plane));
1193 }
1194
1200 template <typename T>
1202 bool isIntersecting(const TPlane3<T>& plane, const TLine3<T>& line)
1203 {
1204 return isIntersecting(line, plane);
1205 }
1206
1212 template <typename T>
1214 bool isIntersecting(const TPlane3<T>& a, const TPlane3<T>& b)
1215 {
1216 return Maths::isZero(GetDistance(a, b));
1217 }
1218
1224 template <typename T>
1226 bool isParallel(const TLine3<T>& line, const TPlane3<T>& plane)
1227 {
1228 return isOrthogonal(line.d, plane.n);
1229 }
1230
1236 template <typename T>
1238 bool isParallel(const TPlane3<T>& plane, const TLine3<T>& line)
1239 {
1240 return isParallel(line, plane);
1241 }
1242
1248 template <typename T>
1250 bool isParallel(const TPlane3<T>& a, const TPlane3<T>& b)
1251 {
1252 return isCollinear(a.n, b.n);
1253 }
1254
1258 template <typename T>
1260 (const T& ax, const T& ay, const T& az,
1261 const T& bx, const T& by, const T& bz,
1262 const T& nx, const T& ny, const T& nz)
1263 {
1264 // Calculate the length of the two normals
1265 T bl = (T) sqrt((double)ax * ax + ay * ay + az * az);
1266 T el = (T) sqrt((double)bx * bx + by * by + bz * bz);
1267 T bel = (T) (ax * bx + ay * by + az * bz);
1268
1269 // if one or both planes are degenerated
1270 if (bl * el == 0)
1271 {
1272 throw Exception::DivisionByZero(__FILE__, __LINE__);
1273 }
1274 bel /= (bl * el);
1275 if (bel > 1.0)
1276 {
1277 bel = 1;
1278 }
1279 else if (bel < -1.0)
1280 {
1281 bel = -1;
1282 }
1283
1284 T acosbel = (T) acos(bel); // >= 0
1285
1286 if (( nx * (az * by - ay * bz)
1287 + ny * (ax * bz - az * bx)
1288 + nz * (ay * bx - ax * by)) > 0)
1289 {
1290 acosbel = Constants::PI+Constants::PI-acosbel;
1291 }
1292
1293 return TAngle<T>(acosbel);
1294 }
1295
1299 template <typename T>
1302 {
1303 return getOrientedAngle(a.x, a.y, a.z, b.x, b.y, b.z, normal.x, normal.y, normal.z);
1304 }
1305
1322 template <typename T>
1324 (const T& ax, const T& ay, const T& az,
1325 const T& bx, const T& by, const T& bz,
1326 const T& cx, const T& cy, const T& cz,
1327 const T& dx, const T& dy, const T& dz)
1328 {
1329 T abx = ax - bx;
1330 T aby = ay - by;
1331 T abz = az - bz;
1332
1333 T cbx = cx - bx;
1334 T cby = cy - by;
1335 T cbz = cz - bz;
1336
1337 T cdx = cx - dx;
1338 T cdy = cy - dy;
1339 T cdz = cz - dz;
1340
1341 // Calculate the normals to the two planes n1 and n2
1342 // this is given as the cross products:
1343 // AB x BC
1344 // --------- = n1
1345 // |AB x BC|
1346 //
1347 // BC x CD
1348 // --------- = n2
1349 // |BC x CD|
1350
1351 // Normal to plane 1
1352 T ndax = aby * cbz - abz * cby;
1353 T nday = abz * cbx - abx * cbz;
1354 T ndaz = abx * cby - aby * cbx;
1355
1356 // Normal to plane 2
1357 T neax = cbz * cdy - cby * cdz;
1358 T neay = cbx * cdz - cbz * cdx;
1359 T neaz = cby * cdx - cbx * cdy;
1360
1361 // Calculate the length of the two normals
1362 T bl = (T) sqrt((double)ndax * ndax + nday * nday + ndaz * ndaz);
1363 T el = (T) sqrt((double)neax * neax + neay * neay + neaz * neaz);
1364 T bel = (T) (ndax * neax + nday * neay + ndaz * neaz);
1365
1366 // if one or both planes are degenerated
1367 if (bl * el == 0)
1368 {
1369 throw Exception::DivisionByZero(__FILE__, __LINE__);
1370 }
1371 bel /= (bl * el);
1372 if (bel > 1.0)
1373 {
1374 bel = 1;
1375 }
1376 else if (bel < -1.0)
1377 {
1378 bel = -1;
1379 }
1380
1381 T acosbel = (T) acos(bel);
1382
1383 if ((cbx * (ndaz * neay - nday * neaz)
1384 + cby * (ndax * neaz - ndaz * neax)
1385 + cbz * (nday * neax - ndax * neay))
1386 < 0)
1387 {
1388 acosbel = -acosbel;
1389 }
1390
1391 acosbel = (acosbel > 0.0)
1392 ? Constants::PI - acosbel
1393 : -(Constants::PI + acosbel);
1394
1395 return TAngle<T>(acosbel);
1396 }
1398} // namespace BALL
1399
1400
1401#endif // BALL_MATHS_ANALYTICALGEOMETRY_H
#define BALL_INLINE
Definition debug.h:15
#define BALL_MATRIX_CELL(m, dim, row, col)
#define BALL_CELL(x, y)
BALL_INLINE bool SolveSystem2(const T &a1, const T &b1, const T &c1, const T &a2, const T &b2, const T &c2, T &x1, T &x2)
BALL_INLINE T getDeterminant_(const T *m, Size dim)
BALL_INLINE bool GetAngle(const TVector3< T > &a, const TVector3< T > &b, TAngle< T > &intersection_angle)
BALL_INLINE bool isCollinear(const TVector3< T > &a, const TVector3< T > &b)
BALL_INLINE T GetDistance(const TVector3< T > &a, const TVector3< T > &b)
short SolveQuadraticEquation(const T &a, const T &b, const T &c, T &x1, T &x2)
BALL_INLINE bool isOrthogonal(const TVector3< T > &a, const TVector3< T > &b)
BALL_INLINE TVector3< T > GetPartition(const TVector3< T > &a, const TVector3< T > &b)
TAngle< T > getOrientedAngle(const T &ax, const T &ay, const T &az, const T &bx, const T &by, const T &bz, const T &nx, const T &ny, const T &nz)
bool GetIntersection(const TLine3< T > &a, const TLine3< T > &b, TVector3< T > &point)
T getDeterminant(const T *m, Size dim)
TVector3< float > Vector3
Definition vector3.h:1084
TAngle< T > getTorsionAngle(const T &ax, const T &ay, const T &az, const T &bx, const T &by, const T &bz, const T &cx, const T &cy, const T &cz, const T &dx, const T &dy, const T &dz)
BALL_INLINE T getDeterminant3(const T *m)
BALL_INLINE bool isComplanar(const TVector3< T > &a, const TVector3< T > &b, const TVector3< T > &c)
BALL_INLINE T getDeterminant2(const T *m)
BALL_INLINE bool isIntersecting(const TVector3< T > &point, const TLine3< T > &line)
bool SolveSystem(const T *m, T *x, const Size dim)
BALL_INLINE bool isParallel(const TLine3< T > &line, const TPlane3< T > &plane)
BALL_INDEX_TYPE Index
BALL_EXTERN_VARIABLE const double PI
PI.
Definition constants.h:35
bool isNan(const T &t)
bool isNotZero(const T &t)
bool isNotEqual(const T1 &a, const T2 &b)
bool isGreater(const T1 &a, const T2 &b)
bool isGreaterOrEqual(const T1 &a, const T2 &b)
bool isLess(const T1 &a, const T2 &b)
bool isZero(const T &t)
T abs(const T &t)
bool isEqual(const T1 &a, const T2 &b)
TVector3< T > p
Definition circle3.h:277
TVector3< T > n
Definition circle3.h:282
void set(const TCircle3 &circle)
Definition circle3.h:134
TVector3< T > p
Definition line3.h:347
TVector3< T > d
Definition line3.h:351
TVector3< T > p
Definition plane3.h:366
TVector3< T > n
Definition plane3.h:370
TVector3< T > p
Definition sphere3.h:250
TAngle< T > getAngle(const TVector3 &vector) const
Definition vector3.h:971
T getSquareLength() const
Definition vector3.h:764
static T getTripleProduct(const TVector3< T > &a, const TVector3< T > &b, const TVector3< T > &c)
Definition vector3.h:1016
TVector3 & normalize()
Definition vector3.h:770
void set(const T *ptr)
Definition vector3.h:615