staubli_rx160_moveit_plugins: staubli_rx160_manipulator_ikfast

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,x48,x49;
00212 x0=IKcos(j[0]);
00213 x1=IKcos(j[1]);
00214 x2=IKcos(j[2]);
00215 x3=IKsin(j[1]);
00216 x4=IKsin(j[2]);
00217 x5=IKsin(j[3]);
00218 x6=IKcos(j[3]);
00219 x7=IKsin(j[0]);
00220 x8=IKcos(j[5]);
00221 x9=IKsin(j[5]);
00222 x10=IKcos(j[4]);
00223 x11=IKsin(j[4]);
00224 x12=((IkReal(0.110000000000000))*(x6));
00225 x13=((IkReal(0.110000000000000))*(x0));
00226 x14=((IkReal(1.00000000000000))*(x7));
00227 x15=((IkReal(1.00000000000000))*(x11));
00228 x16=((IkReal(0.625000000000000))*(x7));
00229 x17=((IkReal(1.00000000000000))*(x0));
00230 x18=((IkReal(1.00000000000000))*(x10));
00231 x19=((IkReal(0.110000000000000))*(x7));
00232 x20=((IkReal(0.625000000000000))*(x0));
00233 x21=((IkReal(0.825000000000000))*(x3));
00234 x22=((x1)*(x2));
00235 x23=((x1)*(x4));
00236 x24=((x3)*(x4));
00237 x25=((x2)*(x3));
00238 x26=((x11)*(x6));
00239 x27=((IkReal(1.00000000000000))*(x22));
00240 x28=((IkReal(1.00000000000000))*(x25));
00241 x29=((x17)*(x24));
00242 x30=((x14)*(x24));
00243 x31=((x24)+(((IkReal(-1.00000000000000))*(x27))));
00244 x32=((x28)+(((IkReal(1.00000000000000))*(x23))));
00245 x33=((IkReal(-1.00000000000000))*(x32));
00246 x34=((((IkReal(-1.00000000000000))*(x29)))+(((x0)*(x22))));
00247 x35=((((x22)*(x7)))+(((IkReal(-1.00000000000000))*(x30))));
00248 x36=((x33)*(x6));
00249 x37=((x32)*(x5));
00250 x38=((x17)*(((((IkReal(-1.00000000000000))*(x23)))+(((IkReal(-1.00000000000000))*(x25))))));
00251 x39=((x17)*(((x25)+(x23))));
00252 x40=((x14)*(((((IkReal(-1.00000000000000))*(x23)))+(((IkReal(-1.00000000000000))*(x25))))));
00253 x41=((x14)*(((x25)+(x23))));
00254 x42=((x35)*(x6));
00255 x43=((x10)*(x36));
00256 x44=((x11)*(x38));
00257 x45=((((IkReal(-1.00000000000000))*(x14)*(x5)))+(((x34)*(x6))));
00258 x46=((x42)+(((x0)*(x5))));
00259 x47=((((x5)*(((x29)+(((IkReal(-1.00000000000000))*(x17)*(x22)))))))+(((IkReal(-1.00000000000000))*(x14)*(x6))));
00260 x48=((((x0)*(x6)))+(((x5)*(((((IkReal(-1.00000000000000))*(x14)*(x22)))+(x30))))));
00261 x49=((x10)*(x45));
00262 eerot[0]=((((x8)*(((x49)+(x44)))))+(((x47)*(x9))));
00263 eerot[1]=((((x9)*(((((IkReal(-1.00000000000000))*(x15)*(x38)))+(((IkReal(-1.00000000000000))*(x18)*(x45)))))))+(((x47)*(x8))));
00264 eerot[2]=((((x10)*(x39)))+(((x11)*(x45))));
00265 eetrans[0]=((((x10)*(((((x13)*(x23)))+(((x13)*(x25)))))))+(((x0)*(x21)))+(((IkReal(0.150000000000000))*(x0)))+(((x11)*(((((IkReal(-1.00000000000000))*(x19)*(x5)))+(((x12)*(x34)))))))+(((x20)*(x25)))+(((x20)*(x23))));
00266 eerot[3]=((((x8)*(((((x11)*(x40)))+(((x10)*(x46)))))))+(((x48)*(x9))));
00267 eerot[4]=((((x9)*(((((IkReal(-1.00000000000000))*(x18)*(x46)))+(((IkReal(-1.00000000000000))*(x15)*(x40)))))))+(((x48)*(x8))));
00268 eerot[5]=((((x11)*(x46)))+(((x10)*(x41))));
00269 eetrans[1]=((((IkReal(0.150000000000000))*(x7)))+(((x21)*(x7)))+(((x11)*(((((x12)*(x35)))+(((x13)*(x5)))))))+(((x16)*(x23)))+(((x16)*(x25)))+(((x10)*(((((x19)*(x25)))+(((x19)*(x23))))))));
00270 eerot[6]=((((x8)*(((x43)+(((x11)*(x31)))))))+(((x37)*(x9))));
00271 eerot[7]=((((x9)*(((((IkReal(-1.00000000000000))*(x18)*(x36)))+(((IkReal(-1.00000000000000))*(x15)*(x31)))))))+(((x37)*(x8))));
00272 eerot[8]=((((x10)*(((x27)+(((IkReal(-1.00000000000000))*(x24)))))))+(((x26)*(x33))));
00273 eetrans[2]=((IkReal(0.550000000000000))+(((IkReal(0.825000000000000))*(x1)))+(((IkReal(-0.625000000000000))*(x24)))+(((IkReal(0.625000000000000))*(x22)))+(((x26)*(((((IkReal(-0.110000000000000))*(x23)))+(((IkReal(-0.110000000000000))*(x25)))))))+(((x10)*(((((IkReal(-0.110000000000000))*(x24)))+(((IkReal(0.110000000000000))*(x22))))))));
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=r00;
00305 new_r01=r01;
00306 new_r02=r02;
00307 new_px=((px)+(((IkReal(-0.110000000000000))*(r02))));
00308 new_r10=r10;
00309 new_r11=r11;
00310 new_r12=r12;
00311 new_py=((((IkReal(-0.110000000000000))*(r12)))+(py));
00312 new_r20=r20;
00313 new_r21=r21;
00314 new_r22=r22;
00315 new_pz=((IkReal(-0.550000000000000))+(((IkReal(-0.110000000000000))*(r22)))+(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))+((pz)*(pz))+((py)*(py)));
00318 npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00))));
00319 npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11))));
00320 npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02))));
00321 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00322 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00323 rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10))));
00324 rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21))));
00325 rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21))));
00326 rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11))));
00327 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00328 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00329 rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12))));
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 cj2array[0]=((IkReal(-1.01696969696970))+(((IkReal(-0.290909090909091))*(py)*(sj0)))+(((IkReal(-0.290909090909091))*(cj0)*(px)))+(((IkReal(0.969696969696970))*(pp))));
00380 if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
00381 {
00382     j2valid[0] = j2valid[1] = true;
00383     j2array[0] = IKacos(cj2array[0]);
00384     sj2array[0] = IKsin(j2array[0]);
00385     cj2array[1] = cj2array[0];
00386     j2array[1] = -j2array[0];
00387     sj2array[1] = -sj2array[0];
00388 }
00389 else if( isnan(cj2array[0]) )
00390 {
00391     // probably any value will work
00392     j2valid[0] = true;
00393     cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
00394 }
00395 for(int ij2 = 0; ij2 < 2; ++ij2)
00396 {
00397 if( !j2valid[ij2] )
00398 {
00399     continue;
00400 }
00401 _ij2[0] = ij2; _ij2[1] = -1;
00402 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
00403 {
00404 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00405 {
00406     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00407 }
00408 }
00409 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00410 
00411 {
00412 IkReal dummyeval[1];
00413 IkReal gconst0;
00414 IkReal x51=((IkReal(5.00000000000000))*(cj2));
00415 IkReal x52=((py)*(sj0));
00416 IkReal x53=((cj0)*(px));
00417 gconst0=IKsign(((IkReal(0.990000000000000))+(((IkReal(0.750000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x51)*(x53)))+(((IkReal(-5.00000000000000))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x51)*(x52)))+(((IkReal(-6.60000000000000))*(x53)))+(((IkReal(-6.60000000000000))*(x52)))));
00418 IkReal x54=((py)*(sj0));
00419 IkReal x55=((cj0)*(px));
00420 IkReal x56=((IkReal(6.66666666666667))*(cj2));
00421 dummyeval[0]=((IkReal(1.32000000000000))+(((IkReal(-6.66666666666667))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x54)*(x56)))+(((IkReal(-8.80000000000000))*(x54)))+(((IkReal(-8.80000000000000))*(x55)))+(((IkReal(-1.00000000000000))*(x55)*(x56)))+(cj2));
00422 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00423 {
00424 {
00425 IkReal dummyeval[1];
00426 IkReal x57=((IkReal(6.66666666666667))*(sj2));
00427 dummyeval[0]=((((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-6.66666666666667))*(cj2)*(pz)))+(((IkReal(-8.80000000000000))*(pz)))+(((cj0)*(px)*(x57)))+(((py)*(sj0)*(x57))));
00428 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00429 {
00430 continue;
00431 
00432 } else
00433 {
00434 {
00435 IkReal j1array[1], cj1array[1], sj1array[1];
00436 bool j1valid[1]={false};
00437 _nj1 = 1;
00438 IkReal x58=((py)*(sj0));
00439 IkReal x59=((IkReal(40.0000000000000))*(sj2));
00440 IkReal x60=((IkReal(8.00000000000000))*(pz));
00441 IkReal x61=((cj0)*(px));
00442 IkReal x62=((cj2)*(pz));
00443 IkReal x63=((IkReal(5.00000000000000))*(sj2));
00444 if( IKabs(((((IKabs(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63))))) != 0)?((IkReal)1/(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x60)*(x61)))+(((IkReal(-1.00000000000000))*(x58)*(x60)))+(((IkReal(4.12500000000000))*(sj2)))+(((IkReal(3.12500000000000))*(cj2)*(sj2)))+(((IkReal(1.20000000000000))*(pz))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59))))) != 0)?((IkReal)1/(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59)))))):(IkReal)1.0e30))*(((((IkReal(-64.0000000000000))*((pz)*(pz))))+(((IkReal(25.0000000000000))*((sj2)*(sj2)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63))))) != 0)?((IkReal)1/(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x60)*(x61)))+(((IkReal(-1.00000000000000))*(x58)*(x60)))+(((IkReal(4.12500000000000))*(sj2)))+(((IkReal(3.12500000000000))*(cj2)*(sj2)))+(((IkReal(1.20000000000000))*(pz)))))))+IKsqr(((((IKabs(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59))))) != 0)?((IkReal)1/(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59)))))):(IkReal)1.0e30))*(((((IkReal(-64.0000000000000))*((pz)*(pz))))+(((IkReal(25.0000000000000))*((sj2)*(sj2))))))))-1) <= IKFAST_SINCOS_THRESH )
00445     continue;
00446 j1array[0]=IKatan2(((((IKabs(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63))))) != 0)?((IkReal)1/(((((IkReal(-5.00000000000000))*(x62)))+(((IkReal(-6.60000000000000))*(pz)))+(((x61)*(x63)))+(((IkReal(-0.750000000000000))*(sj2)))+(((x58)*(x63)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x60)*(x61)))+(((IkReal(-1.00000000000000))*(x58)*(x60)))+(((IkReal(4.12500000000000))*(sj2)))+(((IkReal(3.12500000000000))*(cj2)*(sj2)))+(((IkReal(1.20000000000000))*(pz)))))), ((((IKabs(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59))))) != 0)?((IkReal)1/(((((x59)*(x61)))+(((IkReal(-6.00000000000000))*(sj2)))+(((IkReal(-52.8000000000000))*(pz)))+(((IkReal(-40.0000000000000))*(x62)))+(((x58)*(x59)))))):(IkReal)1.0e30))*(((((IkReal(-64.0000000000000))*((pz)*(pz))))+(((IkReal(25.0000000000000))*((sj2)*(sj2))))))));
00447 sj1array[0]=IKsin(j1array[0]);
00448 cj1array[0]=IKcos(j1array[0]);
00449 if( j1array[0] > IKPI )
00450 {
00451     j1array[0]-=IK2PI;
00452 }
00453 else if( j1array[0] < -IKPI )
00454 {    j1array[0]+=IK2PI;
00455 }
00456 j1valid[0] = true;
00457 for(int ij1 = 0; ij1 < 1; ++ij1)
00458 {
00459 if( !j1valid[ij1] )
00460 {
00461     continue;
00462 }
00463 _ij1[0] = ij1; _ij1[1] = -1;
00464 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00465 {
00466 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00467 {
00468     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00469 }
00470 }
00471 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00472 {
00473 IkReal evalcond[5];
00474 IkReal x64=IKcos(j1);
00475 IkReal x65=IKsin(j1);
00476 IkReal x66=((IkReal(0.625000000000000))*(cj2));
00477 IkReal x67=((cj0)*(px));
00478 IkReal x68=((IkReal(0.625000000000000))*(sj2));
00479 IkReal x69=((py)*(sj0));
00480 IkReal x70=((IkReal(1.00000000000000))*(x64));
00481 IkReal x71=((pz)*(x64));
00482 IkReal x72=((x65)*(x69));
00483 evalcond[0]=((((IkReal(-1.00000000000000))*(x64)*(x66)))+(((x65)*(x68)))+(((IkReal(-0.825000000000000))*(x64)))+(pz));
00484 evalcond[1]=((IkReal(-0.825000000000000))+(((x65)*(x67)))+(((IkReal(-1.00000000000000))*(x66)))+(((IkReal(-0.150000000000000))*(x65)))+(x72)+(x71));
00485 evalcond[2]=((((IkReal(0.150000000000000))*(x64)))+(((IkReal(-1.00000000000000))*(x67)*(x70)))+(x68)+(((IkReal(-1.00000000000000))*(x69)*(x70)))+(((pz)*(x65))));
00486 evalcond[3]=((IkReal(0.150000000000000))+(((x65)*(x66)))+(((IkReal(-1.00000000000000))*(x69)))+(((IkReal(-1.00000000000000))*(x67)))+(((x64)*(x68)))+(((IkReal(0.825000000000000))*(x65))));
00487 evalcond[4]=((IkReal(-0.312500000000000))+(((IkReal(0.300000000000000))*(x67)))+(((IkReal(-0.247500000000000))*(x65)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(1.65000000000000))*(x72)))+(((IkReal(1.65000000000000))*(x65)*(x67)))+(((IkReal(1.65000000000000))*(x71)))+(((IkReal(0.300000000000000))*(x69))));
00488 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  )
00489 {
00490 continue;
00491 }
00492 }
00493 
00494 rotationfunction0(solutions);
00495 }
00496 }
00497 
00498 }
00499 
00500 }
00501 
00502 } else
00503 {
00504 {
00505 IkReal j1array[1], cj1array[1], sj1array[1];
00506 bool j1valid[1]={false};
00507 _nj1 = 1;
00508 IkReal x228=((IkReal(8.00000000000000))*(pz));
00509 if( IKabs(((gconst0)*(((IkReal(-5.44500000000000))+(((pz)*(x228)))+(((IkReal(-3.12500000000000))*((cj2)*(cj2))))+(((IkReal(-8.25000000000000))*(cj2))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-4.12500000000000))*(sj2)))+(((IkReal(-3.12500000000000))*(cj2)*(sj2)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x228)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x228)))+(((IkReal(1.20000000000000))*(pz))))))) < IKFAST_ATAN2_MAGTHRESH )
00510     continue;
00511 j1array[0]=IKatan2(((gconst0)*(((IkReal(-5.44500000000000))+(((pz)*(x228)))+(((IkReal(-3.12500000000000))*((cj2)*(cj2))))+(((IkReal(-8.25000000000000))*(cj2)))))), ((gconst0)*(((((IkReal(-4.12500000000000))*(sj2)))+(((IkReal(-3.12500000000000))*(cj2)*(sj2)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x228)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x228)))+(((IkReal(1.20000000000000))*(pz)))))));
00512 sj1array[0]=IKsin(j1array[0]);
00513 cj1array[0]=IKcos(j1array[0]);
00514 if( j1array[0] > IKPI )
00515 {
00516     j1array[0]-=IK2PI;
00517 }
00518 else if( j1array[0] < -IKPI )
00519 {    j1array[0]+=IK2PI;
00520 }
00521 j1valid[0] = true;
00522 for(int ij1 = 0; ij1 < 1; ++ij1)
00523 {
00524 if( !j1valid[ij1] )
00525 {
00526     continue;
00527 }
00528 _ij1[0] = ij1; _ij1[1] = -1;
00529 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00530 {
00531 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00532 {
00533     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00534 }
00535 }
00536 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00537 {
00538 IkReal evalcond[5];
00539 IkReal x229=IKcos(j1);
00540 IkReal x230=IKsin(j1);
00541 IkReal x231=((IkReal(0.625000000000000))*(cj2));
00542 IkReal x232=((cj0)*(px));
00543 IkReal x233=((IkReal(0.625000000000000))*(sj2));
00544 IkReal x234=((py)*(sj0));
00545 IkReal x235=((IkReal(1.00000000000000))*(x229));
00546 IkReal x236=((pz)*(x229));
00547 IkReal x237=((x230)*(x234));
00548 evalcond[0]=((((x230)*(x233)))+(pz)+(((IkReal(-1.00000000000000))*(x229)*(x231)))+(((IkReal(-0.825000000000000))*(x229))));
00549 evalcond[1]=((IkReal(-0.825000000000000))+(x236)+(x237)+(((IkReal(-0.150000000000000))*(x230)))+(((IkReal(-1.00000000000000))*(x231)))+(((x230)*(x232))));
00550 evalcond[2]=((x233)+(((IkReal(0.150000000000000))*(x229)))+(((pz)*(x230)))+(((IkReal(-1.00000000000000))*(x234)*(x235)))+(((IkReal(-1.00000000000000))*(x232)*(x235))));
00551 evalcond[3]=((IkReal(0.150000000000000))+(((x230)*(x231)))+(((IkReal(0.825000000000000))*(x230)))+(((x229)*(x233)))+(((IkReal(-1.00000000000000))*(x232)))+(((IkReal(-1.00000000000000))*(x234))));
00552 evalcond[4]=((IkReal(-0.312500000000000))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(1.65000000000000))*(x236)))+(((IkReal(0.300000000000000))*(x232)))+(((IkReal(0.300000000000000))*(x234)))+(((IkReal(1.65000000000000))*(x237)))+(((IkReal(1.65000000000000))*(x230)*(x232)))+(((IkReal(-0.247500000000000))*(x230))));
00553 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  )
00554 {
00555 continue;
00556 }
00557 }
00558 
00559 rotationfunction0(solutions);
00560 }
00561 }
00562 
00563 }
00564 
00565 }
00566 }
00567 }
00568 }
00569 }
00570 }
00571 return solutions.GetNumSolutions()>0;
00572 }
00573 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
00574 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00575 IkReal x73=((cj0)*(r00));
00576 IkReal x74=((cj0)*(r01));
00577 IkReal x75=((IkReal(1.00000000000000))*(sj1));
00578 IkReal x76=((IkReal(1.00000000000000))*(sj0));
00579 IkReal x77=((cj1)*(sj2));
00580 IkReal x78=((cj0)*(r02));
00581 IkReal x79=((((IkReal(-1.00000000000000))*(sj2)*(x75)))+(((cj1)*(cj2))));
00582 IkReal x80=((x77)+(((cj2)*(sj1))));
00583 IkReal x81=((cj0)*(x79));
00584 IkReal x82=((sj0)*(x80));
00585 IkReal x83=((sj0)*(x79));
00586 IkReal x84=((((IkReal(-1.00000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(cj2)*(x75))));
00587 new_r00=((((r20)*(x84)))+(((r10)*(x83)))+(((x73)*(x79))));
00588 new_r01=((((x74)*(x79)))+(((r11)*(x83)))+(((r21)*(x84))));
00589 new_r02=((((r12)*(x83)))+(((x78)*(x79)))+(((r22)*(x84))));
00590 new_r10=((((IkReal(-1.00000000000000))*(r00)*(x76)))+(((cj0)*(r10))));
00591 new_r11=((((IkReal(-1.00000000000000))*(r01)*(x76)))+(((cj0)*(r11))));
00592 new_r12=((((IkReal(-1.00000000000000))*(r02)*(x76)))+(((cj0)*(r12))));
00593 new_r20=((((r20)*(x79)))+(((r10)*(x82)))+(((x73)*(x80))));
00594 new_r21=((((x74)*(x80)))+(((r11)*(x82)))+(((r21)*(x79))));
00595 new_r22=((((r22)*(x79)))+(((r12)*(x82)))+(((x78)*(x80))));
00596 {
00597 IkReal j4array[2], cj4array[2], sj4array[2];
00598 bool j4valid[2]={false};
00599 _nj4 = 2;
00600 cj4array[0]=new_r22;
00601 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
00602 {
00603     j4valid[0] = j4valid[1] = true;
00604     j4array[0] = IKacos(cj4array[0]);
00605     sj4array[0] = IKsin(j4array[0]);
00606     cj4array[1] = cj4array[0];
00607     j4array[1] = -j4array[0];
00608     sj4array[1] = -sj4array[0];
00609 }
00610 else if( isnan(cj4array[0]) )
00611 {
00612     // probably any value will work
00613     j4valid[0] = true;
00614     cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
00615 }
00616 for(int ij4 = 0; ij4 < 2; ++ij4)
00617 {
00618 if( !j4valid[ij4] )
00619 {
00620     continue;
00621 }
00622 _ij4[0] = ij4; _ij4[1] = -1;
00623 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
00624 {
00625 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00626 {
00627     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00628 }
00629 }
00630 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00631 
00632 {
00633 IkReal dummyeval[1];
00634 IkReal gconst3;
00635 gconst3=IKsign(sj4);
00636 dummyeval[0]=sj4;
00637 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00638 {
00639 {
00640 IkReal dummyeval[1];
00641 IkReal gconst1;
00642 gconst1=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00643 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00644 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00645 {
00646 {
00647 IkReal dummyeval[1];
00648 IkReal gconst2;
00649 gconst2=IKsign(((((cj4)*((new_r02)*(new_r02))))+(((cj4)*((new_r12)*(new_r12))))));
00650 dummyeval[0]=((((cj4)*((new_r02)*(new_r02))))+(((cj4)*((new_r12)*(new_r12)))));
00651 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00652 {
00653 {
00654 IkReal evalcond[7];
00655 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
00656 evalcond[1]=new_r22;
00657 evalcond[2]=((IkReal(-1.00000000000000))*(new_r22));
00658 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
00659 {
00660 {
00661 IkReal j5array[1], cj5array[1], sj5array[1];
00662 bool j5valid[1]={false};
00663 _nj5 = 1;
00664 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((IkReal(-1.00000000000000))*(new_r20)))-1) <= IKFAST_SINCOS_THRESH )
00665     continue;
00666 j5array[0]=IKatan2(new_r21, ((IkReal(-1.00000000000000))*(new_r20)));
00667 sj5array[0]=IKsin(j5array[0]);
00668 cj5array[0]=IKcos(j5array[0]);
00669 if( j5array[0] > IKPI )
00670 {
00671     j5array[0]-=IK2PI;
00672 }
00673 else if( j5array[0] < -IKPI )
00674 {    j5array[0]+=IK2PI;
00675 }
00676 j5valid[0] = true;
00677 for(int ij5 = 0; ij5 < 1; ++ij5)
00678 {
00679 if( !j5valid[ij5] )
00680 {
00681     continue;
00682 }
00683 _ij5[0] = ij5; _ij5[1] = -1;
00684 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00685 {
00686 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00687 {
00688     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00689 }
00690 }
00691 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00692 {
00693 IkReal evalcond[2];
00694 evalcond[0]=((IKcos(j5))+(new_r20));
00695 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(IKsin(j5)))));
00696 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00697 {
00698 continue;
00699 }
00700 }
00701 
00702 {
00703 IkReal dummyeval[1];
00704 IkReal gconst9;
00705 gconst9=IKsign(((((IkReal(-1.00000000000000))*(new_r02)*(new_r11)))+(((new_r01)*(new_r12)))));
00706 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r02)*(new_r11)))+(((new_r01)*(new_r12))));
00707 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00708 {
00709 {
00710 IkReal dummyeval[1];
00711 IkReal gconst10;
00712 gconst10=IKsign(((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10)))));
00713 dummyeval[0]=((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10))));
00714 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00715 {
00716 continue;
00717 
00718 } else
00719 {
00720 {
00721 IkReal j3array[1], cj3array[1], sj3array[1];
00722 bool j3valid[1]={false};
00723 _nj3 = 1;
00724 if( IKabs(((gconst10)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst10)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
00725     continue;
00726 j3array[0]=IKatan2(((gconst10)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst10)*(new_r10)));
00727 sj3array[0]=IKsin(j3array[0]);
00728 cj3array[0]=IKcos(j3array[0]);
00729 if( j3array[0] > IKPI )
00730 {
00731     j3array[0]-=IK2PI;
00732 }
00733 else if( j3array[0] < -IKPI )
00734 {    j3array[0]+=IK2PI;
00735 }
00736 j3valid[0] = true;
00737 for(int ij3 = 0; ij3 < 1; ++ij3)
00738 {
00739 if( !j3valid[ij3] )
00740 {
00741     continue;
00742 }
00743 _ij3[0] = ij3; _ij3[1] = -1;
00744 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
00745 {
00746 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00747 {
00748     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00749 }
00750 }
00751 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00752 {
00753 IkReal evalcond[6];
00754 IkReal x85=IKsin(j3);
00755 IkReal x86=IKcos(j3);
00756 IkReal x87=((IkReal(1.00000000000000))*(x85));
00757 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x87)))+(((new_r12)*(x86))));
00758 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r12)*(x85)))+(((new_r02)*(x86))));
00759 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x87)))+(((new_r10)*(x86)))+(((IkReal(-1.00000000000000))*(sj5))));
00760 evalcond[3]=((((IkReal(-1.00000000000000))*(cj5)))+(((new_r11)*(x86)))+(((IkReal(-1.00000000000000))*(new_r01)*(x87))));
00761 evalcond[4]=((((new_r11)*(x85)))+(((new_r01)*(x86))));
00762 evalcond[5]=((((new_r10)*(x85)))+(((new_r00)*(x86))));
00763 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  )
00764 {
00765 continue;
00766 }
00767 }
00768 
00769 {
00770 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00771 vinfos[0].jointtype = 1;
00772 vinfos[0].foffset = j0;
00773 vinfos[0].indices[0] = _ij0[0];
00774 vinfos[0].indices[1] = _ij0[1];
00775 vinfos[0].maxsolutions = _nj0;
00776 vinfos[1].jointtype = 1;
00777 vinfos[1].foffset = j1;
00778 vinfos[1].indices[0] = _ij1[0];
00779 vinfos[1].indices[1] = _ij1[1];
00780 vinfos[1].maxsolutions = _nj1;
00781 vinfos[2].jointtype = 1;
00782 vinfos[2].foffset = j2;
00783 vinfos[2].indices[0] = _ij2[0];
00784 vinfos[2].indices[1] = _ij2[1];
00785 vinfos[2].maxsolutions = _nj2;
00786 vinfos[3].jointtype = 1;
00787 vinfos[3].foffset = j3;
00788 vinfos[3].indices[0] = _ij3[0];
00789 vinfos[3].indices[1] = _ij3[1];
00790 vinfos[3].maxsolutions = _nj3;
00791 vinfos[4].jointtype = 1;
00792 vinfos[4].foffset = j4;
00793 vinfos[4].indices[0] = _ij4[0];
00794 vinfos[4].indices[1] = _ij4[1];
00795 vinfos[4].maxsolutions = _nj4;
00796 vinfos[5].jointtype = 1;
00797 vinfos[5].foffset = j5;
00798 vinfos[5].indices[0] = _ij5[0];
00799 vinfos[5].indices[1] = _ij5[1];
00800 vinfos[5].maxsolutions = _nj5;
00801 std::vector<int> vfree(0);
00802 solutions.AddSolution(vinfos,vfree);
00803 }
00804 }
00805 }
00806 
00807 }
00808 
00809 }
00810 
00811 } else
00812 {
00813 {
00814 IkReal j3array[1], cj3array[1], sj3array[1];
00815 bool j3valid[1]={false};
00816 _nj3 = 1;
00817 if( IKabs(((gconst9)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst9)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
00818     continue;
00819 j3array[0]=IKatan2(((gconst9)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst9)*(new_r11)));
00820 sj3array[0]=IKsin(j3array[0]);
00821 cj3array[0]=IKcos(j3array[0]);
00822 if( j3array[0] > IKPI )
00823 {
00824     j3array[0]-=IK2PI;
00825 }
00826 else if( j3array[0] < -IKPI )
00827 {    j3array[0]+=IK2PI;
00828 }
00829 j3valid[0] = true;
00830 for(int ij3 = 0; ij3 < 1; ++ij3)
00831 {
00832 if( !j3valid[ij3] )
00833 {
00834     continue;
00835 }
00836 _ij3[0] = ij3; _ij3[1] = -1;
00837 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
00838 {
00839 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00840 {
00841     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00842 }
00843 }
00844 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00845 {
00846 IkReal evalcond[6];
00847 IkReal x88=IKsin(j3);
00848 IkReal x89=IKcos(j3);
00849 IkReal x90=((IkReal(1.00000000000000))*(x88));
00850 evalcond[0]=((((new_r12)*(x89)))+(((IkReal(-1.00000000000000))*(new_r02)*(x90))));
00851 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r12)*(x88)))+(((new_r02)*(x89))));
00852 evalcond[2]=((((new_r10)*(x89)))+(((IkReal(-1.00000000000000))*(new_r00)*(x90)))+(((IkReal(-1.00000000000000))*(sj5))));
00853 evalcond[3]=((((IkReal(-1.00000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(new_r01)*(x90)))+(((new_r11)*(x89))));
00854 evalcond[4]=((((new_r11)*(x88)))+(((new_r01)*(x89))));
00855 evalcond[5]=((((new_r10)*(x88)))+(((new_r00)*(x89))));
00856 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  )
00857 {
00858 continue;
00859 }
00860 }
00861 
00862 {
00863 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00864 vinfos[0].jointtype = 1;
00865 vinfos[0].foffset = j0;
00866 vinfos[0].indices[0] = _ij0[0];
00867 vinfos[0].indices[1] = _ij0[1];
00868 vinfos[0].maxsolutions = _nj0;
00869 vinfos[1].jointtype = 1;
00870 vinfos[1].foffset = j1;
00871 vinfos[1].indices[0] = _ij1[0];
00872 vinfos[1].indices[1] = _ij1[1];
00873 vinfos[1].maxsolutions = _nj1;
00874 vinfos[2].jointtype = 1;
00875 vinfos[2].foffset = j2;
00876 vinfos[2].indices[0] = _ij2[0];
00877 vinfos[2].indices[1] = _ij2[1];
00878 vinfos[2].maxsolutions = _nj2;
00879 vinfos[3].jointtype = 1;
00880 vinfos[3].foffset = j3;
00881 vinfos[3].indices[0] = _ij3[0];
00882 vinfos[3].indices[1] = _ij3[1];
00883 vinfos[3].maxsolutions = _nj3;
00884 vinfos[4].jointtype = 1;
00885 vinfos[4].foffset = j4;
00886 vinfos[4].indices[0] = _ij4[0];
00887 vinfos[4].indices[1] = _ij4[1];
00888 vinfos[4].maxsolutions = _nj4;
00889 vinfos[5].jointtype = 1;
00890 vinfos[5].foffset = j5;
00891 vinfos[5].indices[0] = _ij5[0];
00892 vinfos[5].indices[1] = _ij5[1];
00893 vinfos[5].maxsolutions = _nj5;
00894 std::vector<int> vfree(0);
00895 solutions.AddSolution(vinfos,vfree);
00896 }
00897 }
00898 }
00899 
00900 }
00901 
00902 }
00903 }
00904 }
00905 
00906 } else
00907 {
00908 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
00909 evalcond[1]=new_r22;
00910 evalcond[2]=new_r22;
00911 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
00912 {
00913 {
00914 IkReal j5array[1], cj5array[1], sj5array[1];
00915 bool j5valid[1]={false};
00916 _nj5 = 1;
00917 if( IKabs(((IkReal(-1.00000000000000))*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
00918     continue;
00919 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)), new_r20);
00920 sj5array[0]=IKsin(j5array[0]);
00921 cj5array[0]=IKcos(j5array[0]);
00922 if( j5array[0] > IKPI )
00923 {
00924     j5array[0]-=IK2PI;
00925 }
00926 else if( j5array[0] < -IKPI )
00927 {    j5array[0]+=IK2PI;
00928 }
00929 j5valid[0] = true;
00930 for(int ij5 = 0; ij5 < 1; ++ij5)
00931 {
00932 if( !j5valid[ij5] )
00933 {
00934     continue;
00935 }
00936 _ij5[0] = ij5; _ij5[1] = -1;
00937 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00938 {
00939 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00940 {
00941     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00942 }
00943 }
00944 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00945 {
00946 IkReal evalcond[2];
00947 evalcond[0]=((((IkReal(-1.00000000000000))*(IKcos(j5))))+(new_r20));
00948 evalcond[1]=((new_r21)+(IKsin(j5)));
00949 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00950 {
00951 continue;
00952 }
00953 }
00954 
00955 {
00956 IkReal dummyeval[1];
00957 IkReal gconst13;
00958 gconst13=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11)))));
00959 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11))));
00960 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00961 {
00962 {
00963 IkReal dummyeval[1];
00964 IkReal gconst14;
00965 gconst14=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))));
00966 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))));
00967 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00968 {
00969 continue;
00970 
00971 } else
00972 {
00973 {
00974 IkReal j3array[1], cj3array[1], sj3array[1];
00975 bool j3valid[1]={false};
00976 _nj3 = 1;
00977 if( IKabs(((gconst14)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst14)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
00978     continue;
00979 j3array[0]=IKatan2(((gconst14)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst14)*(new_r10)));
00980 sj3array[0]=IKsin(j3array[0]);
00981 cj3array[0]=IKcos(j3array[0]);
00982 if( j3array[0] > IKPI )
00983 {
00984     j3array[0]-=IK2PI;
00985 }
00986 else if( j3array[0] < -IKPI )
00987 {    j3array[0]+=IK2PI;
00988 }
00989 j3valid[0] = true;
00990 for(int ij3 = 0; ij3 < 1; ++ij3)
00991 {
00992 if( !j3valid[ij3] )
00993 {
00994     continue;
00995 }
00996 _ij3[0] = ij3; _ij3[1] = -1;
00997 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
00998 {
00999 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01000 {
01001     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01002 }
01003 }
01004 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01005 {
01006 IkReal evalcond[6];
01007 IkReal x91=IKsin(j3);
01008 IkReal x92=IKcos(j3);
01009 IkReal x93=((IkReal(1.00000000000000))*(x91));
01010 evalcond[0]=((((new_r12)*(x92)))+(((IkReal(-1.00000000000000))*(new_r02)*(x93))));
01011 evalcond[1]=((IkReal(1.00000000000000))+(((new_r12)*(x91)))+(((new_r02)*(x92))));
01012 evalcond[2]=((((new_r10)*(x92)))+(((IkReal(-1.00000000000000))*(new_r00)*(x93)))+(((IkReal(-1.00000000000000))*(sj5))));
01013 evalcond[3]=((((IkReal(-1.00000000000000))*(cj5)))+(((new_r11)*(x92)))+(((IkReal(-1.00000000000000))*(new_r01)*(x93))));
01014 evalcond[4]=((((new_r01)*(x92)))+(((new_r11)*(x91))));
01015 evalcond[5]=((((new_r10)*(x91)))+(((new_r00)*(x92))));
01016 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  )
01017 {
01018 continue;
01019 }
01020 }
01021 
01022 {
01023 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01024 vinfos[0].jointtype = 1;
01025 vinfos[0].foffset = j0;
01026 vinfos[0].indices[0] = _ij0[0];
01027 vinfos[0].indices[1] = _ij0[1];
01028 vinfos[0].maxsolutions = _nj0;
01029 vinfos[1].jointtype = 1;
01030 vinfos[1].foffset = j1;
01031 vinfos[1].indices[0] = _ij1[0];
01032 vinfos[1].indices[1] = _ij1[1];
01033 vinfos[1].maxsolutions = _nj1;
01034 vinfos[2].jointtype = 1;
01035 vinfos[2].foffset = j2;
01036 vinfos[2].indices[0] = _ij2[0];
01037 vinfos[2].indices[1] = _ij2[1];
01038 vinfos[2].maxsolutions = _nj2;
01039 vinfos[3].jointtype = 1;
01040 vinfos[3].foffset = j3;
01041 vinfos[3].indices[0] = _ij3[0];
01042 vinfos[3].indices[1] = _ij3[1];
01043 vinfos[3].maxsolutions = _nj3;
01044 vinfos[4].jointtype = 1;
01045 vinfos[4].foffset = j4;
01046 vinfos[4].indices[0] = _ij4[0];
01047 vinfos[4].indices[1] = _ij4[1];
01048 vinfos[4].maxsolutions = _nj4;
01049 vinfos[5].jointtype = 1;
01050 vinfos[5].foffset = j5;
01051 vinfos[5].indices[0] = _ij5[0];
01052 vinfos[5].indices[1] = _ij5[1];
01053 vinfos[5].maxsolutions = _nj5;
01054 std::vector<int> vfree(0);
01055 solutions.AddSolution(vinfos,vfree);
01056 }
01057 }
01058 }
01059 
01060 }
01061 
01062 }
01063 
01064 } else
01065 {
01066 {
01067 IkReal j3array[1], cj3array[1], sj3array[1];
01068 bool j3valid[1]={false};
01069 _nj3 = 1;
01070 if( IKabs(((gconst13)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst13)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
01071     continue;
01072 j3array[0]=IKatan2(((gconst13)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst13)*(new_r11)));
01073 sj3array[0]=IKsin(j3array[0]);
01074 cj3array[0]=IKcos(j3array[0]);
01075 if( j3array[0] > IKPI )
01076 {
01077     j3array[0]-=IK2PI;
01078 }
01079 else if( j3array[0] < -IKPI )
01080 {    j3array[0]+=IK2PI;
01081 }
01082 j3valid[0] = true;
01083 for(int ij3 = 0; ij3 < 1; ++ij3)
01084 {
01085 if( !j3valid[ij3] )
01086 {
01087     continue;
01088 }
01089 _ij3[0] = ij3; _ij3[1] = -1;
01090 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01091 {
01092 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01093 {
01094     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01095 }
01096 }
01097 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01098 {
01099 IkReal evalcond[6];
01100 IkReal x94=IKsin(j3);
01101 IkReal x95=IKcos(j3);
01102 IkReal x96=((IkReal(1.00000000000000))*(x94));
01103 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x96)))+(((new_r12)*(x95))));
01104 evalcond[1]=((IkReal(1.00000000000000))+(((new_r02)*(x95)))+(((new_r12)*(x94))));
01105 evalcond[2]=((((new_r10)*(x95)))+(((IkReal(-1.00000000000000))*(new_r00)*(x96)))+(((IkReal(-1.00000000000000))*(sj5))));
01106 evalcond[3]=((((IkReal(-1.00000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(new_r01)*(x96)))+(((new_r11)*(x95))));
01107 evalcond[4]=((((new_r11)*(x94)))+(((new_r01)*(x95))));
01108 evalcond[5]=((((new_r10)*(x94)))+(((new_r00)*(x95))));
01109 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  )
01110 {
01111 continue;
01112 }
01113 }
01114 
01115 {
01116 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01117 vinfos[0].jointtype = 1;
01118 vinfos[0].foffset = j0;
01119 vinfos[0].indices[0] = _ij0[0];
01120 vinfos[0].indices[1] = _ij0[1];
01121 vinfos[0].maxsolutions = _nj0;
01122 vinfos[1].jointtype = 1;
01123 vinfos[1].foffset = j1;
01124 vinfos[1].indices[0] = _ij1[0];
01125 vinfos[1].indices[1] = _ij1[1];
01126 vinfos[1].maxsolutions = _nj1;
01127 vinfos[2].jointtype = 1;
01128 vinfos[2].foffset = j2;
01129 vinfos[2].indices[0] = _ij2[0];
01130 vinfos[2].indices[1] = _ij2[1];
01131 vinfos[2].maxsolutions = _nj2;
01132 vinfos[3].jointtype = 1;
01133 vinfos[3].foffset = j3;
01134 vinfos[3].indices[0] = _ij3[0];
01135 vinfos[3].indices[1] = _ij3[1];
01136 vinfos[3].maxsolutions = _nj3;
01137 vinfos[4].jointtype = 1;
01138 vinfos[4].foffset = j4;
01139 vinfos[4].indices[0] = _ij4[0];
01140 vinfos[4].indices[1] = _ij4[1];
01141 vinfos[4].maxsolutions = _nj4;
01142 vinfos[5].jointtype = 1;
01143 vinfos[5].foffset = j5;
01144 vinfos[5].indices[0] = _ij5[0];
01145 vinfos[5].indices[1] = _ij5[1];
01146 vinfos[5].maxsolutions = _nj5;
01147 std::vector<int> vfree(0);
01148 solutions.AddSolution(vinfos,vfree);
01149 }
01150 }
01151 }
01152 
01153 }
01154 
01155 }
01156 }
01157 }
01158 
01159 } else
01160 {
01161 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01162 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
01163 evalcond[2]=new_r20;
01164 evalcond[3]=new_r21;
01165 evalcond[4]=((IkReal(-1.00000000000000))*(new_r20));
01166 evalcond[5]=((IkReal(-1.00000000000000))*(new_r21));
01167 evalcond[6]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01168 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  )
01169 {
01170 {
01171 IkReal j3array[2], cj3array[2], sj3array[2];
01172 bool j3valid[2]={false};
01173 _nj3 = 2;
01174 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
01175     continue;
01176 IkReal x97=IKatan2(new_r02, new_r12);
01177 j3array[0]=((IkReal(-1.00000000000000))*(x97));
01178 sj3array[0]=IKsin(j3array[0]);
01179 cj3array[0]=IKcos(j3array[0]);
01180 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x97))));
01181 sj3array[1]=IKsin(j3array[1]);
01182 cj3array[1]=IKcos(j3array[1]);
01183 if( j3array[0] > IKPI )
01184 {
01185     j3array[0]-=IK2PI;
01186 }
01187 else if( j3array[0] < -IKPI )
01188 {    j3array[0]+=IK2PI;
01189 }
01190 j3valid[0] = true;
01191 if( j3array[1] > IKPI )
01192 {
01193     j3array[1]-=IK2PI;
01194 }
01195 else if( j3array[1] < -IKPI )
01196 {    j3array[1]+=IK2PI;
01197 }
01198 j3valid[1] = true;
01199 for(int ij3 = 0; ij3 < 2; ++ij3)
01200 {
01201 if( !j3valid[ij3] )
01202 {
01203     continue;
01204 }
01205 _ij3[0] = ij3; _ij3[1] = -1;
01206 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
01207 {
01208 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01209 {
01210     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01211 }
01212 }
01213 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01214 {
01215 IkReal evalcond[1];
01216 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
01217 if( IKabs(evalcond[0]) > 0.000001  )
01218 {
01219 continue;
01220 }
01221 }
01222 
01223 {
01224 IkReal j5array[1], cj5array[1], sj5array[1];
01225 bool j5valid[1]={false};
01226 _nj5 = 1;
01227 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01228     continue;
01229 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01230 sj5array[0]=IKsin(j5array[0]);
01231 cj5array[0]=IKcos(j5array[0]);
01232 if( j5array[0] > IKPI )
01233 {
01234     j5array[0]-=IK2PI;
01235 }
01236 else if( j5array[0] < -IKPI )
01237 {    j5array[0]+=IK2PI;
01238 }
01239 j5valid[0] = true;
01240 for(int ij5 = 0; ij5 < 1; ++ij5)
01241 {
01242 if( !j5valid[ij5] )
01243 {
01244     continue;
01245 }
01246 _ij5[0] = ij5; _ij5[1] = -1;
01247 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01248 {
01249 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01250 {
01251     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01252 }
01253 }
01254 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01255 {
01256 IkReal evalcond[4];
01257 IkReal x98=IKsin(j5);
01258 IkReal x99=((IkReal(1.00000000000000))*(sj3));
01259 IkReal x100=((IkReal(1.00000000000000))*(IKcos(j5)));
01260 evalcond[0]=((((IkReal(-1.00000000000000))*(x98)))+(((IkReal(-1.00000000000000))*(new_r00)*(x99)))+(((cj3)*(new_r10))));
01261 evalcond[1]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x99)))+(((IkReal(-1.00000000000000))*(x100))));
01262 evalcond[2]=((((new_r11)*(sj3)))+(((cj3)*(new_r01)))+(x98));
01263 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x100)))+(((cj3)*(new_r00))));
01264 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01265 {
01266 continue;
01267 }
01268 }
01269 
01270 {
01271 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01272 vinfos[0].jointtype = 1;
01273 vinfos[0].foffset = j0;
01274 vinfos[0].indices[0] = _ij0[0];
01275 vinfos[0].indices[1] = _ij0[1];
01276 vinfos[0].maxsolutions = _nj0;
01277 vinfos[1].jointtype = 1;
01278 vinfos[1].foffset = j1;
01279 vinfos[1].indices[0] = _ij1[0];
01280 vinfos[1].indices[1] = _ij1[1];
01281 vinfos[1].maxsolutions = _nj1;
01282 vinfos[2].jointtype = 1;
01283 vinfos[2].foffset = j2;
01284 vinfos[2].indices[0] = _ij2[0];
01285 vinfos[2].indices[1] = _ij2[1];
01286 vinfos[2].maxsolutions = _nj2;
01287 vinfos[3].jointtype = 1;
01288 vinfos[3].foffset = j3;
01289 vinfos[3].indices[0] = _ij3[0];
01290 vinfos[3].indices[1] = _ij3[1];
01291 vinfos[3].maxsolutions = _nj3;
01292 vinfos[4].jointtype = 1;
01293 vinfos[4].foffset = j4;
01294 vinfos[4].indices[0] = _ij4[0];
01295 vinfos[4].indices[1] = _ij4[1];
01296 vinfos[4].maxsolutions = _nj4;
01297 vinfos[5].jointtype = 1;
01298 vinfos[5].foffset = j5;
01299 vinfos[5].indices[0] = _ij5[0];
01300 vinfos[5].indices[1] = _ij5[1];
01301 vinfos[5].maxsolutions = _nj5;
01302 std::vector<int> vfree(0);
01303 solutions.AddSolution(vinfos,vfree);
01304 }
01305 }
01306 }
01307 }
01308 }
01309 
01310 } else
01311 {
01312 IkReal x101=((IkReal(1.00000000000000))+(new_r22));
01313 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
01314 evalcond[1]=x101;
01315 evalcond[2]=new_r20;
01316 evalcond[3]=new_r21;
01317 evalcond[4]=new_r20;
01318 evalcond[5]=new_r21;
01319 evalcond[6]=x101;
01320 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  )
01321 {
01322 {
01323 IkReal j3array[2], cj3array[2], sj3array[2];
01324 bool j3valid[2]={false};
01325 _nj3 = 2;
01326 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
01327     continue;
01328 IkReal x102=IKatan2(new_r02, new_r12);
01329 j3array[0]=((IkReal(-1.00000000000000))*(x102));
01330 sj3array[0]=IKsin(j3array[0]);
01331 cj3array[0]=IKcos(j3array[0]);
01332 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x102))));
01333 sj3array[1]=IKsin(j3array[1]);
01334 cj3array[1]=IKcos(j3array[1]);
01335 if( j3array[0] > IKPI )
01336 {
01337     j3array[0]-=IK2PI;
01338 }
01339 else if( j3array[0] < -IKPI )
01340 {    j3array[0]+=IK2PI;
01341 }
01342 j3valid[0] = true;
01343 if( j3array[1] > IKPI )
01344 {
01345     j3array[1]-=IK2PI;
01346 }
01347 else if( j3array[1] < -IKPI )
01348 {    j3array[1]+=IK2PI;
01349 }
01350 j3valid[1] = true;
01351 for(int ij3 = 0; ij3 < 2; ++ij3)
01352 {
01353 if( !j3valid[ij3] )
01354 {
01355     continue;
01356 }
01357 _ij3[0] = ij3; _ij3[1] = -1;
01358 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
01359 {
01360 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01361 {
01362     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01363 }
01364 }
01365 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01366 {
01367 IkReal evalcond[1];
01368 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
01369 if( IKabs(evalcond[0]) > 0.000001  )
01370 {
01371 continue;
01372 }
01373 }
01374 
01375 {
01376 IkReal j5array[1], cj5array[1], sj5array[1];
01377 bool j5valid[1]={false};
01378 _nj5 = 1;
01379 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 )
01380     continue;
01381 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01382 sj5array[0]=IKsin(j5array[0]);
01383 cj5array[0]=IKcos(j5array[0]);
01384 if( j5array[0] > IKPI )
01385 {
01386     j5array[0]-=IK2PI;
01387 }
01388 else if( j5array[0] < -IKPI )
01389 {    j5array[0]+=IK2PI;
01390 }
01391 j5valid[0] = true;
01392 for(int ij5 = 0; ij5 < 1; ++ij5)
01393 {
01394 if( !j5valid[ij5] )
01395 {
01396     continue;
01397 }
01398 _ij5[0] = ij5; _ij5[1] = -1;
01399 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01400 {
01401 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01402 {
01403     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01404 }
01405 }
01406 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01407 {
01408 IkReal evalcond[4];
01409 IkReal x103=IKcos(j5);
01410 IkReal x104=((IkReal(1.00000000000000))*(sj3));
01411 IkReal x105=((IkReal(1.00000000000000))*(IKsin(j5)));
01412 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x104)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x105))));
01413 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x104)))+(((IkReal(-1.00000000000000))*(x103)))+(((cj3)*(new_r11))));
01414 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x105)))+(((cj3)*(new_r01))));
01415 evalcond[3]=((((new_r10)*(sj3)))+(x103)+(((cj3)*(new_r00))));
01416 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01417 {
01418 continue;
01419 }
01420 }
01421 
01422 {
01423 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01424 vinfos[0].jointtype = 1;
01425 vinfos[0].foffset = j0;
01426 vinfos[0].indices[0] = _ij0[0];
01427 vinfos[0].indices[1] = _ij0[1];
01428 vinfos[0].maxsolutions = _nj0;
01429 vinfos[1].jointtype = 1;
01430 vinfos[1].foffset = j1;
01431 vinfos[1].indices[0] = _ij1[0];
01432 vinfos[1].indices[1] = _ij1[1];
01433 vinfos[1].maxsolutions = _nj1;
01434 vinfos[2].jointtype = 1;
01435 vinfos[2].foffset = j2;
01436 vinfos[2].indices[0] = _ij2[0];
01437 vinfos[2].indices[1] = _ij2[1];
01438 vinfos[2].maxsolutions = _nj2;
01439 vinfos[3].jointtype = 1;
01440 vinfos[3].foffset = j3;
01441 vinfos[3].indices[0] = _ij3[0];
01442 vinfos[3].indices[1] = _ij3[1];
01443 vinfos[3].maxsolutions = _nj3;
01444 vinfos[4].jointtype = 1;
01445 vinfos[4].foffset = j4;
01446 vinfos[4].indices[0] = _ij4[0];
01447 vinfos[4].indices[1] = _ij4[1];
01448 vinfos[4].maxsolutions = _nj4;
01449 vinfos[5].jointtype = 1;
01450 vinfos[5].foffset = j5;
01451 vinfos[5].indices[0] = _ij5[0];
01452 vinfos[5].indices[1] = _ij5[1];
01453 vinfos[5].maxsolutions = _nj5;
01454 std::vector<int> vfree(0);
01455 solutions.AddSolution(vinfos,vfree);
01456 }
01457 }
01458 }
01459 }
01460 }
01461 
01462 } else
01463 {
01464 if( 1 )
01465 {
01466 continue;
01467 
01468 } else
01469 {
01470 }
01471 }
01472 }
01473 }
01474 }
01475 }
01476 
01477 } else
01478 {
01479 {
01480 IkReal j3array[1], cj3array[1], sj3array[1];
01481 bool j3valid[1]={false};
01482 _nj3 = 1;
01483 IkReal x106=((gconst2)*(new_r22)*(sj4));
01484 if( IKabs(((new_r12)*(x106))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x106))) < IKFAST_ATAN2_MAGTHRESH )
01485     continue;
01486 j3array[0]=IKatan2(((new_r12)*(x106)), ((new_r02)*(x106)));
01487 sj3array[0]=IKsin(j3array[0]);
01488 cj3array[0]=IKcos(j3array[0]);
01489 if( j3array[0] > IKPI )
01490 {
01491     j3array[0]-=IK2PI;
01492 }
01493 else if( j3array[0] < -IKPI )
01494 {    j3array[0]+=IK2PI;
01495 }
01496 j3valid[0] = true;
01497 for(int ij3 = 0; ij3 < 1; ++ij3)
01498 {
01499 if( !j3valid[ij3] )
01500 {
01501     continue;
01502 }
01503 _ij3[0] = ij3; _ij3[1] = -1;
01504 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01505 {
01506 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01507 {
01508     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01509 }
01510 }
01511 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01512 {
01513 IkReal evalcond[6];
01514 IkReal x107=IKsin(j3);
01515 IkReal x108=IKcos(j3);
01516 IkReal x109=((IkReal(1.00000000000000))*(sj4));
01517 IkReal x110=((IkReal(1.00000000000000))*(cj4));
01518 IkReal x111=((new_r02)*(x108));
01519 IkReal x112=((new_r12)*(x107));
01520 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x107)))+(((new_r12)*(x108))));
01521 evalcond[1]=((((IkReal(-1.00000000000000))*(x109)))+(x112)+(x111));
01522 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r22)*(x109)))+(((cj4)*(x111)))+(((cj4)*(x112))));
01523 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r00)*(x108)*(x109)))+(((IkReal(-1.00000000000000))*(new_r20)*(x110)))+(((IkReal(-1.00000000000000))*(new_r10)*(x107)*(x109))));
01524 evalcond[4]=((((IkReal(-1.00000000000000))*(new_r01)*(x108)*(x109)))+(((IkReal(-1.00000000000000))*(new_r21)*(x110)))+(((IkReal(-1.00000000000000))*(new_r11)*(x107)*(x109))));
01525 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x109)*(x111)))+(((IkReal(-1.00000000000000))*(x109)*(x112)))+(((IkReal(-1.00000000000000))*(new_r22)*(x110))));
01526 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  )
01527 {
01528 continue;
01529 }
01530 }
01531 
01532 {
01533 IkReal dummyeval[1];
01534 IkReal gconst4;
01535 gconst4=IKsign(sj4);
01536 dummyeval[0]=sj4;
01537 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01538 {
01539 {
01540 IkReal dummyeval[1];
01541 dummyeval[0]=sj4;
01542 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01543 {
01544 {
01545 IkReal dummyeval[1];
01546 dummyeval[0]=sj4;
01547 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01548 {
01549 {
01550 IkReal evalcond[11];
01551 IkReal x113=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3))));
01552 IkReal x114=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
01553 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01554 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
01555 evalcond[2]=new_r20;
01556 evalcond[3]=new_r21;
01557 evalcond[4]=x113;
01558 evalcond[5]=x113;
01559 evalcond[6]=x114;
01560 evalcond[7]=x114;
01561 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
01562 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
01563 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01564 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  )
01565 {
01566 {
01567 IkReal j5array[1], cj5array[1], sj5array[1];
01568 bool j5valid[1]={false};
01569 _nj5 = 1;
01570 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01571     continue;
01572 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01573 sj5array[0]=IKsin(j5array[0]);
01574 cj5array[0]=IKcos(j5array[0]);
01575 if( j5array[0] > IKPI )
01576 {
01577     j5array[0]-=IK2PI;
01578 }
01579 else if( j5array[0] < -IKPI )
01580 {    j5array[0]+=IK2PI;
01581 }
01582 j5valid[0] = true;
01583 for(int ij5 = 0; ij5 < 1; ++ij5)
01584 {
01585 if( !j5valid[ij5] )
01586 {
01587     continue;
01588 }
01589 _ij5[0] = ij5; _ij5[1] = -1;
01590 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01591 {
01592 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01593 {
01594     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01595 }
01596 }
01597 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01598 {
01599 IkReal evalcond[4];
01600 IkReal x115=IKsin(j5);
01601 IkReal x116=((IkReal(1.00000000000000))*(sj3));
01602 IkReal x117=((IkReal(1.00000000000000))*(IKcos(j5)));
01603 evalcond[0]=((((IkReal(-1.00000000000000))*(x115)))+(((IkReal(-1.00000000000000))*(new_r00)*(x116)))+(((cj3)*(new_r10))));
01604 evalcond[1]=((((IkReal(-1.00000000000000))*(x117)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x116))));
01605 evalcond[2]=((((new_r11)*(sj3)))+(x115)+(((cj3)*(new_r01))));
01606 evalcond[3]=((((IkReal(-1.00000000000000))*(x117)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
01607 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01608 {
01609 continue;
01610 }
01611 }
01612 
01613 {
01614 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01615 vinfos[0].jointtype = 1;
01616 vinfos[0].foffset = j0;
01617 vinfos[0].indices[0] = _ij0[0];
01618 vinfos[0].indices[1] = _ij0[1];
01619 vinfos[0].maxsolutions = _nj0;
01620 vinfos[1].jointtype = 1;
01621 vinfos[1].foffset = j1;
01622 vinfos[1].indices[0] = _ij1[0];
01623 vinfos[1].indices[1] = _ij1[1];
01624 vinfos[1].maxsolutions = _nj1;
01625 vinfos[2].jointtype = 1;
01626 vinfos[2].foffset = j2;
01627 vinfos[2].indices[0] = _ij2[0];
01628 vinfos[2].indices[1] = _ij2[1];
01629 vinfos[2].maxsolutions = _nj2;
01630 vinfos[3].jointtype = 1;
01631 vinfos[3].foffset = j3;
01632 vinfos[3].indices[0] = _ij3[0];
01633 vinfos[3].indices[1] = _ij3[1];
01634 vinfos[3].maxsolutions = _nj3;
01635 vinfos[4].jointtype = 1;
01636 vinfos[4].foffset = j4;
01637 vinfos[4].indices[0] = _ij4[0];
01638 vinfos[4].indices[1] = _ij4[1];
01639 vinfos[4].maxsolutions = _nj4;
01640 vinfos[5].jointtype = 1;
01641 vinfos[5].foffset = j5;
01642 vinfos[5].indices[0] = _ij5[0];
01643 vinfos[5].indices[1] = _ij5[1];
01644 vinfos[5].maxsolutions = _nj5;
01645 std::vector<int> vfree(0);
01646 solutions.AddSolution(vinfos,vfree);
01647 }
01648 }
01649 }
01650 
01651 } else
01652 {
01653 IkReal x118=((IkReal(1.00000000000000))+(new_r22));
01654 IkReal x119=((new_r12)*(sj3));
01655 IkReal x120=((IkReal(1.00000000000000))*(new_r02));
01656 IkReal x121=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(sj3)*(x120))));
01657 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
01658 evalcond[1]=x118;
01659 evalcond[2]=new_r20;
01660 evalcond[3]=new_r21;
01661 evalcond[4]=x121;
01662 evalcond[5]=x121;
01663 evalcond[6]=((x119)+(((cj3)*(new_r02))));
01664 evalcond[7]=((((IkReal(-1.00000000000000))*(cj3)*(x120)))+(((IkReal(-1.00000000000000))*(x119))));
01665 evalcond[8]=new_r20;
01666 evalcond[9]=new_r21;
01667 evalcond[10]=x118;
01668 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  )
01669 {
01670 {
01671 IkReal j5array[1], cj5array[1], sj5array[1];
01672 bool j5valid[1]={false};
01673 _nj5 = 1;
01674 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 )
01675     continue;
01676 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01677 sj5array[0]=IKsin(j5array[0]);
01678 cj5array[0]=IKcos(j5array[0]);
01679 if( j5array[0] > IKPI )
01680 {
01681     j5array[0]-=IK2PI;
01682 }
01683 else if( j5array[0] < -IKPI )
01684 {    j5array[0]+=IK2PI;
01685 }
01686 j5valid[0] = true;
01687 for(int ij5 = 0; ij5 < 1; ++ij5)
01688 {
01689 if( !j5valid[ij5] )
01690 {
01691     continue;
01692 }
01693 _ij5[0] = ij5; _ij5[1] = -1;
01694 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01695 {
01696 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01697 {
01698     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01699 }
01700 }
01701 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01702 {
01703 IkReal evalcond[4];
01704 IkReal x122=IKcos(j5);
01705 IkReal x123=((IkReal(1.00000000000000))*(sj3));
01706 IkReal x124=((IkReal(1.00000000000000))*(IKsin(j5)));
01707 evalcond[0]=((((IkReal(-1.00000000000000))*(x124)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x123))));
01708 evalcond[1]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x122)))+(((IkReal(-1.00000000000000))*(new_r01)*(x123))));
01709 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x124)))+(((cj3)*(new_r01))));
01710 evalcond[3]=((x122)+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
01711 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01712 {
01713 continue;
01714 }
01715 }
01716 
01717 {
01718 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01719 vinfos[0].jointtype = 1;
01720 vinfos[0].foffset = j0;
01721 vinfos[0].indices[0] = _ij0[0];
01722 vinfos[0].indices[1] = _ij0[1];
01723 vinfos[0].maxsolutions = _nj0;
01724 vinfos[1].jointtype = 1;
01725 vinfos[1].foffset = j1;
01726 vinfos[1].indices[0] = _ij1[0];
01727 vinfos[1].indices[1] = _ij1[1];
01728 vinfos[1].maxsolutions = _nj1;
01729 vinfos[2].jointtype = 1;
01730 vinfos[2].foffset = j2;
01731 vinfos[2].indices[0] = _ij2[0];
01732 vinfos[2].indices[1] = _ij2[1];
01733 vinfos[2].maxsolutions = _nj2;
01734 vinfos[3].jointtype = 1;
01735 vinfos[3].foffset = j3;
01736 vinfos[3].indices[0] = _ij3[0];
01737 vinfos[3].indices[1] = _ij3[1];
01738 vinfos[3].maxsolutions = _nj3;
01739 vinfos[4].jointtype = 1;
01740 vinfos[4].foffset = j4;
01741 vinfos[4].indices[0] = _ij4[0];
01742 vinfos[4].indices[1] = _ij4[1];
01743 vinfos[4].maxsolutions = _nj4;
01744 vinfos[5].jointtype = 1;
01745 vinfos[5].foffset = j5;
01746 vinfos[5].indices[0] = _ij5[0];
01747 vinfos[5].indices[1] = _ij5[1];
01748 vinfos[5].maxsolutions = _nj5;
01749 std::vector<int> vfree(0);
01750 solutions.AddSolution(vinfos,vfree);
01751 }
01752 }
01753 }
01754 
01755 } else
01756 {
01757 if( 1 )
01758 {
01759 continue;
01760 
01761 } else
01762 {
01763 }
01764 }
01765 }
01766 }
01767 
01768 } else
01769 {
01770 {
01771 IkReal j5array[1], cj5array[1], sj5array[1];
01772 bool j5valid[1]={false};
01773 _nj5 = 1;
01774 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
01775     continue;
01776 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
01777 sj5array[0]=IKsin(j5array[0]);
01778 cj5array[0]=IKcos(j5array[0]);
01779 if( j5array[0] > IKPI )
01780 {
01781     j5array[0]-=IK2PI;
01782 }
01783 else if( j5array[0] < -IKPI )
01784 {    j5array[0]+=IK2PI;
01785 }
01786 j5valid[0] = true;
01787 for(int ij5 = 0; ij5 < 1; ++ij5)
01788 {
01789 if( !j5valid[ij5] )
01790 {
01791     continue;
01792 }
01793 _ij5[0] = ij5; _ij5[1] = -1;
01794 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01795 {
01796 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01797 {
01798     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01799 }
01800 }
01801 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01802 {
01803 IkReal evalcond[8];
01804 IkReal x125=IKsin(j5);
01805 IkReal x126=IKcos(j5);
01806 IkReal x127=((IkReal(1.00000000000000))*(sj3));
01807 IkReal x128=((new_r11)*(sj3));
01808 IkReal x129=((new_r10)*(sj3));
01809 IkReal x130=((cj3)*(cj4));
01810 IkReal x131=((IkReal(1.00000000000000))*(sj4));
01811 IkReal x132=((IkReal(1.00000000000000))*(x126));
01812 IkReal x133=((IkReal(1.00000000000000))*(x125));
01813 evalcond[0]=((new_r20)+(((sj4)*(x126))));
01814 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x125)*(x131))));
01815 evalcond[2]=((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x127)))+(((IkReal(-1.00000000000000))*(x133))));
01816 evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x132)))+(((IkReal(-1.00000000000000))*(new_r01)*(x127))));
01817 evalcond[4]=((((cj4)*(x125)))+(x128)+(((cj3)*(new_r01))));
01818 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x132)))+(x129)+(((cj3)*(new_r00))));
01819 evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x131)))+(((new_r01)*(x130)))+(x125)+(((cj4)*(x128))));
01820 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x131)))+(((cj4)*(x129)))+(((new_r00)*(x130)))+(((IkReal(-1.00000000000000))*(x132))));
01821 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  )
01822 {
01823 continue;
01824 }
01825 }
01826 
01827 {
01828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01829 vinfos[0].jointtype = 1;
01830 vinfos[0].foffset = j0;
01831 vinfos[0].indices[0] = _ij0[0];
01832 vinfos[0].indices[1] = _ij0[1];
01833 vinfos[0].maxsolutions = _nj0;
01834 vinfos[1].jointtype = 1;
01835 vinfos[1].foffset = j1;
01836 vinfos[1].indices[0] = _ij1[0];
01837 vinfos[1].indices[1] = _ij1[1];
01838 vinfos[1].maxsolutions = _nj1;
01839 vinfos[2].jointtype = 1;
01840 vinfos[2].foffset = j2;
01841 vinfos[2].indices[0] = _ij2[0];
01842 vinfos[2].indices[1] = _ij2[1];
01843 vinfos[2].maxsolutions = _nj2;
01844 vinfos[3].jointtype = 1;
01845 vinfos[3].foffset = j3;
01846 vinfos[3].indices[0] = _ij3[0];
01847 vinfos[3].indices[1] = _ij3[1];
01848 vinfos[3].maxsolutions = _nj3;
01849 vinfos[4].jointtype = 1;
01850 vinfos[4].foffset = j4;
01851 vinfos[4].indices[0] = _ij4[0];
01852 vinfos[4].indices[1] = _ij4[1];
01853 vinfos[4].maxsolutions = _nj4;
01854 vinfos[5].jointtype = 1;
01855 vinfos[5].foffset = j5;
01856 vinfos[5].indices[0] = _ij5[0];
01857 vinfos[5].indices[1] = _ij5[1];
01858 vinfos[5].maxsolutions = _nj5;
01859 std::vector<int> vfree(0);
01860 solutions.AddSolution(vinfos,vfree);
01861 }
01862 }
01863 }
01864 
01865 }
01866 
01867 }
01868 
01869 } else
01870 {
01871 {
01872 IkReal j5array[1], cj5array[1], sj5array[1];
01873 bool j5valid[1]={false};
01874 _nj5 = 1;
01875 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 )
01876     continue;
01877 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
01878 sj5array[0]=IKsin(j5array[0]);
01879 cj5array[0]=IKcos(j5array[0]);
01880 if( j5array[0] > IKPI )
01881 {
01882     j5array[0]-=IK2PI;
01883 }
01884 else if( j5array[0] < -IKPI )
01885 {    j5array[0]+=IK2PI;
01886 }
01887 j5valid[0] = true;
01888 for(int ij5 = 0; ij5 < 1; ++ij5)
01889 {
01890 if( !j5valid[ij5] )
01891 {
01892     continue;
01893 }
01894 _ij5[0] = ij5; _ij5[1] = -1;
01895 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01896 {
01897 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01898 {
01899     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01900 }
01901 }
01902 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01903 {
01904 IkReal evalcond[8];
01905 IkReal x134=IKsin(j5);
01906 IkReal x135=IKcos(j5);
01907 IkReal x136=((IkReal(1.00000000000000))*(sj3));
01908 IkReal x137=((new_r11)*(sj3));
01909 IkReal x138=((new_r10)*(sj3));
01910 IkReal x139=((cj3)*(cj4));
01911 IkReal x140=((IkReal(1.00000000000000))*(sj4));
01912 IkReal x141=((IkReal(1.00000000000000))*(x135));
01913 IkReal x142=((IkReal(1.00000000000000))*(x134));
01914 evalcond[0]=((new_r20)+(((sj4)*(x135))));
01915 evalcond[1]=((((IkReal(-1.00000000000000))*(x134)*(x140)))+(new_r21));
01916 evalcond[2]=((((IkReal(-1.00000000000000))*(x142)))+(((IkReal(-1.00000000000000))*(new_r00)*(x136)))+(((cj3)*(new_r10))));
01917 evalcond[3]=((((IkReal(-1.00000000000000))*(x141)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x136))));
01918 evalcond[4]=((x137)+(((cj4)*(x134)))+(((cj3)*(new_r01))));
01919 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x141)))+(x138)+(((cj3)*(new_r00))));
01920 evalcond[6]=((((cj4)*(x137)))+(((new_r01)*(x139)))+(x134)+(((IkReal(-1.00000000000000))*(new_r21)*(x140))));
01921 evalcond[7]=((((IkReal(-1.00000000000000))*(x141)))+(((new_r00)*(x139)))+(((IkReal(-1.00000000000000))*(new_r20)*(x140)))+(((cj4)*(x138))));
01922 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  )
01923 {
01924 continue;
01925 }
01926 }
01927 
01928 {
01929 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01930 vinfos[0].jointtype = 1;
01931 vinfos[0].foffset = j0;
01932 vinfos[0].indices[0] = _ij0[0];
01933 vinfos[0].indices[1] = _ij0[1];
01934 vinfos[0].maxsolutions = _nj0;
01935 vinfos[1].jointtype = 1;
01936 vinfos[1].foffset = j1;
01937 vinfos[1].indices[0] = _ij1[0];
01938 vinfos[1].indices[1] = _ij1[1];
01939 vinfos[1].maxsolutions = _nj1;
01940 vinfos[2].jointtype = 1;
01941 vinfos[2].foffset = j2;
01942 vinfos[2].indices[0] = _ij2[0];
01943 vinfos[2].indices[1] = _ij2[1];
01944 vinfos[2].maxsolutions = _nj2;
01945 vinfos[3].jointtype = 1;
01946 vinfos[3].foffset = j3;
01947 vinfos[3].indices[0] = _ij3[0];
01948 vinfos[3].indices[1] = _ij3[1];
01949 vinfos[3].maxsolutions = _nj3;
01950 vinfos[4].jointtype = 1;
01951 vinfos[4].foffset = j4;
01952 vinfos[4].indices[0] = _ij4[0];
01953 vinfos[4].indices[1] = _ij4[1];
01954 vinfos[4].maxsolutions = _nj4;
01955 vinfos[5].jointtype = 1;
01956 vinfos[5].foffset = j5;
01957 vinfos[5].indices[0] = _ij5[0];
01958 vinfos[5].indices[1] = _ij5[1];
01959 vinfos[5].maxsolutions = _nj5;
01960 std::vector<int> vfree(0);
01961 solutions.AddSolution(vinfos,vfree);
01962 }
01963 }
01964 }
01965 
01966 }
01967 
01968 }
01969 
01970 } else
01971 {
01972 {
01973 IkReal j5array[1], cj5array[1], sj5array[1];
01974 bool j5valid[1]={false};
01975 _nj5 = 1;
01976 if( IKabs(((gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
01977     continue;
01978 j5array[0]=IKatan2(((gconst4)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst4)*(new_r20)));
01979 sj5array[0]=IKsin(j5array[0]);
01980 cj5array[0]=IKcos(j5array[0]);
01981 if( j5array[0] > IKPI )
01982 {
01983     j5array[0]-=IK2PI;
01984 }
01985 else if( j5array[0] < -IKPI )
01986 {    j5array[0]+=IK2PI;
01987 }
01988 j5valid[0] = true;
01989 for(int ij5 = 0; ij5 < 1; ++ij5)
01990 {
01991 if( !j5valid[ij5] )
01992 {
01993     continue;
01994 }
01995 _ij5[0] = ij5; _ij5[1] = -1;
01996 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01997 {
01998 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01999 {
02000     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02001 }
02002 }
02003 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02004 {
02005 IkReal evalcond[8];
02006 IkReal x143=IKsin(j5);
02007 IkReal x144=IKcos(j5);
02008 IkReal x145=((IkReal(1.00000000000000))*(sj3));
02009 IkReal x146=((new_r11)*(sj3));
02010 IkReal x147=((new_r10)*(sj3));
02011 IkReal x148=((cj3)*(cj4));
02012 IkReal x149=((IkReal(1.00000000000000))*(sj4));
02013 IkReal x150=((IkReal(1.00000000000000))*(x144));
02014 IkReal x151=((IkReal(1.00000000000000))*(x143));
02015 evalcond[0]=((new_r20)+(((sj4)*(x144))));
02016 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x143)*(x149))));
02017 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x145)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x151))));
02018 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x145)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x150))));
02019 evalcond[4]=((((cj4)*(x143)))+(x146)+(((cj3)*(new_r01))));
02020 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x150)))+(x147)+(((cj3)*(new_r00))));
02021 evalcond[6]=((((cj4)*(x146)))+(((IkReal(-1.00000000000000))*(new_r21)*(x149)))+(((new_r01)*(x148)))+(x143));
02022 evalcond[7]=((((cj4)*(x147)))+(((IkReal(-1.00000000000000))*(new_r20)*(x149)))+(((new_r00)*(x148)))+(((IkReal(-1.00000000000000))*(x150))));
02023 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  )
02024 {
02025 continue;
02026 }
02027 }
02028 
02029 {
02030 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02031 vinfos[0].jointtype = 1;
02032 vinfos[0].foffset = j0;
02033 vinfos[0].indices[0] = _ij0[0];
02034 vinfos[0].indices[1] = _ij0[1];
02035 vinfos[0].maxsolutions = _nj0;
02036 vinfos[1].jointtype = 1;
02037 vinfos[1].foffset = j1;
02038 vinfos[1].indices[0] = _ij1[0];
02039 vinfos[1].indices[1] = _ij1[1];
02040 vinfos[1].maxsolutions = _nj1;
02041 vinfos[2].jointtype = 1;
02042 vinfos[2].foffset = j2;
02043 vinfos[2].indices[0] = _ij2[0];
02044 vinfos[2].indices[1] = _ij2[1];
02045 vinfos[2].maxsolutions = _nj2;
02046 vinfos[3].jointtype = 1;
02047 vinfos[3].foffset = j3;
02048 vinfos[3].indices[0] = _ij3[0];
02049 vinfos[3].indices[1] = _ij3[1];
02050 vinfos[3].maxsolutions = _nj3;
02051 vinfos[4].jointtype = 1;
02052 vinfos[4].foffset = j4;
02053 vinfos[4].indices[0] = _ij4[0];
02054 vinfos[4].indices[1] = _ij4[1];
02055 vinfos[4].maxsolutions = _nj4;
02056 vinfos[5].jointtype = 1;
02057 vinfos[5].foffset = j5;
02058 vinfos[5].indices[0] = _ij5[0];
02059 vinfos[5].indices[1] = _ij5[1];
02060 vinfos[5].maxsolutions = _nj5;
02061 std::vector<int> vfree(0);
02062 solutions.AddSolution(vinfos,vfree);
02063 }
02064 }
02065 }
02066 
02067 }
02068 
02069 }
02070 }
02071 }
02072 
02073 }
02074 
02075 }
02076 
02077 } else
02078 {
02079 {
02080 IkReal j3array[1], cj3array[1], sj3array[1];
02081 bool j3valid[1]={false};
02082 _nj3 = 1;
02083 IkReal x152=((gconst1)*(sj4));
02084 if( IKabs(((new_r12)*(x152))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x152))) < IKFAST_ATAN2_MAGTHRESH )
02085     continue;
02086 j3array[0]=IKatan2(((new_r12)*(x152)), ((new_r02)*(x152)));
02087 sj3array[0]=IKsin(j3array[0]);
02088 cj3array[0]=IKcos(j3array[0]);
02089 if( j3array[0] > IKPI )
02090 {
02091     j3array[0]-=IK2PI;
02092 }
02093 else if( j3array[0] < -IKPI )
02094 {    j3array[0]+=IK2PI;
02095 }
02096 j3valid[0] = true;
02097 for(int ij3 = 0; ij3 < 1; ++ij3)
02098 {
02099 if( !j3valid[ij3] )
02100 {
02101     continue;
02102 }
02103 _ij3[0] = ij3; _ij3[1] = -1;
02104 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02105 {
02106 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02107 {
02108     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02109 }
02110 }
02111 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02112 {
02113 IkReal evalcond[6];
02114 IkReal x153=IKsin(j3);
02115 IkReal x154=IKcos(j3);
02116 IkReal x155=((IkReal(1.00000000000000))*(sj4));
02117 IkReal x156=((IkReal(1.00000000000000))*(cj4));
02118 IkReal x157=((new_r02)*(x154));
02119 IkReal x158=((new_r12)*(x153));
02120 evalcond[0]=((((new_r12)*(x154)))+(((IkReal(-1.00000000000000))*(new_r02)*(x153))));
02121 evalcond[1]=((((IkReal(-1.00000000000000))*(x155)))+(x157)+(x158));
02122 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r22)*(x155)))+(((cj4)*(x158)))+(((cj4)*(x157))));
02123 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r10)*(x153)*(x155)))+(((IkReal(-1.00000000000000))*(new_r20)*(x156)))+(((IkReal(-1.00000000000000))*(new_r00)*(x154)*(x155))));
02124 evalcond[4]=((((IkReal(-1.00000000000000))*(new_r21)*(x156)))+(((IkReal(-1.00000000000000))*(new_r01)*(x154)*(x155)))+(((IkReal(-1.00000000000000))*(new_r11)*(x153)*(x155))));
02125 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)*(x156)))+(((IkReal(-1.00000000000000))*(x155)*(x158)))+(((IkReal(-1.00000000000000))*(x155)*(x157))));
02126 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  )
02127 {
02128 continue;
02129 }
02130 }
02131 
02132 {
02133 IkReal dummyeval[1];
02134 IkReal gconst4;
02135 gconst4=IKsign(sj4);
02136 dummyeval[0]=sj4;
02137 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02138 {
02139 {
02140 IkReal dummyeval[1];
02141 dummyeval[0]=sj4;
02142 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02143 {
02144 {
02145 IkReal dummyeval[1];
02146 dummyeval[0]=sj4;
02147 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02148 {
02149 {
02150 IkReal evalcond[11];
02151 IkReal x159=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3))));
02152 IkReal x160=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
02153 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
02154 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
02155 evalcond[2]=new_r20;
02156 evalcond[3]=new_r21;
02157 evalcond[4]=x159;
02158 evalcond[5]=x159;
02159 evalcond[6]=x160;
02160 evalcond[7]=x160;
02161 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
02162 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
02163 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
02164 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  )
02165 {
02166 {
02167 IkReal j5array[1], cj5array[1], sj5array[1];
02168 bool j5valid[1]={false};
02169 _nj5 = 1;
02170 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
02171     continue;
02172 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
02173 sj5array[0]=IKsin(j5array[0]);
02174 cj5array[0]=IKcos(j5array[0]);
02175 if( j5array[0] > IKPI )
02176 {
02177     j5array[0]-=IK2PI;
02178 }
02179 else if( j5array[0] < -IKPI )
02180 {    j5array[0]+=IK2PI;
02181 }
02182 j5valid[0] = true;
02183 for(int ij5 = 0; ij5 < 1; ++ij5)
02184 {
02185 if( !j5valid[ij5] )
02186 {
02187     continue;
02188 }
02189 _ij5[0] = ij5; _ij5[1] = -1;
02190 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02191 {
02192 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02193 {
02194     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02195 }
02196 }
02197 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02198 {
02199 IkReal evalcond[4];
02200 IkReal x161=IKsin(j5);
02201 IkReal x162=((IkReal(1.00000000000000))*(sj3));
02202 IkReal x163=((IkReal(1.00000000000000))*(IKcos(j5)));
02203 evalcond[0]=((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x162)))+(((IkReal(-1.00000000000000))*(x161))));
02204 evalcond[1]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x163)))+(((IkReal(-1.00000000000000))*(new_r01)*(x162))));
02205 evalcond[2]=((((new_r11)*(sj3)))+(x161)+(((cj3)*(new_r01))));
02206 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x163)))+(((cj3)*(new_r00))));
02207 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02208 {
02209 continue;
02210 }
02211 }
02212 
02213 {
02214 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02215 vinfos[0].jointtype = 1;
02216 vinfos[0].foffset = j0;
02217 vinfos[0].indices[0] = _ij0[0];
02218 vinfos[0].indices[1] = _ij0[1];
02219 vinfos[0].maxsolutions = _nj0;
02220 vinfos[1].jointtype = 1;
02221 vinfos[1].foffset = j1;
02222 vinfos[1].indices[0] = _ij1[0];
02223 vinfos[1].indices[1] = _ij1[1];
02224 vinfos[1].maxsolutions = _nj1;
02225 vinfos[2].jointtype = 1;
02226 vinfos[2].foffset = j2;
02227 vinfos[2].indices[0] = _ij2[0];
02228 vinfos[2].indices[1] = _ij2[1];
02229 vinfos[2].maxsolutions = _nj2;
02230 vinfos[3].jointtype = 1;
02231 vinfos[3].foffset = j3;
02232 vinfos[3].indices[0] = _ij3[0];
02233 vinfos[3].indices[1] = _ij3[1];
02234 vinfos[3].maxsolutions = _nj3;
02235 vinfos[4].jointtype = 1;
02236 vinfos[4].foffset = j4;
02237 vinfos[4].indices[0] = _ij4[0];
02238 vinfos[4].indices[1] = _ij4[1];
02239 vinfos[4].maxsolutions = _nj4;
02240 vinfos[5].jointtype = 1;
02241 vinfos[5].foffset = j5;
02242 vinfos[5].indices[0] = _ij5[0];
02243 vinfos[5].indices[1] = _ij5[1];
02244 vinfos[5].maxsolutions = _nj5;
02245 std::vector<int> vfree(0);
02246 solutions.AddSolution(vinfos,vfree);
02247 }
02248 }
02249 }
02250 
02251 } else
02252 {
02253 IkReal x164=((IkReal(1.00000000000000))+(new_r22));
02254 IkReal x165=((new_r12)*(sj3));
02255 IkReal x166=((IkReal(1.00000000000000))*(new_r02));
02256 IkReal x167=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(sj3)*(x166))));
02257 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
02258 evalcond[1]=x164;
02259 evalcond[2]=new_r20;
02260 evalcond[3]=new_r21;
02261 evalcond[4]=x167;
02262 evalcond[5]=x167;
02263 evalcond[6]=((x165)+(((cj3)*(new_r02))));
02264 evalcond[7]=((((IkReal(-1.00000000000000))*(cj3)*(x166)))+(((IkReal(-1.00000000000000))*(x165))));
02265 evalcond[8]=new_r20;
02266 evalcond[9]=new_r21;
02267 evalcond[10]=x164;
02268 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  )
02269 {
02270 {
02271 IkReal j5array[1], cj5array[1], sj5array[1];
02272 bool j5valid[1]={false};
02273 _nj5 = 1;
02274 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 )
02275     continue;
02276 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
02277 sj5array[0]=IKsin(j5array[0]);
02278 cj5array[0]=IKcos(j5array[0]);
02279 if( j5array[0] > IKPI )
02280 {
02281     j5array[0]-=IK2PI;
02282 }
02283 else if( j5array[0] < -IKPI )
02284 {    j5array[0]+=IK2PI;
02285 }
02286 j5valid[0] = true;
02287 for(int ij5 = 0; ij5 < 1; ++ij5)
02288 {
02289 if( !j5valid[ij5] )
02290 {
02291     continue;
02292 }
02293 _ij5[0] = ij5; _ij5[1] = -1;
02294 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02295 {
02296 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02297 {
02298     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02299 }
02300 }
02301 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02302 {
02303 IkReal evalcond[4];
02304 IkReal x168=IKcos(j5);
02305 IkReal x169=((IkReal(1.00000000000000))*(sj3));
02306 IkReal x170=((IkReal(1.00000000000000))*(IKsin(j5)));
02307 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x169)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x170))));
02308 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x169)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x168))));
02309 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x170)))+(((cj3)*(new_r01))));
02310 evalcond[3]=((x168)+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
02311 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02312 {
02313 continue;
02314 }
02315 }
02316 
02317 {
02318 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02319 vinfos[0].jointtype = 1;
02320 vinfos[0].foffset = j0;
02321 vinfos[0].indices[0] = _ij0[0];
02322 vinfos[0].indices[1] = _ij0[1];
02323 vinfos[0].maxsolutions = _nj0;
02324 vinfos[1].jointtype = 1;
02325 vinfos[1].foffset = j1;
02326 vinfos[1].indices[0] = _ij1[0];
02327 vinfos[1].indices[1] = _ij1[1];
02328 vinfos[1].maxsolutions = _nj1;
02329 vinfos[2].jointtype = 1;
02330 vinfos[2].foffset = j2;
02331 vinfos[2].indices[0] = _ij2[0];
02332 vinfos[2].indices[1] = _ij2[1];
02333 vinfos[2].maxsolutions = _nj2;
02334 vinfos[3].jointtype = 1;
02335 vinfos[3].foffset = j3;
02336 vinfos[3].indices[0] = _ij3[0];
02337 vinfos[3].indices[1] = _ij3[1];
02338 vinfos[3].maxsolutions = _nj3;
02339 vinfos[4].jointtype = 1;
02340 vinfos[4].foffset = j4;
02341 vinfos[4].indices[0] = _ij4[0];
02342 vinfos[4].indices[1] = _ij4[1];
02343 vinfos[4].maxsolutions = _nj4;
02344 vinfos[5].jointtype = 1;
02345 vinfos[5].foffset = j5;
02346 vinfos[5].indices[0] = _ij5[0];
02347 vinfos[5].indices[1] = _ij5[1];
02348 vinfos[5].maxsolutions = _nj5;
02349 std::vector<int> vfree(0);
02350 solutions.AddSolution(vinfos,vfree);
02351 }
02352 }
02353 }
02354 
02355 } else
02356 {
02357 if( 1 )
02358 {
02359 continue;
02360 
02361 } else
02362 {
02363 }
02364 }
02365 }
02366 }
02367 
02368 } else
02369 {
02370 {
02371 IkReal j5array[1], cj5array[1], sj5array[1];
02372 bool j5valid[1]={false};
02373 _nj5 = 1;
02374 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
02375     continue;
02376 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
02377 sj5array[0]=IKsin(j5array[0]);
02378 cj5array[0]=IKcos(j5array[0]);
02379 if( j5array[0] > IKPI )
02380 {
02381     j5array[0]-=IK2PI;
02382 }
02383 else if( j5array[0] < -IKPI )
02384 {    j5array[0]+=IK2PI;
02385 }
02386 j5valid[0] = true;
02387 for(int ij5 = 0; ij5 < 1; ++ij5)
02388 {
02389 if( !j5valid[ij5] )
02390 {
02391     continue;
02392 }
02393 _ij5[0] = ij5; _ij5[1] = -1;
02394 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02395 {
02396 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02397 {
02398     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02399 }
02400 }
02401 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02402 {
02403 IkReal evalcond[8];
02404 IkReal x171=IKsin(j5);
02405 IkReal x172=IKcos(j5);
02406 IkReal x173=((IkReal(1.00000000000000))*(sj3));
02407 IkReal x174=((new_r11)*(sj3));
02408 IkReal x175=((new_r10)*(sj3));
02409 IkReal x176=((cj3)*(cj4));
02410 IkReal x177=((IkReal(1.00000000000000))*(sj4));
02411 IkReal x178=((IkReal(1.00000000000000))*(x172));
02412 IkReal x179=((IkReal(1.00000000000000))*(x171));
02413 evalcond[0]=((new_r20)+(((sj4)*(x172))));
02414 evalcond[1]=((((IkReal(-1.00000000000000))*(x171)*(x177)))+(new_r21));
02415 evalcond[2]=((((IkReal(-1.00000000000000))*(x179)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x173))));
02416 evalcond[3]=((((IkReal(-1.00000000000000))*(x178)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x173))));
02417 evalcond[4]=((x174)+(((cj4)*(x171)))+(((cj3)*(new_r01))));
02418 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x178)))+(x175)+(((cj3)*(new_r00))));
02419 evalcond[6]=((x171)+(((IkReal(-1.00000000000000))*(new_r21)*(x177)))+(((new_r01)*(x176)))+(((cj4)*(x174))));
02420 evalcond[7]=((((IkReal(-1.00000000000000))*(x178)))+(((new_r00)*(x176)))+(((IkReal(-1.00000000000000))*(new_r20)*(x177)))+(((cj4)*(x175))));
02421 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  )
02422 {
02423 continue;
02424 }
02425 }
02426 
02427 {
02428 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02429 vinfos[0].jointtype = 1;
02430 vinfos[0].foffset = j0;
02431 vinfos[0].indices[0] = _ij0[0];
02432 vinfos[0].indices[1] = _ij0[1];
02433 vinfos[0].maxsolutions = _nj0;
02434 vinfos[1].jointtype = 1;
02435 vinfos[1].foffset = j1;
02436 vinfos[1].indices[0] = _ij1[0];
02437 vinfos[1].indices[1] = _ij1[1];
02438 vinfos[1].maxsolutions = _nj1;
02439 vinfos[2].jointtype = 1;
02440 vinfos[2].foffset = j2;
02441 vinfos[2].indices[0] = _ij2[0];
02442 vinfos[2].indices[1] = _ij2[1];
02443 vinfos[2].maxsolutions = _nj2;
02444 vinfos[3].jointtype = 1;
02445 vinfos[3].foffset = j3;
02446 vinfos[3].indices[0] = _ij3[0];
02447 vinfos[3].indices[1] = _ij3[1];
02448 vinfos[3].maxsolutions = _nj3;
02449 vinfos[4].jointtype = 1;
02450 vinfos[4].foffset = j4;
02451 vinfos[4].indices[0] = _ij4[0];
02452 vinfos[4].indices[1] = _ij4[1];
02453 vinfos[4].maxsolutions = _nj4;
02454 vinfos[5].jointtype = 1;
02455 vinfos[5].foffset = j5;
02456 vinfos[5].indices[0] = _ij5[0];
02457 vinfos[5].indices[1] = _ij5[1];
02458 vinfos[5].maxsolutions = _nj5;
02459 std::vector<int> vfree(0);
02460 solutions.AddSolution(vinfos,vfree);
02461 }
02462 }
02463 }
02464 
02465 }
02466 
02467 }
02468 
02469 } else
02470 {
02471 {
02472 IkReal j5array[1], cj5array[1], sj5array[1];
02473 bool j5valid[1]={false};
02474 _nj5 = 1;
02475 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 )
02476     continue;
02477 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
02478 sj5array[0]=IKsin(j5array[0]);
02479 cj5array[0]=IKcos(j5array[0]);
02480 if( j5array[0] > IKPI )
02481 {
02482     j5array[0]-=IK2PI;
02483 }
02484 else if( j5array[0] < -IKPI )
02485 {    j5array[0]+=IK2PI;
02486 }
02487 j5valid[0] = true;
02488 for(int ij5 = 0; ij5 < 1; ++ij5)
02489 {
02490 if( !j5valid[ij5] )
02491 {
02492     continue;
02493 }
02494 _ij5[0] = ij5; _ij5[1] = -1;
02495 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02496 {
02497 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02498 {
02499     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02500 }
02501 }
02502 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02503 {
02504 IkReal evalcond[8];
02505 IkReal x180=IKsin(j5);
02506 IkReal x181=IKcos(j5);
02507 IkReal x182=((IkReal(1.00000000000000))*(sj3));
02508 IkReal x183=((new_r11)*(sj3));
02509 IkReal x184=((new_r10)*(sj3));
02510 IkReal x185=((cj3)*(cj4));
02511 IkReal x186=((IkReal(1.00000000000000))*(sj4));
02512 IkReal x187=((IkReal(1.00000000000000))*(x181));
02513 IkReal x188=((IkReal(1.00000000000000))*(x180));
02514 evalcond[0]=((new_r20)+(((sj4)*(x181))));
02515 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x180)*(x186))));
02516 evalcond[2]=((((IkReal(-1.00000000000000))*(x188)))+(((IkReal(-1.00000000000000))*(new_r00)*(x182)))+(((cj3)*(new_r10))));
02517 evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x187)))+(((IkReal(-1.00000000000000))*(new_r01)*(x182))));
02518 evalcond[4]=((x183)+(((cj4)*(x180)))+(((cj3)*(new_r01))));
02519 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x187)))+(x184)+(((cj3)*(new_r00))));
02520 evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x186)))+(((cj4)*(x183)))+(x180)+(((new_r01)*(x185))));
02521 evalcond[7]=((((cj4)*(x184)))+(((IkReal(-1.00000000000000))*(x187)))+(((new_r00)*(x185)))+(((IkReal(-1.00000000000000))*(new_r20)*(x186))));
02522 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  )
02523 {
02524 continue;
02525 }
02526 }
02527 
02528 {
02529 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02530 vinfos[0].jointtype = 1;
02531 vinfos[0].foffset = j0;
02532 vinfos[0].indices[0] = _ij0[0];
02533 vinfos[0].indices[1] = _ij0[1];
02534 vinfos[0].maxsolutions = _nj0;
02535 vinfos[1].jointtype = 1;
02536 vinfos[1].foffset = j1;
02537 vinfos[1].indices[0] = _ij1[0];
02538 vinfos[1].indices[1] = _ij1[1];
02539 vinfos[1].maxsolutions = _nj1;
02540 vinfos[2].jointtype = 1;
02541 vinfos[2].foffset = j2;
02542 vinfos[2].indices[0] = _ij2[0];
02543 vinfos[2].indices[1] = _ij2[1];
02544 vinfos[2].maxsolutions = _nj2;
02545 vinfos[3].jointtype = 1;
02546 vinfos[3].foffset = j3;
02547 vinfos[3].indices[0] = _ij3[0];
02548 vinfos[3].indices[1] = _ij3[1];
02549 vinfos[3].maxsolutions = _nj3;
02550 vinfos[4].jointtype = 1;
02551 vinfos[4].foffset = j4;
02552 vinfos[4].indices[0] = _ij4[0];
02553 vinfos[4].indices[1] = _ij4[1];
02554 vinfos[4].maxsolutions = _nj4;
02555 vinfos[5].jointtype = 1;
02556 vinfos[5].foffset = j5;
02557 vinfos[5].indices[0] = _ij5[0];
02558 vinfos[5].indices[1] = _ij5[1];
02559 vinfos[5].maxsolutions = _nj5;
02560 std::vector<int> vfree(0);
02561 solutions.AddSolution(vinfos,vfree);
02562 }
02563 }
02564 }
02565 
02566 }
02567 
02568 }
02569 
02570 } else
02571 {
02572 {
02573 IkReal j5array[1], cj5array[1], sj5array[1];
02574 bool j5valid[1]={false};
02575 _nj5 = 1;
02576 if( IKabs(((gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02577     continue;
02578 j5array[0]=IKatan2(((gconst4)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst4)*(new_r20)));
02579 sj5array[0]=IKsin(j5array[0]);
02580 cj5array[0]=IKcos(j5array[0]);
02581 if( j5array[0] > IKPI )
02582 {
02583     j5array[0]-=IK2PI;
02584 }
02585 else if( j5array[0] < -IKPI )
02586 {    j5array[0]+=IK2PI;
02587 }
02588 j5valid[0] = true;
02589 for(int ij5 = 0; ij5 < 1; ++ij5)
02590 {
02591 if( !j5valid[ij5] )
02592 {
02593     continue;
02594 }
02595 _ij5[0] = ij5; _ij5[1] = -1;
02596 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02597 {
02598 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02599 {
02600     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02601 }
02602 }
02603 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02604 {
02605 IkReal evalcond[8];
02606 IkReal x189=IKsin(j5);
02607 IkReal x190=IKcos(j5);
02608 IkReal x191=((IkReal(1.00000000000000))*(sj3));
02609 IkReal x192=((new_r11)*(sj3));
02610 IkReal x193=((new_r10)*(sj3));
02611 IkReal x194=((cj3)*(cj4));
02612 IkReal x195=((IkReal(1.00000000000000))*(sj4));
02613 IkReal x196=((IkReal(1.00000000000000))*(x190));
02614 IkReal x197=((IkReal(1.00000000000000))*(x189));
02615 evalcond[0]=((new_r20)+(((sj4)*(x190))));
02616 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x189)*(x195))));
02617 evalcond[2]=((((IkReal(-1.00000000000000))*(x197)))+(((IkReal(-1.00000000000000))*(new_r00)*(x191)))+(((cj3)*(new_r10))));
02618 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x191)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x196))));
02619 evalcond[4]=((x192)+(((cj4)*(x189)))+(((cj3)*(new_r01))));
02620 evalcond[5]=((x193)+(((IkReal(-1.00000000000000))*(cj4)*(x196)))+(((cj3)*(new_r00))));
02621 evalcond[6]=((x189)+(((new_r01)*(x194)))+(((IkReal(-1.00000000000000))*(new_r21)*(x195)))+(((cj4)*(x192))));
02622 evalcond[7]=((((new_r00)*(x194)))+(((cj4)*(x193)))+(((IkReal(-1.00000000000000))*(new_r20)*(x195)))+(((IkReal(-1.00000000000000))*(x196))));
02623 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  )
02624 {
02625 continue;
02626 }
02627 }
02628 
02629 {
02630 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02631 vinfos[0].jointtype = 1;
02632 vinfos[0].foffset = j0;
02633 vinfos[0].indices[0] = _ij0[0];
02634 vinfos[0].indices[1] = _ij0[1];
02635 vinfos[0].maxsolutions = _nj0;
02636 vinfos[1].jointtype = 1;
02637 vinfos[1].foffset = j1;
02638 vinfos[1].indices[0] = _ij1[0];
02639 vinfos[1].indices[1] = _ij1[1];
02640 vinfos[1].maxsolutions = _nj1;
02641 vinfos[2].jointtype = 1;
02642 vinfos[2].foffset = j2;
02643 vinfos[2].indices[0] = _ij2[0];
02644 vinfos[2].indices[1] = _ij2[1];
02645 vinfos[2].maxsolutions = _nj2;
02646 vinfos[3].jointtype = 1;
02647 vinfos[3].foffset = j3;
02648 vinfos[3].indices[0] = _ij3[0];
02649 vinfos[3].indices[1] = _ij3[1];
02650 vinfos[3].maxsolutions = _nj3;
02651 vinfos[4].jointtype = 1;
02652 vinfos[4].foffset = j4;
02653 vinfos[4].indices[0] = _ij4[0];
02654 vinfos[4].indices[1] = _ij4[1];
02655 vinfos[4].maxsolutions = _nj4;
02656 vinfos[5].jointtype = 1;
02657 vinfos[5].foffset = j5;
02658 vinfos[5].indices[0] = _ij5[0];
02659 vinfos[5].indices[1] = _ij5[1];
02660 vinfos[5].maxsolutions = _nj5;
02661 std::vector<int> vfree(0);
02662 solutions.AddSolution(vinfos,vfree);
02663 }
02664 }
02665 }
02666 
02667 }
02668 
02669 }
02670 }
02671 }
02672 
02673 }
02674 
02675 }
02676 
02677 } else
02678 {
02679 {
02680 IkReal j5array[1], cj5array[1], sj5array[1];
02681 bool j5valid[1]={false};
02682 _nj5 = 1;
02683 if( IKabs(((gconst3)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst3)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02684     continue;
02685 j5array[0]=IKatan2(((gconst3)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst3)*(new_r20)));
02686 sj5array[0]=IKsin(j5array[0]);
02687 cj5array[0]=IKcos(j5array[0]);
02688 if( j5array[0] > IKPI )
02689 {
02690     j5array[0]-=IK2PI;
02691 }
02692 else if( j5array[0] < -IKPI )
02693 {    j5array[0]+=IK2PI;
02694 }
02695 j5valid[0] = true;
02696 for(int ij5 = 0; ij5 < 1; ++ij5)
02697 {
02698 if( !j5valid[ij5] )
02699 {
02700     continue;
02701 }
02702 _ij5[0] = ij5; _ij5[1] = -1;
02703 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02704 {
02705 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02706 {
02707     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02708 }
02709 }
02710 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02711 {
02712 IkReal evalcond[2];
02713 evalcond[0]=((new_r20)+(((sj4)*(IKcos(j5)))));
02714 evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(IKsin(j5))))+(new_r21));
02715 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02716 {
02717 continue;
02718 }
02719 }
02720 
02721 {
02722 IkReal dummyeval[1];
02723 IkReal gconst5;
02724 gconst5=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
02725 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
02726 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02727 {
02728 {
02729 IkReal dummyeval[1];
02730 IkReal gconst6;
02731 gconst6=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12)))));
02732 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12))));
02733 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02734 {
02735 continue;
02736 
02737 } else
02738 {
02739 {
02740 IkReal j3array[1], cj3array[1], sj3array[1];
02741 bool j3valid[1]={false};
02742 _nj3 = 1;
02743 IkReal x198=((cj4)*(gconst6)*(sj5));
02744 if( IKabs(((new_r12)*(x198))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x198))) < IKFAST_ATAN2_MAGTHRESH )
02745     continue;
02746 j3array[0]=IKatan2(((new_r12)*(x198)), ((new_r02)*(x198)));
02747 sj3array[0]=IKsin(j3array[0]);
02748 cj3array[0]=IKcos(j3array[0]);
02749 if( j3array[0] > IKPI )
02750 {
02751     j3array[0]-=IK2PI;
02752 }
02753 else if( j3array[0] < -IKPI )
02754 {    j3array[0]+=IK2PI;
02755 }
02756 j3valid[0] = true;
02757 for(int ij3 = 0; ij3 < 1; ++ij3)
02758 {
02759 if( !j3valid[ij3] )
02760 {
02761     continue;
02762 }
02763 _ij3[0] = ij3; _ij3[1] = -1;
02764 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02765 {
02766 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02767 {
02768     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02769 }
02770 }
02771 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02772 {
02773 IkReal evalcond[12];
02774 IkReal x199=IKsin(j3);
02775 IkReal x200=IKcos(j3);
02776 IkReal x201=((IkReal(1.00000000000000))*(cj5));
02777 IkReal x202=((IkReal(1.00000000000000))*(cj4));
02778 IkReal x203=((IkReal(1.00000000000000))*(sj4));
02779 IkReal x204=((cj4)*(x200));
02780 IkReal x205=((cj4)*(x199));
02781 IkReal x206=((new_r00)*(x200));
02782 IkReal x207=((new_r11)*(x199));
02783 IkReal x208=((new_r02)*(x200));
02784 IkReal x209=((new_r01)*(x200));
02785 IkReal x210=((new_r12)*(x199));
02786 IkReal x211=((IkReal(1.00000000000000))*(x199));
02787 IkReal x212=((new_r10)*(x199));
02788 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x211)))+(((new_r12)*(x200))));
02789 evalcond[1]=((((IkReal(-1.00000000000000))*(x203)))+(x210)+(x208));
02790 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x211)))+(((new_r10)*(x200)))+(((IkReal(-1.00000000000000))*(sj5))));
02791 evalcond[3]=((((new_r11)*(x200)))+(((IkReal(-1.00000000000000))*(new_r01)*(x211)))+(((IkReal(-1.00000000000000))*(x201))));
02792 evalcond[4]=((x207)+(x209)+(((cj4)*(sj5))));
02793 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x201)))+(x212)+(x206));
02794 evalcond[6]=((((new_r12)*(x205)))+(((new_r02)*(x204)))+(((IkReal(-1.00000000000000))*(new_r22)*(x203))));
02795 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x202)))+(((IkReal(-1.00000000000000))*(x203)*(x206)))+(((IkReal(-1.00000000000000))*(x203)*(x212))));
02796 evalcond[8]=((((IkReal(-1.00000000000000))*(x203)*(x209)))+(((IkReal(-1.00000000000000))*(new_r21)*(x202)))+(((IkReal(-1.00000000000000))*(x203)*(x207))));
02797 evalcond[9]=((((new_r11)*(x205)))+(sj5)+(((IkReal(-1.00000000000000))*(new_r21)*(x203)))+(((new_r01)*(x204))));
02798 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)*(x202)))+(((IkReal(-1.00000000000000))*(x203)*(x210)))+(((IkReal(-1.00000000000000))*(x203)*(x208))));
02799 evalcond[11]=((((new_r00)*(x204)))+(((IkReal(-1.00000000000000))*(new_r20)*(x203)))+(((new_r10)*(x205)))+(((IkReal(-1.00000000000000))*(x201))));
02800 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  )
02801 {
02802 continue;
02803 }
02804 }
02805 
02806 {
02807 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02808 vinfos[0].jointtype = 1;
02809 vinfos[0].foffset = j0;
02810 vinfos[0].indices[0] = _ij0[0];
02811 vinfos[0].indices[1] = _ij0[1];
02812 vinfos[0].maxsolutions = _nj0;
02813 vinfos[1].jointtype = 1;
02814 vinfos[1].foffset = j1;
02815 vinfos[1].indices[0] = _ij1[0];
02816 vinfos[1].indices[1] = _ij1[1];
02817 vinfos[1].maxsolutions = _nj1;
02818 vinfos[2].jointtype = 1;
02819 vinfos[2].foffset = j2;
02820 vinfos[2].indices[0] = _ij2[0];
02821 vinfos[2].indices[1] = _ij2[1];
02822 vinfos[2].maxsolutions = _nj2;
02823 vinfos[3].jointtype = 1;
02824 vinfos[3].foffset = j3;
02825 vinfos[3].indices[0] = _ij3[0];
02826 vinfos[3].indices[1] = _ij3[1];
02827 vinfos[3].maxsolutions = _nj3;
02828 vinfos[4].jointtype = 1;
02829 vinfos[4].foffset = j4;
02830 vinfos[4].indices[0] = _ij4[0];
02831 vinfos[4].indices[1] = _ij4[1];
02832 vinfos[4].maxsolutions = _nj4;
02833 vinfos[5].jointtype = 1;
02834 vinfos[5].foffset = j5;
02835 vinfos[5].indices[0] = _ij5[0];
02836 vinfos[5].indices[1] = _ij5[1];
02837 vinfos[5].maxsolutions = _nj5;
02838 std::vector<int> vfree(0);
02839 solutions.AddSolution(vinfos,vfree);
02840 }
02841 }
02842 }
02843 
02844 }
02845 
02846 }
02847 
02848 } else
02849 {
02850 {
02851 IkReal j3array[1], cj3array[1], sj3array[1];
02852 bool j3valid[1]={false};
02853 _nj3 = 1;
02854 IkReal x213=((gconst5)*(sj4));
02855 if( IKabs(((new_r12)*(x213))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x213))) < IKFAST_ATAN2_MAGTHRESH )
02856     continue;
02857 j3array[0]=IKatan2(((new_r12)*(x213)), ((new_r02)*(x213)));
02858 sj3array[0]=IKsin(j3array[0]);
02859 cj3array[0]=IKcos(j3array[0]);
02860 if( j3array[0] > IKPI )
02861 {
02862     j3array[0]-=IK2PI;
02863 }
02864 else if( j3array[0] < -IKPI )
02865 {    j3array[0]+=IK2PI;
02866 }
02867 j3valid[0] = true;
02868 for(int ij3 = 0; ij3 < 1; ++ij3)
02869 {
02870 if( !j3valid[ij3] )
02871 {
02872     continue;
02873 }
02874 _ij3[0] = ij3; _ij3[1] = -1;
02875 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02876 {
02877 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02878 {
02879     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02880 }
02881 }
02882 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02883 {
02884 IkReal evalcond[12];
02885 IkReal x214=IKsin(j3);
02886 IkReal x215=IKcos(j3);
02887 IkReal x216=((IkReal(1.00000000000000))*(cj5));
02888 IkReal x217=((IkReal(1.00000000000000))*(cj4));
02889 IkReal x218=((IkReal(1.00000000000000))*(sj4));
02890 IkReal x219=((cj4)*(x215));
02891 IkReal x220=((cj4)*(x214));
02892 IkReal x221=((new_r00)*(x215));
02893 IkReal x222=((new_r11)*(x214));
02894 IkReal x223=((new_r02)*(x215));
02895 IkReal x224=((new_r01)*(x215));
02896 IkReal x225=((new_r12)*(x214));
02897 IkReal x226=((IkReal(1.00000000000000))*(x214));
02898 IkReal x227=((new_r10)*(x214));
02899 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x226)))+(((new_r12)*(x215))));
02900 evalcond[1]=((((IkReal(-1.00000000000000))*(x218)))+(x223)+(x225));
02901 evalcond[2]=((((new_r10)*(x215)))+(((IkReal(-1.00000000000000))*(new_r00)*(x226)))+(((IkReal(-1.00000000000000))*(sj5))));
02902 evalcond[3]=((((new_r11)*(x215)))+(((IkReal(-1.00000000000000))*(new_r01)*(x226)))+(((IkReal(-1.00000000000000))*(x216))));
02903 evalcond[4]=((x222)+(x224)+(((cj4)*(sj5))));
02904 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x216)))+(x221)+(x227));
02905 evalcond[6]=((((new_r12)*(x220)))+(((IkReal(-1.00000000000000))*(new_r22)*(x218)))+(((new_r02)*(x219))));
02906 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x217)))+(((IkReal(-1.00000000000000))*(x218)*(x227)))+(((IkReal(-1.00000000000000))*(x218)*(x221))));
02907 evalcond[8]=((((IkReal(-1.00000000000000))*(x218)*(x222)))+(((IkReal(-1.00000000000000))*(new_r21)*(x217)))+(((IkReal(-1.00000000000000))*(x218)*(x224))));
02908 evalcond[9]=((((new_r01)*(x219)))+(((IkReal(-1.00000000000000))*(new_r21)*(x218)))+(sj5)+(((new_r11)*(x220))));
02909 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x218)*(x223)))+(((IkReal(-1.00000000000000))*(new_r22)*(x217)))+(((IkReal(-1.00000000000000))*(x218)*(x225))));
02910 evalcond[11]=((((new_r00)*(x219)))+(((IkReal(-1.00000000000000))*(new_r20)*(x218)))+(((IkReal(-1.00000000000000))*(x216)))+(((new_r10)*(x220))));
02911 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  )
02912 {
02913 continue;
02914 }
02915 }
02916 
02917 {
02918 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02919 vinfos[0].jointtype = 1;
02920 vinfos[0].foffset = j0;
02921 vinfos[0].indices[0] = _ij0[0];
02922 vinfos[0].indices[1] = _ij0[1];
02923 vinfos[0].maxsolutions = _nj0;
02924 vinfos[1].jointtype = 1;
02925 vinfos[1].foffset = j1;
02926 vinfos[1].indices[0] = _ij1[0];
02927 vinfos[1].indices[1] = _ij1[1];
02928 vinfos[1].maxsolutions = _nj1;
02929 vinfos[2].jointtype = 1;
02930 vinfos[2].foffset = j2;
02931 vinfos[2].indices[0] = _ij2[0];
02932 vinfos[2].indices[1] = _ij2[1];
02933 vinfos[2].maxsolutions = _nj2;
02934 vinfos[3].jointtype = 1;
02935 vinfos[3].foffset = j3;
02936 vinfos[3].indices[0] = _ij3[0];
02937 vinfos[3].indices[1] = _ij3[1];
02938 vinfos[3].maxsolutions = _nj3;
02939 vinfos[4].jointtype = 1;
02940 vinfos[4].foffset = j4;
02941 vinfos[4].indices[0] = _ij4[0];
02942 vinfos[4].indices[1] = _ij4[1];
02943 vinfos[4].maxsolutions = _nj4;
02944 vinfos[5].jointtype = 1;
02945 vinfos[5].foffset = j5;
02946 vinfos[5].indices[0] = _ij5[0];
02947 vinfos[5].indices[1] = _ij5[1];
02948 vinfos[5].maxsolutions = _nj5;
02949 std::vector<int> vfree(0);
02950 solutions.AddSolution(vinfos,vfree);
02951 }
02952 }
02953 }
02954 
02955 }
02956 
02957 }
02958 }
02959 }
02960 
02961 }
02962 
02963 }
02964 }
02965 }
02966 }
02967 }};
02968 
02969 
02972 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
02973 IKSolver solver;
02974 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
02975 }
02976 
02977 IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - staubli_rx160 (d3ee5b9916b20168fe976c8233d52676)>"; }
02978 
02979 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
02980 
02981 #ifdef IKFAST_NAMESPACE
02982 } // end namespace
02983 #endif
02984 
02985 #ifndef IKFAST_NO_MAIN
02986 #include <stdio.h>
02987 #include <stdlib.h>
02988 #ifdef IKFAST_NAMESPACE
02989 using namespace IKFAST_NAMESPACE;
02990 #endif
02991 int main(int argc, char** argv)
02992 {
02993     if( argc != 12+GetNumFreeParameters()+1 ) {
02994         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
02995                "Returns the ik solutions given the transformation of the end effector specified by\n"
02996                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
02997                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
02998         return 1;
02999     }
03000 
03001     IkSolutionList<IkReal> solutions;
03002     std::vector<IkReal> vfree(GetNumFreeParameters());
03003     IkReal eerot[9],eetrans[3];
03004     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
03005     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
03006     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
03007     for(std::size_t i = 0; i < vfree.size(); ++i)
03008         vfree[i] = atof(argv[13+i]);
03009     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
03010 
03011     if( !bSuccess ) {
03012         fprintf(stderr,"Failed to get ik solution\n");
03013         return -1;
03014     }
03015 
03016     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
03017     std::vector<IkReal> solvalues(GetNumJoints());
03018     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
03019         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
03020         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
03021         std::vector<IkReal> vsolfree(sol.GetFree().size());
03022         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
03023         for( std::size_t j = 0; j < solvalues.size(); ++j)
03024             printf("%.15f, ", solvalues[j]);
03025         printf("\n");
03026     }
03027     return 0;
03028 }
03029 
03030 #endif