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++ )
Array2d(const int &d1, const int &d2)
constructor
Definition: clipper_types.h:370
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)
constructor
Definition: clipper_types.h:188
ftype rational() const
convert from rational to ftype
Definition: clipper_types.cpp:145
const T & mat01() const
element (0,1)
Definition: clipper_types.h:294
String trim() const
Return copy of string without leading and trailing blanks.
Definition: clipper_types.cpp:86
const T & mat00() const
element (0,0)
Definition: clipper_types.h:291
Mat33< T > sqrt() const
square root
Definition: clipper_types.h:503
static Vec3< T > zero()
return zero vector
Definition: clipper_types.h:126
Fatal message (level = 9)
Definition: clipper_message.h:129
T det() const
determinant
Definition: clipper_types.h:465
std::vector< T > solve(const std::vector< T > &b) const
equation solver (square matrices only)
Definition: clipper_types.h:561
int size() const
size
Definition: clipper_types.h:380
Simple 2-d array class.
Definition: clipper_types.h:364
static Vec3< T > cross(const Vec3< T > &v1, const Vec3< T > &v2)
Vector cross product.
Definition: clipper_types.h:135
static Mat33< T > identity()
return identity matrix
Definition: clipper_types.h:206
bool is_null() const
test for null operator
Definition: clipper_types.h:348
ftype f() const
convert to ftype
Definition: clipper_types.cpp:142
String(const char *str)
constructor: from char*
Definition: clipper_types.h:69
RTop(const Mat33< T > &r, const Vec3< T > &t)
constructor: from rotation and translation
Definition: clipper_types.h:330
Mat33sym(const Mat33sym< TT > &m)
constructor: from Mat33sym
Definition: clipper_types.h:268
String head() const
remove trailing path element
Definition: clipper_types.cpp:100
static RTop< T > identity()
return identity operator
Definition: clipper_types.h:342
static Vec3< T > null()
return null vector (only valid for floating point types)
Definition: clipper_types.h:128
Compressed form for 3x3 symmetric matrix class.
Definition: clipper_types.h:56
static Mat33sym< T > null()
return null matrix (only valid for floating point types)
Definition: clipper_types.h:282
bool equals(const Mat33< T > &m, const T &tol) const
test equality
Definition: clipper_types.h:456
bool is_null() const
test for null matrix (only valid for floating point types)
Definition: clipper_types.h:285
const T & mat22() const
element (2,2)
Definition: clipper_types.h:293
T quad_form(const Vec3< T > &v) const
return quadratic form with vector
Definition: clipper_types.h:525
Vec3< T > unit() const
return unit vector with same direction as this vector
Definition: clipper_types.h:123
const T & mat11() const
element (1,1)
Definition: clipper_types.h:292
const Vec3< T > & operator-=(const Vec3< T > &v)
subtract another vector from this one
Definition: clipper_types.h:146
const Vec3< T > & trn() const
get translation
Definition: clipper_types.h:338
static const ftype & nan()
fast Util::nan() value
Definition: clipper_util.h:67
void resize(const int &d1, const int &d2, const T &val)
resize
Definition: clipper_types.h:378
Vec3(const T &v0, const T &v1, const T &v2)
constructor: from individual values
Definition: clipper_types.h:111
String nohead() const
get leading path element
Definition: clipper_types.cpp:103
long l() const
convert to long
Definition: clipper_types.cpp:133
String format() const
return formatted String representation
Definition: clipper_types.h:276
String format() const
return formatted String representation
Definition: clipper_types.h:203
void resize(const int &d1, const int &d2)
resize
Definition: clipper_types.h:375
ftype64 ftype
ftype definition for floating point representation
Definition: clipper_precision.h:58
Mat33sym(const T &c00, const T &c11, const T &c22, const T &c01, const T &c02, const T &c12)
constructor: from coefficients
Definition: clipper_types.h:272
const T & operator()(const int &i1, const int &i2) const
read accessor
Definition: clipper_types.h:384
static Mat33sym< T > identity()
return identity matrix
Definition: clipper_types.h:279
Vec3(const Vec3< TT > &v)
constructor: copy/convert
Definition: clipper_types.h:114
static void swap(T &a, T &b)
swap the contents of two objects
Definition: clipper_util.h:148
Mat33< T > inverse() const
inverse
Definition: clipper_types.h:472
Mat33sym< T > inverse() const
inverse
Definition: clipper_types.h:514
static T dot(const Vec3< T > &v1, const Vec3< T > &v2)
Vector dot product (equivalent to *)
Definition: clipper_types.h:132
const Vec3< T > & operator+=(const Vec3< T > &v)
add another vector to this one
Definition: clipper_types.h:143
Matrix()
null constructor
Definition: clipper_types.h:400
bool is_null() const
test for null vector
Definition: clipper_types.h:130
String notail() const
remove leading path element
Definition: clipper_types.cpp:109
RTop< T > inverse() const
inverse
Definition: clipper_types.h:332
std::vector< String > split(const String sep) const
String splitter - a very simple parser component.
Definition: clipper_types.cpp:71
const T & mat02() const
element (0,2)
Definition: clipper_types.h:295
const int & cols() const
number of cols
Definition: clipper_types.h:382
String extension with simple parsing methods.
Definition: clipper_types.h:64
String tail() const
get trailing path element
Definition: clipper_types.cpp:97
ftype64 f64() const
convert to double
Definition: clipper_types.cpp:139
3-vector class
Definition: clipper_types.h:105
static bool is_nan(const ftype32 f)
fast Util::nan() test
Definition: clipper_util.h:82
Mat33()
null constructor
Definition: clipper_types.h:186
Mat33< T > transpose() const
transpose
Definition: clipper_types.h:488
bool is_null() const
test for null matrix (only valid for floating point types)
Definition: clipper_types.h:210
static Mat33< T > null()
return null matrix (only valid for floating point types)
Definition: clipper_types.h:208
Rotation-translation operator.
Definition: clipper_types.h:322
static void message(const T &message)
pass a message
Definition: clipper_message.h:93
const T & operator()(const int &i, const int &j) const
access elements as 3x3 matrix (inefficient)
Definition: clipper_types.h:531
std::vector< T > eigen(const bool sort=true)
eigenvalue calculation (square symmetric matrices only)
Definition: clipper_types.h:604
Matrix(const int &d1, const int &d2)
constructor
Definition: clipper_types.h:402
ftype32 f32() const
convert to float
Definition: clipper_types.cpp:136
T det() const
determinant
Definition: clipper_types.h:497
String()
null constructor
Definition: clipper_types.h:67
Mat33< T > & rot()
set rotation
Definition: clipper_types.h:339
const Mat33< T > & rot() const
get rotation
Definition: clipper_types.h:337
Matrix(const int &d1, const int &d2, T val)
constructor
Definition: clipper_types.h:405
RTop()
null constructor
Definition: clipper_types.h:326
Array2d(const int &d1, const int &d2, T val)
constructor
Definition: clipper_types.h:372
String format() const
return formatted String representation
Definition: clipper_types.h:140
RTop(const Mat33< T > &r)
constructor: from rotation
Definition: clipper_types.h:328
const T & mat12() const
element (1,2)
Definition: clipper_types.h:296
Mat33sym(const Mat33< TT > &m)
constructor: from Mat33 (does not check for symmetry)
Definition: clipper_types.h:264
bool equals(const RTop< T > &m, const T &tol) const
test equality with some tolerance
Definition: clipper_types.h:335
General matrix class: like Array2d but with numerical methods.
Definition: clipper_types.h:396
Array2d()
null constructor
Definition: clipper_types.h:368
Mat33sym()
null constructor
Definition: clipper_types.h:262
static RTop< T > null()
return identity operator
Definition: clipper_types.h:345
const T & operator[](const int &i) const
get element
Definition: clipper_types.h:119
String format() const
return formatted String representation
Definition: clipper_types.h:350
const int & rows() const
number of rows
Definition: clipper_types.h:381
Vec3< T > & trn()
set translation
Definition: clipper_types.h:340
int i() const
convert to int
Definition: clipper_types.cpp:130
String(const std::string str)
constructor: from string
Definition: clipper_types.h:68
bool equals(const Vec3< T > &v, const T &tol) const
test equality
Definition: clipper_types.h:433
Vec3()
null constructor
Definition: clipper_types.h:109
3x3-matrix class
Definition: clipper_types.h:182
const T & operator()(const int &i, const int &j) const
get element
Definition: clipper_types.h:198