irb_2400_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 // check if the included ikfast version matches what this file was compiled with
00025 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00026 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00027 
00028 #include <cmath>
00029 #include <vector>
00030 #include <limits>
00031 #include <algorithm>
00032 #include <complex>
00033 
00034 #define IKFAST_STRINGIZE2(s) #s
00035 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
00036 
00037 #ifndef IKFAST_ASSERT
00038 #include <stdexcept>
00039 #include <sstream>
00040 #include <iostream>
00041 
00042 #ifdef _MSC_VER
00043 #ifndef __PRETTY_FUNCTION__
00044 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00045 #endif
00046 #endif
00047 
00048 #ifndef __PRETTY_FUNCTION__
00049 #define __PRETTY_FUNCTION__ __func__
00050 #endif
00051 
00052 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
00053 
00054 #endif
00055 
00056 #if defined(_MSC_VER)
00057 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00058 #else
00059 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00060 #endif
00061 
00062 #define IK2PI  ((IkReal)6.28318530717959)
00063 #define IKPI  ((IkReal)3.14159265358979)
00064 #define IKPI_2  ((IkReal)1.57079632679490)
00065 
00066 #ifdef _MSC_VER
00067 #ifndef isnan
00068 #define isnan _isnan
00069 #endif
00070 #endif // _MSC_VER
00071 
00072 // lapack routines
00073 extern "C" {
00074   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00075   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00076   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00077   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00078   void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
00079   void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
00080 }
00081 
00082 using namespace std; // necessary to get std math routines
00083 
00084 #ifdef IKFAST_NAMESPACE
00085 namespace IKFAST_NAMESPACE {
00086 #endif
00087 
00088 inline float IKabs(float f) { return fabsf(f); }
00089 inline double IKabs(double f) { return fabs(f); }
00090 
00091 inline float IKsqr(float f) { return f*f; }
00092 inline double IKsqr(double f) { return f*f; }
00093 
00094 inline float IKlog(float f) { return logf(f); }
00095 inline double IKlog(double f) { return log(f); }
00096 
00097 // allows asin and acos to exceed 1
00098 #ifndef IKFAST_SINCOS_THRESH
00099 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00100 #endif
00101 
00102 // used to check input to atan2 for degenerate cases
00103 #ifndef IKFAST_ATAN2_MAGTHRESH
00104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00105 #endif
00106 
00107 // minimum distance of separate solutions
00108 #ifndef IKFAST_SOLUTION_THRESH
00109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00110 #endif
00111 
00112 inline float IKasin(float f)
00113 {
00114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00115 if( f <= -1 ) return float(-IKPI_2);
00116 else if( f >= 1 ) return float(IKPI_2);
00117 return asinf(f);
00118 }
00119 inline double IKasin(double f)
00120 {
00121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00122 if( f <= -1 ) return -IKPI_2;
00123 else if( f >= 1 ) return IKPI_2;
00124 return asin(f);
00125 }
00126 
00127 // return positive value in [0,y)
00128 inline float IKfmod(float x, float y)
00129 {
00130     while(x < 0) {
00131         x += y;
00132     }
00133     return fmodf(x,y);
00134 }
00135 
00136 // return positive value in [0,y)
00137 inline double IKfmod(double x, double y)
00138 {
00139     while(x < 0) {
00140         x += y;
00141     }
00142     return fmod(x,y);
00143 }
00144 
00145 inline float IKacos(float f)
00146 {
00147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00148 if( f <= -1 ) return float(IKPI);
00149 else if( f >= 1 ) return float(0);
00150 return acosf(f);
00151 }
00152 inline double IKacos(double f)
00153 {
00154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00155 if( f <= -1 ) return IKPI;
00156 else if( f >= 1 ) return 0;
00157 return acos(f);
00158 }
00159 inline float IKsin(float f) { return sinf(f); }
00160 inline double IKsin(double f) { return sin(f); }
00161 inline float IKcos(float f) { return cosf(f); }
00162 inline double IKcos(double f) { return cos(f); }
00163 inline float IKtan(float f) { return tanf(f); }
00164 inline double IKtan(double f) { return tan(f); }
00165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00167 inline float IKatan2(float fy, float fx) {
00168     if( isnan(fy) ) {
00169         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00170         return float(IKPI_2);
00171     }
00172     else if( isnan(fx) ) {
00173         return 0;
00174     }
00175     return atan2f(fy,fx);
00176 }
00177 inline double IKatan2(double fy, double fx) {
00178     if( isnan(fy) ) {
00179         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00180         return IKPI_2;
00181     }
00182     else if( isnan(fx) ) {
00183         return 0;
00184     }
00185     return atan2(fy,fx);
00186 }
00187 
00188 inline float IKsign(float f) {
00189     if( f > 0 ) {
00190         return float(1);
00191     }
00192     else if( f < 0 ) {
00193         return float(-1);
00194     }
00195     return 0;
00196 }
00197 
00198 inline double IKsign(double f) {
00199     if( f > 0 ) {
00200         return 1.0;
00201     }
00202     else if( f < 0 ) {
00203         return -1.0;
00204     }
00205     return 0;
00206 }
00207 
00210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00211 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47;
00212 x0=IKcos(j[0]);
00213 x1=IKcos(j[1]);
00214 x2=IKsin(j[2]);
00215 x3=IKcos(j[2]);
00216 x4=IKsin(j[1]);
00217 x5=IKcos(j[4]);
00218 x6=IKsin(j[4]);
00219 x7=IKsin(j[3]);
00220 x8=IKcos(j[3]);
00221 x9=IKsin(j[0]);
00222 x10=IKcos(j[5]);
00223 x11=IKsin(j[5]);
00224 x12=((IkReal(0.0850000000000000))*(x8));
00225 x13=((IkReal(0.135000000000000))*(x9));
00226 x14=((IkReal(0.135000000000000))*(x0));
00227 x15=((IkReal(1.00000000000000))*(x5));
00228 x16=((IkReal(1.00000000000000))*(x0));
00229 x17=((IkReal(1.00000000000000))*(x9));
00230 x18=((IkReal(0.755000000000000))*(x0));
00231 x19=((IkReal(0.0850000000000000))*(x9));
00232 x20=((IkReal(0.0850000000000000))*(x0));
00233 x21=((IkReal(0.755000000000000))*(x9));
00234 x22=((IkReal(0.705000000000000))*(x4));
00235 x23=((IkReal(1.00000000000000))*(x6));
00236 x24=((x1)*(x3));
00237 x25=((x3)*(x4));
00238 x26=((x7)*(x9));
00239 x27=((x2)*(x4));
00240 x28=((x1)*(x2));
00241 x29=((x6)*(x8));
00242 x30=((x17)*(x8));
00243 x31=((IkReal(1.00000000000000))*(x24));
00244 x32=((x16)*(x27));
00245 x33=((x17)*(x27));
00246 x34=((((IkReal(-1.00000000000000))*(x31)))+(x27));
00247 x35=((((IkReal(1.00000000000000))*(x25)))+(((IkReal(1.00000000000000))*(x28))));
00248 x36=((x34)*(x8));
00249 x37=((x16)*(((x25)+(x28))));
00250 x38=((IkReal(-1.00000000000000))*(x37));
00251 x39=((x17)*(((x25)+(x28))));
00252 x40=((IkReal(-1.00000000000000))*(x39));
00253 x41=((x37)*(x7));
00254 x42=((x39)*(x7));
00255 x43=((((IkReal(-1.00000000000000))*(x17)*(x7)))+(((x38)*(x8))));
00256 x44=((((x0)*(x7)))+(((x40)*(x8))));
00257 x45=((((IkReal(-1.00000000000000))*(x23)*(x35)))+(((IkReal(-1.00000000000000))*(x15)*(x36))));
00258 x46=((((IkReal(-1.00000000000000))*(x15)*(x43)))+(((x23)*(((((IkReal(-1.00000000000000))*(x32)))+(((x16)*(x24))))))));
00259 x47=((((IkReal(-1.00000000000000))*(x15)*(x44)))+(((x23)*(((((IkReal(-1.00000000000000))*(x33)))+(((x17)*(x24))))))));
00260 eerot[0]=((((x5)*(((((IkReal(-1.00000000000000))*(x32)))+(((x0)*(x24)))))))+(((x43)*(x6))));
00261 eerot[1]=((((x11)*(x46)))+(((x10)*(((((IkReal(-1.00000000000000))*(x30)))+(x41))))));
00262 eerot[2]=((((x10)*(x46)))+(((x11)*(((((IkReal(-1.00000000000000))*(x41)))+(x30))))));
00263 IkReal x48=((IkReal(1.00000000000000))*(x27));
00264 eetrans[0]=((((x6)*(((((IkReal(-1.00000000000000))*(x19)*(x7)))+(((x12)*(x38)))))))+(((x18)*(x24)))+(((x0)*(x22)))+(((IkReal(0.100000000000000))*(x0)))+(((IkReal(-1.00000000000000))*(x18)*(x48)))+(((x14)*(x28)))+(((x14)*(x25)))+(((x5)*(((((IkReal(-1.00000000000000))*(x20)*(x48)))+(((x20)*(x24))))))));
00265 eerot[3]=((((x5)*(((((IkReal(-1.00000000000000))*(x33)))+(((x24)*(x9)))))))+(((x44)*(x6))));
00266 eerot[4]=((((x11)*(x47)))+(((x10)*(((((x0)*(x8)))+(x42))))));
00267 eerot[5]=((((x11)*(((((IkReal(-1.00000000000000))*(x42)))+(((IkReal(-1.00000000000000))*(x16)*(x8)))))))+(((x10)*(x47))));
00268 IkReal x49=((IkReal(1.00000000000000))*(x27));
00269 eetrans[1]=((((x5)*(((((x19)*(x24)))+(((IkReal(-1.00000000000000))*(x19)*(x49)))))))+(((x21)*(x24)))+(((x22)*(x9)))+(((IkReal(0.100000000000000))*(x9)))+(((IkReal(-1.00000000000000))*(x21)*(x49)))+(((x6)*(((((x12)*(x40)))+(((x20)*(x7)))))))+(((x13)*(x28)))+(((x13)*(x25))));
00270 eerot[6]=((((x29)*(x34)))+(((IkReal(-1.00000000000000))*(x35)*(x5))));
00271 eerot[7]=((((x10)*(x7)*(((((IkReal(-1.00000000000000))*(x27)))+(x31)))))+(((x11)*(x45))));
00272 eerot[8]=((((x10)*(x45)))+(((x11)*(x34)*(x7))));
00273 eetrans[2]=((IkReal(0.615000000000000))+(((x5)*(((((IkReal(-0.0850000000000000))*(x28)))+(((IkReal(-0.0850000000000000))*(x25)))))))+(((IkReal(-0.135000000000000))*(x27)))+(((IkReal(0.135000000000000))*(x24)))+(((IkReal(-0.755000000000000))*(x25)))+(((IkReal(-0.755000000000000))*(x28)))+(((IkReal(0.705000000000000))*(x1)))+(((x29)*(((((IkReal(0.0850000000000000))*(x27)))+(((IkReal(-0.0850000000000000))*(x24))))))));
00274 }
00275 
00276 IKFAST_API int GetNumFreeParameters() { return 0; }
00277 IKFAST_API int* GetFreeParameters() { return NULL; }
00278 IKFAST_API int GetNumJoints() { return 6; }
00279 
00280 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00281 
00282 IKFAST_API int GetIkType() { return 0x67000001; }
00283 
00284 class IKSolver {
00285 public:
00286 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
00287 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
00288 
00289 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00290 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; 
00291 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00292     solutions.Clear();
00293 r00 = eerot[0*3+0];
00294 r01 = eerot[0*3+1];
00295 r02 = eerot[0*3+2];
00296 r10 = eerot[1*3+0];
00297 r11 = eerot[1*3+1];
00298 r12 = eerot[1*3+2];
00299 r20 = eerot[2*3+0];
00300 r21 = eerot[2*3+1];
00301 r22 = eerot[2*3+2];
00302 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00303 
00304 new_r00=((IkReal(-1.00000000000000))*(r02));
00305 new_r01=r01;
00306 new_r02=r00;
00307 new_px=((px)+(((IkReal(-0.0850000000000000))*(r00))));
00308 new_r10=((IkReal(-1.00000000000000))*(r12));
00309 new_r11=r11;
00310 new_r12=r10;
00311 new_py=((((IkReal(-0.0850000000000000))*(r10)))+(py));
00312 new_r20=((IkReal(-1.00000000000000))*(r22));
00313 new_r21=r21;
00314 new_r22=r20;
00315 new_pz=((IkReal(-0.615000000000000))+(((IkReal(-0.0850000000000000))*(r20)))+(pz));
00316 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
00317 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00318 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00319 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00320 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00321 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00322 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00323 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00324 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00325 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00326 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00327 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00328 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00329 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00330 {
00331 IkReal j0array[2], cj0array[2], sj0array[2];
00332 bool j0valid[2]={false};
00333 _nj0 = 2;
00334 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
00335     continue;
00336 IkReal x50=IKatan2(((IkReal(-1.00000000000000))*(py)), px);
00337 j0array[0]=((IkReal(-1.00000000000000))*(x50));
00338 sj0array[0]=IKsin(j0array[0]);
00339 cj0array[0]=IKcos(j0array[0]);
00340 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x50))));
00341 sj0array[1]=IKsin(j0array[1]);
00342 cj0array[1]=IKcos(j0array[1]);
00343 if( j0array[0] > IKPI )
00344 {
00345     j0array[0]-=IK2PI;
00346 }
00347 else if( j0array[0] < -IKPI )
00348 {    j0array[0]+=IK2PI;
00349 }
00350 j0valid[0] = true;
00351 if( j0array[1] > IKPI )
00352 {
00353     j0array[1]-=IK2PI;
00354 }
00355 else if( j0array[1] < -IKPI )
00356 {    j0array[1]+=IK2PI;
00357 }
00358 j0valid[1] = true;
00359 for(int ij0 = 0; ij0 < 2; ++ij0)
00360 {
00361 if( !j0valid[ij0] )
00362 {
00363     continue;
00364 }
00365 _ij0[0] = ij0; _ij0[1] = -1;
00366 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
00367 {
00368 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00369 {
00370     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00371 }
00372 }
00373 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00374 
00375 {
00376 IkReal j2array[2], cj2array[2], sj2array[2];
00377 bool j2valid[2]={false};
00378 _nj2 = 2;
00379 if( (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(-0.924698002368864))*(pp))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(-0.924698002368864))*(pp))))) > 1+IKFAST_SINCOS_THRESH )
00380     continue;
00381 IkReal x51=IKasin(((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(-0.924698002368864))*(pp)))));
00382 j2array[0]=((IkReal(-2.96465459743209))+(((IkReal(-1.00000000000000))*(x51))));
00383 sj2array[0]=IKsin(j2array[0]);
00384 cj2array[0]=IKcos(j2array[0]);
00385 j2array[1]=((IkReal(0.176938056157703))+(x51));
00386 sj2array[1]=IKsin(j2array[1]);
00387 cj2array[1]=IKcos(j2array[1]);
00388 if( j2array[0] > IKPI )
00389 {
00390     j2array[0]-=IK2PI;
00391 }
00392 else if( j2array[0] < -IKPI )
00393 {    j2array[0]+=IK2PI;
00394 }
00395 j2valid[0] = true;
00396 if( j2array[1] > IKPI )
00397 {
00398     j2array[1]-=IK2PI;
00399 }
00400 else if( j2array[1] < -IKPI )
00401 {    j2array[1]+=IK2PI;
00402 }
00403 j2valid[1] = true;
00404 for(int ij2 = 0; ij2 < 2; ++ij2)
00405 {
00406 if( !j2valid[ij2] )
00407 {
00408     continue;
00409 }
00410 _ij2[0] = ij2; _ij2[1] = -1;
00411 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
00412 {
00413 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00414 {
00415     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00416 }
00417 }
00418 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00419 
00420 {
00421 IkReal dummyeval[1];
00422 IkReal gconst1;
00423 IkReal x52=((IkReal(0.135000000000000))*(sj2));
00424 IkReal x53=((py)*(sj0));
00425 IkReal x54=((cj0)*(px));
00426 IkReal x55=((IkReal(0.755000000000000))*(cj2));
00427 gconst1=IKsign(((((IkReal(-1.00000000000000))*(x53)*(x55)))+(((IkReal(-1.00000000000000))*(x52)*(x53)))+(((IkReal(-1.00000000000000))*(x52)*(x54)))+(((IkReal(0.135000000000000))*(cj2)*(pz)))+(((IkReal(0.0755000000000000))*(cj2)))+(((IkReal(0.0135000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(x54)*(x55)))+(((IkReal(0.705000000000000))*(pz)))+(((IkReal(-0.755000000000000))*(pz)*(sj2)))));
00428 IkReal x56=((IkReal(10.0000000000000))*(sj2));
00429 IkReal x57=((py)*(sj0));
00430 IkReal x58=((cj0)*(px));
00431 IkReal x59=((IkReal(55.9259259259259))*(cj2));
00432 dummyeval[0]=((((IkReal(-55.9259259259259))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x56)*(x58)))+(((IkReal(-1.00000000000000))*(x56)*(x57)))+(((IkReal(52.2222222222222))*(pz)))+(sj2)+(((IkReal(5.59259259259259))*(cj2)))+(((IkReal(-1.00000000000000))*(x58)*(x59)))+(((IkReal(10.0000000000000))*(cj2)*(pz)))+(((IkReal(-1.00000000000000))*(x57)*(x59))));
00433 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00434 {
00435 {
00436 IkReal dummyeval[1];
00437 IkReal gconst0;
00438 IkReal x60=((cj0)*(px));
00439 IkReal x61=((IkReal(0.755000000000000))*(sj2));
00440 IkReal x62=((py)*(sj0));
00441 IkReal x63=((IkReal(0.135000000000000))*(cj2));
00442 gconst0=IKsign(((IkReal(0.0705000000000000))+(((IkReal(-0.705000000000000))*(x62)))+(((IkReal(-0.705000000000000))*(x60)))+(((x60)*(x61)))+(((IkReal(-0.135000000000000))*(pz)*(sj2)))+(((x61)*(x62)))+(((IkReal(-0.0755000000000000))*(sj2)))+(((IkReal(-0.755000000000000))*(cj2)*(pz)))+(((IkReal(0.0135000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x60)*(x63)))+(((IkReal(-1.00000000000000))*(x62)*(x63)))));
00443 IkReal x64=((IkReal(55.9259259259259))*(sj2));
00444 IkReal x65=((cj0)*(px));
00445 IkReal x66=((py)*(sj0));
00446 IkReal x67=((IkReal(10.0000000000000))*(cj2));
00447 dummyeval[0]=((IkReal(5.22222222222222))+(((IkReal(-10.0000000000000))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x66)*(x67)))+(((IkReal(-55.9259259259259))*(cj2)*(pz)))+(cj2)+(((IkReal(-52.2222222222222))*(x66)))+(((IkReal(-52.2222222222222))*(x65)))+(((IkReal(-5.59259259259259))*(sj2)))+(((IkReal(-1.00000000000000))*(x65)*(x67)))+(((x64)*(x66)))+(((x64)*(x65))));
00448 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00449 {
00450 continue;
00451 
00452 } else
00453 {
00454 {
00455 IkReal j1array[1], cj1array[1], sj1array[1];
00456 bool j1valid[1]={false};
00457 _nj1 = 1;
00458 IkReal x68=(sj2)*(sj2);
00459 IkReal x69=(cj2)*(cj2);
00460 IkReal x70=((cj2)*(sj2));
00461 IkReal x71=((IkReal(1.00000000000000))*(pz));
00462 if( IKabs(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(0.203850000000000))*(x70)))+(((IkReal(-0.0182250000000000))*(x69)))+(((IkReal(-0.570025000000000))*(x68)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(-0.190350000000000))*(cj2))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(0.101925000000000))*(x68)))+(((IkReal(0.551800000000000))*(x70)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-0.101925000000000))*(x69)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x71)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x71)))+(((IkReal(-0.532275000000000))*(cj2))))))) < IKFAST_ATAN2_MAGTHRESH )
00463     continue;
00464 j1array[0]=IKatan2(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(0.203850000000000))*(x70)))+(((IkReal(-0.0182250000000000))*(x69)))+(((IkReal(-0.570025000000000))*(x68)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(-0.190350000000000))*(cj2)))))), ((gconst0)*(((((IkReal(0.101925000000000))*(x68)))+(((IkReal(0.551800000000000))*(x70)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-0.101925000000000))*(x69)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x71)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x71)))+(((IkReal(-0.532275000000000))*(cj2)))))));
00465 sj1array[0]=IKsin(j1array[0]);
00466 cj1array[0]=IKcos(j1array[0]);
00467 if( j1array[0] > IKPI )
00468 {
00469     j1array[0]-=IK2PI;
00470 }
00471 else if( j1array[0] < -IKPI )
00472 {    j1array[0]+=IK2PI;
00473 }
00474 j1valid[0] = true;
00475 for(int ij1 = 0; ij1 < 1; ++ij1)
00476 {
00477 if( !j1valid[ij1] )
00478 {
00479     continue;
00480 }
00481 _ij1[0] = ij1; _ij1[1] = -1;
00482 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00483 {
00484 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00485 {
00486     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00487 }
00488 }
00489 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00490 {
00491 IkReal evalcond[5];
00492 IkReal x72=IKsin(j1);
00493 IkReal x73=IKcos(j1);
00494 IkReal x74=((cj0)*(px));
00495 IkReal x75=((py)*(sj0));
00496 IkReal x76=((IkReal(0.755000000000000))*(sj2));
00497 IkReal x77=((IkReal(0.135000000000000))*(sj2));
00498 IkReal x78=((IkReal(0.755000000000000))*(cj2));
00499 IkReal x79=((IkReal(0.135000000000000))*(x72));
00500 IkReal x80=((IkReal(1.00000000000000))*(x73));
00501 IkReal x81=((cj2)*(x73));
00502 IkReal x82=((IkReal(1.41000000000000))*(x72));
00503 IkReal x83=((pz)*(x73));
00504 evalcond[0]=((IkReal(-0.705000000000000))+(((IkReal(-0.135000000000000))*(cj2)))+(((IkReal(-0.100000000000000))*(x72)))+(x76)+(x83)+(((x72)*(x74)))+(((x72)*(x75))));
00505 evalcond[1]=((((pz)*(x72)))+(((IkReal(0.100000000000000))*(x73)))+(x77)+(x78)+(((IkReal(-1.00000000000000))*(x74)*(x80)))+(((IkReal(-1.00000000000000))*(x75)*(x80))));
00506 evalcond[2]=((((IkReal(-0.135000000000000))*(x81)))+(pz)+(((x72)*(x78)))+(((x72)*(x77)))+(((x73)*(x76)))+(((IkReal(-0.705000000000000))*(x73))));
00507 evalcond[3]=((IkReal(0.0812250000000000))+(((IkReal(1.41000000000000))*(x83)))+(((IkReal(0.200000000000000))*(x74)))+(((IkReal(0.200000000000000))*(x75)))+(((x74)*(x82)))+(((IkReal(-1.00000000000000))*(pp)))+(((x75)*(x82)))+(((IkReal(-0.141000000000000))*(x72))));
00508 evalcond[4]=((IkReal(0.100000000000000))+(((IkReal(-1.00000000000000))*(x74)))+(((IkReal(-1.00000000000000))*(x75)))+(((cj2)*(x79)))+(((IkReal(0.705000000000000))*(x72)))+(((IkReal(-1.00000000000000))*(x72)*(x76)))+(((x73)*(x77)))+(((x73)*(x78))));
00509 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  )
00510 {
00511 continue;
00512 }
00513 }
00514 
00515 rotationfunction0(solutions);
00516 }
00517 }
00518 
00519 }
00520 
00521 }
00522 
00523 } else
00524 {
00525 {
00526 IkReal j1array[1], cj1array[1], sj1array[1];
00527 bool j1valid[1]={false};
00528 _nj1 = 1;
00529 IkReal x223=(cj2)*(cj2);
00530 IkReal x224=(sj2)*(sj2);
00531 IkReal x225=((cj2)*(sj2));
00532 if( IKabs(((gconst1)*(((((IkReal(-0.101925000000000))*(x223)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((cj0)*(px)*(pz)))+(((IkReal(0.551800000000000))*(x225)))+(((IkReal(-0.100000000000000))*(pz)))+(((py)*(pz)*(sj0)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(0.101925000000000))*(x224))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-0.203850000000000))*(x225)))+((pz)*(pz))+(((IkReal(-0.570025000000000))*(x223)))+(((IkReal(-0.0182250000000000))*(x224))))))) < IKFAST_ATAN2_MAGTHRESH )
00533     continue;
00534 j1array[0]=IKatan2(((gconst1)*(((((IkReal(-0.101925000000000))*(x223)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((cj0)*(px)*(pz)))+(((IkReal(0.551800000000000))*(x225)))+(((IkReal(-0.100000000000000))*(pz)))+(((py)*(pz)*(sj0)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(0.101925000000000))*(x224)))))), ((gconst1)*(((((IkReal(-0.203850000000000))*(x225)))+((pz)*(pz))+(((IkReal(-0.570025000000000))*(x223)))+(((IkReal(-0.0182250000000000))*(x224)))))));
00535 sj1array[0]=IKsin(j1array[0]);
00536 cj1array[0]=IKcos(j1array[0]);
00537 if( j1array[0] > IKPI )
00538 {
00539     j1array[0]-=IK2PI;
00540 }
00541 else if( j1array[0] < -IKPI )
00542 {    j1array[0]+=IK2PI;
00543 }
00544 j1valid[0] = true;
00545 for(int ij1 = 0; ij1 < 1; ++ij1)
00546 {
00547 if( !j1valid[ij1] )
00548 {
00549     continue;
00550 }
00551 _ij1[0] = ij1; _ij1[1] = -1;
00552 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00553 {
00554 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00555 {
00556     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00557 }
00558 }
00559 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00560 {
00561 IkReal evalcond[5];
00562 IkReal x226=IKsin(j1);
00563 IkReal x227=IKcos(j1);
00564 IkReal x228=((cj0)*(px));
00565 IkReal x229=((py)*(sj0));
00566 IkReal x230=((IkReal(0.755000000000000))*(sj2));
00567 IkReal x231=((IkReal(0.135000000000000))*(sj2));
00568 IkReal x232=((IkReal(0.755000000000000))*(cj2));
00569 IkReal x233=((IkReal(0.135000000000000))*(x226));
00570 IkReal x234=((IkReal(1.00000000000000))*(x227));
00571 IkReal x235=((cj2)*(x227));
00572 IkReal x236=((IkReal(1.41000000000000))*(x226));
00573 IkReal x237=((pz)*(x227));
00574 evalcond[0]=((IkReal(-0.705000000000000))+(((IkReal(-0.135000000000000))*(cj2)))+(((IkReal(-0.100000000000000))*(x226)))+(x230)+(x237)+(((x226)*(x229)))+(((x226)*(x228))));
00575 evalcond[1]=((((pz)*(x226)))+(((IkReal(0.100000000000000))*(x227)))+(x232)+(x231)+(((IkReal(-1.00000000000000))*(x229)*(x234)))+(((IkReal(-1.00000000000000))*(x228)*(x234))));
00576 evalcond[2]=((((x226)*(x232)))+(((x226)*(x231)))+(pz)+(((x227)*(x230)))+(((IkReal(-0.705000000000000))*(x227)))+(((IkReal(-0.135000000000000))*(x235))));
00577 evalcond[3]=((IkReal(0.0812250000000000))+(((x229)*(x236)))+(((x228)*(x236)))+(((IkReal(0.200000000000000))*(x228)))+(((IkReal(0.200000000000000))*(x229)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.141000000000000))*(x226)))+(((IkReal(1.41000000000000))*(x237))));
00578 evalcond[4]=((IkReal(0.100000000000000))+(((cj2)*(x233)))+(((IkReal(-1.00000000000000))*(x226)*(x230)))+(((x227)*(x231)))+(((x227)*(x232)))+(((IkReal(-1.00000000000000))*(x229)))+(((IkReal(-1.00000000000000))*(x228)))+(((IkReal(0.705000000000000))*(x226))));
00579 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  )
00580 {
00581 continue;
00582 }
00583 }
00584 
00585 rotationfunction0(solutions);
00586 }
00587 }
00588 
00589 }
00590 
00591 }
00592 }
00593 }
00594 }
00595 }
00596 }
00597 return solutions.GetNumSolutions()>0;
00598 }
00599 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
00600 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00601 IkReal x84=((cj0)*(r00));
00602 IkReal x85=((cj0)*(r01));
00603 IkReal x86=((sj1)*(sj2));
00604 IkReal x87=((IkReal(1.00000000000000))*(sj0));
00605 IkReal x88=((r10)*(sj0));
00606 IkReal x89=((IkReal(1.00000000000000))*(cj2));
00607 IkReal x90=((r11)*(sj0));
00608 IkReal x91=((cj0)*(r02));
00609 IkReal x92=((r12)*(sj0));
00610 IkReal x93=((x86)+(((IkReal(-1.00000000000000))*(cj1)*(x89))));
00611 IkReal x94=((((IkReal(-1.00000000000000))*(x86)))+(((cj1)*(cj2))));
00612 IkReal x95=((cj0)*(x94));
00613 IkReal x96=((((IkReal(-1.00000000000000))*(sj1)*(x89)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2))));
00614 IkReal x97=((sj0)*(x96));
00615 new_r00=((((r20)*(x93)))+(((x88)*(x96)))+(((x84)*(x96))));
00616 new_r01=((((r21)*(x93)))+(((x85)*(x96)))+(((x90)*(x96))));
00617 new_r02=((((x92)*(x96)))+(((r22)*(x93)))+(((x91)*(x96))));
00618 new_r10=((((IkReal(-1.00000000000000))*(r00)*(x87)))+(((cj0)*(r10))));
00619 new_r11=((((cj0)*(r11)))+(((IkReal(-1.00000000000000))*(r01)*(x87))));
00620 new_r12=((((IkReal(-1.00000000000000))*(r02)*(x87)))+(((cj0)*(r12))));
00621 new_r20=((((r20)*(x96)))+(((x88)*(x94)))+(((x84)*(x94))));
00622 new_r21=((((r21)*(x96)))+(((x85)*(x94)))+(((x90)*(x94))));
00623 new_r22=((((x92)*(x94)))+(((r22)*(x96)))+(((x91)*(x94))));
00624 {
00625 IkReal j4array[2], cj4array[2], sj4array[2];
00626 bool j4valid[2]={false};
00627 _nj4 = 2;
00628 cj4array[0]=new_r22;
00629 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
00630 {
00631     j4valid[0] = j4valid[1] = true;
00632     j4array[0] = IKacos(cj4array[0]);
00633     sj4array[0] = IKsin(j4array[0]);
00634     cj4array[1] = cj4array[0];
00635     j4array[1] = -j4array[0];
00636     sj4array[1] = -sj4array[0];
00637 }
00638 else if( isnan(cj4array[0]) )
00639 {
00640     // probably any value will work
00641     j4valid[0] = true;
00642     cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
00643 }
00644 for(int ij4 = 0; ij4 < 2; ++ij4)
00645 {
00646 if( !j4valid[ij4] )
00647 {
00648     continue;
00649 }
00650 _ij4[0] = ij4; _ij4[1] = -1;
00651 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
00652 {
00653 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00654 {
00655     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00656 }
00657 }
00658 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00659 
00660 {
00661 IkReal dummyeval[1];
00662 IkReal gconst4;
00663 gconst4=IKsign(sj4);
00664 dummyeval[0]=sj4;
00665 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00666 {
00667 {
00668 IkReal dummyeval[1];
00669 IkReal gconst2;
00670 gconst2=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00671 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00672 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00673 {
00674 {
00675 IkReal dummyeval[1];
00676 IkReal gconst3;
00677 gconst3=IKsign(((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4)))));
00678 dummyeval[0]=((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4))));
00679 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00680 {
00681 {
00682 IkReal evalcond[7];
00683 IkReal x98=((IkReal(-1.00000000000000))+(new_r22));
00684 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
00685 evalcond[1]=x98;
00686 evalcond[2]=new_r20;
00687 evalcond[3]=new_r21;
00688 evalcond[4]=new_r20;
00689 evalcond[5]=new_r21;
00690 evalcond[6]=x98;
00691 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
00692 {
00693 {
00694 IkReal j3array[2], cj3array[2], sj3array[2];
00695 bool j3valid[2]={false};
00696 _nj3 = 2;
00697 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
00698     continue;
00699 IkReal x99=IKatan2(new_r02, new_r12);
00700 j3array[0]=((IkReal(-1.00000000000000))*(x99));
00701 sj3array[0]=IKsin(j3array[0]);
00702 cj3array[0]=IKcos(j3array[0]);
00703 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x99))));
00704 sj3array[1]=IKsin(j3array[1]);
00705 cj3array[1]=IKcos(j3array[1]);
00706 if( j3array[0] > IKPI )
00707 {
00708     j3array[0]-=IK2PI;
00709 }
00710 else if( j3array[0] < -IKPI )
00711 {    j3array[0]+=IK2PI;
00712 }
00713 j3valid[0] = true;
00714 if( j3array[1] > IKPI )
00715 {
00716     j3array[1]-=IK2PI;
00717 }
00718 else if( j3array[1] < -IKPI )
00719 {    j3array[1]+=IK2PI;
00720 }
00721 j3valid[1] = true;
00722 for(int ij3 = 0; ij3 < 2; ++ij3)
00723 {
00724 if( !j3valid[ij3] )
00725 {
00726     continue;
00727 }
00728 _ij3[0] = ij3; _ij3[1] = -1;
00729 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00730 {
00731 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00732 {
00733     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00734 }
00735 }
00736 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00737 {
00738 IkReal evalcond[1];
00739 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))+(((new_r12)*(IKcos(j3)))));
00740 if( IKabs(evalcond[0]) > 0.000001  )
00741 {
00742 continue;
00743 }
00744 }
00745 
00746 {
00747 IkReal j5array[1], cj5array[1], sj5array[1];
00748 bool j5valid[1]={false};
00749 _nj5 = 1;
00750 if( IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
00751     continue;
00752 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
00753 sj5array[0]=IKsin(j5array[0]);
00754 cj5array[0]=IKcos(j5array[0]);
00755 if( j5array[0] > IKPI )
00756 {
00757     j5array[0]-=IK2PI;
00758 }
00759 else if( j5array[0] < -IKPI )
00760 {    j5array[0]+=IK2PI;
00761 }
00762 j5valid[0] = true;
00763 for(int ij5 = 0; ij5 < 1; ++ij5)
00764 {
00765 if( !j5valid[ij5] )
00766 {
00767     continue;
00768 }
00769 _ij5[0] = ij5; _ij5[1] = -1;
00770 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00771 {
00772 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00773 {
00774     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00775 }
00776 }
00777 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00778 {
00779 IkReal evalcond[4];
00780 IkReal x100=IKsin(j5);
00781 IkReal x101=((IkReal(1.00000000000000))*(sj3));
00782 IkReal x102=((IkReal(1.00000000000000))*(IKcos(j5)));
00783 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x101)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x100))));
00784 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x101)))+(((IkReal(-1.00000000000000))*(x102)))+(((cj3)*(new_r11))));
00785 evalcond[2]=((((new_r11)*(sj3)))+(x100)+(((cj3)*(new_r01))));
00786 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x102)))+(((cj3)*(new_r00))));
00787 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00788 {
00789 continue;
00790 }
00791 }
00792 
00793 {
00794 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00795 vinfos[0].jointtype = 1;
00796 vinfos[0].foffset = j0;
00797 vinfos[0].indices[0] = _ij0[0];
00798 vinfos[0].indices[1] = _ij0[1];
00799 vinfos[0].maxsolutions = _nj0;
00800 vinfos[1].jointtype = 1;
00801 vinfos[1].foffset = j1;
00802 vinfos[1].indices[0] = _ij1[0];
00803 vinfos[1].indices[1] = _ij1[1];
00804 vinfos[1].maxsolutions = _nj1;
00805 vinfos[2].jointtype = 1;
00806 vinfos[2].foffset = j2;
00807 vinfos[2].indices[0] = _ij2[0];
00808 vinfos[2].indices[1] = _ij2[1];
00809 vinfos[2].maxsolutions = _nj2;
00810 vinfos[3].jointtype = 1;
00811 vinfos[3].foffset = j3;
00812 vinfos[3].indices[0] = _ij3[0];
00813 vinfos[3].indices[1] = _ij3[1];
00814 vinfos[3].maxsolutions = _nj3;
00815 vinfos[4].jointtype = 1;
00816 vinfos[4].foffset = j4;
00817 vinfos[4].indices[0] = _ij4[0];
00818 vinfos[4].indices[1] = _ij4[1];
00819 vinfos[4].maxsolutions = _nj4;
00820 vinfos[5].jointtype = 1;
00821 vinfos[5].foffset = j5;
00822 vinfos[5].indices[0] = _ij5[0];
00823 vinfos[5].indices[1] = _ij5[1];
00824 vinfos[5].maxsolutions = _nj5;
00825 std::vector<int> vfree(0);
00826 solutions.AddSolution(vinfos,vfree);
00827 }
00828 }
00829 }
00830 }
00831 }
00832 
00833 } else
00834 {
00835 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959))));
00836 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
00837 evalcond[2]=new_r20;
00838 evalcond[3]=new_r21;
00839 evalcond[4]=((IkReal(-1.00000000000000))*(new_r20));
00840 evalcond[5]=((IkReal(-1.00000000000000))*(new_r21));
00841 evalcond[6]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
00842 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
00843 {
00844 {
00845 IkReal j3array[2], cj3array[2], sj3array[2];
00846 bool j3valid[2]={false};
00847 _nj3 = 2;
00848 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
00849     continue;
00850 IkReal x103=IKatan2(new_r02, new_r12);
00851 j3array[0]=((IkReal(-1.00000000000000))*(x103));
00852 sj3array[0]=IKsin(j3array[0]);
00853 cj3array[0]=IKcos(j3array[0]);
00854 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x103))));
00855 sj3array[1]=IKsin(j3array[1]);
00856 cj3array[1]=IKcos(j3array[1]);
00857 if( j3array[0] > IKPI )
00858 {
00859     j3array[0]-=IK2PI;
00860 }
00861 else if( j3array[0] < -IKPI )
00862 {    j3array[0]+=IK2PI;
00863 }
00864 j3valid[0] = true;
00865 if( j3array[1] > IKPI )
00866 {
00867     j3array[1]-=IK2PI;
00868 }
00869 else if( j3array[1] < -IKPI )
00870 {    j3array[1]+=IK2PI;
00871 }
00872 j3valid[1] = true;
00873 for(int ij3 = 0; ij3 < 2; ++ij3)
00874 {
00875 if( !j3valid[ij3] )
00876 {
00877     continue;
00878 }
00879 _ij3[0] = ij3; _ij3[1] = -1;
00880 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00881 {
00882 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00883 {
00884     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00885 }
00886 }
00887 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00888 {
00889 IkReal evalcond[1];
00890 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))+(((new_r12)*(IKcos(j3)))));
00891 if( IKabs(evalcond[0]) > 0.000001  )
00892 {
00893 continue;
00894 }
00895 }
00896 
00897 {
00898 IkReal j5array[1], cj5array[1], sj5array[1];
00899 bool j5valid[1]={false};
00900 _nj5 = 1;
00901 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
00902     continue;
00903 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
00904 sj5array[0]=IKsin(j5array[0]);
00905 cj5array[0]=IKcos(j5array[0]);
00906 if( j5array[0] > IKPI )
00907 {
00908     j5array[0]-=IK2PI;
00909 }
00910 else if( j5array[0] < -IKPI )
00911 {    j5array[0]+=IK2PI;
00912 }
00913 j5valid[0] = true;
00914 for(int ij5 = 0; ij5 < 1; ++ij5)
00915 {
00916 if( !j5valid[ij5] )
00917 {
00918     continue;
00919 }
00920 _ij5[0] = ij5; _ij5[1] = -1;
00921 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00922 {
00923 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00924 {
00925     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00926 }
00927 }
00928 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00929 {
00930 IkReal evalcond[4];
00931 IkReal x104=IKcos(j5);
00932 IkReal x105=((IkReal(1.00000000000000))*(sj3));
00933 IkReal x106=((IkReal(1.00000000000000))*(IKsin(j5)));
00934 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x105)))+(((IkReal(-1.00000000000000))*(x106)))+(((cj3)*(new_r10))));
00935 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x105)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x104))));
00936 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x106)))+(((cj3)*(new_r01))));
00937 evalcond[3]=((((new_r10)*(sj3)))+(x104)+(((cj3)*(new_r00))));
00938 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00939 {
00940 continue;
00941 }
00942 }
00943 
00944 {
00945 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00946 vinfos[0].jointtype = 1;
00947 vinfos[0].foffset = j0;
00948 vinfos[0].indices[0] = _ij0[0];
00949 vinfos[0].indices[1] = _ij0[1];
00950 vinfos[0].maxsolutions = _nj0;
00951 vinfos[1].jointtype = 1;
00952 vinfos[1].foffset = j1;
00953 vinfos[1].indices[0] = _ij1[0];
00954 vinfos[1].indices[1] = _ij1[1];
00955 vinfos[1].maxsolutions = _nj1;
00956 vinfos[2].jointtype = 1;
00957 vinfos[2].foffset = j2;
00958 vinfos[2].indices[0] = _ij2[0];
00959 vinfos[2].indices[1] = _ij2[1];
00960 vinfos[2].maxsolutions = _nj2;
00961 vinfos[3].jointtype = 1;
00962 vinfos[3].foffset = j3;
00963 vinfos[3].indices[0] = _ij3[0];
00964 vinfos[3].indices[1] = _ij3[1];
00965 vinfos[3].maxsolutions = _nj3;
00966 vinfos[4].jointtype = 1;
00967 vinfos[4].foffset = j4;
00968 vinfos[4].indices[0] = _ij4[0];
00969 vinfos[4].indices[1] = _ij4[1];
00970 vinfos[4].maxsolutions = _nj4;
00971 vinfos[5].jointtype = 1;
00972 vinfos[5].foffset = j5;
00973 vinfos[5].indices[0] = _ij5[0];
00974 vinfos[5].indices[1] = _ij5[1];
00975 vinfos[5].maxsolutions = _nj5;
00976 std::vector<int> vfree(0);
00977 solutions.AddSolution(vinfos,vfree);
00978 }
00979 }
00980 }
00981 }
00982 }
00983 
00984 } else
00985 {
00986 if( 1 )
00987 {
00988 continue;
00989 
00990 } else
00991 {
00992 }
00993 }
00994 }
00995 }
00996 
00997 } else
00998 {
00999 {
01000 IkReal j3array[1], cj3array[1], sj3array[1];
01001 bool j3valid[1]={false};
01002 _nj3 = 1;
01003 IkReal x107=((IkReal(-1.00000000000000))*(cj4)*(gconst3)*(new_r20));
01004 if( IKabs(((new_r12)*(x107))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x107))) < IKFAST_ATAN2_MAGTHRESH )
01005     continue;
01006 j3array[0]=IKatan2(((new_r12)*(x107)), ((new_r02)*(x107)));
01007 sj3array[0]=IKsin(j3array[0]);
01008 cj3array[0]=IKcos(j3array[0]);
01009 if( j3array[0] > IKPI )
01010 {
01011     j3array[0]-=IK2PI;
01012 }
01013 else if( j3array[0] < -IKPI )
01014 {    j3array[0]+=IK2PI;
01015 }
01016 j3valid[0] = true;
01017 for(int ij3 = 0; ij3 < 1; ++ij3)
01018 {
01019 if( !j3valid[ij3] )
01020 {
01021     continue;
01022 }
01023 _ij3[0] = ij3; _ij3[1] = -1;
01024 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01025 {
01026 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01027 {
01028     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01029 }
01030 }
01031 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01032 {
01033 IkReal evalcond[6];
01034 IkReal x108=IKsin(j3);
01035 IkReal x109=IKcos(j3);
01036 IkReal x110=((IkReal(1.00000000000000))*(sj4));
01037 IkReal x111=((sj4)*(x108));
01038 IkReal x112=((sj4)*(x109));
01039 IkReal x113=((new_r02)*(x109));
01040 IkReal x114=((new_r12)*(x108));
01041 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x108)))+(((new_r12)*(x109))));
01042 evalcond[1]=((((IkReal(-1.00000000000000))*(x110)))+(x113)+(x114));
01043 evalcond[2]=((((new_r10)*(x111)))+(((cj4)*(new_r20)))+(((new_r00)*(x112))));
01044 evalcond[3]=((((new_r11)*(x111)))+(((cj4)*(new_r21)))+(((new_r01)*(x112))));
01045 evalcond[4]=((IkReal(-1.00000000000000))+(((cj4)*(new_r22)))+(((new_r02)*(x112)))+(((new_r12)*(x111))));
01046 evalcond[5]=((((IkReal(-1.00000000000000))*(new_r22)*(x110)))+(((cj4)*(x114)))+(((cj4)*(x113))));
01047 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01048 {
01049 continue;
01050 }
01051 }
01052 
01053 {
01054 IkReal dummyeval[1];
01055 IkReal gconst5;
01056 gconst5=IKsign(sj4);
01057 dummyeval[0]=sj4;
01058 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01059 {
01060 {
01061 IkReal dummyeval[1];
01062 dummyeval[0]=sj4;
01063 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01064 {
01065 {
01066 IkReal dummyeval[1];
01067 dummyeval[0]=sj4;
01068 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01069 {
01070 {
01071 IkReal evalcond[11];
01072 IkReal x115=((IkReal(-1.00000000000000))+(new_r22));
01073 IkReal x116=((((IkReal(-1.00000000000000))*(new_r02)*(sj3)))+(((cj3)*(new_r12))));
01074 IkReal x117=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
01075 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01076 evalcond[1]=x115;
01077 evalcond[2]=new_r20;
01078 evalcond[3]=new_r21;
01079 evalcond[4]=x116;
01080 evalcond[5]=x116;
01081 evalcond[6]=x117;
01082 evalcond[7]=new_r20;
01083 evalcond[8]=new_r21;
01084 evalcond[9]=x115;
01085 evalcond[10]=x117;
01086 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01087 {
01088 {
01089 IkReal j5array[1], cj5array[1], sj5array[1];
01090 bool j5valid[1]={false};
01091 _nj5 = 1;
01092 if( IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01093     continue;
01094 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01095 sj5array[0]=IKsin(j5array[0]);
01096 cj5array[0]=IKcos(j5array[0]);
01097 if( j5array[0] > IKPI )
01098 {
01099     j5array[0]-=IK2PI;
01100 }
01101 else if( j5array[0] < -IKPI )
01102 {    j5array[0]+=IK2PI;
01103 }
01104 j5valid[0] = true;
01105 for(int ij5 = 0; ij5 < 1; ++ij5)
01106 {
01107 if( !j5valid[ij5] )
01108 {
01109     continue;
01110 }
01111 _ij5[0] = ij5; _ij5[1] = -1;
01112 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01113 {
01114 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01115 {
01116     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01117 }
01118 }
01119 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01120 {
01121 IkReal evalcond[4];
01122 IkReal x118=IKsin(j5);
01123 IkReal x119=((IkReal(1.00000000000000))*(sj3));
01124 IkReal x120=((IkReal(1.00000000000000))*(IKcos(j5)));
01125 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x119)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x118))));
01126 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x119)))+(((IkReal(-1.00000000000000))*(x120)))+(((cj3)*(new_r11))));
01127 evalcond[2]=((((new_r11)*(sj3)))+(x118)+(((cj3)*(new_r01))));
01128 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x120)))+(((cj3)*(new_r00))));
01129 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01130 {
01131 continue;
01132 }
01133 }
01134 
01135 {
01136 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01137 vinfos[0].jointtype = 1;
01138 vinfos[0].foffset = j0;
01139 vinfos[0].indices[0] = _ij0[0];
01140 vinfos[0].indices[1] = _ij0[1];
01141 vinfos[0].maxsolutions = _nj0;
01142 vinfos[1].jointtype = 1;
01143 vinfos[1].foffset = j1;
01144 vinfos[1].indices[0] = _ij1[0];
01145 vinfos[1].indices[1] = _ij1[1];
01146 vinfos[1].maxsolutions = _nj1;
01147 vinfos[2].jointtype = 1;
01148 vinfos[2].foffset = j2;
01149 vinfos[2].indices[0] = _ij2[0];
01150 vinfos[2].indices[1] = _ij2[1];
01151 vinfos[2].maxsolutions = _nj2;
01152 vinfos[3].jointtype = 1;
01153 vinfos[3].foffset = j3;
01154 vinfos[3].indices[0] = _ij3[0];
01155 vinfos[3].indices[1] = _ij3[1];
01156 vinfos[3].maxsolutions = _nj3;
01157 vinfos[4].jointtype = 1;
01158 vinfos[4].foffset = j4;
01159 vinfos[4].indices[0] = _ij4[0];
01160 vinfos[4].indices[1] = _ij4[1];
01161 vinfos[4].maxsolutions = _nj4;
01162 vinfos[5].jointtype = 1;
01163 vinfos[5].foffset = j5;
01164 vinfos[5].indices[0] = _ij5[0];
01165 vinfos[5].indices[1] = _ij5[1];
01166 vinfos[5].maxsolutions = _nj5;
01167 std::vector<int> vfree(0);
01168 solutions.AddSolution(vinfos,vfree);
01169 }
01170 }
01171 }
01172 
01173 } else
01174 {
01175 IkReal x121=((new_r12)*(sj3));
01176 IkReal x122=((IkReal(1.00000000000000))*(new_r02));
01177 IkReal x123=((((IkReal(-1.00000000000000))*(sj3)*(x122)))+(((cj3)*(new_r12))));
01178 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959))));
01179 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
01180 evalcond[2]=new_r20;
01181 evalcond[3]=new_r21;
01182 evalcond[4]=x123;
01183 evalcond[5]=x123;
01184 evalcond[6]=((x121)+(((cj3)*(new_r02))));
01185 evalcond[7]=((IkReal(-1.00000000000000))*(new_r20));
01186 evalcond[8]=((IkReal(-1.00000000000000))*(new_r21));
01187 evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01188 evalcond[10]=((((IkReal(-1.00000000000000))*(cj3)*(x122)))+(((IkReal(-1.00000000000000))*(x121))));
01189 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01190 {
01191 {
01192 IkReal j5array[1], cj5array[1], sj5array[1];
01193 bool j5valid[1]={false};
01194 _nj5 = 1;
01195 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
01196     continue;
01197 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01198 sj5array[0]=IKsin(j5array[0]);
01199 cj5array[0]=IKcos(j5array[0]);
01200 if( j5array[0] > IKPI )
01201 {
01202     j5array[0]-=IK2PI;
01203 }
01204 else if( j5array[0] < -IKPI )
01205 {    j5array[0]+=IK2PI;
01206 }
01207 j5valid[0] = true;
01208 for(int ij5 = 0; ij5 < 1; ++ij5)
01209 {
01210 if( !j5valid[ij5] )
01211 {
01212     continue;
01213 }
01214 _ij5[0] = ij5; _ij5[1] = -1;
01215 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01216 {
01217 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01218 {
01219     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01220 }
01221 }
01222 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01223 {
01224 IkReal evalcond[4];
01225 IkReal x124=IKcos(j5);
01226 IkReal x125=((IkReal(1.00000000000000))*(sj3));
01227 IkReal x126=((IkReal(1.00000000000000))*(IKsin(j5)));
01228 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x125)))+(((IkReal(-1.00000000000000))*(x126)))+(((cj3)*(new_r10))));
01229 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x125)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x124))));
01230 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x126)))+(((cj3)*(new_r01))));
01231 evalcond[3]=((((new_r10)*(sj3)))+(x124)+(((cj3)*(new_r00))));
01232 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01233 {
01234 continue;
01235 }
01236 }
01237 
01238 {
01239 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01240 vinfos[0].jointtype = 1;
01241 vinfos[0].foffset = j0;
01242 vinfos[0].indices[0] = _ij0[0];
01243 vinfos[0].indices[1] = _ij0[1];
01244 vinfos[0].maxsolutions = _nj0;
01245 vinfos[1].jointtype = 1;
01246 vinfos[1].foffset = j1;
01247 vinfos[1].indices[0] = _ij1[0];
01248 vinfos[1].indices[1] = _ij1[1];
01249 vinfos[1].maxsolutions = _nj1;
01250 vinfos[2].jointtype = 1;
01251 vinfos[2].foffset = j2;
01252 vinfos[2].indices[0] = _ij2[0];
01253 vinfos[2].indices[1] = _ij2[1];
01254 vinfos[2].maxsolutions = _nj2;
01255 vinfos[3].jointtype = 1;
01256 vinfos[3].foffset = j3;
01257 vinfos[3].indices[0] = _ij3[0];
01258 vinfos[3].indices[1] = _ij3[1];
01259 vinfos[3].maxsolutions = _nj3;
01260 vinfos[4].jointtype = 1;
01261 vinfos[4].foffset = j4;
01262 vinfos[4].indices[0] = _ij4[0];
01263 vinfos[4].indices[1] = _ij4[1];
01264 vinfos[4].maxsolutions = _nj4;
01265 vinfos[5].jointtype = 1;
01266 vinfos[5].foffset = j5;
01267 vinfos[5].indices[0] = _ij5[0];
01268 vinfos[5].indices[1] = _ij5[1];
01269 vinfos[5].maxsolutions = _nj5;
01270 std::vector<int> vfree(0);
01271 solutions.AddSolution(vinfos,vfree);
01272 }
01273 }
01274 }
01275 
01276 } else
01277 {
01278 if( 1 )
01279 {
01280 continue;
01281 
01282 } else
01283 {
01284 }
01285 }
01286 }
01287 }
01288 
01289 } else
01290 {
01291 {
01292 IkReal j5array[1], cj5array[1], sj5array[1];
01293 bool j5valid[1]={false};
01294 _nj5 = 1;
01295 if( IKabs(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
01296     continue;
01297 j5array[0]=IKatan2(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
01298 sj5array[0]=IKsin(j5array[0]);
01299 cj5array[0]=IKcos(j5array[0]);
01300 if( j5array[0] > IKPI )
01301 {
01302     j5array[0]-=IK2PI;
01303 }
01304 else if( j5array[0] < -IKPI )
01305 {    j5array[0]+=IK2PI;
01306 }
01307 j5valid[0] = true;
01308 for(int ij5 = 0; ij5 < 1; ++ij5)
01309 {
01310 if( !j5valid[ij5] )
01311 {
01312     continue;
01313 }
01314 _ij5[0] = ij5; _ij5[1] = -1;
01315 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01316 {
01317 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01318 {
01319     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01320 }
01321 }
01322 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01323 {
01324 IkReal evalcond[8];
01325 IkReal x127=IKsin(j5);
01326 IkReal x128=IKcos(j5);
01327 IkReal x129=((IkReal(1.00000000000000))*(sj3));
01328 IkReal x130=((new_r11)*(sj3));
01329 IkReal x131=((new_r10)*(sj3));
01330 IkReal x132=((cj3)*(cj4));
01331 IkReal x133=((IkReal(1.00000000000000))*(sj4));
01332 IkReal x134=((IkReal(1.00000000000000))*(x128));
01333 IkReal x135=((IkReal(1.00000000000000))*(x127));
01334 evalcond[0]=((((sj4)*(x128)))+(new_r20));
01335 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x127)*(x133))));
01336 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x129)))+(((IkReal(-1.00000000000000))*(x135)))+(((cj3)*(new_r10))));
01337 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x129)))+(((IkReal(-1.00000000000000))*(x134)))+(((cj3)*(new_r11))));
01338 evalcond[4]=((x130)+(((cj3)*(new_r01)))+(((cj4)*(x127))));
01339 evalcond[5]=((x131)+(((cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x134))));
01340 evalcond[6]=((((new_r01)*(x132)))+(x127)+(((IkReal(-1.00000000000000))*(new_r21)*(x133)))+(((cj4)*(x130))));
01341 evalcond[7]=((((new_r00)*(x132)))+(((IkReal(-1.00000000000000))*(new_r20)*(x133)))+(((IkReal(-1.00000000000000))*(x134)))+(((cj4)*(x131))));
01342 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
01343 {
01344 continue;
01345 }
01346 }
01347 
01348 {
01349 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01350 vinfos[0].jointtype = 1;
01351 vinfos[0].foffset = j0;
01352 vinfos[0].indices[0] = _ij0[0];
01353 vinfos[0].indices[1] = _ij0[1];
01354 vinfos[0].maxsolutions = _nj0;
01355 vinfos[1].jointtype = 1;
01356 vinfos[1].foffset = j1;
01357 vinfos[1].indices[0] = _ij1[0];
01358 vinfos[1].indices[1] = _ij1[1];
01359 vinfos[1].maxsolutions = _nj1;
01360 vinfos[2].jointtype = 1;
01361 vinfos[2].foffset = j2;
01362 vinfos[2].indices[0] = _ij2[0];
01363 vinfos[2].indices[1] = _ij2[1];
01364 vinfos[2].maxsolutions = _nj2;
01365 vinfos[3].jointtype = 1;
01366 vinfos[3].foffset = j3;
01367 vinfos[3].indices[0] = _ij3[0];
01368 vinfos[3].indices[1] = _ij3[1];
01369 vinfos[3].maxsolutions = _nj3;
01370 vinfos[4].jointtype = 1;
01371 vinfos[4].foffset = j4;
01372 vinfos[4].indices[0] = _ij4[0];
01373 vinfos[4].indices[1] = _ij4[1];
01374 vinfos[4].maxsolutions = _nj4;
01375 vinfos[5].jointtype = 1;
01376 vinfos[5].foffset = j5;
01377 vinfos[5].indices[0] = _ij5[0];
01378 vinfos[5].indices[1] = _ij5[1];
01379 vinfos[5].maxsolutions = _nj5;
01380 std::vector<int> vfree(0);
01381 solutions.AddSolution(vinfos,vfree);
01382 }
01383 }
01384 }
01385 
01386 }
01387 
01388 }
01389 
01390 } else
01391 {
01392 {
01393 IkReal j5array[1], cj5array[1], sj5array[1];
01394 bool j5valid[1]={false};
01395 _nj5 = 1;
01396 if( IKabs(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
01397     continue;
01398 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
01399 sj5array[0]=IKsin(j5array[0]);
01400 cj5array[0]=IKcos(j5array[0]);
01401 if( j5array[0] > IKPI )
01402 {
01403     j5array[0]-=IK2PI;
01404 }
01405 else if( j5array[0] < -IKPI )
01406 {    j5array[0]+=IK2PI;
01407 }
01408 j5valid[0] = true;
01409 for(int ij5 = 0; ij5 < 1; ++ij5)
01410 {
01411 if( !j5valid[ij5] )
01412 {
01413     continue;
01414 }
01415 _ij5[0] = ij5; _ij5[1] = -1;
01416 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01417 {
01418 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01419 {
01420     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01421 }
01422 }
01423 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01424 {
01425 IkReal evalcond[8];
01426 IkReal x136=IKsin(j5);
01427 IkReal x137=IKcos(j5);
01428 IkReal x138=((IkReal(1.00000000000000))*(sj3));
01429 IkReal x139=((new_r11)*(sj3));
01430 IkReal x140=((new_r10)*(sj3));
01431 IkReal x141=((cj3)*(cj4));
01432 IkReal x142=((IkReal(1.00000000000000))*(sj4));
01433 IkReal x143=((IkReal(1.00000000000000))*(x137));
01434 IkReal x144=((IkReal(1.00000000000000))*(x136));
01435 evalcond[0]=((((sj4)*(x137)))+(new_r20));
01436 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x136)*(x142))));
01437 evalcond[2]=((((IkReal(-1.00000000000000))*(x144)))+(((IkReal(-1.00000000000000))*(new_r00)*(x138)))+(((cj3)*(new_r10))));
01438 evalcond[3]=((((IkReal(-1.00000000000000))*(x143)))+(((IkReal(-1.00000000000000))*(new_r01)*(x138)))+(((cj3)*(new_r11))));
01439 evalcond[4]=((x139)+(((cj3)*(new_r01)))+(((cj4)*(x136))));
01440 evalcond[5]=((x140)+(((cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x143))));
01441 evalcond[6]=((((new_r01)*(x141)))+(((IkReal(-1.00000000000000))*(new_r21)*(x142)))+(x136)+(((cj4)*(x139))));
01442 evalcond[7]=((((IkReal(-1.00000000000000))*(x143)))+(((IkReal(-1.00000000000000))*(new_r20)*(x142)))+(((new_r00)*(x141)))+(((cj4)*(x140))));
01443 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
01444 {
01445 continue;
01446 }
01447 }
01448 
01449 {
01450 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01451 vinfos[0].jointtype = 1;
01452 vinfos[0].foffset = j0;
01453 vinfos[0].indices[0] = _ij0[0];
01454 vinfos[0].indices[1] = _ij0[1];
01455 vinfos[0].maxsolutions = _nj0;
01456 vinfos[1].jointtype = 1;
01457 vinfos[1].foffset = j1;
01458 vinfos[1].indices[0] = _ij1[0];
01459 vinfos[1].indices[1] = _ij1[1];
01460 vinfos[1].maxsolutions = _nj1;
01461 vinfos[2].jointtype = 1;
01462 vinfos[2].foffset = j2;
01463 vinfos[2].indices[0] = _ij2[0];
01464 vinfos[2].indices[1] = _ij2[1];
01465 vinfos[2].maxsolutions = _nj2;
01466 vinfos[3].jointtype = 1;
01467 vinfos[3].foffset = j3;
01468 vinfos[3].indices[0] = _ij3[0];
01469 vinfos[3].indices[1] = _ij3[1];
01470 vinfos[3].maxsolutions = _nj3;
01471 vinfos[4].jointtype = 1;
01472 vinfos[4].foffset = j4;
01473 vinfos[4].indices[0] = _ij4[0];
01474 vinfos[4].indices[1] = _ij4[1];
01475 vinfos[4].maxsolutions = _nj4;
01476 vinfos[5].jointtype = 1;
01477 vinfos[5].foffset = j5;
01478 vinfos[5].indices[0] = _ij5[0];
01479 vinfos[5].indices[1] = _ij5[1];
01480 vinfos[5].maxsolutions = _nj5;
01481 std::vector<int> vfree(0);
01482 solutions.AddSolution(vinfos,vfree);
01483 }
01484 }
01485 }
01486 
01487 }
01488 
01489 }
01490 
01491 } else
01492 {
01493 {
01494 IkReal j5array[1], cj5array[1], sj5array[1];
01495 bool j5valid[1]={false};
01496 _nj5 = 1;
01497 if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
01498     continue;
01499 j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(new_r20)));
01500 sj5array[0]=IKsin(j5array[0]);
01501 cj5array[0]=IKcos(j5array[0]);
01502 if( j5array[0] > IKPI )
01503 {
01504     j5array[0]-=IK2PI;
01505 }
01506 else if( j5array[0] < -IKPI )
01507 {    j5array[0]+=IK2PI;
01508 }
01509 j5valid[0] = true;
01510 for(int ij5 = 0; ij5 < 1; ++ij5)
01511 {
01512 if( !j5valid[ij5] )
01513 {
01514     continue;
01515 }
01516 _ij5[0] = ij5; _ij5[1] = -1;
01517 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01518 {
01519 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01520 {
01521     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01522 }
01523 }
01524 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01525 {
01526 IkReal evalcond[8];
01527 IkReal x145=IKsin(j5);
01528 IkReal x146=IKcos(j5);
01529 IkReal x147=((IkReal(1.00000000000000))*(sj3));
01530 IkReal x148=((new_r11)*(sj3));
01531 IkReal x149=((new_r10)*(sj3));
01532 IkReal x150=((cj3)*(cj4));
01533 IkReal x151=((IkReal(1.00000000000000))*(sj4));
01534 IkReal x152=((IkReal(1.00000000000000))*(x146));
01535 IkReal x153=((IkReal(1.00000000000000))*(x145));
01536 evalcond[0]=((((sj4)*(x146)))+(new_r20));
01537 evalcond[1]=((((IkReal(-1.00000000000000))*(x145)*(x151)))+(new_r21));
01538 evalcond[2]=((((IkReal(-1.00000000000000))*(x153)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x147))));
01539 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x147)))+(((IkReal(-1.00000000000000))*(x152)))+(((cj3)*(new_r11))));
01540 evalcond[4]=((x148)+(((cj3)*(new_r01)))+(((cj4)*(x145))));
01541 evalcond[5]=((x149)+(((cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x152))));
01542 evalcond[6]=((((new_r01)*(x150)))+(((IkReal(-1.00000000000000))*(new_r21)*(x151)))+(x145)+(((cj4)*(x148))));
01543 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x151)))+(((IkReal(-1.00000000000000))*(x152)))+(((new_r00)*(x150)))+(((cj4)*(x149))));
01544 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
01545 {
01546 continue;
01547 }
01548 }
01549 
01550 {
01551 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01552 vinfos[0].jointtype = 1;
01553 vinfos[0].foffset = j0;
01554 vinfos[0].indices[0] = _ij0[0];
01555 vinfos[0].indices[1] = _ij0[1];
01556 vinfos[0].maxsolutions = _nj0;
01557 vinfos[1].jointtype = 1;
01558 vinfos[1].foffset = j1;
01559 vinfos[1].indices[0] = _ij1[0];
01560 vinfos[1].indices[1] = _ij1[1];
01561 vinfos[1].maxsolutions = _nj1;
01562 vinfos[2].jointtype = 1;
01563 vinfos[2].foffset = j2;
01564 vinfos[2].indices[0] = _ij2[0];
01565 vinfos[2].indices[1] = _ij2[1];
01566 vinfos[2].maxsolutions = _nj2;
01567 vinfos[3].jointtype = 1;
01568 vinfos[3].foffset = j3;
01569 vinfos[3].indices[0] = _ij3[0];
01570 vinfos[3].indices[1] = _ij3[1];
01571 vinfos[3].maxsolutions = _nj3;
01572 vinfos[4].jointtype = 1;
01573 vinfos[4].foffset = j4;
01574 vinfos[4].indices[0] = _ij4[0];
01575 vinfos[4].indices[1] = _ij4[1];
01576 vinfos[4].maxsolutions = _nj4;
01577 vinfos[5].jointtype = 1;
01578 vinfos[5].foffset = j5;
01579 vinfos[5].indices[0] = _ij5[0];
01580 vinfos[5].indices[1] = _ij5[1];
01581 vinfos[5].maxsolutions = _nj5;
01582 std::vector<int> vfree(0);
01583 solutions.AddSolution(vinfos,vfree);
01584 }
01585 }
01586 }
01587 
01588 }
01589 
01590 }
01591 }
01592 }
01593 
01594 }
01595 
01596 }
01597 
01598 } else
01599 {
01600 {
01601 IkReal j3array[1], cj3array[1], sj3array[1];
01602 bool j3valid[1]={false};
01603 _nj3 = 1;
01604 IkReal x154=((gconst2)*(sj4));
01605 if( IKabs(((new_r12)*(x154))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x154))) < IKFAST_ATAN2_MAGTHRESH )
01606     continue;
01607 j3array[0]=IKatan2(((new_r12)*(x154)), ((new_r02)*(x154)));
01608 sj3array[0]=IKsin(j3array[0]);
01609 cj3array[0]=IKcos(j3array[0]);
01610 if( j3array[0] > IKPI )
01611 {
01612     j3array[0]-=IK2PI;
01613 }
01614 else if( j3array[0] < -IKPI )
01615 {    j3array[0]+=IK2PI;
01616 }
01617 j3valid[0] = true;
01618 for(int ij3 = 0; ij3 < 1; ++ij3)
01619 {
01620 if( !j3valid[ij3] )
01621 {
01622     continue;
01623 }
01624 _ij3[0] = ij3; _ij3[1] = -1;
01625 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01626 {
01627 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01628 {
01629     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01630 }
01631 }
01632 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01633 {
01634 IkReal evalcond[6];
01635 IkReal x155=IKsin(j3);
01636 IkReal x156=IKcos(j3);
01637 IkReal x157=((IkReal(1.00000000000000))*(sj4));
01638 IkReal x158=((sj4)*(x155));
01639 IkReal x159=((sj4)*(x156));
01640 IkReal x160=((new_r02)*(x156));
01641 IkReal x161=((new_r12)*(x155));
01642 evalcond[0]=((((new_r12)*(x156)))+(((IkReal(-1.00000000000000))*(new_r02)*(x155))));
01643 evalcond[1]=((((IkReal(-1.00000000000000))*(x157)))+(x160)+(x161));
01644 evalcond[2]=((((new_r00)*(x159)))+(((new_r10)*(x158)))+(((cj4)*(new_r20))));
01645 evalcond[3]=((((new_r01)*(x159)))+(((cj4)*(new_r21)))+(((new_r11)*(x158))));
01646 evalcond[4]=((IkReal(-1.00000000000000))+(((new_r12)*(x158)))+(((cj4)*(new_r22)))+(((new_r02)*(x159))));
01647 evalcond[5]=((((cj4)*(x161)))+(((cj4)*(x160)))+(((IkReal(-1.00000000000000))*(new_r22)*(x157))));
01648 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01649 {
01650 continue;
01651 }
01652 }
01653 
01654 {
01655 IkReal dummyeval[1];
01656 IkReal gconst5;
01657 gconst5=IKsign(sj4);
01658 dummyeval[0]=sj4;
01659 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01660 {
01661 {
01662 IkReal dummyeval[1];
01663 dummyeval[0]=sj4;
01664 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01665 {
01666 {
01667 IkReal dummyeval[1];
01668 dummyeval[0]=sj4;
01669 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01670 {
01671 {
01672 IkReal evalcond[11];
01673 IkReal x162=((IkReal(-1.00000000000000))+(new_r22));
01674 IkReal x163=((((IkReal(-1.00000000000000))*(new_r02)*(sj3)))+(((cj3)*(new_r12))));
01675 IkReal x164=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
01676 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01677 evalcond[1]=x162;
01678 evalcond[2]=new_r20;
01679 evalcond[3]=new_r21;
01680 evalcond[4]=x163;
01681 evalcond[5]=x163;
01682 evalcond[6]=x164;
01683 evalcond[7]=new_r20;
01684 evalcond[8]=new_r21;
01685 evalcond[9]=x162;
01686 evalcond[10]=x164;
01687 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01688 {
01689 {
01690 IkReal j5array[1], cj5array[1], sj5array[1];
01691 bool j5valid[1]={false};
01692 _nj5 = 1;
01693 if( IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01694     continue;
01695 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj3)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01696 sj5array[0]=IKsin(j5array[0]);
01697 cj5array[0]=IKcos(j5array[0]);
01698 if( j5array[0] > IKPI )
01699 {
01700     j5array[0]-=IK2PI;
01701 }
01702 else if( j5array[0] < -IKPI )
01703 {    j5array[0]+=IK2PI;
01704 }
01705 j5valid[0] = true;
01706 for(int ij5 = 0; ij5 < 1; ++ij5)
01707 {
01708 if( !j5valid[ij5] )
01709 {
01710     continue;
01711 }
01712 _ij5[0] = ij5; _ij5[1] = -1;
01713 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01714 {
01715 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01716 {
01717     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01718 }
01719 }
01720 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01721 {
01722 IkReal evalcond[4];
01723 IkReal x165=IKsin(j5);
01724 IkReal x166=((IkReal(1.00000000000000))*(sj3));
01725 IkReal x167=((IkReal(1.00000000000000))*(IKcos(j5)));
01726 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x166)))+(((IkReal(-1.00000000000000))*(x165)))+(((cj3)*(new_r10))));
01727 evalcond[1]=((((IkReal(-1.00000000000000))*(x167)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x166))));
01728 evalcond[2]=((((new_r11)*(sj3)))+(x165)+(((cj3)*(new_r01))));
01729 evalcond[3]=((((IkReal(-1.00000000000000))*(x167)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
01730 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01731 {
01732 continue;
01733 }
01734 }
01735 
01736 {
01737 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01738 vinfos[0].jointtype = 1;
01739 vinfos[0].foffset = j0;
01740 vinfos[0].indices[0] = _ij0[0];
01741 vinfos[0].indices[1] = _ij0[1];
01742 vinfos[0].maxsolutions = _nj0;
01743 vinfos[1].jointtype = 1;
01744 vinfos[1].foffset = j1;
01745 vinfos[1].indices[0] = _ij1[0];
01746 vinfos[1].indices[1] = _ij1[1];
01747 vinfos[1].maxsolutions = _nj1;
01748 vinfos[2].jointtype = 1;
01749 vinfos[2].foffset = j2;
01750 vinfos[2].indices[0] = _ij2[0];
01751 vinfos[2].indices[1] = _ij2[1];
01752 vinfos[2].maxsolutions = _nj2;
01753 vinfos[3].jointtype = 1;
01754 vinfos[3].foffset = j3;
01755 vinfos[3].indices[0] = _ij3[0];
01756 vinfos[3].indices[1] = _ij3[1];
01757 vinfos[3].maxsolutions = _nj3;
01758 vinfos[4].jointtype = 1;
01759 vinfos[4].foffset = j4;
01760 vinfos[4].indices[0] = _ij4[0];
01761 vinfos[4].indices[1] = _ij4[1];
01762 vinfos[4].maxsolutions = _nj4;
01763 vinfos[5].jointtype = 1;
01764 vinfos[5].foffset = j5;
01765 vinfos[5].indices[0] = _ij5[0];
01766 vinfos[5].indices[1] = _ij5[1];
01767 vinfos[5].maxsolutions = _nj5;
01768 std::vector<int> vfree(0);
01769 solutions.AddSolution(vinfos,vfree);
01770 }
01771 }
01772 }
01773 
01774 } else
01775 {
01776 IkReal x168=((new_r12)*(sj3));
01777 IkReal x169=((IkReal(1.00000000000000))*(new_r02));
01778 IkReal x170=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(sj3)*(x169))));
01779 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959))));
01780 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
01781 evalcond[2]=new_r20;
01782 evalcond[3]=new_r21;
01783 evalcond[4]=x170;
01784 evalcond[5]=x170;
01785 evalcond[6]=((x168)+(((cj3)*(new_r02))));
01786 evalcond[7]=((IkReal(-1.00000000000000))*(new_r20));
01787 evalcond[8]=((IkReal(-1.00000000000000))*(new_r21));
01788 evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01789 evalcond[10]=((((IkReal(-1.00000000000000))*(x168)))+(((IkReal(-1.00000000000000))*(cj3)*(x169))));
01790 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01791 {
01792 {
01793 IkReal j5array[1], cj5array[1], sj5array[1];
01794 bool j5valid[1]={false};
01795 _nj5 = 1;
01796 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
01797     continue;
01798 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01799 sj5array[0]=IKsin(j5array[0]);
01800 cj5array[0]=IKcos(j5array[0]);
01801 if( j5array[0] > IKPI )
01802 {
01803     j5array[0]-=IK2PI;
01804 }
01805 else if( j5array[0] < -IKPI )
01806 {    j5array[0]+=IK2PI;
01807 }
01808 j5valid[0] = true;
01809 for(int ij5 = 0; ij5 < 1; ++ij5)
01810 {
01811 if( !j5valid[ij5] )
01812 {
01813     continue;
01814 }
01815 _ij5[0] = ij5; _ij5[1] = -1;
01816 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01817 {
01818 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01819 {
01820     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01821 }
01822 }
01823 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01824 {
01825 IkReal evalcond[4];
01826 IkReal x171=IKcos(j5);
01827 IkReal x172=((IkReal(1.00000000000000))*(sj3));
01828 IkReal x173=((IkReal(1.00000000000000))*(IKsin(j5)));
01829 evalcond[0]=((((IkReal(-1.00000000000000))*(x173)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x172))));
01830 evalcond[1]=((((IkReal(-1.00000000000000))*(x171)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x172))));
01831 evalcond[2]=((((IkReal(-1.00000000000000))*(x173)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
01832 evalcond[3]=((((new_r10)*(sj3)))+(x171)+(((cj3)*(new_r00))));
01833 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01834 {
01835 continue;
01836 }
01837 }
01838 
01839 {
01840 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01841 vinfos[0].jointtype = 1;
01842 vinfos[0].foffset = j0;
01843 vinfos[0].indices[0] = _ij0[0];
01844 vinfos[0].indices[1] = _ij0[1];
01845 vinfos[0].maxsolutions = _nj0;
01846 vinfos[1].jointtype = 1;
01847 vinfos[1].foffset = j1;
01848 vinfos[1].indices[0] = _ij1[0];
01849 vinfos[1].indices[1] = _ij1[1];
01850 vinfos[1].maxsolutions = _nj1;
01851 vinfos[2].jointtype = 1;
01852 vinfos[2].foffset = j2;
01853 vinfos[2].indices[0] = _ij2[0];
01854 vinfos[2].indices[1] = _ij2[1];
01855 vinfos[2].maxsolutions = _nj2;
01856 vinfos[3].jointtype = 1;
01857 vinfos[3].foffset = j3;
01858 vinfos[3].indices[0] = _ij3[0];
01859 vinfos[3].indices[1] = _ij3[1];
01860 vinfos[3].maxsolutions = _nj3;
01861 vinfos[4].jointtype = 1;
01862 vinfos[4].foffset = j4;
01863 vinfos[4].indices[0] = _ij4[0];
01864 vinfos[4].indices[1] = _ij4[1];
01865 vinfos[4].maxsolutions = _nj4;
01866 vinfos[5].jointtype = 1;
01867 vinfos[5].foffset = j5;
01868 vinfos[5].indices[0] = _ij5[0];
01869 vinfos[5].indices[1] = _ij5[1];
01870 vinfos[5].maxsolutions = _nj5;
01871 std::vector<int> vfree(0);
01872 solutions.AddSolution(vinfos,vfree);
01873 }
01874 }
01875 }
01876 
01877 } else
01878 {
01879 if( 1 )
01880 {
01881 continue;
01882 
01883 } else
01884 {
01885 }
01886 }
01887 }
01888 }
01889 
01890 } else
01891 {
01892 {
01893 IkReal j5array[1], cj5array[1], sj5array[1];
01894 bool j5valid[1]={false};
01895 _nj5 = 1;
01896 if( IKabs(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
01897     continue;
01898 j5array[0]=IKatan2(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj3)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
01899 sj5array[0]=IKsin(j5array[0]);
01900 cj5array[0]=IKcos(j5array[0]);
01901 if( j5array[0] > IKPI )
01902 {
01903     j5array[0]-=IK2PI;
01904 }
01905 else if( j5array[0] < -IKPI )
01906 {    j5array[0]+=IK2PI;
01907 }
01908 j5valid[0] = true;
01909 for(int ij5 = 0; ij5 < 1; ++ij5)
01910 {
01911 if( !j5valid[ij5] )
01912 {
01913     continue;
01914 }
01915 _ij5[0] = ij5; _ij5[1] = -1;
01916 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01917 {
01918 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01919 {
01920     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01921 }
01922 }
01923 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01924 {
01925 IkReal evalcond[8];
01926 IkReal x174=IKsin(j5);
01927 IkReal x175=IKcos(j5);
01928 IkReal x176=((IkReal(1.00000000000000))*(sj3));
01929 IkReal x177=((new_r11)*(sj3));
01930 IkReal x178=((new_r10)*(sj3));
01931 IkReal x179=((cj3)*(cj4));
01932 IkReal x180=((IkReal(1.00000000000000))*(sj4));
01933 IkReal x181=((IkReal(1.00000000000000))*(x175));
01934 IkReal x182=((IkReal(1.00000000000000))*(x174));
01935 evalcond[0]=((((sj4)*(x175)))+(new_r20));
01936 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x174)*(x180))));
01937 evalcond[2]=((((IkReal(-1.00000000000000))*(x182)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x176))));
01938 evalcond[3]=((((IkReal(-1.00000000000000))*(x181)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x176))));
01939 evalcond[4]=((((cj4)*(x174)))+(x177)+(((cj3)*(new_r01))));
01940 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x181)))+(x178)+(((cj3)*(new_r00))));
01941 evalcond[6]=((((new_r01)*(x179)))+(((cj4)*(x177)))+(((IkReal(-1.00000000000000))*(new_r21)*(x180)))+(x174));
01942 evalcond[7]=((((new_r00)*(x179)))+(((cj4)*(x178)))+(((IkReal(-1.00000000000000))*(x181)))+(((IkReal(-1.00000000000000))*(new_r20)*(x180))));
01943 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
01944 {
01945 continue;
01946 }
01947 }
01948 
01949 {
01950 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01951 vinfos[0].jointtype = 1;
01952 vinfos[0].foffset = j0;
01953 vinfos[0].indices[0] = _ij0[0];
01954 vinfos[0].indices[1] = _ij0[1];
01955 vinfos[0].maxsolutions = _nj0;
01956 vinfos[1].jointtype = 1;
01957 vinfos[1].foffset = j1;
01958 vinfos[1].indices[0] = _ij1[0];
01959 vinfos[1].indices[1] = _ij1[1];
01960 vinfos[1].maxsolutions = _nj1;
01961 vinfos[2].jointtype = 1;
01962 vinfos[2].foffset = j2;
01963 vinfos[2].indices[0] = _ij2[0];
01964 vinfos[2].indices[1] = _ij2[1];
01965 vinfos[2].maxsolutions = _nj2;
01966 vinfos[3].jointtype = 1;
01967 vinfos[3].foffset = j3;
01968 vinfos[3].indices[0] = _ij3[0];
01969 vinfos[3].indices[1] = _ij3[1];
01970 vinfos[3].maxsolutions = _nj3;
01971 vinfos[4].jointtype = 1;
01972 vinfos[4].foffset = j4;
01973 vinfos[4].indices[0] = _ij4[0];
01974 vinfos[4].indices[1] = _ij4[1];
01975 vinfos[4].maxsolutions = _nj4;
01976 vinfos[5].jointtype = 1;
01977 vinfos[5].foffset = j5;
01978 vinfos[5].indices[0] = _ij5[0];
01979 vinfos[5].indices[1] = _ij5[1];
01980 vinfos[5].maxsolutions = _nj5;
01981 std::vector<int> vfree(0);
01982 solutions.AddSolution(vinfos,vfree);
01983 }
01984 }
01985 }
01986 
01987 }
01988 
01989 }
01990 
01991 } else
01992 {
01993 {
01994 IkReal j5array[1], cj5array[1], sj5array[1];
01995 bool j5valid[1]={false};
01996 _nj5 = 1;
01997 if( IKabs(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
01998     continue;
01999 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
02000 sj5array[0]=IKsin(j5array[0]);
02001 cj5array[0]=IKcos(j5array[0]);
02002 if( j5array[0] > IKPI )
02003 {
02004     j5array[0]-=IK2PI;
02005 }
02006 else if( j5array[0] < -IKPI )
02007 {    j5array[0]+=IK2PI;
02008 }
02009 j5valid[0] = true;
02010 for(int ij5 = 0; ij5 < 1; ++ij5)
02011 {
02012 if( !j5valid[ij5] )
02013 {
02014     continue;
02015 }
02016 _ij5[0] = ij5; _ij5[1] = -1;
02017 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02018 {
02019 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02020 {
02021     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02022 }
02023 }
02024 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02025 {
02026 IkReal evalcond[8];
02027 IkReal x183=IKsin(j5);
02028 IkReal x184=IKcos(j5);
02029 IkReal x185=((IkReal(1.00000000000000))*(sj3));
02030 IkReal x186=((new_r11)*(sj3));
02031 IkReal x187=((new_r10)*(sj3));
02032 IkReal x188=((cj3)*(cj4));
02033 IkReal x189=((IkReal(1.00000000000000))*(sj4));
02034 IkReal x190=((IkReal(1.00000000000000))*(x184));
02035 IkReal x191=((IkReal(1.00000000000000))*(x183));
02036 evalcond[0]=((((sj4)*(x184)))+(new_r20));
02037 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x183)*(x189))));
02038 evalcond[2]=((((IkReal(-1.00000000000000))*(x191)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x185))));
02039 evalcond[3]=((((IkReal(-1.00000000000000))*(x190)))+(((IkReal(-1.00000000000000))*(new_r01)*(x185)))+(((cj3)*(new_r11))));
02040 evalcond[4]=((x186)+(((cj3)*(new_r01)))+(((cj4)*(x183))));
02041 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x190)))+(x187)+(((cj3)*(new_r00))));
02042 evalcond[6]=((((new_r01)*(x188)))+(((IkReal(-1.00000000000000))*(new_r21)*(x189)))+(x183)+(((cj4)*(x186))));
02043 evalcond[7]=((((new_r00)*(x188)))+(((IkReal(-1.00000000000000))*(x190)))+(((IkReal(-1.00000000000000))*(new_r20)*(x189)))+(((cj4)*(x187))));
02044 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
02045 {
02046 continue;
02047 }
02048 }
02049 
02050 {
02051 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02052 vinfos[0].jointtype = 1;
02053 vinfos[0].foffset = j0;
02054 vinfos[0].indices[0] = _ij0[0];
02055 vinfos[0].indices[1] = _ij0[1];
02056 vinfos[0].maxsolutions = _nj0;
02057 vinfos[1].jointtype = 1;
02058 vinfos[1].foffset = j1;
02059 vinfos[1].indices[0] = _ij1[0];
02060 vinfos[1].indices[1] = _ij1[1];
02061 vinfos[1].maxsolutions = _nj1;
02062 vinfos[2].jointtype = 1;
02063 vinfos[2].foffset = j2;
02064 vinfos[2].indices[0] = _ij2[0];
02065 vinfos[2].indices[1] = _ij2[1];
02066 vinfos[2].maxsolutions = _nj2;
02067 vinfos[3].jointtype = 1;
02068 vinfos[3].foffset = j3;
02069 vinfos[3].indices[0] = _ij3[0];
02070 vinfos[3].indices[1] = _ij3[1];
02071 vinfos[3].maxsolutions = _nj3;
02072 vinfos[4].jointtype = 1;
02073 vinfos[4].foffset = j4;
02074 vinfos[4].indices[0] = _ij4[0];
02075 vinfos[4].indices[1] = _ij4[1];
02076 vinfos[4].maxsolutions = _nj4;
02077 vinfos[5].jointtype = 1;
02078 vinfos[5].foffset = j5;
02079 vinfos[5].indices[0] = _ij5[0];
02080 vinfos[5].indices[1] = _ij5[1];
02081 vinfos[5].maxsolutions = _nj5;
02082 std::vector<int> vfree(0);
02083 solutions.AddSolution(vinfos,vfree);
02084 }
02085 }
02086 }
02087 
02088 }
02089 
02090 }
02091 
02092 } else
02093 {
02094 {
02095 IkReal j5array[1], cj5array[1], sj5array[1];
02096 bool j5valid[1]={false};
02097 _nj5 = 1;
02098 if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02099     continue;
02100 j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(new_r20)));
02101 sj5array[0]=IKsin(j5array[0]);
02102 cj5array[0]=IKcos(j5array[0]);
02103 if( j5array[0] > IKPI )
02104 {
02105     j5array[0]-=IK2PI;
02106 }
02107 else if( j5array[0] < -IKPI )
02108 {    j5array[0]+=IK2PI;
02109 }
02110 j5valid[0] = true;
02111 for(int ij5 = 0; ij5 < 1; ++ij5)
02112 {
02113 if( !j5valid[ij5] )
02114 {
02115     continue;
02116 }
02117 _ij5[0] = ij5; _ij5[1] = -1;
02118 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02119 {
02120 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02121 {
02122     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02123 }
02124 }
02125 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02126 {
02127 IkReal evalcond[8];
02128 IkReal x192=IKsin(j5);
02129 IkReal x193=IKcos(j5);
02130 IkReal x194=((IkReal(1.00000000000000))*(sj3));
02131 IkReal x195=((new_r11)*(sj3));
02132 IkReal x196=((new_r10)*(sj3));
02133 IkReal x197=((cj3)*(cj4));
02134 IkReal x198=((IkReal(1.00000000000000))*(sj4));
02135 IkReal x199=((IkReal(1.00000000000000))*(x193));
02136 IkReal x200=((IkReal(1.00000000000000))*(x192));
02137 evalcond[0]=((((sj4)*(x193)))+(new_r20));
02138 evalcond[1]=((((IkReal(-1.00000000000000))*(x192)*(x198)))+(new_r21));
02139 evalcond[2]=((((IkReal(-1.00000000000000))*(x200)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x194))));
02140 evalcond[3]=((((IkReal(-1.00000000000000))*(x199)))+(((IkReal(-1.00000000000000))*(new_r01)*(x194)))+(((cj3)*(new_r11))));
02141 evalcond[4]=((((cj4)*(x192)))+(x195)+(((cj3)*(new_r01))));
02142 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x199)))+(x196)+(((cj3)*(new_r00))));
02143 evalcond[6]=((((cj4)*(x195)))+(((new_r01)*(x197)))+(((IkReal(-1.00000000000000))*(new_r21)*(x198)))+(x192));
02144 evalcond[7]=((((cj4)*(x196)))+(((new_r00)*(x197)))+(((IkReal(-1.00000000000000))*(x199)))+(((IkReal(-1.00000000000000))*(new_r20)*(x198))));
02145 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
02146 {
02147 continue;
02148 }
02149 }
02150 
02151 {
02152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02153 vinfos[0].jointtype = 1;
02154 vinfos[0].foffset = j0;
02155 vinfos[0].indices[0] = _ij0[0];
02156 vinfos[0].indices[1] = _ij0[1];
02157 vinfos[0].maxsolutions = _nj0;
02158 vinfos[1].jointtype = 1;
02159 vinfos[1].foffset = j1;
02160 vinfos[1].indices[0] = _ij1[0];
02161 vinfos[1].indices[1] = _ij1[1];
02162 vinfos[1].maxsolutions = _nj1;
02163 vinfos[2].jointtype = 1;
02164 vinfos[2].foffset = j2;
02165 vinfos[2].indices[0] = _ij2[0];
02166 vinfos[2].indices[1] = _ij2[1];
02167 vinfos[2].maxsolutions = _nj2;
02168 vinfos[3].jointtype = 1;
02169 vinfos[3].foffset = j3;
02170 vinfos[3].indices[0] = _ij3[0];
02171 vinfos[3].indices[1] = _ij3[1];
02172 vinfos[3].maxsolutions = _nj3;
02173 vinfos[4].jointtype = 1;
02174 vinfos[4].foffset = j4;
02175 vinfos[4].indices[0] = _ij4[0];
02176 vinfos[4].indices[1] = _ij4[1];
02177 vinfos[4].maxsolutions = _nj4;
02178 vinfos[5].jointtype = 1;
02179 vinfos[5].foffset = j5;
02180 vinfos[5].indices[0] = _ij5[0];
02181 vinfos[5].indices[1] = _ij5[1];
02182 vinfos[5].maxsolutions = _nj5;
02183 std::vector<int> vfree(0);
02184 solutions.AddSolution(vinfos,vfree);
02185 }
02186 }
02187 }
02188 
02189 }
02190 
02191 }
02192 }
02193 }
02194 
02195 }
02196 
02197 }
02198 
02199 } else
02200 {
02201 {
02202 IkReal j5array[1], cj5array[1], sj5array[1];
02203 bool j5valid[1]={false};
02204 _nj5 = 1;
02205 if( IKabs(((gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02206     continue;
02207 j5array[0]=IKatan2(((gconst4)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst4)*(new_r20)));
02208 sj5array[0]=IKsin(j5array[0]);
02209 cj5array[0]=IKcos(j5array[0]);
02210 if( j5array[0] > IKPI )
02211 {
02212     j5array[0]-=IK2PI;
02213 }
02214 else if( j5array[0] < -IKPI )
02215 {    j5array[0]+=IK2PI;
02216 }
02217 j5valid[0] = true;
02218 for(int ij5 = 0; ij5 < 1; ++ij5)
02219 {
02220 if( !j5valid[ij5] )
02221 {
02222     continue;
02223 }
02224 _ij5[0] = ij5; _ij5[1] = -1;
02225 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02226 {
02227 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02228 {
02229     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02230 }
02231 }
02232 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02233 {
02234 IkReal evalcond[2];
02235 evalcond[0]=((((sj4)*(IKcos(j5))))+(new_r20));
02236 evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(IKsin(j5))))+(new_r21));
02237 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02238 {
02239 continue;
02240 }
02241 }
02242 
02243 {
02244 IkReal dummyeval[1];
02245 IkReal gconst6;
02246 gconst6=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
02247 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
02248 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02249 {
02250 {
02251 IkReal dummyeval[1];
02252 IkReal gconst7;
02253 gconst7=IKsign(((((IkReal(-1.00000000000000))*(new_r11)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))));
02254 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r11)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r01)*(new_r02))));
02255 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02256 {
02257 continue;
02258 
02259 } else
02260 {
02261 {
02262 IkReal j3array[1], cj3array[1], sj3array[1];
02263 bool j3valid[1]={false};
02264 _nj3 = 1;
02265 IkReal x201=((cj4)*(gconst7)*(sj5));
02266 if( IKabs(((new_r12)*(x201))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x201))) < IKFAST_ATAN2_MAGTHRESH )
02267     continue;
02268 j3array[0]=IKatan2(((new_r12)*(x201)), ((new_r02)*(x201)));
02269 sj3array[0]=IKsin(j3array[0]);
02270 cj3array[0]=IKcos(j3array[0]);
02271 if( j3array[0] > IKPI )
02272 {
02273     j3array[0]-=IK2PI;
02274 }
02275 else if( j3array[0] < -IKPI )
02276 {    j3array[0]+=IK2PI;
02277 }
02278 j3valid[0] = true;
02279 for(int ij3 = 0; ij3 < 1; ++ij3)
02280 {
02281 if( !j3valid[ij3] )
02282 {
02283     continue;
02284 }
02285 _ij3[0] = ij3; _ij3[1] = -1;
02286 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02287 {
02288 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02289 {
02290     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02291 }
02292 }
02293 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02294 {
02295 IkReal evalcond[12];
02296 IkReal x202=IKsin(j3);
02297 IkReal x203=IKcos(j3);
02298 IkReal x204=((IkReal(1.00000000000000))*(cj5));
02299 IkReal x205=((IkReal(1.00000000000000))*(sj4));
02300 IkReal x206=((cj4)*(x203));
02301 IkReal x207=((sj4)*(x203));
02302 IkReal x208=((cj4)*(x202));
02303 IkReal x209=((new_r11)*(x202));
02304 IkReal x210=((sj4)*(x202));
02305 IkReal x211=((IkReal(1.00000000000000))*(x202));
02306 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x211)))+(((new_r12)*(x203))));
02307 evalcond[1]=((((new_r02)*(x203)))+(((new_r12)*(x202)))+(((IkReal(-1.00000000000000))*(x205))));
02308 evalcond[2]=((((IkReal(-1.00000000000000))*(sj5)))+(((new_r10)*(x203)))+(((IkReal(-1.00000000000000))*(new_r00)*(x211))));
02309 evalcond[3]=((((new_r11)*(x203)))+(((IkReal(-1.00000000000000))*(x204)))+(((IkReal(-1.00000000000000))*(new_r01)*(x211))));
02310 evalcond[4]=((((new_r01)*(x203)))+(((cj4)*(sj5)))+(x209));
02311 evalcond[5]=((((new_r10)*(x202)))+(((new_r00)*(x203)))+(((IkReal(-1.00000000000000))*(cj4)*(x204))));
02312 evalcond[6]=((((new_r00)*(x207)))+(((cj4)*(new_r20)))+(((new_r10)*(x210))));
02313 evalcond[7]=((((cj4)*(new_r21)))+(((new_r01)*(x207)))+(((sj4)*(x209))));
02314 evalcond[8]=((IkReal(-1.00000000000000))+(((new_r02)*(x207)))+(((cj4)*(new_r22)))+(((new_r12)*(x210))));
02315 evalcond[9]=((((IkReal(-1.00000000000000))*(new_r22)*(x205)))+(((new_r02)*(x206)))+(((new_r12)*(x208))));
02316 evalcond[10]=((((IkReal(-1.00000000000000))*(new_r21)*(x205)))+(sj5)+(((new_r11)*(x208)))+(((new_r01)*(x206))));
02317 evalcond[11]=((((new_r10)*(x208)))+(((new_r00)*(x206)))+(((IkReal(-1.00000000000000))*(new_r20)*(x205)))+(((IkReal(-1.00000000000000))*(x204))));
02318 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  || IKabs(evalcond[8]) > 0.000001  || IKabs(evalcond[9]) > 0.000001  || IKabs(evalcond[10]) > 0.000001  || IKabs(evalcond[11]) > 0.000001  )
02319 {
02320 continue;
02321 }
02322 }
02323 
02324 {
02325 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02326 vinfos[0].jointtype = 1;
02327 vinfos[0].foffset = j0;
02328 vinfos[0].indices[0] = _ij0[0];
02329 vinfos[0].indices[1] = _ij0[1];
02330 vinfos[0].maxsolutions = _nj0;
02331 vinfos[1].jointtype = 1;
02332 vinfos[1].foffset = j1;
02333 vinfos[1].indices[0] = _ij1[0];
02334 vinfos[1].indices[1] = _ij1[1];
02335 vinfos[1].maxsolutions = _nj1;
02336 vinfos[2].jointtype = 1;
02337 vinfos[2].foffset = j2;
02338 vinfos[2].indices[0] = _ij2[0];
02339 vinfos[2].indices[1] = _ij2[1];
02340 vinfos[2].maxsolutions = _nj2;
02341 vinfos[3].jointtype = 1;
02342 vinfos[3].foffset = j3;
02343 vinfos[3].indices[0] = _ij3[0];
02344 vinfos[3].indices[1] = _ij3[1];
02345 vinfos[3].maxsolutions = _nj3;
02346 vinfos[4].jointtype = 1;
02347 vinfos[4].foffset = j4;
02348 vinfos[4].indices[0] = _ij4[0];
02349 vinfos[4].indices[1] = _ij4[1];
02350 vinfos[4].maxsolutions = _nj4;
02351 vinfos[5].jointtype = 1;
02352 vinfos[5].foffset = j5;
02353 vinfos[5].indices[0] = _ij5[0];
02354 vinfos[5].indices[1] = _ij5[1];
02355 vinfos[5].maxsolutions = _nj5;
02356 std::vector<int> vfree(0);
02357 solutions.AddSolution(vinfos,vfree);
02358 }
02359 }
02360 }
02361 
02362 }
02363 
02364 }
02365 
02366 } else
02367 {
02368 {
02369 IkReal j3array[1], cj3array[1], sj3array[1];
02370 bool j3valid[1]={false};
02371 _nj3 = 1;
02372 IkReal x212=((gconst6)*(sj4));
02373 if( IKabs(((new_r12)*(x212))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x212))) < IKFAST_ATAN2_MAGTHRESH )
02374     continue;
02375 j3array[0]=IKatan2(((new_r12)*(x212)), ((new_r02)*(x212)));
02376 sj3array[0]=IKsin(j3array[0]);
02377 cj3array[0]=IKcos(j3array[0]);
02378 if( j3array[0] > IKPI )
02379 {
02380     j3array[0]-=IK2PI;
02381 }
02382 else if( j3array[0] < -IKPI )
02383 {    j3array[0]+=IK2PI;
02384 }
02385 j3valid[0] = true;
02386 for(int ij3 = 0; ij3 < 1; ++ij3)
02387 {
02388 if( !j3valid[ij3] )
02389 {
02390     continue;
02391 }
02392 _ij3[0] = ij3; _ij3[1] = -1;
02393 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02394 {
02395 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02396 {
02397     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02398 }
02399 }
02400 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02401 {
02402 IkReal evalcond[12];
02403 IkReal x213=IKsin(j3);
02404 IkReal x214=IKcos(j3);
02405 IkReal x215=((IkReal(1.00000000000000))*(cj5));
02406 IkReal x216=((IkReal(1.00000000000000))*(sj4));
02407 IkReal x217=((cj4)*(x214));
02408 IkReal x218=((sj4)*(x214));
02409 IkReal x219=((cj4)*(x213));
02410 IkReal x220=((new_r11)*(x213));
02411 IkReal x221=((sj4)*(x213));
02412 IkReal x222=((IkReal(1.00000000000000))*(x213));
02413 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x222)))+(((new_r12)*(x214))));
02414 evalcond[1]=((((new_r02)*(x214)))+(((IkReal(-1.00000000000000))*(x216)))+(((new_r12)*(x213))));
02415 evalcond[2]=((((IkReal(-1.00000000000000))*(sj5)))+(((new_r10)*(x214)))+(((IkReal(-1.00000000000000))*(new_r00)*(x222))));
02416 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x222)))+(((IkReal(-1.00000000000000))*(x215)))+(((new_r11)*(x214))));
02417 evalcond[4]=((((cj4)*(sj5)))+(x220)+(((new_r01)*(x214))));
02418 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x215)))+(((new_r00)*(x214)))+(((new_r10)*(x213))));
02419 evalcond[6]=((((cj4)*(new_r20)))+(((new_r00)*(x218)))+(((new_r10)*(x221))));
02420 evalcond[7]=((((sj4)*(x220)))+(((cj4)*(new_r21)))+(((new_r01)*(x218))));
02421 evalcond[8]=((IkReal(-1.00000000000000))+(((cj4)*(new_r22)))+(((new_r12)*(x221)))+(((new_r02)*(x218))));
02422 evalcond[9]=((((new_r02)*(x217)))+(((IkReal(-1.00000000000000))*(new_r22)*(x216)))+(((new_r12)*(x219))));
02423 evalcond[10]=((sj5)+(((IkReal(-1.00000000000000))*(new_r21)*(x216)))+(((new_r01)*(x217)))+(((new_r11)*(x219))));
02424 evalcond[11]=((((new_r00)*(x217)))+(((IkReal(-1.00000000000000))*(new_r20)*(x216)))+(((IkReal(-1.00000000000000))*(x215)))+(((new_r10)*(x219))));
02425 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  || IKabs(evalcond[8]) > 0.000001  || IKabs(evalcond[9]) > 0.000001  || IKabs(evalcond[10]) > 0.000001  || IKabs(evalcond[11]) > 0.000001  )
02426 {
02427 continue;
02428 }
02429 }
02430 
02431 {
02432 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02433 vinfos[0].jointtype = 1;
02434 vinfos[0].foffset = j0;
02435 vinfos[0].indices[0] = _ij0[0];
02436 vinfos[0].indices[1] = _ij0[1];
02437 vinfos[0].maxsolutions = _nj0;
02438 vinfos[1].jointtype = 1;
02439 vinfos[1].foffset = j1;
02440 vinfos[1].indices[0] = _ij1[0];
02441 vinfos[1].indices[1] = _ij1[1];
02442 vinfos[1].maxsolutions = _nj1;
02443 vinfos[2].jointtype = 1;
02444 vinfos[2].foffset = j2;
02445 vinfos[2].indices[0] = _ij2[0];
02446 vinfos[2].indices[1] = _ij2[1];
02447 vinfos[2].maxsolutions = _nj2;
02448 vinfos[3].jointtype = 1;
02449 vinfos[3].foffset = j3;
02450 vinfos[3].indices[0] = _ij3[0];
02451 vinfos[3].indices[1] = _ij3[1];
02452 vinfos[3].maxsolutions = _nj3;
02453 vinfos[4].jointtype = 1;
02454 vinfos[4].foffset = j4;
02455 vinfos[4].indices[0] = _ij4[0];
02456 vinfos[4].indices[1] = _ij4[1];
02457 vinfos[4].maxsolutions = _nj4;
02458 vinfos[5].jointtype = 1;
02459 vinfos[5].foffset = j5;
02460 vinfos[5].indices[0] = _ij5[0];
02461 vinfos[5].indices[1] = _ij5[1];
02462 vinfos[5].maxsolutions = _nj5;
02463 std::vector<int> vfree(0);
02464 solutions.AddSolution(vinfos,vfree);
02465 }
02466 }
02467 }
02468 
02469 }
02470 
02471 }
02472 }
02473 }
02474 
02475 }
02476 
02477 }
02478 }
02479 }
02480 }
02481 }};
02482 
02483 
02486 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
02487 IKSolver solver;
02488 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
02489 }
02490 
02491 IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - irb_2400 (039a63a10572e01f7134c300303ec527)>"; }
02492 
02493 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
02494 
02495 #ifdef IKFAST_NAMESPACE
02496 } // end namespace
02497 #endif
02498 
02499 #ifndef IKFAST_NO_MAIN
02500 #include <stdio.h>
02501 #include <stdlib.h>
02502 #ifdef IKFAST_NAMESPACE
02503 using namespace IKFAST_NAMESPACE;
02504 #endif
02505 int main(int argc, char** argv)
02506 {
02507     if( argc != 12+GetNumFreeParameters()+1 ) {
02508         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
02509                "Returns the ik solutions given the transformation of the end effector specified by\n"
02510                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
02511                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
02512         return 1;
02513     }
02514 
02515     IkSolutionList<IkReal> solutions;
02516     std::vector<IkReal> vfree(GetNumFreeParameters());
02517     IkReal eerot[9],eetrans[3];
02518     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
02519     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
02520     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
02521     for(std::size_t i = 0; i < vfree.size(); ++i)
02522         vfree[i] = atof(argv[13+i]);
02523     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
02524 
02525     if( !bSuccess ) {
02526         fprintf(stderr,"Failed to get ik solution\n");
02527         return -1;
02528     }
02529 
02530     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
02531     std::vector<IkReal> solvalues(GetNumJoints());
02532     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
02533         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
02534         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
02535         std::vector<IkReal> vsolfree(sol.GetFree().size());
02536         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
02537         for( std::size_t j = 0; j < solvalues.size(); ++j)
02538             printf("%.15f, ", solvalues[j]);
02539         printf("\n");
02540     }
02541     return 0;
02542 }
02543 
02544 #endif


irb_2400_arm_navigation
Author(s): Shaun Edwards
autogenerated on Sat Apr 12 2014 13:57:18