49 #include "clipper_util.h"
50 #include "clipper_memory.h"
68 inline String(
const std::string str ) : std::string( str ) {}
69 inline String(
const char* str ) : std::string( str ) {}
70 String(
const char* str,
const int l );
71 String(
const int i,
const int w = 4 );
72 String(
const long i,
const int w = 4 );
73 String(
const float f,
const int w = 6,
const int p = 6 );
76 String(
const double f,
const int w = 6,
const int p = 6 );
93 static String rational(
const double f,
const int b,
const bool sign=
false );
105 template<
class T = ftype>
class Vec3
111 inline Vec3(
const T& v0,
const T& v1,
const T& v2 )
112 { vec[0] = v0; vec[1] = v1; vec[2] = v2; }
115 { vec[0] = TT(v[0]); vec[1] = TT(v[1]); vec[2] = TT(v[2]); }
119 inline const T&
operator [](
const int& i )
const {
return vec[i]; }
124 {
return (*
this)*T(1.0/sqrt(
ftype(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]))); }
133 {
return (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]); }
136 {
return Vec3<T>(v1[1]*v2[2]-v2[1]*v1[2],
137 v1[2]*v2[0]-v2[2]*v1[0],
138 v1[0]*v2[1]-v2[0]*v1[1]); }
141 {
return "("+
String(vec[0],10,4)+
","+
String(vec[1],10,4)+
","+
String(vec[2],10,4)+
")"; }
144 { vec[0] += v[0]; vec[1] += v[1]; vec[2] += v[2];
return (*
this); }
147 { vec[0] -= v[0]; vec[1] -= v[1]; vec[2] -= v[2];
return (*
this); }
167 template<
class T>
inline int operator == (
const Vec3<T>& v1,
const Vec3<T>& v2 ) {
return (v1[0]==v2[0] && v1[1]==v2[1] && v1[2]==v2[2]); }
168 template<
class T>
inline int operator != (
const Vec3<T>& v1,
const Vec3<T>& v2 ) {
return (v1[0]!=v2[0] || v1[1]!=v2[1] || v1[2]!=v2[2]); }
170 {
return Vec3<T>( -v[0], -v[1], -v[2] ); }
174 {
return Vec3<T>( s*v1[0], s*v1[1], s*v1[2]); }
176 {
return Vec3<T>( s*v1[0], s*v1[1], s*v1[2]); }
182 template<
class T = ftype>
class Mat33
188 inline Mat33(
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 )
189 { mat[0][0] = m00; mat[0][1] = m01; mat[0][2] = m02; mat[1][0] = m10; mat[1][1] = m11; mat[1][2] = m12; mat[2][0] = m20; mat[2][1] = m21; mat[2][2] = m22; }
199 {
return mat[i][j]; }
201 {
return mat[i][j]; }
204 {
return "|"+
String(mat[0][0],10,4)+
","+
String(mat[0][1],10,4)+
","+
String(mat[0][2],10,4)+
"|\n|"+
String(mat[1][0],10,4)+
","+
String(mat[1][1],10,4)+
","+
String(mat[1][2],10,4)+
"|\n|"+
String(mat[2][0],10,4)+
","+
String(mat[2][1],10,4)+
","+
String(mat[2][2],10,4)+
"|"; }
206 inline static Mat33<T> identity() {
Mat33<T> m; m.mat[0][0] = m.mat[1][1] = m.mat[2][2] = 1; m.mat[0][1] = m.mat[0][2] = m.mat[1][0] = m.mat[1][2] = m.mat[2][0] = m.mat[2][1] = 0;
return m; }
228 {
return Vec3<T>( m(0,0)*v[0]+m(0,1)*v[1]+m(0,2)*v[2],
229 m(1,0)*v[0]+m(1,1)*v[1]+m(1,2)*v[2],
230 m(2,0)*v[0]+m(2,1)*v[1]+m(2,2)*v[2] ); }
232 {
return Vec3<T>( v[0]*m(0,0)+v[1]*m(1,0)+v[2]*m(2,0),
233 v[0]*m(0,1)+v[1]*m(1,1)+v[2]*m(2,1),
234 v[0]*m(0,2)+v[1]*m(1,2)+v[2]*m(2,2) ); }
237 return Mat33<T> ( m1(0,0)*m2(0,0) + m1(0,1)*m2(1,0) + m1(0,2)*m2(2,0),
238 m1(0,0)*m2(0,1) + m1(0,1)*m2(1,1) + m1(0,2)*m2(2,1),
239 m1(0,0)*m2(0,2) + m1(0,1)*m2(1,2) + m1(0,2)*m2(2,2),
240 m1(1,0)*m2(0,0) + m1(1,1)*m2(1,0) + m1(1,2)*m2(2,0),
241 m1(1,0)*m2(0,1) + m1(1,1)*m2(1,1) + m1(1,2)*m2(2,1),
242 m1(1,0)*m2(0,2) + m1(1,1)*m2(1,2) + m1(1,2)*m2(2,2),
243 m1(2,0)*m2(0,0) + m1(2,1)*m2(1,0) + m1(2,2)*m2(2,0),
244 m1(2,0)*m2(0,1) + m1(2,1)*m2(1,1) + m1(2,2)*m2(2,1),
245 m1(2,0)*m2(0,2) + m1(2,1)*m2(1,2) + m1(2,2)*m2(2,2) );
248 {
return Mat33<T>( m1(0,0)+m2(0,0), m1(0,1)+m2(0,1), m1(0,2)+m2(0,2),
249 m1(1,0)+m2(1,0), m1(1,1)+m2(1,1), m1(1,2)+m2(1,2),
250 m1(2,0)+m2(2,0), m1(2,1)+m2(2,1), m1(2,2)+m2(2,2) ); }
252 {
return Mat33<T>( -m(0,0), -m(0,1), -m(0,2),
253 -m(1,0), -m(1,1), -m(1,2),
254 -m(2,0), -m(2,1), -m(2,2) ); }
258 template<
class T = ftype>
class Mat33sym
265 m00(m(0,0)), m11(m(1,1)), m22(m(2,2)),
266 m01(m(0,1)), m02(m(0,2)), m12(m(1,2)) {}
272 inline Mat33sym(
const T& c00,
const T& c11,
const T& c22,
273 const T& c01,
const T& c02,
const T& c12 ) :
274 m00(c00), m11(c11), m22(c22), m01(c01), m02(c02), m12(c12) {}
277 {
return "|"+
String(m00,10,4)+
","+
String(m01,10,4)+
","+
String(m02,10,4)+
"|\n|"+
String(m01,10,4)+
","+
String(m11,10,4)+
","+
String(m12,10,4)+
"|\n|"+
String(m02,10,4)+
","+
String(m12,10,4)+
","+
String(m22,10,4)+
"|"; }
291 inline const T&
mat00()
const {
return m00; }
292 inline const T&
mat11()
const {
return m11; }
293 inline const T&
mat22()
const {
return m22; }
294 inline const T&
mat01()
const {
return m01; }
295 inline const T&
mat02()
const {
return m02; }
296 inline const T&
mat12()
const {
return m12; }
297 const T&
operator ()(
const int& i,
const int& j )
const;
306 T m00, m11, m22, m01, m02, m12;
322 template<
class T = ftype>
class RTop
348 inline bool is_null()
const {
return rot_.is_null() || trn_.is_null(); }
372 inline Array2d(
const int& d1,
const int& d2, T val )
373 {
resize( d1, d2, val ); }
375 void inline resize(
const int& d1,
const int& d2 )
376 { d1_ = d1; d2_ = d2; data.resize(
size() ); }
378 void inline resize(
const int& d1,
const int& d2,
const T& val )
379 { d1_ = d1; d2_ = d2; data.resize(
size(), val ); }
380 inline int size()
const {
return d1_ * d2_; }
381 inline const int&
rows()
const {
return d1_; }
382 inline const int&
cols()
const {
return d2_; }
383 inline const T&
operator () (
const int& i1,
const int& i2 )
const
385 {
return data[ i1*d2_ + i2 ]; }
388 {
return data[ i1*d2_ + i2 ]; }
402 inline Matrix(
const int& d1,
const int& d2 )
405 inline Matrix(
const int& d1,
const int& d2, T val )
408 std::vector<T>
solve(
const std::vector<T>& b )
const;
410 std::vector<T>
eigen(
const bool sort =
true );
415 template<
class T> std::vector<T> operator *(
const Matrix<T>& m,
const std::vector<T>& v )
417 if ( m.
cols() != v.size() )
419 std::vector<T> r( m.
rows() );
421 for ( i = 0; i < m.
rows(); i++ ) {
423 for ( j = 0; j < m.
cols(); j++ ) s += m(i,j) * v[j];
435 return ( pow(vec[0]-v[0],T(2)) + pow(vec[1]-v[1],T(2)) +
436 pow(vec[2]-v[2],T(2)) <= pow(tol,T(2)) );
441 mat[0][0]=T(m(0,0)); mat[0][1]=T(m(0,1)); mat[0][2]=T(m(0,2));
442 mat[1][0]=T(m(1,0)); mat[1][1]=T(m(1,1)); mat[1][2]=T(m(1,2));
443 mat[2][0]=T(m(2,0)); mat[2][1]=T(m(2,1)); mat[2][2]=T(m(2,2));
448 mat[0][0]=T(m.
mat00());
449 mat[1][1]=T(m.
mat11());
450 mat[2][2]=T(m.
mat22());
451 mat[0][1]=mat[1][0]=T(m.
mat01());
452 mat[0][2]=mat[2][0]=T(m.
mat02());
453 mat[1][2]=mat[2][1]=T(m.
mat12());
458 return ( pow(mat[0][0]-m(0,0),T(2)) + pow(mat[0][1]-m(0,1),T(2)) +
459 pow(mat[0][2]-m(0,2),T(2)) + pow(mat[1][0]-m(1,0),T(2)) +
460 pow(mat[1][1]-m(1,1),T(2)) + pow(mat[1][2]-m(1,2),T(2)) +
461 pow(mat[2][0]-m(2,0),T(2)) + pow(mat[2][1]-m(2,1),T(2)) +
462 pow(mat[2][2]-m(2,2),T(2)) <= pow(tol,T(2)) );
467 return ( mat[0][0]*(mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1]) +
468 mat[0][1]*(mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2]) +
469 mat[0][2]*(mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0]) );
476 inv(0,0) = ( mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1] ) / d;
477 inv(1,0) = ( mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2] ) / d;
478 inv(2,0) = ( mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0] ) / d;
479 inv(0,1) = ( mat[2][1]*mat[0][2] - mat[2][2]*mat[0][1] ) / d;
480 inv(1,1) = ( mat[2][2]*mat[0][0] - mat[2][0]*mat[0][2] ) / d;
481 inv(2,1) = ( mat[2][0]*mat[0][1] - mat[2][1]*mat[0][0] ) / d;
482 inv(0,2) = ( mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1] ) / d;
483 inv(1,2) = ( mat[0][2]*mat[1][0] - mat[0][0]*mat[1][2] ) / d;
484 inv(2,2) = ( mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0] ) / d;
491 t(0,0) = mat[0][0]; t(0,1) = mat[1][0]; t(0,2) = mat[2][0];
492 t(1,0) = mat[0][1]; t(1,1) = mat[1][1]; t(1,2) = mat[2][1];
493 t(2,0) = mat[0][2]; t(2,1) = mat[1][2]; t(2,2) = mat[2][2];
499 return ( m00*(m11*m22 - m12*m12) + m01*(m12*m02 - m01*m22) +
500 m02*(m01*m12 - m11*m02) );
508 result(1,0) = result(2,0) = result(2,1) = 0.0;
509 for (
int i = 0; i < 10; i++ )
510 result = half * ( result.
inverse() * target + result );
518 ( m22*m00 - m02*m02 ) / d,
519 ( m00*m11 - m01*m01 ) / d,
520 ( m12*m02 - m22*m01 ) / d,
521 ( m01*m12 - m02*m11 ) / d,
522 ( m02*m01 - m00*m12 ) / d );
527 return ( v[0]*( v[0]*m00 + 2*(v[1]*m01+v[2]*m02) ) +
528 v[1]*( v[1]*m11 + 2*(v[2]*m12) ) + v[2]*v[2]*m22 );
574 std::vector<T> x = b;
575 for ( i = 0; i < n; i++ ) {
578 for ( k = i+1; k < n; k++ )
579 if ( fabs(a(k,i)) > fabs(a(j,i)) ) j = k;
581 for ( k = 0; k < n; k++ )
586 for ( j = 0; j < n; j++ ) {
589 for ( k = i+1; k < n; k++ ) a(j,k) = a(j,k) - s*a(i,k);
590 x[j] = x[j] - s*x[i];
594 for ( i = 0; i < n; i++ ) x[i] /= a(i,i);
611 T spp, spq, t, s, c, theta, tau, h, ap, aq, a_pq;
615 std::vector<T> eval( n );
616 std::vector<T> b( n );
617 std::vector<T> z( n );
620 for( p = 0; p < n; p++ ) {
622 eval[p] = b[p] = mat(p,p);
625 for ( cyc = 1; cyc <= 50; cyc++ ) {
629 for ( p=0; p<n-1; p++ ) {
630 for ( q=p+1; q<n; q++ )
631 spq += fabs( mat(p,q) );
632 spp += fabs( mat(p,p) );
634 if ( spq <= 1.0e-12 * spp )
break;
637 for ( p = 0; p < n; p++ ) z[p] = 0.0;
640 for( p=0; p<n-1; p++ ) {
641 for( q=p+1; q<n; q++ ) {
643 h = eval[q] - eval[p];
644 if ( fabs(a_pq) > 1.0e-12*fabs(h) ) {
646 t = 1.0/(fabs(theta) + sqrt(1.0 + theta*theta));
647 if ( theta < 0.0 ) t = -t;
653 c = 1.0/sqrt(1.0+t*t);
666 for ( j = 0; j < p; j++ ) {
669 mat( j, p ) = ap - s * ( aq + ap * tau );
670 mat( j, q ) = aq + s * ( ap - aq * tau );
672 for ( j = p+1; j < q; j++ ) {
675 mat( p, j ) = ap - s * ( aq + ap * tau );
676 mat( j, q ) = aq + s * ( ap - aq * tau );
678 for ( j = q+1; j < n; j++ ) {
681 mat( p, j ) = ap - s * ( aq + ap * tau );
682 mat( q, j ) = aq + s * ( ap - aq * tau );
685 for ( j = 0; j < n; j++ ) {
688 evec( j, p ) = ap - s * ( aq + ap * tau );
689 evec( j, q ) = aq + s * ( ap - aq * tau );
694 for ( p = 0; p < n; p++ ) {
702 for ( p = 0; p < n; p++ ) {
704 for ( q = p+1; q < n; q++ )
705 if ( eval[q] < eval[j] ) j = q;
707 for ( q = 0; q < n; q++ )