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
00038 #ifndef FCL_TRANSFORM_H
00039 #define FCL_TRANSFORM_H
00040
00041 #include "fcl/math/matrix_3f.h"
00042 #include <boost/thread/mutex.hpp>
00043
00044 namespace fcl
00045 {
00046
00048 class Quaternion3f
00049 {
00050 public:
00052 Quaternion3f()
00053 {
00054 data[0] = 1;
00055 data[1] = 0;
00056 data[2] = 0;
00057 data[3] = 0;
00058 }
00059
00060 Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
00061 {
00062 data[0] = a;
00063 data[1] = b;
00064 data[2] = c;
00065 data[3] = d;
00066 }
00067
00069 bool isIdentity() const
00070 {
00071 return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0);
00072 }
00073
00075 void fromRotation(const Matrix3f& R);
00076
00078 void toRotation(Matrix3f& R) const;
00079
00081 void fromAxes(const Vec3f axis[3]);
00082
00084 void toAxes(Vec3f axis[3]) const;
00085
00087 void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
00088
00090 void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
00091
00093 FCL_REAL dot(const Quaternion3f& other) const;
00094
00096 Quaternion3f operator + (const Quaternion3f& other) const;
00097 const Quaternion3f& operator += (const Quaternion3f& other);
00098
00100 Quaternion3f operator - (const Quaternion3f& other) const;
00101 const Quaternion3f& operator -= (const Quaternion3f& other);
00102
00104 Quaternion3f operator * (const Quaternion3f& other) const;
00105 const Quaternion3f& operator *= (const Quaternion3f& other);
00106
00108 Quaternion3f operator - () const;
00109
00111 Quaternion3f operator * (FCL_REAL t) const;
00112 const Quaternion3f& operator *= (FCL_REAL t);
00113
00115 Quaternion3f& conj();
00116
00118 Quaternion3f& inverse();
00119
00121 Vec3f transform(const Vec3f& v) const;
00122
00123 inline const FCL_REAL& getW() const { return data[0]; }
00124 inline const FCL_REAL& getX() const { return data[1]; }
00125 inline const FCL_REAL& getY() const { return data[2]; }
00126 inline const FCL_REAL& getZ() const { return data[3]; }
00127
00128 inline FCL_REAL& getW() { return data[0]; }
00129 inline FCL_REAL& getX() { return data[1]; }
00130 inline FCL_REAL& getY() { return data[2]; }
00131 inline FCL_REAL& getZ() { return data[3]; }
00132
00133 private:
00134
00135 FCL_REAL data[4];
00136 };
00137
00139 Quaternion3f conj(const Quaternion3f& q);
00140
00142 Quaternion3f inverse(const Quaternion3f& q);
00143
00145 class Transform3f
00146 {
00147 boost::mutex lock_;
00148
00150 mutable bool matrix_set;
00152 mutable Matrix3f R;
00153
00155 Vec3f T;
00156
00158 Quaternion3f q;
00159
00160 const Matrix3f& getRotationInternal() const;
00161 public:
00162
00164 Transform3f()
00165 {
00166 setIdentity();
00167 }
00168
00170 Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
00171 R(R_),
00172 T(T_)
00173 {
00174 q.fromRotation(R_);
00175 }
00176
00178 Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
00179 T(T_),
00180 q(q_)
00181 {
00182 }
00183
00185 Transform3f(const Matrix3f& R_) : matrix_set(true),
00186 R(R_)
00187 {
00188 q.fromRotation(R_);
00189 }
00190
00192 Transform3f(const Quaternion3f& q_) : matrix_set(false),
00193 q(q_)
00194 {
00195 }
00196
00198 Transform3f(const Vec3f& T_) : matrix_set(true),
00199 T(T_)
00200 {
00201 R.setIdentity();
00202 }
00203
00205 Transform3f(const Transform3f& tf) : matrix_set(tf.matrix_set),
00206 R(tf.R),
00207 T(tf.T),
00208 q(tf.q)
00209 {
00210 }
00211
00213 Transform3f& operator = (const Transform3f& tf)
00214 {
00215 matrix_set = tf.matrix_set;
00216 R = tf.R;
00217 q = tf.q;
00218 T = tf.T;
00219 return *this;
00220 }
00221
00223 inline const Vec3f& getTranslation() const
00224 {
00225 return T;
00226 }
00227
00229 inline const Matrix3f& getRotation() const
00230 {
00231 if(matrix_set) return R;
00232 return getRotationInternal();
00233 }
00234
00236 inline const Quaternion3f& getQuatRotation() const
00237 {
00238 return q;
00239 }
00240
00242 inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
00243 {
00244 R = R_;
00245 T = T_;
00246 matrix_set = true;
00247 q.fromRotation(R_);
00248 }
00249
00251 inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
00252 {
00253 matrix_set = false;
00254 q = q_;
00255 T = T_;
00256 }
00257
00259 inline void setRotation(const Matrix3f& R_)
00260 {
00261 R = R_;
00262 matrix_set = true;
00263 q.fromRotation(R_);
00264 }
00265
00267 inline void setTranslation(const Vec3f& T_)
00268 {
00269 T = T_;
00270 }
00271
00273 inline void setQuatRotation(const Quaternion3f& q_)
00274 {
00275 matrix_set = false;
00276 q = q_;
00277 }
00278
00280 inline Vec3f transform(const Vec3f& v) const
00281 {
00282 return q.transform(v) + T;
00283 }
00284
00286 inline Transform3f& inverse()
00287 {
00288 matrix_set = false;
00289 q.conj();
00290 T = q.transform(-T);
00291 return *this;
00292 }
00293
00295 inline Transform3f inverseTimes(const Transform3f& other) const
00296 {
00297 const Quaternion3f& q_inv = fcl::conj(q);
00298 return Transform3f(q_inv * other.q, q_inv.transform(other.T - T));
00299 }
00300
00302 inline const Transform3f& operator *= (const Transform3f& other)
00303 {
00304 matrix_set = false;
00305 T = q.transform(other.T) + T;
00306 q *= other.q;
00307 return *this;
00308 }
00309
00311 inline Transform3f operator * (const Transform3f& other) const
00312 {
00313 Quaternion3f q_new = q * other.q;
00314 return Transform3f(q_new, q.transform(other.T) + T);
00315 }
00316
00318 inline bool isIdentity() const
00319 {
00320 return q.isIdentity() && T.isZero();
00321 }
00322
00324 inline void setIdentity()
00325 {
00326 R.setIdentity();
00327 T.setValue(0);
00328 q = Quaternion3f();
00329 matrix_set = true;
00330 }
00331
00332 };
00333
00335 Transform3f inverse(const Transform3f& tf);
00336
00338 void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
00339 Transform3f& tf);
00340
00341 }
00342
00343 #endif