transform.cpp
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 
00038 #include "fcl/math/transform.h"
00039 
00040 namespace fcl
00041 {
00042 
00043 void Quaternion3f::fromRotation(const Matrix3f& R)
00044 {
00045   const int next[3] = {1, 2, 0};
00046 
00047   FCL_REAL trace = R(0, 0) + R(1, 1) + R(2, 2);
00048   FCL_REAL root;
00049 
00050   if(trace > 0.0)
00051   {
00052     // |w| > 1/2, may as well choose w > 1/2
00053     root = sqrt(trace + 1.0);  // 2w
00054     data[0] = 0.5 * root;
00055     root = 0.5 / root;  // 1/(4w)
00056     data[1] = (R(2, 1) - R(1, 2))*root;
00057     data[2] = (R(0, 2) - R(2, 0))*root;
00058     data[3] = (R(1, 0) - R(0, 1))*root;
00059   }
00060   else
00061   {
00062     // |w| <= 1/2
00063     int i = 0;
00064     if(R(1, 1) > R(0, 0))
00065     {
00066       i = 1;
00067     }
00068     if(R(2, 2) > R(i, i))
00069     {
00070       i = 2;
00071     }
00072     int j = next[i];
00073     int k = next[j];
00074 
00075     root = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0);
00076     FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
00077     *quat[i] = 0.5 * root;
00078     root = 0.5 / root;
00079     data[0] = (R(k, j) - R(j, k)) * root;
00080     *quat[j] = (R(j, i) + R(i, j)) * root;
00081     *quat[k] = (R(k, i) + R(i, k)) * root;
00082   }
00083 }
00084 
00085 void Quaternion3f::toRotation(Matrix3f& R) const
00086 {
00087   FCL_REAL twoX  = 2.0*data[1];
00088   FCL_REAL twoY  = 2.0*data[2];
00089   FCL_REAL twoZ  = 2.0*data[3];
00090   FCL_REAL twoWX = twoX*data[0];
00091   FCL_REAL twoWY = twoY*data[0];
00092   FCL_REAL twoWZ = twoZ*data[0];
00093   FCL_REAL twoXX = twoX*data[1];
00094   FCL_REAL twoXY = twoY*data[1];
00095   FCL_REAL twoXZ = twoZ*data[1];
00096   FCL_REAL twoYY = twoY*data[2];
00097   FCL_REAL twoYZ = twoZ*data[2];
00098   FCL_REAL twoZZ = twoZ*data[3];
00099 
00100   R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
00101              twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
00102              twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
00103 }
00104 
00105 
00106 void Quaternion3f::fromAxes(const Vec3f axis[3])
00107 {
00108   // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00109   // article "Quaternion Calculus and Fast Animation".
00110 
00111   const int next[3] = {1, 2, 0};
00112 
00113   FCL_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
00114   FCL_REAL root;
00115 
00116   if(trace > 0.0)
00117   {
00118     // |w| > 1/2, may as well choose w > 1/2
00119     root = sqrt(trace + 1.0);  // 2w
00120     data[0] = 0.5 * root;
00121     root = 0.5 / root;  // 1/(4w)
00122     data[1] = (axis[1][2] - axis[2][1])*root;
00123     data[2] = (axis[2][0] - axis[0][2])*root;
00124     data[3] = (axis[0][1] - axis[1][0])*root;
00125   }
00126   else
00127   {
00128     // |w| <= 1/2
00129     int i = 0;
00130     if(axis[1][1] > axis[0][0])
00131     {
00132       i = 1;
00133     }
00134     if(axis[2][2] > axis[i][i])
00135     {
00136       i = 2;
00137     }
00138     int j = next[i];
00139     int k = next[j];
00140 
00141     root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0);
00142     FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
00143     *quat[i] = 0.5 * root;
00144     root = 0.5 / root;
00145     data[0] = (axis[j][k] - axis[k][j]) * root;
00146     *quat[j] = (axis[i][j] + axis[j][i]) * root;
00147     *quat[k] = (axis[i][k] + axis[k][i]) * root;
00148   }
00149 }
00150 
00151 void Quaternion3f::toAxes(Vec3f axis[3]) const
00152 {
00153   FCL_REAL twoX  = 2.0*data[1];
00154   FCL_REAL twoY  = 2.0*data[2];
00155   FCL_REAL twoZ  = 2.0*data[3];
00156   FCL_REAL twoWX = twoX*data[0];
00157   FCL_REAL twoWY = twoY*data[0];
00158   FCL_REAL twoWZ = twoZ*data[0];
00159   FCL_REAL twoXX = twoX*data[1];
00160   FCL_REAL twoXY = twoY*data[1];
00161   FCL_REAL twoXZ = twoZ*data[1];
00162   FCL_REAL twoYY = twoY*data[2];
00163   FCL_REAL twoYZ = twoZ*data[2];
00164   FCL_REAL twoZZ = twoZ*data[3];
00165 
00166   axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
00167   axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
00168   axis[2].setValue(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
00169 }
00170 
00171 
00172 void Quaternion3f::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
00173 {
00174   FCL_REAL half_angle = 0.5 * angle;
00175   FCL_REAL sn = sin((double)half_angle);
00176   data[0] = cos((double)half_angle);
00177   data[1] = sn * axis[0];
00178   data[2] = sn * axis[1];
00179   data[3] = sn * axis[2];
00180 }
00181 
00182 void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
00183 {
00184   double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
00185   if(sqr_length > 0)
00186   {
00187     angle = 2.0 * acos((double)data[0]);
00188     double inv_length = 1.0 / sqrt(sqr_length);
00189     axis[0] = inv_length * data[1];
00190     axis[1] = inv_length * data[2];
00191     axis[2] = inv_length * data[3];
00192   }
00193   else
00194   {
00195     angle = 0;
00196     axis[0] = 1;
00197     axis[1] = 0;
00198     axis[2] = 0;
00199   }
00200 }
00201 
00202 FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const
00203 {
00204   return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
00205 }
00206 
00207 Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const
00208 {
00209   return Quaternion3f(data[0] + other.data[0], data[1] + other.data[1],
00210                       data[2] + other.data[2], data[3] + other.data[3]);
00211 }
00212 
00213 const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
00214 {
00215   data[0] += other.data[0];
00216   data[1] += other.data[1];
00217   data[2] += other.data[2];
00218   data[3] += other.data[3];
00219 
00220   return *this;
00221 }
00222 
00223 Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const
00224 {
00225   return Quaternion3f(data[0] - other.data[0], data[1] - other.data[1],
00226                       data[2] - other.data[2], data[3] - other.data[3]);
00227 }
00228 
00229 const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
00230 {
00231   data[0] -= other.data[0];
00232   data[1] -= other.data[1];
00233   data[2] -= other.data[2];
00234   data[3] -= other.data[3];
00235 
00236   return *this;
00237 }
00238 
00239 Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const
00240 {
00241   return Quaternion3f(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
00242                       data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
00243                       data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
00244                       data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
00245 }
00246 
00247 
00248 const Quaternion3f& Quaternion3f::operator *= (const Quaternion3f& other)
00249 {
00250   FCL_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
00251   FCL_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
00252   FCL_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1];
00253   FCL_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0];
00254 
00255   data[0] = a;
00256   data[1] = b;
00257   data[2] = c;
00258   data[3] = d;
00259   return *this;
00260 }
00261 
00262 Quaternion3f Quaternion3f::operator - () const
00263 {
00264   return Quaternion3f(-data[0], -data[1], -data[2], -data[3]);
00265 }
00266 
00267 Quaternion3f Quaternion3f::operator * (FCL_REAL t) const
00268 {
00269   return Quaternion3f(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
00270 }
00271 
00272 const Quaternion3f& Quaternion3f::operator *= (FCL_REAL t)
00273 {
00274   data[0] *= t;
00275   data[1] *= t;
00276   data[2] *= t;
00277   data[3] *= t;
00278 
00279   return *this;
00280 }
00281 
00282 
00283 Quaternion3f& Quaternion3f::conj()
00284 {
00285   data[1] = -data[1];
00286   data[2] = -data[2];
00287   data[3] = -data[3];
00288   return *this;
00289 }
00290 
00291 Quaternion3f& Quaternion3f::inverse()
00292 {
00293   FCL_REAL sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
00294   if(sqr_length > 0)
00295   {
00296     FCL_REAL inv_length = 1 / std::sqrt(sqr_length);
00297     data[0] *= inv_length;
00298     data[1] *= (-inv_length);
00299     data[2] *= (-inv_length);
00300     data[3] *= (-inv_length);
00301   }
00302   else
00303   {
00304     data[1] = -data[1];
00305     data[2] = -data[2];
00306     data[3] = -data[3];
00307   }
00308 
00309   return *this;
00310 }
00311 
00312 Vec3f Quaternion3f::transform(const Vec3f& v) const
00313 {
00314   Quaternion3f r = (*this) * Quaternion3f(0, v[0], v[1], v[2]) * (fcl::conj(*this));
00315   return Vec3f(r.data[1], r.data[2], r.data[3]);
00316 }
00317 
00318 Quaternion3f conj(const Quaternion3f& q)
00319 {
00320   Quaternion3f r(q);
00321   return r.conj();
00322 }
00323 
00324 Quaternion3f inverse(const Quaternion3f& q)
00325 {
00326   Quaternion3f res(q);
00327   return res.inverse();
00328 }
00329 
00330 Vec3f Quaternion3f::getColumn(std::size_t i) const
00331 {
00332   switch(i)
00333   {
00334   case 0:
00335     return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
00336                  2 * (- data[0] * data[3] + data[1] * data[2]),
00337                  2 * (data[1] * data[3] + data[0] * data[2]));
00338   case 1:
00339     return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]),
00340                  data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
00341                  2 * (data[2] * data[3] - data[0] * data[1]));
00342   case 2:
00343     return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]),
00344                  2 * (data[2] * data[3] + data[0] * data[1]),
00345                  data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
00346   default:
00347     return Vec3f();
00348   }
00349 }
00350 
00351 Vec3f Quaternion3f::getRow(std::size_t i) const
00352 {
00353   switch(i)
00354   {
00355   case 0:
00356     return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
00357                  2 * (data[0] * data[3] + data[1] * data[2]),
00358                  2 * (data[1] * data[3] - data[0] * data[2]));
00359   case 1:
00360     return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]),
00361                  data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
00362                  2 * (data[2] * data[3] + data[0] * data[1]));
00363   case 2:
00364     return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]),
00365                  2 * (data[2] * data[3] - data[0] * data[1]),
00366                  data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
00367   default:
00368     return Vec3f();
00369   }
00370 }
00371 
00372 
00373 const Matrix3f& Transform3f::getRotationInternal() const
00374 {
00375   boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));
00376   if(!matrix_set)
00377   {
00378     q.toRotation(R);
00379     matrix_set = true;
00380   }
00381 
00382   return R;
00383 }
00384 
00385 
00386 Transform3f inverse(const Transform3f& tf)
00387 {
00388   Transform3f res(tf);
00389   return res.inverse();
00390 }
00391 
00392 void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
00393                        Transform3f& tf)
00394 {
00395   const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
00396   tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
00397 }
00398 
00399 
00400 
00401 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


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