math_details.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef FCL_MATH_DETAILS_H
00036 #define FCL_MATH_DETAILS_H
00037 
00038 
00039 #include <cmath>
00040 #include <algorithm>
00041 #include <cstring>
00042 
00043 namespace fcl
00044 {
00045 
00046 namespace details
00047 {
00048 
00049 
00050 template <typename T>
00051 struct Vec3Data
00052 {
00053   typedef T meta_type;
00054 
00055   T vs[3];
00056   Vec3Data() { setValue(0); }
00057   Vec3Data(T x)
00058   {
00059     setValue(x);
00060   }
00061 
00062   Vec3Data(T* x)
00063   {
00064     memcpy(vs, x, sizeof(T) * 3);
00065   }
00066 
00067   Vec3Data(T x, T y, T z)
00068   {
00069     setValue(x, y, z);
00070   }
00071 
00072   inline void setValue(T x, T y, T z)
00073   {
00074     vs[0] = x; vs[1] = y; vs[2] = z;
00075   }
00076 
00077   inline void setValue(T x)
00078   {
00079     vs[0] = x; vs[1] = x; vs[2] = x;
00080   }
00081 
00082   inline void negate()
00083   {
00084     vs[0] = -vs[0]; vs[1] = -vs[1]; vs[2] = -vs[2];
00085   }
00086 
00087   inline Vec3Data<T>& ubound(const Vec3Data<T>& u) 
00088   {
00089     vs[0] = std::min(vs[0], u.vs[0]);
00090     vs[1] = std::min(vs[1], u.vs[1]);
00091     vs[2] = std::min(vs[2], u.vs[2]);
00092     return *this;
00093   }
00094 
00095   inline Vec3Data<T>& lbound(const Vec3Data<T>& l)
00096   {
00097     vs[0] = std::max(vs[0], l.vs[0]);
00098     vs[1] = std::max(vs[1], l.vs[1]);
00099     vs[2] = std::max(vs[2], l.vs[2]);
00100     return *this;
00101   }
00102 
00103   T operator [] (size_t i) const { return vs[i]; }
00104   T& operator [] (size_t i) { return vs[i]; }
00105 
00106   inline Vec3Data<T> operator + (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] + other.vs[0], vs[1] + other.vs[1], vs[2] + other.vs[2]); }
00107   inline Vec3Data<T> operator - (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] - other.vs[0], vs[1] - other.vs[1], vs[2] - other.vs[2]); }
00108   inline Vec3Data<T> operator * (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] * other.vs[0], vs[1] * other.vs[1], vs[2] * other.vs[2]); }
00109   inline Vec3Data<T> operator / (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] / other.vs[0], vs[1] / other.vs[1], vs[2] / other.vs[2]); }
00110   inline Vec3Data<T>& operator += (const Vec3Data<T>& other) { vs[0] += other.vs[0]; vs[1] += other.vs[1]; vs[2] += other.vs[2]; return *this; }
00111   inline Vec3Data<T>& operator -= (const Vec3Data<T>& other) { vs[0] -= other.vs[0]; vs[1] -= other.vs[1]; vs[2] -= other.vs[2]; return *this; }
00112   inline Vec3Data<T>& operator *= (const Vec3Data<T>& other) { vs[0] *= other.vs[0]; vs[1] *= other.vs[1]; vs[2] *= other.vs[2]; return *this; }
00113   inline Vec3Data<T>& operator /= (const Vec3Data<T>& other) { vs[0] /= other.vs[0]; vs[1] /= other.vs[1]; vs[2] /= other.vs[2]; return *this; }
00114   inline Vec3Data<T> operator + (T t) const { return Vec3Data<T>(vs[0] + t, vs[1] + t, vs[2] + t); }
00115   inline Vec3Data<T> operator - (T t) const { return Vec3Data<T>(vs[0] - t, vs[1] - t, vs[2] - t); }
00116   inline Vec3Data<T> operator * (T t) const { return Vec3Data<T>(vs[0] * t, vs[1] * t, vs[2] * t); }
00117   inline Vec3Data<T> operator / (T t) const { T inv_t = 1.0 / t; return Vec3Data<T>(vs[0] * inv_t, vs[1] * inv_t, vs[2] * inv_t); }
00118   inline Vec3Data<T>& operator += (T t) { vs[0] += t; vs[1] += t; vs[2] += t; return *this; }
00119   inline Vec3Data<T>& operator -= (T t) { vs[0] -= t; vs[1] -= t; vs[2] -= t; return *this; }
00120   inline Vec3Data<T>& operator *= (T t) { vs[0] *= t; vs[1] *= t; vs[2] *= t; return *this; }
00121   inline Vec3Data<T>& operator /= (T t) { T inv_t = 1.0 / t; vs[0] *= inv_t; vs[1] *= inv_t; vs[2] *= inv_t; return *this; }
00122   inline Vec3Data<T> operator - () const { return Vec3Data<T>(-vs[0], -vs[1], -vs[2]); }
00123 };
00124 
00125 
00126 template <typename T>
00127 static inline Vec3Data<T> cross_prod(const Vec3Data<T>& l, const Vec3Data<T>& r)
00128 {
00129   return Vec3Data<T>(l.vs[1] * r.vs[2] - l.vs[2] * r.vs[1], 
00130                      l.vs[2] * r.vs[0] - l.vs[0] * r.vs[2],
00131                      l.vs[0] * r.vs[1] - l.vs[1] * r.vs[0]);
00132 }
00133 
00134 template <typename T>
00135 static inline T dot_prod3(const Vec3Data<T>& l, const Vec3Data<T>& r)
00136 {
00137   return l.vs[0] * r.vs[0] + l.vs[1] * r.vs[1] + l.vs[2] * r.vs[2];
00138 }
00139 
00140 
00141 template <typename T>
00142 static inline Vec3Data<T> min(const Vec3Data<T>& x, const Vec3Data<T>& y)
00143 {
00144   return Vec3Data<T>(std::min(x.vs[0], y.vs[0]), std::min(x.vs[1], y.vs[1]), std::min(x.vs[2], y.vs[2]));
00145 }
00146 
00147 template <typename T>
00148 static inline Vec3Data<T> max(const Vec3Data<T>& x, const Vec3Data<T>& y)
00149 {
00150   return Vec3Data<T>(std::max(x.vs[0], y.vs[0]), std::max(x.vs[1], y.vs[1]), std::max(x.vs[2], y.vs[2]));
00151 }
00152 
00153 template <typename T>
00154 static inline Vec3Data<T> abs(const Vec3Data<T>& x)
00155 {
00156   return Vec3Data<T>(std::abs(x.vs[0]), std::abs(x.vs[1]), std::abs(x.vs[2]));
00157 }
00158 
00159 template <typename T>
00160 static inline bool equal(const Vec3Data<T>& x, const Vec3Data<T>& y, T epsilon)
00161 {
00162   return ((x.vs[0] - y.vs[0] < epsilon) &&
00163           (x.vs[0] - y.vs[0] > -epsilon) &&
00164           (x.vs[1] - y.vs[1] < epsilon) &&
00165           (x.vs[1] - y.vs[1] > -epsilon) &&
00166           (x.vs[2] - y.vs[2] < epsilon) &&
00167           (x.vs[2] - y.vs[2] > -epsilon));
00168 }
00169 
00170 
00171 template<typename T>
00172 struct Matrix3Data
00173 {
00174   typedef T meta_type;
00175   typedef Vec3Data<T> vector_type;
00176   
00177   Vec3Data<T> rs[3];
00178   Matrix3Data() {};
00179 
00180   Matrix3Data(T xx, T xy, T xz,
00181               T yx, T yy, T yz,
00182               T zx, T zy, T zz)
00183   {
00184     setValue(xx, xy, xz,
00185              yx, yy, yz,
00186              zx, zy, zz);
00187   }
00188 
00189   Matrix3Data(const Vec3Data<T>& v1, const Vec3Data<T>& v2, const Vec3Data<T>& v3)
00190   {
00191     rs[0] = v1;
00192     rs[1] = v2;
00193     rs[2] = v3;
00194   }
00195 
00196   Matrix3Data(const Matrix3Data<T>& other)
00197   {
00198     rs[0] = other.rs[0];
00199     rs[1] = other.rs[1];
00200     rs[2] = other.rs[2];
00201   }
00202 
00203   inline Vec3Data<T> getColumn(size_t i) const
00204   {
00205     return Vec3Data<T>(rs[0][i], rs[1][i], rs[2][i]);
00206   }
00207 
00208   inline const Vec3Data<T>& getRow(size_t i) const
00209   {
00210     return rs[i];
00211   }
00212 
00213   inline T operator() (size_t i, size_t j) const 
00214   {
00215     return rs[i][j];
00216   }
00217 
00218   inline T& operator() (size_t i, size_t j)
00219   {
00220     return rs[i][j];
00221   }
00222 
00223   inline Vec3Data<T> operator * (const Vec3Data<T>& v) const
00224   {
00225     return Vec3Data<T>(dot_prod3(rs[0], v), dot_prod3(rs[1], v), dot_prod3(rs[2], v));
00226   }
00227 
00228   inline Matrix3Data<T> operator * (const Matrix3Data<T>& other) const
00229   {
00230     return Matrix3Data<T>(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]),
00231                           other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]),
00232                           other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
00233   }
00234 
00235   inline Matrix3Data<T> operator + (const Matrix3Data<T>& other) const
00236   {
00237     return Matrix3Data<T>(rs[0] + other.rs[0], rs[1] + other.rs[1], rs[2] + other.rs[2]);
00238   }
00239 
00240   inline Matrix3Data<T> operator - (const Matrix3Data<T>& other) const
00241   {
00242     return Matrix3Data<T>(rs[0] - other.rs[0], rs[1] - other.rs[1], rs[2] - other.rs[2]);
00243   }
00244 
00245   inline Matrix3Data<T> operator + (T c) const
00246   {
00247     return Matrix3Data<T>(rs[0] + c, rs[1] + c, rs[2] + c);
00248   }
00249 
00250   inline Matrix3Data<T> operator - (T c) const
00251   {
00252     return Matrix3Data<T>(rs[0] - c, rs[1] - c, rs[2] - c);
00253   }
00254 
00255   inline Matrix3Data<T> operator * (T c) const
00256   {
00257     return Matrix3Data<T>(rs[0] * c, rs[1] * c, rs[2] * c);
00258   }
00259 
00260   inline Matrix3Data<T> operator / (T c) const
00261   {
00262     return Matrix3Data<T>(rs[0] / c, rs[1] / c, rs[2] / c);
00263   }
00264 
00265   inline Matrix3Data<T>& operator *= (const Matrix3Data<T>& other)
00266   {
00267     rs[0].setValue(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]));
00268     rs[1].setValue(other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]));
00269     rs[2].setValue(other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
00270     return *this;
00271   }
00272 
00273   inline Matrix3Data<T>& operator += (const Matrix3Data<T>& other)
00274   {
00275     rs[0] += other.rs[0];
00276     rs[1] += other.rs[1];
00277     rs[2] += other.rs[2];
00278     return *this;
00279   }
00280 
00281   inline Matrix3Data<T>& operator -= (const Matrix3Data<T>& other)
00282   {
00283     rs[0] -= other.rs[0];
00284     rs[1] -= other.rs[1];
00285     rs[2] -= other.rs[2];
00286     return *this;
00287   }
00288 
00289   inline Matrix3Data<T>& operator += (T c)
00290   {
00291     rs[0] += c;
00292     rs[1] += c;
00293     rs[2] += c;
00294     return *this;
00295   }
00296 
00297   inline Matrix3Data<T>& operator - (T c)
00298   {
00299     rs[0] -= c;
00300     rs[1] -= c;
00301     rs[2] -= c;
00302     return *this;
00303   }
00304 
00305   inline Matrix3Data<T>& operator * (T c)
00306   {
00307     rs[0] *= c;
00308     rs[1] *= c;
00309     rs[2] *= c;
00310     return *this;
00311   }
00312 
00313   inline Matrix3Data<T>& operator / (T c)
00314   {
00315     rs[0] /= c;
00316     rs[1] /= c;
00317     rs[2] /= c;
00318     return *this;
00319   }
00320 
00321 
00322   void setIdentity() 
00323   {
00324     setValue((T)1, (T)0, (T)0,
00325              (T)0, (T)1, (T)0,
00326              (T)0, (T)0, (T)1);
00327   }
00328 
00329   void setZero()
00330   {
00331     setValue((T)0);
00332   }
00333 
00334   static const Matrix3Data<T>& getIdentity()
00335   {
00336     static const Matrix3Data<T> I((T)1, (T)0, (T)0,
00337                                   (T)0, (T)1, (T)0,
00338                                   (T)0, (T)0, (T)1);
00339     return I;
00340   }
00341 
00342   T determinant() const
00343   {
00344     return dot_prod3(rs[0], cross_prod(rs[1], rs[2]));
00345   }
00346 
00347   Matrix3Data<T>& transpose()
00348   {
00349     register T tmp = rs[0][1];
00350     rs[0][1] = rs[1][0];
00351     rs[1][0] = tmp;
00352     
00353     tmp = rs[0][2];
00354     rs[0][2] = rs[2][0];
00355     rs[2][0] = tmp;
00356     
00357     tmp = rs[2][1];
00358     rs[2][1] = rs[1][2];
00359     rs[1][2] = tmp;
00360     return *this;
00361   }
00362 
00363   Matrix3Data<T>& inverse()
00364   {
00365     T det = determinant();
00366     register T inrsdet = 1 / det;
00367     
00368     setValue((rs[1][1] * rs[2][2] - rs[1][2] * rs[2][1]) * inrsdet,
00369              (rs[0][2] * rs[2][1] - rs[0][1] * rs[2][2]) * inrsdet,
00370              (rs[0][1] * rs[1][2] - rs[0][2] * rs[1][1]) * inrsdet,
00371              (rs[1][2] * rs[2][0] - rs[1][0] * rs[2][2]) * inrsdet,
00372              (rs[0][0] * rs[2][2] - rs[0][2] * rs[2][0]) * inrsdet,
00373              (rs[0][2] * rs[1][0] - rs[0][0] * rs[1][2]) * inrsdet,
00374              (rs[1][0] * rs[2][1] - rs[1][1] * rs[2][0]) * inrsdet,
00375              (rs[0][1] * rs[2][0] - rs[0][0] * rs[2][1]) * inrsdet,
00376              (rs[0][0] * rs[1][1] - rs[0][1] * rs[1][0]) * inrsdet);
00377 
00378     return *this;
00379   }
00380 
00381   Matrix3Data<T> transposeTimes(const Matrix3Data<T>& m) const
00382   {
00383     return Matrix3Data<T>(rs[0][0] * m.rs[0][0] + rs[1][0] * m.rs[1][0] + rs[2][0] * m.rs[2][0],
00384                           rs[0][0] * m.rs[0][1] + rs[1][0] * m.rs[1][1] + rs[2][0] * m.rs[2][1],
00385                           rs[0][0] * m.rs[0][2] + rs[1][0] * m.rs[1][2] + rs[2][0] * m.rs[2][2],
00386                           rs[0][1] * m.rs[0][0] + rs[1][1] * m.rs[1][0] + rs[2][1] * m.rs[2][0],
00387                           rs[0][1] * m.rs[0][1] + rs[1][1] * m.rs[1][1] + rs[2][1] * m.rs[2][1],
00388                           rs[0][1] * m.rs[0][2] + rs[1][1] * m.rs[1][2] + rs[2][1] * m.rs[2][2],
00389                           rs[0][2] * m.rs[0][0] + rs[1][2] * m.rs[1][0] + rs[2][2] * m.rs[2][0],
00390                           rs[0][2] * m.rs[0][1] + rs[1][2] * m.rs[1][1] + rs[2][2] * m.rs[2][1],
00391                           rs[0][2] * m.rs[0][2] + rs[1][2] * m.rs[1][2] + rs[2][2] * m.rs[2][2]);
00392   }
00393    
00394   Matrix3Data<T> timesTranspose(const Matrix3Data<T>& m) const
00395   {
00396     return Matrix3Data<T>(dot_prod3(rs[0], m[0]), dot_prod3(rs[0], m[1]), dot_prod3(rs[0], m[2]),
00397                           dot_prod3(rs[1], m[0]), dot_prod3(rs[1], m[1]), dot_prod3(rs[1], m[2]),
00398                           dot_prod3(rs[2], m[0]), dot_prod3(rs[2], m[1]), dot_prod3(rs[2], m[2]));
00399   }
00400 
00401 
00402   Vec3Data<T> transposeTimes(const Vec3Data<T>& v) const
00403   {
00404     return Vec3Data<T>(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
00405   }
00406 
00407   inline T transposeDotX(const Vec3Data<T>& v) const
00408   {
00409     return rs[0][0] * v[0] + rs[1][0] * v[1] + rs[2][0] * v[2];
00410   }
00411 
00412   inline T transposeDotY(const Vec3Data<T>& v) const
00413   {
00414     return rs[0][1] * v[0] + rs[1][1] * v[1] + rs[2][1] * v[2];
00415   }
00416 
00417   inline T transposeDotZ(const Vec3Data<T>& v) const
00418   {
00419     return rs[0][2] * v[0] + rs[1][2] * v[1] + rs[2][2] * v[2];
00420   }
00421 
00422   inline T transposeDot(size_t i, const Vec3Data<T>& v) const
00423   {
00424     return rs[0][i] * v[0] + rs[1][i] * v[1] + rs[2][i] * v[2];
00425   }
00426 
00427   inline T dotX(const Vec3Data<T>& v) const
00428   {
00429     return rs[0][0] * v[0] + rs[0][1] * v[1] + rs[0][2] * v[2];
00430   }
00431 
00432   inline T dotY(const Vec3Data<T>& v) const
00433   {
00434     return rs[1][0] * v[0] + rs[1][1] * v[1] + rs[1][2] * v[2];
00435   }
00436 
00437   inline T dotZ(const Vec3Data<T>& v) const
00438   {
00439     return rs[2][0] * v[0] + rs[2][1] * v[1] + rs[2][2] * v[2];
00440   }
00441 
00442   inline T dot(size_t i, const Vec3Data<T>& v) const
00443   {
00444     return rs[i][0] * v[0] + rs[i][1] * v[1] + rs[i][2] * v[2];
00445   }
00446     
00447   inline void setValue(T xx, T xy, T xz,
00448                        T yx, T yy, T yz,
00449                        T zx, T zy, T zz)
00450   {
00451     rs[0].setValue(xx, xy, xz);
00452     rs[1].setValue(yx, yy, yz);
00453     rs[2].setValue(zx, zy, zz);
00454   }
00455     
00456   inline void setValue(T x)
00457   {
00458     rs[0].setValue(x);
00459     rs[1].setValue(x);
00460     rs[2].setValue(x);
00461   }
00462 };
00463 
00464 
00465 
00466 template<typename T>
00467 Matrix3Data<T> abs(const Matrix3Data<T>& m)
00468 {
00469   return Matrix3Data<T>(abs(m.rs[0]), abs(m.rs[1]), abs(m.rs[2]));
00470 }
00471 
00472 template<typename T>
00473 Matrix3Data<T> transpose(const Matrix3Data<T>& m)
00474 {
00475   return Matrix3Data<T>(m.rs[0][0], m.rs[1][0], m.rs[2][0],
00476                         m.rs[0][1], m.rs[1][1], m.rs[2][1],
00477                         m.rs[0][2], m.rs[1][2], m.rs[2][2]);
00478 }
00479 
00480 
00481 template<typename T>
00482 Matrix3Data<T> inverse(const Matrix3Data<T>& m)
00483 {
00484   T det = m.determinant();
00485   T inrsdet = 1 / det;
00486 
00487   return Matrix3Data<T>((m.rs[1][1] * m.rs[2][2] - m.rs[1][2] * m.rs[2][1]) * inrsdet,
00488                         (m.rs[0][2] * m.rs[2][1] - m.rs[0][1] * m.rs[2][2]) * inrsdet,
00489                         (m.rs[0][1] * m.rs[1][2] - m.rs[0][2] * m.rs[1][1]) * inrsdet,
00490                         (m.rs[1][2] * m.rs[2][0] - m.rs[1][0] * m.rs[2][2]) * inrsdet,
00491                         (m.rs[0][0] * m.rs[2][2] - m.rs[0][2] * m.rs[2][0]) * inrsdet,
00492                         (m.rs[0][2] * m.rs[1][0] - m.rs[0][0] * m.rs[1][2]) * inrsdet,
00493                         (m.rs[1][0] * m.rs[2][1] - m.rs[1][1] * m.rs[2][0]) * inrsdet,
00494                         (m.rs[0][1] * m.rs[2][0] - m.rs[0][0] * m.rs[2][1]) * inrsdet,
00495                         (m.rs[0][0] * m.rs[1][1] - m.rs[0][1] * m.rs[1][0]) * inrsdet);
00496 }
00497 
00498 }
00499 
00500 }
00501 
00502 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30