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