00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #ifndef BTK_LINEAR_ALGEBRA_H
00026 #define BTK_LINEAR_ALGEBRA_H
00027
00028
00029 #include <cassert>
00030 #include <cmath>
00031
00032 #include <iosfwd>
00033 #include <vector>
00034
00035 #include <boost/numeric/ublas/vector.hpp>
00036 #include <boost/numeric/ublas/matrix.hpp>
00037 #include <boost/numeric/ublas/symmetric.hpp>
00038 #include <boost/numeric/ublas/io.hpp>
00039
00040 namespace uBLAS = boost::numeric::ublas;
00041
00042 namespace BTK {
00043
00045
00046 const double PI = 3.1415926535897323;
00047 const double DEG2RAD = PI/180.0;
00048 const double RAD2DEG = 180.0/PI;
00050
00060 typedef uBLAS::vector<double> BTKVector;
00061 typedef uBLAS::matrix<double> BTKMatrix;
00062 typedef uBLAS::symmetric_matrix<double> BTKSymmetricMatrix;
00064
00065
00066
00067 inline
00068 BTKVector
00069 new_vector(double x=0.,
00070 double y=0.,
00071 double z=0.)
00072 {
00073 BTKVector temp(3);
00074 temp[0]=x; temp[1]=y; temp[2]=z;
00075 return temp;
00076 }
00077
00083 inline double length(const BTKVector& v) { return norm_2(v); }
00084
00086 inline
00087 BTKVector
00088 cross(const BTKVector& lhs,
00089 const BTKVector& rhs)
00090 {
00091 BTKVector temp(3);
00092
00093 temp[0] = lhs[1] * rhs[2] - lhs[2] * rhs[1];
00094 temp[1] = lhs[2] * rhs[0] - lhs[0] * rhs[2];
00095 temp[2] = lhs[0] * rhs[1] - lhs[1] * rhs[0];
00096
00097 return temp;
00098 }
00099
00104 BTKVector
00105 project(const BTKVector& a,
00106 const BTKVector& b);
00107
00112 BTKVector
00113 project_normal(const BTKVector& a,
00114 const BTKVector& b);
00115
00123 bool
00124 within_sqr_dist(const BTKVector& a,
00125 const BTKVector& b,
00126 double r_squared);
00127
00134 inline
00135 BTKVector&
00136 normalize(BTKVector & v)
00137 {
00138 v /= norm_2(v);
00139 return v;
00140 }
00141
00149 inline
00150 BTKVector
00151 normalize(const BTKVector& v)
00152 {
00153 BTKVector tmp = v / norm_2(v);
00154 return tmp;
00155 }
00156
00158 inline
00159 double
00160 cosine_vector_angle(const BTKVector& v1,
00161 const BTKVector& v2)
00162 {
00163 return (prec_inner_prod(v1,v2) / (norm_2(v1) * norm_2(v2)));
00164 }
00165
00167 inline
00168 double
00169 vector_angle(const BTKVector& v1,
00170 const BTKVector& v2)
00171 {
00172 return acos(cosine_vector_angle(v1,v2));
00173 }
00174
00176 inline
00177 double
00178 point_angle(const BTKVector& v1,
00179 const BTKVector& v2,
00180 const BTKVector& v3){
00181 return vector_angle(v1-v2,v3-v2);
00182 }
00183
00184
00190 double
00191 vector_dihedral(const BTKVector& v1,
00192 const BTKVector& v2,
00193 const BTKVector& v3);
00194
00195
00201 double
00202 point_dihedral(const BTKVector& a,
00203 const BTKVector& b,
00204 const BTKVector& c,
00205 const BTKVector& d);
00206
00219 BTKVector
00220 set_vector_from_dihedral(const BTKVector& v3,
00221 const BTKVector& v2,
00222 const BTKVector& v1,
00223 double len34,
00224 double ang234,
00225 double dih1234);
00226
00245 BTKVector
00246 set_vector_from_two_angles(const BTKVector& v3,
00247 const BTKVector& v2,
00248 const BTKVector& v1,
00249 double len34,
00250 double ang234,
00251 double ang134);
00252
00253
00255 struct EigenState {
00256 EigenState() {}
00257 EigenState(double e,BTKVector& v) :
00258 eigenvalue(e), eigenvector(v) {}
00259
00260 bool
00261 operator<(const EigenState& right) const
00262 {
00263 return eigenvalue < right.eigenvalue;
00264 }
00265
00266 double eigenvalue;
00267 BTKVector eigenvector;
00268 };
00269
00270
00272 std::ostream&
00273 operator<<(std::ostream& os,
00274 const std::vector<EigenState>& es);
00275
00277 std::vector<EigenState>
00278 diagonalize_symmetric(const BTKMatrix& m);
00279
00282
00283
00285
00286
00291 BTKMatrix
00292 create_rotation_matrix(const BTKVector& axis,
00293 double theta);
00299 void
00300 initialize_rotation_matrix(const BTKVector& axis,
00301 double theta,
00302 BTKMatrix& rm);
00304
00323
00324 BTKMatrix
00325 create_rotation_matrix(double phi,
00326 double theta,
00327 double psi);
00328
00330 void
00331 initialize_rotation_matrix(double phi,
00332 double theta,
00333 double psi,
00334 BTKMatrix& rm);
00336
00337 }
00338
00339 #endif // LINEAR_ALGEBRA_H