graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Andrew T. Miller and Matei T. Ciocarlie
00021 //
00022 // $Id: matvec.cpp,v 1.12 2009/04/08 15:25:52 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00029 #include "matvec3D.h"
00030 
00031 //#define GRASPITDBG
00032 #include "debug.h"
00033 
00034 #ifdef USE_DMALLOC
00035 #include "dmalloc.h"
00036 #endif
00037 
00038 //#define GCC22
00039 
00040 #define BADCONFIG                                     \
00041 {                                                     \
00042   pr_error("not a valid configuration file!");        \
00043   fclose(fp);                                         \
00044   return FAILURE;                                     \
00045 }
00046 
00047 const vec3 vec3::ZERO(0, 0, 0);
00048 const vec3 vec3::X(1, 0, 0);
00049 const vec3 vec3::Y(0, 1, 0);
00050 const vec3 vec3::Z(0, 0, 1);
00051 const position position::ORIGIN(0, 0, 0);
00052 const mat3 mat3::ZERO(vec3::ZERO,vec3::ZERO,vec3::ZERO);
00053 const mat3 mat3::IDENTITY(vec3(1, 0, 0), vec3(0, 1, 0), vec3(0, 0, 1));
00054 const Quaternion Quaternion::IDENTITY(1,0,0,0);
00055 const transf transf::IDENTITY(Quaternion::IDENTITY,vec3::ZERO);
00056 
00060 void
00061 vec3::perpVectors(vec3 &v1,vec3 &v2)
00062 {
00063   double k;
00064   if (fabs(vec[2]) > M_SQRT1_2) {
00065     // choose v1 in y-z plane
00066     k = 1.0/sqrt(vec[1]*vec[1] + vec[2]*vec[2]);
00067     v1[0] = 0;
00068     v1[1] = -vec[2]*k;
00069     v1[2] = vec[1]*k;
00070   }
00071   else {
00072     // choose v1 in x-y plane
00073     k = 1.0/sqrt(vec[0]*vec[0] + vec[1]*vec[1]);
00074     v1[0] = -vec[1]*k;
00075     v1[1] = vec[0]*k;
00076     v1[2] = 0;
00077   }
00078 
00079   v2 = *this * v1;
00080 }
00081 
00085 void
00086 mat3::ToEulerAngles(double &roll,double &pitch, double &yaw) const
00087 {
00088   double cosPitch;
00089   pitch = atan2(-mat[6],sqrt(mat[0]*mat[0]+mat[3]*mat[3]));
00090   if (pitch == M_PI/2.0) {
00091     yaw = 0.0;
00092     roll = atan2(mat[1],mat[4]);
00093   }
00094   else if (pitch == -M_PI/2.0) {
00095     yaw = 0.0;
00096     roll = -atan2(mat[1],mat[4]);
00097   }
00098   else {
00099     cosPitch = cos(pitch);
00100     yaw = atan2(mat[3]/cosPitch,mat[0]/cosPitch);
00101     roll = atan2(mat[7]/cosPitch,mat[8]/cosPitch);
00102   }
00103 }
00104 
00106 mat3
00107 mat3::inverse() const
00108 {
00109   double det, detInv;
00110   double M[9];
00111 
00112   if (fabs(det = determinant()) < 1.0e-12) return mat3::ZERO;
00113   detInv = 1 / det;
00114 
00115   M[0] = (mat[4] * mat[8] - mat[7] * mat[5]) * detInv;
00116   M[1] = (mat[7] * mat[2] - mat[1] * mat[8]) * detInv;
00117   M[2] = (mat[1] * mat[5] - mat[4] * mat[2]) * detInv;
00118   M[3] = (mat[6] * mat[5] - mat[3] * mat[8]) * detInv;
00119   M[4] = (mat[0] * mat[8] - mat[6] * mat[2]) * detInv;
00120   M[5] = (mat[3] * mat[2] - mat[0] * mat[5]) * detInv;
00121   M[6] = (mat[3] * mat[7] - mat[6] * mat[4]) * detInv;
00122   M[7] = (mat[6] * mat[1] - mat[0] * mat[7]) * detInv;
00123   M[8] = (mat[0] * mat[4] - mat[3] * mat[1]) * detInv;
00124 
00125   return mat3(M);
00126 }
00127 
00129 mat3 const&
00130 mat3::operator*=(const mat3 &m)
00131 {
00132   double M[9];
00133   memcpy(M,mat,9*sizeof(double));
00134 
00135   mat[0] = M[0]*m.mat[0] + M[3]*m.mat[1] + M[6]*m.mat[2]; 
00136   mat[1] = M[1]*m.mat[0] + M[4]*m.mat[1] + M[7]*m.mat[2]; 
00137   mat[2] = M[2]*m.mat[0] + M[5]*m.mat[1] + M[8]*m.mat[2]; 
00138 
00139   mat[3] = M[0]*m.mat[3] + M[3]*m.mat[4] + M[6]*m.mat[5]; 
00140   mat[4] = M[1]*m.mat[3] + M[4]*m.mat[4] + M[7]*m.mat[5]; 
00141   mat[5] = M[2]*m.mat[3] + M[5]*m.mat[4] + M[8]*m.mat[5]; 
00142   
00143   mat[6] = M[0]*m.mat[6] + M[3]*m.mat[7] + M[6]*m.mat[8]; 
00144   mat[7] = M[1]*m.mat[6] + M[4]*m.mat[7] + M[7]*m.mat[8]; 
00145   mat[8] = M[2]*m.mat[6] + M[5]*m.mat[7] + M[8]*m.mat[8]; 
00146   return *this;
00147 }
00148 
00150 mat3
00151 operator*(const mat3 &m1, const mat3 &m2)
00152 {
00153   double M[9];
00154 
00155   M[0] = m1.mat[0]*m2.mat[0] + m1.mat[3]*m2.mat[1] + m1.mat[6]*m2.mat[2]; 
00156   M[1] = m1.mat[1]*m2.mat[0] + m1.mat[4]*m2.mat[1] + m1.mat[7]*m2.mat[2]; 
00157   M[2] = m1.mat[2]*m2.mat[0] + m1.mat[5]*m2.mat[1] + m1.mat[8]*m2.mat[2]; 
00158 
00159   M[3] = m1.mat[0]*m2.mat[3] + m1.mat[3]*m2.mat[4] + m1.mat[6]*m2.mat[5]; 
00160   M[4] = m1.mat[1]*m2.mat[3] + m1.mat[4]*m2.mat[4] + m1.mat[7]*m2.mat[5]; 
00161   M[5] = m1.mat[2]*m2.mat[3] + m1.mat[5]*m2.mat[4] + m1.mat[8]*m2.mat[5]; 
00162 
00163   M[6] = m1.mat[0]*m2.mat[6] + m1.mat[3]*m2.mat[7] + m1.mat[6]*m2.mat[8]; 
00164   M[7] = m1.mat[1]*m2.mat[6] + m1.mat[4]*m2.mat[7] + m1.mat[7]*m2.mat[8]; 
00165   M[8] = m1.mat[2]*m2.mat[6] + m1.mat[5]*m2.mat[7] + m1.mat[8]*m2.mat[8]; 
00166 
00167   return mat3(M);
00168 }
00169 
00184 void
00185 transf::jacobian(double jac[])
00186 {
00187   mat3 m = affine();
00188   vec3 p = translation();
00189 
00190   // first 3 columns of jac
00191   jac[0]  = m.element(0,0); jac[6]  = m.element(0,1); jac[12] = m.element(0,2);
00192   jac[1]  = m.element(1,0); jac[7]  = m.element(1,1); jac[13] = m.element(1,2);
00193   jac[2]  = m.element(2,0); jac[8]  = m.element(2,1); jac[14] = m.element(2,2);
00194   jac[3]  = 0.0;            jac[9]  = 0.0;            jac[15] = 0.0;
00195   jac[4]  = 0.0;            jac[10] = 0.0;            jac[16] = 0.0;
00196   jac[5]  = 0.0;            jac[11] = 0.0;            jac[17] = 0.0;
00197 
00198   // 4th column of jac
00199   jac[18] = p.y()*m.element(0,2) - p.z()*m.element(0,1);
00200   jac[19] = p.y()*m.element(1,2) - p.z()*m.element(1,1);
00201   jac[20] = p.y()*m.element(2,2) - p.z()*m.element(2,1);
00202   jac[21] = m.element(0,0);
00203   jac[22] = m.element(1,0);
00204   jac[23] = m.element(2,0);
00205 
00206   // 5th column of jac
00207   jac[24] = p.z()*m.element(0,0) - p.x()*m.element(0,2);
00208   jac[25] = p.z()*m.element(1,0) - p.x()*m.element(1,2);
00209   jac[26] = p.z()*m.element(2,0) - p.x()*m.element(2,2);
00210   jac[27] = m.element(0,1);
00211   jac[28] = m.element(1,1);
00212   jac[29] = m.element(2,1);
00213 
00214   // 6th column of jac
00215   jac[30] = p.x()*m.element(0,1) - p.y()*m.element(0,0);
00216   jac[31] = p.x()*m.element(1,1) - p.y()*m.element(1,0);
00217   jac[32] = p.x()*m.element(2,1) - p.y()*m.element(2,0);
00218   jac[33] = m.element(0,2);
00219   jac[34] = m.element(1,2);
00220   jac[35] = m.element(2,2);
00221 }
00222 
00223 //---------------------------------------------------------------------------
00227 void
00228 Quaternion::set(const mat3 &R)
00229 {
00230     // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00231     // article "Quaternion Calculus and Fast Animation".
00232 
00233     double trace = R.element(0,0)+R.element(1,1)+R.element(2,2);
00234     double root;
00235 
00236     if ( trace > 0.0 )
00237     {
00238         // |w| > 1/2, may as well choose w > 1/2
00239         root = sqrt(trace+1.0);  // 2w
00240         w = 0.5*root;
00241         root = 0.5/root;  // 1/(4w)
00242         x = (R.element(1,2)-R.element(2,1))*root;
00243         y = (R.element(2,0)-R.element(0,2))*root;
00244         z = (R.element(0,1)-R.element(1,0))*root;
00245     }
00246     else
00247     {
00248         // |w| <= 1/2
00249         static int next[3] = { 1, 2, 0 };
00250         int i = 0;
00251         if ( R.element(1,1) > R.element(0,0) )
00252             i = 1;
00253         if ( R.element(2,2) > R.element(i,i) )
00254             i = 2;
00255         int j = next[i];
00256         int k = next[j];
00257 
00258         root = sqrt(R.element(i,i)-R.element(j,j)-R.element(k,k)+1.0);
00259         double* quat[3] = { &x, &y, &z };
00260         *quat[i] = 0.5*root;
00261         root = 0.5/root;
00262         w = (R.element(j,k)-R.element(k,j))*root;
00263         *quat[j] = (R.element(i,j)+R.element(j,i))*root;
00264         *quat[k] = (R.element(i,k)+R.element(k,i))*root;
00265     }
00266     normalise();
00267 }
00268 
00273 void
00274 Quaternion::set(const double& angle, const vec3& axis)
00275 {
00276     // assert:  axis[] is unit length
00277     //
00278         // The quaternion representing the rotation is
00279         //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
00280 
00281     double halfAngle = 0.5*angle;
00282     double sn = sin(halfAngle);
00283     w = cos(halfAngle);
00284     x = sn*axis.x();
00285     y = sn*axis.y();
00286     z = sn*axis.z();
00287     normalise();
00288 }
00289 
00293 void
00294 Quaternion::set(const SbRotation &SbRot)
00295 {
00296   w = (double)SbRot.getValue()[3];
00297   x = (double)SbRot.getValue()[0];
00298   y = (double)SbRot.getValue()[1];
00299   z = (double)SbRot.getValue()[2];
00300   normalise();
00301 }
00302 
00306 void
00307 Quaternion::ToRotationMatrix(mat3 &R) const
00308 {
00309     double tx  = 2.0*x;
00310     double ty  = 2.0*y;
00311     double tz  = 2.0*z;
00312     double twx = tx*w;
00313     double twy = ty*w;
00314     double twz = tz*w;
00315     double txx = tx*x;
00316     double txy = ty*x;
00317     double txz = tz*x;
00318     double tyy = ty*y;
00319     double tyz = tz*y;
00320     double tzz = tz*z;
00321 
00322     R.element(0,0) = 1.0-(tyy+tzz);
00323     R.element(1,0) = txy-twz;
00324     R.element(2,0) = txz+twy;
00325     R.element(0,1) = txy+twz;
00326     R.element(1,1) = 1.0-(txx+tzz);
00327     R.element(2,1) = tyz-twx;
00328     R.element(0,2) = txz-twy;
00329     R.element(1,2) = tyz+twx;
00330     R.element(2,2) = 1.0-(txx+tyy);
00331 }
00332 
00336 void
00337 Quaternion::ToAngleAxis(double& angle, vec3& axis) const
00338 {
00339         // The quaternion representing the rotation is
00340         //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
00341 
00342     double length2 = x*x+y*y+z*z;
00343     if ( length2 > 1.0e-15 )
00344     {
00345         angle = 2.0*acos(w);
00346         double invlen = 1.0/sqrt(length2);
00347         axis[0] = x*invlen;
00348         axis[1] = y*invlen;
00349         axis[2] = z*invlen;
00350     }
00351     else
00352     {
00353         // angle is 0 (mod 2*pi), so any axis will do
00354         angle = 0.0;
00355         axis[0] = 1.0;
00356         axis[1] = 0.0;
00357         axis[2] = 0.0;
00358     }
00359 }
00360 
00366 Quaternion
00367 Quaternion::Slerp (double t, const Quaternion& p,
00368                    const Quaternion& q)
00369 {
00370   Quaternion q1;
00371   double sn,c0,c1,angle;
00372   double cs;
00373   
00374   if (t == 0.0) return p;
00375   if (t == 1.0) return q;
00376 
00377   cs = p % q;
00378 
00379   if (cs < 0.0) {
00380     cs = -cs;
00381     q1 = -q;
00382   }
00383   else q1 = q;
00384 
00385   // calculate interpolating coeffs
00386   if ((1.0 - cs) > 0.00001) {
00387     // standard case
00388     angle = acos(cs);
00389     sn = sin(angle);
00390     c0 = sin((1.0 - t) * angle) / sn;
00391     c1 = sin(t * angle) / sn;
00392   } else {        
00393     // q0 and q1 very close - just do linear interp.
00394     c0 = 1.0 - t;
00395     c1 = t;
00396   }
00397 
00398   return Quaternion(c0*p.w + c1*q1.w,c0*p.x + c1*q1.x,c0*p.y + c1*q1.y,
00399                     c0*p.z + c1*q1.z);
00400 }
00401 
00402 
00406 std::istream&
00407 operator>>(std::istream &is, vec3 &v)
00408 {
00409   char ch;
00410   double x,y,z;
00411   if (is >> ch && ch == '[' && is >> x && is >> y && is >> z &&
00412       is >> ch && ch == ']') {
00413     v.vec[0] = x; v.vec[1] = y; v.vec[2] = z;
00414   }
00415 //  else
00416 //    is.setstate(ios::failbit);
00417   
00418   return is;
00419 }
00420 
00424 std::ostream &
00425 operator<<(std::ostream &os, const vec3 &v)
00426 {
00427 #ifdef WIN32
00428   int oldFlags = os.setf(std::ios_base::showpos);
00429 #else
00430 #ifdef GCC22
00431  int oldFlags = os.setf(ios::showpos);
00432 #else
00433   std::_Ios_Fmtflags oldFlags = os.setf(std::ios_base::showpos);
00434 #endif 
00435 #endif
00436   os << '[' << v.vec[0] << ' ' << v.vec[1] << ' ' << v.vec[2] << ']';
00437   os.flags(oldFlags);
00438   return os;
00439 }
00440 
00444 std::istream&
00445 operator>>(std::istream &is, position &p)
00446 {
00447   char ch;
00448   double x,y,z;
00449   if (is >> ch && ch == '[' && is >> x && is >> y && is >> z &&
00450       is >> ch && ch == ']') {
00451     p.pos[0] = x; p.pos[1] = y; p.pos[2] = z;
00452   } 
00453 //  else
00454 //    is.setstate(ios::failbit);
00455 
00456   return is;
00457 }
00458 
00461 std::ostream &
00462 operator<<(std::ostream &os, const position &p)
00463 {
00464 #ifdef WIN32
00465   int oldFlags = os.setf(std::ios_base::showpos);
00466 #else
00467 #ifdef GCC22
00468  int oldFlags = os.setf(ios::showpos);
00469 #else
00470   std::_Ios_Fmtflags oldFlags = os.setf(std::ios_base::showpos);
00471 #endif 
00472 #endif
00473   os << '[' << p.pos[0] << ' ' << p.pos[1] << ' ' << p.pos[2] << ']';
00474   os.flags(oldFlags);
00475   return os;
00476 }
00477 
00479 std::istream &
00480 operator>>(std::istream &is, mat3 &m)
00481 {
00482   is >> m.mat[0] >> m.mat[3] >> m.mat[6];
00483   is >> m.mat[1] >> m.mat[4] >> m.mat[7];
00484   is >> m.mat[2] >> m.mat[5] >> m.mat[8];
00485   return is;
00486 }
00487 
00496 std::ostream &
00497 operator<<(std::ostream &os, const mat3 &m)
00498 {
00499 #ifdef WIN32
00500   int oldFlags = os.setf(std::ios_base::showpos);
00501 #else
00502 #ifdef GCC22
00503  int oldFlags = os.setf(ios::showpos);
00504 #else
00505   std::_Ios_Fmtflags oldFlags = os.setf(std::ios_base::showpos);
00506 #endif
00507 #endif
00508   os << '[' << m.mat[0] << ' ' << m.mat[3] << ' ' << m.mat[6]<< ']'<<std::endl;
00509   os << '[' << m.mat[1] << ' ' << m.mat[4] << ' ' << m.mat[7]<< ']'<<std::endl;
00510   os << '[' << m.mat[2] << ' ' << m.mat[5] << ' ' << m.mat[8]<< ']'<<std::endl;
00511   os.flags(oldFlags);
00512   return os;
00513 }
00514 
00519 std::istream &
00520 operator>>(std::istream &is, Quaternion &q)
00521 {
00522   char ch;
00523   double w,x,y,z;
00524   if (is >> ch && ch == '(' && is >> w && is >> x && is >> y && is >> z &&
00525       is >> ch && ch == ')') {
00526     q.w = w; q.x = x; q.y = y; q.z = z;
00527   } 
00528 //  else
00529 //    is.setstate(ios::failbit);
00530   
00531   return is;
00532 }
00533 
00538 std::ostream&
00539 operator<<(std::ostream &os, const Quaternion &q)
00540 {
00541 #ifdef WIN32
00542   int oldFlags = os.setf(std::ios_base::showpos);
00543 #else
00544 #ifdef GCC22
00545  int oldFlags = os.setf(ios::showpos);
00546 #else
00547   std::_Ios_Fmtflags oldFlags = os.setf(std::ios_base::showpos);
00548 #endif
00549 #endif 
00550   os <<'('<< q.w <<' '<< q.x <<' '<< q.y <<' '<< q.z <<')';
00551   os.flags(oldFlags);
00552   return os;
00553 }
00554 
00559 std::istream&
00560 operator>>(std::istream &is, transf &tr)
00561 {
00562   Quaternion q;
00563   vec3 v;
00564 
00565   if (is >> q && is >> v)
00566     tr.set(q,v);
00567 //  else
00568 //    is.setstate(ios::failbit);
00569 
00570   return is;
00571 }
00572 
00577 std::ostream &
00578 operator<<(std::ostream &os, const transf &tr)
00579 {
00580   return os << tr.rot << tr.t;
00581 }
00582 
00583 void FlockTransf::identity()
00584 {
00585         mount = transf::IDENTITY;
00586         mountInv = transf::IDENTITY;
00587         flockBaseInv = transf::IDENTITY;
00588         objBase = transf::IDENTITY;
00589 }
00590 
00591 transf FlockTransf::get(transf t) const
00592 {
00593         return mountInv * t * flockBaseInv * mount * objBase;
00594 }
00595 
00596 transf FlockTransf::getAbsolute(transf t) const
00597 {
00598         return mountInv * t;
00599 }
00600 
00605 transf FlockTransf::get2(transf t) const
00606 {
00607         return mountInv * t * flockBaseInv * objBase;
00608 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/matvec.cpp
