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