00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/ccd/taylor_matrix.h"
00038
00039 namespace fcl
00040 {
00041
00042 TMatrix3::TMatrix3()
00043 {
00044 }
00045
00046 TMatrix3::TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval)
00047 {
00048 setTimeInterval(time_interval);
00049 }
00050
00051 TMatrix3::TMatrix3(TaylorModel m[3][3])
00052 {
00053 v_[0] = TVector3(m[0]);
00054 v_[1] = TVector3(m[1]);
00055 v_[2] = TVector3(m[2]);
00056 }
00057
00058 TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3)
00059 {
00060 v_[0] = v1;
00061 v_[1] = v2;
00062 v_[2] = v3;
00063 }
00064
00065
00066 TMatrix3::TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval)
00067 {
00068 v_[0] = TVector3(m.getRow(0), time_interval);
00069 v_[1] = TVector3(m.getRow(1), time_interval);
00070 v_[2] = TVector3(m.getRow(2), time_interval);
00071 }
00072
00073 void TMatrix3::setIdentity()
00074 {
00075 setZero();
00076 v_[0][0].coeffs_[0] = 1;
00077 v_[1][1].coeffs_[0] = 1;
00078 v_[2][2].coeffs_[0] = 1;
00079
00080 }
00081
00082 void TMatrix3::setZero()
00083 {
00084 v_[0].setZero();
00085 v_[1].setZero();
00086 v_[2].setZero();
00087 }
00088
00089 TVector3 TMatrix3::getColumn(size_t i) const
00090 {
00091 return TVector3(v_[0][i], v_[1][i], v_[2][i]);
00092 }
00093
00094 const TVector3& TMatrix3::getRow(size_t i) const
00095 {
00096 return v_[i];
00097 }
00098
00099 const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const
00100 {
00101 return v_[i][j];
00102 }
00103
00104 TaylorModel& TMatrix3::operator () (size_t i, size_t j)
00105 {
00106 return v_[i][j];
00107 }
00108
00109 TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
00110 {
00111 const Vec3f& mc0 = m.getColumn(0);
00112 const Vec3f& mc1 = m.getColumn(1);
00113 const Vec3f& mc2 = m.getColumn(2);
00114
00115 return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
00116 TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
00117 TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
00118 }
00119
00120
00121 TVector3 TMatrix3::operator * (const Vec3f& v) const
00122 {
00123 return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
00124 }
00125
00126 TVector3 TMatrix3::operator * (const TVector3& v) const
00127 {
00128 return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
00129 }
00130
00131 TMatrix3 TMatrix3::operator * (const TMatrix3& m) const
00132 {
00133 const TVector3& mc0 = m.getColumn(0);
00134 const TVector3& mc1 = m.getColumn(1);
00135 const TVector3& mc2 = m.getColumn(2);
00136
00137 return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
00138 TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
00139 TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
00140 }
00141
00142 TMatrix3 TMatrix3::operator * (const TaylorModel& d) const
00143 {
00144 return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
00145 }
00146
00147 TMatrix3 TMatrix3::operator * (FCL_REAL d) const
00148 {
00149 return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
00150 }
00151
00152
00153 TMatrix3& TMatrix3::operator *= (const Matrix3f& m)
00154 {
00155 const Vec3f& mc0 = m.getColumn(0);
00156 const Vec3f& mc1 = m.getColumn(1);
00157 const Vec3f& mc2 = m.getColumn(2);
00158
00159 v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
00160 v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
00161 v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
00162 return *this;
00163 }
00164
00165 TMatrix3& TMatrix3::operator *= (const TMatrix3& m)
00166 {
00167 const TVector3& mc0 = m.getColumn(0);
00168 const TVector3& mc1 = m.getColumn(1);
00169 const TVector3& mc2 = m.getColumn(2);
00170
00171 v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
00172 v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
00173 v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
00174 return *this;
00175 }
00176
00177 TMatrix3& TMatrix3::operator *= (const TaylorModel& d)
00178 {
00179 v_[0] *= d;
00180 v_[1] *= d;
00181 v_[2] *= d;
00182 return *this;
00183 }
00184
00185 TMatrix3& TMatrix3::operator *= (FCL_REAL d)
00186 {
00187 v_[0] *= d;
00188 v_[1] *= d;
00189 v_[2] *= d;
00190 return *this;
00191 }
00192
00193 TMatrix3 TMatrix3::operator + (const TMatrix3& m) const
00194 {
00195 return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
00196 }
00197
00198 TMatrix3& TMatrix3::operator += (const TMatrix3& m)
00199 {
00200 v_[0] += m.v_[0];
00201 v_[1] += m.v_[1];
00202 v_[2] += m.v_[2];
00203 return *this;
00204 }
00205
00206 TMatrix3 TMatrix3::operator - (const TMatrix3& m) const
00207 {
00208 return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
00209 }
00210
00211 TMatrix3& TMatrix3::operator -= (const TMatrix3& m)
00212 {
00213 v_[0] -= m.v_[0];
00214 v_[1] -= m.v_[1];
00215 v_[2] -= m.v_[2];
00216 return *this;
00217 }
00218
00219 void TMatrix3::print() const
00220 {
00221 getColumn(0).print();
00222 getColumn(1).print();
00223 getColumn(2).print();
00224 }
00225
00226 IMatrix3 TMatrix3::getBound() const
00227 {
00228 return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound());
00229 }
00230
00231 FCL_REAL TMatrix3::diameter() const
00232 {
00233 FCL_REAL d = 0;
00234
00235 FCL_REAL tmp = v_[0][0].r_.diameter();
00236 if(tmp > d) d = tmp;
00237 tmp = v_[0][1].r_.diameter();
00238 if(tmp > d) d = tmp;
00239 tmp = v_[0][2].r_.diameter();
00240 if(tmp > d) d = tmp;
00241
00242 tmp = v_[1][0].r_.diameter();
00243 if(tmp > d) d = tmp;
00244 tmp = v_[1][1].r_.diameter();
00245 if(tmp > d) d = tmp;
00246 tmp = v_[1][2].r_.diameter();
00247 if(tmp > d) d = tmp;
00248
00249 tmp = v_[2][0].r_.diameter();
00250 if(tmp > d) d = tmp;
00251 tmp = v_[2][1].r_.diameter();
00252 if(tmp > d) d = tmp;
00253 tmp = v_[2][2].r_.diameter();
00254 if(tmp > d) d = tmp;
00255
00256 return d;
00257 }
00258
00259 void TMatrix3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
00260 {
00261 v_[0].setTimeInterval(time_interval);
00262 v_[1].setTimeInterval(time_interval);
00263 v_[2].setTimeInterval(time_interval);
00264 }
00265
00266 TMatrix3& TMatrix3::rotationConstrain()
00267 {
00268 for(std::size_t i = 0; i < 3; ++i)
00269 {
00270 for(std::size_t j = 0; j < 3; ++j)
00271 {
00272 if(v_[i][j].r_[0] < -1) v_[i][j].r_[0] = -1;
00273 else if(v_[i][j].r_[0] > 1) v_[i][j].r_[0] = 1;
00274
00275 if(v_[i][j].r_[1] < -1) v_[i][j].r_[1] = -1;
00276 else if(v_[i][j].r_[1] > 1) v_[i][j].r_[1] = 1;
00277
00278 if((v_[i][j].r_[0] == -1) && (v_[i][j].r_[1] == 1))
00279 {
00280 v_[i][j].coeffs_[0] = v_[i][j].coeffs_[1] = v_[i][j].coeffs_[2] = v_[i][j].coeffs_[3] = 0;
00281 }
00282 }
00283 }
00284
00285 return *this;
00286 }
00287
00288
00289 TMatrix3 rotationConstrain(const TMatrix3& m)
00290 {
00291 TMatrix3 res;
00292
00293 for(std::size_t i = 0; i < 3; ++i)
00294 {
00295 for(std::size_t j = 0; j < 3; ++j)
00296 {
00297 if(m(i, j).r_[0] < -1) res(i, j).r_[0] = -1;
00298 else if(m(i, j).r_[0] > 1) res(i, j).r_[0] = 1;
00299
00300 if(m(i, j).r_[1] < -1) res(i, j).r_[1] = -1;
00301 else if(m(i, j).r_[1] > 1) res(i, j).r_[1] = 1;
00302
00303 if((m(i, j).r_[0] == -1) && (m(i, j).r_[1] == 1))
00304 {
00305 res(i, j).coeffs_[0] = res(i, j).coeffs_[1] = res(i, j).coeffs_[2] = res(i, j).coeffs_[3] = 0;
00306 }
00307 }
00308 }
00309
00310 return res;
00311 }
00312
00313
00314 }