fetch_arm_ikfast_solver.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 // check if the included ikfast version matches what this file was compiled with
00025 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00026 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
00027 
00028 #include <cmath>
00029 #include <vector>
00030 #include <limits>
00031 #include <algorithm>
00032 #include <complex>
00033 
00034 #ifndef IKFAST_ASSERT
00035 #include <stdexcept>
00036 #include <sstream>
00037 #include <iostream>
00038 
00039 #ifdef _MSC_VER
00040 #ifndef __PRETTY_FUNCTION__
00041 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00042 #endif
00043 #endif
00044 
00045 #ifndef __PRETTY_FUNCTION__
00046 #define __PRETTY_FUNCTION__ __func__
00047 #endif
00048 
00049 #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()); } }
00050 
00051 #endif
00052 
00053 #if defined(_MSC_VER)
00054 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00055 #else
00056 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00057 #endif
00058 
00059 #define IK2PI  ((IkReal)6.28318530717959)
00060 #define IKPI  ((IkReal)3.14159265358979)
00061 #define IKPI_2  ((IkReal)1.57079632679490)
00062 
00063 #ifdef _MSC_VER
00064 #ifndef isnan
00065 #define isnan _isnan
00066 #endif
00067 #ifndef isinf
00068 #define isinf _isinf
00069 #endif
00070 //#ifndef isfinite
00071 //#define isfinite _isfinite
00072 //#endif
00073 #endif // _MSC_VER
00074 
00075 // lapack routines
00076 extern "C" {
00077   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00078   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00079   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00080   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00081   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);
00082   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);
00083 }
00084 
00085 using namespace std; // necessary to get std math routines
00086 
00087 #ifdef IKFAST_NAMESPACE
00088 namespace IKFAST_NAMESPACE {
00089 #endif
00090 
00091 inline float IKabs(float f) { return fabsf(f); }
00092 inline double IKabs(double f) { return fabs(f); }
00093 
00094 inline float IKsqr(float f) { return f*f; }
00095 inline double IKsqr(double f) { return f*f; }
00096 
00097 inline float IKlog(float f) { return logf(f); }
00098 inline double IKlog(double f) { return log(f); }
00099 
00100 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
00101 #ifndef IKFAST_SINCOS_THRESH
00102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
00103 #endif
00104 
00105 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
00106 #ifndef IKFAST_ATAN2_MAGTHRESH
00107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
00108 #endif
00109 
00110 // minimum distance of separate solutions
00111 #ifndef IKFAST_SOLUTION_THRESH
00112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00113 #endif
00114 
00115 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
00116 #ifndef IKFAST_EVALCOND_THRESH
00117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
00118 #endif
00119 
00120 
00121 inline float IKasin(float f)
00122 {
00123 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00124 if( f <= -1 ) return float(-IKPI_2);
00125 else if( f >= 1 ) return float(IKPI_2);
00126 return asinf(f);
00127 }
00128 inline double IKasin(double f)
00129 {
00130 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00131 if( f <= -1 ) return -IKPI_2;
00132 else if( f >= 1 ) return IKPI_2;
00133 return asin(f);
00134 }
00135 
00136 // return positive value in [0,y)
00137 inline float IKfmod(float x, float y)
00138 {
00139     while(x < 0) {
00140         x += y;
00141     }
00142     return fmodf(x,y);
00143 }
00144 
00145 // return positive value in [0,y)
00146 inline double IKfmod(double x, double y)
00147 {
00148     while(x < 0) {
00149         x += y;
00150     }
00151     return fmod(x,y);
00152 }
00153 
00154 inline float IKacos(float f)
00155 {
00156 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00157 if( f <= -1 ) return float(IKPI);
00158 else if( f >= 1 ) return float(0);
00159 return acosf(f);
00160 }
00161 inline double IKacos(double f)
00162 {
00163 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00164 if( f <= -1 ) return IKPI;
00165 else if( f >= 1 ) return 0;
00166 return acos(f);
00167 }
00168 inline float IKsin(float f) { return sinf(f); }
00169 inline double IKsin(double f) { return sin(f); }
00170 inline float IKcos(float f) { return cosf(f); }
00171 inline double IKcos(double f) { return cos(f); }
00172 inline float IKtan(float f) { return tanf(f); }
00173 inline double IKtan(double f) { return tan(f); }
00174 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00175 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00176 inline float IKatan2Simple(float fy, float fx) {
00177     return atan2f(fy,fx);
00178 }
00179 inline float IKatan2(float fy, float fx) {
00180     if( isnan(fy) ) {
00181         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00182         return float(IKPI_2);
00183     }
00184     else if( isnan(fx) ) {
00185         return 0;
00186     }
00187     return atan2f(fy,fx);
00188 }
00189 inline double IKatan2Simple(double fy, double fx) {
00190     return atan2(fy,fx);
00191 }
00192 inline double IKatan2(double fy, double fx) {
00193     if( isnan(fy) ) {
00194         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00195         return IKPI_2;
00196     }
00197     else if( isnan(fx) ) {
00198         return 0;
00199     }
00200     return atan2(fy,fx);
00201 }
00202 
00203 template <typename T>
00204 struct CheckValue
00205 {
00206     T value;
00207     bool valid;
00208 };
00209 
00210 template <typename T>
00211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
00212 {
00213     CheckValue<T> ret;
00214     ret.valid = false;
00215     ret.value = 0;
00216     if( !isnan(fy) && !isnan(fx) ) {
00217         if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
00218             ret.value = IKatan2Simple(fy,fx);
00219             ret.valid = true;
00220         }
00221     }
00222     return ret;
00223 }
00224 
00225 inline float IKsign(float f) {
00226     if( f > 0 ) {
00227         return float(1);
00228     }
00229     else if( f < 0 ) {
00230         return float(-1);
00231     }
00232     return 0;
00233 }
00234 
00235 inline double IKsign(double f) {
00236     if( f > 0 ) {
00237         return 1.0;
00238     }
00239     else if( f < 0 ) {
00240         return -1.0;
00241     }
00242     return 0;
00243 }
00244 
00245 template <typename T>
00246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
00247 {
00248     CheckValue<T> ret;
00249     ret.valid = true;
00250     if( n == 0 ) {
00251         ret.value = 1.0;
00252         return ret;
00253     }
00254     else if( n == 1 )
00255     {
00256         ret.value = f;
00257         return ret;
00258     }
00259     else if( n < 0 )
00260     {
00261         if( f == 0 )
00262         {
00263             ret.valid = false;
00264             ret.value = (T)1.0e30;
00265             return ret;
00266         }
00267         if( n == -1 ) {
00268             ret.value = T(1.0)/f;
00269             return ret;
00270         }
00271     }
00272 
00273     int num = n > 0 ? n : -n;
00274     if( num == 2 ) {
00275         ret.value = f*f;
00276     }
00277     else if( num == 3 ) {
00278         ret.value = f*f*f;
00279     }
00280     else {
00281         ret.value = 1.0;
00282         while(num>0) {
00283             if( num & 1 ) {
00284                 ret.value *= f;
00285             }
00286             num >>= 1;
00287             f *= f;
00288         }
00289     }
00290     
00291     if( n < 0 ) {
00292         ret.value = T(1.0)/ret.value;
00293     }
00294     return ret;
00295 }
00296 
00299 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00300 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59;
00301 x0=IKsin(j[2]);
00302 x1=IKsin(j[5]);
00303 x2=IKsin(j[4]);
00304 x3=IKcos(j[4]);
00305 x4=IKcos(j[2]);
00306 x5=IKcos(j[3]);
00307 x6=IKsin(j[1]);
00308 x7=IKsin(j[3]);
00309 x8=IKcos(j[5]);
00310 x9=IKcos(j[1]);
00311 x10=IKsin(j[0]);
00312 x11=IKcos(j[0]);
00313 x12=IKcos(j[6]);
00314 x13=IKsin(j[6]);
00315 x14=((1.0)*x1);
00316 x15=((1.0)*x13);
00317 x16=((1.0)*x9);
00318 x17=((1.0)*x12);
00319 x18=((0.1385)*x3);
00320 x19=((1.0)*x6);
00321 x20=((1.0)*x3);
00322 x21=((1.0)*x4);
00323 x22=((1.0)*x8);
00324 x23=(x0*x3);
00325 x24=(x5*x9);
00326 x25=(x0*x7);
00327 x26=(x2*x4);
00328 x27=(x0*x2);
00329 x28=(x5*x6);
00330 x29=(x4*x7);
00331 x30=((0.3215)*x25);
00332 x31=(x19*x7);
00333 x32=((0.1385)*x29);
00334 x33=(x19*x29);
00335 x34=((1.0)*x27*x5);
00336 x35=((0.1385)*x25*x8);
00337 x36=(x16*x27);
00338 x37=(x16*x23);
00339 x38=(((x23*x5))+x26);
00340 x39=((((-1.0)*x34))+((x3*x4)));
00341 x40=((((-1.0)*x31))+((x24*x4)));
00342 x41=(x34+(((-1.0)*x20*x4)));
00343 x42=((((-1.0)*x16*x4*x5))+x31);
00344 x43=(((x16*x7))+((x19*x4*x5)));
00345 x44=((-1.0)*x43);
00346 x45=(((x0*x18*x5))+(((0.1385)*x26)));
00347 x46=((((-1.0)*x19*x5))+(((-1.0)*x16*x29)));
00348 x47=(x30+x35);
00349 x48=(x2*x42);
00350 x49=(x2*x43);
00351 x50=(((x14*x25))+(((-1.0)*x22*x38)));
00352 x51=(((x27*x6))+((x3*x44)));
00353 x52=((1.0)*x50);
00354 x53=(x13*x50);
00355 x54=(((x8*(((((-1.0)*x33))+x24))))+((x1*x51)));
00356 x55=(((x8*(((((-1.0)*x36))+((x3*x40))))))+((x1*x46)));
00357 x56=(((x14*(((((-1.0)*x33))+((x16*x5))))))+(((-1.0)*x22*x51)));
00358 x57=((((0.352)*x9))+(((0.3215)*x24))+(((-0.3215)*x29*x6))+((x1*(((((0.1385)*x27*x6))+((x18*x44))))))+((x8*(((((-1.0)*x32*x6))+(((0.1385)*x24)))))));
00359 x58=(((x13*x56))+((x12*((((x23*x6))+x49)))));
00360 x59=(((x12*x56))+((x13*(((((-1.0)*x49))+(((-1.0)*x19*x23)))))));
00361 eerot[0]=(((x11*x54))+((x10*(((((-1.0)*x14*x38))+(((-1.0)*x22*x25)))))));
00362 eerot[1]=(((x10*(((((-1.0)*x17*x39))+(((-1.0)*x15*x50))))))+((x11*x58)));
00363 eerot[2]=(((x11*x59))+((x10*(((((-1.0)*x15*x41))+(((-1.0)*x17*x50)))))));
00364 eetrans[0]=((0.119525)+((x10*(((((-1.0)*x47))+(((-1.0)*x14*x45))))))+((x11*x57))+(((0.117)*x11)));
00365 eerot[3]=(((x11*((((x1*x38))+((x25*x8))))))+((x10*x54)));
00366 eerot[4]=(((x11*((x53+((x12*x39))))))+((x10*x58)));
00367 eerot[5]=(((x11*((((x12*x50))+((x13*x41))))))+((x10*x59)));
00368 eetrans[1]=(((x10*x57))+((x11*((x47+((x1*x45))))))+(((0.117)*x10)));
00369 eerot[6]=(((x1*(((((-1.0)*x20*x40))+x36))))+((x46*x8)));
00370 eerot[7]=(((x13*x55))+((x12*(((((-1.0)*x48))+x37)))));
00371 eerot[8]=(((x12*x55))+((x13*(((((-1.0)*x37))+x48)))));
00372 eetrans[2]=((0.40858)+(((-0.3215)*x29*x9))+(((-0.3215)*x28))+((x1*(((((0.1385)*x27*x9))+(((-1.0)*x18*x40))))))+(((-0.352)*x6))+((x8*(((((-1.0)*x32*x9))+(((-0.1385)*x28)))))));
00373 }
00374 
00375 IKFAST_API int GetNumFreeParameters() { return 1; }
00376 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {0}; return freeparams; }
00377 IKFAST_API int GetNumJoints() { return 7; }
00378 
00379 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00380 
00381 IKFAST_API int GetIkType() { return 0x67000001; }
00382 
00383 class IKSolver {
00384 public:
00385 IkReal j7,cj7,sj7,htj7,j7mul,j8,cj8,sj8,htj8,j8mul,j9,cj9,sj9,htj9,j9mul,j10,cj10,sj10,htj10,j10mul,j11,cj11,sj11,htj11,j11mul,j12,cj12,sj12,htj12,j12mul,j6,cj6,sj6,htj6,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;
00386 unsigned char _ij7[2], _nj7,_ij8[2], _nj8,_ij9[2], _nj9,_ij10[2], _nj10,_ij11[2], _nj11,_ij12[2], _nj12,_ij6[2], _nj6;
00387 
00388 IkReal j100, cj100, sj100;
00389 unsigned char _ij100[2], _nj100;
00390 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00391 j7=numeric_limits<IkReal>::quiet_NaN(); _ij7[0] = -1; _ij7[1] = -1; _nj7 = -1; j8=numeric_limits<IkReal>::quiet_NaN(); _ij8[0] = -1; _ij8[1] = -1; _nj8 = -1; j9=numeric_limits<IkReal>::quiet_NaN(); _ij9[0] = -1; _ij9[1] = -1; _nj9 = -1; j10=numeric_limits<IkReal>::quiet_NaN(); _ij10[0] = -1; _ij10[1] = -1; _nj10 = -1; j11=numeric_limits<IkReal>::quiet_NaN(); _ij11[0] = -1; _ij11[1] = -1; _nj11 = -1; j12=numeric_limits<IkReal>::quiet_NaN(); _ij12[0] = -1; _ij12[1] = -1; _nj12 = -1;  _ij6[0] = -1; _ij6[1] = -1; _nj6 = 0; 
00392 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00393     solutions.Clear();
00394 j6=pfree[0]; cj6=cos(pfree[0]); sj6=sin(pfree[0]), htj6=tan(pfree[0]*0.5);
00395 r00 = eerot[0*3+0];
00396 r01 = eerot[0*3+1];
00397 r02 = eerot[0*3+2];
00398 r10 = eerot[1*3+0];
00399 r11 = eerot[1*3+1];
00400 r12 = eerot[1*3+2];
00401 r20 = eerot[2*3+0];
00402 r21 = eerot[2*3+1];
00403 r22 = eerot[2*3+2];
00404 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00405 
00406 new_r00=((((-1.0)*r12*(IKsin(j6))))+(((-1.0)*r02*(IKcos(j6)))));
00407 new_r01=(((r11*(IKsin(j6))))+((r01*(IKcos(j6)))));
00408 new_r02=(((r00*(IKcos(j6))))+((r10*(IKsin(j6)))));
00409 IkReal x60=IKcos(j6);
00410 IkReal x61=IKsin(j6);
00411 new_px=((-0.117)+(((-0.1385)*r10*x61))+(((-0.1385)*r00*x60))+((px*x60))+(((-0.119525)*x60))+((py*x61)));
00412 new_r10=r22;
00413 new_r11=((-1.0)*r21);
00414 new_r12=((-1.0)*r20);
00415 new_py=((0.40858)+(((0.1385)*r20))+(((-1.0)*pz)));
00416 new_r20=((((-1.0)*r12*(IKcos(j6))))+((r02*(IKsin(j6)))));
00417 new_r21=(((r11*(IKcos(j6))))+(((-1.0)*r01*(IKsin(j6)))));
00418 new_r22=(((r10*(IKcos(j6))))+(((-1.0)*r00*(IKsin(j6)))));
00419 IkReal x62=IKsin(j6);
00420 IkReal x63=IKcos(j6);
00421 new_pz=((((-0.1385)*r10*x63))+(((0.119525)*x62))+(((0.1385)*r00*x62))+(((-1.0)*px*x62))+((py*x63)));
00422 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;
00423 IkReal x64=((1.0)*px);
00424 IkReal x65=((1.0)*pz);
00425 IkReal x66=((1.0)*py);
00426 pp=((px*px)+(py*py)+(pz*pz));
00427 npx=(((px*r00))+((py*r10))+((pz*r20)));
00428 npy=(((px*r01))+((py*r11))+((pz*r21)));
00429 npz=(((px*r02))+((py*r12))+((pz*r22)));
00430 rxp0_0=((((-1.0)*r20*x66))+((pz*r10)));
00431 rxp0_1=(((px*r20))+(((-1.0)*r00*x65)));
00432 rxp0_2=((((-1.0)*r10*x64))+((py*r00)));
00433 rxp1_0=((((-1.0)*r21*x66))+((pz*r11)));
00434 rxp1_1=(((px*r21))+(((-1.0)*r01*x65)));
00435 rxp1_2=((((-1.0)*r11*x64))+((py*r01)));
00436 rxp2_0=(((pz*r12))+(((-1.0)*r22*x66)));
00437 rxp2_1=(((px*r22))+(((-1.0)*r02*x65)));
00438 rxp2_2=((((-1.0)*r12*x64))+((py*r02)));
00439 {
00440 IkReal j7eval[2];
00441 j7eval[0]=((px*px)+(py*py));
00442 j7eval[1]=(pp+(((-1.0)*(pz*pz))));
00443 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  )
00444 {
00445 {
00446 IkReal evalcond[1];
00447 bool bgotonextstatement = true;
00448 do
00449 {
00450 evalcond[0]=((px*px)+(py*py));
00451 if( IKabs(evalcond[0]) < 0.0000050000000000  )
00452 {
00453 bgotonextstatement=false;
00454 {
00455 IkReal j12eval[1];
00456 IkReal x67=((1.0)*pz);
00457 IkReal x68=((0.3215)*r22);
00458 IkReal x69=(pz*r22);
00459 IkReal x70=((0.643)*x69);
00460 IkReal x71=((0.0410835)+(((-1.0)*x70)));
00461 IkReal x72=(x68+(((-1.0)*x67)));
00462 IkReal x73=((-1.0)*pz);
00463 IkReal x74=((0.0410835)+x70);
00464 IkReal x75=((((-1.0)*x68))+(((-1.0)*x67)));
00465 px=0;
00466 py=0;
00467 pp=pz*pz;
00468 npx=(pz*r20);
00469 npy=(pz*r21);
00470 npz=x69;
00471 rxp0_0=(pz*r10);
00472 rxp0_1=(r00*x73);
00473 rxp0_2=0;
00474 rxp1_0=(pz*r11);
00475 rxp1_1=(r01*x73);
00476 rxp1_2=0;
00477 rxp2_0=(pz*r12);
00478 rxp2_1=(r02*x73);
00479 rxp2_2=0;
00480 IkReal gconst0=x74;
00481 IkReal gconst1=x71;
00482 IkReal gconst2=x72;
00483 IkReal gconst3=x75;
00484 IkReal gconst4=x74;
00485 IkReal gconst5=x71;
00486 IkReal gconst6=x72;
00487 IkReal gconst7=x75;
00488 IkReal gconst8=x74;
00489 IkReal gconst9=x71;
00490 IkReal gconst10=x72;
00491 IkReal gconst11=x75;
00492 IkReal gconst12=x74;
00493 IkReal gconst13=x71;
00494 IkReal gconst14=x72;
00495 IkReal gconst15=x75;
00496 IkReal x76=r21*r21;
00497 IkReal x77=pz*pz;
00498 IkReal x78=pz*pz*pz*pz;
00499 IkReal x79=r22*r22;
00500 IkReal x80=r20*r20;
00501 IkReal x81=(r20*r21);
00502 IkReal x82=((0.271774911864)*x77);
00503 IkReal x83=((6.615184)*x78);
00504 IkReal x84=(x77*x80);
00505 IkReal x85=(x78*x79);
00506 IkReal x86=(x78*x80);
00507 IkReal x87=(x77*x79);
00508 IkReal x88=(((x81*x83))+((x81*x82))+(((0.00279136614789116)*x81)));
00509 j12eval[0]=((((0.5)*(IKabs(((((3.307592)*x86))+(((-9.922776)*x85))+(((0.00139568307394558)*x80))+(((0.135887455932)*x84))+(((-0.543549823728)*x76*x77))+(((-0.407662367796)*x87))+(((-0.00558273229578232)*x76))+(((-0.00418704922183674)*x79))+(((-13.230368)*x76*x78)))))))+(((0.5)*(IKabs(((((-0.067943727966)*x84))+(((-0.067943727966)*x87))+(((-0.00069784153697279)*x80))+(((-0.00069784153697279)*x79))+(((-1.653796)*x85))+(((-1.653796)*x86)))))))+(IKabs(((((-1.0)*x79*x82))+(((-1.0)*x79*x83))+(((-1.0)*x76*x82))+(((-1.0)*x76*x83))+(((-0.00279136614789116)*x76))+(((-0.00279136614789116)*x79)))))+(((2.0)*(IKabs(x88)))));
00510 if( IKabs(j12eval[0]) < 0.0000000010000000  )
00511 {
00512 continue; // no branches [j7, j11, j12]
00513 
00514 } else
00515 {
00516 IkReal op[8+1], zeror[8];
00517 int numroots;
00518 IkReal x89=r20*r20;
00519 IkReal x90=pz*pz;
00520 IkReal x91=r21*r21;
00521 IkReal x92=(r20*r21);
00522 IkReal x93=(gconst0*gconst11);
00523 IkReal x94=(gconst5*gconst6);
00524 IkReal x95=((1.0)*gconst12);
00525 IkReal x96=(gconst13*gconst14);
00526 IkReal x97=(gconst14*gconst5);
00527 IkReal x98=((1.0)*gconst2);
00528 IkReal x99=((0.413449)*gconst12);
00529 IkReal x100=(gconst0*gconst3);
00530 IkReal x101=(gconst11*gconst8);
00531 IkReal x102=(gconst13*gconst6);
00532 IkReal x103=((0.826898)*gconst12);
00533 IkReal x104=((0.413449)*gconst4);
00534 IkReal x105=(gconst4*pz);
00535 IkReal x106=(gconst4*gconst9);
00536 IkReal x107=(gconst1*gconst12);
00537 IkReal x108=((1.653796)*pz);
00538 IkReal x109=(gconst10*gconst7);
00539 IkReal x110=(gconst3*gconst8);
00540 IkReal x111=(gconst1*gconst4);
00541 IkReal x112=(gconst10*gconst15);
00542 IkReal x113=(gconst2*gconst7);
00543 IkReal x114=(gconst12*gconst9);
00544 IkReal x115=((0.826898)*gconst4);
00545 IkReal x116=(gconst15*gconst2);
00546 IkReal x117=(gconst1*x89);
00547 IkReal x118=(gconst6*x108);
00548 IkReal x119=((1.0)*gconst4*gconst7);
00549 IkReal x120=(gconst9*x89);
00550 IkReal x121=((1.0)*gconst1*gconst10);
00551 IkReal x122=(gconst11*x89);
00552 IkReal x123=(gconst14*x90);
00553 IkReal x124=((1.653796)*x91);
00554 IkReal x125=(gconst3*x89);
00555 IkReal x126=((0.826898)*gconst6*pz);
00556 IkReal x127=(gconst6*x90);
00557 IkReal x128=((1.0)*gconst10*gconst9);
00558 IkReal x129=((1.0)*gconst15*gconst4);
00559 IkReal x130=(gconst3*x91);
00560 IkReal x131=(gconst11*x91);
00561 IkReal x132=((0.826898)*gconst14*pz);
00562 IkReal x133=((3.307592)*pz*x91);
00563 IkReal x134=(x120*x99);
00564 IkReal x135=(x117*x99);
00565 IkReal x136=(x104*x120);
00566 IkReal x137=(x104*x117);
00567 IkReal x138=(pz*x103*x122);
00568 IkReal x139=(x120*x132);
00569 IkReal x140=(x120*x126);
00570 IkReal x141=(pz*x103*x125);
00571 IkReal x142=(x117*x132);
00572 IkReal x143=((0.826898)*x105*x122);
00573 IkReal x144=((0.826898)*x105*x125);
00574 IkReal x145=(x117*x126);
00575 IkReal x146=((1.653796)*x122*x123);
00576 IkReal x147=((1.653796)*x122*x127);
00577 IkReal x148=((1.653796)*x123*x125);
00578 IkReal x149=((1.653796)*x125*x127);
00579 IkReal x150=(x144+x145);
00580 IkReal x151=(x139+x138);
00581 IkReal x152=(x146+x134);
00582 IkReal x153=(x149+x137);
00583 IkReal x154=((((-6.615184)*x92*(x90*x90)))+(((-0.271774911864)*x90*x92))+(((-0.00279136614789116)*x92)));
00584 IkReal x155=((((0.271774911864)*x90*x92))+(((6.615184)*x92*(x90*x90)))+(((0.00279136614789116)*x92)));
00585 IkReal x156=(x147+x148+x135+x136);
00586 IkReal x157=(x140+x141+x142+x143);
00587 op[0]=((((-1.0)*x152))+((x112*x114))+(((-1.0)*gconst15*x101*x95))+((x101*x96))+x151+(((-1.0)*x128*x96)));
00588 op[1]=x154;
00589 op[2]=((((-1.0)*x101*x129))+(((-1.0)*x156))+((gconst14*gconst9*x133))+((x106*x112))+((x114*x116))+((x101*x97))+((x103*x120))+(((-1.0)*gconst7*x101*x95))+((x109*x114))+x157+(((-1.0)*gconst15*x110*x95))+(((3.307592)*x122*x123))+(((-1.0)*gconst14*x108*x120))+((x93*x96))+(((-1.0)*gconst12*x108*x122))+(((-1.0)*x114*x124))+(((-1.0)*x121*x96))+(((3.307592)*gconst12*pz*x131))+(((-6.615184)*x123*x131))+(((-1.0)*x102*x128))+((x101*x102))+(((-1.0)*x128*x97))+(((-1.0)*gconst15*x93*x95))+(((-1.0)*gconst9*x96*x98))+((x110*x96))+((x107*x112)));
00590 op[3]=x154;
00591 op[4]=((((-1.0)*x118*x120))+((x107*x109))+(((-1.0)*x152))+(((-1.0)*x153))+(((-1.0)*gconst7*x93*x95))+((x100*x96))+((x106*x109))+((x111*x112))+((gconst6*gconst9*x133))+(((-1.0)*x110*x129))+(((3.307592)*x123*x125))+((x106*x116))+(((-1.0)*x101*x119))+(((-1.0)*gconst15*x100*x95))+(((-1.0)*x106*x124))+((x102*x93))+((x101*x94))+(((-1.0)*gconst1*x96*x98))+((x113*x114))+(((-1.0)*gconst9*x102*x98))+(((3.307592)*x105*x131))+((x102*x110))+((x103*x117))+x151+x150+(((-1.0)*x107*x124))+(((-1.653796)*x105*x122))+(((3.307592)*x122*x127))+((x93*x97))+(((-1.0)*gconst9*x97*x98))+(((0.826898)*x106*x89))+(((-1.0)*gconst7*x110*x95))+(((-1.0)*gconst12*x108*x125))+(((-1.0)*x121*x97))+(((-6.615184)*x127*x131))+(((3.307592)*gconst12*pz*x130))+(((-6.615184)*x123*x130))+((gconst1*gconst14*x133))+(((-1.0)*x102*x121))+(((-1.0)*gconst14*x108*x117))+(((-1.0)*x129*x93))+(((-1.0)*x128*x94))+((x110*x97))+((x107*x116)));
00592 op[5]=x155;
00593 op[6]=((((-1.0)*gconst1*x97*x98))+(((-1.0)*x100*x129))+(((-1.0)*x156))+((x100*x97))+(((3.307592)*x125*x127))+((x111*x116))+(((-1.0)*gconst9*x94*x98))+(((-1.0)*x117*x118))+((x106*x113))+(((-1.0)*x111*x124))+(((3.307592)*x105*x130))+(((-1.0)*x119*x93))+(((0.826898)*x111*x89))+((x109*x111))+(((-1.0)*x110*x119))+x157+((gconst1*gconst6*x133))+(((-1.653796)*x105*x125))+((x93*x94))+(((-1.0)*gconst7*x100*x95))+(((-1.0)*x121*x94))+(((-6.615184)*x127*x130))+(((-1.0)*gconst1*x102*x98))+((x110*x94))+((x107*x113))+((x100*x102)));
00594 op[7]=x155;
00595 op[8]=((((-1.0)*x153))+((x100*x94))+((x111*x113))+(((-1.0)*x100*x119))+(((-1.0)*gconst1*x94*x98))+x150);
00596 polyroots8(op,zeror,numroots);
00597 IkReal j12array[8], cj12array[8], sj12array[8], tempj12array[1];
00598 int numsolutions = 0;
00599 for(int ij12 = 0; ij12 < numroots; ++ij12)
00600 {
00601 IkReal htj12 = zeror[ij12];
00602 tempj12array[0]=((2.0)*(atan(htj12)));
00603 for(int kj12 = 0; kj12 < 1; ++kj12)
00604 {
00605 j12array[numsolutions] = tempj12array[kj12];
00606 if( j12array[numsolutions] > IKPI )
00607 {
00608     j12array[numsolutions]-=IK2PI;
00609 }
00610 else if( j12array[numsolutions] < -IKPI )
00611 {
00612     j12array[numsolutions]+=IK2PI;
00613 }
00614 sj12array[numsolutions] = IKsin(j12array[numsolutions]);
00615 cj12array[numsolutions] = IKcos(j12array[numsolutions]);
00616 numsolutions++;
00617 }
00618 }
00619 bool j12valid[8]={true,true,true,true,true,true,true,true};
00620 _nj12 = 8;
00621 for(int ij12 = 0; ij12 < numsolutions; ++ij12)
00622     {
00623 if( !j12valid[ij12] )
00624 {
00625     continue;
00626 }
00627     j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
00628 htj12 = IKtan(j12/2);
00629 
00630 _ij12[0] = ij12; _ij12[1] = -1;
00631 for(int iij12 = ij12+1; iij12 < numsolutions; ++iij12)
00632 {
00633 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
00634 {
00635     j12valid[iij12]=false; _ij12[1] = iij12; break; 
00636 }
00637 }
00638 {
00639 IkReal j11eval[2];
00640 IkReal x158=((-1.0)*pz);
00641 px=0;
00642 py=0;
00643 pp=pz*pz;
00644 npx=(pz*r20);
00645 npy=(pz*r21);
00646 npz=(pz*r22);
00647 rxp0_0=(pz*r10);
00648 rxp0_1=(r00*x158);
00649 rxp0_2=0;
00650 rxp1_0=(pz*r11);
00651 rxp1_1=(r01*x158);
00652 rxp1_2=0;
00653 rxp2_0=(pz*r12);
00654 rxp2_1=(r02*x158);
00655 rxp2_2=0;
00656 IkReal x159=(cj12*r20);
00657 IkReal x160=(r21*sj12);
00658 j11eval[0]=((r22*r22)+(x160*x160)+(x159*x159)+(((-2.0)*x159*x160)));
00659 j11eval[1]=((IKabs(r22))+(((3.1104199066874)*(IKabs(((((-0.3215)*x159))+(((0.3215)*x160))))))));
00660 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  )
00661 {
00662 {
00663 IkReal j11eval[2];
00664 IkReal x161=((-1.0)*pz);
00665 px=0;
00666 py=0;
00667 pp=pz*pz;
00668 npx=(pz*r20);
00669 npy=(pz*r21);
00670 npz=(pz*r22);
00671 rxp0_0=(pz*r10);
00672 rxp0_1=(r00*x161);
00673 rxp0_2=0;
00674 rxp1_0=(pz*r11);
00675 rxp1_1=(r01*x161);
00676 rxp1_2=0;
00677 rxp2_0=(pz*r12);
00678 rxp2_1=(r02*x161);
00679 rxp2_2=0;
00680 IkReal x162=pz*pz;
00681 IkReal x163=(r21*sj12);
00682 IkReal x164=(cj12*r20);
00683 j11eval[0]=((IKabs((pz*r22)))+(IKabs((((pz*x163))+(((-1.0)*pz*x164))))));
00684 j11eval[1]=(((x162*(x163*x163)))+(((-2.0)*x162*x163*x164))+((x162*(r22*r22)))+((x162*(x164*x164))));
00685 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  )
00686 {
00687 {
00688 IkReal j7eval[2];
00689 IkReal x165=((-1.0)*pz);
00690 px=0;
00691 py=0;
00692 pp=pz*pz;
00693 npx=(pz*r20);
00694 npy=(pz*r21);
00695 npz=(pz*r22);
00696 rxp0_0=(pz*r10);
00697 rxp0_1=(r00*x165);
00698 rxp0_2=0;
00699 rxp1_0=(pz*r11);
00700 rxp1_1=(r01*x165);
00701 rxp1_2=0;
00702 rxp2_0=(pz*r12);
00703 rxp2_1=(r02*x165);
00704 rxp2_2=0;
00705 IkReal x166=sj12*sj12;
00706 IkReal x167=cj12*cj12;
00707 IkReal x168=(r10*sj12);
00708 IkReal x169=(cj12*r11);
00709 IkReal x170=(cj12*r01);
00710 IkReal x171=(r00*sj12);
00711 j7eval[0]=((((2.0)*x170*x171))+((x166*(r00*r00)))+((x166*(r10*r10)))+((x167*(r11*r11)))+(((2.0)*x168*x169))+((x167*(r01*r01))));
00712 j7eval[1]=((IKabs(((((0.352)*x169))+(((0.352)*x168)))))+(IKabs(((((0.352)*x170))+(((0.352)*x171))))));
00713 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  )
00714 {
00715 {
00716 IkReal evalcond[1];
00717 bool bgotonextstatement = true;
00718 do
00719 {
00720 evalcond[0]=IKabs(pz);
00721 if( IKabs(evalcond[0]) < 0.0000050000000000  )
00722 {
00723 bgotonextstatement=false;
00724 {
00725 IkReal j11eval[1];
00726 px=0;
00727 py=0;
00728 pp=0;
00729 npx=0;
00730 npy=0;
00731 npz=0;
00732 rxp0_0=0;
00733 rxp0_1=0;
00734 rxp0_2=0;
00735 rxp1_0=0;
00736 rxp1_1=0;
00737 rxp1_2=0;
00738 rxp2_0=0;
00739 rxp2_1=0;
00740 rxp2_2=0;
00741 pz=0;
00742 j11eval[0]=((((3.1104199066874)*(IKabs(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)))))))+(IKabs(r22)));
00743 if( IKabs(j11eval[0]) < 0.0000010000000000  )
00744 {
00745 {
00746 IkReal j7eval[1];
00747 px=0;
00748 py=0;
00749 pp=0;
00750 npx=0;
00751 npy=0;
00752 npz=0;
00753 rxp0_0=0;
00754 rxp0_1=0;
00755 rxp0_2=0;
00756 rxp1_0=0;
00757 rxp1_1=0;
00758 rxp1_2=0;
00759 rxp2_0=0;
00760 rxp2_1=0;
00761 rxp2_2=0;
00762 pz=0;
00763 IkReal x172=((0.352)*sj12);
00764 IkReal x173=((0.352)*cj12);
00765 j7eval[0]=((IKabs((((r00*x172))+((r01*x173)))))+(IKabs((((r10*x172))+((r11*x173))))));
00766 if( IKabs(j7eval[0]) < 0.0000010000000000  )
00767 {
00768 continue; // 3 cases reached
00769 
00770 } else
00771 {
00772 {
00773 IkReal j7array[2], cj7array[2], sj7array[2];
00774 bool j7valid[2]={false};
00775 _nj7 = 2;
00776 IkReal x174=((0.352)*cj12);
00777 IkReal x175=((0.352)*sj12);
00778 CheckValue<IkReal> x177 = IKatan2WithCheck(IkReal((((r00*x175))+((r01*x174)))),IkReal((((r10*x175))+((r11*x174)))),IKFAST_ATAN2_MAGTHRESH);
00779 if(!x177.valid){
00780 continue;
00781 }
00782 IkReal x176=x177.value;
00783 j7array[0]=((-1.0)*x176);
00784 sj7array[0]=IKsin(j7array[0]);
00785 cj7array[0]=IKcos(j7array[0]);
00786 j7array[1]=((3.14159265358979)+(((-1.0)*x176)));
00787 sj7array[1]=IKsin(j7array[1]);
00788 cj7array[1]=IKcos(j7array[1]);
00789 if( j7array[0] > IKPI )
00790 {
00791     j7array[0]-=IK2PI;
00792 }
00793 else if( j7array[0] < -IKPI )
00794 {    j7array[0]+=IK2PI;
00795 }
00796 j7valid[0] = true;
00797 if( j7array[1] > IKPI )
00798 {
00799     j7array[1]-=IK2PI;
00800 }
00801 else if( j7array[1] < -IKPI )
00802 {    j7array[1]+=IK2PI;
00803 }
00804 j7valid[1] = true;
00805 for(int ij7 = 0; ij7 < 2; ++ij7)
00806 {
00807 if( !j7valid[ij7] )
00808 {
00809     continue;
00810 }
00811 _ij7[0] = ij7; _ij7[1] = -1;
00812 for(int iij7 = ij7+1; iij7 < 2; ++iij7)
00813 {
00814 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
00815 {
00816     j7valid[iij7]=false; _ij7[1] = iij7; break; 
00817 }
00818 }
00819 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
00820 
00821 {
00822 IkReal j11eval[1];
00823 px=0;
00824 py=0;
00825 pp=0;
00826 npx=0;
00827 npy=0;
00828 npz=0;
00829 rxp0_0=0;
00830 rxp0_1=0;
00831 rxp0_2=0;
00832 rxp1_0=0;
00833 rxp1_1=0;
00834 rxp1_2=0;
00835 rxp2_0=0;
00836 rxp2_1=0;
00837 rxp2_2=0;
00838 pz=0;
00839 j11eval[0]=((((-1.0)*cj12*r20))+((r21*sj12)));
00840 if( IKabs(j11eval[0]) < 0.0000010000000000  )
00841 {
00842 {
00843 IkReal j11eval[1];
00844 px=0;
00845 py=0;
00846 pp=0;
00847 npx=0;
00848 npy=0;
00849 npz=0;
00850 rxp0_0=0;
00851 rxp0_1=0;
00852 rxp0_2=0;
00853 rxp1_0=0;
00854 rxp1_1=0;
00855 rxp1_2=0;
00856 rxp2_0=0;
00857 rxp2_1=0;
00858 rxp2_2=0;
00859 pz=0;
00860 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
00861 if( IKabs(j11eval[0]) < 0.0000010000000000  )
00862 {
00863 {
00864 IkReal j11eval[1];
00865 px=0;
00866 py=0;
00867 pp=0;
00868 npx=0;
00869 npy=0;
00870 npz=0;
00871 rxp0_0=0;
00872 rxp0_1=0;
00873 rxp0_2=0;
00874 rxp1_0=0;
00875 rxp1_1=0;
00876 rxp1_2=0;
00877 rxp2_0=0;
00878 rxp2_1=0;
00879 rxp2_2=0;
00880 pz=0;
00881 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
00882 if( IKabs(j11eval[0]) < 0.0000010000000000  )
00883 {
00884 continue; // 3 cases reached
00885 
00886 } else
00887 {
00888 {
00889 IkReal j11array[1], cj11array[1], sj11array[1];
00890 bool j11valid[1]={false};
00891 _nj11 = 1;
00892 IkReal x178=((44.0)*sj7);
00893 IkReal x179=(cj7*r02);
00894 CheckValue<IkReal> x180=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
00895 if(!x180.valid){
00896 continue;
00897 }
00898 if( IKabs(((x180.value)*(((((-1.0)*x178))+(((44.0)*r12*x179))+((x178*(r12*r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x179))+(((-1.09486780715397)*r12*sj7)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x180.value)*(((((-1.0)*x178))+(((44.0)*r12*x179))+((x178*(r12*r12)))))))+IKsqr(((((-1.09486780715397)*x179))+(((-1.09486780715397)*r12*sj7))))-1) <= IKFAST_SINCOS_THRESH )
00899     continue;
00900 j11array[0]=IKatan2(((x180.value)*(((((-1.0)*x178))+(((44.0)*r12*x179))+((x178*(r12*r12)))))), ((((-1.09486780715397)*x179))+(((-1.09486780715397)*r12*sj7))));
00901 sj11array[0]=IKsin(j11array[0]);
00902 cj11array[0]=IKcos(j11array[0]);
00903 if( j11array[0] > IKPI )
00904 {
00905     j11array[0]-=IK2PI;
00906 }
00907 else if( j11array[0] < -IKPI )
00908 {    j11array[0]+=IK2PI;
00909 }
00910 j11valid[0] = true;
00911 for(int ij11 = 0; ij11 < 1; ++ij11)
00912 {
00913 if( !j11valid[ij11] )
00914 {
00915     continue;
00916 }
00917 _ij11[0] = ij11; _ij11[1] = -1;
00918 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
00919 {
00920 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
00921 {
00922     j11valid[iij11]=false; _ij11[1] = iij11; break; 
00923 }
00924 }
00925 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
00926 {
00927 IkReal evalcond[5];
00928 IkReal x181=IKcos(j11);
00929 IkReal x182=IKsin(j11);
00930 IkReal x183=(r01*sj12);
00931 IkReal x184=((0.352)*cj7);
00932 IkReal x185=(cj12*r10);
00933 IkReal x186=(r11*sj12);
00934 IkReal x187=(cj12*r00);
00935 IkReal x188=((0.352)*sj7);
00936 IkReal x189=((0.3215)*x182);
00937 IkReal x190=((0.3215)*x181);
00938 evalcond[0]=(x190+((r12*x188))+((r02*x184)));
00939 evalcond[1]=(((r21*sj12*x189))+(((-1.0)*cj12*r20*x189))+((r22*x190)));
00940 evalcond[2]=((((-1.0)*x187*x189))+x184+((r02*x190))+((x183*x189)));
00941 evalcond[3]=(((x186*x189))+x188+(((-1.0)*x185*x189))+((r12*x190)));
00942 evalcond[4]=((((-1.0)*x186*x188))+(((-1.0)*x189))+((x185*x188))+((x184*x187))+(((-1.0)*x183*x184)));
00943 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
00944 {
00945 continue;
00946 }
00947 }
00948 
00949 rotationfunction0(solutions);
00950 }
00951 }
00952 
00953 }
00954 
00955 }
00956 
00957 } else
00958 {
00959 {
00960 IkReal j11array[1], cj11array[1], sj11array[1];
00961 bool j11valid[1]={false};
00962 _nj11 = 1;
00963 IkReal x732=(r12*sj7);
00964 IkReal x733=((44.0)*cj7);
00965 CheckValue<IkReal> x734=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
00966 if(!x734.valid){
00967 continue;
00968 }
00969 if( IKabs(((x734.value)*(((((-1.0)*x733))+(((44.0)*r02*x732))+((x733*(r02*r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x732)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x734.value)*(((((-1.0)*x733))+(((44.0)*r02*x732))+((x733*(r02*r02)))))))+IKsqr(((((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x732))))-1) <= IKFAST_SINCOS_THRESH )
00970     continue;
00971 j11array[0]=IKatan2(((x734.value)*(((((-1.0)*x733))+(((44.0)*r02*x732))+((x733*(r02*r02)))))), ((((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x732))));
00972 sj11array[0]=IKsin(j11array[0]);
00973 cj11array[0]=IKcos(j11array[0]);
00974 if( j11array[0] > IKPI )
00975 {
00976     j11array[0]-=IK2PI;
00977 }
00978 else if( j11array[0] < -IKPI )
00979 {    j11array[0]+=IK2PI;
00980 }
00981 j11valid[0] = true;
00982 for(int ij11 = 0; ij11 < 1; ++ij11)
00983 {
00984 if( !j11valid[ij11] )
00985 {
00986     continue;
00987 }
00988 _ij11[0] = ij11; _ij11[1] = -1;
00989 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
00990 {
00991 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
00992 {
00993     j11valid[iij11]=false; _ij11[1] = iij11; break; 
00994 }
00995 }
00996 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
00997 {
00998 IkReal evalcond[5];
00999 IkReal x735=IKcos(j11);
01000 IkReal x736=IKsin(j11);
01001 IkReal x737=(r01*sj12);
01002 IkReal x738=((0.352)*cj7);
01003 IkReal x739=(cj12*r10);
01004 IkReal x740=(r11*sj12);
01005 IkReal x741=(cj12*r00);
01006 IkReal x742=((0.352)*sj7);
01007 IkReal x743=((0.3215)*x736);
01008 IkReal x744=((0.3215)*x735);
01009 evalcond[0]=(((r12*x742))+((r02*x738))+x744);
01010 evalcond[1]=(((r22*x744))+(((-1.0)*cj12*r20*x743))+((r21*sj12*x743)));
01011 evalcond[2]=(((r02*x744))+((x737*x743))+(((-1.0)*x741*x743))+x738);
01012 evalcond[3]=(((x740*x743))+((r12*x744))+x742+(((-1.0)*x739*x743)));
01013 evalcond[4]=(((x738*x741))+((x739*x742))+(((-1.0)*x737*x738))+(((-1.0)*x740*x742))+(((-1.0)*x743)));
01014 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
01015 {
01016 continue;
01017 }
01018 }
01019 
01020 rotationfunction0(solutions);
01021 }
01022 }
01023 
01024 }
01025 
01026 }
01027 
01028 } else
01029 {
01030 {
01031 IkReal j11array[1], cj11array[1], sj11array[1];
01032 bool j11valid[1]={false};
01033 _nj11 = 1;
01034 IkReal x745=((0.352)*r22);
01035 IkReal x746=(r12*sj7);
01036 IkReal x747=(cj7*r02);
01037 CheckValue<IkReal> x748=IKPowWithIntegerCheck(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12))),-1);
01038 if(!x748.valid){
01039 continue;
01040 }
01041 if( IKabs(((x748.value)*((((x745*x747))+((x745*x746)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x747))+(((-1.09486780715397)*x746)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x748.value)*((((x745*x747))+((x745*x746))))))+IKsqr(((((-1.09486780715397)*x747))+(((-1.09486780715397)*x746))))-1) <= IKFAST_SINCOS_THRESH )
01042     continue;
01043 j11array[0]=IKatan2(((x748.value)*((((x745*x747))+((x745*x746))))), ((((-1.09486780715397)*x747))+(((-1.09486780715397)*x746))));
01044 sj11array[0]=IKsin(j11array[0]);
01045 cj11array[0]=IKcos(j11array[0]);
01046 if( j11array[0] > IKPI )
01047 {
01048     j11array[0]-=IK2PI;
01049 }
01050 else if( j11array[0] < -IKPI )
01051 {    j11array[0]+=IK2PI;
01052 }
01053 j11valid[0] = true;
01054 for(int ij11 = 0; ij11 < 1; ++ij11)
01055 {
01056 if( !j11valid[ij11] )
01057 {
01058     continue;
01059 }
01060 _ij11[0] = ij11; _ij11[1] = -1;
01061 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
01062 {
01063 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01064 {
01065     j11valid[iij11]=false; _ij11[1] = iij11; break; 
01066 }
01067 }
01068 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01069 {
01070 IkReal evalcond[5];
01071 IkReal x749=IKcos(j11);
01072 IkReal x750=IKsin(j11);
01073 IkReal x751=(r01*sj12);
01074 IkReal x752=((0.352)*cj7);
01075 IkReal x753=(cj12*r10);
01076 IkReal x754=(r11*sj12);
01077 IkReal x755=(cj12*r00);
01078 IkReal x756=((0.352)*sj7);
01079 IkReal x757=((0.3215)*x750);
01080 IkReal x758=((0.3215)*x749);
01081 evalcond[0]=(((r02*x752))+((r12*x756))+x758);
01082 evalcond[1]=(((r22*x758))+((r21*sj12*x757))+(((-1.0)*cj12*r20*x757)));
01083 evalcond[2]=(((r02*x758))+((x751*x757))+(((-1.0)*x755*x757))+x752);
01084 evalcond[3]=((((-1.0)*x753*x757))+((r12*x758))+x756+((x754*x757)));
01085 evalcond[4]=(((x752*x755))+(((-1.0)*x754*x756))+(((-1.0)*x751*x752))+((x753*x756))+(((-1.0)*x757)));
01086 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
01087 {
01088 continue;
01089 }
01090 }
01091 
01092 rotationfunction0(solutions);
01093 }
01094 }
01095 
01096 }
01097 
01098 }
01099 }
01100 }
01101 
01102 }
01103 
01104 }
01105 
01106 } else
01107 {
01108 {
01109 IkReal j11array[2], cj11array[2], sj11array[2];
01110 bool j11valid[2]={false};
01111 _nj11 = 2;
01112 CheckValue<IkReal> x760 = IKatan2WithCheck(IkReal(((0.3215)*r22)),IkReal(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)))),IKFAST_ATAN2_MAGTHRESH);
01113 if(!x760.valid){
01114 continue;
01115 }
01116 IkReal x759=x760.value;
01117 j11array[0]=((-1.0)*x759);
01118 sj11array[0]=IKsin(j11array[0]);
01119 cj11array[0]=IKcos(j11array[0]);
01120 j11array[1]=((3.14159265358979)+(((-1.0)*x759)));
01121 sj11array[1]=IKsin(j11array[1]);
01122 cj11array[1]=IKcos(j11array[1]);
01123 if( j11array[0] > IKPI )
01124 {
01125     j11array[0]-=IK2PI;
01126 }
01127 else if( j11array[0] < -IKPI )
01128 {    j11array[0]+=IK2PI;
01129 }
01130 j11valid[0] = true;
01131 if( j11array[1] > IKPI )
01132 {
01133     j11array[1]-=IK2PI;
01134 }
01135 else if( j11array[1] < -IKPI )
01136 {    j11array[1]+=IK2PI;
01137 }
01138 j11valid[1] = true;
01139 for(int ij11 = 0; ij11 < 2; ++ij11)
01140 {
01141 if( !j11valid[ij11] )
01142 {
01143     continue;
01144 }
01145 _ij11[0] = ij11; _ij11[1] = -1;
01146 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
01147 {
01148 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01149 {
01150     j11valid[iij11]=false; _ij11[1] = iij11; break; 
01151 }
01152 }
01153 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01154 
01155 {
01156 IkReal j7eval[3];
01157 px=0;
01158 py=0;
01159 pp=0;
01160 npx=0;
01161 npy=0;
01162 npz=0;
01163 rxp0_0=0;
01164 rxp0_1=0;
01165 rxp0_2=0;
01166 rxp1_0=0;
01167 rxp1_1=0;
01168 rxp1_2=0;
01169 rxp2_0=0;
01170 rxp2_1=0;
01171 rxp2_2=0;
01172 pz=0;
01173 IkReal x761=(r21*sj12);
01174 IkReal x762=((1.0)*cj12);
01175 IkReal x763=(cj11*sj12);
01176 j7eval[0]=((((-1.0)*r20*x762))+x761);
01177 j7eval[1]=((IKabs(((((-1.0)*cj11*r11*x762))+(((-1.0)*r10*x763)))))+(IKabs((((cj11*cj12*r01))+((r00*x763))))));
01178 j7eval[2]=IKsign(((((247.808)*x761))+(((-247.808)*cj12*r20))));
01179 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  || IKabs(j7eval[2]) < 0.0000010000000000  )
01180 {
01181 {
01182 IkReal j7eval[3];
01183 px=0;
01184 py=0;
01185 pp=0;
01186 npx=0;
01187 npy=0;
01188 npz=0;
01189 rxp0_0=0;
01190 rxp0_1=0;
01191 rxp0_2=0;
01192 rxp1_0=0;
01193 rxp1_1=0;
01194 rxp1_2=0;
01195 rxp2_0=0;
01196 rxp2_1=0;
01197 rxp2_2=0;
01198 pz=0;
01199 IkReal x764=((28292.0)*cj11);
01200 IkReal x765=((28292.0)*sj11);
01201 IkReal x766=(cj12*r21);
01202 IkReal x767=(r20*sj12);
01203 j7eval[0]=(x766+x767);
01204 j7eval[1]=((IKabs((((r02*x765))+(((-1.0)*r01*sj12*x764))+((cj12*r00*x764)))))+(IKabs(((((-1.0)*cj12*r10*x764))+((r11*sj12*x764))+(((-1.0)*r12*x765))))));
01205 j7eval[2]=IKsign(((((30976.0)*x767))+(((30976.0)*x766))));
01206 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  || IKabs(j7eval[2]) < 0.0000010000000000  )
01207 {
01208 {
01209 IkReal j7eval[1];
01210 px=0;
01211 py=0;
01212 pp=0;
01213 npx=0;
01214 npy=0;
01215 npz=0;
01216 rxp0_0=0;
01217 rxp0_1=0;
01218 rxp0_2=0;
01219 rxp1_0=0;
01220 rxp1_1=0;
01221 rxp1_2=0;
01222 rxp2_0=0;
01223 rxp2_1=0;
01224 rxp2_2=0;
01225 pz=0;
01226 j7eval[0]=r12;
01227 if( IKabs(j7eval[0]) < 0.0000010000000000  )
01228 {
01229 continue; // 3 cases reached
01230 
01231 } else
01232 {
01233 {
01234 IkReal j7array[1], cj7array[1], sj7array[1];
01235 bool j7valid[1]={false};
01236 _nj7 = 1;
01237 IkReal x768=((643.0)*cj11);
01238 IkReal x769=((643.0)*r02);
01239 IkReal x770=(cj12*r00*sj11);
01240 IkReal x771=(r01*sj11*sj12);
01241 CheckValue<IkReal> x772=IKPowWithIntegerCheck(r12,-1);
01242 if(!x772.valid){
01243 continue;
01244 }
01245 if( IKabs(((0.00142045454545455)*(x772.value)*(((((-1.0)*x768))+((x769*x771))+(((-1.0)*x769*x770))+((x768*(r02*r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x771))+(((0.913352272727273)*x770)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00142045454545455)*(x772.value)*(((((-1.0)*x768))+((x769*x771))+(((-1.0)*x769*x770))+((x768*(r02*r02)))))))+IKsqr(((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x771))+(((0.913352272727273)*x770))))-1) <= IKFAST_SINCOS_THRESH )
01246     continue;
01247 j7array[0]=IKatan2(((0.00142045454545455)*(x772.value)*(((((-1.0)*x768))+((x769*x771))+(((-1.0)*x769*x770))+((x768*(r02*r02)))))), ((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x771))+(((0.913352272727273)*x770))));
01248 sj7array[0]=IKsin(j7array[0]);
01249 cj7array[0]=IKcos(j7array[0]);
01250 if( j7array[0] > IKPI )
01251 {
01252     j7array[0]-=IK2PI;
01253 }
01254 else if( j7array[0] < -IKPI )
01255 {    j7array[0]+=IK2PI;
01256 }
01257 j7valid[0] = true;
01258 for(int ij7 = 0; ij7 < 1; ++ij7)
01259 {
01260 if( !j7valid[ij7] )
01261 {
01262     continue;
01263 }
01264 _ij7[0] = ij7; _ij7[1] = -1;
01265 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
01266 {
01267 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
01268 {
01269     j7valid[iij7]=false; _ij7[1] = iij7; break; 
01270 }
01271 }
01272 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
01273 {
01274 IkReal evalcond[5];
01275 IkReal x773=IKcos(j7);
01276 IkReal x774=IKsin(j7);
01277 IkReal x775=(r01*sj12);
01278 IkReal x776=((0.3215)*cj11);
01279 IkReal x777=(r11*sj12);
01280 IkReal x778=((0.3215)*sj11);
01281 IkReal x779=(cj12*r10);
01282 IkReal x780=(cj12*r00);
01283 IkReal x781=((0.352)*x774);
01284 IkReal x782=((0.352)*x773);
01285 evalcond[0]=(((r02*x782))+x776+((r12*x781)));
01286 evalcond[1]=(x782+(((-1.0)*x778*x780))+((r02*x776))+((x775*x778)));
01287 evalcond[2]=((((-1.0)*x778*x779))+((x777*x778))+((r12*x776))+x781);
01288 evalcond[3]=(((r10*sj12*x781))+((r00*sj12*x782))+((cj12*r01*x782))+((cj12*r11*x781)));
01289 evalcond[4]=((((-1.0)*x775*x782))+(((-1.0)*x777*x781))+((x779*x781))+((x780*x782))+(((-1.0)*x778)));
01290 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
01291 {
01292 continue;
01293 }
01294 }
01295 
01296 rotationfunction0(solutions);
01297 }
01298 }
01299 
01300 }
01301 
01302 }
01303 
01304 } else
01305 {
01306 {
01307 IkReal j7array[1], cj7array[1], sj7array[1];
01308 bool j7valid[1]={false};
01309 _nj7 = 1;
01310 IkReal x783=((28292.0)*cj11);
01311 IkReal x784=((28292.0)*sj11);
01312 CheckValue<IkReal> x785=IKPowWithIntegerCheck(IKsign(((((30976.0)*r20*sj12))+(((30976.0)*cj12*r21)))),-1);
01313 if(!x785.valid){
01314 continue;
01315 }
01316 CheckValue<IkReal> x786 = IKatan2WithCheck(IkReal((((r02*x784))+(((-1.0)*r01*sj12*x783))+((cj12*r00*x783)))),IkReal(((((-1.0)*cj12*r10*x783))+(((-1.0)*r12*x784))+((r11*sj12*x783)))),IKFAST_ATAN2_MAGTHRESH);
01317 if(!x786.valid){
01318 continue;
01319 }
01320 j7array[0]=((-1.5707963267949)+(((1.5707963267949)*(x785.value)))+(x786.value));
01321 sj7array[0]=IKsin(j7array[0]);
01322 cj7array[0]=IKcos(j7array[0]);
01323 if( j7array[0] > IKPI )
01324 {
01325     j7array[0]-=IK2PI;
01326 }
01327 else if( j7array[0] < -IKPI )
01328 {    j7array[0]+=IK2PI;
01329 }
01330 j7valid[0] = true;
01331 for(int ij7 = 0; ij7 < 1; ++ij7)
01332 {
01333 if( !j7valid[ij7] )
01334 {
01335     continue;
01336 }
01337 _ij7[0] = ij7; _ij7[1] = -1;
01338 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
01339 {
01340 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
01341 {
01342     j7valid[iij7]=false; _ij7[1] = iij7; break; 
01343 }
01344 }
01345 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
01346 {
01347 IkReal evalcond[5];
01348 IkReal x787=IKcos(j7);
01349 IkReal x788=IKsin(j7);
01350 IkReal x789=(r01*sj12);
01351 IkReal x790=((0.3215)*cj11);
01352 IkReal x791=(r11*sj12);
01353 IkReal x792=((0.3215)*sj11);
01354 IkReal x793=(cj12*r10);
01355 IkReal x794=(cj12*r00);
01356 IkReal x795=((0.352)*x788);
01357 IkReal x796=((0.352)*x787);
01358 evalcond[0]=(((r02*x796))+x790+((r12*x795)));
01359 evalcond[1]=((((-1.0)*x792*x794))+((r02*x790))+x796+((x789*x792)));
01360 evalcond[2]=((((-1.0)*x792*x793))+x795+((r12*x790))+((x791*x792)));
01361 evalcond[3]=(((r10*sj12*x795))+((r00*sj12*x796))+((cj12*r01*x796))+((cj12*r11*x795)));
01362 evalcond[4]=((((-1.0)*x792))+((x793*x795))+(((-1.0)*x791*x795))+(((-1.0)*x789*x796))+((x794*x796)));
01363 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
01364 {
01365 continue;
01366 }
01367 }
01368 
01369 rotationfunction0(solutions);
01370 }
01371 }
01372 
01373 }
01374 
01375 }
01376 
01377 } else
01378 {
01379 {
01380 IkReal j7array[1], cj7array[1], sj7array[1];
01381 bool j7valid[1]={false};
01382 _nj7 = 1;
01383 IkReal x797=((226.336)*cj11);
01384 CheckValue<IkReal> x798 = IKatan2WithCheck(IkReal((((r00*sj12*x797))+((cj12*r01*x797)))),IkReal(((((-1.0)*cj12*r11*x797))+(((-1.0)*r10*sj12*x797)))),IKFAST_ATAN2_MAGTHRESH);
01385 if(!x798.valid){
01386 continue;
01387 }
01388 CheckValue<IkReal> x799=IKPowWithIntegerCheck(IKsign(((((-247.808)*cj12*r20))+(((247.808)*r21*sj12)))),-1);
01389 if(!x799.valid){
01390 continue;
01391 }
01392 j7array[0]=((-1.5707963267949)+(x798.value)+(((1.5707963267949)*(x799.value))));
01393 sj7array[0]=IKsin(j7array[0]);
01394 cj7array[0]=IKcos(j7array[0]);
01395 if( j7array[0] > IKPI )
01396 {
01397     j7array[0]-=IK2PI;
01398 }
01399 else if( j7array[0] < -IKPI )
01400 {    j7array[0]+=IK2PI;
01401 }
01402 j7valid[0] = true;
01403 for(int ij7 = 0; ij7 < 1; ++ij7)
01404 {
01405 if( !j7valid[ij7] )
01406 {
01407     continue;
01408 }
01409 _ij7[0] = ij7; _ij7[1] = -1;
01410 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
01411 {
01412 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
01413 {
01414     j7valid[iij7]=false; _ij7[1] = iij7; break; 
01415 }
01416 }
01417 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
01418 {
01419 IkReal evalcond[5];
01420 IkReal x800=IKcos(j7);
01421 IkReal x801=IKsin(j7);
01422 IkReal x802=(r01*sj12);
01423 IkReal x803=((0.3215)*cj11);
01424 IkReal x804=(r11*sj12);
01425 IkReal x805=((0.3215)*sj11);
01426 IkReal x806=(cj12*r10);
01427 IkReal x807=(cj12*r00);
01428 IkReal x808=((0.352)*x801);
01429 IkReal x809=((0.352)*x800);
01430 evalcond[0]=(((r12*x808))+((r02*x809))+x803);
01431 evalcond[1]=((((-1.0)*x805*x807))+((r02*x803))+x809+((x802*x805)));
01432 evalcond[2]=(((x804*x805))+(((-1.0)*x805*x806))+((r12*x803))+x808);
01433 evalcond[3]=(((r00*sj12*x809))+((r10*sj12*x808))+((cj12*r01*x809))+((cj12*r11*x808)));
01434 evalcond[4]=((((-1.0)*x804*x808))+((x806*x808))+(((-1.0)*x805))+(((-1.0)*x802*x809))+((x807*x809)));
01435 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
01436 {
01437 continue;
01438 }
01439 }
01440 
01441 rotationfunction0(solutions);
01442 }
01443 }
01444 
01445 }
01446 
01447 }
01448 }
01449 }
01450 
01451 }
01452 
01453 }
01454 
01455 }
01456 } while(0);
01457 if( bgotonextstatement )
01458 {
01459 bool bgotonextstatement = true;
01460 do
01461 {
01462 if( 1 )
01463 {
01464 bgotonextstatement=false;
01465 continue; // branch miss [j7, j11]
01466 
01467 }
01468 } while(0);
01469 if( bgotonextstatement )
01470 {
01471 }
01472 }
01473 }
01474 
01475 } else
01476 {
01477 {
01478 IkReal j7array[2], cj7array[2], sj7array[2];
01479 bool j7valid[2]={false};
01480 _nj7 = 2;
01481 IkReal x810=((0.352)*cj12);
01482 IkReal x811=((0.352)*sj12);
01483 IkReal x812=((1.0)*pz);
01484 IkReal x813=(((r01*x810))+((r00*x811)));
01485 IkReal x814=(((r11*x810))+((r10*x811)));
01486 CheckValue<IkReal> x817 = IKatan2WithCheck(IkReal(x813),IkReal(x814),IKFAST_ATAN2_MAGTHRESH);
01487 if(!x817.valid){
01488 continue;
01489 }
01490 IkReal x815=((1.0)*(x817.value));
01491 if((((x814*x814)+(x813*x813))) < -0.00001)
01492 continue;
01493 CheckValue<IkReal> x818=IKPowWithIntegerCheck(IKabs(IKsqrt(((x814*x814)+(x813*x813)))),-1);
01494 if(!x818.valid){
01495 continue;
01496 }
01497 if( (((-1.0)*(x818.value)*(((((-1.0)*r20*sj12*x812))+(((-1.0)*cj12*r21*x812)))))) < -1-IKFAST_SINCOS_THRESH || (((-1.0)*(x818.value)*(((((-1.0)*r20*sj12*x812))+(((-1.0)*cj12*r21*x812)))))) > 1+IKFAST_SINCOS_THRESH )
01498     continue;
01499 IkReal x816=((-1.0)*(IKasin(((-1.0)*(x818.value)*(((((-1.0)*r20*sj12*x812))+(((-1.0)*cj12*r21*x812))))))));
01500 j7array[0]=((((-1.0)*x816))+(((-1.0)*x815)));
01501 sj7array[0]=IKsin(j7array[0]);
01502 cj7array[0]=IKcos(j7array[0]);
01503 j7array[1]=((3.14159265358979)+(((-1.0)*x815))+(((1.0)*x816)));
01504 sj7array[1]=IKsin(j7array[1]);
01505 cj7array[1]=IKcos(j7array[1]);
01506 if( j7array[0] > IKPI )
01507 {
01508     j7array[0]-=IK2PI;
01509 }
01510 else if( j7array[0] < -IKPI )
01511 {    j7array[0]+=IK2PI;
01512 }
01513 j7valid[0] = true;
01514 if( j7array[1] > IKPI )
01515 {
01516     j7array[1]-=IK2PI;
01517 }
01518 else if( j7array[1] < -IKPI )
01519 {    j7array[1]+=IK2PI;
01520 }
01521 j7valid[1] = true;
01522 for(int ij7 = 0; ij7 < 2; ++ij7)
01523 {
01524 if( !j7valid[ij7] )
01525 {
01526     continue;
01527 }
01528 _ij7[0] = ij7; _ij7[1] = -1;
01529 for(int iij7 = ij7+1; iij7 < 2; ++iij7)
01530 {
01531 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
01532 {
01533     j7valid[iij7]=false; _ij7[1] = iij7; break; 
01534 }
01535 }
01536 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
01537 
01538 {
01539 IkReal j11eval[1];
01540 IkReal x819=((-1.0)*pz);
01541 px=0;
01542 py=0;
01543 pp=pz*pz;
01544 npx=(pz*r20);
01545 npy=(pz*r21);
01546 npz=(pz*r22);
01547 rxp0_0=(pz*r10);
01548 rxp0_1=(r00*x819);
01549 rxp0_2=0;
01550 rxp1_0=(pz*r11);
01551 rxp1_1=(r01*x819);
01552 rxp1_2=0;
01553 rxp2_0=(pz*r12);
01554 rxp2_1=(r02*x819);
01555 rxp2_2=0;
01556 j11eval[0]=((((-1.0)*cj12*r20))+((r21*sj12)));
01557 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01558 {
01559 {
01560 IkReal j11eval[1];
01561 IkReal x820=((-1.0)*pz);
01562 px=0;
01563 py=0;
01564 pp=pz*pz;
01565 npx=(pz*r20);
01566 npy=(pz*r21);
01567 npz=(pz*r22);
01568 rxp0_0=(pz*r10);
01569 rxp0_1=(r00*x820);
01570 rxp0_2=0;
01571 rxp1_0=(pz*r11);
01572 rxp1_1=(r01*x820);
01573 rxp1_2=0;
01574 rxp2_0=(pz*r12);
01575 rxp2_1=(r02*x820);
01576 rxp2_2=0;
01577 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
01578 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01579 {
01580 {
01581 IkReal j11eval[1];
01582 IkReal x821=((-1.0)*pz);
01583 px=0;
01584 py=0;
01585 pp=pz*pz;
01586 npx=(pz*r20);
01587 npy=(pz*r21);
01588 npz=(pz*r22);
01589 rxp0_0=(pz*r10);
01590 rxp0_1=(r00*x821);
01591 rxp0_2=0;
01592 rxp1_0=(pz*r11);
01593 rxp1_1=(r01*x821);
01594 rxp1_2=0;
01595 rxp2_0=(pz*r12);
01596 rxp2_1=(r02*x821);
01597 rxp2_2=0;
01598 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
01599 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01600 {
01601 {
01602 IkReal evalcond[1];
01603 bool bgotonextstatement = true;
01604 do
01605 {
01606 evalcond[0]=((IKabs(r10))+(IKabs(r11)));
01607 if( IKabs(evalcond[0]) < 0.0000050000000000  )
01608 {
01609 bgotonextstatement=false;
01610 {
01611 IkReal j11eval[1];
01612 IkReal x822=((-1.0)*pz);
01613 px=0;
01614 py=0;
01615 pp=pz*pz;
01616 npx=(pz*r20);
01617 npy=(pz*r21);
01618 npz=0;
01619 rxp0_0=0;
01620 rxp0_1=(r00*x822);
01621 rxp0_2=0;
01622 rxp1_0=0;
01623 rxp1_1=(r01*x822);
01624 rxp1_2=0;
01625 rxp2_0=(pz*r12);
01626 rxp2_1=0;
01627 rxp2_2=0;
01628 r10=0;
01629 r11=0;
01630 r22=0;
01631 r02=0;
01632 j11eval[0]=((((-1.0)*cj12*r20))+((r21*sj12)));
01633 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01634 {
01635 {
01636 IkReal j11eval[1];
01637 IkReal x823=((-1.0)*pz);
01638 px=0;
01639 py=0;
01640 pp=pz*pz;
01641 npx=(pz*r20);
01642 npy=(pz*r21);
01643 npz=0;
01644 rxp0_0=0;
01645 rxp0_1=(r00*x823);
01646 rxp0_2=0;
01647 rxp1_0=0;
01648 rxp1_1=(r01*x823);
01649 rxp1_2=0;
01650 rxp2_0=(pz*r12);
01651 rxp2_1=0;
01652 rxp2_2=0;
01653 r10=0;
01654 r11=0;
01655 r22=0;
01656 r02=0;
01657 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
01658 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01659 {
01660 {
01661 IkReal j11eval[1];
01662 IkReal x824=((-1.0)*pz);
01663 px=0;
01664 py=0;
01665 pp=pz*pz;
01666 npx=(pz*r20);
01667 npy=(pz*r21);
01668 npz=0;
01669 rxp0_0=0;
01670 rxp0_1=(r00*x824);
01671 rxp0_2=0;
01672 rxp1_0=0;
01673 rxp1_1=(r01*x824);
01674 rxp1_2=0;
01675 rxp2_0=(pz*r12);
01676 rxp2_1=0;
01677 rxp2_2=0;
01678 r10=0;
01679 r11=0;
01680 r22=0;
01681 r02=0;
01682 j11eval[0]=(((pz*r21*sj12))+(((-1.0)*cj12*pz*r20)));
01683 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01684 {
01685 continue; // 3 cases reached
01686 
01687 } else
01688 {
01689 {
01690 IkReal j11array[1], cj11array[1], sj11array[1];
01691 bool j11valid[1]={false};
01692 _nj11 = 1;
01693 IkReal x825=((1286000.0)*pz);
01694 CheckValue<IkReal> x826=IKPowWithIntegerCheck((((r21*sj12*x825))+(((-1.0)*cj12*r20*x825))),-1);
01695 if(!x826.valid){
01696 continue;
01697 }
01698 if( IKabs(((-82167.0)*(x826.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.09486780715397)*r12*sj7)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-82167.0)*(x826.value)))+IKsqr(((-1.09486780715397)*r12*sj7))-1) <= IKFAST_SINCOS_THRESH )
01699     continue;
01700 j11array[0]=IKatan2(((-82167.0)*(x826.value)), ((-1.09486780715397)*r12*sj7));
01701 sj11array[0]=IKsin(j11array[0]);
01702 cj11array[0]=IKcos(j11array[0]);
01703 if( j11array[0] > IKPI )
01704 {
01705     j11array[0]-=IK2PI;
01706 }
01707 else if( j11array[0] < -IKPI )
01708 {    j11array[0]+=IK2PI;
01709 }
01710 j11valid[0] = true;
01711 for(int ij11 = 0; ij11 < 1; ++ij11)
01712 {
01713 if( !j11valid[ij11] )
01714 {
01715     continue;
01716 }
01717 _ij11[0] = ij11; _ij11[1] = -1;
01718 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
01719 {
01720 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01721 {
01722     j11valid[iij11]=false; _ij11[1] = iij11; break; 
01723 }
01724 }
01725 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01726 {
01727 IkReal evalcond[6];
01728 IkReal x827=IKcos(j11);
01729 IkReal x828=IKsin(j11);
01730 IkReal x829=(cj12*r20);
01731 IkReal x830=(r01*sj12);
01732 IkReal x831=((0.352)*cj7);
01733 IkReal x832=((1.0)*pz);
01734 IkReal x833=(cj12*r00);
01735 IkReal x834=(r21*sj12);
01736 IkReal x835=((0.352)*sj7);
01737 IkReal x836=((0.3215)*x828);
01738 IkReal x837=((0.3215)*x827);
01739 IkReal x838=((0.643)*pz*x828);
01740 evalcond[0]=(((r12*x835))+x837);
01741 evalcond[1]=(((r12*x837))+x835);
01742 evalcond[2]=((((-1.0)*x832))+((x834*x836))+(((-1.0)*x829*x836)));
01743 evalcond[3]=((0.0410835)+((x834*x838))+(((-1.0)*x829*x838)));
01744 evalcond[4]=((((-1.0)*x833*x836))+((x830*x836))+x831);
01745 evalcond[5]=(((x831*x833))+((pz*x834))+(((-1.0)*x830*x831))+(((-1.0)*x836))+(((-1.0)*x829*x832)));
01746 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
01747 {
01748 continue;
01749 }
01750 }
01751 
01752 rotationfunction0(solutions);
01753 }
01754 }
01755 
01756 }
01757 
01758 }
01759 
01760 } else
01761 {
01762 {
01763 IkReal j11array[1], cj11array[1], sj11array[1];
01764 bool j11valid[1]={false};
01765 _nj11 = 1;
01766 CheckValue<IkReal> x839=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
01767 if(!x839.valid){
01768 continue;
01769 }
01770 if( IKabs(((-44.0)*cj7*(x839.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.09486780715397)*r12*sj7)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*cj7*(x839.value)))+IKsqr(((-1.09486780715397)*r12*sj7))-1) <= IKFAST_SINCOS_THRESH )
01771     continue;
01772 j11array[0]=IKatan2(((-44.0)*cj7*(x839.value)), ((-1.09486780715397)*r12*sj7));
01773 sj11array[0]=IKsin(j11array[0]);
01774 cj11array[0]=IKcos(j11array[0]);
01775 if( j11array[0] > IKPI )
01776 {
01777     j11array[0]-=IK2PI;
01778 }
01779 else if( j11array[0] < -IKPI )
01780 {    j11array[0]+=IK2PI;
01781 }
01782 j11valid[0] = true;
01783 for(int ij11 = 0; ij11 < 1; ++ij11)
01784 {
01785 if( !j11valid[ij11] )
01786 {
01787     continue;
01788 }
01789 _ij11[0] = ij11; _ij11[1] = -1;
01790 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
01791 {
01792 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01793 {
01794     j11valid[iij11]=false; _ij11[1] = iij11; break; 
01795 }
01796 }
01797 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01798 {
01799 IkReal evalcond[6];
01800 IkReal x840=IKcos(j11);
01801 IkReal x841=IKsin(j11);
01802 IkReal x842=(cj12*r20);
01803 IkReal x843=(r01*sj12);
01804 IkReal x844=((0.352)*cj7);
01805 IkReal x845=((1.0)*pz);
01806 IkReal x846=(cj12*r00);
01807 IkReal x847=(r21*sj12);
01808 IkReal x848=((0.352)*sj7);
01809 IkReal x849=((0.3215)*x841);
01810 IkReal x850=((0.3215)*x840);
01811 IkReal x851=((0.643)*pz*x841);
01812 evalcond[0]=(((r12*x848))+x850);
01813 evalcond[1]=(((r12*x850))+x848);
01814 evalcond[2]=((((-1.0)*x842*x849))+(((-1.0)*x845))+((x847*x849)));
01815 evalcond[3]=((0.0410835)+(((-1.0)*x842*x851))+((x847*x851)));
01816 evalcond[4]=(((x843*x849))+(((-1.0)*x846*x849))+x844);
01817 evalcond[5]=((((-1.0)*x842*x845))+((pz*x847))+(((-1.0)*x843*x844))+(((-1.0)*x849))+((x844*x846)));
01818 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
01819 {
01820 continue;
01821 }
01822 }
01823 
01824 rotationfunction0(solutions);
01825 }
01826 }
01827 
01828 }
01829 
01830 }
01831 
01832 } else
01833 {
01834 {
01835 IkReal j11array[1], cj11array[1], sj11array[1];
01836 bool j11valid[1]={false};
01837 _nj11 = 1;
01838 CheckValue<IkReal> x852=IKPowWithIntegerCheck(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12))),-1);
01839 if(!x852.valid){
01840 continue;
01841 }
01842 if( IKabs((pz*(x852.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.09486780715397)*r12*sj7)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((pz*(x852.value)))+IKsqr(((-1.09486780715397)*r12*sj7))-1) <= IKFAST_SINCOS_THRESH )
01843     continue;
01844 j11array[0]=IKatan2((pz*(x852.value)), ((-1.09486780715397)*r12*sj7));
01845 sj11array[0]=IKsin(j11array[0]);
01846 cj11array[0]=IKcos(j11array[0]);
01847 if( j11array[0] > IKPI )
01848 {
01849     j11array[0]-=IK2PI;
01850 }
01851 else if( j11array[0] < -IKPI )
01852 {    j11array[0]+=IK2PI;
01853 }
01854 j11valid[0] = true;
01855 for(int ij11 = 0; ij11 < 1; ++ij11)
01856 {
01857 if( !j11valid[ij11] )
01858 {
01859     continue;
01860 }
01861 _ij11[0] = ij11; _ij11[1] = -1;
01862 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
01863 {
01864 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01865 {
01866     j11valid[iij11]=false; _ij11[1] = iij11; break; 
01867 }
01868 }
01869 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01870 {
01871 IkReal evalcond[6];
01872 IkReal x853=IKcos(j11);
01873 IkReal x854=IKsin(j11);
01874 IkReal x855=(cj12*r20);
01875 IkReal x856=(r01*sj12);
01876 IkReal x857=((0.352)*cj7);
01877 IkReal x858=((1.0)*pz);
01878 IkReal x859=(cj12*r00);
01879 IkReal x860=(r21*sj12);
01880 IkReal x861=((0.352)*sj7);
01881 IkReal x862=((0.3215)*x854);
01882 IkReal x863=((0.3215)*x853);
01883 IkReal x864=((0.643)*pz*x854);
01884 evalcond[0]=(((r12*x861))+x863);
01885 evalcond[1]=(((r12*x863))+x861);
01886 evalcond[2]=((((-1.0)*x858))+((x860*x862))+(((-1.0)*x855*x862)));
01887 evalcond[3]=((0.0410835)+((x860*x864))+(((-1.0)*x855*x864)));
01888 evalcond[4]=(((x856*x862))+(((-1.0)*x859*x862))+x857);
01889 evalcond[5]=((((-1.0)*x862))+(((-1.0)*x856*x857))+(((-1.0)*x855*x858))+((pz*x860))+((x857*x859)));
01890 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
01891 {
01892 continue;
01893 }
01894 }
01895 
01896 rotationfunction0(solutions);
01897 }
01898 }
01899 
01900 }
01901 
01902 }
01903 
01904 }
01905 } while(0);
01906 if( bgotonextstatement )
01907 {
01908 bool bgotonextstatement = true;
01909 do
01910 {
01911 evalcond[0]=((IKabs(r21))+(IKabs(r20)));
01912 if( IKabs(evalcond[0]) < 0.0000050000000000  )
01913 {
01914 bgotonextstatement=false;
01915 {
01916 IkReal j11eval[1];
01917 IkReal x865=((-1.0)*pz);
01918 px=0;
01919 py=0;
01920 pp=pz*pz;
01921 npx=0;
01922 npy=0;
01923 npz=(pz*r22);
01924 rxp0_0=(pz*r10);
01925 rxp0_1=(r00*x865);
01926 rxp0_2=0;
01927 rxp1_0=(pz*r11);
01928 rxp1_1=(r01*x865);
01929 rxp1_2=0;
01930 rxp2_0=0;
01931 rxp2_1=0;
01932 rxp2_2=0;
01933 r20=0;
01934 r21=0;
01935 r02=0;
01936 r12=0;
01937 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
01938 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01939 {
01940 {
01941 IkReal j11eval[1];
01942 IkReal x866=((-1.0)*pz);
01943 px=0;
01944 py=0;
01945 pp=pz*pz;
01946 npx=0;
01947 npy=0;
01948 npz=(pz*r22);
01949 rxp0_0=(pz*r10);
01950 rxp0_1=(r00*x866);
01951 rxp0_2=0;
01952 rxp1_0=(pz*r11);
01953 rxp1_1=(r01*x866);
01954 rxp1_2=0;
01955 rxp2_0=0;
01956 rxp2_1=0;
01957 rxp2_2=0;
01958 r20=0;
01959 r21=0;
01960 r02=0;
01961 r12=0;
01962 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
01963 if( IKabs(j11eval[0]) < 0.0000010000000000  )
01964 {
01965 {
01966 IkReal j11eval[3];
01967 IkReal x867=((-1.0)*pz);
01968 px=0;
01969 py=0;
01970 pp=pz*pz;
01971 npx=0;
01972 npy=0;
01973 npz=(pz*r22);
01974 rxp0_0=(pz*r10);
01975 rxp0_1=(r00*x867);
01976 rxp0_2=0;
01977 rxp1_0=(pz*r11);
01978 rxp1_1=(r01*x867);
01979 rxp1_2=0;
01980 rxp2_0=0;
01981 rxp2_1=0;
01982 rxp2_2=0;
01983 r20=0;
01984 r21=0;
01985 r02=0;
01986 r12=0;
01987 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
01988 j11eval[1]=pz;
01989 j11eval[2]=r22;
01990 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  || IKabs(j11eval[2]) < 0.0000010000000000  )
01991 {
01992 continue; // 3 cases reached
01993 
01994 } else
01995 {
01996 {
01997 IkReal j11array[1], cj11array[1], sj11array[1];
01998 bool j11valid[1]={false};
01999 _nj11 = 1;
02000 CheckValue<IkReal> x868=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
02001 if(!x868.valid){
02002 continue;
02003 }
02004 CheckValue<IkReal> x869=IKPowWithIntegerCheck(pz,-1);
02005 if(!x869.valid){
02006 continue;
02007 }
02008 CheckValue<IkReal> x870=IKPowWithIntegerCheck(r22,-1);
02009 if(!x870.valid){
02010 continue;
02011 }
02012 if( IKabs(((-44.0)*cj7*(x868.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.063893468118196)*(x869.value)*(x870.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*cj7*(x868.value)))+IKsqr(((-0.063893468118196)*(x869.value)*(x870.value)))-1) <= IKFAST_SINCOS_THRESH )
02013     continue;
02014 j11array[0]=IKatan2(((-44.0)*cj7*(x868.value)), ((-0.063893468118196)*(x869.value)*(x870.value)));
02015 sj11array[0]=IKsin(j11array[0]);
02016 cj11array[0]=IKcos(j11array[0]);
02017 if( j11array[0] > IKPI )
02018 {
02019     j11array[0]-=IK2PI;
02020 }
02021 else if( j11array[0] < -IKPI )
02022 {    j11array[0]+=IK2PI;
02023 }
02024 j11valid[0] = true;
02025 for(int ij11 = 0; ij11 < 1; ++ij11)
02026 {
02027 if( !j11valid[ij11] )
02028 {
02029     continue;
02030 }
02031 _ij11[0] = ij11; _ij11[1] = -1;
02032 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02033 {
02034 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02035 {
02036     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02037 }
02038 }
02039 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02040 {
02041 IkReal evalcond[6];
02042 IkReal x871=IKcos(j11);
02043 IkReal x872=IKsin(j11);
02044 IkReal x873=((0.352)*sj7);
02045 IkReal x874=(cj12*r10);
02046 IkReal x875=(r01*sj12);
02047 IkReal x876=(r11*sj12);
02048 IkReal x877=((1.0)*pz);
02049 IkReal x878=((0.352)*cj7);
02050 IkReal x879=(cj12*r00);
02051 IkReal x880=((0.3215)*x872);
02052 IkReal x881=(r22*x871);
02053 evalcond[0]=((0.0410835)+(((0.643)*pz*x881)));
02054 evalcond[1]=((((0.3215)*x871))+(((-1.0)*r22*x877)));
02055 evalcond[2]=((((0.3215)*x881))+(((-1.0)*x877)));
02056 evalcond[3]=((((-1.0)*x879*x880))+((x875*x880))+x878);
02057 evalcond[4]=(((x876*x880))+x873+(((-1.0)*x874*x880)));
02058 evalcond[5]=(((x878*x879))+((x873*x874))+(((-1.0)*x873*x876))+(((-1.0)*x880))+(((-1.0)*x875*x878)));
02059 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02060 {
02061 continue;
02062 }
02063 }
02064 
02065 rotationfunction0(solutions);
02066 }
02067 }
02068 
02069 }
02070 
02071 }
02072 
02073 } else
02074 {
02075 {
02076 IkReal j11array[1], cj11array[1], sj11array[1];
02077 bool j11valid[1]={false};
02078 _nj11 = 1;
02079 CheckValue<IkReal> x882=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
02080 if(!x882.valid){
02081 continue;
02082 }
02083 if( IKabs(((-44.0)*sj7*(x882.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((3.1104199066874)*pz*r22)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*sj7*(x882.value)))+IKsqr(((3.1104199066874)*pz*r22))-1) <= IKFAST_SINCOS_THRESH )
02084     continue;
02085 j11array[0]=IKatan2(((-44.0)*sj7*(x882.value)), ((3.1104199066874)*pz*r22));
02086 sj11array[0]=IKsin(j11array[0]);
02087 cj11array[0]=IKcos(j11array[0]);
02088 if( j11array[0] > IKPI )
02089 {
02090     j11array[0]-=IK2PI;
02091 }
02092 else if( j11array[0] < -IKPI )
02093 {    j11array[0]+=IK2PI;
02094 }
02095 j11valid[0] = true;
02096 for(int ij11 = 0; ij11 < 1; ++ij11)
02097 {
02098 if( !j11valid[ij11] )
02099 {
02100     continue;
02101 }
02102 _ij11[0] = ij11; _ij11[1] = -1;
02103 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02104 {
02105 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02106 {
02107     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02108 }
02109 }
02110 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02111 {
02112 IkReal evalcond[6];
02113 IkReal x883=IKcos(j11);
02114 IkReal x884=IKsin(j11);
02115 IkReal x885=((0.352)*sj7);
02116 IkReal x886=(cj12*r10);
02117 IkReal x887=(r01*sj12);
02118 IkReal x888=(r11*sj12);
02119 IkReal x889=((1.0)*pz);
02120 IkReal x890=((0.352)*cj7);
02121 IkReal x891=(cj12*r00);
02122 IkReal x892=((0.3215)*x884);
02123 IkReal x893=(r22*x883);
02124 evalcond[0]=((0.0410835)+(((0.643)*pz*x893)));
02125 evalcond[1]=((((0.3215)*x883))+(((-1.0)*r22*x889)));
02126 evalcond[2]=((((0.3215)*x893))+(((-1.0)*x889)));
02127 evalcond[3]=(x890+(((-1.0)*x891*x892))+((x887*x892)));
02128 evalcond[4]=(x885+((x888*x892))+(((-1.0)*x886*x892)));
02129 evalcond[5]=((((-1.0)*x892))+((x890*x891))+(((-1.0)*x885*x888))+((x885*x886))+(((-1.0)*x887*x890)));
02130 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02131 {
02132 continue;
02133 }
02134 }
02135 
02136 rotationfunction0(solutions);
02137 }
02138 }
02139 
02140 }
02141 
02142 }
02143 
02144 } else
02145 {
02146 {
02147 IkReal j11array[1], cj11array[1], sj11array[1];
02148 bool j11valid[1]={false};
02149 _nj11 = 1;
02150 CheckValue<IkReal> x894=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
02151 if(!x894.valid){
02152 continue;
02153 }
02154 if( IKabs(((-44.0)*cj7*(x894.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((3.1104199066874)*pz*r22)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*cj7*(x894.value)))+IKsqr(((3.1104199066874)*pz*r22))-1) <= IKFAST_SINCOS_THRESH )
02155     continue;
02156 j11array[0]=IKatan2(((-44.0)*cj7*(x894.value)), ((3.1104199066874)*pz*r22));
02157 sj11array[0]=IKsin(j11array[0]);
02158 cj11array[0]=IKcos(j11array[0]);
02159 if( j11array[0] > IKPI )
02160 {
02161     j11array[0]-=IK2PI;
02162 }
02163 else if( j11array[0] < -IKPI )
02164 {    j11array[0]+=IK2PI;
02165 }
02166 j11valid[0] = true;
02167 for(int ij11 = 0; ij11 < 1; ++ij11)
02168 {
02169 if( !j11valid[ij11] )
02170 {
02171     continue;
02172 }
02173 _ij11[0] = ij11; _ij11[1] = -1;
02174 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02175 {
02176 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02177 {
02178     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02179 }
02180 }
02181 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02182 {
02183 IkReal evalcond[6];
02184 IkReal x895=IKcos(j11);
02185 IkReal x896=IKsin(j11);
02186 IkReal x897=((0.352)*sj7);
02187 IkReal x898=(cj12*r10);
02188 IkReal x899=(r01*sj12);
02189 IkReal x900=(r11*sj12);
02190 IkReal x901=((1.0)*pz);
02191 IkReal x902=((0.352)*cj7);
02192 IkReal x903=(cj12*r00);
02193 IkReal x904=((0.3215)*x896);
02194 IkReal x905=(r22*x895);
02195 evalcond[0]=((0.0410835)+(((0.643)*pz*x905)));
02196 evalcond[1]=((((-1.0)*r22*x901))+(((0.3215)*x895)));
02197 evalcond[2]=((((0.3215)*x905))+(((-1.0)*x901)));
02198 evalcond[3]=((((-1.0)*x903*x904))+((x899*x904))+x902);
02199 evalcond[4]=(((x900*x904))+(((-1.0)*x898*x904))+x897);
02200 evalcond[5]=((((-1.0)*x897*x900))+(((-1.0)*x904))+((x902*x903))+((x897*x898))+(((-1.0)*x899*x902)));
02201 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02202 {
02203 continue;
02204 }
02205 }
02206 
02207 rotationfunction0(solutions);
02208 }
02209 }
02210 
02211 }
02212 
02213 }
02214 
02215 }
02216 } while(0);
02217 if( bgotonextstatement )
02218 {
02219 bool bgotonextstatement = true;
02220 do
02221 {
02222 if( 1 )
02223 {
02224 bgotonextstatement=false;
02225 continue; // branch miss [j11]
02226 
02227 }
02228 } while(0);
02229 if( bgotonextstatement )
02230 {
02231 }
02232 }
02233 }
02234 }
02235 
02236 } else
02237 {
02238 {
02239 IkReal j11array[1], cj11array[1], sj11array[1];
02240 bool j11valid[1]={false};
02241 _nj11 = 1;
02242 IkReal x906=((44.0)*sj7);
02243 IkReal x907=(pz*r22);
02244 IkReal x908=(cj7*r02);
02245 CheckValue<IkReal> x909=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
02246 if(!x909.valid){
02247 continue;
02248 }
02249 if( IKabs(((x909.value)*(((((44.0)*r12*x908))+(((-1.0)*x906))+(((-125.0)*r12*x907))+((x906*(r12*r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x908))+(((3.1104199066874)*x907))+(((-1.09486780715397)*r12*sj7)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x909.value)*(((((44.0)*r12*x908))+(((-1.0)*x906))+(((-125.0)*r12*x907))+((x906*(r12*r12)))))))+IKsqr(((((-1.09486780715397)*x908))+(((3.1104199066874)*x907))+(((-1.09486780715397)*r12*sj7))))-1) <= IKFAST_SINCOS_THRESH )
02250     continue;
02251 j11array[0]=IKatan2(((x909.value)*(((((44.0)*r12*x908))+(((-1.0)*x906))+(((-125.0)*r12*x907))+((x906*(r12*r12)))))), ((((-1.09486780715397)*x908))+(((3.1104199066874)*x907))+(((-1.09486780715397)*r12*sj7))));
02252 sj11array[0]=IKsin(j11array[0]);
02253 cj11array[0]=IKcos(j11array[0]);
02254 if( j11array[0] > IKPI )
02255 {
02256     j11array[0]-=IK2PI;
02257 }
02258 else if( j11array[0] < -IKPI )
02259 {    j11array[0]+=IK2PI;
02260 }
02261 j11valid[0] = true;
02262 for(int ij11 = 0; ij11 < 1; ++ij11)
02263 {
02264 if( !j11valid[ij11] )
02265 {
02266     continue;
02267 }
02268 _ij11[0] = ij11; _ij11[1] = -1;
02269 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02270 {
02271 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02272 {
02273     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02274 }
02275 }
02276 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02277 {
02278 IkReal evalcond[6];
02279 IkReal x910=IKcos(j11);
02280 IkReal x911=IKsin(j11);
02281 IkReal x912=((0.643)*pz);
02282 IkReal x913=((0.352)*sj7);
02283 IkReal x914=(cj12*r10);
02284 IkReal x915=((0.352)*cj7);
02285 IkReal x916=(cj12*r00);
02286 IkReal x917=((1.0)*pz);
02287 IkReal x918=(cj12*r20);
02288 IkReal x919=(sj12*x911);
02289 IkReal x920=((0.3215)*x910);
02290 IkReal x921=((0.3215)*x911);
02291 evalcond[0]=((((-1.0)*r22*x917))+((r12*x913))+((r02*x915))+x920);
02292 evalcond[1]=(((r22*x920))+(((-1.0)*x917))+(((-1.0)*x918*x921))+(((0.3215)*r21*x919)));
02293 evalcond[2]=((((0.3215)*r01*x919))+(((-1.0)*x916*x921))+x915+((r02*x920)));
02294 evalcond[3]=((((0.3215)*r11*x919))+(((-1.0)*x914*x921))+((r12*x920))+x913);
02295 evalcond[4]=((0.0410835)+(((-1.0)*x911*x912*x918))+((r22*x910*x912))+((r21*x912*x919)));
02296 evalcond[5]=((((-1.0)*x921))+((pz*r21*sj12))+(((-1.0)*r01*sj12*x915))+((x915*x916))+(((-1.0)*r11*sj12*x913))+((x913*x914))+(((-1.0)*x917*x918)));
02297 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02298 {
02299 continue;
02300 }
02301 }
02302 
02303 rotationfunction0(solutions);
02304 }
02305 }
02306 
02307 }
02308 
02309 }
02310 
02311 } else
02312 {
02313 {
02314 IkReal j11array[1], cj11array[1], sj11array[1];
02315 bool j11valid[1]={false};
02316 _nj11 = 1;
02317 IkReal x922=(pz*r22);
02318 IkReal x923=(r12*sj7);
02319 IkReal x924=((44.0)*cj7);
02320 CheckValue<IkReal> x925=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
02321 if(!x925.valid){
02322 continue;
02323 }
02324 if( IKabs(((x925.value)*(((((-1.0)*x924))+(((44.0)*r02*x923))+((x924*(r02*r02)))+(((-125.0)*r02*x922)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x923))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x922)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x925.value)*(((((-1.0)*x924))+(((44.0)*r02*x923))+((x924*(r02*r02)))+(((-125.0)*r02*x922))))))+IKsqr(((((-1.09486780715397)*x923))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x922))))-1) <= IKFAST_SINCOS_THRESH )
02325     continue;
02326 j11array[0]=IKatan2(((x925.value)*(((((-1.0)*x924))+(((44.0)*r02*x923))+((x924*(r02*r02)))+(((-125.0)*r02*x922))))), ((((-1.09486780715397)*x923))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x922))));
02327 sj11array[0]=IKsin(j11array[0]);
02328 cj11array[0]=IKcos(j11array[0]);
02329 if( j11array[0] > IKPI )
02330 {
02331     j11array[0]-=IK2PI;
02332 }
02333 else if( j11array[0] < -IKPI )
02334 {    j11array[0]+=IK2PI;
02335 }
02336 j11valid[0] = true;
02337 for(int ij11 = 0; ij11 < 1; ++ij11)
02338 {
02339 if( !j11valid[ij11] )
02340 {
02341     continue;
02342 }
02343 _ij11[0] = ij11; _ij11[1] = -1;
02344 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02345 {
02346 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02347 {
02348     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02349 }
02350 }
02351 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02352 {
02353 IkReal evalcond[6];
02354 IkReal x926=IKcos(j11);
02355 IkReal x927=IKsin(j11);
02356 IkReal x928=((0.643)*pz);
02357 IkReal x929=((0.352)*sj7);
02358 IkReal x930=(cj12*r10);
02359 IkReal x931=((0.352)*cj7);
02360 IkReal x932=(cj12*r00);
02361 IkReal x933=((1.0)*pz);
02362 IkReal x934=(cj12*r20);
02363 IkReal x935=(sj12*x927);
02364 IkReal x936=((0.3215)*x926);
02365 IkReal x937=((0.3215)*x927);
02366 evalcond[0]=((((-1.0)*r22*x933))+((r12*x929))+x936+((r02*x931)));
02367 evalcond[1]=(((r22*x936))+(((0.3215)*r21*x935))+(((-1.0)*x934*x937))+(((-1.0)*x933)));
02368 evalcond[2]=((((-1.0)*x932*x937))+(((0.3215)*r01*x935))+x931+((r02*x936)));
02369 evalcond[3]=((((-1.0)*x930*x937))+((r12*x936))+x929+(((0.3215)*r11*x935)));
02370 evalcond[4]=((0.0410835)+((r21*x928*x935))+((r22*x926*x928))+(((-1.0)*x927*x928*x934)));
02371 evalcond[5]=(((pz*r21*sj12))+(((-1.0)*r11*sj12*x929))+((x931*x932))+(((-1.0)*r01*sj12*x931))+(((-1.0)*x933*x934))+((x929*x930))+(((-1.0)*x937)));
02372 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02373 {
02374 continue;
02375 }
02376 }
02377 
02378 rotationfunction0(solutions);
02379 }
02380 }
02381 
02382 }
02383 
02384 }
02385 
02386 } else
02387 {
02388 {
02389 IkReal j11array[1], cj11array[1], sj11array[1];
02390 bool j11valid[1]={false};
02391 _nj11 = 1;
02392 IkReal x938=((0.352)*r22);
02393 IkReal x939=(r12*sj7);
02394 IkReal x940=(cj7*r02);
02395 CheckValue<IkReal> x941=IKPowWithIntegerCheck(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12))),-1);
02396 if(!x941.valid){
02397 continue;
02398 }
02399 if( IKabs(((x941.value)*((((x938*x940))+(((-1.0)*pz*(r22*r22)))+pz+((x938*x939)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x940))+(((-1.09486780715397)*x939))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x941.value)*((((x938*x940))+(((-1.0)*pz*(r22*r22)))+pz+((x938*x939))))))+IKsqr(((((-1.09486780715397)*x940))+(((-1.09486780715397)*x939))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
02400     continue;
02401 j11array[0]=IKatan2(((x941.value)*((((x938*x940))+(((-1.0)*pz*(r22*r22)))+pz+((x938*x939))))), ((((-1.09486780715397)*x940))+(((-1.09486780715397)*x939))+(((3.1104199066874)*pz*r22))));
02402 sj11array[0]=IKsin(j11array[0]);
02403 cj11array[0]=IKcos(j11array[0]);
02404 if( j11array[0] > IKPI )
02405 {
02406     j11array[0]-=IK2PI;
02407 }
02408 else if( j11array[0] < -IKPI )
02409 {    j11array[0]+=IK2PI;
02410 }
02411 j11valid[0] = true;
02412 for(int ij11 = 0; ij11 < 1; ++ij11)
02413 {
02414 if( !j11valid[ij11] )
02415 {
02416     continue;
02417 }
02418 _ij11[0] = ij11; _ij11[1] = -1;
02419 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02420 {
02421 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02422 {
02423     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02424 }
02425 }
02426 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02427 {
02428 IkReal evalcond[6];
02429 IkReal x942=IKcos(j11);
02430 IkReal x943=IKsin(j11);
02431 IkReal x944=((0.643)*pz);
02432 IkReal x945=((0.352)*sj7);
02433 IkReal x946=(cj12*r10);
02434 IkReal x947=((0.352)*cj7);
02435 IkReal x948=(cj12*r00);
02436 IkReal x949=((1.0)*pz);
02437 IkReal x950=(cj12*r20);
02438 IkReal x951=(sj12*x943);
02439 IkReal x952=((0.3215)*x942);
02440 IkReal x953=((0.3215)*x943);
02441 evalcond[0]=(((r02*x947))+((r12*x945))+(((-1.0)*r22*x949))+x952);
02442 evalcond[1]=((((0.3215)*r21*x951))+((r22*x952))+(((-1.0)*x949))+(((-1.0)*x950*x953)));
02443 evalcond[2]=(((r02*x952))+(((0.3215)*r01*x951))+x947+(((-1.0)*x948*x953)));
02444 evalcond[3]=((((0.3215)*r11*x951))+(((-1.0)*x946*x953))+((r12*x952))+x945);
02445 evalcond[4]=((0.0410835)+(((-1.0)*x943*x944*x950))+((r21*x944*x951))+((r22*x942*x944)));
02446 evalcond[5]=(((pz*r21*sj12))+((x945*x946))+((x947*x948))+(((-1.0)*x949*x950))+(((-1.0)*r01*sj12*x947))+(((-1.0)*x953))+(((-1.0)*r11*sj12*x945)));
02447 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
02448 {
02449 continue;
02450 }
02451 }
02452 
02453 rotationfunction0(solutions);
02454 }
02455 }
02456 
02457 }
02458 
02459 }
02460 }
02461 }
02462 
02463 }
02464 
02465 }
02466 
02467 } else
02468 {
02469 {
02470 IkReal j11array[2], cj11array[2], sj11array[2];
02471 bool j11valid[2]={false};
02472 _nj11 = 2;
02473 IkReal x954=((0.643)*pz);
02474 IkReal x955=(((r21*sj12*x954))+(((-1.0)*cj12*r20*x954)));
02475 CheckValue<IkReal> x958 = IKatan2WithCheck(IkReal((r22*x954)),IkReal(x955),IKFAST_ATAN2_MAGTHRESH);
02476 if(!x958.valid){
02477 continue;
02478 }
02479 IkReal x956=((1.0)*(x958.value));
02480 if((((x955*x955)+(((0.413449)*(pz*pz)*(r22*r22))))) < -0.00001)
02481 continue;
02482 CheckValue<IkReal> x959=IKPowWithIntegerCheck(IKabs(IKsqrt(((x955*x955)+(((0.413449)*(pz*pz)*(r22*r22)))))),-1);
02483 if(!x959.valid){
02484 continue;
02485 }
02486 if( (((0.0410835)*(x959.value))) < -1-IKFAST_SINCOS_THRESH || (((0.0410835)*(x959.value))) > 1+IKFAST_SINCOS_THRESH )
02487     continue;
02488 IkReal x957=IKasin(((0.0410835)*(x959.value)));
02489 j11array[0]=((((-1.0)*x957))+(((-1.0)*x956)));
02490 sj11array[0]=IKsin(j11array[0]);
02491 cj11array[0]=IKcos(j11array[0]);
02492 j11array[1]=((3.14159265358979)+(((-1.0)*x956))+x957);
02493 sj11array[1]=IKsin(j11array[1]);
02494 cj11array[1]=IKcos(j11array[1]);
02495 if( j11array[0] > IKPI )
02496 {
02497     j11array[0]-=IK2PI;
02498 }
02499 else if( j11array[0] < -IKPI )
02500 {    j11array[0]+=IK2PI;
02501 }
02502 j11valid[0] = true;
02503 if( j11array[1] > IKPI )
02504 {
02505     j11array[1]-=IK2PI;
02506 }
02507 else if( j11array[1] < -IKPI )
02508 {    j11array[1]+=IK2PI;
02509 }
02510 j11valid[1] = true;
02511 for(int ij11 = 0; ij11 < 2; ++ij11)
02512 {
02513 if( !j11valid[ij11] )
02514 {
02515     continue;
02516 }
02517 _ij11[0] = ij11; _ij11[1] = -1;
02518 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
02519 {
02520 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02521 {
02522     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02523 }
02524 }
02525 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02526 {
02527 IkReal evalcond[1];
02528 IkReal x960=IKsin(j11);
02529 IkReal x961=((0.3215)*x960);
02530 evalcond[0]=((((-1.0)*pz))+((r21*sj12*x961))+(((-1.0)*cj12*r20*x961))+(((0.3215)*r22*(IKcos(j11)))));
02531 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
02532 {
02533 continue;
02534 }
02535 }
02536 
02537 {
02538 IkReal j7array[1], cj7array[1], sj7array[1];
02539 bool j7valid[1]={false};
02540 _nj7 = 1;
02541 IkReal x962=((0.913352272727273)*cj11);
02542 IkReal x963=((0.913352272727273)*cj12*sj11);
02543 IkReal x964=((0.913352272727273)*sj11*sj12);
02544 if( IKabs(((((-1.0)*r12*x962))+(((-1.0)*r11*x964))+((r10*x963)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r01*x964))+((r00*x963))+(((-1.0)*r02*x962)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*r12*x962))+(((-1.0)*r11*x964))+((r10*x963))))+IKsqr(((((-1.0)*r01*x964))+((r00*x963))+(((-1.0)*r02*x962))))-1) <= IKFAST_SINCOS_THRESH )
02545     continue;
02546 j7array[0]=IKatan2(((((-1.0)*r12*x962))+(((-1.0)*r11*x964))+((r10*x963))), ((((-1.0)*r01*x964))+((r00*x963))+(((-1.0)*r02*x962))));
02547 sj7array[0]=IKsin(j7array[0]);
02548 cj7array[0]=IKcos(j7array[0]);
02549 if( j7array[0] > IKPI )
02550 {
02551     j7array[0]-=IK2PI;
02552 }
02553 else if( j7array[0] < -IKPI )
02554 {    j7array[0]+=IK2PI;
02555 }
02556 j7valid[0] = true;
02557 for(int ij7 = 0; ij7 < 1; ++ij7)
02558 {
02559 if( !j7valid[ij7] )
02560 {
02561     continue;
02562 }
02563 _ij7[0] = ij7; _ij7[1] = -1;
02564 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
02565 {
02566 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
02567 {
02568     j7valid[iij7]=false; _ij7[1] = iij7; break; 
02569 }
02570 }
02571 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
02572 {
02573 IkReal evalcond[5];
02574 IkReal x965=IKcos(j7);
02575 IkReal x966=IKsin(j7);
02576 IkReal x967=(cj12*r00);
02577 IkReal x968=(r11*sj12);
02578 IkReal x969=(r01*sj12);
02579 IkReal x970=(cj12*r10);
02580 IkReal x971=((0.3215)*sj11);
02581 IkReal x972=((0.3215)*cj11);
02582 IkReal x973=((1.0)*pz);
02583 IkReal x974=((0.352)*x965);
02584 IkReal x975=((0.352)*x966);
02585 IkReal x976=(cj12*x973);
02586 evalcond[0]=(((r12*x975))+((r02*x974))+x972+(((-1.0)*r22*x973)));
02587 evalcond[1]=(((x969*x971))+(((-1.0)*x967*x971))+((r02*x972))+x974);
02588 evalcond[2]=(((x968*x971))+(((-1.0)*x970*x971))+((r12*x972))+x975);
02589 evalcond[3]=(((r10*sj12*x975))+((cj12*r01*x974))+((r00*sj12*x974))+(((-1.0)*r20*sj12*x973))+(((-1.0)*r21*x976))+((cj12*r11*x975)));
02590 evalcond[4]=((((-1.0)*r20*x976))+((pz*r21*sj12))+(((-1.0)*x969*x974))+((x967*x974))+((x970*x975))+(((-1.0)*x968*x975))+(((-1.0)*x971)));
02591 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
02592 {
02593 continue;
02594 }
02595 }
02596 
02597 rotationfunction0(solutions);
02598 }
02599 }
02600 }
02601 }
02602 
02603 }
02604 
02605 }
02606 
02607 } else
02608 {
02609 {
02610 IkReal j11array[2], cj11array[2], sj11array[2];
02611 bool j11valid[2]={false};
02612 _nj11 = 2;
02613 IkReal x977=((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)));
02614 CheckValue<IkReal> x980 = IKatan2WithCheck(IkReal(((0.3215)*r22)),IkReal(x977),IKFAST_ATAN2_MAGTHRESH);
02615 if(!x980.valid){
02616 continue;
02617 }
02618 IkReal x978=((1.0)*(x980.value));
02619 if((((((0.10336225)*(r22*r22)))+(x977*x977))) < -0.00001)
02620 continue;
02621 CheckValue<IkReal> x981=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.10336225)*(r22*r22)))+(x977*x977)))),-1);
02622 if(!x981.valid){
02623 continue;
02624 }
02625 if( ((pz*(x981.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x981.value))) > 1+IKFAST_SINCOS_THRESH )
02626     continue;
02627 IkReal x979=IKasin((pz*(x981.value)));
02628 j11array[0]=(x979+(((-1.0)*x978)));
02629 sj11array[0]=IKsin(j11array[0]);
02630 cj11array[0]=IKcos(j11array[0]);
02631 j11array[1]=((3.14159265358979)+(((-1.0)*x979))+(((-1.0)*x978)));
02632 sj11array[1]=IKsin(j11array[1]);
02633 cj11array[1]=IKcos(j11array[1]);
02634 if( j11array[0] > IKPI )
02635 {
02636     j11array[0]-=IK2PI;
02637 }
02638 else if( j11array[0] < -IKPI )
02639 {    j11array[0]+=IK2PI;
02640 }
02641 j11valid[0] = true;
02642 if( j11array[1] > IKPI )
02643 {
02644     j11array[1]-=IK2PI;
02645 }
02646 else if( j11array[1] < -IKPI )
02647 {    j11array[1]+=IK2PI;
02648 }
02649 j11valid[1] = true;
02650 for(int ij11 = 0; ij11 < 2; ++ij11)
02651 {
02652 if( !j11valid[ij11] )
02653 {
02654     continue;
02655 }
02656 _ij11[0] = ij11; _ij11[1] = -1;
02657 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
02658 {
02659 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02660 {
02661     j11valid[iij11]=false; _ij11[1] = iij11; break; 
02662 }
02663 }
02664 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02665 {
02666 IkReal evalcond[1];
02667 IkReal x982=IKsin(j11);
02668 IkReal x983=((0.643)*pz);
02669 evalcond[0]=((0.0410835)+((r21*sj12*x982*x983))+(((-1.0)*cj12*r20*x982*x983))+((r22*x983*(IKcos(j11)))));
02670 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
02671 {
02672 continue;
02673 }
02674 }
02675 
02676 {
02677 IkReal j7array[1], cj7array[1], sj7array[1];
02678 bool j7valid[1]={false};
02679 _nj7 = 1;
02680 IkReal x984=((0.913352272727273)*cj11);
02681 IkReal x985=((0.913352272727273)*cj12*sj11);
02682 IkReal x986=((0.913352272727273)*sj11*sj12);
02683 if( IKabs((((r10*x985))+(((-1.0)*r12*x984))+(((-1.0)*r11*x986)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r01*x986))+((r00*x985))+(((-1.0)*r02*x984)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((r10*x985))+(((-1.0)*r12*x984))+(((-1.0)*r11*x986))))+IKsqr(((((-1.0)*r01*x986))+((r00*x985))+(((-1.0)*r02*x984))))-1) <= IKFAST_SINCOS_THRESH )
02684     continue;
02685 j7array[0]=IKatan2((((r10*x985))+(((-1.0)*r12*x984))+(((-1.0)*r11*x986))), ((((-1.0)*r01*x986))+((r00*x985))+(((-1.0)*r02*x984))));
02686 sj7array[0]=IKsin(j7array[0]);
02687 cj7array[0]=IKcos(j7array[0]);
02688 if( j7array[0] > IKPI )
02689 {
02690     j7array[0]-=IK2PI;
02691 }
02692 else if( j7array[0] < -IKPI )
02693 {    j7array[0]+=IK2PI;
02694 }
02695 j7valid[0] = true;
02696 for(int ij7 = 0; ij7 < 1; ++ij7)
02697 {
02698 if( !j7valid[ij7] )
02699 {
02700     continue;
02701 }
02702 _ij7[0] = ij7; _ij7[1] = -1;
02703 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
02704 {
02705 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
02706 {
02707     j7valid[iij7]=false; _ij7[1] = iij7; break; 
02708 }
02709 }
02710 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
02711 {
02712 IkReal evalcond[5];
02713 IkReal x987=IKcos(j7);
02714 IkReal x988=IKsin(j7);
02715 IkReal x989=(cj12*r00);
02716 IkReal x990=(r11*sj12);
02717 IkReal x991=(r01*sj12);
02718 IkReal x992=(cj12*r10);
02719 IkReal x993=((0.3215)*sj11);
02720 IkReal x994=((0.3215)*cj11);
02721 IkReal x995=((1.0)*pz);
02722 IkReal x996=((0.352)*x987);
02723 IkReal x997=((0.352)*x988);
02724 IkReal x998=(cj12*x995);
02725 evalcond[0]=((((-1.0)*r22*x995))+((r12*x997))+((r02*x996))+x994);
02726 evalcond[1]=(((x991*x993))+((r02*x994))+(((-1.0)*x989*x993))+x996);
02727 evalcond[2]=(((r12*x994))+(((-1.0)*x992*x993))+((x990*x993))+x997);
02728 evalcond[3]=(((r00*sj12*x996))+(((-1.0)*r20*sj12*x995))+((r10*sj12*x997))+((cj12*r11*x997))+((cj12*r01*x996))+(((-1.0)*r21*x998)));
02729 evalcond[4]=(((x989*x996))+((pz*r21*sj12))+((x992*x997))+(((-1.0)*x991*x996))+(((-1.0)*x990*x997))+(((-1.0)*x993))+(((-1.0)*r20*x998)));
02730 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
02731 {
02732 continue;
02733 }
02734 }
02735 
02736 rotationfunction0(solutions);
02737 }
02738 }
02739 }
02740 }
02741 
02742 }
02743 
02744 }
02745     }
02746 
02747 }
02748 
02749 }
02750 
02751 }
02752 } while(0);
02753 if( bgotonextstatement )
02754 {
02755 bool bgotonextstatement = true;
02756 do
02757 {
02758 evalcond[0]=((IKabs(px))+(IKabs(py)));
02759 if( IKabs(evalcond[0]) < 0.0000050000000000  )
02760 {
02761 bgotonextstatement=false;
02762 {
02763 IkReal j12eval[1];
02764 IkReal x999=((1.0)*pz);
02765 IkReal x1000=((0.3215)*r22);
02766 IkReal x1001=(pz*r22);
02767 IkReal x1002=((0.643)*x1001);
02768 IkReal x1003=((0.0410835)+(((-1.0)*x1002)));
02769 IkReal x1004=(x1000+(((-1.0)*x999)));
02770 IkReal x1005=((-1.0)*pz);
02771 IkReal x1006=((0.0410835)+x1002);
02772 IkReal x1007=((((-1.0)*x1000))+(((-1.0)*x999)));
02773 px=0;
02774 py=0;
02775 pp=pz*pz;
02776 npx=(pz*r20);
02777 npy=(pz*r21);
02778 npz=x1001;
02779 rxp0_0=(pz*r10);
02780 rxp0_1=(r00*x1005);
02781 rxp0_2=0;
02782 rxp1_0=(pz*r11);
02783 rxp1_1=(r01*x1005);
02784 rxp1_2=0;
02785 rxp2_0=(pz*r12);
02786 rxp2_1=(r02*x1005);
02787 rxp2_2=0;
02788 IkReal gconst32=x1006;
02789 IkReal gconst33=x1003;
02790 IkReal gconst34=x1004;
02791 IkReal gconst35=x1007;
02792 IkReal gconst36=x1006;
02793 IkReal gconst37=x1003;
02794 IkReal gconst38=x1004;
02795 IkReal gconst39=x1007;
02796 IkReal gconst40=x1006;
02797 IkReal gconst41=x1003;
02798 IkReal gconst42=x1004;
02799 IkReal gconst43=x1007;
02800 IkReal gconst44=x1006;
02801 IkReal gconst45=x1003;
02802 IkReal gconst46=x1004;
02803 IkReal gconst47=x1007;
02804 IkReal x1008=r21*r21;
02805 IkReal x1009=pz*pz;
02806 IkReal x1010=pz*pz*pz*pz;
02807 IkReal x1011=r22*r22;
02808 IkReal x1012=r20*r20;
02809 IkReal x1013=(r20*r21);
02810 IkReal x1014=((0.271774911864)*x1009);
02811 IkReal x1015=((6.615184)*x1010);
02812 IkReal x1016=(x1009*x1012);
02813 IkReal x1017=(x1010*x1011);
02814 IkReal x1018=(x1010*x1012);
02815 IkReal x1019=(x1009*x1011);
02816 IkReal x1020=(((x1013*x1014))+((x1013*x1015))+(((0.00279136614789116)*x1013)));
02817 j12eval[0]=((IKabs(((((-0.00279136614789116)*x1011))+(((-0.00279136614789116)*x1008))+(((-1.0)*x1011*x1015))+(((-1.0)*x1011*x1014))+(((-1.0)*x1008*x1014))+(((-1.0)*x1008*x1015)))))+(((0.5)*(IKabs(((((-0.543549823728)*x1008*x1009))+(((-0.00418704922183674)*x1011))+(((-9.922776)*x1017))+(((-0.00558273229578232)*x1008))+(((0.135887455932)*x1016))+(((3.307592)*x1018))+(((-13.230368)*x1008*x1010))+(((-0.407662367796)*x1019))+(((0.00139568307394558)*x1012)))))))+(((0.5)*(IKabs(((((-0.00069784153697279)*x1012))+(((-0.00069784153697279)*x1011))+(((-0.067943727966)*x1019))+(((-0.067943727966)*x1016))+(((-1.653796)*x1018))+(((-1.653796)*x1017)))))))+(((2.0)*(IKabs(x1020)))));
02818 if( IKabs(j12eval[0]) < 0.0000000010000000  )
02819 {
02820 continue; // no branches [j7, j11, j12]
02821 
02822 } else
02823 {
02824 IkReal op[8+1], zeror[8];
02825 int numroots;
02826 IkReal x1021=r20*r20;
02827 IkReal x1022=pz*pz;
02828 IkReal x1023=r21*r21;
02829 IkReal x1024=(r20*r21);
02830 IkReal x1025=(gconst41*gconst44);
02831 IkReal x1026=(gconst32*gconst36);
02832 IkReal x1027=((3.307592)*gconst43);
02833 IkReal x1028=(gconst34*gconst39);
02834 IkReal x1029=(gconst33*gconst36);
02835 IkReal x1030=(gconst35*gconst40);
02836 IkReal x1031=(gconst45*gconst46);
02837 IkReal x1032=(gconst40*gconst44);
02838 IkReal x1033=(gconst33*gconst46);
02839 IkReal x1034=((1.0)*gconst36);
02840 IkReal x1035=((1.653796)*gconst35);
02841 IkReal x1036=(gconst37*gconst38);
02842 IkReal x1037=(gconst32*gconst43);
02843 IkReal x1038=((0.826898)*gconst41);
02844 IkReal x1039=(gconst35*gconst36);
02845 IkReal x1040=(gconst39*gconst42);
02846 IkReal x1041=((1.0)*gconst42);
02847 IkReal x1042=(gconst37*gconst46);
02848 IkReal x1043=(gconst36*gconst43);
02849 IkReal x1044=((1.0)*gconst34);
02850 IkReal x1045=(gconst38*gconst41);
02851 IkReal x1046=(gconst33*gconst38);
02852 IkReal x1047=(gconst32*gconst35);
02853 IkReal x1048=((1.0)*gconst39);
02854 IkReal x1049=(gconst38*gconst45);
02855 IkReal x1050=(gconst40*gconst43);
02856 IkReal x1051=(gconst33*gconst44);
02857 IkReal x1052=(gconst34*gconst47);
02858 IkReal x1053=((1.0)*gconst47);
02859 IkReal x1054=(gconst36*gconst41);
02860 IkReal x1055=(gconst35*gconst44);
02861 IkReal x1056=((1.653796)*gconst43);
02862 IkReal x1057=(gconst41*gconst46);
02863 IkReal x1058=((3.307592)*gconst35);
02864 IkReal x1059=(gconst42*gconst47);
02865 IkReal x1060=((1.653796)*x1023);
02866 IkReal x1061=(gconst33*x1059);
02867 IkReal x1062=(gconst43*x1048);
02868 IkReal x1063=(pz*x1021);
02869 IkReal x1064=(gconst36*x1021);
02870 IkReal x1065=((0.826898)*x1021);
02871 IkReal x1066=(gconst46*x1022);
02872 IkReal x1067=((6.615184)*x1023);
02873 IkReal x1068=((0.413449)*x1021);
02874 IkReal x1069=(pz*x1023);
02875 IkReal x1070=((3.307592)*x1069);
02876 IkReal x1071=(x1025*x1068);
02877 IkReal x1072=(x1051*x1068);
02878 IkReal x1073=(x1054*x1068);
02879 IkReal x1074=(x1029*x1068);
02880 IkReal x1075=(gconst38*x1021*x1022);
02881 IkReal x1076=(gconst46*x1038*x1063);
02882 IkReal x1077=((0.826898)*gconst43*gconst44*x1063);
02883 IkReal x1078=(gconst38*x1038*x1063);
02884 IkReal x1079=((0.826898)*x1043*x1063);
02885 IkReal x1080=((0.826898)*x1033*x1063);
02886 IkReal x1081=((0.826898)*x1055*x1063);
02887 IkReal x1082=((0.826898)*x1039*x1063);
02888 IkReal x1083=((0.826898)*x1046*x1063);
02889 IkReal x1084=(gconst38*x1022*x1067);
02890 IkReal x1085=(x1021*x1056*x1066);
02891 IkReal x1086=(x1056*x1075);
02892 IkReal x1087=(x1021*x1035*x1066);
02893 IkReal x1088=(x1035*x1075);
02894 IkReal x1089=(x1076+x1077);
02895 IkReal x1090=(x1083+x1082);
02896 IkReal x1091=(x1071+x1085);
02897 IkReal x1092=(x1074+x1088);
02898 IkReal x1093=((((-0.271774911864)*x1022*x1024))+(((-6.615184)*x1024*(x1022*x1022)))+(((-0.00279136614789116)*x1024)));
02899 IkReal x1094=((((0.271774911864)*x1022*x1024))+(((0.00279136614789116)*x1024))+(((6.615184)*x1024*(x1022*x1022))));
02900 IkReal x1095=(x1078+x1079+x1081+x1080);
02901 IkReal x1096=(x1072+x1073+x1087+x1086);
02902 op[0]=(x1089+(((-1.0)*gconst41*x1031*x1041))+((x1025*x1059))+(((-1.0)*x1091))+(((-1.0)*gconst43*x1032*x1053))+((x1031*x1050)));
02903 op[1]=x1093;
02904 op[2]=(((x1031*x1037))+((x1030*x1031))+((x1054*x1059))+x1095+(((-1.653796)*x1057*x1063))+(((-1.0)*gconst45*x1041*x1045))+(((-1.0)*gconst43*x1066*x1067))+((gconst44*x1027*x1069))+((x1042*x1050))+(((-1.0)*gconst33*x1031*x1041))+(((-1.0)*gconst41*x1031*x1044))+((x1025*x1052))+(((-1.0)*gconst44*x1056*x1063))+((x1025*x1040))+(((-1.0)*x1096))+(((-1.0)*x1025*x1060))+((x1025*x1065))+(((-1.0)*gconst44*x1030*x1053))+((x1057*x1070))+((x1051*x1059))+(((-1.0)*gconst47*x1034*x1050))+((x1021*x1027*x1066))+(((-1.0)*gconst41*x1041*x1042))+((x1049*x1050))+(((-1.0)*x1032*x1062))+(((-1.0)*gconst44*x1037*x1053)));
02905 op[3]=x1093;
02906 op[4]=((((-1.0)*gconst44*x1037*x1048))+((x1055*x1070))+((x1033*x1070))+(((-1.0)*gconst43*x1026*x1053))+(((-1.653796)*x1043*x1063))+x1090+x1089+(((-1.0)*gconst47*x1030*x1034))+(((-1.0)*gconst45*x1041*x1046))+((x1021*x1058*x1066))+((x1037*x1042))+((x1037*x1049))+(((-1.0)*gconst44*x1035*x1063))+(((-1.0)*gconst33*x1031*x1044))+((x1038*x1064))+(((-1.0)*gconst39*x1034*x1050))+((x1051*x1065))+((x1040*x1054))+((x1040*x1051))+(((-1.0)*gconst43*x1084))+((gconst36*x1027*x1069))+(((-1.0)*x1091))+(((-1.0)*x1092))+((x1052*x1054))+(((-1.0)*gconst44*x1030*x1048))+(((-1.653796)*x1045*x1063))+(((-1.0)*gconst35*x1066*x1067))+(((-1.0)*gconst41*x1036*x1041))+((x1027*x1075))+((x1031*x1047))+((x1051*x1052))+((x1030*x1042))+((x1030*x1049))+((x1036*x1050))+((x1029*x1059))+(((-1.0)*gconst45*x1044*x1045))+(((-1.0)*gconst44*x1047*x1053))+(((-1.0)*gconst37*x1033*x1041))+((x1025*x1028))+(((-1.0)*x1054*x1060))+((x1045*x1070))+(((-1.0)*x1051*x1060))+(((-1.653796)*x1033*x1063))+(((-1.0)*gconst41*x1042*x1044)));
02907 op[5]=x1094;
02908 op[6]=(((x1036*x1037))+((x1058*x1075))+((x1039*x1070))+((x1030*x1036))+(((-1.653796)*x1046*x1063))+x1095+((x1042*x1047))+(((-1.0)*gconst35*x1026*x1053))+(((-1.0)*x1096))+(((-1.0)*gconst33*x1036*x1041))+(((-1.0)*gconst39*x1030*x1034))+(((-1.0)*gconst35*x1084))+(((-1.0)*gconst41*x1036*x1044))+(((-1.0)*x1029*x1060))+((x1029*x1052))+(((-1.0)*gconst45*x1044*x1046))+((x1046*x1070))+((x1028*x1051))+((x1028*x1054))+(((-1.0)*gconst37*x1033*x1044))+((x1047*x1049))+((x1029*x1040))+(((-1.0)*gconst44*x1047*x1048))+(((-1.0)*gconst36*x1035*x1063))+((x1029*x1065))+(((-1.0)*x1026*x1062)));
02909 op[7]=x1094;
02910 op[8]=(x1090+((x1028*x1029))+(((-1.0)*x1092))+(((-1.0)*gconst33*x1036*x1044))+(((-1.0)*gconst35*x1026*x1048))+((x1036*x1047)));
02911 polyroots8(op,zeror,numroots);
02912 IkReal j12array[8], cj12array[8], sj12array[8], tempj12array[1];
02913 int numsolutions = 0;
02914 for(int ij12 = 0; ij12 < numroots; ++ij12)
02915 {
02916 IkReal htj12 = zeror[ij12];
02917 tempj12array[0]=((2.0)*(atan(htj12)));
02918 for(int kj12 = 0; kj12 < 1; ++kj12)
02919 {
02920 j12array[numsolutions] = tempj12array[kj12];
02921 if( j12array[numsolutions] > IKPI )
02922 {
02923     j12array[numsolutions]-=IK2PI;
02924 }
02925 else if( j12array[numsolutions] < -IKPI )
02926 {
02927     j12array[numsolutions]+=IK2PI;
02928 }
02929 sj12array[numsolutions] = IKsin(j12array[numsolutions]);
02930 cj12array[numsolutions] = IKcos(j12array[numsolutions]);
02931 numsolutions++;
02932 }
02933 }
02934 bool j12valid[8]={true,true,true,true,true,true,true,true};
02935 _nj12 = 8;
02936 for(int ij12 = 0; ij12 < numsolutions; ++ij12)
02937     {
02938 if( !j12valid[ij12] )
02939 {
02940     continue;
02941 }
02942     j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
02943 htj12 = IKtan(j12/2);
02944 
02945 _ij12[0] = ij12; _ij12[1] = -1;
02946 for(int iij12 = ij12+1; iij12 < numsolutions; ++iij12)
02947 {
02948 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
02949 {
02950     j12valid[iij12]=false; _ij12[1] = iij12; break; 
02951 }
02952 }
02953 {
02954 IkReal j11eval[2];
02955 IkReal x1097=((-1.0)*pz);
02956 px=0;
02957 py=0;
02958 pp=pz*pz;
02959 npx=(pz*r20);
02960 npy=(pz*r21);
02961 npz=(pz*r22);
02962 rxp0_0=(pz*r10);
02963 rxp0_1=(r00*x1097);
02964 rxp0_2=0;
02965 rxp1_0=(pz*r11);
02966 rxp1_1=(r01*x1097);
02967 rxp1_2=0;
02968 rxp2_0=(pz*r12);
02969 rxp2_1=(r02*x1097);
02970 rxp2_2=0;
02971 IkReal x1098=(cj12*r20);
02972 IkReal x1099=(r21*sj12);
02973 j11eval[0]=((r22*r22)+(x1098*x1098)+(x1099*x1099)+(((-2.0)*x1098*x1099)));
02974 j11eval[1]=((IKabs(r22))+(((3.1104199066874)*(IKabs(((((0.3215)*x1099))+(((-0.3215)*x1098))))))));
02975 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  )
02976 {
02977 {
02978 IkReal j11eval[2];
02979 IkReal x1100=((-1.0)*pz);
02980 px=0;
02981 py=0;
02982 pp=pz*pz;
02983 npx=(pz*r20);
02984 npy=(pz*r21);
02985 npz=(pz*r22);
02986 rxp0_0=(pz*r10);
02987 rxp0_1=(r00*x1100);
02988 rxp0_2=0;
02989 rxp1_0=(pz*r11);
02990 rxp1_1=(r01*x1100);
02991 rxp1_2=0;
02992 rxp2_0=(pz*r12);
02993 rxp2_1=(r02*x1100);
02994 rxp2_2=0;
02995 IkReal x1101=pz*pz;
02996 IkReal x1102=(r21*sj12);
02997 IkReal x1103=(cj12*r20);
02998 j11eval[0]=((IKabs((pz*r22)))+(IKabs((((pz*x1102))+(((-1.0)*pz*x1103))))));
02999 j11eval[1]=(((x1101*(x1103*x1103)))+((x1101*(x1102*x1102)))+(((-2.0)*x1101*x1102*x1103))+((x1101*(r22*r22))));
03000 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  )
03001 {
03002 {
03003 IkReal j7eval[2];
03004 IkReal x1104=((-1.0)*pz);
03005 px=0;
03006 py=0;
03007 pp=pz*pz;
03008 npx=(pz*r20);
03009 npy=(pz*r21);
03010 npz=(pz*r22);
03011 rxp0_0=(pz*r10);
03012 rxp0_1=(r00*x1104);
03013 rxp0_2=0;
03014 rxp1_0=(pz*r11);
03015 rxp1_1=(r01*x1104);
03016 rxp1_2=0;
03017 rxp2_0=(pz*r12);
03018 rxp2_1=(r02*x1104);
03019 rxp2_2=0;
03020 IkReal x1105=sj12*sj12;
03021 IkReal x1106=cj12*cj12;
03022 IkReal x1107=(r10*sj12);
03023 IkReal x1108=(cj12*r11);
03024 IkReal x1109=(cj12*r01);
03025 IkReal x1110=(r00*sj12);
03026 j7eval[0]=(((x1106*(r11*r11)))+((x1106*(r01*r01)))+((x1105*(r10*r10)))+(((2.0)*x1109*x1110))+(((2.0)*x1107*x1108))+((x1105*(r00*r00))));
03027 j7eval[1]=((IKabs(((((0.352)*x1108))+(((0.352)*x1107)))))+(IKabs(((((0.352)*x1110))+(((0.352)*x1109))))));
03028 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  )
03029 {
03030 {
03031 IkReal evalcond[1];
03032 bool bgotonextstatement = true;
03033 do
03034 {
03035 evalcond[0]=IKabs(pz);
03036 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03037 {
03038 bgotonextstatement=false;
03039 {
03040 IkReal j11eval[1];
03041 px=0;
03042 py=0;
03043 pp=0;
03044 npx=0;
03045 npy=0;
03046 npz=0;
03047 rxp0_0=0;
03048 rxp0_1=0;
03049 rxp0_2=0;
03050 rxp1_0=0;
03051 rxp1_1=0;
03052 rxp1_2=0;
03053 rxp2_0=0;
03054 rxp2_1=0;
03055 rxp2_2=0;
03056 pz=0;
03057 j11eval[0]=((((3.1104199066874)*(IKabs(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)))))))+(IKabs(r22)));
03058 if( IKabs(j11eval[0]) < 0.0000010000000000  )
03059 {
03060 {
03061 IkReal j7eval[1];
03062 px=0;
03063 py=0;
03064 pp=0;
03065 npx=0;
03066 npy=0;
03067 npz=0;
03068 rxp0_0=0;
03069 rxp0_1=0;
03070 rxp0_2=0;
03071 rxp1_0=0;
03072 rxp1_1=0;
03073 rxp1_2=0;
03074 rxp2_0=0;
03075 rxp2_1=0;
03076 rxp2_2=0;
03077 pz=0;
03078 IkReal x1111=((0.352)*sj12);
03079 IkReal x1112=((0.352)*cj12);
03080 j7eval[0]=((IKabs((((r11*x1112))+((r10*x1111)))))+(IKabs((((r01*x1112))+((r00*x1111))))));
03081 if( IKabs(j7eval[0]) < 0.0000010000000000  )
03082 {
03083 continue; // no branches [j7, j11]
03084 
03085 } else
03086 {
03087 {
03088 IkReal j7array[2], cj7array[2], sj7array[2];
03089 bool j7valid[2]={false};
03090 _nj7 = 2;
03091 IkReal x1113=((0.352)*cj12);
03092 IkReal x1114=((0.352)*sj12);
03093 CheckValue<IkReal> x1116 = IKatan2WithCheck(IkReal((((r01*x1113))+((r00*x1114)))),IkReal((((r11*x1113))+((r10*x1114)))),IKFAST_ATAN2_MAGTHRESH);
03094 if(!x1116.valid){
03095 continue;
03096 }
03097 IkReal x1115=x1116.value;
03098 j7array[0]=((-1.0)*x1115);
03099 sj7array[0]=IKsin(j7array[0]);
03100 cj7array[0]=IKcos(j7array[0]);
03101 j7array[1]=((3.14159265358979)+(((-1.0)*x1115)));
03102 sj7array[1]=IKsin(j7array[1]);
03103 cj7array[1]=IKcos(j7array[1]);
03104 if( j7array[0] > IKPI )
03105 {
03106     j7array[0]-=IK2PI;
03107 }
03108 else if( j7array[0] < -IKPI )
03109 {    j7array[0]+=IK2PI;
03110 }
03111 j7valid[0] = true;
03112 if( j7array[1] > IKPI )
03113 {
03114     j7array[1]-=IK2PI;
03115 }
03116 else if( j7array[1] < -IKPI )
03117 {    j7array[1]+=IK2PI;
03118 }
03119 j7valid[1] = true;
03120 for(int ij7 = 0; ij7 < 2; ++ij7)
03121 {
03122 if( !j7valid[ij7] )
03123 {
03124     continue;
03125 }
03126 _ij7[0] = ij7; _ij7[1] = -1;
03127 for(int iij7 = ij7+1; iij7 < 2; ++iij7)
03128 {
03129 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
03130 {
03131     j7valid[iij7]=false; _ij7[1] = iij7; break; 
03132 }
03133 }
03134 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
03135 
03136 {
03137 IkReal j11eval[1];
03138 px=0;
03139 py=0;
03140 pp=0;
03141 npx=0;
03142 npy=0;
03143 npz=0;
03144 rxp0_0=0;
03145 rxp0_1=0;
03146 rxp0_2=0;
03147 rxp1_0=0;
03148 rxp1_1=0;
03149 rxp1_2=0;
03150 rxp2_0=0;
03151 rxp2_1=0;
03152 rxp2_2=0;
03153 pz=0;
03154 j11eval[0]=((((-1.0)*cj12*r20))+((r21*sj12)));
03155 if( IKabs(j11eval[0]) < 0.0000010000000000  )
03156 {
03157 {
03158 IkReal j11eval[1];
03159 px=0;
03160 py=0;
03161 pp=0;
03162 npx=0;
03163 npy=0;
03164 npz=0;
03165 rxp0_0=0;
03166 rxp0_1=0;
03167 rxp0_2=0;
03168 rxp1_0=0;
03169 rxp1_1=0;
03170 rxp1_2=0;
03171 rxp2_0=0;
03172 rxp2_1=0;
03173 rxp2_2=0;
03174 pz=0;
03175 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
03176 if( IKabs(j11eval[0]) < 0.0000010000000000  )
03177 {
03178 {
03179 IkReal j11eval[1];
03180 px=0;
03181 py=0;
03182 pp=0;
03183 npx=0;
03184 npy=0;
03185 npz=0;
03186 rxp0_0=0;
03187 rxp0_1=0;
03188 rxp0_2=0;
03189 rxp1_0=0;
03190 rxp1_1=0;
03191 rxp1_2=0;
03192 rxp2_0=0;
03193 rxp2_1=0;
03194 rxp2_2=0;
03195 pz=0;
03196 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
03197 if( IKabs(j11eval[0]) < 0.0000010000000000  )
03198 {
03199 {
03200 IkReal evalcond[1];
03201 bool bgotonextstatement = true;
03202 do
03203 {
03204 evalcond[0]=((IKabs(r10))+(IKabs(r11)));
03205 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03206 {
03207 bgotonextstatement=false;
03208 {
03209 IkReal j11array[2], cj11array[2], sj11array[2];
03210 bool j11valid[2]={false};
03211 _nj11 = 2;
03212 cj11array[0]=((-1.09486780715397)*r12*sj7);
03213 if( cj11array[0] >= -1-IKFAST_SINCOS_THRESH && cj11array[0] <= 1+IKFAST_SINCOS_THRESH )
03214 {
03215     j11valid[0] = j11valid[1] = true;
03216     j11array[0] = IKacos(cj11array[0]);
03217     sj11array[0] = IKsin(j11array[0]);
03218     cj11array[1] = cj11array[0];
03219     j11array[1] = -j11array[0];
03220     sj11array[1] = -sj11array[0];
03221 }
03222 else if( isnan(cj11array[0]) )
03223 {
03224     // probably any value will work
03225     j11valid[0] = true;
03226     cj11array[0] = 1; sj11array[0] = 0; j11array[0] = 0;
03227 }
03228 for(int ij11 = 0; ij11 < 2; ++ij11)
03229 {
03230 if( !j11valid[ij11] )
03231 {
03232     continue;
03233 }
03234 _ij11[0] = ij11; _ij11[1] = -1;
03235 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
03236 {
03237 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03238 {
03239     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03240 }
03241 }
03242 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03243 {
03244 IkReal evalcond[4];
03245 IkReal x1117=IKsin(j11);
03246 IkReal x1118=((0.3215)*x1117);
03247 evalcond[0]=((-0.3215)*x1117);
03248 evalcond[1]=((((0.352)*sj7))+(((0.3215)*r12*(IKcos(j11)))));
03249 evalcond[2]=((((-1.0)*cj12*r20*x1118))+((r21*sj12*x1118)));
03250 evalcond[3]=(((r01*sj12*x1118))+(((-1.0)*cj12*r00*x1118)));
03251 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
03252 {
03253 continue;
03254 }
03255 }
03256 
03257 rotationfunction0(solutions);
03258 }
03259 }
03260 
03261 }
03262 } while(0);
03263 if( bgotonextstatement )
03264 {
03265 bool bgotonextstatement = true;
03266 do
03267 {
03268 evalcond[0]=((IKabs(r21))+(IKabs(r20)));
03269 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03270 {
03271 bgotonextstatement=false;
03272 {
03273 IkReal j11array[2], cj11array[2], sj11array[2];
03274 bool j11valid[2]={false};
03275 _nj11 = 2;
03276 IkReal x1119=((1.09486780715397)*cj12);
03277 IkReal x1120=((1.09486780715397)*sj12);
03278 sj11array[0]=((((-1.0)*r11*sj7*x1120))+((r10*sj7*x1119))+(((-1.0)*cj7*r01*x1120))+((cj7*r00*x1119)));
03279 if( sj11array[0] >= -1-IKFAST_SINCOS_THRESH && sj11array[0] <= 1+IKFAST_SINCOS_THRESH )
03280 {
03281     j11valid[0] = j11valid[1] = true;
03282     j11array[0] = IKasin(sj11array[0]);
03283     cj11array[0] = IKcos(j11array[0]);
03284     sj11array[1] = sj11array[0];
03285     j11array[1] = j11array[0] > 0 ? (IKPI-j11array[0]) : (-IKPI-j11array[0]);
03286     cj11array[1] = -cj11array[0];
03287 }
03288 else if( isnan(sj11array[0]) )
03289 {
03290     // probably any value will work
03291     j11valid[0] = true;
03292     cj11array[0] = 1; sj11array[0] = 0; j11array[0] = 0;
03293 }
03294 for(int ij11 = 0; ij11 < 2; ++ij11)
03295 {
03296 if( !j11valid[ij11] )
03297 {
03298     continue;
03299 }
03300 _ij11[0] = ij11; _ij11[1] = -1;
03301 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
03302 {
03303 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03304 {
03305     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03306 }
03307 }
03308 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03309 {
03310 IkReal evalcond[4];
03311 IkReal x1121=IKcos(j11);
03312 IkReal x1122=IKsin(j11);
03313 IkReal x1123=((0.3215)*x1122);
03314 IkReal x1124=((0.3215)*x1121);
03315 evalcond[0]=x1124;
03316 evalcond[1]=(r22*x1124);
03317 evalcond[2]=((((-1.0)*cj12*r00*x1123))+((r01*sj12*x1123))+(((0.352)*cj7)));
03318 evalcond[3]=((((-1.0)*cj12*r10*x1123))+((r11*sj12*x1123))+(((0.352)*sj7)));
03319 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
03320 {
03321 continue;
03322 }
03323 }
03324 
03325 rotationfunction0(solutions);
03326 }
03327 }
03328 
03329 }
03330 } while(0);
03331 if( bgotonextstatement )
03332 {
03333 bool bgotonextstatement = true;
03334 do
03335 {
03336 if( 1 )
03337 {
03338 bgotonextstatement=false;
03339 continue; // branch miss [j11]
03340 
03341 }
03342 } while(0);
03343 if( bgotonextstatement )
03344 {
03345 }
03346 }
03347 }
03348 }
03349 
03350 } else
03351 {
03352 {
03353 IkReal j11array[1], cj11array[1], sj11array[1];
03354 bool j11valid[1]={false};
03355 _nj11 = 1;
03356 IkReal x1125=((44.0)*sj7);
03357 IkReal x1126=(cj7*r02);
03358 CheckValue<IkReal> x1127=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
03359 if(!x1127.valid){
03360 continue;
03361 }
03362 if( IKabs(((x1127.value)*(((((-1.0)*x1125))+((x1125*(r12*r12)))+(((44.0)*r12*x1126)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1126))+(((-1.09486780715397)*r12*sj7)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1127.value)*(((((-1.0)*x1125))+((x1125*(r12*r12)))+(((44.0)*r12*x1126))))))+IKsqr(((((-1.09486780715397)*x1126))+(((-1.09486780715397)*r12*sj7))))-1) <= IKFAST_SINCOS_THRESH )
03363     continue;
03364 j11array[0]=IKatan2(((x1127.value)*(((((-1.0)*x1125))+((x1125*(r12*r12)))+(((44.0)*r12*x1126))))), ((((-1.09486780715397)*x1126))+(((-1.09486780715397)*r12*sj7))));
03365 sj11array[0]=IKsin(j11array[0]);
03366 cj11array[0]=IKcos(j11array[0]);
03367 if( j11array[0] > IKPI )
03368 {
03369     j11array[0]-=IK2PI;
03370 }
03371 else if( j11array[0] < -IKPI )
03372 {    j11array[0]+=IK2PI;
03373 }
03374 j11valid[0] = true;
03375 for(int ij11 = 0; ij11 < 1; ++ij11)
03376 {
03377 if( !j11valid[ij11] )
03378 {
03379     continue;
03380 }
03381 _ij11[0] = ij11; _ij11[1] = -1;
03382 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
03383 {
03384 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03385 {
03386     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03387 }
03388 }
03389 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03390 {
03391 IkReal evalcond[5];
03392 IkReal x1128=IKcos(j11);
03393 IkReal x1129=IKsin(j11);
03394 IkReal x1130=(r01*sj12);
03395 IkReal x1131=((0.352)*cj7);
03396 IkReal x1132=(cj12*r10);
03397 IkReal x1133=(r11*sj12);
03398 IkReal x1134=(cj12*r00);
03399 IkReal x1135=((0.352)*sj7);
03400 IkReal x1136=((0.3215)*x1129);
03401 IkReal x1137=((0.3215)*x1128);
03402 evalcond[0]=(x1137+((r12*x1135))+((r02*x1131)));
03403 evalcond[1]=(((r22*x1137))+((r21*sj12*x1136))+(((-1.0)*cj12*r20*x1136)));
03404 evalcond[2]=(x1131+((x1130*x1136))+(((-1.0)*x1134*x1136))+((r02*x1137)));
03405 evalcond[3]=(x1135+(((-1.0)*x1132*x1136))+((r12*x1137))+((x1133*x1136)));
03406 evalcond[4]=(((x1131*x1134))+(((-1.0)*x1130*x1131))+(((-1.0)*x1136))+(((-1.0)*x1133*x1135))+((x1132*x1135)));
03407 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03408 {
03409 continue;
03410 }
03411 }
03412 
03413 rotationfunction0(solutions);
03414 }
03415 }
03416 
03417 }
03418 
03419 }
03420 
03421 } else
03422 {
03423 {
03424 IkReal j11array[1], cj11array[1], sj11array[1];
03425 bool j11valid[1]={false};
03426 _nj11 = 1;
03427 IkReal x1138=(r12*sj7);
03428 IkReal x1139=((44.0)*cj7);
03429 CheckValue<IkReal> x1140=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
03430 if(!x1140.valid){
03431 continue;
03432 }
03433 if( IKabs(((x1140.value)*((((x1139*(r02*r02)))+(((44.0)*r02*x1138))+(((-1.0)*x1139)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1138))+(((-1.09486780715397)*cj7*r02)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1140.value)*((((x1139*(r02*r02)))+(((44.0)*r02*x1138))+(((-1.0)*x1139))))))+IKsqr(((((-1.09486780715397)*x1138))+(((-1.09486780715397)*cj7*r02))))-1) <= IKFAST_SINCOS_THRESH )
03434     continue;
03435 j11array[0]=IKatan2(((x1140.value)*((((x1139*(r02*r02)))+(((44.0)*r02*x1138))+(((-1.0)*x1139))))), ((((-1.09486780715397)*x1138))+(((-1.09486780715397)*cj7*r02))));
03436 sj11array[0]=IKsin(j11array[0]);
03437 cj11array[0]=IKcos(j11array[0]);
03438 if( j11array[0] > IKPI )
03439 {
03440     j11array[0]-=IK2PI;
03441 }
03442 else if( j11array[0] < -IKPI )
03443 {    j11array[0]+=IK2PI;
03444 }
03445 j11valid[0] = true;
03446 for(int ij11 = 0; ij11 < 1; ++ij11)
03447 {
03448 if( !j11valid[ij11] )
03449 {
03450     continue;
03451 }
03452 _ij11[0] = ij11; _ij11[1] = -1;
03453 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
03454 {
03455 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03456 {
03457     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03458 }
03459 }
03460 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03461 {
03462 IkReal evalcond[5];
03463 IkReal x1141=IKcos(j11);
03464 IkReal x1142=IKsin(j11);
03465 IkReal x1143=(r01*sj12);
03466 IkReal x1144=((0.352)*cj7);
03467 IkReal x1145=(cj12*r10);
03468 IkReal x1146=(r11*sj12);
03469 IkReal x1147=(cj12*r00);
03470 IkReal x1148=((0.352)*sj7);
03471 IkReal x1149=((0.3215)*x1142);
03472 IkReal x1150=((0.3215)*x1141);
03473 evalcond[0]=(x1150+((r02*x1144))+((r12*x1148)));
03474 evalcond[1]=(((r22*x1150))+(((-1.0)*cj12*r20*x1149))+((r21*sj12*x1149)));
03475 evalcond[2]=(x1144+((x1143*x1149))+((r02*x1150))+(((-1.0)*x1147*x1149)));
03476 evalcond[3]=((((-1.0)*x1145*x1149))+x1148+((r12*x1150))+((x1146*x1149)));
03477 evalcond[4]=(((x1145*x1148))+(((-1.0)*x1143*x1144))+(((-1.0)*x1149))+(((-1.0)*x1146*x1148))+((x1144*x1147)));
03478 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03479 {
03480 continue;
03481 }
03482 }
03483 
03484 rotationfunction0(solutions);
03485 }
03486 }
03487 
03488 }
03489 
03490 }
03491 
03492 } else
03493 {
03494 {
03495 IkReal j11array[1], cj11array[1], sj11array[1];
03496 bool j11valid[1]={false};
03497 _nj11 = 1;
03498 IkReal x1151=((0.352)*r22);
03499 IkReal x1152=(r12*sj7);
03500 IkReal x1153=(cj7*r02);
03501 CheckValue<IkReal> x1154=IKPowWithIntegerCheck(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12))),-1);
03502 if(!x1154.valid){
03503 continue;
03504 }
03505 if( IKabs(((x1154.value)*((((x1151*x1152))+((x1151*x1153)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1152))+(((-1.09486780715397)*x1153)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1154.value)*((((x1151*x1152))+((x1151*x1153))))))+IKsqr(((((-1.09486780715397)*x1152))+(((-1.09486780715397)*x1153))))-1) <= IKFAST_SINCOS_THRESH )
03506     continue;
03507 j11array[0]=IKatan2(((x1154.value)*((((x1151*x1152))+((x1151*x1153))))), ((((-1.09486780715397)*x1152))+(((-1.09486780715397)*x1153))));
03508 sj11array[0]=IKsin(j11array[0]);
03509 cj11array[0]=IKcos(j11array[0]);
03510 if( j11array[0] > IKPI )
03511 {
03512     j11array[0]-=IK2PI;
03513 }
03514 else if( j11array[0] < -IKPI )
03515 {    j11array[0]+=IK2PI;
03516 }
03517 j11valid[0] = true;
03518 for(int ij11 = 0; ij11 < 1; ++ij11)
03519 {
03520 if( !j11valid[ij11] )
03521 {
03522     continue;
03523 }
03524 _ij11[0] = ij11; _ij11[1] = -1;
03525 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
03526 {
03527 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03528 {
03529     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03530 }
03531 }
03532 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03533 {
03534 IkReal evalcond[5];
03535 IkReal x1155=IKcos(j11);
03536 IkReal x1156=IKsin(j11);
03537 IkReal x1157=(r01*sj12);
03538 IkReal x1158=((0.352)*cj7);
03539 IkReal x1159=(cj12*r10);
03540 IkReal x1160=(r11*sj12);
03541 IkReal x1161=(cj12*r00);
03542 IkReal x1162=((0.352)*sj7);
03543 IkReal x1163=((0.3215)*x1156);
03544 IkReal x1164=((0.3215)*x1155);
03545 evalcond[0]=(x1164+((r02*x1158))+((r12*x1162)));
03546 evalcond[1]=(((r21*sj12*x1163))+((r22*x1164))+(((-1.0)*cj12*r20*x1163)));
03547 evalcond[2]=(x1158+((r02*x1164))+((x1157*x1163))+(((-1.0)*x1161*x1163)));
03548 evalcond[3]=(x1162+(((-1.0)*x1159*x1163))+((r12*x1164))+((x1160*x1163)));
03549 evalcond[4]=(((x1159*x1162))+(((-1.0)*x1157*x1158))+((x1158*x1161))+(((-1.0)*x1160*x1162))+(((-1.0)*x1163)));
03550 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03551 {
03552 continue;
03553 }
03554 }
03555 
03556 rotationfunction0(solutions);
03557 }
03558 }
03559 
03560 }
03561 
03562 }
03563 }
03564 }
03565 
03566 }
03567 
03568 }
03569 
03570 } else
03571 {
03572 {
03573 IkReal j11array[2], cj11array[2], sj11array[2];
03574 bool j11valid[2]={false};
03575 _nj11 = 2;
03576 CheckValue<IkReal> x1166 = IKatan2WithCheck(IkReal(((0.3215)*r22)),IkReal(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)))),IKFAST_ATAN2_MAGTHRESH);
03577 if(!x1166.valid){
03578 continue;
03579 }
03580 IkReal x1165=x1166.value;
03581 j11array[0]=((-1.0)*x1165);
03582 sj11array[0]=IKsin(j11array[0]);
03583 cj11array[0]=IKcos(j11array[0]);
03584 j11array[1]=((3.14159265358979)+(((-1.0)*x1165)));
03585 sj11array[1]=IKsin(j11array[1]);
03586 cj11array[1]=IKcos(j11array[1]);
03587 if( j11array[0] > IKPI )
03588 {
03589     j11array[0]-=IK2PI;
03590 }
03591 else if( j11array[0] < -IKPI )
03592 {    j11array[0]+=IK2PI;
03593 }
03594 j11valid[0] = true;
03595 if( j11array[1] > IKPI )
03596 {
03597     j11array[1]-=IK2PI;
03598 }
03599 else if( j11array[1] < -IKPI )
03600 {    j11array[1]+=IK2PI;
03601 }
03602 j11valid[1] = true;
03603 for(int ij11 = 0; ij11 < 2; ++ij11)
03604 {
03605 if( !j11valid[ij11] )
03606 {
03607     continue;
03608 }
03609 _ij11[0] = ij11; _ij11[1] = -1;
03610 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
03611 {
03612 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
03613 {
03614     j11valid[iij11]=false; _ij11[1] = iij11; break; 
03615 }
03616 }
03617 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
03618 
03619 {
03620 IkReal j7eval[3];
03621 px=0;
03622 py=0;
03623 pp=0;
03624 npx=0;
03625 npy=0;
03626 npz=0;
03627 rxp0_0=0;
03628 rxp0_1=0;
03629 rxp0_2=0;
03630 rxp1_0=0;
03631 rxp1_1=0;
03632 rxp1_2=0;
03633 rxp2_0=0;
03634 rxp2_1=0;
03635 rxp2_2=0;
03636 pz=0;
03637 IkReal x1167=(r21*sj12);
03638 IkReal x1168=((1.0)*cj12);
03639 IkReal x1169=(cj11*sj12);
03640 j7eval[0]=((((-1.0)*r20*x1168))+x1167);
03641 j7eval[1]=((IKabs((((r00*x1169))+((cj11*cj12*r01)))))+(IKabs(((((-1.0)*cj11*r11*x1168))+(((-1.0)*r10*x1169))))));
03642 j7eval[2]=IKsign(((((247.808)*x1167))+(((-247.808)*cj12*r20))));
03643 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  || IKabs(j7eval[2]) < 0.0000010000000000  )
03644 {
03645 {
03646 IkReal j7eval[3];
03647 px=0;
03648 py=0;
03649 pp=0;
03650 npx=0;
03651 npy=0;
03652 npz=0;
03653 rxp0_0=0;
03654 rxp0_1=0;
03655 rxp0_2=0;
03656 rxp1_0=0;
03657 rxp1_1=0;
03658 rxp1_2=0;
03659 rxp2_0=0;
03660 rxp2_1=0;
03661 rxp2_2=0;
03662 pz=0;
03663 IkReal x1170=((28292.0)*cj11);
03664 IkReal x1171=((28292.0)*sj11);
03665 IkReal x1172=(cj12*r21);
03666 IkReal x1173=(r20*sj12);
03667 j7eval[0]=(x1173+x1172);
03668 j7eval[1]=((IKabs(((((-1.0)*r12*x1171))+((r11*sj12*x1170))+(((-1.0)*cj12*r10*x1170)))))+(IKabs((((r02*x1171))+(((-1.0)*r01*sj12*x1170))+((cj12*r00*x1170))))));
03669 j7eval[2]=IKsign(((((30976.0)*x1172))+(((30976.0)*x1173))));
03670 if( IKabs(j7eval[0]) < 0.0000010000000000  || IKabs(j7eval[1]) < 0.0000010000000000  || IKabs(j7eval[2]) < 0.0000010000000000  )
03671 {
03672 {
03673 IkReal j7eval[1];
03674 px=0;
03675 py=0;
03676 pp=0;
03677 npx=0;
03678 npy=0;
03679 npz=0;
03680 rxp0_0=0;
03681 rxp0_1=0;
03682 rxp0_2=0;
03683 rxp1_0=0;
03684 rxp1_1=0;
03685 rxp1_2=0;
03686 rxp2_0=0;
03687 rxp2_1=0;
03688 rxp2_2=0;
03689 pz=0;
03690 j7eval[0]=r12;
03691 if( IKabs(j7eval[0]) < 0.0000010000000000  )
03692 {
03693 {
03694 IkReal evalcond[1];
03695 bool bgotonextstatement = true;
03696 do
03697 {
03698 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j11)))), 6.28318530717959)));
03699 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03700 {
03701 bgotonextstatement=false;
03702 {
03703 IkReal j7array[1], cj7array[1], sj7array[1];
03704 bool j7valid[1]={false};
03705 _nj7 = 1;
03706 IkReal x1174=((0.913352272727273)*cj12);
03707 IkReal x1175=((0.913352272727273)*sj12);
03708 if( IKabs((((r10*x1174))+(((-1.0)*r11*x1175)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r00*x1174))+(((-1.0)*r01*x1175)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((r10*x1174))+(((-1.0)*r11*x1175))))+IKsqr((((r00*x1174))+(((-1.0)*r01*x1175))))-1) <= IKFAST_SINCOS_THRESH )
03709     continue;
03710 j7array[0]=IKatan2((((r10*x1174))+(((-1.0)*r11*x1175))), (((r00*x1174))+(((-1.0)*r01*x1175))));
03711 sj7array[0]=IKsin(j7array[0]);
03712 cj7array[0]=IKcos(j7array[0]);
03713 if( j7array[0] > IKPI )
03714 {
03715     j7array[0]-=IK2PI;
03716 }
03717 else if( j7array[0] < -IKPI )
03718 {    j7array[0]+=IK2PI;
03719 }
03720 j7valid[0] = true;
03721 for(int ij7 = 0; ij7 < 1; ++ij7)
03722 {
03723 if( !j7valid[ij7] )
03724 {
03725     continue;
03726 }
03727 _ij7[0] = ij7; _ij7[1] = -1;
03728 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
03729 {
03730 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
03731 {
03732     j7valid[iij7]=false; _ij7[1] = iij7; break; 
03733 }
03734 }
03735 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
03736 {
03737 IkReal evalcond[5];
03738 IkReal x1176=IKcos(j7);
03739 IkReal x1177=IKsin(j7);
03740 IkReal x1178=(r01*sj12);
03741 IkReal x1179=(cj12*r10);
03742 IkReal x1180=(r11*sj12);
03743 IkReal x1181=(cj12*r00);
03744 IkReal x1182=((0.352)*x1177);
03745 IkReal x1183=((0.352)*x1176);
03746 evalcond[0]=(((r02*x1183))+((r12*x1182)));
03747 evalcond[1]=(x1183+(((0.3215)*x1178))+(((-0.3215)*x1181)));
03748 evalcond[2]=(x1182+(((-0.3215)*x1179))+(((0.3215)*x1180)));
03749 evalcond[3]=(((r10*sj12*x1182))+((cj12*r01*x1183))+((cj12*r11*x1182))+((r00*sj12*x1183)));
03750 evalcond[4]=((-0.3215)+((x1179*x1182))+(((-1.0)*x1178*x1183))+(((-1.0)*x1180*x1182))+((x1181*x1183)));
03751 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03752 {
03753 continue;
03754 }
03755 }
03756 
03757 rotationfunction0(solutions);
03758 }
03759 }
03760 
03761 }
03762 } while(0);
03763 if( bgotonextstatement )
03764 {
03765 bool bgotonextstatement = true;
03766 do
03767 {
03768 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j11)))), 6.28318530717959)));
03769 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03770 {
03771 bgotonextstatement=false;
03772 {
03773 IkReal j7array[1], cj7array[1], sj7array[1];
03774 bool j7valid[1]={false};
03775 _nj7 = 1;
03776 IkReal x1184=((0.913352272727273)*cj12);
03777 IkReal x1185=((0.913352272727273)*sj12);
03778 if( IKabs(((((-1.0)*r10*x1184))+((r11*x1185)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r01*x1185))+(((-1.0)*r00*x1184)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*r10*x1184))+((r11*x1185))))+IKsqr((((r01*x1185))+(((-1.0)*r00*x1184))))-1) <= IKFAST_SINCOS_THRESH )
03779     continue;
03780 j7array[0]=IKatan2(((((-1.0)*r10*x1184))+((r11*x1185))), (((r01*x1185))+(((-1.0)*r00*x1184))));
03781 sj7array[0]=IKsin(j7array[0]);
03782 cj7array[0]=IKcos(j7array[0]);
03783 if( j7array[0] > IKPI )
03784 {
03785     j7array[0]-=IK2PI;
03786 }
03787 else if( j7array[0] < -IKPI )
03788 {    j7array[0]+=IK2PI;
03789 }
03790 j7valid[0] = true;
03791 for(int ij7 = 0; ij7 < 1; ++ij7)
03792 {
03793 if( !j7valid[ij7] )
03794 {
03795     continue;
03796 }
03797 _ij7[0] = ij7; _ij7[1] = -1;
03798 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
03799 {
03800 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
03801 {
03802     j7valid[iij7]=false; _ij7[1] = iij7; break; 
03803 }
03804 }
03805 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
03806 {
03807 IkReal evalcond[5];
03808 IkReal x1186=IKcos(j7);
03809 IkReal x1187=IKsin(j7);
03810 IkReal x1188=((0.3215)*sj12);
03811 IkReal x1189=(cj12*r10);
03812 IkReal x1190=(cj12*r00);
03813 IkReal x1191=((0.352)*x1187);
03814 IkReal x1192=((0.352)*x1186);
03815 evalcond[0]=(((r02*x1192))+((r12*x1191)));
03816 evalcond[1]=((((-1.0)*r01*x1188))+x1192+(((0.3215)*x1190)));
03817 evalcond[2]=((((-1.0)*r11*x1188))+x1191+(((0.3215)*x1189)));
03818 evalcond[3]=(((r00*sj12*x1192))+((r10*sj12*x1191))+((cj12*r01*x1192))+((cj12*r11*x1191)));
03819 evalcond[4]=((0.3215)+(((-1.0)*r11*sj12*x1191))+((x1189*x1191))+(((-1.0)*r01*sj12*x1192))+((x1190*x1192)));
03820 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03821 {
03822 continue;
03823 }
03824 }
03825 
03826 rotationfunction0(solutions);
03827 }
03828 }
03829 
03830 }
03831 } while(0);
03832 if( bgotonextstatement )
03833 {
03834 bool bgotonextstatement = true;
03835 do
03836 {
03837 evalcond[0]=((IKabs(r21))+(IKabs(r20)));
03838 if( IKabs(evalcond[0]) < 0.0000050000000000  )
03839 {
03840 bgotonextstatement=false;
03841 {
03842 IkReal j7array[1], cj7array[1], sj7array[1];
03843 bool j7valid[1]={false};
03844 _nj7 = 1;
03845 IkReal x1193=((0.913352272727273)*cj12*sj11);
03846 IkReal x1194=((0.913352272727273)*sj11*sj12);
03847 if( IKabs((((r10*x1193))+(((-1.0)*r11*x1194)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r00*x1193))+(((-1.0)*r01*x1194)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((r10*x1193))+(((-1.0)*r11*x1194))))+IKsqr((((r00*x1193))+(((-1.0)*r01*x1194))))-1) <= IKFAST_SINCOS_THRESH )
03848     continue;
03849 j7array[0]=IKatan2((((r10*x1193))+(((-1.0)*r11*x1194))), (((r00*x1193))+(((-1.0)*r01*x1194))));
03850 sj7array[0]=IKsin(j7array[0]);
03851 cj7array[0]=IKcos(j7array[0]);
03852 if( j7array[0] > IKPI )
03853 {
03854     j7array[0]-=IK2PI;
03855 }
03856 else if( j7array[0] < -IKPI )
03857 {    j7array[0]+=IK2PI;
03858 }
03859 j7valid[0] = true;
03860 for(int ij7 = 0; ij7 < 1; ++ij7)
03861 {
03862 if( !j7valid[ij7] )
03863 {
03864     continue;
03865 }
03866 _ij7[0] = ij7; _ij7[1] = -1;
03867 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
03868 {
03869 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
03870 {
03871     j7valid[iij7]=false; _ij7[1] = iij7; break; 
03872 }
03873 }
03874 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
03875 {
03876 IkReal evalcond[4];
03877 IkReal x1195=IKcos(j7);
03878 IkReal x1196=IKsin(j7);
03879 IkReal x1197=((0.3215)*sj11);
03880 IkReal x1198=(r01*sj12);
03881 IkReal x1199=(cj12*r10);
03882 IkReal x1200=((0.352)*x1195);
03883 IkReal x1201=((0.352)*x1196);
03884 IkReal x1202=(r00*x1200);
03885 IkReal x1203=(sj12*x1201);
03886 evalcond[0]=(x1200+((x1197*x1198))+(((-1.0)*cj12*r00*x1197)));
03887 evalcond[1]=(x1201+(((-1.0)*x1197*x1199))+((r11*sj12*x1197)));
03888 evalcond[2]=(((r10*x1203))+((sj12*x1202))+((cj12*r11*x1201))+((cj12*r01*x1200)));
03889 evalcond[3]=((((-1.0)*x1197))+((x1199*x1201))+(((-1.0)*r11*x1203))+((cj12*x1202))+(((-1.0)*x1198*x1200)));
03890 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
03891 {
03892 continue;
03893 }
03894 }
03895 
03896 rotationfunction0(solutions);
03897 }
03898 }
03899 
03900 }
03901 } while(0);
03902 if( bgotonextstatement )
03903 {
03904 bool bgotonextstatement = true;
03905 do
03906 {
03907 if( 1 )
03908 {
03909 bgotonextstatement=false;
03910 continue; // branch miss [j7]
03911 
03912 }
03913 } while(0);
03914 if( bgotonextstatement )
03915 {
03916 }
03917 }
03918 }
03919 }
03920 }
03921 
03922 } else
03923 {
03924 {
03925 IkReal j7array[1], cj7array[1], sj7array[1];
03926 bool j7valid[1]={false};
03927 _nj7 = 1;
03928 IkReal x1204=((643.0)*cj11);
03929 IkReal x1205=((643.0)*r02);
03930 IkReal x1206=(cj12*r00*sj11);
03931 IkReal x1207=(r01*sj11*sj12);
03932 CheckValue<IkReal> x1208=IKPowWithIntegerCheck(r12,-1);
03933 if(!x1208.valid){
03934 continue;
03935 }
03936 if( IKabs(((0.00142045454545455)*(x1208.value)*(((((-1.0)*x1205*x1206))+((x1205*x1207))+(((-1.0)*x1204))+((x1204*(r02*r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x1207))+(((0.913352272727273)*x1206)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00142045454545455)*(x1208.value)*(((((-1.0)*x1205*x1206))+((x1205*x1207))+(((-1.0)*x1204))+((x1204*(r02*r02)))))))+IKsqr(((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x1207))+(((0.913352272727273)*x1206))))-1) <= IKFAST_SINCOS_THRESH )
03937     continue;
03938 j7array[0]=IKatan2(((0.00142045454545455)*(x1208.value)*(((((-1.0)*x1205*x1206))+((x1205*x1207))+(((-1.0)*x1204))+((x1204*(r02*r02)))))), ((((-0.913352272727273)*cj11*r02))+(((-0.913352272727273)*x1207))+(((0.913352272727273)*x1206))));
03939 sj7array[0]=IKsin(j7array[0]);
03940 cj7array[0]=IKcos(j7array[0]);
03941 if( j7array[0] > IKPI )
03942 {
03943     j7array[0]-=IK2PI;
03944 }
03945 else if( j7array[0] < -IKPI )
03946 {    j7array[0]+=IK2PI;
03947 }
03948 j7valid[0] = true;
03949 for(int ij7 = 0; ij7 < 1; ++ij7)
03950 {
03951 if( !j7valid[ij7] )
03952 {
03953     continue;
03954 }
03955 _ij7[0] = ij7; _ij7[1] = -1;
03956 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
03957 {
03958 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
03959 {
03960     j7valid[iij7]=false; _ij7[1] = iij7; break; 
03961 }
03962 }
03963 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
03964 {
03965 IkReal evalcond[5];
03966 IkReal x1209=IKcos(j7);
03967 IkReal x1210=IKsin(j7);
03968 IkReal x1211=(r01*sj12);
03969 IkReal x1212=((0.3215)*cj11);
03970 IkReal x1213=(r11*sj12);
03971 IkReal x1214=((0.3215)*sj11);
03972 IkReal x1215=(cj12*r10);
03973 IkReal x1216=(cj12*r00);
03974 IkReal x1217=((0.352)*x1210);
03975 IkReal x1218=((0.352)*x1209);
03976 evalcond[0]=(((r12*x1217))+x1212+((r02*x1218)));
03977 evalcond[1]=(x1218+((r02*x1212))+(((-1.0)*x1214*x1216))+((x1211*x1214)));
03978 evalcond[2]=(((r12*x1212))+x1217+((x1213*x1214))+(((-1.0)*x1214*x1215)));
03979 evalcond[3]=(((r10*sj12*x1217))+((r00*sj12*x1218))+((cj12*r01*x1218))+((cj12*r11*x1217)));
03980 evalcond[4]=((((-1.0)*x1213*x1217))+(((-1.0)*x1211*x1218))+((x1216*x1218))+(((-1.0)*x1214))+((x1215*x1217)));
03981 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
03982 {
03983 continue;
03984 }
03985 }
03986 
03987 rotationfunction0(solutions);
03988 }
03989 }
03990 
03991 }
03992 
03993 }
03994 
03995 } else
03996 {
03997 {
03998 IkReal j7array[1], cj7array[1], sj7array[1];
03999 bool j7valid[1]={false};
04000 _nj7 = 1;
04001 IkReal x1219=((28292.0)*cj11);
04002 IkReal x1220=((28292.0)*sj11);
04003 CheckValue<IkReal> x1221 = IKatan2WithCheck(IkReal((((r02*x1220))+((cj12*r00*x1219))+(((-1.0)*r01*sj12*x1219)))),IkReal((((r11*sj12*x1219))+(((-1.0)*r12*x1220))+(((-1.0)*cj12*r10*x1219)))),IKFAST_ATAN2_MAGTHRESH);
04004 if(!x1221.valid){
04005 continue;
04006 }
04007 CheckValue<IkReal> x1222=IKPowWithIntegerCheck(IKsign(((((30976.0)*r20*sj12))+(((30976.0)*cj12*r21)))),-1);
04008 if(!x1222.valid){
04009 continue;
04010 }
04011 j7array[0]=((-1.5707963267949)+(x1221.value)+(((1.5707963267949)*(x1222.value))));
04012 sj7array[0]=IKsin(j7array[0]);
04013 cj7array[0]=IKcos(j7array[0]);
04014 if( j7array[0] > IKPI )
04015 {
04016     j7array[0]-=IK2PI;
04017 }
04018 else if( j7array[0] < -IKPI )
04019 {    j7array[0]+=IK2PI;
04020 }
04021 j7valid[0] = true;
04022 for(int ij7 = 0; ij7 < 1; ++ij7)
04023 {
04024 if( !j7valid[ij7] )
04025 {
04026     continue;
04027 }
04028 _ij7[0] = ij7; _ij7[1] = -1;
04029 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
04030 {
04031 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
04032 {
04033     j7valid[iij7]=false; _ij7[1] = iij7; break; 
04034 }
04035 }
04036 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
04037 {
04038 IkReal evalcond[5];
04039 IkReal x1223=IKcos(j7);
04040 IkReal x1224=IKsin(j7);
04041 IkReal x1225=(r01*sj12);
04042 IkReal x1226=((0.3215)*cj11);
04043 IkReal x1227=(r11*sj12);
04044 IkReal x1228=((0.3215)*sj11);
04045 IkReal x1229=(cj12*r10);
04046 IkReal x1230=(cj12*r00);
04047 IkReal x1231=((0.352)*x1224);
04048 IkReal x1232=((0.352)*x1223);
04049 evalcond[0]=(x1226+((r02*x1232))+((r12*x1231)));
04050 evalcond[1]=(x1232+((x1225*x1228))+((r02*x1226))+(((-1.0)*x1228*x1230)));
04051 evalcond[2]=(x1231+(((-1.0)*x1228*x1229))+((x1227*x1228))+((r12*x1226)));
04052 evalcond[3]=(((cj12*r11*x1231))+((r10*sj12*x1231))+((cj12*r01*x1232))+((r00*sj12*x1232)));
04053 evalcond[4]=((((-1.0)*x1225*x1232))+(((-1.0)*x1228))+(((-1.0)*x1227*x1231))+((x1230*x1232))+((x1229*x1231)));
04054 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
04055 {
04056 continue;
04057 }
04058 }
04059 
04060 rotationfunction0(solutions);
04061 }
04062 }
04063 
04064 }
04065 
04066 }
04067 
04068 } else
04069 {
04070 {
04071 IkReal j7array[1], cj7array[1], sj7array[1];
04072 bool j7valid[1]={false};
04073 _nj7 = 1;
04074 IkReal x1233=((226.336)*cj11);
04075 CheckValue<IkReal> x1234=IKPowWithIntegerCheck(IKsign(((((-247.808)*cj12*r20))+(((247.808)*r21*sj12)))),-1);
04076 if(!x1234.valid){
04077 continue;
04078 }
04079 CheckValue<IkReal> x1235 = IKatan2WithCheck(IkReal((((cj12*r01*x1233))+((r00*sj12*x1233)))),IkReal(((((-1.0)*cj12*r11*x1233))+(((-1.0)*r10*sj12*x1233)))),IKFAST_ATAN2_MAGTHRESH);
04080 if(!x1235.valid){
04081 continue;
04082 }
04083 j7array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1234.value)))+(x1235.value));
04084 sj7array[0]=IKsin(j7array[0]);
04085 cj7array[0]=IKcos(j7array[0]);
04086 if( j7array[0] > IKPI )
04087 {
04088     j7array[0]-=IK2PI;
04089 }
04090 else if( j7array[0] < -IKPI )
04091 {    j7array[0]+=IK2PI;
04092 }
04093 j7valid[0] = true;
04094 for(int ij7 = 0; ij7 < 1; ++ij7)
04095 {
04096 if( !j7valid[ij7] )
04097 {
04098     continue;
04099 }
04100 _ij7[0] = ij7; _ij7[1] = -1;
04101 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
04102 {
04103 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
04104 {
04105     j7valid[iij7]=false; _ij7[1] = iij7; break; 
04106 }
04107 }
04108 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
04109 {
04110 IkReal evalcond[5];
04111 IkReal x1236=IKcos(j7);
04112 IkReal x1237=IKsin(j7);
04113 IkReal x1238=(r01*sj12);
04114 IkReal x1239=((0.3215)*cj11);
04115 IkReal x1240=(r11*sj12);
04116 IkReal x1241=((0.3215)*sj11);
04117 IkReal x1242=(cj12*r10);
04118 IkReal x1243=(cj12*r00);
04119 IkReal x1244=((0.352)*x1237);
04120 IkReal x1245=((0.352)*x1236);
04121 evalcond[0]=(x1239+((r02*x1245))+((r12*x1244)));
04122 evalcond[1]=(x1245+((r02*x1239))+((x1238*x1241))+(((-1.0)*x1241*x1243)));
04123 evalcond[2]=(x1244+((x1240*x1241))+(((-1.0)*x1241*x1242))+((r12*x1239)));
04124 evalcond[3]=(((cj12*r11*x1244))+((cj12*r01*x1245))+((r10*sj12*x1244))+((r00*sj12*x1245)));
04125 evalcond[4]=((((-1.0)*x1241))+(((-1.0)*x1238*x1245))+(((-1.0)*x1240*x1244))+((x1242*x1244))+((x1243*x1245)));
04126 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
04127 {
04128 continue;
04129 }
04130 }
04131 
04132 rotationfunction0(solutions);
04133 }
04134 }
04135 
04136 }
04137 
04138 }
04139 }
04140 }
04141 
04142 }
04143 
04144 }
04145 
04146 }
04147 } while(0);
04148 if( bgotonextstatement )
04149 {
04150 bool bgotonextstatement = true;
04151 do
04152 {
04153 if( 1 )
04154 {
04155 bgotonextstatement=false;
04156 continue; // branch miss [j7, j11]
04157 
04158 }
04159 } while(0);
04160 if( bgotonextstatement )
04161 {
04162 }
04163 }
04164 }
04165 
04166 } else
04167 {
04168 {
04169 IkReal j7array[2], cj7array[2], sj7array[2];
04170 bool j7valid[2]={false};
04171 _nj7 = 2;
04172 IkReal x1246=((0.352)*cj12);
04173 IkReal x1247=((0.352)*sj12);
04174 IkReal x1248=((1.0)*pz);
04175 IkReal x1249=(((r01*x1246))+((r00*x1247)));
04176 IkReal x1250=(((r11*x1246))+((r10*x1247)));
04177 CheckValue<IkReal> x1253 = IKatan2WithCheck(IkReal(x1249),IkReal(x1250),IKFAST_ATAN2_MAGTHRESH);
04178 if(!x1253.valid){
04179 continue;
04180 }
04181 IkReal x1251=((1.0)*(x1253.value));
04182 if((((x1249*x1249)+(x1250*x1250))) < -0.00001)
04183 continue;
04184 CheckValue<IkReal> x1254=IKPowWithIntegerCheck(IKabs(IKsqrt(((x1249*x1249)+(x1250*x1250)))),-1);
04185 if(!x1254.valid){
04186 continue;
04187 }
04188 if( (((-1.0)*(x1254.value)*(((((-1.0)*cj12*r21*x1248))+(((-1.0)*r20*sj12*x1248)))))) < -1-IKFAST_SINCOS_THRESH || (((-1.0)*(x1254.value)*(((((-1.0)*cj12*r21*x1248))+(((-1.0)*r20*sj12*x1248)))))) > 1+IKFAST_SINCOS_THRESH )
04189     continue;
04190 IkReal x1252=((-1.0)*(IKasin(((-1.0)*(x1254.value)*(((((-1.0)*cj12*r21*x1248))+(((-1.0)*r20*sj12*x1248))))))));
04191 j7array[0]=((((-1.0)*x1252))+(((-1.0)*x1251)));
04192 sj7array[0]=IKsin(j7array[0]);
04193 cj7array[0]=IKcos(j7array[0]);
04194 j7array[1]=((3.14159265358979)+(((-1.0)*x1251))+(((1.0)*x1252)));
04195 sj7array[1]=IKsin(j7array[1]);
04196 cj7array[1]=IKcos(j7array[1]);
04197 if( j7array[0] > IKPI )
04198 {
04199     j7array[0]-=IK2PI;
04200 }
04201 else if( j7array[0] < -IKPI )
04202 {    j7array[0]+=IK2PI;
04203 }
04204 j7valid[0] = true;
04205 if( j7array[1] > IKPI )
04206 {
04207     j7array[1]-=IK2PI;
04208 }
04209 else if( j7array[1] < -IKPI )
04210 {    j7array[1]+=IK2PI;
04211 }
04212 j7valid[1] = true;
04213 for(int ij7 = 0; ij7 < 2; ++ij7)
04214 {
04215 if( !j7valid[ij7] )
04216 {
04217     continue;
04218 }
04219 _ij7[0] = ij7; _ij7[1] = -1;
04220 for(int iij7 = ij7+1; iij7 < 2; ++iij7)
04221 {
04222 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
04223 {
04224     j7valid[iij7]=false; _ij7[1] = iij7; break; 
04225 }
04226 }
04227 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
04228 
04229 {
04230 IkReal j11eval[1];
04231 IkReal x1255=((-1.0)*pz);
04232 px=0;
04233 py=0;
04234 pp=pz*pz;
04235 npx=(pz*r20);
04236 npy=(pz*r21);
04237 npz=(pz*r22);
04238 rxp0_0=(pz*r10);
04239 rxp0_1=(r00*x1255);
04240 rxp0_2=0;
04241 rxp1_0=(pz*r11);
04242 rxp1_1=(r01*x1255);
04243 rxp1_2=0;
04244 rxp2_0=(pz*r12);
04245 rxp2_1=(r02*x1255);
04246 rxp2_2=0;
04247 j11eval[0]=((((-1.0)*cj12*r20))+((r21*sj12)));
04248 if( IKabs(j11eval[0]) < 0.0000010000000000  )
04249 {
04250 {
04251 IkReal j11eval[1];
04252 IkReal x1256=((-1.0)*pz);
04253 px=0;
04254 py=0;
04255 pp=pz*pz;
04256 npx=(pz*r20);
04257 npy=(pz*r21);
04258 npz=(pz*r22);
04259 rxp0_0=(pz*r10);
04260 rxp0_1=(r00*x1256);
04261 rxp0_2=0;
04262 rxp1_0=(pz*r11);
04263 rxp1_1=(r01*x1256);
04264 rxp1_2=0;
04265 rxp2_0=(pz*r12);
04266 rxp2_1=(r02*x1256);
04267 rxp2_2=0;
04268 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
04269 if( IKabs(j11eval[0]) < 0.0000010000000000  )
04270 {
04271 {
04272 IkReal j11eval[1];
04273 IkReal x1257=((-1.0)*pz);
04274 px=0;
04275 py=0;
04276 pp=pz*pz;
04277 npx=(pz*r20);
04278 npy=(pz*r21);
04279 npz=(pz*r22);
04280 rxp0_0=(pz*r10);
04281 rxp0_1=(r00*x1257);
04282 rxp0_2=0;
04283 rxp1_0=(pz*r11);
04284 rxp1_1=(r01*x1257);
04285 rxp1_2=0;
04286 rxp2_0=(pz*r12);
04287 rxp2_1=(r02*x1257);
04288 rxp2_2=0;
04289 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
04290 if( IKabs(j11eval[0]) < 0.0000010000000000  )
04291 {
04292 {
04293 IkReal evalcond[1];
04294 bool bgotonextstatement = true;
04295 do
04296 {
04297 IkReal x1259 = ((1.0)+(((-1.0)*(r12*r12))));
04298 if(IKabs(x1259)==0){
04299 continue;
04300 }
04301 IkReal x1258=pow(x1259,-0.5);
04302 CheckValue<IkReal> x1260 = IKatan2WithCheck(IkReal(((-1.0)*r10)),IkReal(r11),IKFAST_ATAN2_MAGTHRESH);
04303 if(!x1260.valid){
04304 continue;
04305 }
04306 IkReal gconst64=((-1.0)*(x1260.value));
04307 IkReal gconst65=(r10*x1258);
04308 IkReal gconst66=(r11*x1258);
04309 CheckValue<IkReal> x1261 = IKatan2WithCheck(IkReal(((-1.0)*r10)),IkReal(r11),IKFAST_ATAN2_MAGTHRESH);
04310 if(!x1261.valid){
04311 continue;
04312 }
04313 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j12+(x1261.value))))), 6.28318530717959)));
04314 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04315 {
04316 bgotonextstatement=false;
04317 {
04318 IkReal j11array[1], cj11array[1], sj11array[1];
04319 bool j11valid[1]={false};
04320 _nj11 = 1;
04321 IkReal x1262=((0.352)*r22);
04322 IkReal x1263=(r12*sj7);
04323 IkReal x1264=(cj7*r02);
04324 CheckValue<IkReal> x1265=IKPowWithIntegerCheck(((((0.3215)*gconst65*r21))+(((-0.3215)*gconst66*r20))),-1);
04325 if(!x1265.valid){
04326 continue;
04327 }
04328 if( IKabs(((x1265.value)*(((((-1.0)*pz*(r22*r22)))+((x1262*x1264))+((x1262*x1263))+pz)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1264))+(((-1.09486780715397)*x1263))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1265.value)*(((((-1.0)*pz*(r22*r22)))+((x1262*x1264))+((x1262*x1263))+pz))))+IKsqr(((((-1.09486780715397)*x1264))+(((-1.09486780715397)*x1263))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
04329     continue;
04330 j11array[0]=IKatan2(((x1265.value)*(((((-1.0)*pz*(r22*r22)))+((x1262*x1264))+((x1262*x1263))+pz))), ((((-1.09486780715397)*x1264))+(((-1.09486780715397)*x1263))+(((3.1104199066874)*pz*r22))));
04331 sj11array[0]=IKsin(j11array[0]);
04332 cj11array[0]=IKcos(j11array[0]);
04333 if( j11array[0] > IKPI )
04334 {
04335     j11array[0]-=IK2PI;
04336 }
04337 else if( j11array[0] < -IKPI )
04338 {    j11array[0]+=IK2PI;
04339 }
04340 j11valid[0] = true;
04341 for(int ij11 = 0; ij11 < 1; ++ij11)
04342 {
04343 if( !j11valid[ij11] )
04344 {
04345     continue;
04346 }
04347 _ij11[0] = ij11; _ij11[1] = -1;
04348 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04349 {
04350 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04351 {
04352     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04353 }
04354 }
04355 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04356 {
04357 IkReal evalcond[6];
04358 IkReal x1266=IKcos(j11);
04359 IkReal x1267=IKsin(j11);
04360 IkReal x1268=((0.352)*cj7);
04361 IkReal x1269=(gconst65*r01);
04362 IkReal x1270=((0.352)*sj7);
04363 IkReal x1271=((0.643)*pz);
04364 IkReal x1272=((1.0)*pz);
04365 IkReal x1273=(gconst65*r21);
04366 IkReal x1274=(gconst66*x1267);
04367 IkReal x1275=((0.3215)*x1266);
04368 IkReal x1276=((0.3215)*x1267);
04369 IkReal x1277=(gconst65*x1276);
04370 evalcond[0]=(x1275+((r12*x1270))+((r02*x1268))+(((-1.0)*r22*x1272)));
04371 evalcond[1]=((((-1.0)*x1272))+((r22*x1275))+((x1273*x1276))+(((-0.3215)*r20*x1274)));
04372 evalcond[2]=(((x1269*x1276))+x1268+((r02*x1275))+(((-0.3215)*r00*x1274)));
04373 evalcond[3]=(x1270+((r12*x1275))+((r11*x1277))+(((-0.3215)*r10*x1274)));
04374 evalcond[4]=((0.0410835)+((x1267*x1271*x1273))+((r22*x1266*x1271))+(((-1.0)*r20*x1271*x1274)));
04375 evalcond[5]=(((pz*x1273))+((gconst66*r10*x1270))+(((-1.0)*gconst65*r11*x1270))+(((-1.0)*x1276))+((gconst66*r00*x1268))+(((-1.0)*x1268*x1269))+(((-1.0)*gconst66*r20*x1272)));
04376 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04377 {
04378 continue;
04379 }
04380 }
04381 
04382 rotationfunction0(solutions);
04383 }
04384 }
04385 
04386 }
04387 } while(0);
04388 if( bgotonextstatement )
04389 {
04390 bool bgotonextstatement = true;
04391 do
04392 {
04393 IkReal x1280 = ((1.0)+(((-1.0)*(r12*r12))));
04394 if(IKabs(x1280)==0){
04395 continue;
04396 }
04397 IkReal x1278=pow(x1280,-0.5);
04398 IkReal x1279=((-1.0)*x1278);
04399 CheckValue<IkReal> x1281 = IKatan2WithCheck(IkReal(((-1.0)*r10)),IkReal(r11),IKFAST_ATAN2_MAGTHRESH);
04400 if(!x1281.valid){
04401 continue;
04402 }
04403 IkReal gconst67=((3.14159265358979)+(((-1.0)*(x1281.value))));
04404 IkReal gconst68=(r10*x1279);
04405 IkReal gconst69=(r11*x1279);
04406 CheckValue<IkReal> x1282 = IKatan2WithCheck(IkReal(((-1.0)*r10)),IkReal(r11),IKFAST_ATAN2_MAGTHRESH);
04407 if(!x1282.valid){
04408 continue;
04409 }
04410 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j12+(x1282.value))))), 6.28318530717959)));
04411 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04412 {
04413 bgotonextstatement=false;
04414 {
04415 IkReal j11array[1], cj11array[1], sj11array[1];
04416 bool j11valid[1]={false};
04417 _nj11 = 1;
04418 IkReal x1283=((0.352)*r22);
04419 IkReal x1284=(r12*sj7);
04420 IkReal x1285=(cj7*r02);
04421 CheckValue<IkReal> x1286=IKPowWithIntegerCheck(((((0.3215)*gconst68*r21))+(((-0.3215)*gconst69*r20))),-1);
04422 if(!x1286.valid){
04423 continue;
04424 }
04425 if( IKabs(((x1286.value)*(((((-1.0)*pz*(r22*r22)))+((x1283*x1285))+((x1283*x1284))+pz)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1285))+(((-1.09486780715397)*x1284))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1286.value)*(((((-1.0)*pz*(r22*r22)))+((x1283*x1285))+((x1283*x1284))+pz))))+IKsqr(((((-1.09486780715397)*x1285))+(((-1.09486780715397)*x1284))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
04426     continue;
04427 j11array[0]=IKatan2(((x1286.value)*(((((-1.0)*pz*(r22*r22)))+((x1283*x1285))+((x1283*x1284))+pz))), ((((-1.09486780715397)*x1285))+(((-1.09486780715397)*x1284))+(((3.1104199066874)*pz*r22))));
04428 sj11array[0]=IKsin(j11array[0]);
04429 cj11array[0]=IKcos(j11array[0]);
04430 if( j11array[0] > IKPI )
04431 {
04432     j11array[0]-=IK2PI;
04433 }
04434 else if( j11array[0] < -IKPI )
04435 {    j11array[0]+=IK2PI;
04436 }
04437 j11valid[0] = true;
04438 for(int ij11 = 0; ij11 < 1; ++ij11)
04439 {
04440 if( !j11valid[ij11] )
04441 {
04442     continue;
04443 }
04444 _ij11[0] = ij11; _ij11[1] = -1;
04445 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04446 {
04447 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04448 {
04449     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04450 }
04451 }
04452 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04453 {
04454 IkReal evalcond[6];
04455 IkReal x1287=IKcos(j11);
04456 IkReal x1288=IKsin(j11);
04457 IkReal x1289=((0.352)*sj7);
04458 IkReal x1290=(gconst68*r11);
04459 IkReal x1291=((1.0)*pz);
04460 IkReal x1292=(gconst69*r20);
04461 IkReal x1293=(gconst68*r01);
04462 IkReal x1294=((0.352)*cj7);
04463 IkReal x1295=(gconst69*r10);
04464 IkReal x1296=(gconst68*r21);
04465 IkReal x1297=(gconst69*r00);
04466 IkReal x1298=((0.3215)*x1288);
04467 IkReal x1299=((0.3215)*x1287);
04468 IkReal x1300=((0.643)*pz*x1288);
04469 evalcond[0]=(x1299+((r02*x1294))+(((-1.0)*r22*x1291))+((r12*x1289)));
04470 evalcond[1]=(((x1296*x1298))+(((-1.0)*x1291))+(((-1.0)*x1292*x1298))+((r22*x1299)));
04471 evalcond[2]=(x1294+((x1293*x1298))+((r02*x1299))+(((-1.0)*x1297*x1298)));
04472 evalcond[3]=(x1289+((x1290*x1298))+((r12*x1299))+(((-1.0)*x1295*x1298)));
04473 evalcond[4]=((0.0410835)+(((-1.0)*x1292*x1300))+((x1296*x1300))+(((0.643)*pz*r22*x1287)));
04474 evalcond[5]=((((-1.0)*x1298))+(((-1.0)*x1289*x1290))+((x1294*x1297))+((pz*x1296))+(((-1.0)*x1293*x1294))+(((-1.0)*x1291*x1292))+((x1289*x1295)));
04475 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04476 {
04477 continue;
04478 }
04479 }
04480 
04481 rotationfunction0(solutions);
04482 }
04483 }
04484 
04485 }
04486 } while(0);
04487 if( bgotonextstatement )
04488 {
04489 bool bgotonextstatement = true;
04490 do
04491 {
04492 IkReal x1302 = ((1.0)+(((-1.0)*(r02*r02))));
04493 if(IKabs(x1302)==0){
04494 continue;
04495 }
04496 IkReal x1301=pow(x1302,-0.5);
04497 CheckValue<IkReal> x1303 = IKatan2WithCheck(IkReal(((-1.0)*r00)),IkReal(r01),IKFAST_ATAN2_MAGTHRESH);
04498 if(!x1303.valid){
04499 continue;
04500 }
04501 IkReal gconst70=((-1.0)*(x1303.value));
04502 IkReal gconst71=(r00*x1301);
04503 IkReal gconst72=(r01*x1301);
04504 CheckValue<IkReal> x1304 = IKatan2WithCheck(IkReal(((-1.0)*r00)),IkReal(r01),IKFAST_ATAN2_MAGTHRESH);
04505 if(!x1304.valid){
04506 continue;
04507 }
04508 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j12+(x1304.value))))), 6.28318530717959)));
04509 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04510 {
04511 bgotonextstatement=false;
04512 {
04513 IkReal j11array[1], cj11array[1], sj11array[1];
04514 bool j11valid[1]={false};
04515 _nj11 = 1;
04516 IkReal x1305=((0.352)*r22);
04517 IkReal x1306=(r12*sj7);
04518 IkReal x1307=(cj7*r02);
04519 CheckValue<IkReal> x1308=IKPowWithIntegerCheck(((((0.3215)*gconst71*r21))+(((-0.3215)*gconst72*r20))),-1);
04520 if(!x1308.valid){
04521 continue;
04522 }
04523 if( IKabs(((x1308.value)*((((x1305*x1307))+((x1305*x1306))+(((-1.0)*pz*(r22*r22)))+pz)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1307))+(((-1.09486780715397)*x1306))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1308.value)*((((x1305*x1307))+((x1305*x1306))+(((-1.0)*pz*(r22*r22)))+pz))))+IKsqr(((((-1.09486780715397)*x1307))+(((-1.09486780715397)*x1306))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
04524     continue;
04525 j11array[0]=IKatan2(((x1308.value)*((((x1305*x1307))+((x1305*x1306))+(((-1.0)*pz*(r22*r22)))+pz))), ((((-1.09486780715397)*x1307))+(((-1.09486780715397)*x1306))+(((3.1104199066874)*pz*r22))));
04526 sj11array[0]=IKsin(j11array[0]);
04527 cj11array[0]=IKcos(j11array[0]);
04528 if( j11array[0] > IKPI )
04529 {
04530     j11array[0]-=IK2PI;
04531 }
04532 else if( j11array[0] < -IKPI )
04533 {    j11array[0]+=IK2PI;
04534 }
04535 j11valid[0] = true;
04536 for(int ij11 = 0; ij11 < 1; ++ij11)
04537 {
04538 if( !j11valid[ij11] )
04539 {
04540     continue;
04541 }
04542 _ij11[0] = ij11; _ij11[1] = -1;
04543 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04544 {
04545 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04546 {
04547     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04548 }
04549 }
04550 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04551 {
04552 IkReal evalcond[6];
04553 IkReal x1309=IKcos(j11);
04554 IkReal x1310=IKsin(j11);
04555 IkReal x1311=((0.352)*sj7);
04556 IkReal x1312=(gconst72*r10);
04557 IkReal x1313=(gconst72*r20);
04558 IkReal x1314=(gconst71*r11);
04559 IkReal x1315=((1.0)*pz);
04560 IkReal x1316=(gconst71*r01);
04561 IkReal x1317=((0.352)*cj7);
04562 IkReal x1318=(gconst72*r00);
04563 IkReal x1319=((0.3215)*x1310);
04564 IkReal x1320=((0.3215)*x1309);
04565 IkReal x1321=(gconst71*pz*r21);
04566 IkReal x1322=((0.643)*x1310);
04567 evalcond[0]=(x1320+(((-1.0)*r22*x1315))+((r12*x1311))+((r02*x1317)));
04568 evalcond[1]=((((-1.0)*x1313*x1319))+((gconst71*r21*x1319))+(((-1.0)*x1315))+((r22*x1320)));
04569 evalcond[2]=(x1317+(((-1.0)*x1318*x1319))+((x1316*x1319))+((r02*x1320)));
04570 evalcond[3]=(x1311+((x1314*x1319))+((r12*x1320))+(((-1.0)*x1312*x1319)));
04571 evalcond[4]=((0.0410835)+(((-1.0)*pz*x1313*x1322))+((x1321*x1322))+(((0.643)*pz*r22*x1309)));
04572 evalcond[5]=(x1321+(((-1.0)*x1311*x1314))+((x1317*x1318))+((x1311*x1312))+(((-1.0)*x1313*x1315))+(((-1.0)*x1319))+(((-1.0)*x1316*x1317)));
04573 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04574 {
04575 continue;
04576 }
04577 }
04578 
04579 rotationfunction0(solutions);
04580 }
04581 }
04582 
04583 }
04584 } while(0);
04585 if( bgotonextstatement )
04586 {
04587 bool bgotonextstatement = true;
04588 do
04589 {
04590 IkReal x1323=((-1.0)*r00);
04591 IkReal x1325 = ((1.0)+(((-1.0)*(r02*r02))));
04592 if(IKabs(x1325)==0){
04593 continue;
04594 }
04595 IkReal x1324=pow(x1325,-0.5);
04596 CheckValue<IkReal> x1326 = IKatan2WithCheck(IkReal(x1323),IkReal(r01),IKFAST_ATAN2_MAGTHRESH);
04597 if(!x1326.valid){
04598 continue;
04599 }
04600 IkReal gconst73=((3.14159265358979)+(((-1.0)*(x1326.value))));
04601 IkReal gconst74=(x1323*x1324);
04602 IkReal gconst75=((-1.0)*r01*x1324);
04603 CheckValue<IkReal> x1327 = IKatan2WithCheck(IkReal(((-1.0)*r00)),IkReal(r01),IKFAST_ATAN2_MAGTHRESH);
04604 if(!x1327.valid){
04605 continue;
04606 }
04607 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j12+(x1327.value))))), 6.28318530717959)));
04608 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04609 {
04610 bgotonextstatement=false;
04611 {
04612 IkReal j11array[1], cj11array[1], sj11array[1];
04613 bool j11valid[1]={false};
04614 _nj11 = 1;
04615 IkReal x1328=((0.352)*r22);
04616 IkReal x1329=(r12*sj7);
04617 IkReal x1330=(cj7*r02);
04618 CheckValue<IkReal> x1331=IKPowWithIntegerCheck(((((0.3215)*gconst74*r21))+(((-0.3215)*gconst75*r20))),-1);
04619 if(!x1331.valid){
04620 continue;
04621 }
04622 if( IKabs(((x1331.value)*(((((-1.0)*pz*(r22*r22)))+((x1328*x1329))+((x1328*x1330))+pz)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1330))+(((-1.09486780715397)*x1329))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1331.value)*(((((-1.0)*pz*(r22*r22)))+((x1328*x1329))+((x1328*x1330))+pz))))+IKsqr(((((-1.09486780715397)*x1330))+(((-1.09486780715397)*x1329))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
04623     continue;
04624 j11array[0]=IKatan2(((x1331.value)*(((((-1.0)*pz*(r22*r22)))+((x1328*x1329))+((x1328*x1330))+pz))), ((((-1.09486780715397)*x1330))+(((-1.09486780715397)*x1329))+(((3.1104199066874)*pz*r22))));
04625 sj11array[0]=IKsin(j11array[0]);
04626 cj11array[0]=IKcos(j11array[0]);
04627 if( j11array[0] > IKPI )
04628 {
04629     j11array[0]-=IK2PI;
04630 }
04631 else if( j11array[0] < -IKPI )
04632 {    j11array[0]+=IK2PI;
04633 }
04634 j11valid[0] = true;
04635 for(int ij11 = 0; ij11 < 1; ++ij11)
04636 {
04637 if( !j11valid[ij11] )
04638 {
04639     continue;
04640 }
04641 _ij11[0] = ij11; _ij11[1] = -1;
04642 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04643 {
04644 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04645 {
04646     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04647 }
04648 }
04649 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04650 {
04651 IkReal evalcond[6];
04652 IkReal x1332=IKcos(j11);
04653 IkReal x1333=IKsin(j11);
04654 IkReal x1334=(pz*r20);
04655 IkReal x1335=((0.352)*sj7);
04656 IkReal x1336=((0.352)*cj7);
04657 IkReal x1337=((1.0)*pz);
04658 IkReal x1338=(gconst74*r11);
04659 IkReal x1339=(pz*r21);
04660 IkReal x1340=(gconst75*x1333);
04661 IkReal x1341=((0.3215)*x1332);
04662 IkReal x1342=(gconst74*x1333);
04663 IkReal x1343=((0.3215)*x1333);
04664 evalcond[0]=(x1341+((r12*x1335))+(((-1.0)*r22*x1337))+((r02*x1336)));
04665 evalcond[1]=(((r22*x1341))+(((0.3215)*r21*x1342))+(((-0.3215)*r20*x1340))+(((-1.0)*x1337)));
04666 evalcond[2]=(x1336+(((-0.3215)*r00*x1340))+((r02*x1341))+(((0.3215)*r01*x1342)));
04667 evalcond[3]=(x1335+((x1338*x1343))+((r12*x1341))+(((-0.3215)*r10*x1340)));
04668 evalcond[4]=((0.0410835)+(((0.643)*pz*r22*x1332))+(((-0.643)*x1334*x1340))+(((0.643)*x1339*x1342)));
04669 evalcond[5]=((((-1.0)*x1335*x1338))+(((-1.0)*x1343))+(((-1.0)*gconst75*x1334))+(((-1.0)*gconst74*r01*x1336))+((gconst75*r10*x1335))+((gconst74*x1339))+((gconst75*r00*x1336)));
04670 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04671 {
04672 continue;
04673 }
04674 }
04675 
04676 rotationfunction0(solutions);
04677 }
04678 }
04679 
04680 }
04681 } while(0);
04682 if( bgotonextstatement )
04683 {
04684 bool bgotonextstatement = true;
04685 do
04686 {
04687 IkReal x1345 = ((1.0)+(((-1.0)*(r22*r22))));
04688 if(IKabs(x1345)==0){
04689 continue;
04690 }
04691 IkReal x1344=pow(x1345,-0.5);
04692 CheckValue<IkReal> x1346 = IKatan2WithCheck(IkReal(((-1.0)*r20)),IkReal(r21),IKFAST_ATAN2_MAGTHRESH);
04693 if(!x1346.valid){
04694 continue;
04695 }
04696 IkReal gconst76=((-1.0)*(x1346.value));
04697 IkReal gconst77=(r20*x1344);
04698 IkReal gconst78=(r21*x1344);
04699 CheckValue<IkReal> x1347 = IKatan2WithCheck(IkReal(((-1.0)*r20)),IkReal(r21),IKFAST_ATAN2_MAGTHRESH);
04700 if(!x1347.valid){
04701 continue;
04702 }
04703 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j12+(x1347.value))))), 6.28318530717959)));
04704 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04705 {
04706 bgotonextstatement=false;
04707 {
04708 IkReal j11array[1], cj11array[1], sj11array[1];
04709 bool j11valid[1]={false};
04710 _nj11 = 1;
04711 IkReal x1348=(pz*r22);
04712 IkReal x1349=(r12*sj7);
04713 IkReal x1350=((44.0)*cj7);
04714 CheckValue<IkReal> x1351=IKPowWithIntegerCheck(((((-40.1875)*gconst78*r00))+(((40.1875)*gconst77*r01))),-1);
04715 if(!x1351.valid){
04716 continue;
04717 }
04718 if( IKabs(((x1351.value)*(((((-1.0)*x1350))+((x1350*(r02*r02)))+(((-125.0)*r02*x1348))+(((44.0)*r02*x1349)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((3.1104199066874)*x1348))+(((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x1349)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1351.value)*(((((-1.0)*x1350))+((x1350*(r02*r02)))+(((-125.0)*r02*x1348))+(((44.0)*r02*x1349))))))+IKsqr(((((3.1104199066874)*x1348))+(((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x1349))))-1) <= IKFAST_SINCOS_THRESH )
04719     continue;
04720 j11array[0]=IKatan2(((x1351.value)*(((((-1.0)*x1350))+((x1350*(r02*r02)))+(((-125.0)*r02*x1348))+(((44.0)*r02*x1349))))), ((((3.1104199066874)*x1348))+(((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*x1349))));
04721 sj11array[0]=IKsin(j11array[0]);
04722 cj11array[0]=IKcos(j11array[0]);
04723 if( j11array[0] > IKPI )
04724 {
04725     j11array[0]-=IK2PI;
04726 }
04727 else if( j11array[0] < -IKPI )
04728 {    j11array[0]+=IK2PI;
04729 }
04730 j11valid[0] = true;
04731 for(int ij11 = 0; ij11 < 1; ++ij11)
04732 {
04733 if( !j11valid[ij11] )
04734 {
04735     continue;
04736 }
04737 _ij11[0] = ij11; _ij11[1] = -1;
04738 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04739 {
04740 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04741 {
04742     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04743 }
04744 }
04745 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04746 {
04747 IkReal evalcond[6];
04748 IkReal x1352=IKcos(j11);
04749 IkReal x1353=IKsin(j11);
04750 IkReal x1354=(gconst77*r21);
04751 IkReal x1355=(gconst78*r10);
04752 IkReal x1356=((0.352)*sj7);
04753 IkReal x1357=((1.0)*pz);
04754 IkReal x1358=(gconst78*r20);
04755 IkReal x1359=(gconst78*r00);
04756 IkReal x1360=((0.352)*cj7);
04757 IkReal x1361=(gconst77*r01);
04758 IkReal x1362=(gconst77*r11);
04759 IkReal x1363=((0.3215)*x1353);
04760 IkReal x1364=((0.3215)*x1352);
04761 IkReal x1365=((0.643)*pz*x1353);
04762 evalcond[0]=(x1364+((r02*x1360))+(((-1.0)*r22*x1357))+((r12*x1356)));
04763 evalcond[1]=((((-1.0)*x1357))+((r22*x1364))+((x1354*x1363))+(((-1.0)*x1358*x1363)));
04764 evalcond[2]=(x1360+((x1361*x1363))+((r02*x1364))+(((-1.0)*x1359*x1363)));
04765 evalcond[3]=(x1356+(((-1.0)*x1355*x1363))+((r12*x1364))+((x1362*x1363)));
04766 evalcond[4]=((0.0410835)+((x1354*x1365))+(((0.643)*pz*r22*x1352))+(((-1.0)*x1358*x1365)));
04767 evalcond[5]=(((x1355*x1356))+((x1359*x1360))+(((-1.0)*x1356*x1362))+((pz*x1354))+(((-1.0)*x1357*x1358))+(((-1.0)*x1360*x1361))+(((-1.0)*x1363)));
04768 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04769 {
04770 continue;
04771 }
04772 }
04773 
04774 rotationfunction0(solutions);
04775 }
04776 }
04777 
04778 }
04779 } while(0);
04780 if( bgotonextstatement )
04781 {
04782 bool bgotonextstatement = true;
04783 do
04784 {
04785 IkReal x1368 = ((1.0)+(((-1.0)*(r22*r22))));
04786 if(IKabs(x1368)==0){
04787 continue;
04788 }
04789 IkReal x1366=pow(x1368,-0.5);
04790 IkReal x1367=((-1.0)*x1366);
04791 CheckValue<IkReal> x1369 = IKatan2WithCheck(IkReal(((-1.0)*r20)),IkReal(r21),IKFAST_ATAN2_MAGTHRESH);
04792 if(!x1369.valid){
04793 continue;
04794 }
04795 IkReal gconst79=((3.14159265358979)+(((-1.0)*(x1369.value))));
04796 IkReal gconst80=(r20*x1367);
04797 IkReal gconst81=(r21*x1367);
04798 CheckValue<IkReal> x1370 = IKatan2WithCheck(IkReal(((-1.0)*r20)),IkReal(r21),IKFAST_ATAN2_MAGTHRESH);
04799 if(!x1370.valid){
04800 continue;
04801 }
04802 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j12+(x1370.value))))), 6.28318530717959)));
04803 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04804 {
04805 bgotonextstatement=false;
04806 {
04807 IkReal j11array[1], cj11array[1], sj11array[1];
04808 bool j11valid[1]={false};
04809 _nj11 = 1;
04810 IkReal x1371=(pz*r22);
04811 IkReal x1372=(r12*sj7);
04812 IkReal x1373=((44.0)*cj7);
04813 CheckValue<IkReal> x1374=IKPowWithIntegerCheck(((((-40.1875)*gconst81*r00))+(((40.1875)*gconst80*r01))),-1);
04814 if(!x1374.valid){
04815 continue;
04816 }
04817 if( IKabs(((x1374.value)*(((((-125.0)*r02*x1371))+(((-1.0)*x1373))+(((44.0)*r02*x1372))+((x1373*(r02*r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((3.1104199066874)*x1371))+(((-1.09486780715397)*x1372))+(((-1.09486780715397)*cj7*r02)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1374.value)*(((((-125.0)*r02*x1371))+(((-1.0)*x1373))+(((44.0)*r02*x1372))+((x1373*(r02*r02)))))))+IKsqr(((((3.1104199066874)*x1371))+(((-1.09486780715397)*x1372))+(((-1.09486780715397)*cj7*r02))))-1) <= IKFAST_SINCOS_THRESH )
04818     continue;
04819 j11array[0]=IKatan2(((x1374.value)*(((((-125.0)*r02*x1371))+(((-1.0)*x1373))+(((44.0)*r02*x1372))+((x1373*(r02*r02)))))), ((((3.1104199066874)*x1371))+(((-1.09486780715397)*x1372))+(((-1.09486780715397)*cj7*r02))));
04820 sj11array[0]=IKsin(j11array[0]);
04821 cj11array[0]=IKcos(j11array[0]);
04822 if( j11array[0] > IKPI )
04823 {
04824     j11array[0]-=IK2PI;
04825 }
04826 else if( j11array[0] < -IKPI )
04827 {    j11array[0]+=IK2PI;
04828 }
04829 j11valid[0] = true;
04830 for(int ij11 = 0; ij11 < 1; ++ij11)
04831 {
04832 if( !j11valid[ij11] )
04833 {
04834     continue;
04835 }
04836 _ij11[0] = ij11; _ij11[1] = -1;
04837 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
04838 {
04839 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
04840 {
04841     j11valid[iij11]=false; _ij11[1] = iij11; break; 
04842 }
04843 }
04844 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
04845 {
04846 IkReal evalcond[6];
04847 IkReal x1375=IKcos(j11);
04848 IkReal x1376=IKsin(j11);
04849 IkReal x1377=(gconst80*r11);
04850 IkReal x1378=(gconst81*r00);
04851 IkReal x1379=((0.352)*sj7);
04852 IkReal x1380=(gconst81*r20);
04853 IkReal x1381=(gconst80*r21);
04854 IkReal x1382=((1.0)*pz);
04855 IkReal x1383=(gconst80*r01);
04856 IkReal x1384=((0.352)*cj7);
04857 IkReal x1385=(gconst81*r10);
04858 IkReal x1386=((0.3215)*x1376);
04859 IkReal x1387=((0.3215)*x1375);
04860 IkReal x1388=((0.643)*pz*x1376);
04861 evalcond[0]=(x1387+((r12*x1379))+(((-1.0)*r22*x1382))+((r02*x1384)));
04862 evalcond[1]=((((-1.0)*x1382))+((x1381*x1386))+(((-1.0)*x1380*x1386))+((r22*x1387)));
04863 evalcond[2]=(x1384+(((-1.0)*x1378*x1386))+((r02*x1387))+((x1383*x1386)));
04864 evalcond[3]=(x1379+((x1377*x1386))+((r12*x1387))+(((-1.0)*x1385*x1386)));
04865 evalcond[4]=((0.0410835)+((x1381*x1388))+(((-1.0)*x1380*x1388))+(((0.643)*pz*r22*x1375)));
04866 evalcond[5]=((((-1.0)*x1386))+(((-1.0)*x1383*x1384))+((pz*x1381))+(((-1.0)*x1380*x1382))+(((-1.0)*x1377*x1379))+((x1379*x1385))+((x1378*x1384)));
04867 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
04868 {
04869 continue;
04870 }
04871 }
04872 
04873 rotationfunction0(solutions);
04874 }
04875 }
04876 
04877 }
04878 } while(0);
04879 if( bgotonextstatement )
04880 {
04881 bool bgotonextstatement = true;
04882 do
04883 {
04884 evalcond[0]=((IKabs(r21))+(IKabs(r20)));
04885 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04886 {
04887 bgotonextstatement=false;
04888 {
04889 IkReal j11eval[1];
04890 IkReal x1389=((-1.0)*pz);
04891 px=0;
04892 py=0;
04893 pp=pz*pz;
04894 npx=0;
04895 npy=0;
04896 npz=(pz*r22);
04897 rxp0_0=(pz*r10);
04898 rxp0_1=(r00*x1389);
04899 rxp0_2=0;
04900 rxp1_0=(pz*r11);
04901 rxp1_1=(r01*x1389);
04902 rxp1_2=0;
04903 rxp2_0=0;
04904 rxp2_1=0;
04905 rxp2_2=0;
04906 r20=0;
04907 r21=0;
04908 r02=0;
04909 r12=0;
04910 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
04911 if( IKabs(j11eval[0]) < 0.0000010000000000  )
04912 {
04913 {
04914 IkReal j11eval[1];
04915 IkReal x1390=((-1.0)*pz);
04916 px=0;
04917 py=0;
04918 pp=pz*pz;
04919 npx=0;
04920 npy=0;
04921 npz=(pz*r22);
04922 rxp0_0=(pz*r10);
04923 rxp0_1=(r00*x1390);
04924 rxp0_2=0;
04925 rxp1_0=(pz*r11);
04926 rxp1_1=(r01*x1390);
04927 rxp1_2=0;
04928 rxp2_0=0;
04929 rxp2_1=0;
04930 rxp2_2=0;
04931 r20=0;
04932 r21=0;
04933 r02=0;
04934 r12=0;
04935 j11eval[0]=(((r11*sj12))+(((-1.0)*cj12*r10)));
04936 if( IKabs(j11eval[0]) < 0.0000010000000000  )
04937 {
04938 {
04939 IkReal j11eval[3];
04940 IkReal x1391=((-1.0)*pz);
04941 px=0;
04942 py=0;
04943 pp=pz*pz;
04944 npx=0;
04945 npy=0;
04946 npz=(pz*r22);
04947 rxp0_0=(pz*r10);
04948 rxp0_1=(r00*x1391);
04949 rxp0_2=0;
04950 rxp1_0=(pz*r11);
04951 rxp1_1=(r01*x1391);
04952 rxp1_2=0;
04953 rxp2_0=0;
04954 rxp2_1=0;
04955 rxp2_2=0;
04956 r20=0;
04957 r21=0;
04958 r02=0;
04959 r12=0;
04960 j11eval[0]=((((-1.0)*cj12*r00))+((r01*sj12)));
04961 j11eval[1]=pz;
04962 j11eval[2]=r22;
04963 if( IKabs(j11eval[0]) < 0.0000010000000000  || IKabs(j11eval[1]) < 0.0000010000000000  || IKabs(j11eval[2]) < 0.0000010000000000  )
04964 {
04965 {
04966 IkReal evalcond[1];
04967 bool bgotonextstatement = true;
04968 do
04969 {
04970 evalcond[0]=IKabs(pz);
04971 if( IKabs(evalcond[0]) < 0.0000050000000000  )
04972 {
04973 bgotonextstatement=false;
04974 {
04975 IkReal j11array[2], cj11array[2], sj11array[2];
04976 bool j11valid[2]={false};
04977 _nj11 = 2;
04978 IkReal x1392=((1.09486780715397)*cj12);
04979 IkReal x1393=((1.09486780715397)*sj12);
04980 sj11array[0]=((((-1.0)*cj7*r01*x1393))+(((-1.0)*r11*sj7*x1393))+((cj7*r00*x1392))+((r10*sj7*x1392)));
04981 if( sj11array[0] >= -1-IKFAST_SINCOS_THRESH && sj11array[0] <= 1+IKFAST_SINCOS_THRESH )
04982 {
04983     j11valid[0] = j11valid[1] = true;
04984     j11array[0] = IKasin(sj11array[0]);
04985     cj11array[0] = IKcos(j11array[0]);
04986     sj11array[1] = sj11array[0];
04987     j11array[1] = j11array[0] > 0 ? (IKPI-j11array[0]) : (-IKPI-j11array[0]);
04988     cj11array[1] = -cj11array[0];
04989 }
04990 else if( isnan(sj11array[0]) )
04991 {
04992     // probably any value will work
04993     j11valid[0] = true;
04994     cj11array[0] = 1; sj11array[0] = 0; j11array[0] = 0;
04995 }
04996 for(int ij11 = 0; ij11 < 2; ++ij11)
04997 {
04998 if( !j11valid[ij11] )
04999 {
05000     continue;
05001 }
05002 _ij11[0] = ij11; _ij11[1] = -1;
05003 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
05004 {
05005 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05006 {
05007     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05008 }
05009 }
05010 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05011 {
05012 IkReal evalcond[4];
05013 IkReal x1394=IKcos(j11);
05014 IkReal x1395=IKsin(j11);
05015 IkReal x1396=((0.3215)*x1395);
05016 IkReal x1397=((0.3215)*x1394);
05017 evalcond[0]=x1397;
05018 evalcond[1]=(r22*x1397);
05019 evalcond[2]=(((r01*sj12*x1396))+(((-1.0)*cj12*r00*x1396))+(((0.352)*cj7)));
05020 evalcond[3]=((((-1.0)*cj12*r10*x1396))+(((0.352)*sj7))+((r11*sj12*x1396)));
05021 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
05022 {
05023 continue;
05024 }
05025 }
05026 
05027 rotationfunction0(solutions);
05028 }
05029 }
05030 
05031 }
05032 } while(0);
05033 if( bgotonextstatement )
05034 {
05035 bool bgotonextstatement = true;
05036 do
05037 {
05038 if( 1 )
05039 {
05040 bgotonextstatement=false;
05041 continue; // branch miss [j11]
05042 
05043 }
05044 } while(0);
05045 if( bgotonextstatement )
05046 {
05047 }
05048 }
05049 }
05050 
05051 } else
05052 {
05053 {
05054 IkReal j11array[1], cj11array[1], sj11array[1];
05055 bool j11valid[1]={false};
05056 _nj11 = 1;
05057 CheckValue<IkReal> x1398=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
05058 if(!x1398.valid){
05059 continue;
05060 }
05061 CheckValue<IkReal> x1399=IKPowWithIntegerCheck(pz,-1);
05062 if(!x1399.valid){
05063 continue;
05064 }
05065 CheckValue<IkReal> x1400=IKPowWithIntegerCheck(r22,-1);
05066 if(!x1400.valid){
05067 continue;
05068 }
05069 if( IKabs(((-44.0)*cj7*(x1398.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.063893468118196)*(x1399.value)*(x1400.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*cj7*(x1398.value)))+IKsqr(((-0.063893468118196)*(x1399.value)*(x1400.value)))-1) <= IKFAST_SINCOS_THRESH )
05070     continue;
05071 j11array[0]=IKatan2(((-44.0)*cj7*(x1398.value)), ((-0.063893468118196)*(x1399.value)*(x1400.value)));
05072 sj11array[0]=IKsin(j11array[0]);
05073 cj11array[0]=IKcos(j11array[0]);
05074 if( j11array[0] > IKPI )
05075 {
05076     j11array[0]-=IK2PI;
05077 }
05078 else if( j11array[0] < -IKPI )
05079 {    j11array[0]+=IK2PI;
05080 }
05081 j11valid[0] = true;
05082 for(int ij11 = 0; ij11 < 1; ++ij11)
05083 {
05084 if( !j11valid[ij11] )
05085 {
05086     continue;
05087 }
05088 _ij11[0] = ij11; _ij11[1] = -1;
05089 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05090 {
05091 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05092 {
05093     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05094 }
05095 }
05096 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05097 {
05098 IkReal evalcond[6];
05099 IkReal x1401=IKcos(j11);
05100 IkReal x1402=IKsin(j11);
05101 IkReal x1403=((0.352)*sj7);
05102 IkReal x1404=(cj12*r10);
05103 IkReal x1405=(r01*sj12);
05104 IkReal x1406=(r11*sj12);
05105 IkReal x1407=((1.0)*pz);
05106 IkReal x1408=((0.352)*cj7);
05107 IkReal x1409=(cj12*r00);
05108 IkReal x1410=((0.3215)*x1402);
05109 IkReal x1411=(r22*x1401);
05110 evalcond[0]=((0.0410835)+(((0.643)*pz*x1411)));
05111 evalcond[1]=((((-1.0)*r22*x1407))+(((0.3215)*x1401)));
05112 evalcond[2]=((((0.3215)*x1411))+(((-1.0)*x1407)));
05113 evalcond[3]=(((x1405*x1410))+x1408+(((-1.0)*x1409*x1410)));
05114 evalcond[4]=((((-1.0)*x1404*x1410))+((x1406*x1410))+x1403);
05115 evalcond[5]=((((-1.0)*x1410))+(((-1.0)*x1405*x1408))+((x1403*x1404))+((x1408*x1409))+(((-1.0)*x1403*x1406)));
05116 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05117 {
05118 continue;
05119 }
05120 }
05121 
05122 rotationfunction0(solutions);
05123 }
05124 }
05125 
05126 }
05127 
05128 }
05129 
05130 } else
05131 {
05132 {
05133 IkReal j11array[1], cj11array[1], sj11array[1];
05134 bool j11valid[1]={false};
05135 _nj11 = 1;
05136 CheckValue<IkReal> x1412=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
05137 if(!x1412.valid){
05138 continue;
05139 }
05140 if( IKabs(((-44.0)*sj7*(x1412.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((3.1104199066874)*pz*r22)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*sj7*(x1412.value)))+IKsqr(((3.1104199066874)*pz*r22))-1) <= IKFAST_SINCOS_THRESH )
05141     continue;
05142 j11array[0]=IKatan2(((-44.0)*sj7*(x1412.value)), ((3.1104199066874)*pz*r22));
05143 sj11array[0]=IKsin(j11array[0]);
05144 cj11array[0]=IKcos(j11array[0]);
05145 if( j11array[0] > IKPI )
05146 {
05147     j11array[0]-=IK2PI;
05148 }
05149 else if( j11array[0] < -IKPI )
05150 {    j11array[0]+=IK2PI;
05151 }
05152 j11valid[0] = true;
05153 for(int ij11 = 0; ij11 < 1; ++ij11)
05154 {
05155 if( !j11valid[ij11] )
05156 {
05157     continue;
05158 }
05159 _ij11[0] = ij11; _ij11[1] = -1;
05160 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05161 {
05162 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05163 {
05164     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05165 }
05166 }
05167 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05168 {
05169 IkReal evalcond[6];
05170 IkReal x1413=IKcos(j11);
05171 IkReal x1414=IKsin(j11);
05172 IkReal x1415=((0.352)*sj7);
05173 IkReal x1416=(cj12*r10);
05174 IkReal x1417=(r01*sj12);
05175 IkReal x1418=(r11*sj12);
05176 IkReal x1419=((1.0)*pz);
05177 IkReal x1420=((0.352)*cj7);
05178 IkReal x1421=(cj12*r00);
05179 IkReal x1422=((0.3215)*x1414);
05180 IkReal x1423=(r22*x1413);
05181 evalcond[0]=((0.0410835)+(((0.643)*pz*x1423)));
05182 evalcond[1]=((((0.3215)*x1413))+(((-1.0)*r22*x1419)));
05183 evalcond[2]=((((-1.0)*x1419))+(((0.3215)*x1423)));
05184 evalcond[3]=((((-1.0)*x1421*x1422))+x1420+((x1417*x1422)));
05185 evalcond[4]=((((-1.0)*x1416*x1422))+x1415+((x1418*x1422)));
05186 evalcond[5]=((((-1.0)*x1415*x1418))+((x1415*x1416))+((x1420*x1421))+(((-1.0)*x1417*x1420))+(((-1.0)*x1422)));
05187 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05188 {
05189 continue;
05190 }
05191 }
05192 
05193 rotationfunction0(solutions);
05194 }
05195 }
05196 
05197 }
05198 
05199 }
05200 
05201 } else
05202 {
05203 {
05204 IkReal j11array[1], cj11array[1], sj11array[1];
05205 bool j11valid[1]={false};
05206 _nj11 = 1;
05207 CheckValue<IkReal> x1424=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
05208 if(!x1424.valid){
05209 continue;
05210 }
05211 if( IKabs(((-44.0)*cj7*(x1424.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((3.1104199066874)*pz*r22)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-44.0)*cj7*(x1424.value)))+IKsqr(((3.1104199066874)*pz*r22))-1) <= IKFAST_SINCOS_THRESH )
05212     continue;
05213 j11array[0]=IKatan2(((-44.0)*cj7*(x1424.value)), ((3.1104199066874)*pz*r22));
05214 sj11array[0]=IKsin(j11array[0]);
05215 cj11array[0]=IKcos(j11array[0]);
05216 if( j11array[0] > IKPI )
05217 {
05218     j11array[0]-=IK2PI;
05219 }
05220 else if( j11array[0] < -IKPI )
05221 {    j11array[0]+=IK2PI;
05222 }
05223 j11valid[0] = true;
05224 for(int ij11 = 0; ij11 < 1; ++ij11)
05225 {
05226 if( !j11valid[ij11] )
05227 {
05228     continue;
05229 }
05230 _ij11[0] = ij11; _ij11[1] = -1;
05231 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05232 {
05233 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05234 {
05235     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05236 }
05237 }
05238 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05239 {
05240 IkReal evalcond[6];
05241 IkReal x1425=IKcos(j11);
05242 IkReal x1426=IKsin(j11);
05243 IkReal x1427=((0.352)*sj7);
05244 IkReal x1428=(cj12*r10);
05245 IkReal x1429=(r01*sj12);
05246 IkReal x1430=(r11*sj12);
05247 IkReal x1431=((1.0)*pz);
05248 IkReal x1432=((0.352)*cj7);
05249 IkReal x1433=(cj12*r00);
05250 IkReal x1434=((0.3215)*x1426);
05251 IkReal x1435=(r22*x1425);
05252 evalcond[0]=((0.0410835)+(((0.643)*pz*x1435)));
05253 evalcond[1]=((((0.3215)*x1425))+(((-1.0)*r22*x1431)));
05254 evalcond[2]=((((-1.0)*x1431))+(((0.3215)*x1435)));
05255 evalcond[3]=(((x1429*x1434))+x1432+(((-1.0)*x1433*x1434)));
05256 evalcond[4]=(((x1430*x1434))+(((-1.0)*x1428*x1434))+x1427);
05257 evalcond[5]=(((x1432*x1433))+(((-1.0)*x1429*x1432))+(((-1.0)*x1434))+(((-1.0)*x1427*x1430))+((x1427*x1428)));
05258 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05259 {
05260 continue;
05261 }
05262 }
05263 
05264 rotationfunction0(solutions);
05265 }
05266 }
05267 
05268 }
05269 
05270 }
05271 
05272 }
05273 } while(0);
05274 if( bgotonextstatement )
05275 {
05276 bool bgotonextstatement = true;
05277 do
05278 {
05279 if( 1 )
05280 {
05281 bgotonextstatement=false;
05282 continue; // branch miss [j11]
05283 
05284 }
05285 } while(0);
05286 if( bgotonextstatement )
05287 {
05288 }
05289 }
05290 }
05291 }
05292 }
05293 }
05294 }
05295 }
05296 }
05297 
05298 } else
05299 {
05300 {
05301 IkReal j11array[1], cj11array[1], sj11array[1];
05302 bool j11valid[1]={false};
05303 _nj11 = 1;
05304 IkReal x1436=((44.0)*sj7);
05305 IkReal x1437=(pz*r22);
05306 IkReal x1438=(cj7*r02);
05307 CheckValue<IkReal> x1439=IKPowWithIntegerCheck(((((40.1875)*r11*sj12))+(((-40.1875)*cj12*r10))),-1);
05308 if(!x1439.valid){
05309 continue;
05310 }
05311 if( IKabs(((x1439.value)*((((x1436*(r12*r12)))+(((-1.0)*x1436))+(((44.0)*r12*x1438))+(((-125.0)*r12*x1437)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1438))+(((3.1104199066874)*x1437))+(((-1.09486780715397)*r12*sj7)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1439.value)*((((x1436*(r12*r12)))+(((-1.0)*x1436))+(((44.0)*r12*x1438))+(((-125.0)*r12*x1437))))))+IKsqr(((((-1.09486780715397)*x1438))+(((3.1104199066874)*x1437))+(((-1.09486780715397)*r12*sj7))))-1) <= IKFAST_SINCOS_THRESH )
05312     continue;
05313 j11array[0]=IKatan2(((x1439.value)*((((x1436*(r12*r12)))+(((-1.0)*x1436))+(((44.0)*r12*x1438))+(((-125.0)*r12*x1437))))), ((((-1.09486780715397)*x1438))+(((3.1104199066874)*x1437))+(((-1.09486780715397)*r12*sj7))));
05314 sj11array[0]=IKsin(j11array[0]);
05315 cj11array[0]=IKcos(j11array[0]);
05316 if( j11array[0] > IKPI )
05317 {
05318     j11array[0]-=IK2PI;
05319 }
05320 else if( j11array[0] < -IKPI )
05321 {    j11array[0]+=IK2PI;
05322 }
05323 j11valid[0] = true;
05324 for(int ij11 = 0; ij11 < 1; ++ij11)
05325 {
05326 if( !j11valid[ij11] )
05327 {
05328     continue;
05329 }
05330 _ij11[0] = ij11; _ij11[1] = -1;
05331 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05332 {
05333 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05334 {
05335     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05336 }
05337 }
05338 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05339 {
05340 IkReal evalcond[6];
05341 IkReal x1440=IKcos(j11);
05342 IkReal x1441=IKsin(j11);
05343 IkReal x1442=((0.643)*pz);
05344 IkReal x1443=((0.352)*sj7);
05345 IkReal x1444=(cj12*r10);
05346 IkReal x1445=((0.352)*cj7);
05347 IkReal x1446=(cj12*r00);
05348 IkReal x1447=((1.0)*pz);
05349 IkReal x1448=(cj12*r20);
05350 IkReal x1449=(sj12*x1441);
05351 IkReal x1450=((0.3215)*x1440);
05352 IkReal x1451=((0.3215)*x1441);
05353 evalcond[0]=((((-1.0)*r22*x1447))+((r12*x1443))+((r02*x1445))+x1450);
05354 evalcond[1]=((((-1.0)*x1448*x1451))+(((0.3215)*r21*x1449))+(((-1.0)*x1447))+((r22*x1450)));
05355 evalcond[2]=((((0.3215)*r01*x1449))+(((-1.0)*x1446*x1451))+x1445+((r02*x1450)));
05356 evalcond[3]=((((0.3215)*r11*x1449))+((r12*x1450))+x1443+(((-1.0)*x1444*x1451)));
05357 evalcond[4]=((0.0410835)+((r21*x1442*x1449))+(((-1.0)*x1441*x1442*x1448))+((r22*x1440*x1442)));
05358 evalcond[5]=(((x1445*x1446))+((pz*r21*sj12))+((x1443*x1444))+(((-1.0)*x1451))+(((-1.0)*x1447*x1448))+(((-1.0)*r01*sj12*x1445))+(((-1.0)*r11*sj12*x1443)));
05359 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05360 {
05361 continue;
05362 }
05363 }
05364 
05365 rotationfunction0(solutions);
05366 }
05367 }
05368 
05369 }
05370 
05371 }
05372 
05373 } else
05374 {
05375 {
05376 IkReal j11array[1], cj11array[1], sj11array[1];
05377 bool j11valid[1]={false};
05378 _nj11 = 1;
05379 IkReal x1452=(pz*r22);
05380 IkReal x1453=(r12*sj7);
05381 IkReal x1454=((44.0)*cj7);
05382 CheckValue<IkReal> x1455=IKPowWithIntegerCheck(((((-40.1875)*cj12*r00))+(((40.1875)*r01*sj12))),-1);
05383 if(!x1455.valid){
05384 continue;
05385 }
05386 if( IKabs(((x1455.value)*((((x1454*(r02*r02)))+(((-1.0)*x1454))+(((-125.0)*r02*x1452))+(((44.0)*r02*x1453)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1453))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x1452)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1455.value)*((((x1454*(r02*r02)))+(((-1.0)*x1454))+(((-125.0)*r02*x1452))+(((44.0)*r02*x1453))))))+IKsqr(((((-1.09486780715397)*x1453))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x1452))))-1) <= IKFAST_SINCOS_THRESH )
05387     continue;
05388 j11array[0]=IKatan2(((x1455.value)*((((x1454*(r02*r02)))+(((-1.0)*x1454))+(((-125.0)*r02*x1452))+(((44.0)*r02*x1453))))), ((((-1.09486780715397)*x1453))+(((-1.09486780715397)*cj7*r02))+(((3.1104199066874)*x1452))));
05389 sj11array[0]=IKsin(j11array[0]);
05390 cj11array[0]=IKcos(j11array[0]);
05391 if( j11array[0] > IKPI )
05392 {
05393     j11array[0]-=IK2PI;
05394 }
05395 else if( j11array[0] < -IKPI )
05396 {    j11array[0]+=IK2PI;
05397 }
05398 j11valid[0] = true;
05399 for(int ij11 = 0; ij11 < 1; ++ij11)
05400 {
05401 if( !j11valid[ij11] )
05402 {
05403     continue;
05404 }
05405 _ij11[0] = ij11; _ij11[1] = -1;
05406 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05407 {
05408 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05409 {
05410     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05411 }
05412 }
05413 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05414 {
05415 IkReal evalcond[6];
05416 IkReal x1456=IKcos(j11);
05417 IkReal x1457=IKsin(j11);
05418 IkReal x1458=((0.643)*pz);
05419 IkReal x1459=((0.352)*sj7);
05420 IkReal x1460=(cj12*r10);
05421 IkReal x1461=((0.352)*cj7);
05422 IkReal x1462=(cj12*r00);
05423 IkReal x1463=((1.0)*pz);
05424 IkReal x1464=(cj12*r20);
05425 IkReal x1465=(sj12*x1457);
05426 IkReal x1466=((0.3215)*x1456);
05427 IkReal x1467=((0.3215)*x1457);
05428 evalcond[0]=((((-1.0)*r22*x1463))+((r12*x1459))+x1466+((r02*x1461)));
05429 evalcond[1]=((((-1.0)*x1463))+(((-1.0)*x1464*x1467))+((r22*x1466))+(((0.3215)*r21*x1465)));
05430 evalcond[2]=((((-1.0)*x1462*x1467))+x1461+((r02*x1466))+(((0.3215)*r01*x1465)));
05431 evalcond[3]=(((r12*x1466))+(((-1.0)*x1460*x1467))+x1459+(((0.3215)*r11*x1465)));
05432 evalcond[4]=((0.0410835)+((r21*x1458*x1465))+((r22*x1456*x1458))+(((-1.0)*x1457*x1458*x1464)));
05433 evalcond[5]=(((pz*r21*sj12))+(((-1.0)*r11*sj12*x1459))+(((-1.0)*r01*sj12*x1461))+(((-1.0)*x1467))+((x1459*x1460))+((x1461*x1462))+(((-1.0)*x1463*x1464)));
05434 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05435 {
05436 continue;
05437 }
05438 }
05439 
05440 rotationfunction0(solutions);
05441 }
05442 }
05443 
05444 }
05445 
05446 }
05447 
05448 } else
05449 {
05450 {
05451 IkReal j11array[1], cj11array[1], sj11array[1];
05452 bool j11valid[1]={false};
05453 _nj11 = 1;
05454 IkReal x1468=((0.352)*r22);
05455 IkReal x1469=(r12*sj7);
05456 IkReal x1470=(cj7*r02);
05457 CheckValue<IkReal> x1471=IKPowWithIntegerCheck(((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12))),-1);
05458 if(!x1471.valid){
05459 continue;
05460 }
05461 if( IKabs(((x1471.value)*(((((-1.0)*pz*(r22*r22)))+((x1468*x1469))+pz+((x1468*x1470)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.09486780715397)*x1469))+(((-1.09486780715397)*x1470))+(((3.1104199066874)*pz*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1471.value)*(((((-1.0)*pz*(r22*r22)))+((x1468*x1469))+pz+((x1468*x1470))))))+IKsqr(((((-1.09486780715397)*x1469))+(((-1.09486780715397)*x1470))+(((3.1104199066874)*pz*r22))))-1) <= IKFAST_SINCOS_THRESH )
05462     continue;
05463 j11array[0]=IKatan2(((x1471.value)*(((((-1.0)*pz*(r22*r22)))+((x1468*x1469))+pz+((x1468*x1470))))), ((((-1.09486780715397)*x1469))+(((-1.09486780715397)*x1470))+(((3.1104199066874)*pz*r22))));
05464 sj11array[0]=IKsin(j11array[0]);
05465 cj11array[0]=IKcos(j11array[0]);
05466 if( j11array[0] > IKPI )
05467 {
05468     j11array[0]-=IK2PI;
05469 }
05470 else if( j11array[0] < -IKPI )
05471 {    j11array[0]+=IK2PI;
05472 }
05473 j11valid[0] = true;
05474 for(int ij11 = 0; ij11 < 1; ++ij11)
05475 {
05476 if( !j11valid[ij11] )
05477 {
05478     continue;
05479 }
05480 _ij11[0] = ij11; _ij11[1] = -1;
05481 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
05482 {
05483 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05484 {
05485     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05486 }
05487 }
05488 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05489 {
05490 IkReal evalcond[6];
05491 IkReal x1472=IKcos(j11);
05492 IkReal x1473=IKsin(j11);
05493 IkReal x1474=((0.643)*pz);
05494 IkReal x1475=((0.352)*sj7);
05495 IkReal x1476=(cj12*r10);
05496 IkReal x1477=((0.352)*cj7);
05497 IkReal x1478=(cj12*r00);
05498 IkReal x1479=((1.0)*pz);
05499 IkReal x1480=(cj12*r20);
05500 IkReal x1481=(sj12*x1473);
05501 IkReal x1482=((0.3215)*x1472);
05502 IkReal x1483=((0.3215)*x1473);
05503 evalcond[0]=((((-1.0)*r22*x1479))+((r12*x1475))+((r02*x1477))+x1482);
05504 evalcond[1]=((((-1.0)*x1480*x1483))+(((-1.0)*x1479))+((r22*x1482))+(((0.3215)*r21*x1481)));
05505 evalcond[2]=((((0.3215)*r01*x1481))+x1477+((r02*x1482))+(((-1.0)*x1478*x1483)));
05506 evalcond[3]=(((r12*x1482))+x1475+(((0.3215)*r11*x1481))+(((-1.0)*x1476*x1483)));
05507 evalcond[4]=((0.0410835)+((r21*x1474*x1481))+((r22*x1472*x1474))+(((-1.0)*x1473*x1474*x1480)));
05508 evalcond[5]=(((pz*r21*sj12))+(((-1.0)*x1479*x1480))+(((-1.0)*x1483))+(((-1.0)*r01*sj12*x1477))+(((-1.0)*r11*sj12*x1475))+((x1477*x1478))+((x1475*x1476)));
05509 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
05510 {
05511 continue;
05512 }
05513 }
05514 
05515 rotationfunction0(solutions);
05516 }
05517 }
05518 
05519 }
05520 
05521 }
05522 }
05523 }
05524 
05525 }
05526 
05527 }
05528 
05529 } else
05530 {
05531 {
05532 IkReal j11array[2], cj11array[2], sj11array[2];
05533 bool j11valid[2]={false};
05534 _nj11 = 2;
05535 IkReal x1484=((0.643)*pz);
05536 IkReal x1485=((((-1.0)*cj12*r20*x1484))+((r21*sj12*x1484)));
05537 CheckValue<IkReal> x1488 = IKatan2WithCheck(IkReal((r22*x1484)),IkReal(x1485),IKFAST_ATAN2_MAGTHRESH);
05538 if(!x1488.valid){
05539 continue;
05540 }
05541 IkReal x1486=((1.0)*(x1488.value));
05542 if((((((0.413449)*(pz*pz)*(r22*r22)))+(x1485*x1485))) < -0.00001)
05543 continue;
05544 CheckValue<IkReal> x1489=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.413449)*(pz*pz)*(r22*r22)))+(x1485*x1485)))),-1);
05545 if(!x1489.valid){
05546 continue;
05547 }
05548 if( (((0.0410835)*(x1489.value))) < -1-IKFAST_SINCOS_THRESH || (((0.0410835)*(x1489.value))) > 1+IKFAST_SINCOS_THRESH )
05549     continue;
05550 IkReal x1487=IKasin(((0.0410835)*(x1489.value)));
05551 j11array[0]=((((-1.0)*x1486))+(((-1.0)*x1487)));
05552 sj11array[0]=IKsin(j11array[0]);
05553 cj11array[0]=IKcos(j11array[0]);
05554 j11array[1]=((3.14159265358979)+(((-1.0)*x1486))+x1487);
05555 sj11array[1]=IKsin(j11array[1]);
05556 cj11array[1]=IKcos(j11array[1]);
05557 if( j11array[0] > IKPI )
05558 {
05559     j11array[0]-=IK2PI;
05560 }
05561 else if( j11array[0] < -IKPI )
05562 {    j11array[0]+=IK2PI;
05563 }
05564 j11valid[0] = true;
05565 if( j11array[1] > IKPI )
05566 {
05567     j11array[1]-=IK2PI;
05568 }
05569 else if( j11array[1] < -IKPI )
05570 {    j11array[1]+=IK2PI;
05571 }
05572 j11valid[1] = true;
05573 for(int ij11 = 0; ij11 < 2; ++ij11)
05574 {
05575 if( !j11valid[ij11] )
05576 {
05577     continue;
05578 }
05579 _ij11[0] = ij11; _ij11[1] = -1;
05580 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
05581 {
05582 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05583 {
05584     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05585 }
05586 }
05587 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05588 {
05589 IkReal evalcond[1];
05590 IkReal x1490=IKsin(j11);
05591 IkReal x1491=((0.3215)*x1490);
05592 evalcond[0]=(((r21*sj12*x1491))+(((-1.0)*pz))+(((0.3215)*r22*(IKcos(j11))))+(((-1.0)*cj12*r20*x1491)));
05593 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
05594 {
05595 continue;
05596 }
05597 }
05598 
05599 {
05600 IkReal j7array[1], cj7array[1], sj7array[1];
05601 bool j7valid[1]={false};
05602 _nj7 = 1;
05603 IkReal x1492=((0.913352272727273)*cj11);
05604 IkReal x1493=((0.913352272727273)*cj12*sj11);
05605 IkReal x1494=((0.913352272727273)*sj11*sj12);
05606 if( IKabs((((r10*x1493))+(((-1.0)*r12*x1492))+(((-1.0)*r11*x1494)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r00*x1493))+(((-1.0)*r01*x1494))+(((-1.0)*r02*x1492)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((r10*x1493))+(((-1.0)*r12*x1492))+(((-1.0)*r11*x1494))))+IKsqr((((r00*x1493))+(((-1.0)*r01*x1494))+(((-1.0)*r02*x1492))))-1) <= IKFAST_SINCOS_THRESH )
05607     continue;
05608 j7array[0]=IKatan2((((r10*x1493))+(((-1.0)*r12*x1492))+(((-1.0)*r11*x1494))), (((r00*x1493))+(((-1.0)*r01*x1494))+(((-1.0)*r02*x1492))));
05609 sj7array[0]=IKsin(j7array[0]);
05610 cj7array[0]=IKcos(j7array[0]);
05611 if( j7array[0] > IKPI )
05612 {
05613     j7array[0]-=IK2PI;
05614 }
05615 else if( j7array[0] < -IKPI )
05616 {    j7array[0]+=IK2PI;
05617 }
05618 j7valid[0] = true;
05619 for(int ij7 = 0; ij7 < 1; ++ij7)
05620 {
05621 if( !j7valid[ij7] )
05622 {
05623     continue;
05624 }
05625 _ij7[0] = ij7; _ij7[1] = -1;
05626 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
05627 {
05628 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
05629 {
05630     j7valid[iij7]=false; _ij7[1] = iij7; break; 
05631 }
05632 }
05633 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
05634 {
05635 IkReal evalcond[5];
05636 IkReal x1495=IKcos(j7);
05637 IkReal x1496=IKsin(j7);
05638 IkReal x1497=(cj12*r00);
05639 IkReal x1498=(r11*sj12);
05640 IkReal x1499=(r01*sj12);
05641 IkReal x1500=(cj12*r10);
05642 IkReal x1501=((0.3215)*sj11);
05643 IkReal x1502=((0.3215)*cj11);
05644 IkReal x1503=((1.0)*pz);
05645 IkReal x1504=((0.352)*x1495);
05646 IkReal x1505=((0.352)*x1496);
05647 IkReal x1506=(cj12*x1503);
05648 evalcond[0]=((((-1.0)*r22*x1503))+((r12*x1505))+x1502+((r02*x1504)));
05649 evalcond[1]=(((x1499*x1501))+x1504+((r02*x1502))+(((-1.0)*x1497*x1501)));
05650 evalcond[2]=(((x1498*x1501))+((r12*x1502))+x1505+(((-1.0)*x1500*x1501)));
05651 evalcond[3]=(((cj12*r01*x1504))+((cj12*r11*x1505))+((r00*sj12*x1504))+((r10*sj12*x1505))+(((-1.0)*r20*sj12*x1503))+(((-1.0)*r21*x1506)));
05652 evalcond[4]=((((-1.0)*r20*x1506))+(((-1.0)*x1501))+(((-1.0)*x1499*x1504))+((pz*r21*sj12))+(((-1.0)*x1498*x1505))+((x1497*x1504))+((x1500*x1505)));
05653 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
05654 {
05655 continue;
05656 }
05657 }
05658 
05659 rotationfunction0(solutions);
05660 }
05661 }
05662 }
05663 }
05664 
05665 }
05666 
05667 }
05668 
05669 } else
05670 {
05671 {
05672 IkReal j11array[2], cj11array[2], sj11array[2];
05673 bool j11valid[2]={false};
05674 _nj11 = 2;
05675 IkReal x1507=((((-0.3215)*cj12*r20))+(((0.3215)*r21*sj12)));
05676 CheckValue<IkReal> x1510 = IKatan2WithCheck(IkReal(((0.3215)*r22)),IkReal(x1507),IKFAST_ATAN2_MAGTHRESH);
05677 if(!x1510.valid){
05678 continue;
05679 }
05680 IkReal x1508=((1.0)*(x1510.value));
05681 if((((((0.10336225)*(r22*r22)))+(x1507*x1507))) < -0.00001)
05682 continue;
05683 CheckValue<IkReal> x1511=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.10336225)*(r22*r22)))+(x1507*x1507)))),-1);
05684 if(!x1511.valid){
05685 continue;
05686 }
05687 if( ((pz*(x1511.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x1511.value))) > 1+IKFAST_SINCOS_THRESH )
05688     continue;
05689 IkReal x1509=IKasin((pz*(x1511.value)));
05690 j11array[0]=((((-1.0)*x1508))+x1509);
05691 sj11array[0]=IKsin(j11array[0]);
05692 cj11array[0]=IKcos(j11array[0]);
05693 j11array[1]=((3.14159265358979)+(((-1.0)*x1508))+(((-1.0)*x1509)));
05694 sj11array[1]=IKsin(j11array[1]);
05695 cj11array[1]=IKcos(j11array[1]);
05696 if( j11array[0] > IKPI )
05697 {
05698     j11array[0]-=IK2PI;
05699 }
05700 else if( j11array[0] < -IKPI )
05701 {    j11array[0]+=IK2PI;
05702 }
05703 j11valid[0] = true;
05704 if( j11array[1] > IKPI )
05705 {
05706     j11array[1]-=IK2PI;
05707 }
05708 else if( j11array[1] < -IKPI )
05709 {    j11array[1]+=IK2PI;
05710 }
05711 j11valid[1] = true;
05712 for(int ij11 = 0; ij11 < 2; ++ij11)
05713 {
05714 if( !j11valid[ij11] )
05715 {
05716     continue;
05717 }
05718 _ij11[0] = ij11; _ij11[1] = -1;
05719 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
05720 {
05721 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05722 {
05723     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05724 }
05725 }
05726 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05727 {
05728 IkReal evalcond[1];
05729 IkReal x1512=IKsin(j11);
05730 IkReal x1513=((0.643)*pz);
05731 evalcond[0]=((0.0410835)+(((-1.0)*cj12*r20*x1512*x1513))+((r21*sj12*x1512*x1513))+((r22*x1513*(IKcos(j11)))));
05732 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
05733 {
05734 continue;
05735 }
05736 }
05737 
05738 {
05739 IkReal j7array[1], cj7array[1], sj7array[1];
05740 bool j7valid[1]={false};
05741 _nj7 = 1;
05742 IkReal x1514=((0.913352272727273)*cj11);
05743 IkReal x1515=((0.913352272727273)*cj12*sj11);
05744 IkReal x1516=((0.913352272727273)*sj11*sj12);
05745 if( IKabs((((r10*x1515))+(((-1.0)*r11*x1516))+(((-1.0)*r12*x1514)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r01*x1516))+((r00*x1515))+(((-1.0)*r02*x1514)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((r10*x1515))+(((-1.0)*r11*x1516))+(((-1.0)*r12*x1514))))+IKsqr(((((-1.0)*r01*x1516))+((r00*x1515))+(((-1.0)*r02*x1514))))-1) <= IKFAST_SINCOS_THRESH )
05746     continue;
05747 j7array[0]=IKatan2((((r10*x1515))+(((-1.0)*r11*x1516))+(((-1.0)*r12*x1514))), ((((-1.0)*r01*x1516))+((r00*x1515))+(((-1.0)*r02*x1514))));
05748 sj7array[0]=IKsin(j7array[0]);
05749 cj7array[0]=IKcos(j7array[0]);
05750 if( j7array[0] > IKPI )
05751 {
05752     j7array[0]-=IK2PI;
05753 }
05754 else if( j7array[0] < -IKPI )
05755 {    j7array[0]+=IK2PI;
05756 }
05757 j7valid[0] = true;
05758 for(int ij7 = 0; ij7 < 1; ++ij7)
05759 {
05760 if( !j7valid[ij7] )
05761 {
05762     continue;
05763 }
05764 _ij7[0] = ij7; _ij7[1] = -1;
05765 for(int iij7 = ij7+1; iij7 < 1; ++iij7)
05766 {
05767 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
05768 {
05769     j7valid[iij7]=false; _ij7[1] = iij7; break; 
05770 }
05771 }
05772 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
05773 {
05774 IkReal evalcond[5];
05775 IkReal x1517=IKcos(j7);
05776 IkReal x1518=IKsin(j7);
05777 IkReal x1519=(cj12*r00);
05778 IkReal x1520=(r11*sj12);
05779 IkReal x1521=(r01*sj12);
05780 IkReal x1522=(cj12*r10);
05781 IkReal x1523=((0.3215)*sj11);
05782 IkReal x1524=((0.3215)*cj11);
05783 IkReal x1525=((1.0)*pz);
05784 IkReal x1526=((0.352)*x1517);
05785 IkReal x1527=((0.352)*x1518);
05786 IkReal x1528=(cj12*x1525);
05787 evalcond[0]=(((r12*x1527))+(((-1.0)*r22*x1525))+x1524+((r02*x1526)));
05788 evalcond[1]=((((-1.0)*x1519*x1523))+x1526+((x1521*x1523))+((r02*x1524)));
05789 evalcond[2]=(((r12*x1524))+((x1520*x1523))+x1527+(((-1.0)*x1522*x1523)));
05790 evalcond[3]=(((cj12*r11*x1527))+((r10*sj12*x1527))+((r00*sj12*x1526))+((cj12*r01*x1526))+(((-1.0)*r20*sj12*x1525))+(((-1.0)*r21*x1528)));
05791 evalcond[4]=(((pz*r21*sj12))+((x1519*x1526))+((x1522*x1527))+(((-1.0)*x1523))+(((-1.0)*r20*x1528))+(((-1.0)*x1520*x1527))+(((-1.0)*x1521*x1526)));
05792 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
05793 {
05794 continue;
05795 }
05796 }
05797 
05798 rotationfunction0(solutions);
05799 }
05800 }
05801 }
05802 }
05803 
05804 }
05805 
05806 }
05807     }
05808 
05809 }
05810 
05811 }
05812 
05813 }
05814 } while(0);
05815 if( bgotonextstatement )
05816 {
05817 bool bgotonextstatement = true;
05818 do
05819 {
05820 if( 1 )
05821 {
05822 bgotonextstatement=false;
05823 continue; // branch miss [j7, j11, j12]
05824 
05825 }
05826 } while(0);
05827 if( bgotonextstatement )
05828 {
05829 }
05830 }
05831 }
05832 }
05833 
05834 } else
05835 {
05836 {
05837 IkReal j7array[2], cj7array[2], sj7array[2];
05838 bool j7valid[2]={false};
05839 _nj7 = 2;
05840 CheckValue<IkReal> x1531 = IKatan2WithCheck(IkReal(((-0.704)*px)),IkReal(((-0.704)*py)),IKFAST_ATAN2_MAGTHRESH);
05841 if(!x1531.valid){
05842 continue;
05843 }
05844 IkReal x1529=((1.0)*(x1531.value));
05845 if((((((0.495616)*(py*py)))+(((0.495616)*(px*px))))) < -0.00001)
05846 continue;
05847 CheckValue<IkReal> x1532=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.495616)*(py*py)))+(((0.495616)*(px*px)))))),-1);
05848 if(!x1532.valid){
05849 continue;
05850 }
05851 if( (((x1532.value)*(((0.02054175)+pp)))) < -1-IKFAST_SINCOS_THRESH || (((x1532.value)*(((0.02054175)+pp)))) > 1+IKFAST_SINCOS_THRESH )
05852     continue;
05853 IkReal x1530=IKasin(((x1532.value)*(((0.02054175)+pp))));
05854 j7array[0]=((((-1.0)*x1530))+(((-1.0)*x1529)));
05855 sj7array[0]=IKsin(j7array[0]);
05856 cj7array[0]=IKcos(j7array[0]);
05857 j7array[1]=((3.14159265358979)+(((-1.0)*x1529))+x1530);
05858 sj7array[1]=IKsin(j7array[1]);
05859 cj7array[1]=IKcos(j7array[1]);
05860 if( j7array[0] > IKPI )
05861 {
05862     j7array[0]-=IK2PI;
05863 }
05864 else if( j7array[0] < -IKPI )
05865 {    j7array[0]+=IK2PI;
05866 }
05867 j7valid[0] = true;
05868 if( j7array[1] > IKPI )
05869 {
05870     j7array[1]-=IK2PI;
05871 }
05872 else if( j7array[1] < -IKPI )
05873 {    j7array[1]+=IK2PI;
05874 }
05875 j7valid[1] = true;
05876 for(int ij7 = 0; ij7 < 2; ++ij7)
05877 {
05878 if( !j7valid[ij7] )
05879 {
05880     continue;
05881 }
05882 _ij7[0] = ij7; _ij7[1] = -1;
05883 for(int iij7 = ij7+1; iij7 < 2; ++iij7)
05884 {
05885 if( j7valid[iij7] && IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH )
05886 {
05887     j7valid[iij7]=false; _ij7[1] = iij7; break; 
05888 }
05889 }
05890 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
05891 
05892 {
05893 IkReal j11array[2], cj11array[2], sj11array[2];
05894 bool j11valid[2]={false};
05895 _nj11 = 2;
05896 cj11array[0]=((((3.1104199066874)*npz))+(((-1.09486780715397)*cj7*r02))+(((-1.09486780715397)*r12*sj7)));
05897 if( cj11array[0] >= -1-IKFAST_SINCOS_THRESH && cj11array[0] <= 1+IKFAST_SINCOS_THRESH )
05898 {
05899     j11valid[0] = j11valid[1] = true;
05900     j11array[0] = IKacos(cj11array[0]);
05901     sj11array[0] = IKsin(j11array[0]);
05902     cj11array[1] = cj11array[0];
05903     j11array[1] = -j11array[0];
05904     sj11array[1] = -sj11array[0];
05905 }
05906 else if( isnan(cj11array[0]) )
05907 {
05908     // probably any value will work
05909     j11valid[0] = true;
05910     cj11array[0] = 1; sj11array[0] = 0; j11array[0] = 0;
05911 }
05912 for(int ij11 = 0; ij11 < 2; ++ij11)
05913 {
05914 if( !j11valid[ij11] )
05915 {
05916     continue;
05917 }
05918 _ij11[0] = ij11; _ij11[1] = -1;
05919 for(int iij11 = ij11+1; iij11 < 2; ++iij11)
05920 {
05921 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
05922 {
05923     j11valid[iij11]=false; _ij11[1] = iij11; break; 
05924 }
05925 }
05926 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
05927 
05928 {
05929 IkReal j12eval[3];
05930 IkReal x1533=(r02*sj11);
05931 IkReal x1534=((643.0)*cj11);
05932 IkReal x1535=((704.0)*sj7);
05933 j12eval[0]=x1533;
05934 j12eval[1]=IKsign(x1533);
05935 j12eval[2]=((IKabs(((((-1.0)*r00*x1534))+((r21*x1535))+(((2000.0)*rxp1_0)))))+(IKabs((((r01*x1534))+((r20*x1535))+(((2000.0)*rxp0_0))))));
05936 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
05937 {
05938 {
05939 IkReal j12eval[3];
05940 IkReal x1536=(r12*sj11);
05941 IkReal x1537=((643.0)*cj11);
05942 IkReal x1538=((704.0)*cj7);
05943 j12eval[0]=x1536;
05944 j12eval[1]=((IKabs(((((-2000.0)*rxp1_1))+((r10*x1537))+((r21*x1538)))))+(IKabs(((((-1.0)*r11*x1537))+((r20*x1538))+(((-2000.0)*rxp0_1))))));
05945 j12eval[2]=IKsign(x1536);
05946 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
05947 {
05948 {
05949 IkReal j12eval[3];
05950 IkReal x1539=((643.0)*cj11);
05951 IkReal x1540=((2000.0)*pz);
05952 IkReal x1541=((1000.0)*pp);
05953 IkReal x1542=(npx*r21*sj11);
05954 IkReal x1543=(npy*r20*sj11);
05955 j12eval[0]=((((-1.0)*x1542))+x1543);
05956 j12eval[1]=IKsign(((((-643.0)*x1542))+(((643.0)*x1543))));
05957 j12eval[2]=((IKabs(((((-20.54175)*r20))+(((-1.0)*npz*r20*x1539))+((npx*r22*x1539))+(((-1.0)*npx*x1540))+((r20*x1541)))))+(IKabs(((((-20.54175)*r21))+((npy*r22*x1539))+((r21*x1541))+(((-1.0)*npy*x1540))+(((-1.0)*npz*r21*x1539))))));
05958 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
05959 {
05960 {
05961 IkReal evalcond[1];
05962 bool bgotonextstatement = true;
05963 do
05964 {
05965 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j11))), 6.28318530717959)));
05966 if( IKabs(evalcond[0]) < 0.0000050000000000  )
05967 {
05968 bgotonextstatement=false;
05969 {
05970 IkReal j12eval[1];
05971 sj11=0;
05972 cj11=1.0;
05973 j11=0;
05974 IkReal x1544=((0.352)*sj7);
05975 IkReal x1545=((0.352)*cj7);
05976 j12eval[0]=((IKabs((npy+(((-1.0)*r01*x1545))+(((-1.0)*r11*x1544)))))+(IKabs(((((-1.0)*npx))+((r10*x1544))+((r00*x1545))))));
05977 if( IKabs(j12eval[0]) < 0.0000010000000000  )
05978 {
05979 {
05980 IkReal j12eval[1];
05981 sj11=0;
05982 cj11=1.0;
05983 j11=0;
05984 IkReal x1546=((0.352)*sj7);
05985 IkReal x1547=((0.352)*cj7);
05986 j12eval[0]=((IKabs(((((-1.0)*npx))+((r10*x1546))+((r00*x1547)))))+(IKabs(((((-1.0)*npy))+((r11*x1546))+((r01*x1547))))));
05987 if( IKabs(j12eval[0]) < 0.0000010000000000  )
05988 {
05989 continue; // no branches [j12]
05990 
05991 } else
05992 {
05993 {
05994 IkReal j12array[2], cj12array[2], sj12array[2];
05995 bool j12valid[2]={false};
05996 _nj12 = 2;
05997 IkReal x1548=((0.352)*sj7);
05998 IkReal x1549=((0.352)*cj7);
05999 CheckValue<IkReal> x1551 = IKatan2WithCheck(IkReal(((((-1.0)*npy))+((r11*x1548))+((r01*x1549)))),IkReal(((((-1.0)*npx))+((r10*x1548))+((r00*x1549)))),IKFAST_ATAN2_MAGTHRESH);
06000 if(!x1551.valid){
06001 continue;
06002 }
06003 IkReal x1550=x1551.value;
06004 j12array[0]=((-1.0)*x1550);
06005 sj12array[0]=IKsin(j12array[0]);
06006 cj12array[0]=IKcos(j12array[0]);
06007 j12array[1]=((3.14159265358979)+(((-1.0)*x1550)));
06008 sj12array[1]=IKsin(j12array[1]);
06009 cj12array[1]=IKcos(j12array[1]);
06010 if( j12array[0] > IKPI )
06011 {
06012     j12array[0]-=IK2PI;
06013 }
06014 else if( j12array[0] < -IKPI )
06015 {    j12array[0]+=IK2PI;
06016 }
06017 j12valid[0] = true;
06018 if( j12array[1] > IKPI )
06019 {
06020     j12array[1]-=IK2PI;
06021 }
06022 else if( j12array[1] < -IKPI )
06023 {    j12array[1]+=IK2PI;
06024 }
06025 j12valid[1] = true;
06026 for(int ij12 = 0; ij12 < 2; ++ij12)
06027 {
06028 if( !j12valid[ij12] )
06029 {
06030     continue;
06031 }
06032 _ij12[0] = ij12; _ij12[1] = -1;
06033 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06034 {
06035 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06036 {
06037     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06038 }
06039 }
06040 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06041 {
06042 IkReal evalcond[1];
06043 IkReal x1552=IKcos(j12);
06044 IkReal x1553=IKsin(j12);
06045 IkReal x1554=((0.352)*x1553);
06046 IkReal x1555=((0.352)*x1552);
06047 evalcond[0]=(((r10*sj7*x1555))+(((-1.0)*r11*sj7*x1554))+(((-1.0)*npx*x1552))+((npy*x1553))+((cj7*r00*x1555))+(((-1.0)*cj7*r01*x1554)));
06048 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06049 {
06050 continue;
06051 }
06052 }
06053 
06054 rotationfunction0(solutions);
06055 }
06056 }
06057 
06058 }
06059 
06060 }
06061 
06062 } else
06063 {
06064 {
06065 IkReal j12array[2], cj12array[2], sj12array[2];
06066 bool j12valid[2]={false};
06067 _nj12 = 2;
06068 IkReal x1556=((0.352)*sj7);
06069 IkReal x1557=((0.352)*cj7);
06070 CheckValue<IkReal> x1559 = IKatan2WithCheck(IkReal((((r00*x1557))+(((-1.0)*npx))+((r10*x1556)))),IkReal((npy+(((-1.0)*r11*x1556))+(((-1.0)*r01*x1557)))),IKFAST_ATAN2_MAGTHRESH);
06071 if(!x1559.valid){
06072 continue;
06073 }
06074 IkReal x1558=x1559.value;
06075 j12array[0]=((-1.0)*x1558);
06076 sj12array[0]=IKsin(j12array[0]);
06077 cj12array[0]=IKcos(j12array[0]);
06078 j12array[1]=((3.14159265358979)+(((-1.0)*x1558)));
06079 sj12array[1]=IKsin(j12array[1]);
06080 cj12array[1]=IKcos(j12array[1]);
06081 if( j12array[0] > IKPI )
06082 {
06083     j12array[0]-=IK2PI;
06084 }
06085 else if( j12array[0] < -IKPI )
06086 {    j12array[0]+=IK2PI;
06087 }
06088 j12valid[0] = true;
06089 if( j12array[1] > IKPI )
06090 {
06091     j12array[1]-=IK2PI;
06092 }
06093 else if( j12array[1] < -IKPI )
06094 {    j12array[1]+=IK2PI;
06095 }
06096 j12valid[1] = true;
06097 for(int ij12 = 0; ij12 < 2; ++ij12)
06098 {
06099 if( !j12valid[ij12] )
06100 {
06101     continue;
06102 }
06103 _ij12[0] = ij12; _ij12[1] = -1;
06104 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06105 {
06106 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06107 {
06108     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06109 }
06110 }
06111 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06112 {
06113 IkReal evalcond[1];
06114 IkReal x1560=IKsin(j12);
06115 IkReal x1561=IKcos(j12);
06116 IkReal x1562=((0.352)*cj7);
06117 IkReal x1563=((0.352)*sj7);
06118 evalcond[0]=(((r10*x1560*x1563))+((r00*x1560*x1562))+(((-1.0)*npx*x1560))+((r01*x1561*x1562))+((r11*x1561*x1563))+(((-1.0)*npy*x1561)));
06119 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06120 {
06121 continue;
06122 }
06123 }
06124 
06125 rotationfunction0(solutions);
06126 }
06127 }
06128 
06129 }
06130 
06131 }
06132 
06133 }
06134 } while(0);
06135 if( bgotonextstatement )
06136 {
06137 bool bgotonextstatement = true;
06138 do
06139 {
06140 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j11)))), 6.28318530717959)));
06141 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06142 {
06143 bgotonextstatement=false;
06144 {
06145 IkReal j12eval[1];
06146 sj11=0;
06147 cj11=-1.0;
06148 j11=3.14159265358979;
06149 IkReal x1564=((0.352)*sj7);
06150 IkReal x1565=((0.352)*cj7);
06151 j12eval[0]=((IKabs((npy+(((-1.0)*r01*x1565))+(((-1.0)*r11*x1564)))))+(IKabs(((((-1.0)*npx))+((r10*x1564))+((r00*x1565))))));
06152 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06153 {
06154 {
06155 IkReal j12eval[1];
06156 sj11=0;
06157 cj11=-1.0;
06158 j11=3.14159265358979;
06159 IkReal x1566=((0.352)*sj7);
06160 IkReal x1567=((0.352)*cj7);
06161 j12eval[0]=((IKabs(((((-1.0)*npy))+((r11*x1566))+((r01*x1567)))))+(IKabs(((((-1.0)*npx))+((r10*x1566))+((r00*x1567))))));
06162 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06163 {
06164 continue; // no branches [j12]
06165 
06166 } else
06167 {
06168 {
06169 IkReal j12array[2], cj12array[2], sj12array[2];
06170 bool j12valid[2]={false};
06171 _nj12 = 2;
06172 IkReal x1568=((0.352)*sj7);
06173 IkReal x1569=((0.352)*cj7);
06174 CheckValue<IkReal> x1571 = IKatan2WithCheck(IkReal(((((-1.0)*npy))+((r11*x1568))+((r01*x1569)))),IkReal(((((-1.0)*npx))+((r10*x1568))+((r00*x1569)))),IKFAST_ATAN2_MAGTHRESH);
06175 if(!x1571.valid){
06176 continue;
06177 }
06178 IkReal x1570=x1571.value;
06179 j12array[0]=((-1.0)*x1570);
06180 sj12array[0]=IKsin(j12array[0]);
06181 cj12array[0]=IKcos(j12array[0]);
06182 j12array[1]=((3.14159265358979)+(((-1.0)*x1570)));
06183 sj12array[1]=IKsin(j12array[1]);
06184 cj12array[1]=IKcos(j12array[1]);
06185 if( j12array[0] > IKPI )
06186 {
06187     j12array[0]-=IK2PI;
06188 }
06189 else if( j12array[0] < -IKPI )
06190 {    j12array[0]+=IK2PI;
06191 }
06192 j12valid[0] = true;
06193 if( j12array[1] > IKPI )
06194 {
06195     j12array[1]-=IK2PI;
06196 }
06197 else if( j12array[1] < -IKPI )
06198 {    j12array[1]+=IK2PI;
06199 }
06200 j12valid[1] = true;
06201 for(int ij12 = 0; ij12 < 2; ++ij12)
06202 {
06203 if( !j12valid[ij12] )
06204 {
06205     continue;
06206 }
06207 _ij12[0] = ij12; _ij12[1] = -1;
06208 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06209 {
06210 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06211 {
06212     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06213 }
06214 }
06215 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06216 {
06217 IkReal evalcond[1];
06218 IkReal x1572=IKcos(j12);
06219 IkReal x1573=IKsin(j12);
06220 IkReal x1574=((0.352)*x1573);
06221 IkReal x1575=((0.352)*x1572);
06222 evalcond[0]=(((r10*sj7*x1575))+(((-1.0)*r11*sj7*x1574))+(((-1.0)*npx*x1572))+((npy*x1573))+(((-1.0)*cj7*r01*x1574))+((cj7*r00*x1575)));
06223 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06224 {
06225 continue;
06226 }
06227 }
06228 
06229 rotationfunction0(solutions);
06230 }
06231 }
06232 
06233 }
06234 
06235 }
06236 
06237 } else
06238 {
06239 {
06240 IkReal j12array[2], cj12array[2], sj12array[2];
06241 bool j12valid[2]={false};
06242 _nj12 = 2;
06243 IkReal x1576=((0.352)*sj7);
06244 IkReal x1577=((0.352)*cj7);
06245 CheckValue<IkReal> x1579 = IKatan2WithCheck(IkReal(((((-1.0)*npx))+((r10*x1576))+((r00*x1577)))),IkReal((npy+(((-1.0)*r01*x1577))+(((-1.0)*r11*x1576)))),IKFAST_ATAN2_MAGTHRESH);
06246 if(!x1579.valid){
06247 continue;
06248 }
06249 IkReal x1578=x1579.value;
06250 j12array[0]=((-1.0)*x1578);
06251 sj12array[0]=IKsin(j12array[0]);
06252 cj12array[0]=IKcos(j12array[0]);
06253 j12array[1]=((3.14159265358979)+(((-1.0)*x1578)));
06254 sj12array[1]=IKsin(j12array[1]);
06255 cj12array[1]=IKcos(j12array[1]);
06256 if( j12array[0] > IKPI )
06257 {
06258     j12array[0]-=IK2PI;
06259 }
06260 else if( j12array[0] < -IKPI )
06261 {    j12array[0]+=IK2PI;
06262 }
06263 j12valid[0] = true;
06264 if( j12array[1] > IKPI )
06265 {
06266     j12array[1]-=IK2PI;
06267 }
06268 else if( j12array[1] < -IKPI )
06269 {    j12array[1]+=IK2PI;
06270 }
06271 j12valid[1] = true;
06272 for(int ij12 = 0; ij12 < 2; ++ij12)
06273 {
06274 if( !j12valid[ij12] )
06275 {
06276     continue;
06277 }
06278 _ij12[0] = ij12; _ij12[1] = -1;
06279 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06280 {
06281 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06282 {
06283     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06284 }
06285 }
06286 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06287 {
06288 IkReal evalcond[1];
06289 IkReal x1580=IKsin(j12);
06290 IkReal x1581=IKcos(j12);
06291 IkReal x1582=((0.352)*cj7);
06292 IkReal x1583=((0.352)*sj7);
06293 evalcond[0]=((((-1.0)*npy*x1581))+((r11*x1581*x1583))+((r10*x1580*x1583))+(((-1.0)*npx*x1580))+((r00*x1580*x1582))+((r01*x1581*x1582)));
06294 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06295 {
06296 continue;
06297 }
06298 }
06299 
06300 rotationfunction0(solutions);
06301 }
06302 }
06303 
06304 }
06305 
06306 }
06307 
06308 }
06309 } while(0);
06310 if( bgotonextstatement )
06311 {
06312 bool bgotonextstatement = true;
06313 do
06314 {
06315 evalcond[0]=IKabs(r02);
06316 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06317 {
06318 bgotonextstatement=false;
06319 {
06320 IkReal j12eval[3];
06321 r02=0;
06322 npz=(((py*r12))+((pz*r22)));
06323 rxp2_1=(px*r22);
06324 rxp2_2=((-1.0)*px*r12);
06325 IkReal x1584=(r12*sj11);
06326 IkReal x1585=((704.0)*cj7);
06327 IkReal x1586=((643.0)*cj11*r22);
06328 j12eval[0]=x1584;
06329 j12eval[1]=IKsign(x1584);
06330 j12eval[2]=((IKabs(((((-2000.0)*rxp1_1))+(((-1.0)*r01*x1586))+((r21*x1585)))))+(IKabs((((r20*x1585))+(((-1.0)*r00*x1586))+(((-2000.0)*rxp0_1))))));
06331 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
06332 {
06333 {
06334 IkReal j12eval[3];
06335 r02=0;
06336 npz=(((py*r12))+((pz*r22)));
06337 rxp2_1=(px*r22);
06338 rxp2_2=((-1.0)*px*r12);
06339 IkReal x1587=(r22*sj11);
06340 IkReal x1588=((704.0)*cj7);
06341 IkReal x1589=((704.0)*sj7);
06342 IkReal x1590=((643.0)*cj11*r12);
06343 j12eval[0]=x1587;
06344 j12eval[1]=IKsign(x1587);
06345 j12eval[2]=((IKabs(((((-1.0)*r00*x1590))+(((-1.0)*r00*x1589))+(((2000.0)*rxp0_2))+((r10*x1588)))))+(IKabs((((r11*x1588))+(((-1.0)*r01*x1590))+(((-1.0)*r01*x1589))+(((2000.0)*rxp1_2))))));
06346 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
06347 {
06348 {
06349 IkReal evalcond[1];
06350 bool bgotonextstatement = true;
06351 do
06352 {
06353 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j11))), 6.28318530717959)));
06354 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06355 {
06356 bgotonextstatement=false;
06357 {
06358 IkReal j12eval[1];
06359 r02=0;
06360 npz=(((py*r12))+((pz*r22)));
06361 rxp2_1=(px*r22);
06362 rxp2_2=((-1.0)*px*r12);
06363 sj11=0;
06364 cj11=1.0;
06365 j11=0;
06366 IkReal x1591=((0.352)*sj7);
06367 IkReal x1592=((0.352)*cj7);
06368 j12eval[0]=((IKabs((((r00*x1592))+(((-1.0)*npx))+((r10*x1591)))))+(IKabs((npy+(((-1.0)*r01*x1592))+(((-1.0)*r11*x1591))))));
06369 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06370 {
06371 {
06372 IkReal j12eval[1];
06373 r02=0;
06374 npz=(((py*r12))+((pz*r22)));
06375 rxp2_1=(px*r22);
06376 rxp2_2=((-1.0)*px*r12);
06377 sj11=0;
06378 cj11=1.0;
06379 j11=0;
06380 IkReal x1593=((0.352)*sj7);
06381 IkReal x1594=((0.352)*cj7);
06382 j12eval[0]=((IKabs((((r00*x1594))+(((-1.0)*npx))+((r10*x1593)))))+(IKabs((((r01*x1594))+((r11*x1593))+(((-1.0)*npy))))));
06383 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06384 {
06385 continue; // no branches [j12]
06386 
06387 } else
06388 {
06389 {
06390 IkReal j12array[2], cj12array[2], sj12array[2];
06391 bool j12valid[2]={false};
06392 _nj12 = 2;
06393 IkReal x1595=((0.352)*sj7);
06394 IkReal x1596=((0.352)*cj7);
06395 CheckValue<IkReal> x1598 = IKatan2WithCheck(IkReal((((r01*x1596))+((r11*x1595))+(((-1.0)*npy)))),IkReal((((r00*x1596))+(((-1.0)*npx))+((r10*x1595)))),IKFAST_ATAN2_MAGTHRESH);
06396 if(!x1598.valid){
06397 continue;
06398 }
06399 IkReal x1597=x1598.value;
06400 j12array[0]=((-1.0)*x1597);
06401 sj12array[0]=IKsin(j12array[0]);
06402 cj12array[0]=IKcos(j12array[0]);
06403 j12array[1]=((3.14159265358979)+(((-1.0)*x1597)));
06404 sj12array[1]=IKsin(j12array[1]);
06405 cj12array[1]=IKcos(j12array[1]);
06406 if( j12array[0] > IKPI )
06407 {
06408     j12array[0]-=IK2PI;
06409 }
06410 else if( j12array[0] < -IKPI )
06411 {    j12array[0]+=IK2PI;
06412 }
06413 j12valid[0] = true;
06414 if( j12array[1] > IKPI )
06415 {
06416     j12array[1]-=IK2PI;
06417 }
06418 else if( j12array[1] < -IKPI )
06419 {    j12array[1]+=IK2PI;
06420 }
06421 j12valid[1] = true;
06422 for(int ij12 = 0; ij12 < 2; ++ij12)
06423 {
06424 if( !j12valid[ij12] )
06425 {
06426     continue;
06427 }
06428 _ij12[0] = ij12; _ij12[1] = -1;
06429 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06430 {
06431 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06432 {
06433     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06434 }
06435 }
06436 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06437 {
06438 IkReal evalcond[1];
06439 IkReal x1599=IKcos(j12);
06440 IkReal x1600=IKsin(j12);
06441 IkReal x1601=((0.352)*x1600);
06442 IkReal x1602=((0.352)*x1599);
06443 evalcond[0]=(((npy*x1600))+(((-1.0)*r11*sj7*x1601))+((r10*sj7*x1602))+((cj7*r00*x1602))+(((-1.0)*cj7*r01*x1601))+(((-1.0)*npx*x1599)));
06444 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06445 {
06446 continue;
06447 }
06448 }
06449 
06450 rotationfunction0(solutions);
06451 }
06452 }
06453 
06454 }
06455 
06456 }
06457 
06458 } else
06459 {
06460 {
06461 IkReal j12array[2], cj12array[2], sj12array[2];
06462 bool j12valid[2]={false};
06463 _nj12 = 2;
06464 IkReal x1603=((0.352)*sj7);
06465 IkReal x1604=((0.352)*cj7);
06466 CheckValue<IkReal> x1606 = IKatan2WithCheck(IkReal((((r00*x1604))+(((-1.0)*npx))+((r10*x1603)))),IkReal((npy+(((-1.0)*r01*x1604))+(((-1.0)*r11*x1603)))),IKFAST_ATAN2_MAGTHRESH);
06467 if(!x1606.valid){
06468 continue;
06469 }
06470 IkReal x1605=x1606.value;
06471 j12array[0]=((-1.0)*x1605);
06472 sj12array[0]=IKsin(j12array[0]);
06473 cj12array[0]=IKcos(j12array[0]);
06474 j12array[1]=((3.14159265358979)+(((-1.0)*x1605)));
06475 sj12array[1]=IKsin(j12array[1]);
06476 cj12array[1]=IKcos(j12array[1]);
06477 if( j12array[0] > IKPI )
06478 {
06479     j12array[0]-=IK2PI;
06480 }
06481 else if( j12array[0] < -IKPI )
06482 {    j12array[0]+=IK2PI;
06483 }
06484 j12valid[0] = true;
06485 if( j12array[1] > IKPI )
06486 {
06487     j12array[1]-=IK2PI;
06488 }
06489 else if( j12array[1] < -IKPI )
06490 {    j12array[1]+=IK2PI;
06491 }
06492 j12valid[1] = true;
06493 for(int ij12 = 0; ij12 < 2; ++ij12)
06494 {
06495 if( !j12valid[ij12] )
06496 {
06497     continue;
06498 }
06499 _ij12[0] = ij12; _ij12[1] = -1;
06500 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06501 {
06502 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06503 {
06504     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06505 }
06506 }
06507 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06508 {
06509 IkReal evalcond[1];
06510 IkReal x1607=IKsin(j12);
06511 IkReal x1608=IKcos(j12);
06512 IkReal x1609=((0.352)*cj7);
06513 IkReal x1610=((0.352)*sj7);
06514 evalcond[0]=(((r10*x1607*x1610))+(((-1.0)*npy*x1608))+((r01*x1608*x1609))+((r00*x1607*x1609))+((r11*x1608*x1610))+(((-1.0)*npx*x1607)));
06515 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06516 {
06517 continue;
06518 }
06519 }
06520 
06521 rotationfunction0(solutions);
06522 }
06523 }
06524 
06525 }
06526 
06527 }
06528 
06529 }
06530 } while(0);
06531 if( bgotonextstatement )
06532 {
06533 bool bgotonextstatement = true;
06534 do
06535 {
06536 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j11)))), 6.28318530717959)));
06537 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06538 {
06539 bgotonextstatement=false;
06540 {
06541 IkReal j12eval[1];
06542 r02=0;
06543 npz=(((py*r12))+((pz*r22)));
06544 rxp2_1=(px*r22);
06545 rxp2_2=((-1.0)*px*r12);
06546 sj11=0;
06547 cj11=-1.0;
06548 j11=3.14159265358979;
06549 IkReal x1611=((0.352)*sj7);
06550 IkReal x1612=((0.352)*cj7);
06551 j12eval[0]=((IKabs((((r00*x1612))+(((-1.0)*npx))+((r10*x1611)))))+(IKabs(((((-1.0)*r11*x1611))+(((-1.0)*r01*x1612))+npy))));
06552 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06553 {
06554 {
06555 IkReal j12eval[1];
06556 r02=0;
06557 npz=(((py*r12))+((pz*r22)));
06558 rxp2_1=(px*r22);
06559 rxp2_2=((-1.0)*px*r12);
06560 sj11=0;
06561 cj11=-1.0;
06562 j11=3.14159265358979;
06563 IkReal x1613=((0.352)*sj7);
06564 IkReal x1614=((0.352)*cj7);
06565 j12eval[0]=((IKabs((((r00*x1614))+(((-1.0)*npx))+((r10*x1613)))))+(IKabs((((r01*x1614))+(((-1.0)*npy))+((r11*x1613))))));
06566 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06567 {
06568 continue; // no branches [j12]
06569 
06570 } else
06571 {
06572 {
06573 IkReal j12array[2], cj12array[2], sj12array[2];
06574 bool j12valid[2]={false};
06575 _nj12 = 2;
06576 IkReal x1615=((0.352)*sj7);
06577 IkReal x1616=((0.352)*cj7);
06578 CheckValue<IkReal> x1618 = IKatan2WithCheck(IkReal((((r01*x1616))+(((-1.0)*npy))+((r11*x1615)))),IkReal((((r00*x1616))+(((-1.0)*npx))+((r10*x1615)))),IKFAST_ATAN2_MAGTHRESH);
06579 if(!x1618.valid){
06580 continue;
06581 }
06582 IkReal x1617=x1618.value;
06583 j12array[0]=((-1.0)*x1617);
06584 sj12array[0]=IKsin(j12array[0]);
06585 cj12array[0]=IKcos(j12array[0]);
06586 j12array[1]=((3.14159265358979)+(((-1.0)*x1617)));
06587 sj12array[1]=IKsin(j12array[1]);
06588 cj12array[1]=IKcos(j12array[1]);
06589 if( j12array[0] > IKPI )
06590 {
06591     j12array[0]-=IK2PI;
06592 }
06593 else if( j12array[0] < -IKPI )
06594 {    j12array[0]+=IK2PI;
06595 }
06596 j12valid[0] = true;
06597 if( j12array[1] > IKPI )
06598 {
06599     j12array[1]-=IK2PI;
06600 }
06601 else if( j12array[1] < -IKPI )
06602 {    j12array[1]+=IK2PI;
06603 }
06604 j12valid[1] = true;
06605 for(int ij12 = 0; ij12 < 2; ++ij12)
06606 {
06607 if( !j12valid[ij12] )
06608 {
06609     continue;
06610 }
06611 _ij12[0] = ij12; _ij12[1] = -1;
06612 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06613 {
06614 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06615 {
06616     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06617 }
06618 }
06619 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06620 {
06621 IkReal evalcond[1];
06622 IkReal x1619=IKcos(j12);
06623 IkReal x1620=IKsin(j12);
06624 IkReal x1621=((0.352)*x1620);
06625 IkReal x1622=((0.352)*x1619);
06626 evalcond[0]=((((-1.0)*npx*x1619))+((npy*x1620))+((cj7*r00*x1622))+(((-1.0)*r11*sj7*x1621))+(((-1.0)*cj7*r01*x1621))+((r10*sj7*x1622)));
06627 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06628 {
06629 continue;
06630 }
06631 }
06632 
06633 rotationfunction0(solutions);
06634 }
06635 }
06636 
06637 }
06638 
06639 }
06640 
06641 } else
06642 {
06643 {
06644 IkReal j12array[2], cj12array[2], sj12array[2];
06645 bool j12valid[2]={false};
06646 _nj12 = 2;
06647 IkReal x1623=((0.352)*sj7);
06648 IkReal x1624=((0.352)*cj7);
06649 CheckValue<IkReal> x1626 = IKatan2WithCheck(IkReal((((r00*x1624))+(((-1.0)*npx))+((r10*x1623)))),IkReal(((((-1.0)*r11*x1623))+npy+(((-1.0)*r01*x1624)))),IKFAST_ATAN2_MAGTHRESH);
06650 if(!x1626.valid){
06651 continue;
06652 }
06653 IkReal x1625=x1626.value;
06654 j12array[0]=((-1.0)*x1625);
06655 sj12array[0]=IKsin(j12array[0]);
06656 cj12array[0]=IKcos(j12array[0]);
06657 j12array[1]=((3.14159265358979)+(((-1.0)*x1625)));
06658 sj12array[1]=IKsin(j12array[1]);
06659 cj12array[1]=IKcos(j12array[1]);
06660 if( j12array[0] > IKPI )
06661 {
06662     j12array[0]-=IK2PI;
06663 }
06664 else if( j12array[0] < -IKPI )
06665 {    j12array[0]+=IK2PI;
06666 }
06667 j12valid[0] = true;
06668 if( j12array[1] > IKPI )
06669 {
06670     j12array[1]-=IK2PI;
06671 }
06672 else if( j12array[1] < -IKPI )
06673 {    j12array[1]+=IK2PI;
06674 }
06675 j12valid[1] = true;
06676 for(int ij12 = 0; ij12 < 2; ++ij12)
06677 {
06678 if( !j12valid[ij12] )
06679 {
06680     continue;
06681 }
06682 _ij12[0] = ij12; _ij12[1] = -1;
06683 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06684 {
06685 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06686 {
06687     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06688 }
06689 }
06690 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06691 {
06692 IkReal evalcond[1];
06693 IkReal x1627=IKsin(j12);
06694 IkReal x1628=IKcos(j12);
06695 IkReal x1629=((0.352)*cj7);
06696 IkReal x1630=((0.352)*sj7);
06697 evalcond[0]=((((-1.0)*npx*x1627))+((r11*x1628*x1630))+(((-1.0)*npy*x1628))+((r10*x1627*x1630))+((r01*x1628*x1629))+((r00*x1627*x1629)));
06698 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06699 {
06700 continue;
06701 }
06702 }
06703 
06704 rotationfunction0(solutions);
06705 }
06706 }
06707 
06708 }
06709 
06710 }
06711 
06712 }
06713 } while(0);
06714 if( bgotonextstatement )
06715 {
06716 bool bgotonextstatement = true;
06717 do
06718 {
06719 evalcond[0]=IKabs(r12);
06720 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06721 {
06722 bgotonextstatement=false;
06723 {
06724 IkReal j12eval[3];
06725 r02=0;
06726 npz=(pz*r22);
06727 rxp2_1=(px*r22);
06728 rxp2_2=0;
06729 r12=0;
06730 rxp2_0=((-1.0)*py*r22);
06731 IkReal x1631=(r22*sj11);
06732 IkReal x1632=((704.0)*cj7);
06733 IkReal x1633=((704.0)*sj7);
06734 j12eval[0]=x1631;
06735 j12eval[1]=((IKabs((((r10*x1632))+(((-1.0)*r00*x1633))+(((2000.0)*rxp0_2)))))+(IKabs(((((-1.0)*r01*x1633))+((r11*x1632))+(((2000.0)*rxp1_2))))));
06736 j12eval[2]=IKsign(x1631);
06737 if( IKabs(j12eval[0]) < 0.0000010000000000  || IKabs(j12eval[1]) < 0.0000010000000000  || IKabs(j12eval[2]) < 0.0000010000000000  )
06738 {
06739 {
06740 IkReal evalcond[1];
06741 bool bgotonextstatement = true;
06742 do
06743 {
06744 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j11))), 6.28318530717959)));
06745 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06746 {
06747 bgotonextstatement=false;
06748 {
06749 IkReal j12eval[1];
06750 r02=0;
06751 npz=(pz*r22);
06752 rxp2_1=(px*r22);
06753 rxp2_2=0;
06754 r12=0;
06755 rxp2_0=((-1.0)*py*r22);
06756 sj11=0;
06757 cj11=1.0;
06758 j11=0;
06759 j12eval[0]=((IKabs((pz*r20)))+(IKabs((pz*r21))));
06760 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06761 {
06762 continue; // 3 cases reached
06763 
06764 } else
06765 {
06766 {
06767 IkReal j12array[2], cj12array[2], sj12array[2];
06768 bool j12valid[2]={false};
06769 _nj12 = 2;
06770 CheckValue<IkReal> x1635 = IKatan2WithCheck(IkReal((((px*r01))+(((-1.0)*npy))+((py*r11)))),IkReal((((px*r00))+(((-1.0)*npx))+((py*r10)))),IKFAST_ATAN2_MAGTHRESH);
06771 if(!x1635.valid){
06772 continue;
06773 }
06774 IkReal x1634=x1635.value;
06775 j12array[0]=((-1.0)*x1634);
06776 sj12array[0]=IKsin(j12array[0]);
06777 cj12array[0]=IKcos(j12array[0]);
06778 j12array[1]=((3.14159265358979)+(((-1.0)*x1634)));
06779 sj12array[1]=IKsin(j12array[1]);
06780 cj12array[1]=IKcos(j12array[1]);
06781 if( j12array[0] > IKPI )
06782 {
06783     j12array[0]-=IK2PI;
06784 }
06785 else if( j12array[0] < -IKPI )
06786 {    j12array[0]+=IK2PI;
06787 }
06788 j12valid[0] = true;
06789 if( j12array[1] > IKPI )
06790 {
06791     j12array[1]-=IK2PI;
06792 }
06793 else if( j12array[1] < -IKPI )
06794 {    j12array[1]+=IK2PI;
06795 }
06796 j12valid[1] = true;
06797 for(int ij12 = 0; ij12 < 2; ++ij12)
06798 {
06799 if( !j12valid[ij12] )
06800 {
06801     continue;
06802 }
06803 _ij12[0] = ij12; _ij12[1] = -1;
06804 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06805 {
06806 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06807 {
06808     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06809 }
06810 }
06811 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06812 {
06813 IkReal evalcond[1];
06814 IkReal x1636=IKcos(j12);
06815 IkReal x1637=IKsin(j12);
06816 IkReal x1638=((1.0)*x1637);
06817 evalcond[0]=((((-1.0)*npx*x1636))+((py*r10*x1636))+((px*r00*x1636))+(((-1.0)*px*r01*x1638))+(((-1.0)*py*r11*x1638))+((npy*x1637)));
06818 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06819 {
06820 continue;
06821 }
06822 }
06823 
06824 rotationfunction0(solutions);
06825 }
06826 }
06827 
06828 }
06829 
06830 }
06831 
06832 }
06833 } while(0);
06834 if( bgotonextstatement )
06835 {
06836 bool bgotonextstatement = true;
06837 do
06838 {
06839 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j11)))), 6.28318530717959)));
06840 if( IKabs(evalcond[0]) < 0.0000050000000000  )
06841 {
06842 bgotonextstatement=false;
06843 {
06844 IkReal j12eval[1];
06845 r02=0;
06846 npz=(pz*r22);
06847 rxp2_1=(px*r22);
06848 rxp2_2=0;
06849 r12=0;
06850 rxp2_0=((-1.0)*py*r22);
06851 sj11=0;
06852 cj11=-1.0;
06853 j11=3.14159265358979;
06854 j12eval[0]=((IKabs((pz*r20)))+(IKabs((pz*r21))));
06855 if( IKabs(j12eval[0]) < 0.0000010000000000  )
06856 {
06857 continue; // 3 cases reached
06858 
06859 } else
06860 {
06861 {
06862 IkReal j12array[2], cj12array[2], sj12array[2];
06863 bool j12valid[2]={false};
06864 _nj12 = 2;
06865 CheckValue<IkReal> x1640 = IKatan2WithCheck(IkReal((((px*r01))+(((-1.0)*npy))+((py*r11)))),IkReal((((px*r00))+(((-1.0)*npx))+((py*r10)))),IKFAST_ATAN2_MAGTHRESH);
06866 if(!x1640.valid){
06867 continue;
06868 }
06869 IkReal x1639=x1640.value;
06870 j12array[0]=((-1.0)*x1639);
06871 sj12array[0]=IKsin(j12array[0]);
06872 cj12array[0]=IKcos(j12array[0]);
06873 j12array[1]=((3.14159265358979)+(((-1.0)*x1639)));
06874 sj12array[1]=IKsin(j12array[1]);
06875 cj12array[1]=IKcos(j12array[1]);
06876 if( j12array[0] > IKPI )
06877 {
06878     j12array[0]-=IK2PI;
06879 }
06880 else if( j12array[0] < -IKPI )
06881 {    j12array[0]+=IK2PI;
06882 }
06883 j12valid[0] = true;
06884 if( j12array[1] > IKPI )
06885 {
06886     j12array[1]-=IK2PI;
06887 }
06888 else if( j12array[1] < -IKPI )
06889 {    j12array[1]+=IK2PI;
06890 }
06891 j12valid[1] = true;
06892 for(int ij12 = 0; ij12 < 2; ++ij12)
06893 {
06894 if( !j12valid[ij12] )
06895 {
06896     continue;
06897 }
06898 _ij12[0] = ij12; _ij12[1] = -1;
06899 for(int iij12 = ij12+1; iij12 < 2; ++iij12)
06900 {
06901 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06902 {
06903     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06904 }
06905 }
06906 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06907 {
06908 IkReal evalcond[1];
06909 IkReal x1641=IKcos(j12);
06910 IkReal x1642=IKsin(j12);
06911 IkReal x1643=((1.0)*x1642);
06912 evalcond[0]=(((px*r00*x1641))+(((-1.0)*py*r11*x1643))+(((-1.0)*npx*x1641))+((py*r10*x1641))+(((-1.0)*px*r01*x1643))+((npy*x1642)));
06913 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
06914 {
06915 continue;
06916 }
06917 }
06918 
06919 rotationfunction0(solutions);
06920 }
06921 }
06922 
06923 }
06924 
06925 }
06926 
06927 }
06928 } while(0);
06929 if( bgotonextstatement )
06930 {
06931 bool bgotonextstatement = true;
06932 do
06933 {
06934 if( 1 )
06935 {
06936 bgotonextstatement=false;
06937 continue; // branch miss [j12]
06938 
06939 }
06940 } while(0);
06941 if( bgotonextstatement )
06942 {
06943 }
06944 }
06945 }
06946 }
06947 
06948 } else
06949 {
06950 {
06951 IkReal j12array[1], cj12array[1], sj12array[1];
06952 bool j12valid[1]={false};
06953 _nj12 = 1;
06954 IkReal x1644=((704.0)*cj7);
06955 IkReal x1645=((704.0)*sj7);
06956 CheckValue<IkReal> x1646 = IKatan2WithCheck(IkReal(((((-1.0)*r00*x1645))+((r10*x1644))+(((2000.0)*rxp0_2)))),IkReal((((r11*x1644))+(((-1.0)*r01*x1645))+(((2000.0)*rxp1_2)))),IKFAST_ATAN2_MAGTHRESH);
06957 if(!x1646.valid){
06958 continue;
06959 }
06960 CheckValue<IkReal> x1647=IKPowWithIntegerCheck(IKsign((r22*sj11)),-1);
06961 if(!x1647.valid){
06962 continue;
06963 }
06964 j12array[0]=((-1.5707963267949)+(x1646.value)+(((1.5707963267949)*(x1647.value))));
06965 sj12array[0]=IKsin(j12array[0]);
06966 cj12array[0]=IKcos(j12array[0]);
06967 if( j12array[0] > IKPI )
06968 {
06969     j12array[0]-=IK2PI;
06970 }
06971 else if( j12array[0] < -IKPI )
06972 {    j12array[0]+=IK2PI;
06973 }
06974 j12valid[0] = true;
06975 for(int ij12 = 0; ij12 < 1; ++ij12)
06976 {
06977 if( !j12valid[ij12] )
06978 {
06979     continue;
06980 }
06981 _ij12[0] = ij12; _ij12[1] = -1;
06982 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
06983 {
06984 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
06985 {
06986     j12valid[iij12]=false; _ij12[1] = iij12; break; 
06987 }
06988 }
06989 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
06990 {
06991 IkReal evalcond[6];
06992 IkReal x1648=IKsin(j12);
06993 IkReal x1649=IKcos(j12);
06994 IkReal x1650=((0.3215)*sj11);
06995 IkReal x1651=((1.0)*npx);
06996 IkReal x1652=(cj7*r01);
06997 IkReal x1653=((0.643)*sj11);
06998 IkReal x1654=(cj11*r22);
06999 IkReal x1655=((0.352)*sj7);
07000 IkReal x1656=((0.352)*cj7*r00);
07001 IkReal x1657=(npy*x1648);
07002 IkReal x1658=((0.352)*x1649);
07003 IkReal x1659=(r11*x1648);
07004 evalcond[0]=((((-1.0)*px))+(((-1.0)*r00*x1649*x1650))+(((0.352)*cj7))+((r01*x1648*x1650)));
07005 evalcond[1]=(((x1650*x1659))+x1655+(((-1.0)*r10*x1649*x1650))+(((-1.0)*py)));
07006 evalcond[2]=(((r21*x1648*x1650))+(((-1.0)*pz))+(((-1.0)*r20*x1649*x1650))+(((0.3215)*x1654)));
07007 evalcond[3]=((0.02054175)+(((-1.0)*pp))+(((-1.0)*npx*x1649*x1653))+(((0.643)*pz*x1654))+((x1653*x1657)));
07008 evalcond[4]=((((-1.0)*npy*x1649))+((x1648*x1656))+((r10*x1648*x1655))+((r11*x1649*x1655))+(((-1.0)*x1648*x1651))+((x1652*x1658)));
07009 evalcond[5]=(x1657+(((-1.0)*x1655*x1659))+((x1649*x1656))+(((-1.0)*x1650))+(((-0.352)*x1648*x1652))+((r10*x1649*x1655))+(((-1.0)*x1649*x1651)));
07010 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07011 {
07012 continue;
07013 }
07014 }
07015 
07016 rotationfunction0(solutions);
07017 }
07018 }
07019 
07020 }
07021 
07022 }
07023 
07024 }
07025 } while(0);
07026 if( bgotonextstatement )
07027 {
07028 bool bgotonextstatement = true;
07029 do
07030 {
07031 if( 1 )
07032 {
07033 bgotonextstatement=false;
07034 continue; // branch miss [j12]
07035 
07036 }
07037 } while(0);
07038 if( bgotonextstatement )
07039 {
07040 }
07041 }
07042 }
07043 }
07044 }
07045 
07046 } else
07047 {
07048 {
07049 IkReal j12array[1], cj12array[1], sj12array[1];
07050 bool j12valid[1]={false};
07051 _nj12 = 1;
07052 IkReal x1660=((704.0)*cj7);
07053 IkReal x1661=((704.0)*sj7);
07054 IkReal x1662=((643.0)*cj11*r12);
07055 CheckValue<IkReal> x1663 = IKatan2WithCheck(IkReal(((((-1.0)*r00*x1662))+(((-1.0)*r00*x1661))+((r10*x1660))+(((2000.0)*rxp0_2)))),IkReal(((((-1.0)*r01*x1662))+(((-1.0)*r01*x1661))+((r11*x1660))+(((2000.0)*rxp1_2)))),IKFAST_ATAN2_MAGTHRESH);
07056 if(!x1663.valid){
07057 continue;
07058 }
07059 CheckValue<IkReal> x1664=IKPowWithIntegerCheck(IKsign((r22*sj11)),-1);
07060 if(!x1664.valid){
07061 continue;
07062 }
07063 j12array[0]=((-1.5707963267949)+(x1663.value)+(((1.5707963267949)*(x1664.value))));
07064 sj12array[0]=IKsin(j12array[0]);
07065 cj12array[0]=IKcos(j12array[0]);
07066 if( j12array[0] > IKPI )
07067 {
07068     j12array[0]-=IK2PI;
07069 }
07070 else if( j12array[0] < -IKPI )
07071 {    j12array[0]+=IK2PI;
07072 }
07073 j12valid[0] = true;
07074 for(int ij12 = 0; ij12 < 1; ++ij12)
07075 {
07076 if( !j12valid[ij12] )
07077 {
07078     continue;
07079 }
07080 _ij12[0] = ij12; _ij12[1] = -1;
07081 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
07082 {
07083 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
07084 {
07085     j12valid[iij12]=false; _ij12[1] = iij12; break; 
07086 }
07087 }
07088 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
07089 {
07090 IkReal evalcond[6];
07091 IkReal x1665=IKsin(j12);
07092 IkReal x1666=IKcos(j12);
07093 IkReal x1667=((0.3215)*sj11);
07094 IkReal x1668=((0.352)*cj7);
07095 IkReal x1669=((1.0)*npx);
07096 IkReal x1670=((0.643)*sj11);
07097 IkReal x1671=(cj11*r22);
07098 IkReal x1672=(cj11*r12);
07099 IkReal x1673=((0.352)*sj7);
07100 IkReal x1674=(npy*x1665);
07101 IkReal x1675=(r11*x1673);
07102 IkReal x1676=(r10*x1666);
07103 IkReal x1677=(r01*x1665);
07104 IkReal x1678=(r00*x1666);
07105 evalcond[0]=((((-1.0)*x1667*x1678))+x1668+((x1667*x1677))+(((-1.0)*px)));
07106 evalcond[1]=(((r21*x1665*x1667))+(((-1.0)*pz))+(((0.3215)*x1671))+(((-1.0)*r20*x1666*x1667)));
07107 evalcond[2]=((((-1.0)*x1667*x1676))+x1673+(((-1.0)*py))+(((0.3215)*x1672))+((r11*x1665*x1667)));
07108 evalcond[3]=((0.02054175)+(((0.643)*py*x1672))+(((0.643)*pz*x1671))+(((-1.0)*pp))+(((-1.0)*npx*x1666*x1670))+((x1670*x1674)));
07109 evalcond[4]=(((x1666*x1675))+((r00*x1665*x1668))+((r01*x1666*x1668))+((r10*x1665*x1673))+(((-1.0)*npy*x1666))+(((-1.0)*x1665*x1669)));
07110 evalcond[5]=((((-1.0)*x1668*x1677))+x1674+((x1673*x1676))+((x1668*x1678))+(((-1.0)*x1667))+(((-1.0)*x1665*x1675))+(((-1.0)*x1666*x1669)));
07111 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07112 {
07113 continue;
07114 }
07115 }
07116 
07117 rotationfunction0(solutions);
07118 }
07119 }
07120 
07121 }
07122 
07123 }
07124 
07125 } else
07126 {
07127 {
07128 IkReal j12array[1], cj12array[1], sj12array[1];
07129 bool j12valid[1]={false};
07130 _nj12 = 1;
07131 IkReal x1679=((704.0)*cj7);
07132 IkReal x1680=((643.0)*cj11*r22);
07133 CheckValue<IkReal> x1681=IKPowWithIntegerCheck(IKsign((r12*sj11)),-1);
07134 if(!x1681.valid){
07135 continue;
07136 }
07137 CheckValue<IkReal> x1682 = IKatan2WithCheck(IkReal((((r20*x1679))+(((-2000.0)*rxp0_1))+(((-1.0)*r00*x1680)))),IkReal((((r21*x1679))+(((-1.0)*r01*x1680))+(((-2000.0)*rxp1_1)))),IKFAST_ATAN2_MAGTHRESH);
07138 if(!x1682.valid){
07139 continue;
07140 }
07141 j12array[0]=((-1.5707963267949)+(((-1.5707963267949)*(x1681.value)))+(x1682.value));
07142 sj12array[0]=IKsin(j12array[0]);
07143 cj12array[0]=IKcos(j12array[0]);
07144 if( j12array[0] > IKPI )
07145 {
07146     j12array[0]-=IK2PI;
07147 }
07148 else if( j12array[0] < -IKPI )
07149 {    j12array[0]+=IK2PI;
07150 }
07151 j12valid[0] = true;
07152 for(int ij12 = 0; ij12 < 1; ++ij12)
07153 {
07154 if( !j12valid[ij12] )
07155 {
07156     continue;
07157 }
07158 _ij12[0] = ij12; _ij12[1] = -1;
07159 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
07160 {
07161 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
07162 {
07163     j12valid[iij12]=false; _ij12[1] = iij12; break; 
07164 }
07165 }
07166 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
07167 {
07168 IkReal evalcond[6];
07169 IkReal x1683=IKsin(j12);
07170 IkReal x1684=IKcos(j12);
07171 IkReal x1685=((0.3215)*sj11);
07172 IkReal x1686=((0.352)*cj7);
07173 IkReal x1687=((1.0)*npx);
07174 IkReal x1688=((0.643)*sj11);
07175 IkReal x1689=(cj11*r22);
07176 IkReal x1690=(cj11*r12);
07177 IkReal x1691=((0.352)*sj7);
07178 IkReal x1692=(npy*x1683);
07179 IkReal x1693=(r11*x1691);
07180 IkReal x1694=(r10*x1684);
07181 IkReal x1695=(r01*x1683);
07182 IkReal x1696=(r00*x1684);
07183 evalcond[0]=(x1686+(((-1.0)*px))+((x1685*x1695))+(((-1.0)*x1685*x1696)));
07184 evalcond[1]=((((-1.0)*r20*x1684*x1685))+((r21*x1683*x1685))+(((-1.0)*pz))+(((0.3215)*x1689)));
07185 evalcond[2]=(x1691+(((-1.0)*py))+((r11*x1683*x1685))+(((0.3215)*x1690))+(((-1.0)*x1685*x1694)));
07186 evalcond[3]=((0.02054175)+(((-1.0)*pp))+((x1688*x1692))+(((-1.0)*npx*x1684*x1688))+(((0.643)*pz*x1689))+(((0.643)*py*x1690)));
07187 evalcond[4]=(((r10*x1683*x1691))+(((-1.0)*x1683*x1687))+(((-1.0)*npy*x1684))+((r01*x1684*x1686))+((r00*x1683*x1686))+((x1684*x1693)));
07188 evalcond[5]=((((-1.0)*x1686*x1695))+(((-1.0)*x1685))+x1692+((x1691*x1694))+((x1686*x1696))+(((-1.0)*x1683*x1693))+(((-1.0)*x1684*x1687)));
07189 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07190 {
07191 continue;
07192 }
07193 }
07194 
07195 rotationfunction0(solutions);
07196 }
07197 }
07198 
07199 }
07200 
07201 }
07202 
07203 }
07204 } while(0);
07205 if( bgotonextstatement )
07206 {
07207 bool bgotonextstatement = true;
07208 do
07209 {
07210 if( 1 )
07211 {
07212 bgotonextstatement=false;
07213 continue; // branch miss [j12]
07214 
07215 }
07216 } while(0);
07217 if( bgotonextstatement )
07218 {
07219 }
07220 }
07221 }
07222 }
07223 }
07224 
07225 } else
07226 {
07227 {
07228 IkReal j12array[1], cj12array[1], sj12array[1];
07229 bool j12valid[1]={false};
07230 _nj12 = 1;
07231 IkReal x1697=((643.0)*cj11);
07232 IkReal x1698=((2000.0)*pz);
07233 IkReal x1699=((643.0)*sj11);
07234 IkReal x1700=((1000.0)*pp);
07235 CheckValue<IkReal> x1701=IKPowWithIntegerCheck(IKsign((((npy*r20*x1699))+(((-1.0)*npx*r21*x1699)))),-1);
07236 if(!x1701.valid){
07237 continue;
07238 }
07239 CheckValue<IkReal> x1702 = IKatan2WithCheck(IkReal(((((-20.54175)*r20))+(((-1.0)*npz*r20*x1697))+(((-1.0)*npx*x1698))+((r20*x1700))+((npx*r22*x1697)))),IkReal(((((-20.54175)*r21))+(((-1.0)*npy*x1698))+((npy*r22*x1697))+((r21*x1700))+(((-1.0)*npz*r21*x1697)))),IKFAST_ATAN2_MAGTHRESH);
07240 if(!x1702.valid){
07241 continue;
07242 }
07243 j12array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1701.value)))+(x1702.value));
07244 sj12array[0]=IKsin(j12array[0]);
07245 cj12array[0]=IKcos(j12array[0]);
07246 if( j12array[0] > IKPI )
07247 {
07248     j12array[0]-=IK2PI;
07249 }
07250 else if( j12array[0] < -IKPI )
07251 {    j12array[0]+=IK2PI;
07252 }
07253 j12valid[0] = true;
07254 for(int ij12 = 0; ij12 < 1; ++ij12)
07255 {
07256 if( !j12valid[ij12] )
07257 {
07258     continue;
07259 }
07260 _ij12[0] = ij12; _ij12[1] = -1;
07261 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
07262 {
07263 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
07264 {
07265     j12valid[iij12]=false; _ij12[1] = iij12; break; 
07266 }
07267 }
07268 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
07269 {
07270 IkReal evalcond[6];
07271 IkReal x1703=IKsin(j12);
07272 IkReal x1704=IKcos(j12);
07273 IkReal x1705=((0.3215)*sj11);
07274 IkReal x1706=((0.352)*cj7);
07275 IkReal x1707=((1.0)*npx);
07276 IkReal x1708=((0.3215)*cj11);
07277 IkReal x1709=((0.643)*sj11);
07278 IkReal x1710=((0.352)*sj7);
07279 IkReal x1711=(npy*x1703);
07280 IkReal x1712=(r11*x1710);
07281 IkReal x1713=(r01*x1703);
07282 IkReal x1714=(r10*x1704);
07283 IkReal x1715=(r00*x1704);
07284 evalcond[0]=(((r22*x1708))+(((-1.0)*r20*x1704*x1705))+(((-1.0)*pz))+((r21*x1703*x1705)));
07285 evalcond[1]=((0.02054175)+(((0.643)*cj11*npz))+((x1709*x1711))+(((-1.0)*npx*x1704*x1709))+(((-1.0)*pp)));
07286 evalcond[2]=(((r02*x1708))+x1706+(((-1.0)*px))+(((-1.0)*x1705*x1715))+((x1705*x1713)));
07287 evalcond[3]=(((r11*x1703*x1705))+((r12*x1708))+x1710+(((-1.0)*py))+(((-1.0)*x1705*x1714)));
07288 evalcond[4]=((((-1.0)*x1703*x1707))+((x1704*x1712))+((r10*x1703*x1710))+(((-1.0)*npy*x1704))+((r00*x1703*x1706))+((r01*x1704*x1706)));
07289 evalcond[5]=(((x1710*x1714))+x1711+(((-1.0)*x1705))+(((-1.0)*x1704*x1707))+(((-1.0)*x1706*x1713))+((x1706*x1715))+(((-1.0)*x1703*x1712)));
07290 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07291 {
07292 continue;
07293 }
07294 }
07295 
07296 rotationfunction0(solutions);
07297 }
07298 }
07299 
07300 }
07301 
07302 }
07303 
07304 } else
07305 {
07306 {
07307 IkReal j12array[1], cj12array[1], sj12array[1];
07308 bool j12valid[1]={false};
07309 _nj12 = 1;
07310 IkReal x1716=((643.0)*cj11);
07311 IkReal x1717=((704.0)*cj7);
07312 CheckValue<IkReal> x1718=IKPowWithIntegerCheck(IKsign((r12*sj11)),-1);
07313 if(!x1718.valid){
07314 continue;
07315 }
07316 CheckValue<IkReal> x1719 = IKatan2WithCheck(IkReal((((r20*x1717))+(((-1.0)*r11*x1716))+(((-2000.0)*rxp0_1)))),IkReal(((((-2000.0)*rxp1_1))+((r10*x1716))+((r21*x1717)))),IKFAST_ATAN2_MAGTHRESH);
07317 if(!x1719.valid){
07318 continue;
07319 }
07320 j12array[0]=((-1.5707963267949)+(((-1.5707963267949)*(x1718.value)))+(x1719.value));
07321 sj12array[0]=IKsin(j12array[0]);
07322 cj12array[0]=IKcos(j12array[0]);
07323 if( j12array[0] > IKPI )
07324 {
07325     j12array[0]-=IK2PI;
07326 }
07327 else if( j12array[0] < -IKPI )
07328 {    j12array[0]+=IK2PI;
07329 }
07330 j12valid[0] = true;
07331 for(int ij12 = 0; ij12 < 1; ++ij12)
07332 {
07333 if( !j12valid[ij12] )
07334 {
07335     continue;
07336 }
07337 _ij12[0] = ij12; _ij12[1] = -1;
07338 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
07339 {
07340 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
07341 {
07342     j12valid[iij12]=false; _ij12[1] = iij12; break; 
07343 }
07344 }
07345 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
07346 {
07347 IkReal evalcond[6];
07348 IkReal x1720=IKsin(j12);
07349 IkReal x1721=IKcos(j12);
07350 IkReal x1722=((0.3215)*sj11);
07351 IkReal x1723=((0.352)*cj7);
07352 IkReal x1724=((1.0)*npx);
07353 IkReal x1725=((0.3215)*cj11);
07354 IkReal x1726=((0.643)*sj11);
07355 IkReal x1727=((0.352)*sj7);
07356 IkReal x1728=(npy*x1720);
07357 IkReal x1729=(r11*x1727);
07358 IkReal x1730=(r01*x1720);
07359 IkReal x1731=(r10*x1721);
07360 IkReal x1732=(r00*x1721);
07361 evalcond[0]=(((r22*x1725))+(((-1.0)*pz))+((r21*x1720*x1722))+(((-1.0)*r20*x1721*x1722)));
07362 evalcond[1]=((0.02054175)+(((0.643)*cj11*npz))+(((-1.0)*npx*x1721*x1726))+((x1726*x1728))+(((-1.0)*pp)));
07363 evalcond[2]=(((x1722*x1730))+x1723+(((-1.0)*x1722*x1732))+((r02*x1725))+(((-1.0)*px)));
07364 evalcond[3]=(x1727+((r11*x1720*x1722))+(((-1.0)*x1722*x1731))+(((-1.0)*py))+((r12*x1725)));
07365 evalcond[4]=((((-1.0)*npy*x1721))+((r10*x1720*x1727))+((x1721*x1729))+(((-1.0)*x1720*x1724))+((r00*x1720*x1723))+((r01*x1721*x1723)));
07366 evalcond[5]=(((x1723*x1732))+x1728+(((-1.0)*x1720*x1729))+(((-1.0)*x1723*x1730))+((x1727*x1731))+(((-1.0)*x1722))+(((-1.0)*x1721*x1724)));
07367 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07368 {
07369 continue;
07370 }
07371 }
07372 
07373 rotationfunction0(solutions);
07374 }
07375 }
07376 
07377 }
07378 
07379 }
07380 
07381 } else
07382 {
07383 {
07384 IkReal j12array[1], cj12array[1], sj12array[1];
07385 bool j12valid[1]={false};
07386 _nj12 = 1;
07387 IkReal x1733=((643.0)*cj11);
07388 IkReal x1734=((704.0)*sj7);
07389 CheckValue<IkReal> x1735 = IKatan2WithCheck(IkReal((((r01*x1733))+(((2000.0)*rxp0_0))+((r20*x1734)))),IkReal((((r21*x1734))+(((-1.0)*r00*x1733))+(((2000.0)*rxp1_0)))),IKFAST_ATAN2_MAGTHRESH);
07390 if(!x1735.valid){
07391 continue;
07392 }
07393 CheckValue<IkReal> x1736=IKPowWithIntegerCheck(IKsign((r02*sj11)),-1);
07394 if(!x1736.valid){
07395 continue;
07396 }
07397 j12array[0]=((-1.5707963267949)+(x1735.value)+(((1.5707963267949)*(x1736.value))));
07398 sj12array[0]=IKsin(j12array[0]);
07399 cj12array[0]=IKcos(j12array[0]);
07400 if( j12array[0] > IKPI )
07401 {
07402     j12array[0]-=IK2PI;
07403 }
07404 else if( j12array[0] < -IKPI )
07405 {    j12array[0]+=IK2PI;
07406 }
07407 j12valid[0] = true;
07408 for(int ij12 = 0; ij12 < 1; ++ij12)
07409 {
07410 if( !j12valid[ij12] )
07411 {
07412     continue;
07413 }
07414 _ij12[0] = ij12; _ij12[1] = -1;
07415 for(int iij12 = ij12+1; iij12 < 1; ++iij12)
07416 {
07417 if( j12valid[iij12] && IKabs(cj12array[ij12]-cj12array[iij12]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij12]-sj12array[iij12]) < IKFAST_SOLUTION_THRESH )
07418 {
07419     j12valid[iij12]=false; _ij12[1] = iij12; break; 
07420 }
07421 }
07422 j12 = j12array[ij12]; cj12 = cj12array[ij12]; sj12 = sj12array[ij12];
07423 {
07424 IkReal evalcond[6];
07425 IkReal x1737=IKsin(j12);
07426 IkReal x1738=IKcos(j12);
07427 IkReal x1739=((0.3215)*sj11);
07428 IkReal x1740=((0.352)*cj7);
07429 IkReal x1741=((1.0)*npx);
07430 IkReal x1742=((0.3215)*cj11);
07431 IkReal x1743=((0.643)*sj11);
07432 IkReal x1744=((0.352)*sj7);
07433 IkReal x1745=(npy*x1737);
07434 IkReal x1746=(r11*x1744);
07435 IkReal x1747=(r01*x1737);
07436 IkReal x1748=(r10*x1738);
07437 IkReal x1749=(r00*x1738);
07438 evalcond[0]=((((-1.0)*r20*x1738*x1739))+((r22*x1742))+(((-1.0)*pz))+((r21*x1737*x1739)));
07439 evalcond[1]=((0.02054175)+(((0.643)*cj11*npz))+(((-1.0)*pp))+(((-1.0)*npx*x1738*x1743))+((x1743*x1745)));
07440 evalcond[2]=(((x1739*x1747))+x1740+((r02*x1742))+(((-1.0)*x1739*x1749))+(((-1.0)*px)));
07441 evalcond[3]=(((r12*x1742))+x1744+(((-1.0)*x1739*x1748))+(((-1.0)*py))+((r11*x1737*x1739)));
07442 evalcond[4]=(((r00*x1737*x1740))+((r10*x1737*x1744))+(((-1.0)*npy*x1738))+(((-1.0)*x1737*x1741))+((x1738*x1746))+((r01*x1738*x1740)));
07443 evalcond[5]=(x1745+(((-1.0)*x1738*x1741))+(((-1.0)*x1740*x1747))+((x1744*x1748))+(((-1.0)*x1737*x1746))+((x1740*x1749))+(((-1.0)*x1739)));
07444 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
07445 {
07446 continue;
07447 }
07448 }
07449 
07450 rotationfunction0(solutions);
07451 }
07452 }
07453 
07454 }
07455 
07456 }
07457 }
07458 }
07459 }
07460 }
07461 
07462 }
07463 
07464 }
07465 }
07466 return solutions.GetNumSolutions()>0;
07467 }
07468 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
07469 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
07470 IkReal x191=((1.0)*sj11);
07471 IkReal x192=((1.0)*sj7);
07472 IkReal x193=((1.0)*sj12);
07473 IkReal x194=((((-1.0)*r00*x192))+((cj7*r10)));
07474 IkReal x195=((((-1.0)*r01*x192))+((cj7*r11)));
07475 IkReal x196=((((-1.0)*r02*x192))+((cj7*r12)));
07476 IkReal x197=(((cj12*r20))+(((-1.0)*r21*x193)));
07477 IkReal x198=(((r10*sj7))+((cj7*r00)));
07478 IkReal x199=(((r11*sj7))+((cj7*r01)));
07479 IkReal x200=(((r12*sj7))+((cj7*r02)));
07480 IkReal x201=(((cj12*x194))+(((-1.0)*x193*x195)));
07481 IkReal x202=(((cj12*x198))+(((-1.0)*x193*x199)));
07482 new_r00=(((sj11*x196))+((cj11*x201)));
07483 new_r01=(((cj12*x195))+((sj12*x194)));
07484 new_r02=((((-1.0)*x191*x201))+((cj11*x196)));
07485 new_r10=(((cj11*x197))+((r22*sj11)));
07486 new_r11=(((r20*sj12))+((cj12*r21)));
07487 new_r12=((((-1.0)*x191*x197))+((cj11*r22)));
07488 new_r20=(((sj11*x200))+((cj11*x202)));
07489 new_r21=(((cj12*x199))+((sj12*x198)));
07490 new_r22=((((-1.0)*x191*x202))+((cj11*x200)));
07491 {
07492 IkReal j9array[2], cj9array[2], sj9array[2];
07493 bool j9valid[2]={false};
07494 _nj9 = 2;
07495 cj9array[0]=new_r22;
07496 if( cj9array[0] >= -1-IKFAST_SINCOS_THRESH && cj9array[0] <= 1+IKFAST_SINCOS_THRESH )
07497 {
07498     j9valid[0] = j9valid[1] = true;
07499     j9array[0] = IKacos(cj9array[0]);
07500     sj9array[0] = IKsin(j9array[0]);
07501     cj9array[1] = cj9array[0];
07502     j9array[1] = -j9array[0];
07503     sj9array[1] = -sj9array[0];
07504 }
07505 else if( isnan(cj9array[0]) )
07506 {
07507     // probably any value will work
07508     j9valid[0] = true;
07509     cj9array[0] = 1; sj9array[0] = 0; j9array[0] = 0;
07510 }
07511 for(int ij9 = 0; ij9 < 2; ++ij9)
07512 {
07513 if( !j9valid[ij9] )
07514 {
07515     continue;
07516 }
07517 _ij9[0] = ij9; _ij9[1] = -1;
07518 for(int iij9 = ij9+1; iij9 < 2; ++iij9)
07519 {
07520 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
07521 {
07522     j9valid[iij9]=false; _ij9[1] = iij9; break; 
07523 }
07524 }
07525 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
07526 
07527 {
07528 IkReal j8eval[3];
07529 j8eval[0]=sj9;
07530 j8eval[1]=((IKabs(new_r12))+(IKabs(new_r02)));
07531 j8eval[2]=IKsign(sj9);
07532 if( IKabs(j8eval[0]) < 0.0000010000000000  || IKabs(j8eval[1]) < 0.0000010000000000  || IKabs(j8eval[2]) < 0.0000010000000000  )
07533 {
07534 {
07535 IkReal j10eval[3];
07536 j10eval[0]=sj9;
07537 j10eval[1]=IKsign(sj9);
07538 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
07539 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
07540 {
07541 {
07542 IkReal j8eval[2];
07543 j8eval[0]=new_r12;
07544 j8eval[1]=sj9;
07545 if( IKabs(j8eval[0]) < 0.0000010000000000  || IKabs(j8eval[1]) < 0.0000010000000000  )
07546 {
07547 {
07548 IkReal evalcond[5];
07549 bool bgotonextstatement = true;
07550 do
07551 {
07552 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
07553 evalcond[1]=new_r20;
07554 evalcond[2]=new_r02;
07555 evalcond[3]=new_r12;
07556 evalcond[4]=new_r21;
07557 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
07558 {
07559 bgotonextstatement=false;
07560 IkReal j10mul = 1;
07561 j10=0;
07562 j8mul=-1.0;
07563 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
07564     continue;
07565 j8=IKatan2(((-1.0)*new_r01), new_r00);
07566 {
07567 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07568 vinfos[0].jointtype = 1;
07569 vinfos[0].foffset = j6;
07570 vinfos[0].indices[0] = _ij6[0];
07571 vinfos[0].indices[1] = _ij6[1];
07572 vinfos[0].maxsolutions = _nj6;
07573 vinfos[1].jointtype = 1;
07574 vinfos[1].foffset = j7;
07575 vinfos[1].indices[0] = _ij7[0];
07576 vinfos[1].indices[1] = _ij7[1];
07577 vinfos[1].maxsolutions = _nj7;
07578 vinfos[2].jointtype = 1;
07579 vinfos[2].foffset = j8;
07580 vinfos[2].fmul = j8mul;
07581 vinfos[2].freeind = 0;
07582 vinfos[2].maxsolutions = 0;
07583 vinfos[3].jointtype = 1;
07584 vinfos[3].foffset = j9;
07585 vinfos[3].indices[0] = _ij9[0];
07586 vinfos[3].indices[1] = _ij9[1];
07587 vinfos[3].maxsolutions = _nj9;
07588 vinfos[4].jointtype = 1;
07589 vinfos[4].foffset = j10;
07590 vinfos[4].fmul = j10mul;
07591 vinfos[4].freeind = 0;
07592 vinfos[4].maxsolutions = 0;
07593 vinfos[5].jointtype = 1;
07594 vinfos[5].foffset = j11;
07595 vinfos[5].indices[0] = _ij11[0];
07596 vinfos[5].indices[1] = _ij11[1];
07597 vinfos[5].maxsolutions = _nj11;
07598 vinfos[6].jointtype = 1;
07599 vinfos[6].foffset = j12;
07600 vinfos[6].indices[0] = _ij12[0];
07601 vinfos[6].indices[1] = _ij12[1];
07602 vinfos[6].maxsolutions = _nj12;
07603 std::vector<int> vfree(1);
07604 vfree[0] = 4;
07605 solutions.AddSolution(vinfos,vfree);
07606 }
07607 
07608 }
07609 } while(0);
07610 if( bgotonextstatement )
07611 {
07612 bool bgotonextstatement = true;
07613 do
07614 {
07615 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
07616 evalcond[1]=new_r20;
07617 evalcond[2]=new_r02;
07618 evalcond[3]=new_r12;
07619 evalcond[4]=new_r21;
07620 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
07621 {
07622 bgotonextstatement=false;
07623 IkReal j10mul = 1;
07624 j10=0;
07625 j8mul=1.0;
07626 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
07627     continue;
07628 j8=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
07629 {
07630 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07631 vinfos[0].jointtype = 1;
07632 vinfos[0].foffset = j6;
07633 vinfos[0].indices[0] = _ij6[0];
07634 vinfos[0].indices[1] = _ij6[1];
07635 vinfos[0].maxsolutions = _nj6;
07636 vinfos[1].jointtype = 1;
07637 vinfos[1].foffset = j7;
07638 vinfos[1].indices[0] = _ij7[0];
07639 vinfos[1].indices[1] = _ij7[1];
07640 vinfos[1].maxsolutions = _nj7;
07641 vinfos[2].jointtype = 1;
07642 vinfos[2].foffset = j8;
07643 vinfos[2].fmul = j8mul;
07644 vinfos[2].freeind = 0;
07645 vinfos[2].maxsolutions = 0;
07646 vinfos[3].jointtype = 1;
07647 vinfos[3].foffset = j9;
07648 vinfos[3].indices[0] = _ij9[0];
07649 vinfos[3].indices[1] = _ij9[1];
07650 vinfos[3].maxsolutions = _nj9;
07651 vinfos[4].jointtype = 1;
07652 vinfos[4].foffset = j10;
07653 vinfos[4].fmul = j10mul;
07654 vinfos[4].freeind = 0;
07655 vinfos[4].maxsolutions = 0;
07656 vinfos[5].jointtype = 1;
07657 vinfos[5].foffset = j11;
07658 vinfos[5].indices[0] = _ij11[0];
07659 vinfos[5].indices[1] = _ij11[1];
07660 vinfos[5].maxsolutions = _nj11;
07661 vinfos[6].jointtype = 1;
07662 vinfos[6].foffset = j12;
07663 vinfos[6].indices[0] = _ij12[0];
07664 vinfos[6].indices[1] = _ij12[1];
07665 vinfos[6].maxsolutions = _nj12;
07666 std::vector<int> vfree(1);
07667 vfree[0] = 4;
07668 solutions.AddSolution(vinfos,vfree);
07669 }
07670 
07671 }
07672 } while(0);
07673 if( bgotonextstatement )
07674 {
07675 bool bgotonextstatement = true;
07676 do
07677 {
07678 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
07679 if( IKabs(evalcond[0]) < 0.0000050000000000  )
07680 {
07681 bgotonextstatement=false;
07682 {
07683 IkReal j8eval[1];
07684 new_r02=0;
07685 new_r12=0;
07686 new_r20=0;
07687 new_r21=0;
07688 IkReal x203=new_r22*new_r22;
07689 IkReal x204=((16.0)*new_r10);
07690 IkReal x205=((16.0)*new_r01);
07691 IkReal x206=((16.0)*new_r22);
07692 IkReal x207=((8.0)*new_r11);
07693 IkReal x208=((8.0)*new_r00);
07694 IkReal x209=(x203*x204);
07695 IkReal x210=(x203*x205);
07696 j8eval[0]=((IKabs(((((-1.0)*x205))+x210)))+(IKabs(((((-1.0)*x204))+x209)))+(IKabs(((((-1.0)*new_r00*x206))+(((32.0)*new_r11))+(((-16.0)*new_r11*x203)))))+(IKabs((((new_r11*x206))+(((-32.0)*new_r00*x203))+(((16.0)*new_r00)))))+(IKabs(((((-1.0)*x208))+((new_r22*x207)))))+(IKabs(((((-1.0)*new_r22*x208))+((x203*x207)))))+(IKabs(((((-1.0)*x209))+x204)))+(IKabs((x205+(((-1.0)*x210))))));
07697 if( IKabs(j8eval[0]) < 0.0000000010000000  )
07698 {
07699 continue; // no branches [j8, j10]
07700 
07701 } else
07702 {
07703 IkReal op[4+1], zeror[4];
07704 int numroots;
07705 IkReal j8evalpoly[1];
07706 IkReal x211=new_r22*new_r22;
07707 IkReal x212=((16.0)*new_r10);
07708 IkReal x213=(new_r11*new_r22);
07709 IkReal x214=(x211*x212);
07710 IkReal x215=((((8.0)*x213))+(((-8.0)*new_r00)));
07711 op[0]=x215;
07712 op[1]=(x212+(((-1.0)*x214)));
07713 op[2]=((((-32.0)*new_r00*x211))+(((16.0)*x213))+(((16.0)*new_r00)));
07714 op[3]=(x214+(((-1.0)*x212)));
07715 op[4]=x215;
07716 polyroots4(op,zeror,numroots);
07717 IkReal j8array[4], cj8array[4], sj8array[4], tempj8array[1];
07718 int numsolutions = 0;
07719 for(int ij8 = 0; ij8 < numroots; ++ij8)
07720 {
07721 IkReal htj8 = zeror[ij8];
07722 tempj8array[0]=((2.0)*(atan(htj8)));
07723 for(int kj8 = 0; kj8 < 1; ++kj8)
07724 {
07725 j8array[numsolutions] = tempj8array[kj8];
07726 if( j8array[numsolutions] > IKPI )
07727 {
07728     j8array[numsolutions]-=IK2PI;
07729 }
07730 else if( j8array[numsolutions] < -IKPI )
07731 {
07732     j8array[numsolutions]+=IK2PI;
07733 }
07734 sj8array[numsolutions] = IKsin(j8array[numsolutions]);
07735 cj8array[numsolutions] = IKcos(j8array[numsolutions]);
07736 numsolutions++;
07737 }
07738 }
07739 bool j8valid[4]={true,true,true,true};
07740 _nj8 = 4;
07741 for(int ij8 = 0; ij8 < numsolutions; ++ij8)
07742     {
07743 if( !j8valid[ij8] )
07744 {
07745     continue;
07746 }
07747     j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
07748 htj8 = IKtan(j8/2);
07749 
07750 IkReal x216=new_r22*new_r22;
07751 IkReal x217=((16.0)*new_r01);
07752 IkReal x218=(new_r00*new_r22);
07753 IkReal x219=((8.0)*x218);
07754 IkReal x220=(new_r11*x216);
07755 IkReal x221=((8.0)*x220);
07756 IkReal x222=(x216*x217);
07757 j8evalpoly[0]=((((htj8*htj8*htj8*htj8)*((x221+(((-1.0)*x219))))))+((htj8*((x217+(((-1.0)*x222))))))+x221+(((htj8*htj8)*(((((32.0)*new_r11))+(((-16.0)*x218))+(((-16.0)*x220))))))+(((-1.0)*x219))+(((htj8*htj8*htj8)*((x222+(((-1.0)*x217)))))));
07758 if( IKabs(j8evalpoly[0]) > 0.0000000010000000  )
07759 {
07760     continue;
07761 }
07762 _ij8[0] = ij8; _ij8[1] = -1;
07763 for(int iij8 = ij8+1; iij8 < numsolutions; ++iij8)
07764 {
07765 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
07766 {
07767     j8valid[iij8]=false; _ij8[1] = iij8; break; 
07768 }
07769 }
07770 {
07771 IkReal j10eval[3];
07772 new_r02=0;
07773 new_r12=0;
07774 new_r20=0;
07775 new_r21=0;
07776 IkReal x223=cj8*cj8;
07777 IkReal x224=(cj8*new_r22);
07778 IkReal x225=((-1.0)+(((-1.0)*x223*(new_r22*new_r22)))+x223);
07779 j10eval[0]=x225;
07780 j10eval[1]=IKsign(x225);
07781 j10eval[2]=((IKabs((((new_r01*x224))+((new_r00*sj8)))))+(IKabs((((new_r01*sj8))+(((-1.0)*new_r00*x224))))));
07782 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
07783 {
07784 {
07785 IkReal j10eval[1];
07786 new_r02=0;
07787 new_r12=0;
07788 new_r20=0;
07789 new_r21=0;
07790 j10eval[0]=new_r22;
07791 if( IKabs(j10eval[0]) < 0.0000010000000000  )
07792 {
07793 {
07794 IkReal j10eval[2];
07795 new_r02=0;
07796 new_r12=0;
07797 new_r20=0;
07798 new_r21=0;
07799 IkReal x226=new_r22*new_r22;
07800 j10eval[0]=(((cj8*x226))+(((-1.0)*cj8)));
07801 j10eval[1]=((((-1.0)*sj8))+((sj8*x226)));
07802 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  )
07803 {
07804 {
07805 IkReal evalcond[1];
07806 bool bgotonextstatement = true;
07807 do
07808 {
07809 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j8)))), 6.28318530717959)));
07810 if( IKabs(evalcond[0]) < 0.0000050000000000  )
07811 {
07812 bgotonextstatement=false;
07813 {
07814 IkReal j10array[1], cj10array[1], sj10array[1];
07815 bool j10valid[1]={false};
07816 _nj10 = 1;
07817 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
07818     continue;
07819 j10array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
07820 sj10array[0]=IKsin(j10array[0]);
07821 cj10array[0]=IKcos(j10array[0]);
07822 if( j10array[0] > IKPI )
07823 {
07824     j10array[0]-=IK2PI;
07825 }
07826 else if( j10array[0] < -IKPI )
07827 {    j10array[0]+=IK2PI;
07828 }
07829 j10valid[0] = true;
07830 for(int ij10 = 0; ij10 < 1; ++ij10)
07831 {
07832 if( !j10valid[ij10] )
07833 {
07834     continue;
07835 }
07836 _ij10[0] = ij10; _ij10[1] = -1;
07837 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07838 {
07839 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07840 {
07841     j10valid[iij10]=false; _ij10[1] = iij10; break; 
07842 }
07843 }
07844 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07845 {
07846 IkReal evalcond[4];
07847 IkReal x227=IKsin(j10);
07848 IkReal x228=IKcos(j10);
07849 evalcond[0]=x227;
07850 evalcond[1]=((-1.0)*x228);
07851 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x227)));
07852 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x228)));
07853 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
07854 {
07855 continue;
07856 }
07857 }
07858 
07859 {
07860 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07861 vinfos[0].jointtype = 1;
07862 vinfos[0].foffset = j6;
07863 vinfos[0].indices[0] = _ij6[0];
07864 vinfos[0].indices[1] = _ij6[1];
07865 vinfos[0].maxsolutions = _nj6;
07866 vinfos[1].jointtype = 1;
07867 vinfos[1].foffset = j7;
07868 vinfos[1].indices[0] = _ij7[0];
07869 vinfos[1].indices[1] = _ij7[1];
07870 vinfos[1].maxsolutions = _nj7;
07871 vinfos[2].jointtype = 1;
07872 vinfos[2].foffset = j8;
07873 vinfos[2].indices[0] = _ij8[0];
07874 vinfos[2].indices[1] = _ij8[1];
07875 vinfos[2].maxsolutions = _nj8;
07876 vinfos[3].jointtype = 1;
07877 vinfos[3].foffset = j9;
07878 vinfos[3].indices[0] = _ij9[0];
07879 vinfos[3].indices[1] = _ij9[1];
07880 vinfos[3].maxsolutions = _nj9;
07881 vinfos[4].jointtype = 1;
07882 vinfos[4].foffset = j10;
07883 vinfos[4].indices[0] = _ij10[0];
07884 vinfos[4].indices[1] = _ij10[1];
07885 vinfos[4].maxsolutions = _nj10;
07886 vinfos[5].jointtype = 1;
07887 vinfos[5].foffset = j11;
07888 vinfos[5].indices[0] = _ij11[0];
07889 vinfos[5].indices[1] = _ij11[1];
07890 vinfos[5].maxsolutions = _nj11;
07891 vinfos[6].jointtype = 1;
07892 vinfos[6].foffset = j12;
07893 vinfos[6].indices[0] = _ij12[0];
07894 vinfos[6].indices[1] = _ij12[1];
07895 vinfos[6].maxsolutions = _nj12;
07896 std::vector<int> vfree(0);
07897 solutions.AddSolution(vinfos,vfree);
07898 }
07899 }
07900 }
07901 
07902 }
07903 } while(0);
07904 if( bgotonextstatement )
07905 {
07906 bool bgotonextstatement = true;
07907 do
07908 {
07909 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j8)))), 6.28318530717959)));
07910 if( IKabs(evalcond[0]) < 0.0000050000000000  )
07911 {
07912 bgotonextstatement=false;
07913 {
07914 IkReal j10array[1], cj10array[1], sj10array[1];
07915 bool j10valid[1]={false};
07916 _nj10 = 1;
07917 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
07918     continue;
07919 j10array[0]=IKatan2(new_r00, new_r01);
07920 sj10array[0]=IKsin(j10array[0]);
07921 cj10array[0]=IKcos(j10array[0]);
07922 if( j10array[0] > IKPI )
07923 {
07924     j10array[0]-=IK2PI;
07925 }
07926 else if( j10array[0] < -IKPI )
07927 {    j10array[0]+=IK2PI;
07928 }
07929 j10valid[0] = true;
07930 for(int ij10 = 0; ij10 < 1; ++ij10)
07931 {
07932 if( !j10valid[ij10] )
07933 {
07934     continue;
07935 }
07936 _ij10[0] = ij10; _ij10[1] = -1;
07937 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07938 {
07939 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07940 {
07941     j10valid[iij10]=false; _ij10[1] = iij10; break; 
07942 }
07943 }
07944 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07945 {
07946 IkReal evalcond[4];
07947 IkReal x229=IKsin(j10);
07948 IkReal x230=IKcos(j10);
07949 evalcond[0]=x229;
07950 evalcond[1]=((-1.0)*x230);
07951 evalcond[2]=(new_r00+(((-1.0)*x229)));
07952 evalcond[3]=(new_r01+(((-1.0)*x230)));
07953 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
07954 {
07955 continue;
07956 }
07957 }
07958 
07959 {
07960 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07961 vinfos[0].jointtype = 1;
07962 vinfos[0].foffset = j6;
07963 vinfos[0].indices[0] = _ij6[0];
07964 vinfos[0].indices[1] = _ij6[1];
07965 vinfos[0].maxsolutions = _nj6;
07966 vinfos[1].jointtype = 1;
07967 vinfos[1].foffset = j7;
07968 vinfos[1].indices[0] = _ij7[0];
07969 vinfos[1].indices[1] = _ij7[1];
07970 vinfos[1].maxsolutions = _nj7;
07971 vinfos[2].jointtype = 1;
07972 vinfos[2].foffset = j8;
07973 vinfos[2].indices[0] = _ij8[0];
07974 vinfos[2].indices[1] = _ij8[1];
07975 vinfos[2].maxsolutions = _nj8;
07976 vinfos[3].jointtype = 1;
07977 vinfos[3].foffset = j9;
07978 vinfos[3].indices[0] = _ij9[0];
07979 vinfos[3].indices[1] = _ij9[1];
07980 vinfos[3].maxsolutions = _nj9;
07981 vinfos[4].jointtype = 1;
07982 vinfos[4].foffset = j10;
07983 vinfos[4].indices[0] = _ij10[0];
07984 vinfos[4].indices[1] = _ij10[1];
07985 vinfos[4].maxsolutions = _nj10;
07986 vinfos[5].jointtype = 1;
07987 vinfos[5].foffset = j11;
07988 vinfos[5].indices[0] = _ij11[0];
07989 vinfos[5].indices[1] = _ij11[1];
07990 vinfos[5].maxsolutions = _nj11;
07991 vinfos[6].jointtype = 1;
07992 vinfos[6].foffset = j12;
07993 vinfos[6].indices[0] = _ij12[0];
07994 vinfos[6].indices[1] = _ij12[1];
07995 vinfos[6].maxsolutions = _nj12;
07996 std::vector<int> vfree(0);
07997 solutions.AddSolution(vinfos,vfree);
07998 }
07999 }
08000 }
08001 
08002 }
08003 } while(0);
08004 if( bgotonextstatement )
08005 {
08006 bool bgotonextstatement = true;
08007 do
08008 {
08009 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j8))), 6.28318530717959)));
08010 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08011 {
08012 bgotonextstatement=false;
08013 {
08014 IkReal j10array[1], cj10array[1], sj10array[1];
08015 bool j10valid[1]={false};
08016 _nj10 = 1;
08017 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
08018     continue;
08019 j10array[0]=IKatan2(new_r10, new_r11);
08020 sj10array[0]=IKsin(j10array[0]);
08021 cj10array[0]=IKcos(j10array[0]);
08022 if( j10array[0] > IKPI )
08023 {
08024     j10array[0]-=IK2PI;
08025 }
08026 else if( j10array[0] < -IKPI )
08027 {    j10array[0]+=IK2PI;
08028 }
08029 j10valid[0] = true;
08030 for(int ij10 = 0; ij10 < 1; ++ij10)
08031 {
08032 if( !j10valid[ij10] )
08033 {
08034     continue;
08035 }
08036 _ij10[0] = ij10; _ij10[1] = -1;
08037 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08038 {
08039 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08040 {
08041     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08042 }
08043 }
08044 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08045 {
08046 IkReal evalcond[4];
08047 IkReal x231=IKsin(j10);
08048 IkReal x232=IKcos(j10);
08049 evalcond[0]=x231;
08050 evalcond[1]=((-1.0)*x232);
08051 evalcond[2]=(new_r10+(((-1.0)*x231)));
08052 evalcond[3]=(new_r11+(((-1.0)*x232)));
08053 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
08054 {
08055 continue;
08056 }
08057 }
08058 
08059 {
08060 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08061 vinfos[0].jointtype = 1;
08062 vinfos[0].foffset = j6;
08063 vinfos[0].indices[0] = _ij6[0];
08064 vinfos[0].indices[1] = _ij6[1];
08065 vinfos[0].maxsolutions = _nj6;
08066 vinfos[1].jointtype = 1;
08067 vinfos[1].foffset = j7;
08068 vinfos[1].indices[0] = _ij7[0];
08069 vinfos[1].indices[1] = _ij7[1];
08070 vinfos[1].maxsolutions = _nj7;
08071 vinfos[2].jointtype = 1;
08072 vinfos[2].foffset = j8;
08073 vinfos[2].indices[0] = _ij8[0];
08074 vinfos[2].indices[1] = _ij8[1];
08075 vinfos[2].maxsolutions = _nj8;
08076 vinfos[3].jointtype = 1;
08077 vinfos[3].foffset = j9;
08078 vinfos[3].indices[0] = _ij9[0];
08079 vinfos[3].indices[1] = _ij9[1];
08080 vinfos[3].maxsolutions = _nj9;
08081 vinfos[4].jointtype = 1;
08082 vinfos[4].foffset = j10;
08083 vinfos[4].indices[0] = _ij10[0];
08084 vinfos[4].indices[1] = _ij10[1];
08085 vinfos[4].maxsolutions = _nj10;
08086 vinfos[5].jointtype = 1;
08087 vinfos[5].foffset = j11;
08088 vinfos[5].indices[0] = _ij11[0];
08089 vinfos[5].indices[1] = _ij11[1];
08090 vinfos[5].maxsolutions = _nj11;
08091 vinfos[6].jointtype = 1;
08092 vinfos[6].foffset = j12;
08093 vinfos[6].indices[0] = _ij12[0];
08094 vinfos[6].indices[1] = _ij12[1];
08095 vinfos[6].maxsolutions = _nj12;
08096 std::vector<int> vfree(0);
08097 solutions.AddSolution(vinfos,vfree);
08098 }
08099 }
08100 }
08101 
08102 }
08103 } while(0);
08104 if( bgotonextstatement )
08105 {
08106 bool bgotonextstatement = true;
08107 do
08108 {
08109 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j8)))), 6.28318530717959)));
08110 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08111 {
08112 bgotonextstatement=false;
08113 {
08114 IkReal j10array[1], cj10array[1], sj10array[1];
08115 bool j10valid[1]={false};
08116 _nj10 = 1;
08117 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
08118     continue;
08119 j10array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
08120 sj10array[0]=IKsin(j10array[0]);
08121 cj10array[0]=IKcos(j10array[0]);
08122 if( j10array[0] > IKPI )
08123 {
08124     j10array[0]-=IK2PI;
08125 }
08126 else if( j10array[0] < -IKPI )
08127 {    j10array[0]+=IK2PI;
08128 }
08129 j10valid[0] = true;
08130 for(int ij10 = 0; ij10 < 1; ++ij10)
08131 {
08132 if( !j10valid[ij10] )
08133 {
08134     continue;
08135 }
08136 _ij10[0] = ij10; _ij10[1] = -1;
08137 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08138 {
08139 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08140 {
08141     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08142 }
08143 }
08144 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08145 {
08146 IkReal evalcond[4];
08147 IkReal x233=IKsin(j10);
08148 IkReal x234=IKcos(j10);
08149 evalcond[0]=x233;
08150 evalcond[1]=((-1.0)*x234);
08151 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x233)));
08152 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x234)));
08153 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
08154 {
08155 continue;
08156 }
08157 }
08158 
08159 {
08160 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08161 vinfos[0].jointtype = 1;
08162 vinfos[0].foffset = j6;
08163 vinfos[0].indices[0] = _ij6[0];
08164 vinfos[0].indices[1] = _ij6[1];
08165 vinfos[0].maxsolutions = _nj6;
08166 vinfos[1].jointtype = 1;
08167 vinfos[1].foffset = j7;
08168 vinfos[1].indices[0] = _ij7[0];
08169 vinfos[1].indices[1] = _ij7[1];
08170 vinfos[1].maxsolutions = _nj7;
08171 vinfos[2].jointtype = 1;
08172 vinfos[2].foffset = j8;
08173 vinfos[2].indices[0] = _ij8[0];
08174 vinfos[2].indices[1] = _ij8[1];
08175 vinfos[2].maxsolutions = _nj8;
08176 vinfos[3].jointtype = 1;
08177 vinfos[3].foffset = j9;
08178 vinfos[3].indices[0] = _ij9[0];
08179 vinfos[3].indices[1] = _ij9[1];
08180 vinfos[3].maxsolutions = _nj9;
08181 vinfos[4].jointtype = 1;
08182 vinfos[4].foffset = j10;
08183 vinfos[4].indices[0] = _ij10[0];
08184 vinfos[4].indices[1] = _ij10[1];
08185 vinfos[4].maxsolutions = _nj10;
08186 vinfos[5].jointtype = 1;
08187 vinfos[5].foffset = j11;
08188 vinfos[5].indices[0] = _ij11[0];
08189 vinfos[5].indices[1] = _ij11[1];
08190 vinfos[5].maxsolutions = _nj11;
08191 vinfos[6].jointtype = 1;
08192 vinfos[6].foffset = j12;
08193 vinfos[6].indices[0] = _ij12[0];
08194 vinfos[6].indices[1] = _ij12[1];
08195 vinfos[6].maxsolutions = _nj12;
08196 std::vector<int> vfree(0);
08197 solutions.AddSolution(vinfos,vfree);
08198 }
08199 }
08200 }
08201 
08202 }
08203 } while(0);
08204 if( bgotonextstatement )
08205 {
08206 bool bgotonextstatement = true;
08207 do
08208 {
08209 CheckValue<IkReal> x235=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08210 if(!x235.valid){
08211 continue;
08212 }
08213 if((x235.value) < -0.00001)
08214 continue;
08215 IkReal gconst82=((-1.0)*(IKsqrt(x235.value)));
08216 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj8)))))+(IKabs((cj8+(((-1.0)*gconst82)))))), 6.28318530717959)));
08217 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08218 {
08219 bgotonextstatement=false;
08220 {
08221 IkReal j10eval[1];
08222 new_r02=0;
08223 new_r12=0;
08224 new_r20=0;
08225 new_r21=0;
08226 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08227 continue;
08228 sj8=IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))));
08229 cj8=gconst82;
08230 if( (gconst82) < -1-IKFAST_SINCOS_THRESH || (gconst82) > 1+IKFAST_SINCOS_THRESH )
08231     continue;
08232 j8=IKacos(gconst82);
08233 CheckValue<IkReal> x236=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08234 if(!x236.valid){
08235 continue;
08236 }
08237 if((x236.value) < -0.00001)
08238 continue;
08239 IkReal gconst82=((-1.0)*(IKsqrt(x236.value)));
08240 j10eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
08241 if( IKabs(j10eval[0]) < 0.0000010000000000  )
08242 {
08243 {
08244 IkReal j10array[1], cj10array[1], sj10array[1];
08245 bool j10valid[1]={false};
08246 _nj10 = 1;
08247 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08248 continue;
08249 CheckValue<IkReal> x237=IKPowWithIntegerCheck(gconst82,-1);
08250 if(!x237.valid){
08251 continue;
08252 }
08253 if( IKabs(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x237.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10))))+IKsqr((new_r11*(x237.value)))-1) <= IKFAST_SINCOS_THRESH )
08254     continue;
08255 j10array[0]=IKatan2(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10))), (new_r11*(x237.value)));
08256 sj10array[0]=IKsin(j10array[0]);
08257 cj10array[0]=IKcos(j10array[0]);
08258 if( j10array[0] > IKPI )
08259 {
08260     j10array[0]-=IK2PI;
08261 }
08262 else if( j10array[0] < -IKPI )
08263 {    j10array[0]+=IK2PI;
08264 }
08265 j10valid[0] = true;
08266 for(int ij10 = 0; ij10 < 1; ++ij10)
08267 {
08268 if( !j10valid[ij10] )
08269 {
08270     continue;
08271 }
08272 _ij10[0] = ij10; _ij10[1] = -1;
08273 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08274 {
08275 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08276 {
08277     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08278 }
08279 }
08280 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08281 {
08282 IkReal evalcond[8];
08283 IkReal x238=IKcos(j10);
08284 IkReal x239=IKsin(j10);
08285 IkReal x240=((1.0)*x238);
08286 IkReal x241=((1.0)*x239);
08287 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08288 continue;
08289 IkReal x242=IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))));
08290 IkReal x243=((1.0)*x242);
08291 evalcond[0]=x239;
08292 evalcond[1]=((-1.0)*x238);
08293 evalcond[2]=(new_r11+(((-1.0)*gconst82*x240)));
08294 evalcond[3]=(new_r10+(((-1.0)*gconst82*x241)));
08295 evalcond[4]=(((x238*x242))+new_r01);
08296 evalcond[5]=(((x239*x242))+new_r00);
08297 evalcond[6]=((((-1.0)*new_r00*x243))+((gconst82*new_r10))+(((-1.0)*x241)));
08298 evalcond[7]=((((-1.0)*new_r01*x243))+((gconst82*new_r11))+(((-1.0)*x240)));
08299 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08300 {
08301 continue;
08302 }
08303 }
08304 
08305 {
08306 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08307 vinfos[0].jointtype = 1;
08308 vinfos[0].foffset = j6;
08309 vinfos[0].indices[0] = _ij6[0];
08310 vinfos[0].indices[1] = _ij6[1];
08311 vinfos[0].maxsolutions = _nj6;
08312 vinfos[1].jointtype = 1;
08313 vinfos[1].foffset = j7;
08314 vinfos[1].indices[0] = _ij7[0];
08315 vinfos[1].indices[1] = _ij7[1];
08316 vinfos[1].maxsolutions = _nj7;
08317 vinfos[2].jointtype = 1;
08318 vinfos[2].foffset = j8;
08319 vinfos[2].indices[0] = _ij8[0];
08320 vinfos[2].indices[1] = _ij8[1];
08321 vinfos[2].maxsolutions = _nj8;
08322 vinfos[3].jointtype = 1;
08323 vinfos[3].foffset = j9;
08324 vinfos[3].indices[0] = _ij9[0];
08325 vinfos[3].indices[1] = _ij9[1];
08326 vinfos[3].maxsolutions = _nj9;
08327 vinfos[4].jointtype = 1;
08328 vinfos[4].foffset = j10;
08329 vinfos[4].indices[0] = _ij10[0];
08330 vinfos[4].indices[1] = _ij10[1];
08331 vinfos[4].maxsolutions = _nj10;
08332 vinfos[5].jointtype = 1;
08333 vinfos[5].foffset = j11;
08334 vinfos[5].indices[0] = _ij11[0];
08335 vinfos[5].indices[1] = _ij11[1];
08336 vinfos[5].maxsolutions = _nj11;
08337 vinfos[6].jointtype = 1;
08338 vinfos[6].foffset = j12;
08339 vinfos[6].indices[0] = _ij12[0];
08340 vinfos[6].indices[1] = _ij12[1];
08341 vinfos[6].maxsolutions = _nj12;
08342 std::vector<int> vfree(0);
08343 solutions.AddSolution(vinfos,vfree);
08344 }
08345 }
08346 }
08347 
08348 } else
08349 {
08350 {
08351 IkReal j10array[1], cj10array[1], sj10array[1];
08352 bool j10valid[1]={false};
08353 _nj10 = 1;
08354 CheckValue<IkReal> x244 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
08355 if(!x244.valid){
08356 continue;
08357 }
08358 CheckValue<IkReal> x245=IKPowWithIntegerCheck(IKsign(gconst82),-1);
08359 if(!x245.valid){
08360 continue;
08361 }
08362 j10array[0]=((-1.5707963267949)+(x244.value)+(((1.5707963267949)*(x245.value))));
08363 sj10array[0]=IKsin(j10array[0]);
08364 cj10array[0]=IKcos(j10array[0]);
08365 if( j10array[0] > IKPI )
08366 {
08367     j10array[0]-=IK2PI;
08368 }
08369 else if( j10array[0] < -IKPI )
08370 {    j10array[0]+=IK2PI;
08371 }
08372 j10valid[0] = true;
08373 for(int ij10 = 0; ij10 < 1; ++ij10)
08374 {
08375 if( !j10valid[ij10] )
08376 {
08377     continue;
08378 }
08379 _ij10[0] = ij10; _ij10[1] = -1;
08380 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08381 {
08382 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08383 {
08384     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08385 }
08386 }
08387 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08388 {
08389 IkReal evalcond[8];
08390 IkReal x246=IKcos(j10);
08391 IkReal x247=IKsin(j10);
08392 IkReal x248=((1.0)*x246);
08393 IkReal x249=((1.0)*x247);
08394 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08395 continue;
08396 IkReal x250=IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))));
08397 IkReal x251=((1.0)*x250);
08398 evalcond[0]=x247;
08399 evalcond[1]=((-1.0)*x246);
08400 evalcond[2]=(new_r11+(((-1.0)*gconst82*x248)));
08401 evalcond[3]=(new_r10+(((-1.0)*gconst82*x249)));
08402 evalcond[4]=(((x246*x250))+new_r01);
08403 evalcond[5]=(new_r00+((x247*x250)));
08404 evalcond[6]=(((gconst82*new_r10))+(((-1.0)*x249))+(((-1.0)*new_r00*x251)));
08405 evalcond[7]=((((-1.0)*new_r01*x251))+((gconst82*new_r11))+(((-1.0)*x248)));
08406 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08407 {
08408 continue;
08409 }
08410 }
08411 
08412 {
08413 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08414 vinfos[0].jointtype = 1;
08415 vinfos[0].foffset = j6;
08416 vinfos[0].indices[0] = _ij6[0];
08417 vinfos[0].indices[1] = _ij6[1];
08418 vinfos[0].maxsolutions = _nj6;
08419 vinfos[1].jointtype = 1;
08420 vinfos[1].foffset = j7;
08421 vinfos[1].indices[0] = _ij7[0];
08422 vinfos[1].indices[1] = _ij7[1];
08423 vinfos[1].maxsolutions = _nj7;
08424 vinfos[2].jointtype = 1;
08425 vinfos[2].foffset = j8;
08426 vinfos[2].indices[0] = _ij8[0];
08427 vinfos[2].indices[1] = _ij8[1];
08428 vinfos[2].maxsolutions = _nj8;
08429 vinfos[3].jointtype = 1;
08430 vinfos[3].foffset = j9;
08431 vinfos[3].indices[0] = _ij9[0];
08432 vinfos[3].indices[1] = _ij9[1];
08433 vinfos[3].maxsolutions = _nj9;
08434 vinfos[4].jointtype = 1;
08435 vinfos[4].foffset = j10;
08436 vinfos[4].indices[0] = _ij10[0];
08437 vinfos[4].indices[1] = _ij10[1];
08438 vinfos[4].maxsolutions = _nj10;
08439 vinfos[5].jointtype = 1;
08440 vinfos[5].foffset = j11;
08441 vinfos[5].indices[0] = _ij11[0];
08442 vinfos[5].indices[1] = _ij11[1];
08443 vinfos[5].maxsolutions = _nj11;
08444 vinfos[6].jointtype = 1;
08445 vinfos[6].foffset = j12;
08446 vinfos[6].indices[0] = _ij12[0];
08447 vinfos[6].indices[1] = _ij12[1];
08448 vinfos[6].maxsolutions = _nj12;
08449 std::vector<int> vfree(0);
08450 solutions.AddSolution(vinfos,vfree);
08451 }
08452 }
08453 }
08454 
08455 }
08456 
08457 }
08458 
08459 }
08460 } while(0);
08461 if( bgotonextstatement )
08462 {
08463 bool bgotonextstatement = true;
08464 do
08465 {
08466 CheckValue<IkReal> x252=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08467 if(!x252.valid){
08468 continue;
08469 }
08470 if((x252.value) < -0.00001)
08471 continue;
08472 IkReal gconst82=((-1.0)*(IKsqrt(x252.value)));
08473 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj8)))))+(IKabs((cj8+(((-1.0)*gconst82)))))), 6.28318530717959)));
08474 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08475 {
08476 bgotonextstatement=false;
08477 {
08478 IkReal j10eval[1];
08479 new_r02=0;
08480 new_r12=0;
08481 new_r20=0;
08482 new_r21=0;
08483 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08484 continue;
08485 sj8=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))))));
08486 cj8=gconst82;
08487 if( (gconst82) < -1-IKFAST_SINCOS_THRESH || (gconst82) > 1+IKFAST_SINCOS_THRESH )
08488     continue;
08489 j8=((-1.0)*(IKacos(gconst82)));
08490 CheckValue<IkReal> x253=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08491 if(!x253.valid){
08492 continue;
08493 }
08494 if((x253.value) < -0.00001)
08495 continue;
08496 IkReal gconst82=((-1.0)*(IKsqrt(x253.value)));
08497 j10eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
08498 if( IKabs(j10eval[0]) < 0.0000010000000000  )
08499 {
08500 {
08501 IkReal j10array[1], cj10array[1], sj10array[1];
08502 bool j10valid[1]={false};
08503 _nj10 = 1;
08504 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08505 continue;
08506 CheckValue<IkReal> x254=IKPowWithIntegerCheck(gconst82,-1);
08507 if(!x254.valid){
08508 continue;
08509 }
08510 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x254.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10))))+IKsqr((new_r11*(x254.value)))-1) <= IKFAST_SINCOS_THRESH )
08511     continue;
08512 j10array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82))))))))+((gconst82*new_r10))), (new_r11*(x254.value)));
08513 sj10array[0]=IKsin(j10array[0]);
08514 cj10array[0]=IKcos(j10array[0]);
08515 if( j10array[0] > IKPI )
08516 {
08517     j10array[0]-=IK2PI;
08518 }
08519 else if( j10array[0] < -IKPI )
08520 {    j10array[0]+=IK2PI;
08521 }
08522 j10valid[0] = true;
08523 for(int ij10 = 0; ij10 < 1; ++ij10)
08524 {
08525 if( !j10valid[ij10] )
08526 {
08527     continue;
08528 }
08529 _ij10[0] = ij10; _ij10[1] = -1;
08530 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08531 {
08532 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08533 {
08534     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08535 }
08536 }
08537 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08538 {
08539 IkReal evalcond[8];
08540 IkReal x255=IKcos(j10);
08541 IkReal x256=IKsin(j10);
08542 IkReal x257=((1.0)*x255);
08543 IkReal x258=((1.0)*x256);
08544 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08545 continue;
08546 IkReal x259=IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))));
08547 evalcond[0]=x256;
08548 evalcond[1]=((-1.0)*x255);
08549 evalcond[2]=((((-1.0)*gconst82*x257))+new_r11);
08550 evalcond[3]=((((-1.0)*gconst82*x258))+new_r10);
08551 evalcond[4]=((((-1.0)*x257*x259))+new_r01);
08552 evalcond[5]=(new_r00+(((-1.0)*x258*x259)));
08553 evalcond[6]=(((new_r00*x259))+((gconst82*new_r10))+(((-1.0)*x258)));
08554 evalcond[7]=(((new_r01*x259))+((gconst82*new_r11))+(((-1.0)*x257)));
08555 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08556 {
08557 continue;
08558 }
08559 }
08560 
08561 {
08562 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08563 vinfos[0].jointtype = 1;
08564 vinfos[0].foffset = j6;
08565 vinfos[0].indices[0] = _ij6[0];
08566 vinfos[0].indices[1] = _ij6[1];
08567 vinfos[0].maxsolutions = _nj6;
08568 vinfos[1].jointtype = 1;
08569 vinfos[1].foffset = j7;
08570 vinfos[1].indices[0] = _ij7[0];
08571 vinfos[1].indices[1] = _ij7[1];
08572 vinfos[1].maxsolutions = _nj7;
08573 vinfos[2].jointtype = 1;
08574 vinfos[2].foffset = j8;
08575 vinfos[2].indices[0] = _ij8[0];
08576 vinfos[2].indices[1] = _ij8[1];
08577 vinfos[2].maxsolutions = _nj8;
08578 vinfos[3].jointtype = 1;
08579 vinfos[3].foffset = j9;
08580 vinfos[3].indices[0] = _ij9[0];
08581 vinfos[3].indices[1] = _ij9[1];
08582 vinfos[3].maxsolutions = _nj9;
08583 vinfos[4].jointtype = 1;
08584 vinfos[4].foffset = j10;
08585 vinfos[4].indices[0] = _ij10[0];
08586 vinfos[4].indices[1] = _ij10[1];
08587 vinfos[4].maxsolutions = _nj10;
08588 vinfos[5].jointtype = 1;
08589 vinfos[5].foffset = j11;
08590 vinfos[5].indices[0] = _ij11[0];
08591 vinfos[5].indices[1] = _ij11[1];
08592 vinfos[5].maxsolutions = _nj11;
08593 vinfos[6].jointtype = 1;
08594 vinfos[6].foffset = j12;
08595 vinfos[6].indices[0] = _ij12[0];
08596 vinfos[6].indices[1] = _ij12[1];
08597 vinfos[6].maxsolutions = _nj12;
08598 std::vector<int> vfree(0);
08599 solutions.AddSolution(vinfos,vfree);
08600 }
08601 }
08602 }
08603 
08604 } else
08605 {
08606 {
08607 IkReal j10array[1], cj10array[1], sj10array[1];
08608 bool j10valid[1]={false};
08609 _nj10 = 1;
08610 CheckValue<IkReal> x260 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
08611 if(!x260.valid){
08612 continue;
08613 }
08614 CheckValue<IkReal> x261=IKPowWithIntegerCheck(IKsign(gconst82),-1);
08615 if(!x261.valid){
08616 continue;
08617 }
08618 j10array[0]=((-1.5707963267949)+(x260.value)+(((1.5707963267949)*(x261.value))));
08619 sj10array[0]=IKsin(j10array[0]);
08620 cj10array[0]=IKcos(j10array[0]);
08621 if( j10array[0] > IKPI )
08622 {
08623     j10array[0]-=IK2PI;
08624 }
08625 else if( j10array[0] < -IKPI )
08626 {    j10array[0]+=IK2PI;
08627 }
08628 j10valid[0] = true;
08629 for(int ij10 = 0; ij10 < 1; ++ij10)
08630 {
08631 if( !j10valid[ij10] )
08632 {
08633     continue;
08634 }
08635 _ij10[0] = ij10; _ij10[1] = -1;
08636 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08637 {
08638 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08639 {
08640     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08641 }
08642 }
08643 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08644 {
08645 IkReal evalcond[8];
08646 IkReal x262=IKcos(j10);
08647 IkReal x263=IKsin(j10);
08648 IkReal x264=((1.0)*x262);
08649 IkReal x265=((1.0)*x263);
08650 if((((1.0)+(((-1.0)*(gconst82*gconst82))))) < -0.00001)
08651 continue;
08652 IkReal x266=IKsqrt(((1.0)+(((-1.0)*(gconst82*gconst82)))));
08653 evalcond[0]=x263;
08654 evalcond[1]=((-1.0)*x262);
08655 evalcond[2]=(new_r11+(((-1.0)*gconst82*x264)));
08656 evalcond[3]=(new_r10+(((-1.0)*gconst82*x265)));
08657 evalcond[4]=(new_r01+(((-1.0)*x264*x266)));
08658 evalcond[5]=((((-1.0)*x265*x266))+new_r00);
08659 evalcond[6]=(((gconst82*new_r10))+((new_r00*x266))+(((-1.0)*x265)));
08660 evalcond[7]=(((new_r01*x266))+((gconst82*new_r11))+(((-1.0)*x264)));
08661 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08662 {
08663 continue;
08664 }
08665 }
08666 
08667 {
08668 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08669 vinfos[0].jointtype = 1;
08670 vinfos[0].foffset = j6;
08671 vinfos[0].indices[0] = _ij6[0];
08672 vinfos[0].indices[1] = _ij6[1];
08673 vinfos[0].maxsolutions = _nj6;
08674 vinfos[1].jointtype = 1;
08675 vinfos[1].foffset = j7;
08676 vinfos[1].indices[0] = _ij7[0];
08677 vinfos[1].indices[1] = _ij7[1];
08678 vinfos[1].maxsolutions = _nj7;
08679 vinfos[2].jointtype = 1;
08680 vinfos[2].foffset = j8;
08681 vinfos[2].indices[0] = _ij8[0];
08682 vinfos[2].indices[1] = _ij8[1];
08683 vinfos[2].maxsolutions = _nj8;
08684 vinfos[3].jointtype = 1;
08685 vinfos[3].foffset = j9;
08686 vinfos[3].indices[0] = _ij9[0];
08687 vinfos[3].indices[1] = _ij9[1];
08688 vinfos[3].maxsolutions = _nj9;
08689 vinfos[4].jointtype = 1;
08690 vinfos[4].foffset = j10;
08691 vinfos[4].indices[0] = _ij10[0];
08692 vinfos[4].indices[1] = _ij10[1];
08693 vinfos[4].maxsolutions = _nj10;
08694 vinfos[5].jointtype = 1;
08695 vinfos[5].foffset = j11;
08696 vinfos[5].indices[0] = _ij11[0];
08697 vinfos[5].indices[1] = _ij11[1];
08698 vinfos[5].maxsolutions = _nj11;
08699 vinfos[6].jointtype = 1;
08700 vinfos[6].foffset = j12;
08701 vinfos[6].indices[0] = _ij12[0];
08702 vinfos[6].indices[1] = _ij12[1];
08703 vinfos[6].maxsolutions = _nj12;
08704 std::vector<int> vfree(0);
08705 solutions.AddSolution(vinfos,vfree);
08706 }
08707 }
08708 }
08709 
08710 }
08711 
08712 }
08713 
08714 }
08715 } while(0);
08716 if( bgotonextstatement )
08717 {
08718 bool bgotonextstatement = true;
08719 do
08720 {
08721 CheckValue<IkReal> x267=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08722 if(!x267.valid){
08723 continue;
08724 }
08725 if((x267.value) < -0.00001)
08726 continue;
08727 IkReal gconst83=IKsqrt(x267.value);
08728 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj8+(((-1.0)*gconst83)))))+(IKabs(((-1.0)+(IKsign(sj8)))))), 6.28318530717959)));
08729 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08730 {
08731 bgotonextstatement=false;
08732 {
08733 IkReal j10eval[1];
08734 new_r02=0;
08735 new_r12=0;
08736 new_r20=0;
08737 new_r21=0;
08738 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
08739 continue;
08740 sj8=IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))));
08741 cj8=gconst83;
08742 if( (gconst83) < -1-IKFAST_SINCOS_THRESH || (gconst83) > 1+IKFAST_SINCOS_THRESH )
08743     continue;
08744 j8=IKacos(gconst83);
08745 CheckValue<IkReal> x268=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08746 if(!x268.valid){
08747 continue;
08748 }
08749 if((x268.value) < -0.00001)
08750 continue;
08751 IkReal gconst83=IKsqrt(x268.value);
08752 j10eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
08753 if( IKabs(j10eval[0]) < 0.0000010000000000  )
08754 {
08755 {
08756 IkReal j10array[1], cj10array[1], sj10array[1];
08757 bool j10valid[1]={false};
08758 _nj10 = 1;
08759 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
08760 continue;
08761 CheckValue<IkReal> x269=IKPowWithIntegerCheck(gconst83,-1);
08762 if(!x269.valid){
08763 continue;
08764 }
08765 if( IKabs((((gconst83*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x269.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst83*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83))))))))))+IKsqr((new_r11*(x269.value)))-1) <= IKFAST_SINCOS_THRESH )
08766     continue;
08767 j10array[0]=IKatan2((((gconst83*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83))))))))), (new_r11*(x269.value)));
08768 sj10array[0]=IKsin(j10array[0]);
08769 cj10array[0]=IKcos(j10array[0]);
08770 if( j10array[0] > IKPI )
08771 {
08772     j10array[0]-=IK2PI;
08773 }
08774 else if( j10array[0] < -IKPI )
08775 {    j10array[0]+=IK2PI;
08776 }
08777 j10valid[0] = true;
08778 for(int ij10 = 0; ij10 < 1; ++ij10)
08779 {
08780 if( !j10valid[ij10] )
08781 {
08782     continue;
08783 }
08784 _ij10[0] = ij10; _ij10[1] = -1;
08785 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08786 {
08787 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08788 {
08789     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08790 }
08791 }
08792 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08793 {
08794 IkReal evalcond[8];
08795 IkReal x270=IKcos(j10);
08796 IkReal x271=IKsin(j10);
08797 IkReal x272=((1.0)*x271);
08798 IkReal x273=((1.0)*x270);
08799 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
08800 continue;
08801 IkReal x274=IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))));
08802 IkReal x275=((1.0)*x274);
08803 evalcond[0]=x271;
08804 evalcond[1]=((-1.0)*x270);
08805 evalcond[2]=((((-1.0)*gconst83*x273))+new_r11);
08806 evalcond[3]=((((-1.0)*gconst83*x272))+new_r10);
08807 evalcond[4]=(((x270*x274))+new_r01);
08808 evalcond[5]=(((x271*x274))+new_r00);
08809 evalcond[6]=((((-1.0)*new_r00*x275))+((gconst83*new_r10))+(((-1.0)*x272)));
08810 evalcond[7]=((((-1.0)*new_r01*x275))+((gconst83*new_r11))+(((-1.0)*x273)));
08811 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08812 {
08813 continue;
08814 }
08815 }
08816 
08817 {
08818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08819 vinfos[0].jointtype = 1;
08820 vinfos[0].foffset = j6;
08821 vinfos[0].indices[0] = _ij6[0];
08822 vinfos[0].indices[1] = _ij6[1];
08823 vinfos[0].maxsolutions = _nj6;
08824 vinfos[1].jointtype = 1;
08825 vinfos[1].foffset = j7;
08826 vinfos[1].indices[0] = _ij7[0];
08827 vinfos[1].indices[1] = _ij7[1];
08828 vinfos[1].maxsolutions = _nj7;
08829 vinfos[2].jointtype = 1;
08830 vinfos[2].foffset = j8;
08831 vinfos[2].indices[0] = _ij8[0];
08832 vinfos[2].indices[1] = _ij8[1];
08833 vinfos[2].maxsolutions = _nj8;
08834 vinfos[3].jointtype = 1;
08835 vinfos[3].foffset = j9;
08836 vinfos[3].indices[0] = _ij9[0];
08837 vinfos[3].indices[1] = _ij9[1];
08838 vinfos[3].maxsolutions = _nj9;
08839 vinfos[4].jointtype = 1;
08840 vinfos[4].foffset = j10;
08841 vinfos[4].indices[0] = _ij10[0];
08842 vinfos[4].indices[1] = _ij10[1];
08843 vinfos[4].maxsolutions = _nj10;
08844 vinfos[5].jointtype = 1;
08845 vinfos[5].foffset = j11;
08846 vinfos[5].indices[0] = _ij11[0];
08847 vinfos[5].indices[1] = _ij11[1];
08848 vinfos[5].maxsolutions = _nj11;
08849 vinfos[6].jointtype = 1;
08850 vinfos[6].foffset = j12;
08851 vinfos[6].indices[0] = _ij12[0];
08852 vinfos[6].indices[1] = _ij12[1];
08853 vinfos[6].maxsolutions = _nj12;
08854 std::vector<int> vfree(0);
08855 solutions.AddSolution(vinfos,vfree);
08856 }
08857 }
08858 }
08859 
08860 } else
08861 {
08862 {
08863 IkReal j10array[1], cj10array[1], sj10array[1];
08864 bool j10valid[1]={false};
08865 _nj10 = 1;
08866 CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(gconst83),-1);
08867 if(!x276.valid){
08868 continue;
08869 }
08870 CheckValue<IkReal> x277 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
08871 if(!x277.valid){
08872 continue;
08873 }
08874 j10array[0]=((-1.5707963267949)+(((1.5707963267949)*(x276.value)))+(x277.value));
08875 sj10array[0]=IKsin(j10array[0]);
08876 cj10array[0]=IKcos(j10array[0]);
08877 if( j10array[0] > IKPI )
08878 {
08879     j10array[0]-=IK2PI;
08880 }
08881 else if( j10array[0] < -IKPI )
08882 {    j10array[0]+=IK2PI;
08883 }
08884 j10valid[0] = true;
08885 for(int ij10 = 0; ij10 < 1; ++ij10)
08886 {
08887 if( !j10valid[ij10] )
08888 {
08889     continue;
08890 }
08891 _ij10[0] = ij10; _ij10[1] = -1;
08892 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08893 {
08894 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08895 {
08896     j10valid[iij10]=false; _ij10[1] = iij10; break; 
08897 }
08898 }
08899 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08900 {
08901 IkReal evalcond[8];
08902 IkReal x278=IKcos(j10);
08903 IkReal x279=IKsin(j10);
08904 IkReal x280=((1.0)*x279);
08905 IkReal x281=((1.0)*x278);
08906 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
08907 continue;
08908 IkReal x282=IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))));
08909 IkReal x283=((1.0)*x282);
08910 evalcond[0]=x279;
08911 evalcond[1]=((-1.0)*x278);
08912 evalcond[2]=(new_r11+(((-1.0)*gconst83*x281)));
08913 evalcond[3]=(new_r10+(((-1.0)*gconst83*x280)));
08914 evalcond[4]=(((x278*x282))+new_r01);
08915 evalcond[5]=(((x279*x282))+new_r00);
08916 evalcond[6]=((((-1.0)*new_r00*x283))+((gconst83*new_r10))+(((-1.0)*x280)));
08917 evalcond[7]=(((gconst83*new_r11))+(((-1.0)*new_r01*x283))+(((-1.0)*x281)));
08918 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
08919 {
08920 continue;
08921 }
08922 }
08923 
08924 {
08925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08926 vinfos[0].jointtype = 1;
08927 vinfos[0].foffset = j6;
08928 vinfos[0].indices[0] = _ij6[0];
08929 vinfos[0].indices[1] = _ij6[1];
08930 vinfos[0].maxsolutions = _nj6;
08931 vinfos[1].jointtype = 1;
08932 vinfos[1].foffset = j7;
08933 vinfos[1].indices[0] = _ij7[0];
08934 vinfos[1].indices[1] = _ij7[1];
08935 vinfos[1].maxsolutions = _nj7;
08936 vinfos[2].jointtype = 1;
08937 vinfos[2].foffset = j8;
08938 vinfos[2].indices[0] = _ij8[0];
08939 vinfos[2].indices[1] = _ij8[1];
08940 vinfos[2].maxsolutions = _nj8;
08941 vinfos[3].jointtype = 1;
08942 vinfos[3].foffset = j9;
08943 vinfos[3].indices[0] = _ij9[0];
08944 vinfos[3].indices[1] = _ij9[1];
08945 vinfos[3].maxsolutions = _nj9;
08946 vinfos[4].jointtype = 1;
08947 vinfos[4].foffset = j10;
08948 vinfos[4].indices[0] = _ij10[0];
08949 vinfos[4].indices[1] = _ij10[1];
08950 vinfos[4].maxsolutions = _nj10;
08951 vinfos[5].jointtype = 1;
08952 vinfos[5].foffset = j11;
08953 vinfos[5].indices[0] = _ij11[0];
08954 vinfos[5].indices[1] = _ij11[1];
08955 vinfos[5].maxsolutions = _nj11;
08956 vinfos[6].jointtype = 1;
08957 vinfos[6].foffset = j12;
08958 vinfos[6].indices[0] = _ij12[0];
08959 vinfos[6].indices[1] = _ij12[1];
08960 vinfos[6].maxsolutions = _nj12;
08961 std::vector<int> vfree(0);
08962 solutions.AddSolution(vinfos,vfree);
08963 }
08964 }
08965 }
08966 
08967 }
08968 
08969 }
08970 
08971 }
08972 } while(0);
08973 if( bgotonextstatement )
08974 {
08975 bool bgotonextstatement = true;
08976 do
08977 {
08978 CheckValue<IkReal> x284=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
08979 if(!x284.valid){
08980 continue;
08981 }
08982 if((x284.value) < -0.00001)
08983 continue;
08984 IkReal gconst83=IKsqrt(x284.value);
08985 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj8+(((-1.0)*gconst83)))))+(IKabs(((1.0)+(IKsign(sj8)))))), 6.28318530717959)));
08986 if( IKabs(evalcond[0]) < 0.0000050000000000  )
08987 {
08988 bgotonextstatement=false;
08989 {
08990 IkReal j10eval[1];
08991 new_r02=0;
08992 new_r12=0;
08993 new_r20=0;
08994 new_r21=0;
08995 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
08996 continue;
08997 sj8=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))))));
08998 cj8=gconst83;
08999 if( (gconst83) < -1-IKFAST_SINCOS_THRESH || (gconst83) > 1+IKFAST_SINCOS_THRESH )
09000     continue;
09001 j8=((-1.0)*(IKacos(gconst83)));
09002 CheckValue<IkReal> x285=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
09003 if(!x285.valid){
09004 continue;
09005 }
09006 if((x285.value) < -0.00001)
09007 continue;
09008 IkReal gconst83=IKsqrt(x285.value);
09009 j10eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
09010 if( IKabs(j10eval[0]) < 0.0000010000000000  )
09011 {
09012 {
09013 IkReal j10array[1], cj10array[1], sj10array[1];
09014 bool j10valid[1]={false};
09015 _nj10 = 1;
09016 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
09017 continue;
09018 CheckValue<IkReal> x286=IKPowWithIntegerCheck(gconst83,-1);
09019 if(!x286.valid){
09020 continue;
09021 }
09022 if( IKabs((((gconst83*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x286.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst83*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83))))))))))+IKsqr((new_r11*(x286.value)))-1) <= IKFAST_SINCOS_THRESH )
09023     continue;
09024 j10array[0]=IKatan2((((gconst83*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83))))))))), (new_r11*(x286.value)));
09025 sj10array[0]=IKsin(j10array[0]);
09026 cj10array[0]=IKcos(j10array[0]);
09027 if( j10array[0] > IKPI )
09028 {
09029     j10array[0]-=IK2PI;
09030 }
09031 else if( j10array[0] < -IKPI )
09032 {    j10array[0]+=IK2PI;
09033 }
09034 j10valid[0] = true;
09035 for(int ij10 = 0; ij10 < 1; ++ij10)
09036 {
09037 if( !j10valid[ij10] )
09038 {
09039     continue;
09040 }
09041 _ij10[0] = ij10; _ij10[1] = -1;
09042 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09043 {
09044 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09045 {
09046     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09047 }
09048 }
09049 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09050 {
09051 IkReal evalcond[8];
09052 IkReal x287=IKcos(j10);
09053 IkReal x288=IKsin(j10);
09054 IkReal x289=((1.0)*x287);
09055 IkReal x290=((1.0)*x288);
09056 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
09057 continue;
09058 IkReal x291=IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))));
09059 evalcond[0]=x288;
09060 evalcond[1]=((-1.0)*x287);
09061 evalcond[2]=(new_r11+(((-1.0)*gconst83*x289)));
09062 evalcond[3]=((((-1.0)*gconst83*x290))+new_r10);
09063 evalcond[4]=((((-1.0)*x289*x291))+new_r01);
09064 evalcond[5]=((((-1.0)*x290*x291))+new_r00);
09065 evalcond[6]=((((-1.0)*x290))+((new_r00*x291))+((gconst83*new_r10)));
09066 evalcond[7]=(((new_r01*x291))+((gconst83*new_r11))+(((-1.0)*x289)));
09067 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
09068 {
09069 continue;
09070 }
09071 }
09072 
09073 {
09074 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09075 vinfos[0].jointtype = 1;
09076 vinfos[0].foffset = j6;
09077 vinfos[0].indices[0] = _ij6[0];
09078 vinfos[0].indices[1] = _ij6[1];
09079 vinfos[0].maxsolutions = _nj6;
09080 vinfos[1].jointtype = 1;
09081 vinfos[1].foffset = j7;
09082 vinfos[1].indices[0] = _ij7[0];
09083 vinfos[1].indices[1] = _ij7[1];
09084 vinfos[1].maxsolutions = _nj7;
09085 vinfos[2].jointtype = 1;
09086 vinfos[2].foffset = j8;
09087 vinfos[2].indices[0] = _ij8[0];
09088 vinfos[2].indices[1] = _ij8[1];
09089 vinfos[2].maxsolutions = _nj8;
09090 vinfos[3].jointtype = 1;
09091 vinfos[3].foffset = j9;
09092 vinfos[3].indices[0] = _ij9[0];
09093 vinfos[3].indices[1] = _ij9[1];
09094 vinfos[3].maxsolutions = _nj9;
09095 vinfos[4].jointtype = 1;
09096 vinfos[4].foffset = j10;
09097 vinfos[4].indices[0] = _ij10[0];
09098 vinfos[4].indices[1] = _ij10[1];
09099 vinfos[4].maxsolutions = _nj10;
09100 vinfos[5].jointtype = 1;
09101 vinfos[5].foffset = j11;
09102 vinfos[5].indices[0] = _ij11[0];
09103 vinfos[5].indices[1] = _ij11[1];
09104 vinfos[5].maxsolutions = _nj11;
09105 vinfos[6].jointtype = 1;
09106 vinfos[6].foffset = j12;
09107 vinfos[6].indices[0] = _ij12[0];
09108 vinfos[6].indices[1] = _ij12[1];
09109 vinfos[6].maxsolutions = _nj12;
09110 std::vector<int> vfree(0);
09111 solutions.AddSolution(vinfos,vfree);
09112 }
09113 }
09114 }
09115 
09116 } else
09117 {
09118 {
09119 IkReal j10array[1], cj10array[1], sj10array[1];
09120 bool j10valid[1]={false};
09121 _nj10 = 1;
09122 CheckValue<IkReal> x292=IKPowWithIntegerCheck(IKsign(gconst83),-1);
09123 if(!x292.valid){
09124 continue;
09125 }
09126 CheckValue<IkReal> x293 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
09127 if(!x293.valid){
09128 continue;
09129 }
09130 j10array[0]=((-1.5707963267949)+(((1.5707963267949)*(x292.value)))+(x293.value));
09131 sj10array[0]=IKsin(j10array[0]);
09132 cj10array[0]=IKcos(j10array[0]);
09133 if( j10array[0] > IKPI )
09134 {
09135     j10array[0]-=IK2PI;
09136 }
09137 else if( j10array[0] < -IKPI )
09138 {    j10array[0]+=IK2PI;
09139 }
09140 j10valid[0] = true;
09141 for(int ij10 = 0; ij10 < 1; ++ij10)
09142 {
09143 if( !j10valid[ij10] )
09144 {
09145     continue;
09146 }
09147 _ij10[0] = ij10; _ij10[1] = -1;
09148 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09149 {
09150 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09151 {
09152     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09153 }
09154 }
09155 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09156 {
09157 IkReal evalcond[8];
09158 IkReal x294=IKcos(j10);
09159 IkReal x295=IKsin(j10);
09160 IkReal x296=((1.0)*x294);
09161 IkReal x297=((1.0)*x295);
09162 if((((1.0)+(((-1.0)*(gconst83*gconst83))))) < -0.00001)
09163 continue;
09164 IkReal x298=IKsqrt(((1.0)+(((-1.0)*(gconst83*gconst83)))));
09165 evalcond[0]=x295;
09166 evalcond[1]=((-1.0)*x294);
09167 evalcond[2]=((((-1.0)*gconst83*x296))+new_r11);
09168 evalcond[3]=((((-1.0)*gconst83*x297))+new_r10);
09169 evalcond[4]=((((-1.0)*x296*x298))+new_r01);
09170 evalcond[5]=((((-1.0)*x297*x298))+new_r00);
09171 evalcond[6]=((((-1.0)*x297))+((new_r00*x298))+((gconst83*new_r10)));
09172 evalcond[7]=((((-1.0)*x296))+((new_r01*x298))+((gconst83*new_r11)));
09173 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
09174 {
09175 continue;
09176 }
09177 }
09178 
09179 {
09180 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09181 vinfos[0].jointtype = 1;
09182 vinfos[0].foffset = j6;
09183 vinfos[0].indices[0] = _ij6[0];
09184 vinfos[0].indices[1] = _ij6[1];
09185 vinfos[0].maxsolutions = _nj6;
09186 vinfos[1].jointtype = 1;
09187 vinfos[1].foffset = j7;
09188 vinfos[1].indices[0] = _ij7[0];
09189 vinfos[1].indices[1] = _ij7[1];
09190 vinfos[1].maxsolutions = _nj7;
09191 vinfos[2].jointtype = 1;
09192 vinfos[2].foffset = j8;
09193 vinfos[2].indices[0] = _ij8[0];
09194 vinfos[2].indices[1] = _ij8[1];
09195 vinfos[2].maxsolutions = _nj8;
09196 vinfos[3].jointtype = 1;
09197 vinfos[3].foffset = j9;
09198 vinfos[3].indices[0] = _ij9[0];
09199 vinfos[3].indices[1] = _ij9[1];
09200 vinfos[3].maxsolutions = _nj9;
09201 vinfos[4].jointtype = 1;
09202 vinfos[4].foffset = j10;
09203 vinfos[4].indices[0] = _ij10[0];
09204 vinfos[4].indices[1] = _ij10[1];
09205 vinfos[4].maxsolutions = _nj10;
09206 vinfos[5].jointtype = 1;
09207 vinfos[5].foffset = j11;
09208 vinfos[5].indices[0] = _ij11[0];
09209 vinfos[5].indices[1] = _ij11[1];
09210 vinfos[5].maxsolutions = _nj11;
09211 vinfos[6].jointtype = 1;
09212 vinfos[6].foffset = j12;
09213 vinfos[6].indices[0] = _ij12[0];
09214 vinfos[6].indices[1] = _ij12[1];
09215 vinfos[6].maxsolutions = _nj12;
09216 std::vector<int> vfree(0);
09217 solutions.AddSolution(vinfos,vfree);
09218 }
09219 }
09220 }
09221 
09222 }
09223 
09224 }
09225 
09226 }
09227 } while(0);
09228 if( bgotonextstatement )
09229 {
09230 bool bgotonextstatement = true;
09231 do
09232 {
09233 if( 1 )
09234 {
09235 bgotonextstatement=false;
09236 continue; // branch miss [j10]
09237 
09238 }
09239 } while(0);
09240 if( bgotonextstatement )
09241 {
09242 }
09243 }
09244 }
09245 }
09246 }
09247 }
09248 }
09249 }
09250 }
09251 }
09252 
09253 } else
09254 {
09255 {
09256 IkReal j10array[1], cj10array[1], sj10array[1];
09257 bool j10valid[1]={false};
09258 _nj10 = 1;
09259 IkReal x299=new_r22*new_r22;
09260 CheckValue<IkReal> x300=IKPowWithIntegerCheck((((cj8*x299))+(((-1.0)*cj8))),-1);
09261 if(!x300.valid){
09262 continue;
09263 }
09264 CheckValue<IkReal> x301=IKPowWithIntegerCheck(((((-1.0)*sj8))+((sj8*x299))),-1);
09265 if(!x301.valid){
09266 continue;
09267 }
09268 if( IKabs(((x300.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x301.value)*((((new_r10*new_r22))+new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x300.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))))+IKsqr(((x301.value)*((((new_r10*new_r22))+new_r01))))-1) <= IKFAST_SINCOS_THRESH )
09269     continue;
09270 j10array[0]=IKatan2(((x300.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))), ((x301.value)*((((new_r10*new_r22))+new_r01))));
09271 sj10array[0]=IKsin(j10array[0]);
09272 cj10array[0]=IKcos(j10array[0]);
09273 if( j10array[0] > IKPI )
09274 {
09275     j10array[0]-=IK2PI;
09276 }
09277 else if( j10array[0] < -IKPI )
09278 {    j10array[0]+=IK2PI;
09279 }
09280 j10valid[0] = true;
09281 for(int ij10 = 0; ij10 < 1; ++ij10)
09282 {
09283 if( !j10valid[ij10] )
09284 {
09285     continue;
09286 }
09287 _ij10[0] = ij10; _ij10[1] = -1;
09288 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09289 {
09290 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09291 {
09292     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09293 }
09294 }
09295 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09296 {
09297 IkReal evalcond[10];
09298 IkReal x302=IKsin(j10);
09299 IkReal x303=IKcos(j10);
09300 IkReal x304=(new_r22*sj8);
09301 IkReal x305=(cj8*new_r00);
09302 IkReal x306=((1.0)*sj8);
09303 IkReal x307=(cj8*new_r01);
09304 IkReal x308=((1.0)*x303);
09305 IkReal x309=(new_r22*x302);
09306 IkReal x310=((1.0)*x302);
09307 evalcond[0]=(((new_r11*sj8))+x309+x307);
09308 evalcond[1]=(((new_r11*x304))+((new_r22*x307))+x302);
09309 evalcond[2]=(((cj8*new_r10))+(((-1.0)*x310))+(((-1.0)*new_r00*x306)));
09310 evalcond[3]=(((cj8*new_r11))+(((-1.0)*new_r01*x306))+(((-1.0)*x308)));
09311 evalcond[4]=(((sj8*x303))+((cj8*x309))+new_r01);
09312 evalcond[5]=(((new_r10*sj8))+(((-1.0)*new_r22*x308))+x305);
09313 evalcond[6]=(((sj8*x302))+(((-1.0)*cj8*new_r22*x308))+new_r00);
09314 evalcond[7]=(((x302*x304))+(((-1.0)*cj8*x308))+new_r11);
09315 evalcond[8]=(((new_r10*x304))+((new_r22*x305))+(((-1.0)*x308)));
09316 evalcond[9]=((((-1.0)*x304*x308))+new_r10+(((-1.0)*cj8*x310)));
09317 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  )
09318 {
09319 continue;
09320 }
09321 }
09322 
09323 {
09324 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09325 vinfos[0].jointtype = 1;
09326 vinfos[0].foffset = j6;
09327 vinfos[0].indices[0] = _ij6[0];
09328 vinfos[0].indices[1] = _ij6[1];
09329 vinfos[0].maxsolutions = _nj6;
09330 vinfos[1].jointtype = 1;
09331 vinfos[1].foffset = j7;
09332 vinfos[1].indices[0] = _ij7[0];
09333 vinfos[1].indices[1] = _ij7[1];
09334 vinfos[1].maxsolutions = _nj7;
09335 vinfos[2].jointtype = 1;
09336 vinfos[2].foffset = j8;
09337 vinfos[2].indices[0] = _ij8[0];
09338 vinfos[2].indices[1] = _ij8[1];
09339 vinfos[2].maxsolutions = _nj8;
09340 vinfos[3].jointtype = 1;
09341 vinfos[3].foffset = j9;
09342 vinfos[3].indices[0] = _ij9[0];
09343 vinfos[3].indices[1] = _ij9[1];
09344 vinfos[3].maxsolutions = _nj9;
09345 vinfos[4].jointtype = 1;
09346 vinfos[4].foffset = j10;
09347 vinfos[4].indices[0] = _ij10[0];
09348 vinfos[4].indices[1] = _ij10[1];
09349 vinfos[4].maxsolutions = _nj10;
09350 vinfos[5].jointtype = 1;
09351 vinfos[5].foffset = j11;
09352 vinfos[5].indices[0] = _ij11[0];
09353 vinfos[5].indices[1] = _ij11[1];
09354 vinfos[5].maxsolutions = _nj11;
09355 vinfos[6].jointtype = 1;
09356 vinfos[6].foffset = j12;
09357 vinfos[6].indices[0] = _ij12[0];
09358 vinfos[6].indices[1] = _ij12[1];
09359 vinfos[6].maxsolutions = _nj12;
09360 std::vector<int> vfree(0);
09361 solutions.AddSolution(vinfos,vfree);
09362 }
09363 }
09364 }
09365 
09366 }
09367 
09368 }
09369 
09370 } else
09371 {
09372 {
09373 IkReal j10array[1], cj10array[1], sj10array[1];
09374 bool j10valid[1]={false};
09375 _nj10 = 1;
09376 IkReal x311=((1.0)*new_r01);
09377 CheckValue<IkReal> x312=IKPowWithIntegerCheck(new_r22,-1);
09378 if(!x312.valid){
09379 continue;
09380 }
09381 if( IKabs(((x312.value)*(((((-1.0)*new_r11*sj8))+(((-1.0)*cj8*x311)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj8*x311))+((cj8*new_r11)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x312.value)*(((((-1.0)*new_r11*sj8))+(((-1.0)*cj8*x311))))))+IKsqr(((((-1.0)*sj8*x311))+((cj8*new_r11))))-1) <= IKFAST_SINCOS_THRESH )
09382     continue;
09383 j10array[0]=IKatan2(((x312.value)*(((((-1.0)*new_r11*sj8))+(((-1.0)*cj8*x311))))), ((((-1.0)*sj8*x311))+((cj8*new_r11))));
09384 sj10array[0]=IKsin(j10array[0]);
09385 cj10array[0]=IKcos(j10array[0]);
09386 if( j10array[0] > IKPI )
09387 {
09388     j10array[0]-=IK2PI;
09389 }
09390 else if( j10array[0] < -IKPI )
09391 {    j10array[0]+=IK2PI;
09392 }
09393 j10valid[0] = true;
09394 for(int ij10 = 0; ij10 < 1; ++ij10)
09395 {
09396 if( !j10valid[ij10] )
09397 {
09398     continue;
09399 }
09400 _ij10[0] = ij10; _ij10[1] = -1;
09401 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09402 {
09403 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09404 {
09405     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09406 }
09407 }
09408 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09409 {
09410 IkReal evalcond[10];
09411 IkReal x313=IKsin(j10);
09412 IkReal x314=IKcos(j10);
09413 IkReal x315=(new_r22*sj8);
09414 IkReal x316=(cj8*new_r00);
09415 IkReal x317=((1.0)*sj8);
09416 IkReal x318=(cj8*new_r01);
09417 IkReal x319=((1.0)*x314);
09418 IkReal x320=(new_r22*x313);
09419 IkReal x321=((1.0)*x313);
09420 evalcond[0]=(((new_r11*sj8))+x320+x318);
09421 evalcond[1]=(x313+((new_r11*x315))+((new_r22*x318)));
09422 evalcond[2]=((((-1.0)*new_r00*x317))+(((-1.0)*x321))+((cj8*new_r10)));
09423 evalcond[3]=(((cj8*new_r11))+(((-1.0)*x319))+(((-1.0)*new_r01*x317)));
09424 evalcond[4]=(((cj8*x320))+((sj8*x314))+new_r01);
09425 evalcond[5]=(((new_r10*sj8))+(((-1.0)*new_r22*x319))+x316);
09426 evalcond[6]=(((sj8*x313))+new_r00+(((-1.0)*cj8*new_r22*x319)));
09427 evalcond[7]=(new_r11+(((-1.0)*cj8*x319))+((x313*x315)));
09428 evalcond[8]=((((-1.0)*x319))+((new_r10*x315))+((new_r22*x316)));
09429 evalcond[9]=((((-1.0)*cj8*x321))+(((-1.0)*x315*x319))+new_r10);
09430 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  )
09431 {
09432 continue;
09433 }
09434 }
09435 
09436 {
09437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09438 vinfos[0].jointtype = 1;
09439 vinfos[0].foffset = j6;
09440 vinfos[0].indices[0] = _ij6[0];
09441 vinfos[0].indices[1] = _ij6[1];
09442 vinfos[0].maxsolutions = _nj6;
09443 vinfos[1].jointtype = 1;
09444 vinfos[1].foffset = j7;
09445 vinfos[1].indices[0] = _ij7[0];
09446 vinfos[1].indices[1] = _ij7[1];
09447 vinfos[1].maxsolutions = _nj7;
09448 vinfos[2].jointtype = 1;
09449 vinfos[2].foffset = j8;
09450 vinfos[2].indices[0] = _ij8[0];
09451 vinfos[2].indices[1] = _ij8[1];
09452 vinfos[2].maxsolutions = _nj8;
09453 vinfos[3].jointtype = 1;
09454 vinfos[3].foffset = j9;
09455 vinfos[3].indices[0] = _ij9[0];
09456 vinfos[3].indices[1] = _ij9[1];
09457 vinfos[3].maxsolutions = _nj9;
09458 vinfos[4].jointtype = 1;
09459 vinfos[4].foffset = j10;
09460 vinfos[4].indices[0] = _ij10[0];
09461 vinfos[4].indices[1] = _ij10[1];
09462 vinfos[4].maxsolutions = _nj10;
09463 vinfos[5].jointtype = 1;
09464 vinfos[5].foffset = j11;
09465 vinfos[5].indices[0] = _ij11[0];
09466 vinfos[5].indices[1] = _ij11[1];
09467 vinfos[5].maxsolutions = _nj11;
09468 vinfos[6].jointtype = 1;
09469 vinfos[6].foffset = j12;
09470 vinfos[6].indices[0] = _ij12[0];
09471 vinfos[6].indices[1] = _ij12[1];
09472 vinfos[6].maxsolutions = _nj12;
09473 std::vector<int> vfree(0);
09474 solutions.AddSolution(vinfos,vfree);
09475 }
09476 }
09477 }
09478 
09479 }
09480 
09481 }
09482 
09483 } else
09484 {
09485 {
09486 IkReal j10array[1], cj10array[1], sj10array[1];
09487 bool j10valid[1]={false};
09488 _nj10 = 1;
09489 IkReal x322=cj8*cj8;
09490 IkReal x323=(cj8*new_r22);
09491 CheckValue<IkReal> x324=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x322*(new_r22*new_r22)))+x322)),-1);
09492 if(!x324.valid){
09493 continue;
09494 }
09495 CheckValue<IkReal> x325 = IKatan2WithCheck(IkReal((((new_r01*x323))+((new_r00*sj8)))),IkReal((((new_r01*sj8))+(((-1.0)*new_r00*x323)))),IKFAST_ATAN2_MAGTHRESH);
09496 if(!x325.valid){
09497 continue;
09498 }
09499 j10array[0]=((-1.5707963267949)+(((1.5707963267949)*(x324.value)))+(x325.value));
09500 sj10array[0]=IKsin(j10array[0]);
09501 cj10array[0]=IKcos(j10array[0]);
09502 if( j10array[0] > IKPI )
09503 {
09504     j10array[0]-=IK2PI;
09505 }
09506 else if( j10array[0] < -IKPI )
09507 {    j10array[0]+=IK2PI;
09508 }
09509 j10valid[0] = true;
09510 for(int ij10 = 0; ij10 < 1; ++ij10)
09511 {
09512 if( !j10valid[ij10] )
09513 {
09514     continue;
09515 }
09516 _ij10[0] = ij10; _ij10[1] = -1;
09517 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09518 {
09519 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09520 {
09521     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09522 }
09523 }
09524 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09525 {
09526 IkReal evalcond[10];
09527 IkReal x326=IKsin(j10);
09528 IkReal x327=IKcos(j10);
09529 IkReal x328=(new_r22*sj8);
09530 IkReal x329=(cj8*new_r00);
09531 IkReal x330=((1.0)*sj8);
09532 IkReal x331=(cj8*new_r01);
09533 IkReal x332=((1.0)*x327);
09534 IkReal x333=(new_r22*x326);
09535 IkReal x334=((1.0)*x326);
09536 evalcond[0]=(((new_r11*sj8))+x333+x331);
09537 evalcond[1]=(((new_r22*x331))+x326+((new_r11*x328)));
09538 evalcond[2]=(((cj8*new_r10))+(((-1.0)*new_r00*x330))+(((-1.0)*x334)));
09539 evalcond[3]=(((cj8*new_r11))+(((-1.0)*x332))+(((-1.0)*new_r01*x330)));
09540 evalcond[4]=(((cj8*x333))+((sj8*x327))+new_r01);
09541 evalcond[5]=((((-1.0)*new_r22*x332))+((new_r10*sj8))+x329);
09542 evalcond[6]=((((-1.0)*cj8*new_r22*x332))+((sj8*x326))+new_r00);
09543 evalcond[7]=(((x326*x328))+(((-1.0)*cj8*x332))+new_r11);
09544 evalcond[8]=(((new_r22*x329))+(((-1.0)*x332))+((new_r10*x328)));
09545 evalcond[9]=((((-1.0)*x328*x332))+(((-1.0)*cj8*x334))+new_r10);
09546 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  )
09547 {
09548 continue;
09549 }
09550 }
09551 
09552 {
09553 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09554 vinfos[0].jointtype = 1;
09555 vinfos[0].foffset = j6;
09556 vinfos[0].indices[0] = _ij6[0];
09557 vinfos[0].indices[1] = _ij6[1];
09558 vinfos[0].maxsolutions = _nj6;
09559 vinfos[1].jointtype = 1;
09560 vinfos[1].foffset = j7;
09561 vinfos[1].indices[0] = _ij7[0];
09562 vinfos[1].indices[1] = _ij7[1];
09563 vinfos[1].maxsolutions = _nj7;
09564 vinfos[2].jointtype = 1;
09565 vinfos[2].foffset = j8;
09566 vinfos[2].indices[0] = _ij8[0];
09567 vinfos[2].indices[1] = _ij8[1];
09568 vinfos[2].maxsolutions = _nj8;
09569 vinfos[3].jointtype = 1;
09570 vinfos[3].foffset = j9;
09571 vinfos[3].indices[0] = _ij9[0];
09572 vinfos[3].indices[1] = _ij9[1];
09573 vinfos[3].maxsolutions = _nj9;
09574 vinfos[4].jointtype = 1;
09575 vinfos[4].foffset = j10;
09576 vinfos[4].indices[0] = _ij10[0];
09577 vinfos[4].indices[1] = _ij10[1];
09578 vinfos[4].maxsolutions = _nj10;
09579 vinfos[5].jointtype = 1;
09580 vinfos[5].foffset = j11;
09581 vinfos[5].indices[0] = _ij11[0];
09582 vinfos[5].indices[1] = _ij11[1];
09583 vinfos[5].maxsolutions = _nj11;
09584 vinfos[6].jointtype = 1;
09585 vinfos[6].foffset = j12;
09586 vinfos[6].indices[0] = _ij12[0];
09587 vinfos[6].indices[1] = _ij12[1];
09588 vinfos[6].maxsolutions = _nj12;
09589 std::vector<int> vfree(0);
09590 solutions.AddSolution(vinfos,vfree);
09591 }
09592 }
09593 }
09594 
09595 }
09596 
09597 }
09598     }
09599 
09600 }
09601 
09602 }
09603 
09604 }
09605 } while(0);
09606 if( bgotonextstatement )
09607 {
09608 bool bgotonextstatement = true;
09609 do
09610 {
09611 if( 1 )
09612 {
09613 bgotonextstatement=false;
09614 continue; // branch miss [j8, j10]
09615 
09616 }
09617 } while(0);
09618 if( bgotonextstatement )
09619 {
09620 }
09621 }
09622 }
09623 }
09624 }
09625 
09626 } else
09627 {
09628 {
09629 IkReal j8array[1], cj8array[1], sj8array[1];
09630 bool j8valid[1]={false};
09631 _nj8 = 1;
09632 CheckValue<IkReal> x336=IKPowWithIntegerCheck(sj9,-1);
09633 if(!x336.valid){
09634 continue;
09635 }
09636 IkReal x335=x336.value;
09637 CheckValue<IkReal> x337=IKPowWithIntegerCheck(new_r12,-1);
09638 if(!x337.valid){
09639 continue;
09640 }
09641 if( IKabs((x335*(x337.value)*(((1.0)+(((-1.0)*(cj9*cj9)))+(((-1.0)*(new_r02*new_r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r02*x335)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x335*(x337.value)*(((1.0)+(((-1.0)*(cj9*cj9)))+(((-1.0)*(new_r02*new_r02)))))))+IKsqr((new_r02*x335))-1) <= IKFAST_SINCOS_THRESH )
09642     continue;
09643 j8array[0]=IKatan2((x335*(x337.value)*(((1.0)+(((-1.0)*(cj9*cj9)))+(((-1.0)*(new_r02*new_r02)))))), (new_r02*x335));
09644 sj8array[0]=IKsin(j8array[0]);
09645 cj8array[0]=IKcos(j8array[0]);
09646 if( j8array[0] > IKPI )
09647 {
09648     j8array[0]-=IK2PI;
09649 }
09650 else if( j8array[0] < -IKPI )
09651 {    j8array[0]+=IK2PI;
09652 }
09653 j8valid[0] = true;
09654 for(int ij8 = 0; ij8 < 1; ++ij8)
09655 {
09656 if( !j8valid[ij8] )
09657 {
09658     continue;
09659 }
09660 _ij8[0] = ij8; _ij8[1] = -1;
09661 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
09662 {
09663 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
09664 {
09665     j8valid[iij8]=false; _ij8[1] = iij8; break; 
09666 }
09667 }
09668 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
09669 {
09670 IkReal evalcond[8];
09671 IkReal x338=IKcos(j8);
09672 IkReal x339=IKsin(j8);
09673 IkReal x340=((1.0)*sj9);
09674 IkReal x341=((1.0)*x339);
09675 IkReal x342=(sj9*x338);
09676 IkReal x343=(new_r12*x339);
09677 IkReal x344=(new_r02*x338);
09678 IkReal x345=(sj9*x339);
09679 evalcond[0]=(new_r02+(((-1.0)*x338*x340)));
09680 evalcond[1]=(new_r12+(((-1.0)*x339*x340)));
09681 evalcond[2]=((((-1.0)*new_r02*x341))+((new_r12*x338)));
09682 evalcond[3]=(x343+x344+(((-1.0)*x340)));
09683 evalcond[4]=(((cj9*new_r20))+((new_r00*x342))+((new_r10*x345)));
09684 evalcond[5]=(((cj9*new_r21))+((new_r11*x345))+((new_r01*x342)));
09685 evalcond[6]=((-1.0)+((new_r02*x342))+((sj9*x343))+((cj9*new_r22)));
09686 evalcond[7]=(((cj9*x343))+((cj9*x344))+(((-1.0)*new_r22*x340)));
09687 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
09688 {
09689 continue;
09690 }
09691 }
09692 
09693 {
09694 IkReal j10eval[3];
09695 j10eval[0]=sj9;
09696 j10eval[1]=IKsign(sj9);
09697 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
09698 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
09699 {
09700 {
09701 IkReal j10eval[2];
09702 j10eval[0]=sj8;
09703 j10eval[1]=sj9;
09704 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  )
09705 {
09706 {
09707 IkReal j10eval[3];
09708 j10eval[0]=cj8;
09709 j10eval[1]=cj9;
09710 j10eval[2]=sj9;
09711 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
09712 {
09713 {
09714 IkReal evalcond[5];
09715 bool bgotonextstatement = true;
09716 do
09717 {
09718 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j8)))), 6.28318530717959)));
09719 evalcond[1]=new_r02;
09720 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
09721 {
09722 bgotonextstatement=false;
09723 {
09724 IkReal j10eval[3];
09725 sj8=1.0;
09726 cj8=0;
09727 j8=1.5707963267949;
09728 j10eval[0]=sj9;
09729 j10eval[1]=IKsign(sj9);
09730 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
09731 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
09732 {
09733 {
09734 IkReal j10eval[3];
09735 sj8=1.0;
09736 cj8=0;
09737 j8=1.5707963267949;
09738 j10eval[0]=cj9;
09739 j10eval[1]=IKsign(cj9);
09740 j10eval[2]=((IKabs(new_r11))+(IKabs(new_r10)));
09741 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
09742 {
09743 {
09744 IkReal j10eval[1];
09745 sj8=1.0;
09746 cj8=0;
09747 j8=1.5707963267949;
09748 j10eval[0]=sj9;
09749 if( IKabs(j10eval[0]) < 0.0000010000000000  )
09750 {
09751 {
09752 IkReal evalcond[4];
09753 bool bgotonextstatement = true;
09754 do
09755 {
09756 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
09757 evalcond[1]=new_r20;
09758 evalcond[2]=new_r12;
09759 evalcond[3]=new_r21;
09760 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
09761 {
09762 bgotonextstatement=false;
09763 {
09764 IkReal j10array[1], cj10array[1], sj10array[1];
09765 bool j10valid[1]={false};
09766 _nj10 = 1;
09767 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
09768     continue;
09769 j10array[0]=IKatan2(((-1.0)*new_r11), new_r10);
09770 sj10array[0]=IKsin(j10array[0]);
09771 cj10array[0]=IKcos(j10array[0]);
09772 if( j10array[0] > IKPI )
09773 {
09774     j10array[0]-=IK2PI;
09775 }
09776 else if( j10array[0] < -IKPI )
09777 {    j10array[0]+=IK2PI;
09778 }
09779 j10valid[0] = true;
09780 for(int ij10 = 0; ij10 < 1; ++ij10)
09781 {
09782 if( !j10valid[ij10] )
09783 {
09784     continue;
09785 }
09786 _ij10[0] = ij10; _ij10[1] = -1;
09787 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09788 {
09789 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09790 {
09791     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09792 }
09793 }
09794 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09795 {
09796 IkReal evalcond[4];
09797 IkReal x346=IKsin(j10);
09798 IkReal x347=((1.0)*(IKcos(j10)));
09799 evalcond[0]=(x346+new_r11);
09800 evalcond[1]=(new_r10+(((-1.0)*x347)));
09801 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x346)));
09802 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x347)));
09803 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
09804 {
09805 continue;
09806 }
09807 }
09808 
09809 {
09810 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09811 vinfos[0].jointtype = 1;
09812 vinfos[0].foffset = j6;
09813 vinfos[0].indices[0] = _ij6[0];
09814 vinfos[0].indices[1] = _ij6[1];
09815 vinfos[0].maxsolutions = _nj6;
09816 vinfos[1].jointtype = 1;
09817 vinfos[1].foffset = j7;
09818 vinfos[1].indices[0] = _ij7[0];
09819 vinfos[1].indices[1] = _ij7[1];
09820 vinfos[1].maxsolutions = _nj7;
09821 vinfos[2].jointtype = 1;
09822 vinfos[2].foffset = j8;
09823 vinfos[2].indices[0] = _ij8[0];
09824 vinfos[2].indices[1] = _ij8[1];
09825 vinfos[2].maxsolutions = _nj8;
09826 vinfos[3].jointtype = 1;
09827 vinfos[3].foffset = j9;
09828 vinfos[3].indices[0] = _ij9[0];
09829 vinfos[3].indices[1] = _ij9[1];
09830 vinfos[3].maxsolutions = _nj9;
09831 vinfos[4].jointtype = 1;
09832 vinfos[4].foffset = j10;
09833 vinfos[4].indices[0] = _ij10[0];
09834 vinfos[4].indices[1] = _ij10[1];
09835 vinfos[4].maxsolutions = _nj10;
09836 vinfos[5].jointtype = 1;
09837 vinfos[5].foffset = j11;
09838 vinfos[5].indices[0] = _ij11[0];
09839 vinfos[5].indices[1] = _ij11[1];
09840 vinfos[5].maxsolutions = _nj11;
09841 vinfos[6].jointtype = 1;
09842 vinfos[6].foffset = j12;
09843 vinfos[6].indices[0] = _ij12[0];
09844 vinfos[6].indices[1] = _ij12[1];
09845 vinfos[6].maxsolutions = _nj12;
09846 std::vector<int> vfree(0);
09847 solutions.AddSolution(vinfos,vfree);
09848 }
09849 }
09850 }
09851 
09852 }
09853 } while(0);
09854 if( bgotonextstatement )
09855 {
09856 bool bgotonextstatement = true;
09857 do
09858 {
09859 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
09860 evalcond[1]=new_r20;
09861 evalcond[2]=new_r12;
09862 evalcond[3]=new_r21;
09863 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
09864 {
09865 bgotonextstatement=false;
09866 {
09867 IkReal j10array[1], cj10array[1], sj10array[1];
09868 bool j10valid[1]={false};
09869 _nj10 = 1;
09870 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
09871     continue;
09872 j10array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
09873 sj10array[0]=IKsin(j10array[0]);
09874 cj10array[0]=IKcos(j10array[0]);
09875 if( j10array[0] > IKPI )
09876 {
09877     j10array[0]-=IK2PI;
09878 }
09879 else if( j10array[0] < -IKPI )
09880 {    j10array[0]+=IK2PI;
09881 }
09882 j10valid[0] = true;
09883 for(int ij10 = 0; ij10 < 1; ++ij10)
09884 {
09885 if( !j10valid[ij10] )
09886 {
09887     continue;
09888 }
09889 _ij10[0] = ij10; _ij10[1] = -1;
09890 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09891 {
09892 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09893 {
09894     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09895 }
09896 }
09897 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09898 {
09899 IkReal evalcond[4];
09900 IkReal x348=IKcos(j10);
09901 IkReal x349=((1.0)*(IKsin(j10)));
09902 evalcond[0]=(x348+new_r10);
09903 evalcond[1]=(new_r11+(((-1.0)*x349)));
09904 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x349)));
09905 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x348)));
09906 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
09907 {
09908 continue;
09909 }
09910 }
09911 
09912 {
09913 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09914 vinfos[0].jointtype = 1;
09915 vinfos[0].foffset = j6;
09916 vinfos[0].indices[0] = _ij6[0];
09917 vinfos[0].indices[1] = _ij6[1];
09918 vinfos[0].maxsolutions = _nj6;
09919 vinfos[1].jointtype = 1;
09920 vinfos[1].foffset = j7;
09921 vinfos[1].indices[0] = _ij7[0];
09922 vinfos[1].indices[1] = _ij7[1];
09923 vinfos[1].maxsolutions = _nj7;
09924 vinfos[2].jointtype = 1;
09925 vinfos[2].foffset = j8;
09926 vinfos[2].indices[0] = _ij8[0];
09927 vinfos[2].indices[1] = _ij8[1];
09928 vinfos[2].maxsolutions = _nj8;
09929 vinfos[3].jointtype = 1;
09930 vinfos[3].foffset = j9;
09931 vinfos[3].indices[0] = _ij9[0];
09932 vinfos[3].indices[1] = _ij9[1];
09933 vinfos[3].maxsolutions = _nj9;
09934 vinfos[4].jointtype = 1;
09935 vinfos[4].foffset = j10;
09936 vinfos[4].indices[0] = _ij10[0];
09937 vinfos[4].indices[1] = _ij10[1];
09938 vinfos[4].maxsolutions = _nj10;
09939 vinfos[5].jointtype = 1;
09940 vinfos[5].foffset = j11;
09941 vinfos[5].indices[0] = _ij11[0];
09942 vinfos[5].indices[1] = _ij11[1];
09943 vinfos[5].maxsolutions = _nj11;
09944 vinfos[6].jointtype = 1;
09945 vinfos[6].foffset = j12;
09946 vinfos[6].indices[0] = _ij12[0];
09947 vinfos[6].indices[1] = _ij12[1];
09948 vinfos[6].maxsolutions = _nj12;
09949 std::vector<int> vfree(0);
09950 solutions.AddSolution(vinfos,vfree);
09951 }
09952 }
09953 }
09954 
09955 }
09956 } while(0);
09957 if( bgotonextstatement )
09958 {
09959 bool bgotonextstatement = true;
09960 do
09961 {
09962 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
09963 evalcond[1]=new_r22;
09964 evalcond[2]=new_r11;
09965 evalcond[3]=new_r10;
09966 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
09967 {
09968 bgotonextstatement=false;
09969 {
09970 IkReal j10array[1], cj10array[1], sj10array[1];
09971 bool j10valid[1]={false};
09972 _nj10 = 1;
09973 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
09974     continue;
09975 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
09976 sj10array[0]=IKsin(j10array[0]);
09977 cj10array[0]=IKcos(j10array[0]);
09978 if( j10array[0] > IKPI )
09979 {
09980     j10array[0]-=IK2PI;
09981 }
09982 else if( j10array[0] < -IKPI )
09983 {    j10array[0]+=IK2PI;
09984 }
09985 j10valid[0] = true;
09986 for(int ij10 = 0; ij10 < 1; ++ij10)
09987 {
09988 if( !j10valid[ij10] )
09989 {
09990     continue;
09991 }
09992 _ij10[0] = ij10; _ij10[1] = -1;
09993 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09994 {
09995 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09996 {
09997     j10valid[iij10]=false; _ij10[1] = iij10; break; 
09998 }
09999 }
10000 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10001 {
10002 IkReal evalcond[4];
10003 IkReal x350=IKcos(j10);
10004 IkReal x351=((1.0)*(IKsin(j10)));
10005 evalcond[0]=(x350+new_r20);
10006 evalcond[1]=((((-1.0)*x351))+new_r21);
10007 evalcond[2]=((((-1.0)*x351))+(((-1.0)*new_r00)));
10008 evalcond[3]=((((-1.0)*x350))+(((-1.0)*new_r01)));
10009 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
10010 {
10011 continue;
10012 }
10013 }
10014 
10015 {
10016 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10017 vinfos[0].jointtype = 1;
10018 vinfos[0].foffset = j6;
10019 vinfos[0].indices[0] = _ij6[0];
10020 vinfos[0].indices[1] = _ij6[1];
10021 vinfos[0].maxsolutions = _nj6;
10022 vinfos[1].jointtype = 1;
10023 vinfos[1].foffset = j7;
10024 vinfos[1].indices[0] = _ij7[0];
10025 vinfos[1].indices[1] = _ij7[1];
10026 vinfos[1].maxsolutions = _nj7;
10027 vinfos[2].jointtype = 1;
10028 vinfos[2].foffset = j8;
10029 vinfos[2].indices[0] = _ij8[0];
10030 vinfos[2].indices[1] = _ij8[1];
10031 vinfos[2].maxsolutions = _nj8;
10032 vinfos[3].jointtype = 1;
10033 vinfos[3].foffset = j9;
10034 vinfos[3].indices[0] = _ij9[0];
10035 vinfos[3].indices[1] = _ij9[1];
10036 vinfos[3].maxsolutions = _nj9;
10037 vinfos[4].jointtype = 1;
10038 vinfos[4].foffset = j10;
10039 vinfos[4].indices[0] = _ij10[0];
10040 vinfos[4].indices[1] = _ij10[1];
10041 vinfos[4].maxsolutions = _nj10;
10042 vinfos[5].jointtype = 1;
10043 vinfos[5].foffset = j11;
10044 vinfos[5].indices[0] = _ij11[0];
10045 vinfos[5].indices[1] = _ij11[1];
10046 vinfos[5].maxsolutions = _nj11;
10047 vinfos[6].jointtype = 1;
10048 vinfos[6].foffset = j12;
10049 vinfos[6].indices[0] = _ij12[0];
10050 vinfos[6].indices[1] = _ij12[1];
10051 vinfos[6].maxsolutions = _nj12;
10052 std::vector<int> vfree(0);
10053 solutions.AddSolution(vinfos,vfree);
10054 }
10055 }
10056 }
10057 
10058 }
10059 } while(0);
10060 if( bgotonextstatement )
10061 {
10062 bool bgotonextstatement = true;
10063 do
10064 {
10065 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
10066 evalcond[1]=new_r22;
10067 evalcond[2]=new_r11;
10068 evalcond[3]=new_r10;
10069 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
10070 {
10071 bgotonextstatement=false;
10072 {
10073 IkReal j10array[1], cj10array[1], sj10array[1];
10074 bool j10valid[1]={false};
10075 _nj10 = 1;
10076 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
10077     continue;
10078 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
10079 sj10array[0]=IKsin(j10array[0]);
10080 cj10array[0]=IKcos(j10array[0]);
10081 if( j10array[0] > IKPI )
10082 {
10083     j10array[0]-=IK2PI;
10084 }
10085 else if( j10array[0] < -IKPI )
10086 {    j10array[0]+=IK2PI;
10087 }
10088 j10valid[0] = true;
10089 for(int ij10 = 0; ij10 < 1; ++ij10)
10090 {
10091 if( !j10valid[ij10] )
10092 {
10093     continue;
10094 }
10095 _ij10[0] = ij10; _ij10[1] = -1;
10096 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10097 {
10098 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10099 {
10100     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10101 }
10102 }
10103 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10104 {
10105 IkReal evalcond[4];
10106 IkReal x352=IKsin(j10);
10107 IkReal x353=((1.0)*(IKcos(j10)));
10108 evalcond[0]=(x352+new_r21);
10109 evalcond[1]=((((-1.0)*x353))+new_r20);
10110 evalcond[2]=((((-1.0)*x352))+(((-1.0)*new_r00)));
10111 evalcond[3]=((((-1.0)*x353))+(((-1.0)*new_r01)));
10112 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
10113 {
10114 continue;
10115 }
10116 }
10117 
10118 {
10119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10120 vinfos[0].jointtype = 1;
10121 vinfos[0].foffset = j6;
10122 vinfos[0].indices[0] = _ij6[0];
10123 vinfos[0].indices[1] = _ij6[1];
10124 vinfos[0].maxsolutions = _nj6;
10125 vinfos[1].jointtype = 1;
10126 vinfos[1].foffset = j7;
10127 vinfos[1].indices[0] = _ij7[0];
10128 vinfos[1].indices[1] = _ij7[1];
10129 vinfos[1].maxsolutions = _nj7;
10130 vinfos[2].jointtype = 1;
10131 vinfos[2].foffset = j8;
10132 vinfos[2].indices[0] = _ij8[0];
10133 vinfos[2].indices[1] = _ij8[1];
10134 vinfos[2].maxsolutions = _nj8;
10135 vinfos[3].jointtype = 1;
10136 vinfos[3].foffset = j9;
10137 vinfos[3].indices[0] = _ij9[0];
10138 vinfos[3].indices[1] = _ij9[1];
10139 vinfos[3].maxsolutions = _nj9;
10140 vinfos[4].jointtype = 1;
10141 vinfos[4].foffset = j10;
10142 vinfos[4].indices[0] = _ij10[0];
10143 vinfos[4].indices[1] = _ij10[1];
10144 vinfos[4].maxsolutions = _nj10;
10145 vinfos[5].jointtype = 1;
10146 vinfos[5].foffset = j11;
10147 vinfos[5].indices[0] = _ij11[0];
10148 vinfos[5].indices[1] = _ij11[1];
10149 vinfos[5].maxsolutions = _nj11;
10150 vinfos[6].jointtype = 1;
10151 vinfos[6].foffset = j12;
10152 vinfos[6].indices[0] = _ij12[0];
10153 vinfos[6].indices[1] = _ij12[1];
10154 vinfos[6].maxsolutions = _nj12;
10155 std::vector<int> vfree(0);
10156 solutions.AddSolution(vinfos,vfree);
10157 }
10158 }
10159 }
10160 
10161 }
10162 } while(0);
10163 if( bgotonextstatement )
10164 {
10165 bool bgotonextstatement = true;
10166 do
10167 {
10168 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
10169 if( IKabs(evalcond[0]) < 0.0000050000000000  )
10170 {
10171 bgotonextstatement=false;
10172 {
10173 IkReal j10array[1], cj10array[1], sj10array[1];
10174 bool j10valid[1]={false};
10175 _nj10 = 1;
10176 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
10177     continue;
10178 j10array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
10179 sj10array[0]=IKsin(j10array[0]);
10180 cj10array[0]=IKcos(j10array[0]);
10181 if( j10array[0] > IKPI )
10182 {
10183     j10array[0]-=IK2PI;
10184 }
10185 else if( j10array[0] < -IKPI )
10186 {    j10array[0]+=IK2PI;
10187 }
10188 j10valid[0] = true;
10189 for(int ij10 = 0; ij10 < 1; ++ij10)
10190 {
10191 if( !j10valid[ij10] )
10192 {
10193     continue;
10194 }
10195 _ij10[0] = ij10; _ij10[1] = -1;
10196 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10197 {
10198 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10199 {
10200     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10201 }
10202 }
10203 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10204 {
10205 IkReal evalcond[6];
10206 IkReal x354=IKsin(j10);
10207 IkReal x355=IKcos(j10);
10208 IkReal x356=((-1.0)*x355);
10209 evalcond[0]=x354;
10210 evalcond[1]=(new_r22*x354);
10211 evalcond[2]=x356;
10212 evalcond[3]=(new_r22*x356);
10213 evalcond[4]=((((-1.0)*x354))+(((-1.0)*new_r00)));
10214 evalcond[5]=((((-1.0)*x355))+(((-1.0)*new_r01)));
10215 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
10216 {
10217 continue;
10218 }
10219 }
10220 
10221 {
10222 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10223 vinfos[0].jointtype = 1;
10224 vinfos[0].foffset = j6;
10225 vinfos[0].indices[0] = _ij6[0];
10226 vinfos[0].indices[1] = _ij6[1];
10227 vinfos[0].maxsolutions = _nj6;
10228 vinfos[1].jointtype = 1;
10229 vinfos[1].foffset = j7;
10230 vinfos[1].indices[0] = _ij7[0];
10231 vinfos[1].indices[1] = _ij7[1];
10232 vinfos[1].maxsolutions = _nj7;
10233 vinfos[2].jointtype = 1;
10234 vinfos[2].foffset = j8;
10235 vinfos[2].indices[0] = _ij8[0];
10236 vinfos[2].indices[1] = _ij8[1];
10237 vinfos[2].maxsolutions = _nj8;
10238 vinfos[3].jointtype = 1;
10239 vinfos[3].foffset = j9;
10240 vinfos[3].indices[0] = _ij9[0];
10241 vinfos[3].indices[1] = _ij9[1];
10242 vinfos[3].maxsolutions = _nj9;
10243 vinfos[4].jointtype = 1;
10244 vinfos[4].foffset = j10;
10245 vinfos[4].indices[0] = _ij10[0];
10246 vinfos[4].indices[1] = _ij10[1];
10247 vinfos[4].maxsolutions = _nj10;
10248 vinfos[5].jointtype = 1;
10249 vinfos[5].foffset = j11;
10250 vinfos[5].indices[0] = _ij11[0];
10251 vinfos[5].indices[1] = _ij11[1];
10252 vinfos[5].maxsolutions = _nj11;
10253 vinfos[6].jointtype = 1;
10254 vinfos[6].foffset = j12;
10255 vinfos[6].indices[0] = _ij12[0];
10256 vinfos[6].indices[1] = _ij12[1];
10257 vinfos[6].maxsolutions = _nj12;
10258 std::vector<int> vfree(0);
10259 solutions.AddSolution(vinfos,vfree);
10260 }
10261 }
10262 }
10263 
10264 }
10265 } while(0);
10266 if( bgotonextstatement )
10267 {
10268 bool bgotonextstatement = true;
10269 do
10270 {
10271 if( 1 )
10272 {
10273 bgotonextstatement=false;
10274 continue; // branch miss [j10]
10275 
10276 }
10277 } while(0);
10278 if( bgotonextstatement )
10279 {
10280 }
10281 }
10282 }
10283 }
10284 }
10285 }
10286 }
10287 
10288 } else
10289 {
10290 {
10291 IkReal j10array[1], cj10array[1], sj10array[1];
10292 bool j10valid[1]={false};
10293 _nj10 = 1;
10294 CheckValue<IkReal> x357=IKPowWithIntegerCheck(sj9,-1);
10295 if(!x357.valid){
10296 continue;
10297 }
10298 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x357.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r20*(x357.value)))-1) <= IKFAST_SINCOS_THRESH )
10299     continue;
10300 j10array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x357.value)));
10301 sj10array[0]=IKsin(j10array[0]);
10302 cj10array[0]=IKcos(j10array[0]);
10303 if( j10array[0] > IKPI )
10304 {
10305     j10array[0]-=IK2PI;
10306 }
10307 else if( j10array[0] < -IKPI )
10308 {    j10array[0]+=IK2PI;
10309 }
10310 j10valid[0] = true;
10311 for(int ij10 = 0; ij10 < 1; ++ij10)
10312 {
10313 if( !j10valid[ij10] )
10314 {
10315     continue;
10316 }
10317 _ij10[0] = ij10; _ij10[1] = -1;
10318 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10319 {
10320 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10321 {
10322     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10323 }
10324 }
10325 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10326 {
10327 IkReal evalcond[8];
10328 IkReal x358=IKsin(j10);
10329 IkReal x359=IKcos(j10);
10330 IkReal x360=((1.0)*sj9);
10331 IkReal x361=((1.0)*x359);
10332 evalcond[0]=(((sj9*x359))+new_r20);
10333 evalcond[1]=(((cj9*x358))+new_r11);
10334 evalcond[2]=((((-1.0)*x358*x360))+new_r21);
10335 evalcond[3]=((((-1.0)*cj9*x361))+new_r10);
10336 evalcond[4]=((((-1.0)*x358))+(((-1.0)*new_r00)));
10337 evalcond[5]=((((-1.0)*x361))+(((-1.0)*new_r01)));
10338 evalcond[6]=(((cj9*new_r11))+x358+(((-1.0)*new_r21*x360)));
10339 evalcond[7]=((((-1.0)*new_r20*x360))+(((-1.0)*x361))+((cj9*new_r10)));
10340 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10341 {
10342 continue;
10343 }
10344 }
10345 
10346 {
10347 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10348 vinfos[0].jointtype = 1;
10349 vinfos[0].foffset = j6;
10350 vinfos[0].indices[0] = _ij6[0];
10351 vinfos[0].indices[1] = _ij6[1];
10352 vinfos[0].maxsolutions = _nj6;
10353 vinfos[1].jointtype = 1;
10354 vinfos[1].foffset = j7;
10355 vinfos[1].indices[0] = _ij7[0];
10356 vinfos[1].indices[1] = _ij7[1];
10357 vinfos[1].maxsolutions = _nj7;
10358 vinfos[2].jointtype = 1;
10359 vinfos[2].foffset = j8;
10360 vinfos[2].indices[0] = _ij8[0];
10361 vinfos[2].indices[1] = _ij8[1];
10362 vinfos[2].maxsolutions = _nj8;
10363 vinfos[3].jointtype = 1;
10364 vinfos[3].foffset = j9;
10365 vinfos[3].indices[0] = _ij9[0];
10366 vinfos[3].indices[1] = _ij9[1];
10367 vinfos[3].maxsolutions = _nj9;
10368 vinfos[4].jointtype = 1;
10369 vinfos[4].foffset = j10;
10370 vinfos[4].indices[0] = _ij10[0];
10371 vinfos[4].indices[1] = _ij10[1];
10372 vinfos[4].maxsolutions = _nj10;
10373 vinfos[5].jointtype = 1;
10374 vinfos[5].foffset = j11;
10375 vinfos[5].indices[0] = _ij11[0];
10376 vinfos[5].indices[1] = _ij11[1];
10377 vinfos[5].maxsolutions = _nj11;
10378 vinfos[6].jointtype = 1;
10379 vinfos[6].foffset = j12;
10380 vinfos[6].indices[0] = _ij12[0];
10381 vinfos[6].indices[1] = _ij12[1];
10382 vinfos[6].maxsolutions = _nj12;
10383 std::vector<int> vfree(0);
10384 solutions.AddSolution(vinfos,vfree);
10385 }
10386 }
10387 }
10388 
10389 }
10390 
10391 }
10392 
10393 } else
10394 {
10395 {
10396 IkReal j10array[1], cj10array[1], sj10array[1];
10397 bool j10valid[1]={false};
10398 _nj10 = 1;
10399 CheckValue<IkReal> x362=IKPowWithIntegerCheck(IKsign(cj9),-1);
10400 if(!x362.valid){
10401 continue;
10402 }
10403 CheckValue<IkReal> x363 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
10404 if(!x363.valid){
10405 continue;
10406 }
10407 j10array[0]=((-1.5707963267949)+(((1.5707963267949)*(x362.value)))+(x363.value));
10408 sj10array[0]=IKsin(j10array[0]);
10409 cj10array[0]=IKcos(j10array[0]);
10410 if( j10array[0] > IKPI )
10411 {
10412     j10array[0]-=IK2PI;
10413 }
10414 else if( j10array[0] < -IKPI )
10415 {    j10array[0]+=IK2PI;
10416 }
10417 j10valid[0] = true;
10418 for(int ij10 = 0; ij10 < 1; ++ij10)
10419 {
10420 if( !j10valid[ij10] )
10421 {
10422     continue;
10423 }
10424 _ij10[0] = ij10; _ij10[1] = -1;
10425 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10426 {
10427 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10428 {
10429     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10430 }
10431 }
10432 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10433 {
10434 IkReal evalcond[8];
10435 IkReal x364=IKsin(j10);
10436 IkReal x365=IKcos(j10);
10437 IkReal x366=((1.0)*sj9);
10438 IkReal x367=((1.0)*x365);
10439 evalcond[0]=(((sj9*x365))+new_r20);
10440 evalcond[1]=(((cj9*x364))+new_r11);
10441 evalcond[2]=((((-1.0)*x364*x366))+new_r21);
10442 evalcond[3]=((((-1.0)*cj9*x367))+new_r10);
10443 evalcond[4]=((((-1.0)*x364))+(((-1.0)*new_r00)));
10444 evalcond[5]=((((-1.0)*x367))+(((-1.0)*new_r01)));
10445 evalcond[6]=(((cj9*new_r11))+x364+(((-1.0)*new_r21*x366)));
10446 evalcond[7]=((((-1.0)*new_r20*x366))+(((-1.0)*x367))+((cj9*new_r10)));
10447 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10448 {
10449 continue;
10450 }
10451 }
10452 
10453 {
10454 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10455 vinfos[0].jointtype = 1;
10456 vinfos[0].foffset = j6;
10457 vinfos[0].indices[0] = _ij6[0];
10458 vinfos[0].indices[1] = _ij6[1];
10459 vinfos[0].maxsolutions = _nj6;
10460 vinfos[1].jointtype = 1;
10461 vinfos[1].foffset = j7;
10462 vinfos[1].indices[0] = _ij7[0];
10463 vinfos[1].indices[1] = _ij7[1];
10464 vinfos[1].maxsolutions = _nj7;
10465 vinfos[2].jointtype = 1;
10466 vinfos[2].foffset = j8;
10467 vinfos[2].indices[0] = _ij8[0];
10468 vinfos[2].indices[1] = _ij8[1];
10469 vinfos[2].maxsolutions = _nj8;
10470 vinfos[3].jointtype = 1;
10471 vinfos[3].foffset = j9;
10472 vinfos[3].indices[0] = _ij9[0];
10473 vinfos[3].indices[1] = _ij9[1];
10474 vinfos[3].maxsolutions = _nj9;
10475 vinfos[4].jointtype = 1;
10476 vinfos[4].foffset = j10;
10477 vinfos[4].indices[0] = _ij10[0];
10478 vinfos[4].indices[1] = _ij10[1];
10479 vinfos[4].maxsolutions = _nj10;
10480 vinfos[5].jointtype = 1;
10481 vinfos[5].foffset = j11;
10482 vinfos[5].indices[0] = _ij11[0];
10483 vinfos[5].indices[1] = _ij11[1];
10484 vinfos[5].maxsolutions = _nj11;
10485 vinfos[6].jointtype = 1;
10486 vinfos[6].foffset = j12;
10487 vinfos[6].indices[0] = _ij12[0];
10488 vinfos[6].indices[1] = _ij12[1];
10489 vinfos[6].maxsolutions = _nj12;
10490 std::vector<int> vfree(0);
10491 solutions.AddSolution(vinfos,vfree);
10492 }
10493 }
10494 }
10495 
10496 }
10497 
10498 }
10499 
10500 } else
10501 {
10502 {
10503 IkReal j10array[1], cj10array[1], sj10array[1];
10504 bool j10valid[1]={false};
10505 _nj10 = 1;
10506 CheckValue<IkReal> x368 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
10507 if(!x368.valid){
10508 continue;
10509 }
10510 CheckValue<IkReal> x369=IKPowWithIntegerCheck(IKsign(sj9),-1);
10511 if(!x369.valid){
10512 continue;
10513 }
10514 j10array[0]=((-1.5707963267949)+(x368.value)+(((1.5707963267949)*(x369.value))));
10515 sj10array[0]=IKsin(j10array[0]);
10516 cj10array[0]=IKcos(j10array[0]);
10517 if( j10array[0] > IKPI )
10518 {
10519     j10array[0]-=IK2PI;
10520 }
10521 else if( j10array[0] < -IKPI )
10522 {    j10array[0]+=IK2PI;
10523 }
10524 j10valid[0] = true;
10525 for(int ij10 = 0; ij10 < 1; ++ij10)
10526 {
10527 if( !j10valid[ij10] )
10528 {
10529     continue;
10530 }
10531 _ij10[0] = ij10; _ij10[1] = -1;
10532 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10533 {
10534 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10535 {
10536     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10537 }
10538 }
10539 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10540 {
10541 IkReal evalcond[8];
10542 IkReal x370=IKsin(j10);
10543 IkReal x371=IKcos(j10);
10544 IkReal x372=((1.0)*sj9);
10545 IkReal x373=((1.0)*x371);
10546 evalcond[0]=(((sj9*x371))+new_r20);
10547 evalcond[1]=(((cj9*x370))+new_r11);
10548 evalcond[2]=((((-1.0)*x370*x372))+new_r21);
10549 evalcond[3]=((((-1.0)*cj9*x373))+new_r10);
10550 evalcond[4]=((((-1.0)*x370))+(((-1.0)*new_r00)));
10551 evalcond[5]=((((-1.0)*x373))+(((-1.0)*new_r01)));
10552 evalcond[6]=((((-1.0)*new_r21*x372))+((cj9*new_r11))+x370);
10553 evalcond[7]=((((-1.0)*new_r20*x372))+(((-1.0)*x373))+((cj9*new_r10)));
10554 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10555 {
10556 continue;
10557 }
10558 }
10559 
10560 {
10561 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10562 vinfos[0].jointtype = 1;
10563 vinfos[0].foffset = j6;
10564 vinfos[0].indices[0] = _ij6[0];
10565 vinfos[0].indices[1] = _ij6[1];
10566 vinfos[0].maxsolutions = _nj6;
10567 vinfos[1].jointtype = 1;
10568 vinfos[1].foffset = j7;
10569 vinfos[1].indices[0] = _ij7[0];
10570 vinfos[1].indices[1] = _ij7[1];
10571 vinfos[1].maxsolutions = _nj7;
10572 vinfos[2].jointtype = 1;
10573 vinfos[2].foffset = j8;
10574 vinfos[2].indices[0] = _ij8[0];
10575 vinfos[2].indices[1] = _ij8[1];
10576 vinfos[2].maxsolutions = _nj8;
10577 vinfos[3].jointtype = 1;
10578 vinfos[3].foffset = j9;
10579 vinfos[3].indices[0] = _ij9[0];
10580 vinfos[3].indices[1] = _ij9[1];
10581 vinfos[3].maxsolutions = _nj9;
10582 vinfos[4].jointtype = 1;
10583 vinfos[4].foffset = j10;
10584 vinfos[4].indices[0] = _ij10[0];
10585 vinfos[4].indices[1] = _ij10[1];
10586 vinfos[4].maxsolutions = _nj10;
10587 vinfos[5].jointtype = 1;
10588 vinfos[5].foffset = j11;
10589 vinfos[5].indices[0] = _ij11[0];
10590 vinfos[5].indices[1] = _ij11[1];
10591 vinfos[5].maxsolutions = _nj11;
10592 vinfos[6].jointtype = 1;
10593 vinfos[6].foffset = j12;
10594 vinfos[6].indices[0] = _ij12[0];
10595 vinfos[6].indices[1] = _ij12[1];
10596 vinfos[6].maxsolutions = _nj12;
10597 std::vector<int> vfree(0);
10598 solutions.AddSolution(vinfos,vfree);
10599 }
10600 }
10601 }
10602 
10603 }
10604 
10605 }
10606 
10607 }
10608 } while(0);
10609 if( bgotonextstatement )
10610 {
10611 bool bgotonextstatement = true;
10612 do
10613 {
10614 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j8)))), 6.28318530717959)));
10615 evalcond[1]=new_r02;
10616 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
10617 {
10618 bgotonextstatement=false;
10619 {
10620 IkReal j10array[1], cj10array[1], sj10array[1];
10621 bool j10valid[1]={false};
10622 _nj10 = 1;
10623 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
10624     continue;
10625 j10array[0]=IKatan2(new_r00, new_r01);
10626 sj10array[0]=IKsin(j10array[0]);
10627 cj10array[0]=IKcos(j10array[0]);
10628 if( j10array[0] > IKPI )
10629 {
10630     j10array[0]-=IK2PI;
10631 }
10632 else if( j10array[0] < -IKPI )
10633 {    j10array[0]+=IK2PI;
10634 }
10635 j10valid[0] = true;
10636 for(int ij10 = 0; ij10 < 1; ++ij10)
10637 {
10638 if( !j10valid[ij10] )
10639 {
10640     continue;
10641 }
10642 _ij10[0] = ij10; _ij10[1] = -1;
10643 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10644 {
10645 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10646 {
10647     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10648 }
10649 }
10650 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10651 {
10652 IkReal evalcond[8];
10653 IkReal x374=IKcos(j10);
10654 IkReal x375=IKsin(j10);
10655 IkReal x376=((1.0)*sj9);
10656 IkReal x377=((1.0)*new_r10);
10657 IkReal x378=((1.0)*new_r11);
10658 IkReal x379=((1.0)*x374);
10659 evalcond[0]=(((sj9*x374))+new_r20);
10660 evalcond[1]=((((-1.0)*x375))+new_r00);
10661 evalcond[2]=((((-1.0)*x379))+new_r01);
10662 evalcond[3]=(new_r21+(((-1.0)*x375*x376)));
10663 evalcond[4]=((((-1.0)*x378))+((cj9*x375)));
10664 evalcond[5]=((((-1.0)*x377))+(((-1.0)*cj9*x379)));
10665 evalcond[6]=((((-1.0)*new_r21*x376))+(((-1.0)*cj9*x378))+x375);
10666 evalcond[7]=((((-1.0)*new_r20*x376))+(((-1.0)*x379))+(((-1.0)*cj9*x377)));
10667 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10668 {
10669 continue;
10670 }
10671 }
10672 
10673 {
10674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10675 vinfos[0].jointtype = 1;
10676 vinfos[0].foffset = j6;
10677 vinfos[0].indices[0] = _ij6[0];
10678 vinfos[0].indices[1] = _ij6[1];
10679 vinfos[0].maxsolutions = _nj6;
10680 vinfos[1].jointtype = 1;
10681 vinfos[1].foffset = j7;
10682 vinfos[1].indices[0] = _ij7[0];
10683 vinfos[1].indices[1] = _ij7[1];
10684 vinfos[1].maxsolutions = _nj7;
10685 vinfos[2].jointtype = 1;
10686 vinfos[2].foffset = j8;
10687 vinfos[2].indices[0] = _ij8[0];
10688 vinfos[2].indices[1] = _ij8[1];
10689 vinfos[2].maxsolutions = _nj8;
10690 vinfos[3].jointtype = 1;
10691 vinfos[3].foffset = j9;
10692 vinfos[3].indices[0] = _ij9[0];
10693 vinfos[3].indices[1] = _ij9[1];
10694 vinfos[3].maxsolutions = _nj9;
10695 vinfos[4].jointtype = 1;
10696 vinfos[4].foffset = j10;
10697 vinfos[4].indices[0] = _ij10[0];
10698 vinfos[4].indices[1] = _ij10[1];
10699 vinfos[4].maxsolutions = _nj10;
10700 vinfos[5].jointtype = 1;
10701 vinfos[5].foffset = j11;
10702 vinfos[5].indices[0] = _ij11[0];
10703 vinfos[5].indices[1] = _ij11[1];
10704 vinfos[5].maxsolutions = _nj11;
10705 vinfos[6].jointtype = 1;
10706 vinfos[6].foffset = j12;
10707 vinfos[6].indices[0] = _ij12[0];
10708 vinfos[6].indices[1] = _ij12[1];
10709 vinfos[6].maxsolutions = _nj12;
10710 std::vector<int> vfree(0);
10711 solutions.AddSolution(vinfos,vfree);
10712 }
10713 }
10714 }
10715 
10716 }
10717 } while(0);
10718 if( bgotonextstatement )
10719 {
10720 bool bgotonextstatement = true;
10721 do
10722 {
10723 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
10724 evalcond[1]=new_r22;
10725 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
10726 {
10727 bgotonextstatement=false;
10728 {
10729 IkReal j10array[1], cj10array[1], sj10array[1];
10730 bool j10valid[1]={false};
10731 _nj10 = 1;
10732 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
10733     continue;
10734 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
10735 sj10array[0]=IKsin(j10array[0]);
10736 cj10array[0]=IKcos(j10array[0]);
10737 if( j10array[0] > IKPI )
10738 {
10739     j10array[0]-=IK2PI;
10740 }
10741 else if( j10array[0] < -IKPI )
10742 {    j10array[0]+=IK2PI;
10743 }
10744 j10valid[0] = true;
10745 for(int ij10 = 0; ij10 < 1; ++ij10)
10746 {
10747 if( !j10valid[ij10] )
10748 {
10749     continue;
10750 }
10751 _ij10[0] = ij10; _ij10[1] = -1;
10752 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10753 {
10754 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10755 {
10756     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10757 }
10758 }
10759 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10760 {
10761 IkReal evalcond[8];
10762 IkReal x380=IKcos(j10);
10763 IkReal x381=IKsin(j10);
10764 IkReal x382=((1.0)*sj8);
10765 IkReal x383=((1.0)*x381);
10766 IkReal x384=((1.0)*x380);
10767 evalcond[0]=(x380+new_r20);
10768 evalcond[1]=((((-1.0)*x383))+new_r21);
10769 evalcond[2]=(((sj8*x380))+new_r01);
10770 evalcond[3]=(((sj8*x381))+new_r00);
10771 evalcond[4]=(new_r11+(((-1.0)*cj8*x384)));
10772 evalcond[5]=(new_r10+(((-1.0)*cj8*x383)));
10773 evalcond[6]=((((-1.0)*new_r00*x382))+((cj8*new_r10))+(((-1.0)*x383)));
10774 evalcond[7]=((((-1.0)*new_r01*x382))+((cj8*new_r11))+(((-1.0)*x384)));
10775 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10776 {
10777 continue;
10778 }
10779 }
10780 
10781 {
10782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10783 vinfos[0].jointtype = 1;
10784 vinfos[0].foffset = j6;
10785 vinfos[0].indices[0] = _ij6[0];
10786 vinfos[0].indices[1] = _ij6[1];
10787 vinfos[0].maxsolutions = _nj6;
10788 vinfos[1].jointtype = 1;
10789 vinfos[1].foffset = j7;
10790 vinfos[1].indices[0] = _ij7[0];
10791 vinfos[1].indices[1] = _ij7[1];
10792 vinfos[1].maxsolutions = _nj7;
10793 vinfos[2].jointtype = 1;
10794 vinfos[2].foffset = j8;
10795 vinfos[2].indices[0] = _ij8[0];
10796 vinfos[2].indices[1] = _ij8[1];
10797 vinfos[2].maxsolutions = _nj8;
10798 vinfos[3].jointtype = 1;
10799 vinfos[3].foffset = j9;
10800 vinfos[3].indices[0] = _ij9[0];
10801 vinfos[3].indices[1] = _ij9[1];
10802 vinfos[3].maxsolutions = _nj9;
10803 vinfos[4].jointtype = 1;
10804 vinfos[4].foffset = j10;
10805 vinfos[4].indices[0] = _ij10[0];
10806 vinfos[4].indices[1] = _ij10[1];
10807 vinfos[4].maxsolutions = _nj10;
10808 vinfos[5].jointtype = 1;
10809 vinfos[5].foffset = j11;
10810 vinfos[5].indices[0] = _ij11[0];
10811 vinfos[5].indices[1] = _ij11[1];
10812 vinfos[5].maxsolutions = _nj11;
10813 vinfos[6].jointtype = 1;
10814 vinfos[6].foffset = j12;
10815 vinfos[6].indices[0] = _ij12[0];
10816 vinfos[6].indices[1] = _ij12[1];
10817 vinfos[6].maxsolutions = _nj12;
10818 std::vector<int> vfree(0);
10819 solutions.AddSolution(vinfos,vfree);
10820 }
10821 }
10822 }
10823 
10824 }
10825 } while(0);
10826 if( bgotonextstatement )
10827 {
10828 bool bgotonextstatement = true;
10829 do
10830 {
10831 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
10832 evalcond[1]=new_r22;
10833 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
10834 {
10835 bgotonextstatement=false;
10836 {
10837 IkReal j10array[1], cj10array[1], sj10array[1];
10838 bool j10valid[1]={false};
10839 _nj10 = 1;
10840 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
10841     continue;
10842 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
10843 sj10array[0]=IKsin(j10array[0]);
10844 cj10array[0]=IKcos(j10array[0]);
10845 if( j10array[0] > IKPI )
10846 {
10847     j10array[0]-=IK2PI;
10848 }
10849 else if( j10array[0] < -IKPI )
10850 {    j10array[0]+=IK2PI;
10851 }
10852 j10valid[0] = true;
10853 for(int ij10 = 0; ij10 < 1; ++ij10)
10854 {
10855 if( !j10valid[ij10] )
10856 {
10857     continue;
10858 }
10859 _ij10[0] = ij10; _ij10[1] = -1;
10860 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10861 {
10862 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10863 {
10864     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10865 }
10866 }
10867 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10868 {
10869 IkReal evalcond[8];
10870 IkReal x385=IKcos(j10);
10871 IkReal x386=IKsin(j10);
10872 IkReal x387=((1.0)*sj8);
10873 IkReal x388=((1.0)*x385);
10874 IkReal x389=((1.0)*x386);
10875 evalcond[0]=(x386+new_r21);
10876 evalcond[1]=((((-1.0)*x388))+new_r20);
10877 evalcond[2]=(((sj8*x385))+new_r01);
10878 evalcond[3]=(((sj8*x386))+new_r00);
10879 evalcond[4]=(new_r11+(((-1.0)*cj8*x388)));
10880 evalcond[5]=(new_r10+(((-1.0)*cj8*x389)));
10881 evalcond[6]=((((-1.0)*new_r00*x387))+((cj8*new_r10))+(((-1.0)*x389)));
10882 evalcond[7]=((((-1.0)*new_r01*x387))+((cj8*new_r11))+(((-1.0)*x388)));
10883 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10884 {
10885 continue;
10886 }
10887 }
10888 
10889 {
10890 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10891 vinfos[0].jointtype = 1;
10892 vinfos[0].foffset = j6;
10893 vinfos[0].indices[0] = _ij6[0];
10894 vinfos[0].indices[1] = _ij6[1];
10895 vinfos[0].maxsolutions = _nj6;
10896 vinfos[1].jointtype = 1;
10897 vinfos[1].foffset = j7;
10898 vinfos[1].indices[0] = _ij7[0];
10899 vinfos[1].indices[1] = _ij7[1];
10900 vinfos[1].maxsolutions = _nj7;
10901 vinfos[2].jointtype = 1;
10902 vinfos[2].foffset = j8;
10903 vinfos[2].indices[0] = _ij8[0];
10904 vinfos[2].indices[1] = _ij8[1];
10905 vinfos[2].maxsolutions = _nj8;
10906 vinfos[3].jointtype = 1;
10907 vinfos[3].foffset = j9;
10908 vinfos[3].indices[0] = _ij9[0];
10909 vinfos[3].indices[1] = _ij9[1];
10910 vinfos[3].maxsolutions = _nj9;
10911 vinfos[4].jointtype = 1;
10912 vinfos[4].foffset = j10;
10913 vinfos[4].indices[0] = _ij10[0];
10914 vinfos[4].indices[1] = _ij10[1];
10915 vinfos[4].maxsolutions = _nj10;
10916 vinfos[5].jointtype = 1;
10917 vinfos[5].foffset = j11;
10918 vinfos[5].indices[0] = _ij11[0];
10919 vinfos[5].indices[1] = _ij11[1];
10920 vinfos[5].maxsolutions = _nj11;
10921 vinfos[6].jointtype = 1;
10922 vinfos[6].foffset = j12;
10923 vinfos[6].indices[0] = _ij12[0];
10924 vinfos[6].indices[1] = _ij12[1];
10925 vinfos[6].maxsolutions = _nj12;
10926 std::vector<int> vfree(0);
10927 solutions.AddSolution(vinfos,vfree);
10928 }
10929 }
10930 }
10931 
10932 }
10933 } while(0);
10934 if( bgotonextstatement )
10935 {
10936 bool bgotonextstatement = true;
10937 do
10938 {
10939 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
10940 evalcond[1]=new_r20;
10941 evalcond[2]=new_r02;
10942 evalcond[3]=new_r12;
10943 evalcond[4]=new_r21;
10944 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
10945 {
10946 bgotonextstatement=false;
10947 {
10948 IkReal j10array[1], cj10array[1], sj10array[1];
10949 bool j10valid[1]={false};
10950 _nj10 = 1;
10951 IkReal x390=((1.0)*new_r01);
10952 if( IKabs(((((-1.0)*cj8*x390))+(((-1.0)*new_r00*sj8)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj8*new_r00))+(((-1.0)*sj8*x390)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj8*x390))+(((-1.0)*new_r00*sj8))))+IKsqr((((cj8*new_r00))+(((-1.0)*sj8*x390))))-1) <= IKFAST_SINCOS_THRESH )
10953     continue;
10954 j10array[0]=IKatan2(((((-1.0)*cj8*x390))+(((-1.0)*new_r00*sj8))), (((cj8*new_r00))+(((-1.0)*sj8*x390))));
10955 sj10array[0]=IKsin(j10array[0]);
10956 cj10array[0]=IKcos(j10array[0]);
10957 if( j10array[0] > IKPI )
10958 {
10959     j10array[0]-=IK2PI;
10960 }
10961 else if( j10array[0] < -IKPI )
10962 {    j10array[0]+=IK2PI;
10963 }
10964 j10valid[0] = true;
10965 for(int ij10 = 0; ij10 < 1; ++ij10)
10966 {
10967 if( !j10valid[ij10] )
10968 {
10969     continue;
10970 }
10971 _ij10[0] = ij10; _ij10[1] = -1;
10972 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10973 {
10974 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10975 {
10976     j10valid[iij10]=false; _ij10[1] = iij10; break; 
10977 }
10978 }
10979 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10980 {
10981 IkReal evalcond[8];
10982 IkReal x391=IKsin(j10);
10983 IkReal x392=IKcos(j10);
10984 IkReal x393=((1.0)*sj8);
10985 IkReal x394=((1.0)*x392);
10986 IkReal x395=(sj8*x391);
10987 IkReal x396=(cj8*x391);
10988 IkReal x397=(cj8*x394);
10989 evalcond[0]=(((new_r11*sj8))+x391+((cj8*new_r01)));
10990 evalcond[1]=(((sj8*x392))+x396+new_r01);
10991 evalcond[2]=((((-1.0)*x394))+((new_r10*sj8))+((cj8*new_r00)));
10992 evalcond[3]=((((-1.0)*x391))+((cj8*new_r10))+(((-1.0)*new_r00*x393)));
10993 evalcond[4]=((((-1.0)*new_r01*x393))+(((-1.0)*x394))+((cj8*new_r11)));
10994 evalcond[5]=((((-1.0)*x397))+x395+new_r00);
10995 evalcond[6]=((((-1.0)*x397))+x395+new_r11);
10996 evalcond[7]=((((-1.0)*x396))+new_r10+(((-1.0)*x392*x393)));
10997 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
10998 {
10999 continue;
11000 }
11001 }
11002 
11003 {
11004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11005 vinfos[0].jointtype = 1;
11006 vinfos[0].foffset = j6;
11007 vinfos[0].indices[0] = _ij6[0];
11008 vinfos[0].indices[1] = _ij6[1];
11009 vinfos[0].maxsolutions = _nj6;
11010 vinfos[1].jointtype = 1;
11011 vinfos[1].foffset = j7;
11012 vinfos[1].indices[0] = _ij7[0];
11013 vinfos[1].indices[1] = _ij7[1];
11014 vinfos[1].maxsolutions = _nj7;
11015 vinfos[2].jointtype = 1;
11016 vinfos[2].foffset = j8;
11017 vinfos[2].indices[0] = _ij8[0];
11018 vinfos[2].indices[1] = _ij8[1];
11019 vinfos[2].maxsolutions = _nj8;
11020 vinfos[3].jointtype = 1;
11021 vinfos[3].foffset = j9;
11022 vinfos[3].indices[0] = _ij9[0];
11023 vinfos[3].indices[1] = _ij9[1];
11024 vinfos[3].maxsolutions = _nj9;
11025 vinfos[4].jointtype = 1;
11026 vinfos[4].foffset = j10;
11027 vinfos[4].indices[0] = _ij10[0];
11028 vinfos[4].indices[1] = _ij10[1];
11029 vinfos[4].maxsolutions = _nj10;
11030 vinfos[5].jointtype = 1;
11031 vinfos[5].foffset = j11;
11032 vinfos[5].indices[0] = _ij11[0];
11033 vinfos[5].indices[1] = _ij11[1];
11034 vinfos[5].maxsolutions = _nj11;
11035 vinfos[6].jointtype = 1;
11036 vinfos[6].foffset = j12;
11037 vinfos[6].indices[0] = _ij12[0];
11038 vinfos[6].indices[1] = _ij12[1];
11039 vinfos[6].maxsolutions = _nj12;
11040 std::vector<int> vfree(0);
11041 solutions.AddSolution(vinfos,vfree);
11042 }
11043 }
11044 }
11045 
11046 }
11047 } while(0);
11048 if( bgotonextstatement )
11049 {
11050 bool bgotonextstatement = true;
11051 do
11052 {
11053 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
11054 evalcond[1]=new_r20;
11055 evalcond[2]=new_r02;
11056 evalcond[3]=new_r12;
11057 evalcond[4]=new_r21;
11058 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
11059 {
11060 bgotonextstatement=false;
11061 {
11062 IkReal j10array[1], cj10array[1], sj10array[1];
11063 bool j10valid[1]={false};
11064 _nj10 = 1;
11065 IkReal x398=((1.0)*new_r00);
11066 if( IKabs((((cj8*new_r01))+(((-1.0)*sj8*x398)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x398)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj8*new_r01))+(((-1.0)*sj8*x398))))+IKsqr(((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x398))))-1) <= IKFAST_SINCOS_THRESH )
11067     continue;
11068 j10array[0]=IKatan2((((cj8*new_r01))+(((-1.0)*sj8*x398))), ((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x398))));
11069 sj10array[0]=IKsin(j10array[0]);
11070 cj10array[0]=IKcos(j10array[0]);
11071 if( j10array[0] > IKPI )
11072 {
11073     j10array[0]-=IK2PI;
11074 }
11075 else if( j10array[0] < -IKPI )
11076 {    j10array[0]+=IK2PI;
11077 }
11078 j10valid[0] = true;
11079 for(int ij10 = 0; ij10 < 1; ++ij10)
11080 {
11081 if( !j10valid[ij10] )
11082 {
11083     continue;
11084 }
11085 _ij10[0] = ij10; _ij10[1] = -1;
11086 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11087 {
11088 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11089 {
11090     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11091 }
11092 }
11093 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11094 {
11095 IkReal evalcond[8];
11096 IkReal x399=IKsin(j10);
11097 IkReal x400=IKcos(j10);
11098 IkReal x401=((1.0)*sj8);
11099 IkReal x402=((1.0)*x399);
11100 IkReal x403=(sj8*x400);
11101 IkReal x404=((1.0)*x400);
11102 IkReal x405=(cj8*x402);
11103 evalcond[0]=(((new_r10*sj8))+((cj8*new_r00))+x400);
11104 evalcond[1]=(((new_r11*sj8))+((cj8*new_r01))+(((-1.0)*x402)));
11105 evalcond[2]=(((sj8*x399))+((cj8*x400))+new_r00);
11106 evalcond[3]=(((cj8*new_r10))+(((-1.0)*new_r00*x401))+(((-1.0)*x402)));
11107 evalcond[4]=(((cj8*new_r11))+(((-1.0)*new_r01*x401))+(((-1.0)*x404)));
11108 evalcond[5]=(x403+new_r01+(((-1.0)*x405)));
11109 evalcond[6]=(x403+new_r10+(((-1.0)*x405)));
11110 evalcond[7]=((((-1.0)*cj8*x404))+(((-1.0)*x399*x401))+new_r11);
11111 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
11112 {
11113 continue;
11114 }
11115 }
11116 
11117 {
11118 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11119 vinfos[0].jointtype = 1;
11120 vinfos[0].foffset = j6;
11121 vinfos[0].indices[0] = _ij6[0];
11122 vinfos[0].indices[1] = _ij6[1];
11123 vinfos[0].maxsolutions = _nj6;
11124 vinfos[1].jointtype = 1;
11125 vinfos[1].foffset = j7;
11126 vinfos[1].indices[0] = _ij7[0];
11127 vinfos[1].indices[1] = _ij7[1];
11128 vinfos[1].maxsolutions = _nj7;
11129 vinfos[2].jointtype = 1;
11130 vinfos[2].foffset = j8;
11131 vinfos[2].indices[0] = _ij8[0];
11132 vinfos[2].indices[1] = _ij8[1];
11133 vinfos[2].maxsolutions = _nj8;
11134 vinfos[3].jointtype = 1;
11135 vinfos[3].foffset = j9;
11136 vinfos[3].indices[0] = _ij9[0];
11137 vinfos[3].indices[1] = _ij9[1];
11138 vinfos[3].maxsolutions = _nj9;
11139 vinfos[4].jointtype = 1;
11140 vinfos[4].foffset = j10;
11141 vinfos[4].indices[0] = _ij10[0];
11142 vinfos[4].indices[1] = _ij10[1];
11143 vinfos[4].maxsolutions = _nj10;
11144 vinfos[5].jointtype = 1;
11145 vinfos[5].foffset = j11;
11146 vinfos[5].indices[0] = _ij11[0];
11147 vinfos[5].indices[1] = _ij11[1];
11148 vinfos[5].maxsolutions = _nj11;
11149 vinfos[6].jointtype = 1;
11150 vinfos[6].foffset = j12;
11151 vinfos[6].indices[0] = _ij12[0];
11152 vinfos[6].indices[1] = _ij12[1];
11153 vinfos[6].maxsolutions = _nj12;
11154 std::vector<int> vfree(0);
11155 solutions.AddSolution(vinfos,vfree);
11156 }
11157 }
11158 }
11159 
11160 }
11161 } while(0);
11162 if( bgotonextstatement )
11163 {
11164 bool bgotonextstatement = true;
11165 do
11166 {
11167 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j8))), 6.28318530717959)));
11168 evalcond[1]=new_r12;
11169 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
11170 {
11171 bgotonextstatement=false;
11172 {
11173 IkReal j10array[1], cj10array[1], sj10array[1];
11174 bool j10valid[1]={false};
11175 _nj10 = 1;
11176 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
11177     continue;
11178 j10array[0]=IKatan2(new_r10, new_r11);
11179 sj10array[0]=IKsin(j10array[0]);
11180 cj10array[0]=IKcos(j10array[0]);
11181 if( j10array[0] > IKPI )
11182 {
11183     j10array[0]-=IK2PI;
11184 }
11185 else if( j10array[0] < -IKPI )
11186 {    j10array[0]+=IK2PI;
11187 }
11188 j10valid[0] = true;
11189 for(int ij10 = 0; ij10 < 1; ++ij10)
11190 {
11191 if( !j10valid[ij10] )
11192 {
11193     continue;
11194 }
11195 _ij10[0] = ij10; _ij10[1] = -1;
11196 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11197 {
11198 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11199 {
11200     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11201 }
11202 }
11203 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11204 {
11205 IkReal evalcond[8];
11206 IkReal x406=IKcos(j10);
11207 IkReal x407=IKsin(j10);
11208 IkReal x408=((1.0)*sj9);
11209 IkReal x409=((1.0)*x406);
11210 evalcond[0]=(new_r20+((sj9*x406)));
11211 evalcond[1]=(new_r10+(((-1.0)*x407)));
11212 evalcond[2]=(new_r11+(((-1.0)*x409)));
11213 evalcond[3]=(new_r01+((cj9*x407)));
11214 evalcond[4]=((((-1.0)*x407*x408))+new_r21);
11215 evalcond[5]=((((-1.0)*cj9*x409))+new_r00);
11216 evalcond[6]=(((cj9*new_r01))+x407+(((-1.0)*new_r21*x408)));
11217 evalcond[7]=(((cj9*new_r00))+(((-1.0)*new_r20*x408))+(((-1.0)*x409)));
11218 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
11219 {
11220 continue;
11221 }
11222 }
11223 
11224 {
11225 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11226 vinfos[0].jointtype = 1;
11227 vinfos[0].foffset = j6;
11228 vinfos[0].indices[0] = _ij6[0];
11229 vinfos[0].indices[1] = _ij6[1];
11230 vinfos[0].maxsolutions = _nj6;
11231 vinfos[1].jointtype = 1;
11232 vinfos[1].foffset = j7;
11233 vinfos[1].indices[0] = _ij7[0];
11234 vinfos[1].indices[1] = _ij7[1];
11235 vinfos[1].maxsolutions = _nj7;
11236 vinfos[2].jointtype = 1;
11237 vinfos[2].foffset = j8;
11238 vinfos[2].indices[0] = _ij8[0];
11239 vinfos[2].indices[1] = _ij8[1];
11240 vinfos[2].maxsolutions = _nj8;
11241 vinfos[3].jointtype = 1;
11242 vinfos[3].foffset = j9;
11243 vinfos[3].indices[0] = _ij9[0];
11244 vinfos[3].indices[1] = _ij9[1];
11245 vinfos[3].maxsolutions = _nj9;
11246 vinfos[4].jointtype = 1;
11247 vinfos[4].foffset = j10;
11248 vinfos[4].indices[0] = _ij10[0];
11249 vinfos[4].indices[1] = _ij10[1];
11250 vinfos[4].maxsolutions = _nj10;
11251 vinfos[5].jointtype = 1;
11252 vinfos[5].foffset = j11;
11253 vinfos[5].indices[0] = _ij11[0];
11254 vinfos[5].indices[1] = _ij11[1];
11255 vinfos[5].maxsolutions = _nj11;
11256 vinfos[6].jointtype = 1;
11257 vinfos[6].foffset = j12;
11258 vinfos[6].indices[0] = _ij12[0];
11259 vinfos[6].indices[1] = _ij12[1];
11260 vinfos[6].maxsolutions = _nj12;
11261 std::vector<int> vfree(0);
11262 solutions.AddSolution(vinfos,vfree);
11263 }
11264 }
11265 }
11266 
11267 }
11268 } while(0);
11269 if( bgotonextstatement )
11270 {
11271 bool bgotonextstatement = true;
11272 do
11273 {
11274 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j8)))), 6.28318530717959)));
11275 evalcond[1]=new_r12;
11276 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
11277 {
11278 bgotonextstatement=false;
11279 {
11280 IkReal j10eval[3];
11281 sj8=0;
11282 cj8=-1.0;
11283 j8=3.14159265358979;
11284 j10eval[0]=sj9;
11285 j10eval[1]=IKsign(sj9);
11286 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
11287 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
11288 {
11289 {
11290 IkReal j10eval[1];
11291 sj8=0;
11292 cj8=-1.0;
11293 j8=3.14159265358979;
11294 j10eval[0]=sj9;
11295 if( IKabs(j10eval[0]) < 0.0000010000000000  )
11296 {
11297 {
11298 IkReal j10eval[2];
11299 sj8=0;
11300 cj8=-1.0;
11301 j8=3.14159265358979;
11302 j10eval[0]=cj9;
11303 j10eval[1]=sj9;
11304 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  )
11305 {
11306 {
11307 IkReal evalcond[4];
11308 bool bgotonextstatement = true;
11309 do
11310 {
11311 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
11312 evalcond[1]=new_r22;
11313 evalcond[2]=new_r01;
11314 evalcond[3]=new_r00;
11315 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
11316 {
11317 bgotonextstatement=false;
11318 {
11319 IkReal j10array[1], cj10array[1], sj10array[1];
11320 bool j10valid[1]={false};
11321 _nj10 = 1;
11322 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
11323     continue;
11324 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
11325 sj10array[0]=IKsin(j10array[0]);
11326 cj10array[0]=IKcos(j10array[0]);
11327 if( j10array[0] > IKPI )
11328 {
11329     j10array[0]-=IK2PI;
11330 }
11331 else if( j10array[0] < -IKPI )
11332 {    j10array[0]+=IK2PI;
11333 }
11334 j10valid[0] = true;
11335 for(int ij10 = 0; ij10 < 1; ++ij10)
11336 {
11337 if( !j10valid[ij10] )
11338 {
11339     continue;
11340 }
11341 _ij10[0] = ij10; _ij10[1] = -1;
11342 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11343 {
11344 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11345 {
11346     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11347 }
11348 }
11349 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11350 {
11351 IkReal evalcond[4];
11352 IkReal x410=IKcos(j10);
11353 IkReal x411=((1.0)*(IKsin(j10)));
11354 evalcond[0]=(x410+new_r20);
11355 evalcond[1]=(new_r21+(((-1.0)*x411)));
11356 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x411)));
11357 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x410)));
11358 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
11359 {
11360 continue;
11361 }
11362 }
11363 
11364 {
11365 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11366 vinfos[0].jointtype = 1;
11367 vinfos[0].foffset = j6;
11368 vinfos[0].indices[0] = _ij6[0];
11369 vinfos[0].indices[1] = _ij6[1];
11370 vinfos[0].maxsolutions = _nj6;
11371 vinfos[1].jointtype = 1;
11372 vinfos[1].foffset = j7;
11373 vinfos[1].indices[0] = _ij7[0];
11374 vinfos[1].indices[1] = _ij7[1];
11375 vinfos[1].maxsolutions = _nj7;
11376 vinfos[2].jointtype = 1;
11377 vinfos[2].foffset = j8;
11378 vinfos[2].indices[0] = _ij8[0];
11379 vinfos[2].indices[1] = _ij8[1];
11380 vinfos[2].maxsolutions = _nj8;
11381 vinfos[3].jointtype = 1;
11382 vinfos[3].foffset = j9;
11383 vinfos[3].indices[0] = _ij9[0];
11384 vinfos[3].indices[1] = _ij9[1];
11385 vinfos[3].maxsolutions = _nj9;
11386 vinfos[4].jointtype = 1;
11387 vinfos[4].foffset = j10;
11388 vinfos[4].indices[0] = _ij10[0];
11389 vinfos[4].indices[1] = _ij10[1];
11390 vinfos[4].maxsolutions = _nj10;
11391 vinfos[5].jointtype = 1;
11392 vinfos[5].foffset = j11;
11393 vinfos[5].indices[0] = _ij11[0];
11394 vinfos[5].indices[1] = _ij11[1];
11395 vinfos[5].maxsolutions = _nj11;
11396 vinfos[6].jointtype = 1;
11397 vinfos[6].foffset = j12;
11398 vinfos[6].indices[0] = _ij12[0];
11399 vinfos[6].indices[1] = _ij12[1];
11400 vinfos[6].maxsolutions = _nj12;
11401 std::vector<int> vfree(0);
11402 solutions.AddSolution(vinfos,vfree);
11403 }
11404 }
11405 }
11406 
11407 }
11408 } while(0);
11409 if( bgotonextstatement )
11410 {
11411 bool bgotonextstatement = true;
11412 do
11413 {
11414 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
11415 evalcond[1]=new_r22;
11416 evalcond[2]=new_r01;
11417 evalcond[3]=new_r00;
11418 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
11419 {
11420 bgotonextstatement=false;
11421 {
11422 IkReal j10array[1], cj10array[1], sj10array[1];
11423 bool j10valid[1]={false};
11424 _nj10 = 1;
11425 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
11426     continue;
11427 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
11428 sj10array[0]=IKsin(j10array[0]);
11429 cj10array[0]=IKcos(j10array[0]);
11430 if( j10array[0] > IKPI )
11431 {
11432     j10array[0]-=IK2PI;
11433 }
11434 else if( j10array[0] < -IKPI )
11435 {    j10array[0]+=IK2PI;
11436 }
11437 j10valid[0] = true;
11438 for(int ij10 = 0; ij10 < 1; ++ij10)
11439 {
11440 if( !j10valid[ij10] )
11441 {
11442     continue;
11443 }
11444 _ij10[0] = ij10; _ij10[1] = -1;
11445 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11446 {
11447 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11448 {
11449     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11450 }
11451 }
11452 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11453 {
11454 IkReal evalcond[4];
11455 IkReal x412=IKsin(j10);
11456 IkReal x413=((1.0)*(IKcos(j10)));
11457 evalcond[0]=(x412+new_r21);
11458 evalcond[1]=(new_r20+(((-1.0)*x413)));
11459 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x412)));
11460 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x413)));
11461 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
11462 {
11463 continue;
11464 }
11465 }
11466 
11467 {
11468 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11469 vinfos[0].jointtype = 1;
11470 vinfos[0].foffset = j6;
11471 vinfos[0].indices[0] = _ij6[0];
11472 vinfos[0].indices[1] = _ij6[1];
11473 vinfos[0].maxsolutions = _nj6;
11474 vinfos[1].jointtype = 1;
11475 vinfos[1].foffset = j7;
11476 vinfos[1].indices[0] = _ij7[0];
11477 vinfos[1].indices[1] = _ij7[1];
11478 vinfos[1].maxsolutions = _nj7;
11479 vinfos[2].jointtype = 1;
11480 vinfos[2].foffset = j8;
11481 vinfos[2].indices[0] = _ij8[0];
11482 vinfos[2].indices[1] = _ij8[1];
11483 vinfos[2].maxsolutions = _nj8;
11484 vinfos[3].jointtype = 1;
11485 vinfos[3].foffset = j9;
11486 vinfos[3].indices[0] = _ij9[0];
11487 vinfos[3].indices[1] = _ij9[1];
11488 vinfos[3].maxsolutions = _nj9;
11489 vinfos[4].jointtype = 1;
11490 vinfos[4].foffset = j10;
11491 vinfos[4].indices[0] = _ij10[0];
11492 vinfos[4].indices[1] = _ij10[1];
11493 vinfos[4].maxsolutions = _nj10;
11494 vinfos[5].jointtype = 1;
11495 vinfos[5].foffset = j11;
11496 vinfos[5].indices[0] = _ij11[0];
11497 vinfos[5].indices[1] = _ij11[1];
11498 vinfos[5].maxsolutions = _nj11;
11499 vinfos[6].jointtype = 1;
11500 vinfos[6].foffset = j12;
11501 vinfos[6].indices[0] = _ij12[0];
11502 vinfos[6].indices[1] = _ij12[1];
11503 vinfos[6].maxsolutions = _nj12;
11504 std::vector<int> vfree(0);
11505 solutions.AddSolution(vinfos,vfree);
11506 }
11507 }
11508 }
11509 
11510 }
11511 } while(0);
11512 if( bgotonextstatement )
11513 {
11514 bool bgotonextstatement = true;
11515 do
11516 {
11517 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
11518 evalcond[1]=new_r20;
11519 evalcond[2]=new_r02;
11520 evalcond[3]=new_r21;
11521 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
11522 {
11523 bgotonextstatement=false;
11524 {
11525 IkReal j10array[1], cj10array[1], sj10array[1];
11526 bool j10valid[1]={false};
11527 _nj10 = 1;
11528 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
11529     continue;
11530 j10array[0]=IKatan2(new_r01, ((-1.0)*new_r11));
11531 sj10array[0]=IKsin(j10array[0]);
11532 cj10array[0]=IKcos(j10array[0]);
11533 if( j10array[0] > IKPI )
11534 {
11535     j10array[0]-=IK2PI;
11536 }
11537 else if( j10array[0] < -IKPI )
11538 {    j10array[0]+=IK2PI;
11539 }
11540 j10valid[0] = true;
11541 for(int ij10 = 0; ij10 < 1; ++ij10)
11542 {
11543 if( !j10valid[ij10] )
11544 {
11545     continue;
11546 }
11547 _ij10[0] = ij10; _ij10[1] = -1;
11548 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11549 {
11550 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11551 {
11552     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11553 }
11554 }
11555 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11556 {
11557 IkReal evalcond[4];
11558 IkReal x414=IKsin(j10);
11559 IkReal x415=((1.0)*(IKcos(j10)));
11560 evalcond[0]=(x414+(((-1.0)*new_r01)));
11561 evalcond[1]=((((-1.0)*new_r10))+(((-1.0)*x414)));
11562 evalcond[2]=((((-1.0)*new_r11))+(((-1.0)*x415)));
11563 evalcond[3]=((((-1.0)*new_r00))+(((-1.0)*x415)));
11564 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
11565 {
11566 continue;
11567 }
11568 }
11569 
11570 {
11571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11572 vinfos[0].jointtype = 1;
11573 vinfos[0].foffset = j6;
11574 vinfos[0].indices[0] = _ij6[0];
11575 vinfos[0].indices[1] = _ij6[1];
11576 vinfos[0].maxsolutions = _nj6;
11577 vinfos[1].jointtype = 1;
11578 vinfos[1].foffset = j7;
11579 vinfos[1].indices[0] = _ij7[0];
11580 vinfos[1].indices[1] = _ij7[1];
11581 vinfos[1].maxsolutions = _nj7;
11582 vinfos[2].jointtype = 1;
11583 vinfos[2].foffset = j8;
11584 vinfos[2].indices[0] = _ij8[0];
11585 vinfos[2].indices[1] = _ij8[1];
11586 vinfos[2].maxsolutions = _nj8;
11587 vinfos[3].jointtype = 1;
11588 vinfos[3].foffset = j9;
11589 vinfos[3].indices[0] = _ij9[0];
11590 vinfos[3].indices[1] = _ij9[1];
11591 vinfos[3].maxsolutions = _nj9;
11592 vinfos[4].jointtype = 1;
11593 vinfos[4].foffset = j10;
11594 vinfos[4].indices[0] = _ij10[0];
11595 vinfos[4].indices[1] = _ij10[1];
11596 vinfos[4].maxsolutions = _nj10;
11597 vinfos[5].jointtype = 1;
11598 vinfos[5].foffset = j11;
11599 vinfos[5].indices[0] = _ij11[0];
11600 vinfos[5].indices[1] = _ij11[1];
11601 vinfos[5].maxsolutions = _nj11;
11602 vinfos[6].jointtype = 1;
11603 vinfos[6].foffset = j12;
11604 vinfos[6].indices[0] = _ij12[0];
11605 vinfos[6].indices[1] = _ij12[1];
11606 vinfos[6].maxsolutions = _nj12;
11607 std::vector<int> vfree(0);
11608 solutions.AddSolution(vinfos,vfree);
11609 }
11610 }
11611 }
11612 
11613 }
11614 } while(0);
11615 if( bgotonextstatement )
11616 {
11617 bool bgotonextstatement = true;
11618 do
11619 {
11620 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
11621 evalcond[1]=new_r20;
11622 evalcond[2]=new_r02;
11623 evalcond[3]=new_r21;
11624 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
11625 {
11626 bgotonextstatement=false;
11627 {
11628 IkReal j10array[1], cj10array[1], sj10array[1];
11629 bool j10valid[1]={false};
11630 _nj10 = 1;
11631 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
11632     continue;
11633 j10array[0]=IKatan2(((-1.0)*new_r10), new_r00);
11634 sj10array[0]=IKsin(j10array[0]);
11635 cj10array[0]=IKcos(j10array[0]);
11636 if( j10array[0] > IKPI )
11637 {
11638     j10array[0]-=IK2PI;
11639 }
11640 else if( j10array[0] < -IKPI )
11641 {    j10array[0]+=IK2PI;
11642 }
11643 j10valid[0] = true;
11644 for(int ij10 = 0; ij10 < 1; ++ij10)
11645 {
11646 if( !j10valid[ij10] )
11647 {
11648     continue;
11649 }
11650 _ij10[0] = ij10; _ij10[1] = -1;
11651 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11652 {
11653 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11654 {
11655     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11656 }
11657 }
11658 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11659 {
11660 IkReal evalcond[4];
11661 IkReal x416=IKcos(j10);
11662 IkReal x417=((1.0)*(IKsin(j10)));
11663 evalcond[0]=(x416+(((-1.0)*new_r00)));
11664 evalcond[1]=((((-1.0)*new_r10))+(((-1.0)*x417)));
11665 evalcond[2]=((((-1.0)*new_r11))+(((-1.0)*x416)));
11666 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x417)));
11667 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
11668 {
11669 continue;
11670 }
11671 }
11672 
11673 {
11674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11675 vinfos[0].jointtype = 1;
11676 vinfos[0].foffset = j6;
11677 vinfos[0].indices[0] = _ij6[0];
11678 vinfos[0].indices[1] = _ij6[1];
11679 vinfos[0].maxsolutions = _nj6;
11680 vinfos[1].jointtype = 1;
11681 vinfos[1].foffset = j7;
11682 vinfos[1].indices[0] = _ij7[0];
11683 vinfos[1].indices[1] = _ij7[1];
11684 vinfos[1].maxsolutions = _nj7;
11685 vinfos[2].jointtype = 1;
11686 vinfos[2].foffset = j8;
11687 vinfos[2].indices[0] = _ij8[0];
11688 vinfos[2].indices[1] = _ij8[1];
11689 vinfos[2].maxsolutions = _nj8;
11690 vinfos[3].jointtype = 1;
11691 vinfos[3].foffset = j9;
11692 vinfos[3].indices[0] = _ij9[0];
11693 vinfos[3].indices[1] = _ij9[1];
11694 vinfos[3].maxsolutions = _nj9;
11695 vinfos[4].jointtype = 1;
11696 vinfos[4].foffset = j10;
11697 vinfos[4].indices[0] = _ij10[0];
11698 vinfos[4].indices[1] = _ij10[1];
11699 vinfos[4].maxsolutions = _nj10;
11700 vinfos[5].jointtype = 1;
11701 vinfos[5].foffset = j11;
11702 vinfos[5].indices[0] = _ij11[0];
11703 vinfos[5].indices[1] = _ij11[1];
11704 vinfos[5].maxsolutions = _nj11;
11705 vinfos[6].jointtype = 1;
11706 vinfos[6].foffset = j12;
11707 vinfos[6].indices[0] = _ij12[0];
11708 vinfos[6].indices[1] = _ij12[1];
11709 vinfos[6].maxsolutions = _nj12;
11710 std::vector<int> vfree(0);
11711 solutions.AddSolution(vinfos,vfree);
11712 }
11713 }
11714 }
11715 
11716 }
11717 } while(0);
11718 if( bgotonextstatement )
11719 {
11720 bool bgotonextstatement = true;
11721 do
11722 {
11723 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
11724 if( IKabs(evalcond[0]) < 0.0000050000000000  )
11725 {
11726 bgotonextstatement=false;
11727 {
11728 IkReal j10array[1], cj10array[1], sj10array[1];
11729 bool j10valid[1]={false};
11730 _nj10 = 1;
11731 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
11732     continue;
11733 j10array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
11734 sj10array[0]=IKsin(j10array[0]);
11735 cj10array[0]=IKcos(j10array[0]);
11736 if( j10array[0] > IKPI )
11737 {
11738     j10array[0]-=IK2PI;
11739 }
11740 else if( j10array[0] < -IKPI )
11741 {    j10array[0]+=IK2PI;
11742 }
11743 j10valid[0] = true;
11744 for(int ij10 = 0; ij10 < 1; ++ij10)
11745 {
11746 if( !j10valid[ij10] )
11747 {
11748     continue;
11749 }
11750 _ij10[0] = ij10; _ij10[1] = -1;
11751 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11752 {
11753 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11754 {
11755     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11756 }
11757 }
11758 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11759 {
11760 IkReal evalcond[6];
11761 IkReal x418=IKsin(j10);
11762 IkReal x419=IKcos(j10);
11763 IkReal x420=((-1.0)*x419);
11764 evalcond[0]=x418;
11765 evalcond[1]=(new_r22*x418);
11766 evalcond[2]=x420;
11767 evalcond[3]=(new_r22*x420);
11768 evalcond[4]=((((-1.0)*new_r10))+(((-1.0)*x418)));
11769 evalcond[5]=((((-1.0)*new_r11))+(((-1.0)*x419)));
11770 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
11771 {
11772 continue;
11773 }
11774 }
11775 
11776 {
11777 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11778 vinfos[0].jointtype = 1;
11779 vinfos[0].foffset = j6;
11780 vinfos[0].indices[0] = _ij6[0];
11781 vinfos[0].indices[1] = _ij6[1];
11782 vinfos[0].maxsolutions = _nj6;
11783 vinfos[1].jointtype = 1;
11784 vinfos[1].foffset = j7;
11785 vinfos[1].indices[0] = _ij7[0];
11786 vinfos[1].indices[1] = _ij7[1];
11787 vinfos[1].maxsolutions = _nj7;
11788 vinfos[2].jointtype = 1;
11789 vinfos[2].foffset = j8;
11790 vinfos[2].indices[0] = _ij8[0];
11791 vinfos[2].indices[1] = _ij8[1];
11792 vinfos[2].maxsolutions = _nj8;
11793 vinfos[3].jointtype = 1;
11794 vinfos[3].foffset = j9;
11795 vinfos[3].indices[0] = _ij9[0];
11796 vinfos[3].indices[1] = _ij9[1];
11797 vinfos[3].maxsolutions = _nj9;
11798 vinfos[4].jointtype = 1;
11799 vinfos[4].foffset = j10;
11800 vinfos[4].indices[0] = _ij10[0];
11801 vinfos[4].indices[1] = _ij10[1];
11802 vinfos[4].maxsolutions = _nj10;
11803 vinfos[5].jointtype = 1;
11804 vinfos[5].foffset = j11;
11805 vinfos[5].indices[0] = _ij11[0];
11806 vinfos[5].indices[1] = _ij11[1];
11807 vinfos[5].maxsolutions = _nj11;
11808 vinfos[6].jointtype = 1;
11809 vinfos[6].foffset = j12;
11810 vinfos[6].indices[0] = _ij12[0];
11811 vinfos[6].indices[1] = _ij12[1];
11812 vinfos[6].maxsolutions = _nj12;
11813 std::vector<int> vfree(0);
11814 solutions.AddSolution(vinfos,vfree);
11815 }
11816 }
11817 }
11818 
11819 }
11820 } while(0);
11821 if( bgotonextstatement )
11822 {
11823 bool bgotonextstatement = true;
11824 do
11825 {
11826 if( 1 )
11827 {
11828 bgotonextstatement=false;
11829 continue; // branch miss [j10]
11830 
11831 }
11832 } while(0);
11833 if( bgotonextstatement )
11834 {
11835 }
11836 }
11837 }
11838 }
11839 }
11840 }
11841 }
11842 
11843 } else
11844 {
11845 {
11846 IkReal j10array[1], cj10array[1], sj10array[1];
11847 bool j10valid[1]={false};
11848 _nj10 = 1;
11849 CheckValue<IkReal> x421=IKPowWithIntegerCheck(cj9,-1);
11850 if(!x421.valid){
11851 continue;
11852 }
11853 CheckValue<IkReal> x422=IKPowWithIntegerCheck(sj9,-1);
11854 if(!x422.valid){
11855 continue;
11856 }
11857 if( IKabs((new_r01*(x421.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x422.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r01*(x421.value)))+IKsqr(((-1.0)*new_r20*(x422.value)))-1) <= IKFAST_SINCOS_THRESH )
11858     continue;
11859 j10array[0]=IKatan2((new_r01*(x421.value)), ((-1.0)*new_r20*(x422.value)));
11860 sj10array[0]=IKsin(j10array[0]);
11861 cj10array[0]=IKcos(j10array[0]);
11862 if( j10array[0] > IKPI )
11863 {
11864     j10array[0]-=IK2PI;
11865 }
11866 else if( j10array[0] < -IKPI )
11867 {    j10array[0]+=IK2PI;
11868 }
11869 j10valid[0] = true;
11870 for(int ij10 = 0; ij10 < 1; ++ij10)
11871 {
11872 if( !j10valid[ij10] )
11873 {
11874     continue;
11875 }
11876 _ij10[0] = ij10; _ij10[1] = -1;
11877 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11878 {
11879 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11880 {
11881     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11882 }
11883 }
11884 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11885 {
11886 IkReal evalcond[8];
11887 IkReal x423=IKsin(j10);
11888 IkReal x424=IKcos(j10);
11889 IkReal x425=((1.0)*sj9);
11890 IkReal x426=((1.0)*new_r01);
11891 IkReal x427=((1.0)*new_r00);
11892 IkReal x428=((1.0)*x424);
11893 evalcond[0]=(((sj9*x424))+new_r20);
11894 evalcond[1]=(new_r21+(((-1.0)*x423*x425)));
11895 evalcond[2]=((((-1.0)*x423))+(((-1.0)*new_r10)));
11896 evalcond[3]=((((-1.0)*x428))+(((-1.0)*new_r11)));
11897 evalcond[4]=(((cj9*x423))+(((-1.0)*x426)));
11898 evalcond[5]=((((-1.0)*cj9*x428))+(((-1.0)*x427)));
11899 evalcond[6]=((((-1.0)*cj9*x426))+x423+(((-1.0)*new_r21*x425)));
11900 evalcond[7]=((((-1.0)*cj9*x427))+(((-1.0)*x428))+(((-1.0)*new_r20*x425)));
11901 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
11902 {
11903 continue;
11904 }
11905 }
11906 
11907 {
11908 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11909 vinfos[0].jointtype = 1;
11910 vinfos[0].foffset = j6;
11911 vinfos[0].indices[0] = _ij6[0];
11912 vinfos[0].indices[1] = _ij6[1];
11913 vinfos[0].maxsolutions = _nj6;
11914 vinfos[1].jointtype = 1;
11915 vinfos[1].foffset = j7;
11916 vinfos[1].indices[0] = _ij7[0];
11917 vinfos[1].indices[1] = _ij7[1];
11918 vinfos[1].maxsolutions = _nj7;
11919 vinfos[2].jointtype = 1;
11920 vinfos[2].foffset = j8;
11921 vinfos[2].indices[0] = _ij8[0];
11922 vinfos[2].indices[1] = _ij8[1];
11923 vinfos[2].maxsolutions = _nj8;
11924 vinfos[3].jointtype = 1;
11925 vinfos[3].foffset = j9;
11926 vinfos[3].indices[0] = _ij9[0];
11927 vinfos[3].indices[1] = _ij9[1];
11928 vinfos[3].maxsolutions = _nj9;
11929 vinfos[4].jointtype = 1;
11930 vinfos[4].foffset = j10;
11931 vinfos[4].indices[0] = _ij10[0];
11932 vinfos[4].indices[1] = _ij10[1];
11933 vinfos[4].maxsolutions = _nj10;
11934 vinfos[5].jointtype = 1;
11935 vinfos[5].foffset = j11;
11936 vinfos[5].indices[0] = _ij11[0];
11937 vinfos[5].indices[1] = _ij11[1];
11938 vinfos[5].maxsolutions = _nj11;
11939 vinfos[6].jointtype = 1;
11940 vinfos[6].foffset = j12;
11941 vinfos[6].indices[0] = _ij12[0];
11942 vinfos[6].indices[1] = _ij12[1];
11943 vinfos[6].maxsolutions = _nj12;
11944 std::vector<int> vfree(0);
11945 solutions.AddSolution(vinfos,vfree);
11946 }
11947 }
11948 }
11949 
11950 }
11951 
11952 }
11953 
11954 } else
11955 {
11956 {
11957 IkReal j10array[1], cj10array[1], sj10array[1];
11958 bool j10valid[1]={false};
11959 _nj10 = 1;
11960 CheckValue<IkReal> x429=IKPowWithIntegerCheck(sj9,-1);
11961 if(!x429.valid){
11962 continue;
11963 }
11964 if( IKabs((new_r21*(x429.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r21*(x429.value)))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
11965     continue;
11966 j10array[0]=IKatan2((new_r21*(x429.value)), ((-1.0)*new_r11));
11967 sj10array[0]=IKsin(j10array[0]);
11968 cj10array[0]=IKcos(j10array[0]);
11969 if( j10array[0] > IKPI )
11970 {
11971     j10array[0]-=IK2PI;
11972 }
11973 else if( j10array[0] < -IKPI )
11974 {    j10array[0]+=IK2PI;
11975 }
11976 j10valid[0] = true;
11977 for(int ij10 = 0; ij10 < 1; ++ij10)
11978 {
11979 if( !j10valid[ij10] )
11980 {
11981     continue;
11982 }
11983 _ij10[0] = ij10; _ij10[1] = -1;
11984 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11985 {
11986 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11987 {
11988     j10valid[iij10]=false; _ij10[1] = iij10; break; 
11989 }
11990 }
11991 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11992 {
11993 IkReal evalcond[8];
11994 IkReal x430=IKsin(j10);
11995 IkReal x431=IKcos(j10);
11996 IkReal x432=((1.0)*sj9);
11997 IkReal x433=((1.0)*new_r01);
11998 IkReal x434=((1.0)*new_r00);
11999 IkReal x435=((1.0)*x431);
12000 evalcond[0]=(((sj9*x431))+new_r20);
12001 evalcond[1]=((((-1.0)*x430*x432))+new_r21);
12002 evalcond[2]=((((-1.0)*x430))+(((-1.0)*new_r10)));
12003 evalcond[3]=((((-1.0)*x435))+(((-1.0)*new_r11)));
12004 evalcond[4]=((((-1.0)*x433))+((cj9*x430)));
12005 evalcond[5]=((((-1.0)*x434))+(((-1.0)*cj9*x435)));
12006 evalcond[6]=((((-1.0)*cj9*x433))+x430+(((-1.0)*new_r21*x432)));
12007 evalcond[7]=((((-1.0)*x435))+(((-1.0)*cj9*x434))+(((-1.0)*new_r20*x432)));
12008 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
12009 {
12010 continue;
12011 }
12012 }
12013 
12014 {
12015 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12016 vinfos[0].jointtype = 1;
12017 vinfos[0].foffset = j6;
12018 vinfos[0].indices[0] = _ij6[0];
12019 vinfos[0].indices[1] = _ij6[1];
12020 vinfos[0].maxsolutions = _nj6;
12021 vinfos[1].jointtype = 1;
12022 vinfos[1].foffset = j7;
12023 vinfos[1].indices[0] = _ij7[0];
12024 vinfos[1].indices[1] = _ij7[1];
12025 vinfos[1].maxsolutions = _nj7;
12026 vinfos[2].jointtype = 1;
12027 vinfos[2].foffset = j8;
12028 vinfos[2].indices[0] = _ij8[0];
12029 vinfos[2].indices[1] = _ij8[1];
12030 vinfos[2].maxsolutions = _nj8;
12031 vinfos[3].jointtype = 1;
12032 vinfos[3].foffset = j9;
12033 vinfos[3].indices[0] = _ij9[0];
12034 vinfos[3].indices[1] = _ij9[1];
12035 vinfos[3].maxsolutions = _nj9;
12036 vinfos[4].jointtype = 1;
12037 vinfos[4].foffset = j10;
12038 vinfos[4].indices[0] = _ij10[0];
12039 vinfos[4].indices[1] = _ij10[1];
12040 vinfos[4].maxsolutions = _nj10;
12041 vinfos[5].jointtype = 1;
12042 vinfos[5].foffset = j11;
12043 vinfos[5].indices[0] = _ij11[0];
12044 vinfos[5].indices[1] = _ij11[1];
12045 vinfos[5].maxsolutions = _nj11;
12046 vinfos[6].jointtype = 1;
12047 vinfos[6].foffset = j12;
12048 vinfos[6].indices[0] = _ij12[0];
12049 vinfos[6].indices[1] = _ij12[1];
12050 vinfos[6].maxsolutions = _nj12;
12051 std::vector<int> vfree(0);
12052 solutions.AddSolution(vinfos,vfree);
12053 }
12054 }
12055 }
12056 
12057 }
12058 
12059 }
12060 
12061 } else
12062 {
12063 {
12064 IkReal j10array[1], cj10array[1], sj10array[1];
12065 bool j10valid[1]={false};
12066 _nj10 = 1;
12067 CheckValue<IkReal> x436 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
12068 if(!x436.valid){
12069 continue;
12070 }
12071 CheckValue<IkReal> x437=IKPowWithIntegerCheck(IKsign(sj9),-1);
12072 if(!x437.valid){
12073 continue;
12074 }
12075 j10array[0]=((-1.5707963267949)+(x436.value)+(((1.5707963267949)*(x437.value))));
12076 sj10array[0]=IKsin(j10array[0]);
12077 cj10array[0]=IKcos(j10array[0]);
12078 if( j10array[0] > IKPI )
12079 {
12080     j10array[0]-=IK2PI;
12081 }
12082 else if( j10array[0] < -IKPI )
12083 {    j10array[0]+=IK2PI;
12084 }
12085 j10valid[0] = true;
12086 for(int ij10 = 0; ij10 < 1; ++ij10)
12087 {
12088 if( !j10valid[ij10] )
12089 {
12090     continue;
12091 }
12092 _ij10[0] = ij10; _ij10[1] = -1;
12093 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12094 {
12095 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12096 {
12097     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12098 }
12099 }
12100 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12101 {
12102 IkReal evalcond[8];
12103 IkReal x438=IKsin(j10);
12104 IkReal x439=IKcos(j10);
12105 IkReal x440=((1.0)*sj9);
12106 IkReal x441=((1.0)*new_r01);
12107 IkReal x442=((1.0)*new_r00);
12108 IkReal x443=((1.0)*x439);
12109 evalcond[0]=(((sj9*x439))+new_r20);
12110 evalcond[1]=((((-1.0)*x438*x440))+new_r21);
12111 evalcond[2]=((((-1.0)*x438))+(((-1.0)*new_r10)));
12112 evalcond[3]=((((-1.0)*x443))+(((-1.0)*new_r11)));
12113 evalcond[4]=((((-1.0)*x441))+((cj9*x438)));
12114 evalcond[5]=((((-1.0)*x442))+(((-1.0)*cj9*x443)));
12115 evalcond[6]=((((-1.0)*cj9*x441))+x438+(((-1.0)*new_r21*x440)));
12116 evalcond[7]=((((-1.0)*x443))+(((-1.0)*cj9*x442))+(((-1.0)*new_r20*x440)));
12117 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
12118 {
12119 continue;
12120 }
12121 }
12122 
12123 {
12124 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12125 vinfos[0].jointtype = 1;
12126 vinfos[0].foffset = j6;
12127 vinfos[0].indices[0] = _ij6[0];
12128 vinfos[0].indices[1] = _ij6[1];
12129 vinfos[0].maxsolutions = _nj6;
12130 vinfos[1].jointtype = 1;
12131 vinfos[1].foffset = j7;
12132 vinfos[1].indices[0] = _ij7[0];
12133 vinfos[1].indices[1] = _ij7[1];
12134 vinfos[1].maxsolutions = _nj7;
12135 vinfos[2].jointtype = 1;
12136 vinfos[2].foffset = j8;
12137 vinfos[2].indices[0] = _ij8[0];
12138 vinfos[2].indices[1] = _ij8[1];
12139 vinfos[2].maxsolutions = _nj8;
12140 vinfos[3].jointtype = 1;
12141 vinfos[3].foffset = j9;
12142 vinfos[3].indices[0] = _ij9[0];
12143 vinfos[3].indices[1] = _ij9[1];
12144 vinfos[3].maxsolutions = _nj9;
12145 vinfos[4].jointtype = 1;
12146 vinfos[4].foffset = j10;
12147 vinfos[4].indices[0] = _ij10[0];
12148 vinfos[4].indices[1] = _ij10[1];
12149 vinfos[4].maxsolutions = _nj10;
12150 vinfos[5].jointtype = 1;
12151 vinfos[5].foffset = j11;
12152 vinfos[5].indices[0] = _ij11[0];
12153 vinfos[5].indices[1] = _ij11[1];
12154 vinfos[5].maxsolutions = _nj11;
12155 vinfos[6].jointtype = 1;
12156 vinfos[6].foffset = j12;
12157 vinfos[6].indices[0] = _ij12[0];
12158 vinfos[6].indices[1] = _ij12[1];
12159 vinfos[6].maxsolutions = _nj12;
12160 std::vector<int> vfree(0);
12161 solutions.AddSolution(vinfos,vfree);
12162 }
12163 }
12164 }
12165 
12166 }
12167 
12168 }
12169 
12170 }
12171 } while(0);
12172 if( bgotonextstatement )
12173 {
12174 bool bgotonextstatement = true;
12175 do
12176 {
12177 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
12178 if( IKabs(evalcond[0]) < 0.0000050000000000  )
12179 {
12180 bgotonextstatement=false;
12181 {
12182 IkReal j10eval[1];
12183 new_r21=0;
12184 new_r20=0;
12185 new_r02=0;
12186 new_r12=0;
12187 j10eval[0]=IKabs(new_r22);
12188 if( IKabs(j10eval[0]) < 0.0000000100000000  )
12189 {
12190 continue; // no branches [j10]
12191 
12192 } else
12193 {
12194 IkReal op[2+1], zeror[2];
12195 int numroots;
12196 op[0]=new_r22;
12197 op[1]=0;
12198 op[2]=((-1.0)*new_r22);
12199 polyroots2(op,zeror,numroots);
12200 IkReal j10array[2], cj10array[2], sj10array[2], tempj10array[1];
12201 int numsolutions = 0;
12202 for(int ij10 = 0; ij10 < numroots; ++ij10)
12203 {
12204 IkReal htj10 = zeror[ij10];
12205 tempj10array[0]=((2.0)*(atan(htj10)));
12206 for(int kj10 = 0; kj10 < 1; ++kj10)
12207 {
12208 j10array[numsolutions] = tempj10array[kj10];
12209 if( j10array[numsolutions] > IKPI )
12210 {
12211     j10array[numsolutions]-=IK2PI;
12212 }
12213 else if( j10array[numsolutions] < -IKPI )
12214 {
12215     j10array[numsolutions]+=IK2PI;
12216 }
12217 sj10array[numsolutions] = IKsin(j10array[numsolutions]);
12218 cj10array[numsolutions] = IKcos(j10array[numsolutions]);
12219 numsolutions++;
12220 }
12221 }
12222 bool j10valid[2]={true,true};
12223 _nj10 = 2;
12224 for(int ij10 = 0; ij10 < numsolutions; ++ij10)
12225     {
12226 if( !j10valid[ij10] )
12227 {
12228     continue;
12229 }
12230     j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12231 htj10 = IKtan(j10/2);
12232 
12233 _ij10[0] = ij10; _ij10[1] = -1;
12234 for(int iij10 = ij10+1; iij10 < numsolutions; ++iij10)
12235 {
12236 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12237 {
12238     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12239 }
12240 }
12241 {
12242 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12243 vinfos[0].jointtype = 1;
12244 vinfos[0].foffset = j6;
12245 vinfos[0].indices[0] = _ij6[0];
12246 vinfos[0].indices[1] = _ij6[1];
12247 vinfos[0].maxsolutions = _nj6;
12248 vinfos[1].jointtype = 1;
12249 vinfos[1].foffset = j7;
12250 vinfos[1].indices[0] = _ij7[0];
12251 vinfos[1].indices[1] = _ij7[1];
12252 vinfos[1].maxsolutions = _nj7;
12253 vinfos[2].jointtype = 1;
12254 vinfos[2].foffset = j8;
12255 vinfos[2].indices[0] = _ij8[0];
12256 vinfos[2].indices[1] = _ij8[1];
12257 vinfos[2].maxsolutions = _nj8;
12258 vinfos[3].jointtype = 1;
12259 vinfos[3].foffset = j9;
12260 vinfos[3].indices[0] = _ij9[0];
12261 vinfos[3].indices[1] = _ij9[1];
12262 vinfos[3].maxsolutions = _nj9;
12263 vinfos[4].jointtype = 1;
12264 vinfos[4].foffset = j10;
12265 vinfos[4].indices[0] = _ij10[0];
12266 vinfos[4].indices[1] = _ij10[1];
12267 vinfos[4].maxsolutions = _nj10;
12268 vinfos[5].jointtype = 1;
12269 vinfos[5].foffset = j11;
12270 vinfos[5].indices[0] = _ij11[0];
12271 vinfos[5].indices[1] = _ij11[1];
12272 vinfos[5].maxsolutions = _nj11;
12273 vinfos[6].jointtype = 1;
12274 vinfos[6].foffset = j12;
12275 vinfos[6].indices[0] = _ij12[0];
12276 vinfos[6].indices[1] = _ij12[1];
12277 vinfos[6].maxsolutions = _nj12;
12278 std::vector<int> vfree(0);
12279 solutions.AddSolution(vinfos,vfree);
12280 }
12281     }
12282 
12283 }
12284 
12285 }
12286 
12287 }
12288 } while(0);
12289 if( bgotonextstatement )
12290 {
12291 bool bgotonextstatement = true;
12292 do
12293 {
12294 if( 1 )
12295 {
12296 bgotonextstatement=false;
12297 continue; // branch miss [j10]
12298 
12299 }
12300 } while(0);
12301 if( bgotonextstatement )
12302 {
12303 }
12304 }
12305 }
12306 }
12307 }
12308 }
12309 }
12310 }
12311 }
12312 }
12313 }
12314 
12315 } else
12316 {
12317 {
12318 IkReal j10array[1], cj10array[1], sj10array[1];
12319 bool j10valid[1]={false};
12320 _nj10 = 1;
12321 CheckValue<IkReal> x445=IKPowWithIntegerCheck(sj9,-1);
12322 if(!x445.valid){
12323 continue;
12324 }
12325 IkReal x444=x445.value;
12326 CheckValue<IkReal> x446=IKPowWithIntegerCheck(cj8,-1);
12327 if(!x446.valid){
12328 continue;
12329 }
12330 CheckValue<IkReal> x447=IKPowWithIntegerCheck(cj9,-1);
12331 if(!x447.valid){
12332 continue;
12333 }
12334 if( IKabs((x444*(x446.value)*(x447.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x444)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x444*(x446.value)*(x447.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9))))))+IKsqr(((-1.0)*new_r20*x444))-1) <= IKFAST_SINCOS_THRESH )
12335     continue;
12336 j10array[0]=IKatan2((x444*(x446.value)*(x447.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9))))), ((-1.0)*new_r20*x444));
12337 sj10array[0]=IKsin(j10array[0]);
12338 cj10array[0]=IKcos(j10array[0]);
12339 if( j10array[0] > IKPI )
12340 {
12341     j10array[0]-=IK2PI;
12342 }
12343 else if( j10array[0] < -IKPI )
12344 {    j10array[0]+=IK2PI;
12345 }
12346 j10valid[0] = true;
12347 for(int ij10 = 0; ij10 < 1; ++ij10)
12348 {
12349 if( !j10valid[ij10] )
12350 {
12351     continue;
12352 }
12353 _ij10[0] = ij10; _ij10[1] = -1;
12354 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12355 {
12356 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12357 {
12358     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12359 }
12360 }
12361 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12362 {
12363 IkReal evalcond[12];
12364 IkReal x448=IKsin(j10);
12365 IkReal x449=IKcos(j10);
12366 IkReal x450=(cj9*sj8);
12367 IkReal x451=((1.0)*sj9);
12368 IkReal x452=((1.0)*sj8);
12369 IkReal x453=((1.0)*cj8);
12370 IkReal x454=(cj8*new_r00);
12371 IkReal x455=(cj8*new_r01);
12372 IkReal x456=((1.0)*x449);
12373 IkReal x457=(cj9*x448);
12374 IkReal x458=(cj9*x456);
12375 evalcond[0]=(((sj9*x449))+new_r20);
12376 evalcond[1]=(new_r21+(((-1.0)*x448*x451)));
12377 evalcond[2]=(((new_r11*sj8))+x457+x455);
12378 evalcond[3]=((((-1.0)*x448))+(((-1.0)*new_r00*x452))+((cj8*new_r10)));
12379 evalcond[4]=((((-1.0)*new_r01*x452))+((cj8*new_r11))+(((-1.0)*x456)));
12380 evalcond[5]=(((cj8*x457))+new_r01+((sj8*x449)));
12381 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x458))+x454);
12382 evalcond[7]=((((-1.0)*cj9*x449*x453))+new_r00+((sj8*x448)));
12383 evalcond[8]=(((x448*x450))+new_r11+(((-1.0)*x449*x453)));
12384 evalcond[9]=((((-1.0)*x450*x456))+new_r10+(((-1.0)*x448*x453)));
12385 evalcond[10]=((((-1.0)*new_r21*x451))+((new_r11*x450))+((cj9*x455))+x448);
12386 evalcond[11]=(((new_r10*x450))+(((-1.0)*new_r20*x451))+((cj9*x454))+(((-1.0)*x456)));
12387 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
12388 {
12389 continue;
12390 }
12391 }
12392 
12393 {
12394 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12395 vinfos[0].jointtype = 1;
12396 vinfos[0].foffset = j6;
12397 vinfos[0].indices[0] = _ij6[0];
12398 vinfos[0].indices[1] = _ij6[1];
12399 vinfos[0].maxsolutions = _nj6;
12400 vinfos[1].jointtype = 1;
12401 vinfos[1].foffset = j7;
12402 vinfos[1].indices[0] = _ij7[0];
12403 vinfos[1].indices[1] = _ij7[1];
12404 vinfos[1].maxsolutions = _nj7;
12405 vinfos[2].jointtype = 1;
12406 vinfos[2].foffset = j8;
12407 vinfos[2].indices[0] = _ij8[0];
12408 vinfos[2].indices[1] = _ij8[1];
12409 vinfos[2].maxsolutions = _nj8;
12410 vinfos[3].jointtype = 1;
12411 vinfos[3].foffset = j9;
12412 vinfos[3].indices[0] = _ij9[0];
12413 vinfos[3].indices[1] = _ij9[1];
12414 vinfos[3].maxsolutions = _nj9;
12415 vinfos[4].jointtype = 1;
12416 vinfos[4].foffset = j10;
12417 vinfos[4].indices[0] = _ij10[0];
12418 vinfos[4].indices[1] = _ij10[1];
12419 vinfos[4].maxsolutions = _nj10;
12420 vinfos[5].jointtype = 1;
12421 vinfos[5].foffset = j11;
12422 vinfos[5].indices[0] = _ij11[0];
12423 vinfos[5].indices[1] = _ij11[1];
12424 vinfos[5].maxsolutions = _nj11;
12425 vinfos[6].jointtype = 1;
12426 vinfos[6].foffset = j12;
12427 vinfos[6].indices[0] = _ij12[0];
12428 vinfos[6].indices[1] = _ij12[1];
12429 vinfos[6].maxsolutions = _nj12;
12430 std::vector<int> vfree(0);
12431 solutions.AddSolution(vinfos,vfree);
12432 }
12433 }
12434 }
12435 
12436 }
12437 
12438 }
12439 
12440 } else
12441 {
12442 {
12443 IkReal j10array[1], cj10array[1], sj10array[1];
12444 bool j10valid[1]={false};
12445 _nj10 = 1;
12446 CheckValue<IkReal> x460=IKPowWithIntegerCheck(sj9,-1);
12447 if(!x460.valid){
12448 continue;
12449 }
12450 IkReal x459=x460.value;
12451 CheckValue<IkReal> x461=IKPowWithIntegerCheck(sj8,-1);
12452 if(!x461.valid){
12453 continue;
12454 }
12455 if( IKabs((x459*(x461.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x459)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x459*(x461.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9))))))+IKsqr(((-1.0)*new_r20*x459))-1) <= IKFAST_SINCOS_THRESH )
12456     continue;
12457 j10array[0]=IKatan2((x459*(x461.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9))))), ((-1.0)*new_r20*x459));
12458 sj10array[0]=IKsin(j10array[0]);
12459 cj10array[0]=IKcos(j10array[0]);
12460 if( j10array[0] > IKPI )
12461 {
12462     j10array[0]-=IK2PI;
12463 }
12464 else if( j10array[0] < -IKPI )
12465 {    j10array[0]+=IK2PI;
12466 }
12467 j10valid[0] = true;
12468 for(int ij10 = 0; ij10 < 1; ++ij10)
12469 {
12470 if( !j10valid[ij10] )
12471 {
12472     continue;
12473 }
12474 _ij10[0] = ij10; _ij10[1] = -1;
12475 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12476 {
12477 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12478 {
12479     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12480 }
12481 }
12482 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12483 {
12484 IkReal evalcond[12];
12485 IkReal x462=IKsin(j10);
12486 IkReal x463=IKcos(j10);
12487 IkReal x464=(cj9*sj8);
12488 IkReal x465=((1.0)*sj9);
12489 IkReal x466=((1.0)*sj8);
12490 IkReal x467=((1.0)*cj8);
12491 IkReal x468=(cj8*new_r00);
12492 IkReal x469=(cj8*new_r01);
12493 IkReal x470=((1.0)*x463);
12494 IkReal x471=(cj9*x462);
12495 IkReal x472=(cj9*x470);
12496 evalcond[0]=(new_r20+((sj9*x463)));
12497 evalcond[1]=((((-1.0)*x462*x465))+new_r21);
12498 evalcond[2]=(((new_r11*sj8))+x469+x471);
12499 evalcond[3]=((((-1.0)*x462))+((cj8*new_r10))+(((-1.0)*new_r00*x466)));
12500 evalcond[4]=((((-1.0)*x470))+(((-1.0)*new_r01*x466))+((cj8*new_r11)));
12501 evalcond[5]=(((cj8*x471))+new_r01+((sj8*x463)));
12502 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x472))+x468);
12503 evalcond[7]=((((-1.0)*cj9*x463*x467))+new_r00+((sj8*x462)));
12504 evalcond[8]=((((-1.0)*x463*x467))+new_r11+((x462*x464)));
12505 evalcond[9]=((((-1.0)*x462*x467))+(((-1.0)*x464*x470))+new_r10);
12506 evalcond[10]=((((-1.0)*new_r21*x465))+((new_r11*x464))+((cj9*x469))+x462);
12507 evalcond[11]=(((new_r10*x464))+(((-1.0)*new_r20*x465))+((cj9*x468))+(((-1.0)*x470)));
12508 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
12509 {
12510 continue;
12511 }
12512 }
12513 
12514 {
12515 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12516 vinfos[0].jointtype = 1;
12517 vinfos[0].foffset = j6;
12518 vinfos[0].indices[0] = _ij6[0];
12519 vinfos[0].indices[1] = _ij6[1];
12520 vinfos[0].maxsolutions = _nj6;
12521 vinfos[1].jointtype = 1;
12522 vinfos[1].foffset = j7;
12523 vinfos[1].indices[0] = _ij7[0];
12524 vinfos[1].indices[1] = _ij7[1];
12525 vinfos[1].maxsolutions = _nj7;
12526 vinfos[2].jointtype = 1;
12527 vinfos[2].foffset = j8;
12528 vinfos[2].indices[0] = _ij8[0];
12529 vinfos[2].indices[1] = _ij8[1];
12530 vinfos[2].maxsolutions = _nj8;
12531 vinfos[3].jointtype = 1;
12532 vinfos[3].foffset = j9;
12533 vinfos[3].indices[0] = _ij9[0];
12534 vinfos[3].indices[1] = _ij9[1];
12535 vinfos[3].maxsolutions = _nj9;
12536 vinfos[4].jointtype = 1;
12537 vinfos[4].foffset = j10;
12538 vinfos[4].indices[0] = _ij10[0];
12539 vinfos[4].indices[1] = _ij10[1];
12540 vinfos[4].maxsolutions = _nj10;
12541 vinfos[5].jointtype = 1;
12542 vinfos[5].foffset = j11;
12543 vinfos[5].indices[0] = _ij11[0];
12544 vinfos[5].indices[1] = _ij11[1];
12545 vinfos[5].maxsolutions = _nj11;
12546 vinfos[6].jointtype = 1;
12547 vinfos[6].foffset = j12;
12548 vinfos[6].indices[0] = _ij12[0];
12549 vinfos[6].indices[1] = _ij12[1];
12550 vinfos[6].maxsolutions = _nj12;
12551 std::vector<int> vfree(0);
12552 solutions.AddSolution(vinfos,vfree);
12553 }
12554 }
12555 }
12556 
12557 }
12558 
12559 }
12560 
12561 } else
12562 {
12563 {
12564 IkReal j10array[1], cj10array[1], sj10array[1];
12565 bool j10valid[1]={false};
12566 _nj10 = 1;
12567 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
12568 if(!x473.valid){
12569 continue;
12570 }
12571 CheckValue<IkReal> x474=IKPowWithIntegerCheck(IKsign(sj9),-1);
12572 if(!x474.valid){
12573 continue;
12574 }
12575 j10array[0]=((-1.5707963267949)+(x473.value)+(((1.5707963267949)*(x474.value))));
12576 sj10array[0]=IKsin(j10array[0]);
12577 cj10array[0]=IKcos(j10array[0]);
12578 if( j10array[0] > IKPI )
12579 {
12580     j10array[0]-=IK2PI;
12581 }
12582 else if( j10array[0] < -IKPI )
12583 {    j10array[0]+=IK2PI;
12584 }
12585 j10valid[0] = true;
12586 for(int ij10 = 0; ij10 < 1; ++ij10)
12587 {
12588 if( !j10valid[ij10] )
12589 {
12590     continue;
12591 }
12592 _ij10[0] = ij10; _ij10[1] = -1;
12593 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12594 {
12595 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12596 {
12597     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12598 }
12599 }
12600 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12601 {
12602 IkReal evalcond[12];
12603 IkReal x475=IKsin(j10);
12604 IkReal x476=IKcos(j10);
12605 IkReal x477=(cj9*sj8);
12606 IkReal x478=((1.0)*sj9);
12607 IkReal x479=((1.0)*sj8);
12608 IkReal x480=((1.0)*cj8);
12609 IkReal x481=(cj8*new_r00);
12610 IkReal x482=(cj8*new_r01);
12611 IkReal x483=((1.0)*x476);
12612 IkReal x484=(cj9*x475);
12613 IkReal x485=(cj9*x483);
12614 evalcond[0]=(((sj9*x476))+new_r20);
12615 evalcond[1]=((((-1.0)*x475*x478))+new_r21);
12616 evalcond[2]=(((new_r11*sj8))+x482+x484);
12617 evalcond[3]=((((-1.0)*new_r00*x479))+((cj8*new_r10))+(((-1.0)*x475)));
12618 evalcond[4]=((((-1.0)*x483))+(((-1.0)*new_r01*x479))+((cj8*new_r11)));
12619 evalcond[5]=(((sj8*x476))+new_r01+((cj8*x484)));
12620 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x485))+x481);
12621 evalcond[7]=((((-1.0)*cj9*x476*x480))+((sj8*x475))+new_r00);
12622 evalcond[8]=((((-1.0)*x476*x480))+new_r11+((x475*x477)));
12623 evalcond[9]=((((-1.0)*x475*x480))+(((-1.0)*x477*x483))+new_r10);
12624 evalcond[10]=((((-1.0)*new_r21*x478))+((new_r11*x477))+x475+((cj9*x482)));
12625 evalcond[11]=((((-1.0)*x483))+((new_r10*x477))+((cj9*x481))+(((-1.0)*new_r20*x478)));
12626 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
12627 {
12628 continue;
12629 }
12630 }
12631 
12632 {
12633 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12634 vinfos[0].jointtype = 1;
12635 vinfos[0].foffset = j6;
12636 vinfos[0].indices[0] = _ij6[0];
12637 vinfos[0].indices[1] = _ij6[1];
12638 vinfos[0].maxsolutions = _nj6;
12639 vinfos[1].jointtype = 1;
12640 vinfos[1].foffset = j7;
12641 vinfos[1].indices[0] = _ij7[0];
12642 vinfos[1].indices[1] = _ij7[1];
12643 vinfos[1].maxsolutions = _nj7;
12644 vinfos[2].jointtype = 1;
12645 vinfos[2].foffset = j8;
12646 vinfos[2].indices[0] = _ij8[0];
12647 vinfos[2].indices[1] = _ij8[1];
12648 vinfos[2].maxsolutions = _nj8;
12649 vinfos[3].jointtype = 1;
12650 vinfos[3].foffset = j9;
12651 vinfos[3].indices[0] = _ij9[0];
12652 vinfos[3].indices[1] = _ij9[1];
12653 vinfos[3].maxsolutions = _nj9;
12654 vinfos[4].jointtype = 1;
12655 vinfos[4].foffset = j10;
12656 vinfos[4].indices[0] = _ij10[0];
12657 vinfos[4].indices[1] = _ij10[1];
12658 vinfos[4].maxsolutions = _nj10;
12659 vinfos[5].jointtype = 1;
12660 vinfos[5].foffset = j11;
12661 vinfos[5].indices[0] = _ij11[0];
12662 vinfos[5].indices[1] = _ij11[1];
12663 vinfos[5].maxsolutions = _nj11;
12664 vinfos[6].jointtype = 1;
12665 vinfos[6].foffset = j12;
12666 vinfos[6].indices[0] = _ij12[0];
12667 vinfos[6].indices[1] = _ij12[1];
12668 vinfos[6].maxsolutions = _nj12;
12669 std::vector<int> vfree(0);
12670 solutions.AddSolution(vinfos,vfree);
12671 }
12672 }
12673 }
12674 
12675 }
12676 
12677 }
12678 }
12679 }
12680 
12681 }
12682 
12683 }
12684 
12685 } else
12686 {
12687 {
12688 IkReal j10array[1], cj10array[1], sj10array[1];
12689 bool j10valid[1]={false};
12690 _nj10 = 1;
12691 CheckValue<IkReal> x486 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
12692 if(!x486.valid){
12693 continue;
12694 }
12695 CheckValue<IkReal> x487=IKPowWithIntegerCheck(IKsign(sj9),-1);
12696 if(!x487.valid){
12697 continue;
12698 }
12699 j10array[0]=((-1.5707963267949)+(x486.value)+(((1.5707963267949)*(x487.value))));
12700 sj10array[0]=IKsin(j10array[0]);
12701 cj10array[0]=IKcos(j10array[0]);
12702 if( j10array[0] > IKPI )
12703 {
12704     j10array[0]-=IK2PI;
12705 }
12706 else if( j10array[0] < -IKPI )
12707 {    j10array[0]+=IK2PI;
12708 }
12709 j10valid[0] = true;
12710 for(int ij10 = 0; ij10 < 1; ++ij10)
12711 {
12712 if( !j10valid[ij10] )
12713 {
12714     continue;
12715 }
12716 _ij10[0] = ij10; _ij10[1] = -1;
12717 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12718 {
12719 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12720 {
12721     j10valid[iij10]=false; _ij10[1] = iij10; break; 
12722 }
12723 }
12724 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12725 {
12726 IkReal evalcond[2];
12727 evalcond[0]=(((sj9*(IKcos(j10))))+new_r20);
12728 evalcond[1]=((((-1.0)*sj9*(IKsin(j10))))+new_r21);
12729 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  )
12730 {
12731 continue;
12732 }
12733 }
12734 
12735 {
12736 IkReal j8eval[3];
12737 j8eval[0]=sj9;
12738 j8eval[1]=((IKabs(new_r12))+(IKabs(new_r02)));
12739 j8eval[2]=IKsign(sj9);
12740 if( IKabs(j8eval[0]) < 0.0000010000000000  || IKabs(j8eval[1]) < 0.0000010000000000  || IKabs(j8eval[2]) < 0.0000010000000000  )
12741 {
12742 {
12743 IkReal j8eval[2];
12744 j8eval[0]=cj10;
12745 j8eval[1]=sj9;
12746 if( IKabs(j8eval[0]) < 0.0000010000000000  || IKabs(j8eval[1]) < 0.0000010000000000  )
12747 {
12748 {
12749 IkReal evalcond[5];
12750 bool bgotonextstatement = true;
12751 do
12752 {
12753 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j10)))), 6.28318530717959)));
12754 evalcond[1]=new_r20;
12755 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
12756 {
12757 bgotonextstatement=false;
12758 {
12759 IkReal j8array[1], cj8array[1], sj8array[1];
12760 bool j8valid[1]={false};
12761 _nj8 = 1;
12762 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
12763     continue;
12764 j8array[0]=IKatan2(((-1.0)*new_r00), new_r10);
12765 sj8array[0]=IKsin(j8array[0]);
12766 cj8array[0]=IKcos(j8array[0]);
12767 if( j8array[0] > IKPI )
12768 {
12769     j8array[0]-=IK2PI;
12770 }
12771 else if( j8array[0] < -IKPI )
12772 {    j8array[0]+=IK2PI;
12773 }
12774 j8valid[0] = true;
12775 for(int ij8 = 0; ij8 < 1; ++ij8)
12776 {
12777 if( !j8valid[ij8] )
12778 {
12779     continue;
12780 }
12781 _ij8[0] = ij8; _ij8[1] = -1;
12782 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
12783 {
12784 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
12785 {
12786     j8valid[iij8]=false; _ij8[1] = iij8; break; 
12787 }
12788 }
12789 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
12790 {
12791 IkReal evalcond[18];
12792 IkReal x488=IKsin(j8);
12793 IkReal x489=IKcos(j8);
12794 IkReal x490=((1.0)*sj9);
12795 IkReal x491=((1.0)*x488);
12796 IkReal x492=(cj9*x488);
12797 IkReal x493=(new_r11*x488);
12798 IkReal x494=(new_r10*x488);
12799 IkReal x495=(new_r02*x489);
12800 IkReal x496=(cj9*x489);
12801 IkReal x497=(new_r12*x488);
12802 IkReal x498=(new_r01*x489);
12803 IkReal x499=(new_r00*x489);
12804 evalcond[0]=(x488+new_r00);
12805 evalcond[1]=(x496+new_r01);
12806 evalcond[2]=(x492+new_r11);
12807 evalcond[3]=((((-1.0)*x489))+new_r10);
12808 evalcond[4]=((((-1.0)*x489*x490))+new_r02);
12809 evalcond[5]=((((-1.0)*x488*x490))+new_r12);
12810 evalcond[6]=(x499+x494);
12811 evalcond[7]=((((-1.0)*new_r02*x491))+((new_r12*x489)));
12812 evalcond[8]=((((-1.0)*new_r01*x491))+((new_r11*x489)));
12813 evalcond[9]=(cj9+x498+x493);
12814 evalcond[10]=((-1.0)+(((-1.0)*new_r00*x491))+((new_r10*x489)));
12815 evalcond[11]=(((sj9*x499))+((sj9*x494)));
12816 evalcond[12]=(((new_r10*x492))+((new_r00*x496)));
12817 evalcond[13]=((((-1.0)*x490))+x497+x495);
12818 evalcond[14]=(((cj9*new_r21))+((sj9*x498))+((sj9*x493)));
12819 evalcond[15]=((-1.0)+((cj9*new_r22))+((sj9*x495))+((sj9*x497)));
12820 evalcond[16]=(((new_r12*x492))+(((-1.0)*new_r22*x490))+((cj9*x495)));
12821 evalcond[17]=((1.0)+(((-1.0)*new_r21*x490))+((new_r11*x492))+((new_r01*x496)));
12822 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH  )
12823 {
12824 continue;
12825 }
12826 }
12827 
12828 {
12829 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12830 vinfos[0].jointtype = 1;
12831 vinfos[0].foffset = j6;
12832 vinfos[0].indices[0] = _ij6[0];
12833 vinfos[0].indices[1] = _ij6[1];
12834 vinfos[0].maxsolutions = _nj6;
12835 vinfos[1].jointtype = 1;
12836 vinfos[1].foffset = j7;
12837 vinfos[1].indices[0] = _ij7[0];
12838 vinfos[1].indices[1] = _ij7[1];
12839 vinfos[1].maxsolutions = _nj7;
12840 vinfos[2].jointtype = 1;
12841 vinfos[2].foffset = j8;
12842 vinfos[2].indices[0] = _ij8[0];
12843 vinfos[2].indices[1] = _ij8[1];
12844 vinfos[2].maxsolutions = _nj8;
12845 vinfos[3].jointtype = 1;
12846 vinfos[3].foffset = j9;
12847 vinfos[3].indices[0] = _ij9[0];
12848 vinfos[3].indices[1] = _ij9[1];
12849 vinfos[3].maxsolutions = _nj9;
12850 vinfos[4].jointtype = 1;
12851 vinfos[4].foffset = j10;
12852 vinfos[4].indices[0] = _ij10[0];
12853 vinfos[4].indices[1] = _ij10[1];
12854 vinfos[4].maxsolutions = _nj10;
12855 vinfos[5].jointtype = 1;
12856 vinfos[5].foffset = j11;
12857 vinfos[5].indices[0] = _ij11[0];
12858 vinfos[5].indices[1] = _ij11[1];
12859 vinfos[5].maxsolutions = _nj11;
12860 vinfos[6].jointtype = 1;
12861 vinfos[6].foffset = j12;
12862 vinfos[6].indices[0] = _ij12[0];
12863 vinfos[6].indices[1] = _ij12[1];
12864 vinfos[6].maxsolutions = _nj12;
12865 std::vector<int> vfree(0);
12866 solutions.AddSolution(vinfos,vfree);
12867 }
12868 }
12869 }
12870 
12871 }
12872 } while(0);
12873 if( bgotonextstatement )
12874 {
12875 bool bgotonextstatement = true;
12876 do
12877 {
12878 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j10)))), 6.28318530717959)));
12879 evalcond[1]=new_r20;
12880 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
12881 {
12882 bgotonextstatement=false;
12883 {
12884 IkReal j8array[1], cj8array[1], sj8array[1];
12885 bool j8valid[1]={false};
12886 _nj8 = 1;
12887 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
12888     continue;
12889 j8array[0]=IKatan2(new_r00, ((-1.0)*new_r10));
12890 sj8array[0]=IKsin(j8array[0]);
12891 cj8array[0]=IKcos(j8array[0]);
12892 if( j8array[0] > IKPI )
12893 {
12894     j8array[0]-=IK2PI;
12895 }
12896 else if( j8array[0] < -IKPI )
12897 {    j8array[0]+=IK2PI;
12898 }
12899 j8valid[0] = true;
12900 for(int ij8 = 0; ij8 < 1; ++ij8)
12901 {
12902 if( !j8valid[ij8] )
12903 {
12904     continue;
12905 }
12906 _ij8[0] = ij8; _ij8[1] = -1;
12907 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
12908 {
12909 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
12910 {
12911     j8valid[iij8]=false; _ij8[1] = iij8; break; 
12912 }
12913 }
12914 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
12915 {
12916 IkReal evalcond[18];
12917 IkReal x500=IKcos(j8);
12918 IkReal x501=IKsin(j8);
12919 IkReal x502=((1.0)*cj9);
12920 IkReal x503=((1.0)*sj9);
12921 IkReal x504=((1.0)*x501);
12922 IkReal x505=(new_r11*x501);
12923 IkReal x506=(new_r10*x501);
12924 IkReal x507=(new_r02*x500);
12925 IkReal x508=(cj9*x500);
12926 IkReal x509=(new_r12*x501);
12927 IkReal x510=(new_r01*x500);
12928 IkReal x511=(new_r00*x500);
12929 evalcond[0]=(x500+new_r10);
12930 evalcond[1]=((((-1.0)*x504))+new_r00);
12931 evalcond[2]=(new_r02+(((-1.0)*x500*x503)));
12932 evalcond[3]=((((-1.0)*x501*x503))+new_r12);
12933 evalcond[4]=(new_r01+(((-1.0)*x500*x502)));
12934 evalcond[5]=((((-1.0)*x501*x502))+new_r11);
12935 evalcond[6]=(x506+x511);
12936 evalcond[7]=((((-1.0)*new_r02*x504))+((new_r12*x500)));
12937 evalcond[8]=((((-1.0)*new_r01*x504))+((new_r11*x500)));
12938 evalcond[9]=((1.0)+(((-1.0)*new_r00*x504))+((new_r10*x500)));
12939 evalcond[10]=(((sj9*x511))+((sj9*x506)));
12940 evalcond[11]=(((cj9*x506))+((new_r00*x508)));
12941 evalcond[12]=((((-1.0)*x503))+x509+x507);
12942 evalcond[13]=((((-1.0)*x502))+x505+x510);
12943 evalcond[14]=(((cj9*new_r21))+((sj9*x510))+((sj9*x505)));
12944 evalcond[15]=((-1.0)+((cj9*new_r22))+((sj9*x509))+((sj9*x507)));
12945 evalcond[16]=(((cj9*x509))+((cj9*x507))+(((-1.0)*new_r22*x503)));
12946 evalcond[17]=((-1.0)+(sj9*sj9)+((cj9*x505))+((new_r01*x508)));
12947 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH  )
12948 {
12949 continue;
12950 }
12951 }
12952 
12953 {
12954 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12955 vinfos[0].jointtype = 1;
12956 vinfos[0].foffset = j6;
12957 vinfos[0].indices[0] = _ij6[0];
12958 vinfos[0].indices[1] = _ij6[1];
12959 vinfos[0].maxsolutions = _nj6;
12960 vinfos[1].jointtype = 1;
12961 vinfos[1].foffset = j7;
12962 vinfos[1].indices[0] = _ij7[0];
12963 vinfos[1].indices[1] = _ij7[1];
12964 vinfos[1].maxsolutions = _nj7;
12965 vinfos[2].jointtype = 1;
12966 vinfos[2].foffset = j8;
12967 vinfos[2].indices[0] = _ij8[0];
12968 vinfos[2].indices[1] = _ij8[1];
12969 vinfos[2].maxsolutions = _nj8;
12970 vinfos[3].jointtype = 1;
12971 vinfos[3].foffset = j9;
12972 vinfos[3].indices[0] = _ij9[0];
12973 vinfos[3].indices[1] = _ij9[1];
12974 vinfos[3].maxsolutions = _nj9;
12975 vinfos[4].jointtype = 1;
12976 vinfos[4].foffset = j10;
12977 vinfos[4].indices[0] = _ij10[0];
12978 vinfos[4].indices[1] = _ij10[1];
12979 vinfos[4].maxsolutions = _nj10;
12980 vinfos[5].jointtype = 1;
12981 vinfos[5].foffset = j11;
12982 vinfos[5].indices[0] = _ij11[0];
12983 vinfos[5].indices[1] = _ij11[1];
12984 vinfos[5].maxsolutions = _nj11;
12985 vinfos[6].jointtype = 1;
12986 vinfos[6].foffset = j12;
12987 vinfos[6].indices[0] = _ij12[0];
12988 vinfos[6].indices[1] = _ij12[1];
12989 vinfos[6].maxsolutions = _nj12;
12990 std::vector<int> vfree(0);
12991 solutions.AddSolution(vinfos,vfree);
12992 }
12993 }
12994 }
12995 
12996 }
12997 } while(0);
12998 if( bgotonextstatement )
12999 {
13000 bool bgotonextstatement = true;
13001 do
13002 {
13003 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
13004 evalcond[1]=new_r20;
13005 evalcond[2]=new_r02;
13006 evalcond[3]=new_r12;
13007 evalcond[4]=new_r21;
13008 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
13009 {
13010 bgotonextstatement=false;
13011 {
13012 IkReal j8array[1], cj8array[1], sj8array[1];
13013 bool j8valid[1]={false};
13014 _nj8 = 1;
13015 IkReal x512=((1.0)*sj10);
13016 if( IKabs(((((-1.0)*cj10*new_r01))+(((-1.0)*new_r00*x512)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj10*new_r00))+(((-1.0)*new_r01*x512)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj10*new_r01))+(((-1.0)*new_r00*x512))))+IKsqr((((cj10*new_r00))+(((-1.0)*new_r01*x512))))-1) <= IKFAST_SINCOS_THRESH )
13017     continue;
13018 j8array[0]=IKatan2(((((-1.0)*cj10*new_r01))+(((-1.0)*new_r00*x512))), (((cj10*new_r00))+(((-1.0)*new_r01*x512))));
13019 sj8array[0]=IKsin(j8array[0]);
13020 cj8array[0]=IKcos(j8array[0]);
13021 if( j8array[0] > IKPI )
13022 {
13023     j8array[0]-=IK2PI;
13024 }
13025 else if( j8array[0] < -IKPI )
13026 {    j8array[0]+=IK2PI;
13027 }
13028 j8valid[0] = true;
13029 for(int ij8 = 0; ij8 < 1; ++ij8)
13030 {
13031 if( !j8valid[ij8] )
13032 {
13033     continue;
13034 }
13035 _ij8[0] = ij8; _ij8[1] = -1;
13036 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
13037 {
13038 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13039 {
13040     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13041 }
13042 }
13043 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13044 {
13045 IkReal evalcond[8];
13046 IkReal x513=IKsin(j8);
13047 IkReal x514=IKcos(j8);
13048 IkReal x515=((1.0)*cj10);
13049 IkReal x516=((1.0)*sj10);
13050 IkReal x517=(sj10*x513);
13051 IkReal x518=((1.0)*x513);
13052 IkReal x519=(x514*x515);
13053 evalcond[0]=(((new_r01*x514))+((new_r11*x513))+sj10);
13054 evalcond[1]=(((sj10*x514))+((cj10*x513))+new_r01);
13055 evalcond[2]=(x517+new_r00+(((-1.0)*x519)));
13056 evalcond[3]=(x517+new_r11+(((-1.0)*x519)));
13057 evalcond[4]=(((new_r00*x514))+((new_r10*x513))+(((-1.0)*x515)));
13058 evalcond[5]=((((-1.0)*x514*x516))+(((-1.0)*x513*x515))+new_r10);
13059 evalcond[6]=(((new_r10*x514))+(((-1.0)*new_r00*x518))+(((-1.0)*x516)));
13060 evalcond[7]=(((new_r11*x514))+(((-1.0)*new_r01*x518))+(((-1.0)*x515)));
13061 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
13062 {
13063 continue;
13064 }
13065 }
13066 
13067 {
13068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13069 vinfos[0].jointtype = 1;
13070 vinfos[0].foffset = j6;
13071 vinfos[0].indices[0] = _ij6[0];
13072 vinfos[0].indices[1] = _ij6[1];
13073 vinfos[0].maxsolutions = _nj6;
13074 vinfos[1].jointtype = 1;
13075 vinfos[1].foffset = j7;
13076 vinfos[1].indices[0] = _ij7[0];
13077 vinfos[1].indices[1] = _ij7[1];
13078 vinfos[1].maxsolutions = _nj7;
13079 vinfos[2].jointtype = 1;
13080 vinfos[2].foffset = j8;
13081 vinfos[2].indices[0] = _ij8[0];
13082 vinfos[2].indices[1] = _ij8[1];
13083 vinfos[2].maxsolutions = _nj8;
13084 vinfos[3].jointtype = 1;
13085 vinfos[3].foffset = j9;
13086 vinfos[3].indices[0] = _ij9[0];
13087 vinfos[3].indices[1] = _ij9[1];
13088 vinfos[3].maxsolutions = _nj9;
13089 vinfos[4].jointtype = 1;
13090 vinfos[4].foffset = j10;
13091 vinfos[4].indices[0] = _ij10[0];
13092 vinfos[4].indices[1] = _ij10[1];
13093 vinfos[4].maxsolutions = _nj10;
13094 vinfos[5].jointtype = 1;
13095 vinfos[5].foffset = j11;
13096 vinfos[5].indices[0] = _ij11[0];
13097 vinfos[5].indices[1] = _ij11[1];
13098 vinfos[5].maxsolutions = _nj11;
13099 vinfos[6].jointtype = 1;
13100 vinfos[6].foffset = j12;
13101 vinfos[6].indices[0] = _ij12[0];
13102 vinfos[6].indices[1] = _ij12[1];
13103 vinfos[6].maxsolutions = _nj12;
13104 std::vector<int> vfree(0);
13105 solutions.AddSolution(vinfos,vfree);
13106 }
13107 }
13108 }
13109 
13110 }
13111 } while(0);
13112 if( bgotonextstatement )
13113 {
13114 bool bgotonextstatement = true;
13115 do
13116 {
13117 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
13118 evalcond[1]=new_r20;
13119 evalcond[2]=new_r02;
13120 evalcond[3]=new_r12;
13121 evalcond[4]=new_r21;
13122 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
13123 {
13124 bgotonextstatement=false;
13125 {
13126 IkReal j8array[1], cj8array[1], sj8array[1];
13127 bool j8valid[1]={false};
13128 _nj8 = 1;
13129 IkReal x520=((1.0)*new_r00);
13130 if( IKabs(((((-1.0)*cj10*new_r01))+(((-1.0)*sj10*x520)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj10*x520))+((new_r01*sj10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj10*new_r01))+(((-1.0)*sj10*x520))))+IKsqr(((((-1.0)*cj10*x520))+((new_r01*sj10))))-1) <= IKFAST_SINCOS_THRESH )
13131     continue;
13132 j8array[0]=IKatan2(((((-1.0)*cj10*new_r01))+(((-1.0)*sj10*x520))), ((((-1.0)*cj10*x520))+((new_r01*sj10))));
13133 sj8array[0]=IKsin(j8array[0]);
13134 cj8array[0]=IKcos(j8array[0]);
13135 if( j8array[0] > IKPI )
13136 {
13137     j8array[0]-=IK2PI;
13138 }
13139 else if( j8array[0] < -IKPI )
13140 {    j8array[0]+=IK2PI;
13141 }
13142 j8valid[0] = true;
13143 for(int ij8 = 0; ij8 < 1; ++ij8)
13144 {
13145 if( !j8valid[ij8] )
13146 {
13147     continue;
13148 }
13149 _ij8[0] = ij8; _ij8[1] = -1;
13150 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
13151 {
13152 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13153 {
13154     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13155 }
13156 }
13157 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13158 {
13159 IkReal evalcond[8];
13160 IkReal x521=IKcos(j8);
13161 IkReal x522=IKsin(j8);
13162 IkReal x523=((1.0)*sj10);
13163 IkReal x524=((1.0)*cj10);
13164 IkReal x525=(cj10*x522);
13165 IkReal x526=((1.0)*x522);
13166 IkReal x527=(x521*x523);
13167 evalcond[0]=(((new_r10*x522))+cj10+((new_r00*x521)));
13168 evalcond[1]=(((sj10*x522))+((cj10*x521))+new_r00);
13169 evalcond[2]=((((-1.0)*x527))+x525+new_r01);
13170 evalcond[3]=((((-1.0)*x527))+x525+new_r10);
13171 evalcond[4]=(((new_r01*x521))+((new_r11*x522))+(((-1.0)*x523)));
13172 evalcond[5]=((((-1.0)*x521*x524))+(((-1.0)*x522*x523))+new_r11);
13173 evalcond[6]=((((-1.0)*new_r00*x526))+(((-1.0)*x523))+((new_r10*x521)));
13174 evalcond[7]=(((new_r11*x521))+(((-1.0)*x524))+(((-1.0)*new_r01*x526)));
13175 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
13176 {
13177 continue;
13178 }
13179 }
13180 
13181 {
13182 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13183 vinfos[0].jointtype = 1;
13184 vinfos[0].foffset = j6;
13185 vinfos[0].indices[0] = _ij6[0];
13186 vinfos[0].indices[1] = _ij6[1];
13187 vinfos[0].maxsolutions = _nj6;
13188 vinfos[1].jointtype = 1;
13189 vinfos[1].foffset = j7;
13190 vinfos[1].indices[0] = _ij7[0];
13191 vinfos[1].indices[1] = _ij7[1];
13192 vinfos[1].maxsolutions = _nj7;
13193 vinfos[2].jointtype = 1;
13194 vinfos[2].foffset = j8;
13195 vinfos[2].indices[0] = _ij8[0];
13196 vinfos[2].indices[1] = _ij8[1];
13197 vinfos[2].maxsolutions = _nj8;
13198 vinfos[3].jointtype = 1;
13199 vinfos[3].foffset = j9;
13200 vinfos[3].indices[0] = _ij9[0];
13201 vinfos[3].indices[1] = _ij9[1];
13202 vinfos[3].maxsolutions = _nj9;
13203 vinfos[4].jointtype = 1;
13204 vinfos[4].foffset = j10;
13205 vinfos[4].indices[0] = _ij10[0];
13206 vinfos[4].indices[1] = _ij10[1];
13207 vinfos[4].maxsolutions = _nj10;
13208 vinfos[5].jointtype = 1;
13209 vinfos[5].foffset = j11;
13210 vinfos[5].indices[0] = _ij11[0];
13211 vinfos[5].indices[1] = _ij11[1];
13212 vinfos[5].maxsolutions = _nj11;
13213 vinfos[6].jointtype = 1;
13214 vinfos[6].foffset = j12;
13215 vinfos[6].indices[0] = _ij12[0];
13216 vinfos[6].indices[1] = _ij12[1];
13217 vinfos[6].maxsolutions = _nj12;
13218 std::vector<int> vfree(0);
13219 solutions.AddSolution(vinfos,vfree);
13220 }
13221 }
13222 }
13223 
13224 }
13225 } while(0);
13226 if( bgotonextstatement )
13227 {
13228 bool bgotonextstatement = true;
13229 do
13230 {
13231 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
13232 if( IKabs(evalcond[0]) < 0.0000050000000000  )
13233 {
13234 bgotonextstatement=false;
13235 {
13236 IkReal j8eval[1];
13237 new_r02=0;
13238 new_r12=0;
13239 new_r20=0;
13240 new_r21=0;
13241 j8eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13242 if( IKabs(j8eval[0]) < 0.0000010000000000  )
13243 {
13244 {
13245 IkReal j8eval[1];
13246 new_r02=0;
13247 new_r12=0;
13248 new_r20=0;
13249 new_r21=0;
13250 j8eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
13251 if( IKabs(j8eval[0]) < 0.0000010000000000  )
13252 {
13253 {
13254 IkReal j8eval[1];
13255 new_r02=0;
13256 new_r12=0;
13257 new_r20=0;
13258 new_r21=0;
13259 j8eval[0]=((IKabs((new_r11*new_r22)))+(IKabs((new_r01*new_r22))));
13260 if( IKabs(j8eval[0]) < 0.0000010000000000  )
13261 {
13262 continue; // no branches [j8]
13263 
13264 } else
13265 {
13266 {
13267 IkReal j8array[2], cj8array[2], sj8array[2];
13268 bool j8valid[2]={false};
13269 _nj8 = 2;
13270 CheckValue<IkReal> x529 = IKatan2WithCheck(IkReal((new_r01*new_r22)),IkReal((new_r11*new_r22)),IKFAST_ATAN2_MAGTHRESH);
13271 if(!x529.valid){
13272 continue;
13273 }
13274 IkReal x528=x529.value;
13275 j8array[0]=((-1.0)*x528);
13276 sj8array[0]=IKsin(j8array[0]);
13277 cj8array[0]=IKcos(j8array[0]);
13278 j8array[1]=((3.14159265358979)+(((-1.0)*x528)));
13279 sj8array[1]=IKsin(j8array[1]);
13280 cj8array[1]=IKcos(j8array[1]);
13281 if( j8array[0] > IKPI )
13282 {
13283     j8array[0]-=IK2PI;
13284 }
13285 else if( j8array[0] < -IKPI )
13286 {    j8array[0]+=IK2PI;
13287 }
13288 j8valid[0] = true;
13289 if( j8array[1] > IKPI )
13290 {
13291     j8array[1]-=IK2PI;
13292 }
13293 else if( j8array[1] < -IKPI )
13294 {    j8array[1]+=IK2PI;
13295 }
13296 j8valid[1] = true;
13297 for(int ij8 = 0; ij8 < 2; ++ij8)
13298 {
13299 if( !j8valid[ij8] )
13300 {
13301     continue;
13302 }
13303 _ij8[0] = ij8; _ij8[1] = -1;
13304 for(int iij8 = ij8+1; iij8 < 2; ++iij8)
13305 {
13306 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13307 {
13308     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13309 }
13310 }
13311 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13312 {
13313 IkReal evalcond[5];
13314 IkReal x530=IKsin(j8);
13315 IkReal x531=IKcos(j8);
13316 IkReal x532=(new_r00*x531);
13317 IkReal x533=((1.0)*x530);
13318 IkReal x534=(new_r10*x530);
13319 evalcond[0]=(((new_r01*x531))+((new_r11*x530)));
13320 evalcond[1]=(x532+x534);
13321 evalcond[2]=((((-1.0)*new_r00*x533))+((new_r10*x531)));
13322 evalcond[3]=(((new_r11*x531))+(((-1.0)*new_r01*x533)));
13323 evalcond[4]=(((new_r22*x532))+((new_r22*x534)));
13324 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
13325 {
13326 continue;
13327 }
13328 }
13329 
13330 {
13331 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13332 vinfos[0].jointtype = 1;
13333 vinfos[0].foffset = j6;
13334 vinfos[0].indices[0] = _ij6[0];
13335 vinfos[0].indices[1] = _ij6[1];
13336 vinfos[0].maxsolutions = _nj6;
13337 vinfos[1].jointtype = 1;
13338 vinfos[1].foffset = j7;
13339 vinfos[1].indices[0] = _ij7[0];
13340 vinfos[1].indices[1] = _ij7[1];
13341 vinfos[1].maxsolutions = _nj7;
13342 vinfos[2].jointtype = 1;
13343 vinfos[2].foffset = j8;
13344 vinfos[2].indices[0] = _ij8[0];
13345 vinfos[2].indices[1] = _ij8[1];
13346 vinfos[2].maxsolutions = _nj8;
13347 vinfos[3].jointtype = 1;
13348 vinfos[3].foffset = j9;
13349 vinfos[3].indices[0] = _ij9[0];
13350 vinfos[3].indices[1] = _ij9[1];
13351 vinfos[3].maxsolutions = _nj9;
13352 vinfos[4].jointtype = 1;
13353 vinfos[4].foffset = j10;
13354 vinfos[4].indices[0] = _ij10[0];
13355 vinfos[4].indices[1] = _ij10[1];
13356 vinfos[4].maxsolutions = _nj10;
13357 vinfos[5].jointtype = 1;
13358 vinfos[5].foffset = j11;
13359 vinfos[5].indices[0] = _ij11[0];
13360 vinfos[5].indices[1] = _ij11[1];
13361 vinfos[5].maxsolutions = _nj11;
13362 vinfos[6].jointtype = 1;
13363 vinfos[6].foffset = j12;
13364 vinfos[6].indices[0] = _ij12[0];
13365 vinfos[6].indices[1] = _ij12[1];
13366 vinfos[6].maxsolutions = _nj12;
13367 std::vector<int> vfree(0);
13368 solutions.AddSolution(vinfos,vfree);
13369 }
13370 }
13371 }
13372 
13373 }
13374 
13375 }
13376 
13377 } else
13378 {
13379 {
13380 IkReal j8array[2], cj8array[2], sj8array[2];
13381 bool j8valid[2]={false};
13382 _nj8 = 2;
13383 CheckValue<IkReal> x536 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13384 if(!x536.valid){
13385 continue;
13386 }
13387 IkReal x535=x536.value;
13388 j8array[0]=((-1.0)*x535);
13389 sj8array[0]=IKsin(j8array[0]);
13390 cj8array[0]=IKcos(j8array[0]);
13391 j8array[1]=((3.14159265358979)+(((-1.0)*x535)));
13392 sj8array[1]=IKsin(j8array[1]);
13393 cj8array[1]=IKcos(j8array[1]);
13394 if( j8array[0] > IKPI )
13395 {
13396     j8array[0]-=IK2PI;
13397 }
13398 else if( j8array[0] < -IKPI )
13399 {    j8array[0]+=IK2PI;
13400 }
13401 j8valid[0] = true;
13402 if( j8array[1] > IKPI )
13403 {
13404     j8array[1]-=IK2PI;
13405 }
13406 else if( j8array[1] < -IKPI )
13407 {    j8array[1]+=IK2PI;
13408 }
13409 j8valid[1] = true;
13410 for(int ij8 = 0; ij8 < 2; ++ij8)
13411 {
13412 if( !j8valid[ij8] )
13413 {
13414     continue;
13415 }
13416 _ij8[0] = ij8; _ij8[1] = -1;
13417 for(int iij8 = ij8+1; iij8 < 2; ++iij8)
13418 {
13419 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13420 {
13421     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13422 }
13423 }
13424 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13425 {
13426 IkReal evalcond[5];
13427 IkReal x537=IKcos(j8);
13428 IkReal x538=IKsin(j8);
13429 IkReal x539=(new_r01*x537);
13430 IkReal x540=(new_r11*x538);
13431 IkReal x541=((1.0)*x538);
13432 evalcond[0]=(x540+x539);
13433 evalcond[1]=((((-1.0)*new_r00*x541))+((new_r10*x537)));
13434 evalcond[2]=(((new_r11*x537))+(((-1.0)*new_r01*x541)));
13435 evalcond[3]=(((new_r22*x540))+((new_r22*x539)));
13436 evalcond[4]=(((new_r00*new_r22*x537))+((new_r10*new_r22*x538)));
13437 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
13438 {
13439 continue;
13440 }
13441 }
13442 
13443 {
13444 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13445 vinfos[0].jointtype = 1;
13446 vinfos[0].foffset = j6;
13447 vinfos[0].indices[0] = _ij6[0];
13448 vinfos[0].indices[1] = _ij6[1];
13449 vinfos[0].maxsolutions = _nj6;
13450 vinfos[1].jointtype = 1;
13451 vinfos[1].foffset = j7;
13452 vinfos[1].indices[0] = _ij7[0];
13453 vinfos[1].indices[1] = _ij7[1];
13454 vinfos[1].maxsolutions = _nj7;
13455 vinfos[2].jointtype = 1;
13456 vinfos[2].foffset = j8;
13457 vinfos[2].indices[0] = _ij8[0];
13458 vinfos[2].indices[1] = _ij8[1];
13459 vinfos[2].maxsolutions = _nj8;
13460 vinfos[3].jointtype = 1;
13461 vinfos[3].foffset = j9;
13462 vinfos[3].indices[0] = _ij9[0];
13463 vinfos[3].indices[1] = _ij9[1];
13464 vinfos[3].maxsolutions = _nj9;
13465 vinfos[4].jointtype = 1;
13466 vinfos[4].foffset = j10;
13467 vinfos[4].indices[0] = _ij10[0];
13468 vinfos[4].indices[1] = _ij10[1];
13469 vinfos[4].maxsolutions = _nj10;
13470 vinfos[5].jointtype = 1;
13471 vinfos[5].foffset = j11;
13472 vinfos[5].indices[0] = _ij11[0];
13473 vinfos[5].indices[1] = _ij11[1];
13474 vinfos[5].maxsolutions = _nj11;
13475 vinfos[6].jointtype = 1;
13476 vinfos[6].foffset = j12;
13477 vinfos[6].indices[0] = _ij12[0];
13478 vinfos[6].indices[1] = _ij12[1];
13479 vinfos[6].maxsolutions = _nj12;
13480 std::vector<int> vfree(0);
13481 solutions.AddSolution(vinfos,vfree);
13482 }
13483 }
13484 }
13485 
13486 }
13487 
13488 }
13489 
13490 } else
13491 {
13492 {
13493 IkReal j8array[2], cj8array[2], sj8array[2];
13494 bool j8valid[2]={false};
13495 _nj8 = 2;
13496 CheckValue<IkReal> x543 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13497 if(!x543.valid){
13498 continue;
13499 }
13500 IkReal x542=x543.value;
13501 j8array[0]=((-1.0)*x542);
13502 sj8array[0]=IKsin(j8array[0]);
13503 cj8array[0]=IKcos(j8array[0]);
13504 j8array[1]=((3.14159265358979)+(((-1.0)*x542)));
13505 sj8array[1]=IKsin(j8array[1]);
13506 cj8array[1]=IKcos(j8array[1]);
13507 if( j8array[0] > IKPI )
13508 {
13509     j8array[0]-=IK2PI;
13510 }
13511 else if( j8array[0] < -IKPI )
13512 {    j8array[0]+=IK2PI;
13513 }
13514 j8valid[0] = true;
13515 if( j8array[1] > IKPI )
13516 {
13517     j8array[1]-=IK2PI;
13518 }
13519 else if( j8array[1] < -IKPI )
13520 {    j8array[1]+=IK2PI;
13521 }
13522 j8valid[1] = true;
13523 for(int ij8 = 0; ij8 < 2; ++ij8)
13524 {
13525 if( !j8valid[ij8] )
13526 {
13527     continue;
13528 }
13529 _ij8[0] = ij8; _ij8[1] = -1;
13530 for(int iij8 = ij8+1; iij8 < 2; ++iij8)
13531 {
13532 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13533 {
13534     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13535 }
13536 }
13537 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13538 {
13539 IkReal evalcond[5];
13540 IkReal x544=IKcos(j8);
13541 IkReal x545=IKsin(j8);
13542 IkReal x546=(new_r22*x544);
13543 IkReal x547=(new_r22*x545);
13544 IkReal x548=((1.0)*x545);
13545 evalcond[0]=(((new_r00*x544))+((new_r10*x545)));
13546 evalcond[1]=((((-1.0)*new_r00*x548))+((new_r10*x544)));
13547 evalcond[2]=(((new_r11*x544))+(((-1.0)*new_r01*x548)));
13548 evalcond[3]=(((new_r11*x547))+((new_r01*x546)));
13549 evalcond[4]=(((new_r00*x546))+((new_r10*x547)));
13550 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  )
13551 {
13552 continue;
13553 }
13554 }
13555 
13556 {
13557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13558 vinfos[0].jointtype = 1;
13559 vinfos[0].foffset = j6;
13560 vinfos[0].indices[0] = _ij6[0];
13561 vinfos[0].indices[1] = _ij6[1];
13562 vinfos[0].maxsolutions = _nj6;
13563 vinfos[1].jointtype = 1;
13564 vinfos[1].foffset = j7;
13565 vinfos[1].indices[0] = _ij7[0];
13566 vinfos[1].indices[1] = _ij7[1];
13567 vinfos[1].maxsolutions = _nj7;
13568 vinfos[2].jointtype = 1;
13569 vinfos[2].foffset = j8;
13570 vinfos[2].indices[0] = _ij8[0];
13571 vinfos[2].indices[1] = _ij8[1];
13572 vinfos[2].maxsolutions = _nj8;
13573 vinfos[3].jointtype = 1;
13574 vinfos[3].foffset = j9;
13575 vinfos[3].indices[0] = _ij9[0];
13576 vinfos[3].indices[1] = _ij9[1];
13577 vinfos[3].maxsolutions = _nj9;
13578 vinfos[4].jointtype = 1;
13579 vinfos[4].foffset = j10;
13580 vinfos[4].indices[0] = _ij10[0];
13581 vinfos[4].indices[1] = _ij10[1];
13582 vinfos[4].maxsolutions = _nj10;
13583 vinfos[5].jointtype = 1;
13584 vinfos[5].foffset = j11;
13585 vinfos[5].indices[0] = _ij11[0];
13586 vinfos[5].indices[1] = _ij11[1];
13587 vinfos[5].maxsolutions = _nj11;
13588 vinfos[6].jointtype = 1;
13589 vinfos[6].foffset = j12;
13590 vinfos[6].indices[0] = _ij12[0];
13591 vinfos[6].indices[1] = _ij12[1];
13592 vinfos[6].maxsolutions = _nj12;
13593 std::vector<int> vfree(0);
13594 solutions.AddSolution(vinfos,vfree);
13595 }
13596 }
13597 }
13598 
13599 }
13600 
13601 }
13602 
13603 }
13604 } while(0);
13605 if( bgotonextstatement )
13606 {
13607 bool bgotonextstatement = true;
13608 do
13609 {
13610 if( 1 )
13611 {
13612 bgotonextstatement=false;
13613 continue; // branch miss [j8]
13614 
13615 }
13616 } while(0);
13617 if( bgotonextstatement )
13618 {
13619 }
13620 }
13621 }
13622 }
13623 }
13624 }
13625 }
13626 
13627 } else
13628 {
13629 {
13630 IkReal j8array[1], cj8array[1], sj8array[1];
13631 bool j8valid[1]={false};
13632 _nj8 = 1;
13633 CheckValue<IkReal> x550=IKPowWithIntegerCheck(sj9,-1);
13634 if(!x550.valid){
13635 continue;
13636 }
13637 IkReal x549=x550.value;
13638 CheckValue<IkReal> x551=IKPowWithIntegerCheck(cj10,-1);
13639 if(!x551.valid){
13640 continue;
13641 }
13642 if( IKabs((x549*(x551.value)*(((((-1.0)*cj9*new_r02*sj10))+(((-1.0)*new_r01*sj9)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r02*x549)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x549*(x551.value)*(((((-1.0)*cj9*new_r02*sj10))+(((-1.0)*new_r01*sj9))))))+IKsqr((new_r02*x549))-1) <= IKFAST_SINCOS_THRESH )
13643     continue;
13644 j8array[0]=IKatan2((x549*(x551.value)*(((((-1.0)*cj9*new_r02*sj10))+(((-1.0)*new_r01*sj9))))), (new_r02*x549));
13645 sj8array[0]=IKsin(j8array[0]);
13646 cj8array[0]=IKcos(j8array[0]);
13647 if( j8array[0] > IKPI )
13648 {
13649     j8array[0]-=IK2PI;
13650 }
13651 else if( j8array[0] < -IKPI )
13652 {    j8array[0]+=IK2PI;
13653 }
13654 j8valid[0] = true;
13655 for(int ij8 = 0; ij8 < 1; ++ij8)
13656 {
13657 if( !j8valid[ij8] )
13658 {
13659     continue;
13660 }
13661 _ij8[0] = ij8; _ij8[1] = -1;
13662 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
13663 {
13664 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13665 {
13666     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13667 }
13668 }
13669 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13670 {
13671 IkReal evalcond[18];
13672 IkReal x552=IKcos(j8);
13673 IkReal x553=IKsin(j8);
13674 IkReal x554=((1.0)*cj10);
13675 IkReal x555=((1.0)*sj9);
13676 IkReal x556=((1.0)*sj10);
13677 IkReal x557=((1.0)*x553);
13678 IkReal x558=(sj10*x553);
13679 IkReal x559=(new_r11*x553);
13680 IkReal x560=(new_r10*x553);
13681 IkReal x561=(new_r02*x552);
13682 IkReal x562=(cj9*x552);
13683 IkReal x563=(new_r12*x553);
13684 IkReal x564=(new_r01*x552);
13685 IkReal x565=(new_r00*x552);
13686 evalcond[0]=((((-1.0)*x552*x555))+new_r02);
13687 evalcond[1]=((((-1.0)*x553*x555))+new_r12);
13688 evalcond[2]=(((new_r12*x552))+(((-1.0)*new_r02*x557)));
13689 evalcond[3]=(((cj10*x553))+((sj10*x562))+new_r01);
13690 evalcond[4]=((((-1.0)*x555))+x563+x561);
13691 evalcond[5]=(((cj9*sj10))+x559+x564);
13692 evalcond[6]=(x558+(((-1.0)*x554*x562))+new_r00);
13693 evalcond[7]=(((cj9*x558))+(((-1.0)*x552*x554))+new_r11);
13694 evalcond[8]=(((new_r10*x552))+(((-1.0)*new_r00*x557))+(((-1.0)*x556)));
13695 evalcond[9]=(((new_r11*x552))+(((-1.0)*new_r01*x557))+(((-1.0)*x554)));
13696 evalcond[10]=((((-1.0)*cj9*x554))+x565+x560);
13697 evalcond[11]=((((-1.0)*x552*x556))+new_r10+(((-1.0)*cj9*x553*x554)));
13698 evalcond[12]=(((cj9*new_r20))+((sj9*x565))+((sj9*x560)));
13699 evalcond[13]=(((cj9*new_r21))+((sj9*x559))+((sj9*x564)));
13700 evalcond[14]=((-1.0)+((cj9*new_r22))+((sj9*x561))+((sj9*x563)));
13701 evalcond[15]=((((-1.0)*new_r22*x555))+((cj9*x561))+((cj9*x563)));
13702 evalcond[16]=((((-1.0)*new_r21*x555))+((new_r01*x562))+((cj9*x559))+sj10);
13703 evalcond[17]=(((new_r00*x562))+(((-1.0)*x554))+(((-1.0)*new_r20*x555))+((cj9*x560)));
13704 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH  )
13705 {
13706 continue;
13707 }
13708 }
13709 
13710 {
13711 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13712 vinfos[0].jointtype = 1;
13713 vinfos[0].foffset = j6;
13714 vinfos[0].indices[0] = _ij6[0];
13715 vinfos[0].indices[1] = _ij6[1];
13716 vinfos[0].maxsolutions = _nj6;
13717 vinfos[1].jointtype = 1;
13718 vinfos[1].foffset = j7;
13719 vinfos[1].indices[0] = _ij7[0];
13720 vinfos[1].indices[1] = _ij7[1];
13721 vinfos[1].maxsolutions = _nj7;
13722 vinfos[2].jointtype = 1;
13723 vinfos[2].foffset = j8;
13724 vinfos[2].indices[0] = _ij8[0];
13725 vinfos[2].indices[1] = _ij8[1];
13726 vinfos[2].maxsolutions = _nj8;
13727 vinfos[3].jointtype = 1;
13728 vinfos[3].foffset = j9;
13729 vinfos[3].indices[0] = _ij9[0];
13730 vinfos[3].indices[1] = _ij9[1];
13731 vinfos[3].maxsolutions = _nj9;
13732 vinfos[4].jointtype = 1;
13733 vinfos[4].foffset = j10;
13734 vinfos[4].indices[0] = _ij10[0];
13735 vinfos[4].indices[1] = _ij10[1];
13736 vinfos[4].maxsolutions = _nj10;
13737 vinfos[5].jointtype = 1;
13738 vinfos[5].foffset = j11;
13739 vinfos[5].indices[0] = _ij11[0];
13740 vinfos[5].indices[1] = _ij11[1];
13741 vinfos[5].maxsolutions = _nj11;
13742 vinfos[6].jointtype = 1;
13743 vinfos[6].foffset = j12;
13744 vinfos[6].indices[0] = _ij12[0];
13745 vinfos[6].indices[1] = _ij12[1];
13746 vinfos[6].maxsolutions = _nj12;
13747 std::vector<int> vfree(0);
13748 solutions.AddSolution(vinfos,vfree);
13749 }
13750 }
13751 }
13752 
13753 }
13754 
13755 }
13756 
13757 } else
13758 {
13759 {
13760 IkReal j8array[1], cj8array[1], sj8array[1];
13761 bool j8valid[1]={false};
13762 _nj8 = 1;
13763 CheckValue<IkReal> x566 = IKatan2WithCheck(IkReal(new_r12),IkReal(new_r02),IKFAST_ATAN2_MAGTHRESH);
13764 if(!x566.valid){
13765 continue;
13766 }
13767 CheckValue<IkReal> x567=IKPowWithIntegerCheck(IKsign(sj9),-1);
13768 if(!x567.valid){
13769 continue;
13770 }
13771 j8array[0]=((-1.5707963267949)+(x566.value)+(((1.5707963267949)*(x567.value))));
13772 sj8array[0]=IKsin(j8array[0]);
13773 cj8array[0]=IKcos(j8array[0]);
13774 if( j8array[0] > IKPI )
13775 {
13776     j8array[0]-=IK2PI;
13777 }
13778 else if( j8array[0] < -IKPI )
13779 {    j8array[0]+=IK2PI;
13780 }
13781 j8valid[0] = true;
13782 for(int ij8 = 0; ij8 < 1; ++ij8)
13783 {
13784 if( !j8valid[ij8] )
13785 {
13786     continue;
13787 }
13788 _ij8[0] = ij8; _ij8[1] = -1;
13789 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
13790 {
13791 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13792 {
13793     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13794 }
13795 }
13796 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13797 {
13798 IkReal evalcond[18];
13799 IkReal x568=IKcos(j8);
13800 IkReal x569=IKsin(j8);
13801 IkReal x570=((1.0)*cj10);
13802 IkReal x571=((1.0)*sj9);
13803 IkReal x572=((1.0)*sj10);
13804 IkReal x573=((1.0)*x569);
13805 IkReal x574=(sj10*x569);
13806 IkReal x575=(new_r11*x569);
13807 IkReal x576=(new_r10*x569);
13808 IkReal x577=(new_r02*x568);
13809 IkReal x578=(cj9*x568);
13810 IkReal x579=(new_r12*x569);
13811 IkReal x580=(new_r01*x568);
13812 IkReal x581=(new_r00*x568);
13813 evalcond[0]=((((-1.0)*x568*x571))+new_r02);
13814 evalcond[1]=((((-1.0)*x569*x571))+new_r12);
13815 evalcond[2]=((((-1.0)*new_r02*x573))+((new_r12*x568)));
13816 evalcond[3]=(((sj10*x578))+((cj10*x569))+new_r01);
13817 evalcond[4]=((((-1.0)*x571))+x579+x577);
13818 evalcond[5]=(((cj9*sj10))+x575+x580);
13819 evalcond[6]=((((-1.0)*x570*x578))+x574+new_r00);
13820 evalcond[7]=(((cj9*x574))+(((-1.0)*x568*x570))+new_r11);
13821 evalcond[8]=((((-1.0)*x572))+(((-1.0)*new_r00*x573))+((new_r10*x568)));
13822 evalcond[9]=((((-1.0)*x570))+((new_r11*x568))+(((-1.0)*new_r01*x573)));
13823 evalcond[10]=((((-1.0)*cj9*x570))+x576+x581);
13824 evalcond[11]=((((-1.0)*cj9*x569*x570))+(((-1.0)*x568*x572))+new_r10);
13825 evalcond[12]=(((cj9*new_r20))+((sj9*x576))+((sj9*x581)));
13826 evalcond[13]=(((cj9*new_r21))+((sj9*x575))+((sj9*x580)));
13827 evalcond[14]=((-1.0)+((cj9*new_r22))+((sj9*x579))+((sj9*x577)));
13828 evalcond[15]=((((-1.0)*new_r22*x571))+((cj9*x579))+((cj9*x577)));
13829 evalcond[16]=(((new_r01*x578))+sj10+((cj9*x575))+(((-1.0)*new_r21*x571)));
13830 evalcond[17]=(((new_r00*x578))+(((-1.0)*x570))+(((-1.0)*new_r20*x571))+((cj9*x576)));
13831 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH  )
13832 {
13833 continue;
13834 }
13835 }
13836 
13837 {
13838 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13839 vinfos[0].jointtype = 1;
13840 vinfos[0].foffset = j6;
13841 vinfos[0].indices[0] = _ij6[0];
13842 vinfos[0].indices[1] = _ij6[1];
13843 vinfos[0].maxsolutions = _nj6;
13844 vinfos[1].jointtype = 1;
13845 vinfos[1].foffset = j7;
13846 vinfos[1].indices[0] = _ij7[0];
13847 vinfos[1].indices[1] = _ij7[1];
13848 vinfos[1].maxsolutions = _nj7;
13849 vinfos[2].jointtype = 1;
13850 vinfos[2].foffset = j8;
13851 vinfos[2].indices[0] = _ij8[0];
13852 vinfos[2].indices[1] = _ij8[1];
13853 vinfos[2].maxsolutions = _nj8;
13854 vinfos[3].jointtype = 1;
13855 vinfos[3].foffset = j9;
13856 vinfos[3].indices[0] = _ij9[0];
13857 vinfos[3].indices[1] = _ij9[1];
13858 vinfos[3].maxsolutions = _nj9;
13859 vinfos[4].jointtype = 1;
13860 vinfos[4].foffset = j10;
13861 vinfos[4].indices[0] = _ij10[0];
13862 vinfos[4].indices[1] = _ij10[1];
13863 vinfos[4].maxsolutions = _nj10;
13864 vinfos[5].jointtype = 1;
13865 vinfos[5].foffset = j11;
13866 vinfos[5].indices[0] = _ij11[0];
13867 vinfos[5].indices[1] = _ij11[1];
13868 vinfos[5].maxsolutions = _nj11;
13869 vinfos[6].jointtype = 1;
13870 vinfos[6].foffset = j12;
13871 vinfos[6].indices[0] = _ij12[0];
13872 vinfos[6].indices[1] = _ij12[1];
13873 vinfos[6].maxsolutions = _nj12;
13874 std::vector<int> vfree(0);
13875 solutions.AddSolution(vinfos,vfree);
13876 }
13877 }
13878 }
13879 
13880 }
13881 
13882 }
13883 }
13884 }
13885 
13886 }
13887 
13888 }
13889 
13890 } else
13891 {
13892 {
13893 IkReal j8array[1], cj8array[1], sj8array[1];
13894 bool j8valid[1]={false};
13895 _nj8 = 1;
13896 CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal(new_r12),IkReal(new_r02),IKFAST_ATAN2_MAGTHRESH);
13897 if(!x582.valid){
13898 continue;
13899 }
13900 CheckValue<IkReal> x583=IKPowWithIntegerCheck(IKsign(sj9),-1);
13901 if(!x583.valid){
13902 continue;
13903 }
13904 j8array[0]=((-1.5707963267949)+(x582.value)+(((1.5707963267949)*(x583.value))));
13905 sj8array[0]=IKsin(j8array[0]);
13906 cj8array[0]=IKcos(j8array[0]);
13907 if( j8array[0] > IKPI )
13908 {
13909     j8array[0]-=IK2PI;
13910 }
13911 else if( j8array[0] < -IKPI )
13912 {    j8array[0]+=IK2PI;
13913 }
13914 j8valid[0] = true;
13915 for(int ij8 = 0; ij8 < 1; ++ij8)
13916 {
13917 if( !j8valid[ij8] )
13918 {
13919     continue;
13920 }
13921 _ij8[0] = ij8; _ij8[1] = -1;
13922 for(int iij8 = ij8+1; iij8 < 1; ++iij8)
13923 {
13924 if( j8valid[iij8] && IKabs(cj8array[ij8]-cj8array[iij8]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij8]-sj8array[iij8]) < IKFAST_SOLUTION_THRESH )
13925 {
13926     j8valid[iij8]=false; _ij8[1] = iij8; break; 
13927 }
13928 }
13929 j8 = j8array[ij8]; cj8 = cj8array[ij8]; sj8 = sj8array[ij8];
13930 {
13931 IkReal evalcond[8];
13932 IkReal x584=IKcos(j8);
13933 IkReal x585=IKsin(j8);
13934 IkReal x586=((1.0)*sj9);
13935 IkReal x587=((1.0)*x585);
13936 IkReal x588=(sj9*x584);
13937 IkReal x589=(new_r12*x585);
13938 IkReal x590=(new_r02*x584);
13939 IkReal x591=(sj9*x585);
13940 evalcond[0]=((((-1.0)*x584*x586))+new_r02);
13941 evalcond[1]=((((-1.0)*x585*x586))+new_r12);
13942 evalcond[2]=(((new_r12*x584))+(((-1.0)*new_r02*x587)));
13943 evalcond[3]=(x589+x590+(((-1.0)*x586)));
13944 evalcond[4]=(((cj9*new_r20))+((new_r10*x591))+((new_r00*x588)));
13945 evalcond[5]=(((cj9*new_r21))+((new_r11*x591))+((new_r01*x588)));
13946 evalcond[6]=((-1.0)+((cj9*new_r22))+((sj9*x589))+((new_r02*x588)));
13947 evalcond[7]=(((cj9*x590))+(((-1.0)*new_r22*x586))+((cj9*x589)));
13948 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
13949 {
13950 continue;
13951 }
13952 }
13953 
13954 {
13955 IkReal j10eval[3];
13956 j10eval[0]=sj9;
13957 j10eval[1]=IKsign(sj9);
13958 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
13959 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
13960 {
13961 {
13962 IkReal j10eval[2];
13963 j10eval[0]=sj8;
13964 j10eval[1]=sj9;
13965 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  )
13966 {
13967 {
13968 IkReal j10eval[3];
13969 j10eval[0]=cj8;
13970 j10eval[1]=cj9;
13971 j10eval[2]=sj9;
13972 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
13973 {
13974 {
13975 IkReal evalcond[5];
13976 bool bgotonextstatement = true;
13977 do
13978 {
13979 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j8)))), 6.28318530717959)));
13980 evalcond[1]=new_r02;
13981 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
13982 {
13983 bgotonextstatement=false;
13984 {
13985 IkReal j10eval[3];
13986 sj8=1.0;
13987 cj8=0;
13988 j8=1.5707963267949;
13989 j10eval[0]=sj9;
13990 j10eval[1]=IKsign(sj9);
13991 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
13992 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
13993 {
13994 {
13995 IkReal j10eval[3];
13996 sj8=1.0;
13997 cj8=0;
13998 j8=1.5707963267949;
13999 j10eval[0]=cj9;
14000 j10eval[1]=IKsign(cj9);
14001 j10eval[2]=((IKabs(new_r11))+(IKabs(new_r10)));
14002 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
14003 {
14004 {
14005 IkReal j10eval[1];
14006 sj8=1.0;
14007 cj8=0;
14008 j8=1.5707963267949;
14009 j10eval[0]=sj9;
14010 if( IKabs(j10eval[0]) < 0.0000010000000000  )
14011 {
14012 {
14013 IkReal evalcond[4];
14014 bool bgotonextstatement = true;
14015 do
14016 {
14017 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
14018 evalcond[1]=new_r20;
14019 evalcond[2]=new_r12;
14020 evalcond[3]=new_r21;
14021 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
14022 {
14023 bgotonextstatement=false;
14024 {
14025 IkReal j10array[1], cj10array[1], sj10array[1];
14026 bool j10valid[1]={false};
14027 _nj10 = 1;
14028 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
14029     continue;
14030 j10array[0]=IKatan2(((-1.0)*new_r11), new_r10);
14031 sj10array[0]=IKsin(j10array[0]);
14032 cj10array[0]=IKcos(j10array[0]);
14033 if( j10array[0] > IKPI )
14034 {
14035     j10array[0]-=IK2PI;
14036 }
14037 else if( j10array[0] < -IKPI )
14038 {    j10array[0]+=IK2PI;
14039 }
14040 j10valid[0] = true;
14041 for(int ij10 = 0; ij10 < 1; ++ij10)
14042 {
14043 if( !j10valid[ij10] )
14044 {
14045     continue;
14046 }
14047 _ij10[0] = ij10; _ij10[1] = -1;
14048 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14049 {
14050 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14051 {
14052     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14053 }
14054 }
14055 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14056 {
14057 IkReal evalcond[4];
14058 IkReal x592=IKsin(j10);
14059 IkReal x593=((1.0)*(IKcos(j10)));
14060 evalcond[0]=(x592+new_r11);
14061 evalcond[1]=((((-1.0)*x593))+new_r10);
14062 evalcond[2]=((((-1.0)*x592))+(((-1.0)*new_r00)));
14063 evalcond[3]=((((-1.0)*x593))+(((-1.0)*new_r01)));
14064 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
14065 {
14066 continue;
14067 }
14068 }
14069 
14070 {
14071 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14072 vinfos[0].jointtype = 1;
14073 vinfos[0].foffset = j6;
14074 vinfos[0].indices[0] = _ij6[0];
14075 vinfos[0].indices[1] = _ij6[1];
14076 vinfos[0].maxsolutions = _nj6;
14077 vinfos[1].jointtype = 1;
14078 vinfos[1].foffset = j7;
14079 vinfos[1].indices[0] = _ij7[0];
14080 vinfos[1].indices[1] = _ij7[1];
14081 vinfos[1].maxsolutions = _nj7;
14082 vinfos[2].jointtype = 1;
14083 vinfos[2].foffset = j8;
14084 vinfos[2].indices[0] = _ij8[0];
14085 vinfos[2].indices[1] = _ij8[1];
14086 vinfos[2].maxsolutions = _nj8;
14087 vinfos[3].jointtype = 1;
14088 vinfos[3].foffset = j9;
14089 vinfos[3].indices[0] = _ij9[0];
14090 vinfos[3].indices[1] = _ij9[1];
14091 vinfos[3].maxsolutions = _nj9;
14092 vinfos[4].jointtype = 1;
14093 vinfos[4].foffset = j10;
14094 vinfos[4].indices[0] = _ij10[0];
14095 vinfos[4].indices[1] = _ij10[1];
14096 vinfos[4].maxsolutions = _nj10;
14097 vinfos[5].jointtype = 1;
14098 vinfos[5].foffset = j11;
14099 vinfos[5].indices[0] = _ij11[0];
14100 vinfos[5].indices[1] = _ij11[1];
14101 vinfos[5].maxsolutions = _nj11;
14102 vinfos[6].jointtype = 1;
14103 vinfos[6].foffset = j12;
14104 vinfos[6].indices[0] = _ij12[0];
14105 vinfos[6].indices[1] = _ij12[1];
14106 vinfos[6].maxsolutions = _nj12;
14107 std::vector<int> vfree(0);
14108 solutions.AddSolution(vinfos,vfree);
14109 }
14110 }
14111 }
14112 
14113 }
14114 } while(0);
14115 if( bgotonextstatement )
14116 {
14117 bool bgotonextstatement = true;
14118 do
14119 {
14120 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
14121 evalcond[1]=new_r20;
14122 evalcond[2]=new_r12;
14123 evalcond[3]=new_r21;
14124 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
14125 {
14126 bgotonextstatement=false;
14127 {
14128 IkReal j10array[1], cj10array[1], sj10array[1];
14129 bool j10valid[1]={false};
14130 _nj10 = 1;
14131 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
14132     continue;
14133 j10array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
14134 sj10array[0]=IKsin(j10array[0]);
14135 cj10array[0]=IKcos(j10array[0]);
14136 if( j10array[0] > IKPI )
14137 {
14138     j10array[0]-=IK2PI;
14139 }
14140 else if( j10array[0] < -IKPI )
14141 {    j10array[0]+=IK2PI;
14142 }
14143 j10valid[0] = true;
14144 for(int ij10 = 0; ij10 < 1; ++ij10)
14145 {
14146 if( !j10valid[ij10] )
14147 {
14148     continue;
14149 }
14150 _ij10[0] = ij10; _ij10[1] = -1;
14151 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14152 {
14153 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14154 {
14155     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14156 }
14157 }
14158 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14159 {
14160 IkReal evalcond[4];
14161 IkReal x594=IKcos(j10);
14162 IkReal x595=((1.0)*(IKsin(j10)));
14163 evalcond[0]=(x594+new_r10);
14164 evalcond[1]=((((-1.0)*x595))+new_r11);
14165 evalcond[2]=((((-1.0)*x595))+(((-1.0)*new_r00)));
14166 evalcond[3]=((((-1.0)*x594))+(((-1.0)*new_r01)));
14167 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
14168 {
14169 continue;
14170 }
14171 }
14172 
14173 {
14174 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14175 vinfos[0].jointtype = 1;
14176 vinfos[0].foffset = j6;
14177 vinfos[0].indices[0] = _ij6[0];
14178 vinfos[0].indices[1] = _ij6[1];
14179 vinfos[0].maxsolutions = _nj6;
14180 vinfos[1].jointtype = 1;
14181 vinfos[1].foffset = j7;
14182 vinfos[1].indices[0] = _ij7[0];
14183 vinfos[1].indices[1] = _ij7[1];
14184 vinfos[1].maxsolutions = _nj7;
14185 vinfos[2].jointtype = 1;
14186 vinfos[2].foffset = j8;
14187 vinfos[2].indices[0] = _ij8[0];
14188 vinfos[2].indices[1] = _ij8[1];
14189 vinfos[2].maxsolutions = _nj8;
14190 vinfos[3].jointtype = 1;
14191 vinfos[3].foffset = j9;
14192 vinfos[3].indices[0] = _ij9[0];
14193 vinfos[3].indices[1] = _ij9[1];
14194 vinfos[3].maxsolutions = _nj9;
14195 vinfos[4].jointtype = 1;
14196 vinfos[4].foffset = j10;
14197 vinfos[4].indices[0] = _ij10[0];
14198 vinfos[4].indices[1] = _ij10[1];
14199 vinfos[4].maxsolutions = _nj10;
14200 vinfos[5].jointtype = 1;
14201 vinfos[5].foffset = j11;
14202 vinfos[5].indices[0] = _ij11[0];
14203 vinfos[5].indices[1] = _ij11[1];
14204 vinfos[5].maxsolutions = _nj11;
14205 vinfos[6].jointtype = 1;
14206 vinfos[6].foffset = j12;
14207 vinfos[6].indices[0] = _ij12[0];
14208 vinfos[6].indices[1] = _ij12[1];
14209 vinfos[6].maxsolutions = _nj12;
14210 std::vector<int> vfree(0);
14211 solutions.AddSolution(vinfos,vfree);
14212 }
14213 }
14214 }
14215 
14216 }
14217 } while(0);
14218 if( bgotonextstatement )
14219 {
14220 bool bgotonextstatement = true;
14221 do
14222 {
14223 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
14224 evalcond[1]=new_r22;
14225 evalcond[2]=new_r11;
14226 evalcond[3]=new_r10;
14227 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
14228 {
14229 bgotonextstatement=false;
14230 {
14231 IkReal j10array[1], cj10array[1], sj10array[1];
14232 bool j10valid[1]={false};
14233 _nj10 = 1;
14234 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
14235     continue;
14236 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
14237 sj10array[0]=IKsin(j10array[0]);
14238 cj10array[0]=IKcos(j10array[0]);
14239 if( j10array[0] > IKPI )
14240 {
14241     j10array[0]-=IK2PI;
14242 }
14243 else if( j10array[0] < -IKPI )
14244 {    j10array[0]+=IK2PI;
14245 }
14246 j10valid[0] = true;
14247 for(int ij10 = 0; ij10 < 1; ++ij10)
14248 {
14249 if( !j10valid[ij10] )
14250 {
14251     continue;
14252 }
14253 _ij10[0] = ij10; _ij10[1] = -1;
14254 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14255 {
14256 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14257 {
14258     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14259 }
14260 }
14261 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14262 {
14263 IkReal evalcond[4];
14264 IkReal x596=IKcos(j10);
14265 IkReal x597=((1.0)*(IKsin(j10)));
14266 evalcond[0]=(x596+new_r20);
14267 evalcond[1]=((((-1.0)*x597))+new_r21);
14268 evalcond[2]=((((-1.0)*x597))+(((-1.0)*new_r00)));
14269 evalcond[3]=((((-1.0)*x596))+(((-1.0)*new_r01)));
14270 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
14271 {
14272 continue;
14273 }
14274 }
14275 
14276 {
14277 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14278 vinfos[0].jointtype = 1;
14279 vinfos[0].foffset = j6;
14280 vinfos[0].indices[0] = _ij6[0];
14281 vinfos[0].indices[1] = _ij6[1];
14282 vinfos[0].maxsolutions = _nj6;
14283 vinfos[1].jointtype = 1;
14284 vinfos[1].foffset = j7;
14285 vinfos[1].indices[0] = _ij7[0];
14286 vinfos[1].indices[1] = _ij7[1];
14287 vinfos[1].maxsolutions = _nj7;
14288 vinfos[2].jointtype = 1;
14289 vinfos[2].foffset = j8;
14290 vinfos[2].indices[0] = _ij8[0];
14291 vinfos[2].indices[1] = _ij8[1];
14292 vinfos[2].maxsolutions = _nj8;
14293 vinfos[3].jointtype = 1;
14294 vinfos[3].foffset = j9;
14295 vinfos[3].indices[0] = _ij9[0];
14296 vinfos[3].indices[1] = _ij9[1];
14297 vinfos[3].maxsolutions = _nj9;
14298 vinfos[4].jointtype = 1;
14299 vinfos[4].foffset = j10;
14300 vinfos[4].indices[0] = _ij10[0];
14301 vinfos[4].indices[1] = _ij10[1];
14302 vinfos[4].maxsolutions = _nj10;
14303 vinfos[5].jointtype = 1;
14304 vinfos[5].foffset = j11;
14305 vinfos[5].indices[0] = _ij11[0];
14306 vinfos[5].indices[1] = _ij11[1];
14307 vinfos[5].maxsolutions = _nj11;
14308 vinfos[6].jointtype = 1;
14309 vinfos[6].foffset = j12;
14310 vinfos[6].indices[0] = _ij12[0];
14311 vinfos[6].indices[1] = _ij12[1];
14312 vinfos[6].maxsolutions = _nj12;
14313 std::vector<int> vfree(0);
14314 solutions.AddSolution(vinfos,vfree);
14315 }
14316 }
14317 }
14318 
14319 }
14320 } while(0);
14321 if( bgotonextstatement )
14322 {
14323 bool bgotonextstatement = true;
14324 do
14325 {
14326 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
14327 evalcond[1]=new_r22;
14328 evalcond[2]=new_r11;
14329 evalcond[3]=new_r10;
14330 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
14331 {
14332 bgotonextstatement=false;
14333 {
14334 IkReal j10array[1], cj10array[1], sj10array[1];
14335 bool j10valid[1]={false};
14336 _nj10 = 1;
14337 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
14338     continue;
14339 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
14340 sj10array[0]=IKsin(j10array[0]);
14341 cj10array[0]=IKcos(j10array[0]);
14342 if( j10array[0] > IKPI )
14343 {
14344     j10array[0]-=IK2PI;
14345 }
14346 else if( j10array[0] < -IKPI )
14347 {    j10array[0]+=IK2PI;
14348 }
14349 j10valid[0] = true;
14350 for(int ij10 = 0; ij10 < 1; ++ij10)
14351 {
14352 if( !j10valid[ij10] )
14353 {
14354     continue;
14355 }
14356 _ij10[0] = ij10; _ij10[1] = -1;
14357 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14358 {
14359 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14360 {
14361     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14362 }
14363 }
14364 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14365 {
14366 IkReal evalcond[4];
14367 IkReal x598=IKsin(j10);
14368 IkReal x599=((1.0)*(IKcos(j10)));
14369 evalcond[0]=(x598+new_r21);
14370 evalcond[1]=((((-1.0)*x599))+new_r20);
14371 evalcond[2]=((((-1.0)*x598))+(((-1.0)*new_r00)));
14372 evalcond[3]=((((-1.0)*x599))+(((-1.0)*new_r01)));
14373 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
14374 {
14375 continue;
14376 }
14377 }
14378 
14379 {
14380 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14381 vinfos[0].jointtype = 1;
14382 vinfos[0].foffset = j6;
14383 vinfos[0].indices[0] = _ij6[0];
14384 vinfos[0].indices[1] = _ij6[1];
14385 vinfos[0].maxsolutions = _nj6;
14386 vinfos[1].jointtype = 1;
14387 vinfos[1].foffset = j7;
14388 vinfos[1].indices[0] = _ij7[0];
14389 vinfos[1].indices[1] = _ij7[1];
14390 vinfos[1].maxsolutions = _nj7;
14391 vinfos[2].jointtype = 1;
14392 vinfos[2].foffset = j8;
14393 vinfos[2].indices[0] = _ij8[0];
14394 vinfos[2].indices[1] = _ij8[1];
14395 vinfos[2].maxsolutions = _nj8;
14396 vinfos[3].jointtype = 1;
14397 vinfos[3].foffset = j9;
14398 vinfos[3].indices[0] = _ij9[0];
14399 vinfos[3].indices[1] = _ij9[1];
14400 vinfos[3].maxsolutions = _nj9;
14401 vinfos[4].jointtype = 1;
14402 vinfos[4].foffset = j10;
14403 vinfos[4].indices[0] = _ij10[0];
14404 vinfos[4].indices[1] = _ij10[1];
14405 vinfos[4].maxsolutions = _nj10;
14406 vinfos[5].jointtype = 1;
14407 vinfos[5].foffset = j11;
14408 vinfos[5].indices[0] = _ij11[0];
14409 vinfos[5].indices[1] = _ij11[1];
14410 vinfos[5].maxsolutions = _nj11;
14411 vinfos[6].jointtype = 1;
14412 vinfos[6].foffset = j12;
14413 vinfos[6].indices[0] = _ij12[0];
14414 vinfos[6].indices[1] = _ij12[1];
14415 vinfos[6].maxsolutions = _nj12;
14416 std::vector<int> vfree(0);
14417 solutions.AddSolution(vinfos,vfree);
14418 }
14419 }
14420 }
14421 
14422 }
14423 } while(0);
14424 if( bgotonextstatement )
14425 {
14426 bool bgotonextstatement = true;
14427 do
14428 {
14429 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
14430 if( IKabs(evalcond[0]) < 0.0000050000000000  )
14431 {
14432 bgotonextstatement=false;
14433 {
14434 IkReal j10array[1], cj10array[1], sj10array[1];
14435 bool j10valid[1]={false};
14436 _nj10 = 1;
14437 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
14438     continue;
14439 j10array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
14440 sj10array[0]=IKsin(j10array[0]);
14441 cj10array[0]=IKcos(j10array[0]);
14442 if( j10array[0] > IKPI )
14443 {
14444     j10array[0]-=IK2PI;
14445 }
14446 else if( j10array[0] < -IKPI )
14447 {    j10array[0]+=IK2PI;
14448 }
14449 j10valid[0] = true;
14450 for(int ij10 = 0; ij10 < 1; ++ij10)
14451 {
14452 if( !j10valid[ij10] )
14453 {
14454     continue;
14455 }
14456 _ij10[0] = ij10; _ij10[1] = -1;
14457 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14458 {
14459 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14460 {
14461     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14462 }
14463 }
14464 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14465 {
14466 IkReal evalcond[6];
14467 IkReal x600=IKsin(j10);
14468 IkReal x601=IKcos(j10);
14469 IkReal x602=((-1.0)*x601);
14470 evalcond[0]=x600;
14471 evalcond[1]=(new_r22*x600);
14472 evalcond[2]=x602;
14473 evalcond[3]=(new_r22*x602);
14474 evalcond[4]=((((-1.0)*x600))+(((-1.0)*new_r00)));
14475 evalcond[5]=((((-1.0)*x601))+(((-1.0)*new_r01)));
14476 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
14477 {
14478 continue;
14479 }
14480 }
14481 
14482 {
14483 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14484 vinfos[0].jointtype = 1;
14485 vinfos[0].foffset = j6;
14486 vinfos[0].indices[0] = _ij6[0];
14487 vinfos[0].indices[1] = _ij6[1];
14488 vinfos[0].maxsolutions = _nj6;
14489 vinfos[1].jointtype = 1;
14490 vinfos[1].foffset = j7;
14491 vinfos[1].indices[0] = _ij7[0];
14492 vinfos[1].indices[1] = _ij7[1];
14493 vinfos[1].maxsolutions = _nj7;
14494 vinfos[2].jointtype = 1;
14495 vinfos[2].foffset = j8;
14496 vinfos[2].indices[0] = _ij8[0];
14497 vinfos[2].indices[1] = _ij8[1];
14498 vinfos[2].maxsolutions = _nj8;
14499 vinfos[3].jointtype = 1;
14500 vinfos[3].foffset = j9;
14501 vinfos[3].indices[0] = _ij9[0];
14502 vinfos[3].indices[1] = _ij9[1];
14503 vinfos[3].maxsolutions = _nj9;
14504 vinfos[4].jointtype = 1;
14505 vinfos[4].foffset = j10;
14506 vinfos[4].indices[0] = _ij10[0];
14507 vinfos[4].indices[1] = _ij10[1];
14508 vinfos[4].maxsolutions = _nj10;
14509 vinfos[5].jointtype = 1;
14510 vinfos[5].foffset = j11;
14511 vinfos[5].indices[0] = _ij11[0];
14512 vinfos[5].indices[1] = _ij11[1];
14513 vinfos[5].maxsolutions = _nj11;
14514 vinfos[6].jointtype = 1;
14515 vinfos[6].foffset = j12;
14516 vinfos[6].indices[0] = _ij12[0];
14517 vinfos[6].indices[1] = _ij12[1];
14518 vinfos[6].maxsolutions = _nj12;
14519 std::vector<int> vfree(0);
14520 solutions.AddSolution(vinfos,vfree);
14521 }
14522 }
14523 }
14524 
14525 }
14526 } while(0);
14527 if( bgotonextstatement )
14528 {
14529 bool bgotonextstatement = true;
14530 do
14531 {
14532 if( 1 )
14533 {
14534 bgotonextstatement=false;
14535 continue; // branch miss [j10]
14536 
14537 }
14538 } while(0);
14539 if( bgotonextstatement )
14540 {
14541 }
14542 }
14543 }
14544 }
14545 }
14546 }
14547 }
14548 
14549 } else
14550 {
14551 {
14552 IkReal j10array[1], cj10array[1], sj10array[1];
14553 bool j10valid[1]={false};
14554 _nj10 = 1;
14555 CheckValue<IkReal> x603=IKPowWithIntegerCheck(sj9,-1);
14556 if(!x603.valid){
14557 continue;
14558 }
14559 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x603.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r20*(x603.value)))-1) <= IKFAST_SINCOS_THRESH )
14560     continue;
14561 j10array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x603.value)));
14562 sj10array[0]=IKsin(j10array[0]);
14563 cj10array[0]=IKcos(j10array[0]);
14564 if( j10array[0] > IKPI )
14565 {
14566     j10array[0]-=IK2PI;
14567 }
14568 else if( j10array[0] < -IKPI )
14569 {    j10array[0]+=IK2PI;
14570 }
14571 j10valid[0] = true;
14572 for(int ij10 = 0; ij10 < 1; ++ij10)
14573 {
14574 if( !j10valid[ij10] )
14575 {
14576     continue;
14577 }
14578 _ij10[0] = ij10; _ij10[1] = -1;
14579 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14580 {
14581 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14582 {
14583     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14584 }
14585 }
14586 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14587 {
14588 IkReal evalcond[8];
14589 IkReal x604=IKsin(j10);
14590 IkReal x605=IKcos(j10);
14591 IkReal x606=((1.0)*sj9);
14592 IkReal x607=((1.0)*x605);
14593 evalcond[0]=(((sj9*x605))+new_r20);
14594 evalcond[1]=(((cj9*x604))+new_r11);
14595 evalcond[2]=((((-1.0)*x604*x606))+new_r21);
14596 evalcond[3]=((((-1.0)*cj9*x607))+new_r10);
14597 evalcond[4]=((((-1.0)*x604))+(((-1.0)*new_r00)));
14598 evalcond[5]=((((-1.0)*x607))+(((-1.0)*new_r01)));
14599 evalcond[6]=(((cj9*new_r11))+x604+(((-1.0)*new_r21*x606)));
14600 evalcond[7]=((((-1.0)*new_r20*x606))+((cj9*new_r10))+(((-1.0)*x607)));
14601 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
14602 {
14603 continue;
14604 }
14605 }
14606 
14607 {
14608 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14609 vinfos[0].jointtype = 1;
14610 vinfos[0].foffset = j6;
14611 vinfos[0].indices[0] = _ij6[0];
14612 vinfos[0].indices[1] = _ij6[1];
14613 vinfos[0].maxsolutions = _nj6;
14614 vinfos[1].jointtype = 1;
14615 vinfos[1].foffset = j7;
14616 vinfos[1].indices[0] = _ij7[0];
14617 vinfos[1].indices[1] = _ij7[1];
14618 vinfos[1].maxsolutions = _nj7;
14619 vinfos[2].jointtype = 1;
14620 vinfos[2].foffset = j8;
14621 vinfos[2].indices[0] = _ij8[0];
14622 vinfos[2].indices[1] = _ij8[1];
14623 vinfos[2].maxsolutions = _nj8;
14624 vinfos[3].jointtype = 1;
14625 vinfos[3].foffset = j9;
14626 vinfos[3].indices[0] = _ij9[0];
14627 vinfos[3].indices[1] = _ij9[1];
14628 vinfos[3].maxsolutions = _nj9;
14629 vinfos[4].jointtype = 1;
14630 vinfos[4].foffset = j10;
14631 vinfos[4].indices[0] = _ij10[0];
14632 vinfos[4].indices[1] = _ij10[1];
14633 vinfos[4].maxsolutions = _nj10;
14634 vinfos[5].jointtype = 1;
14635 vinfos[5].foffset = j11;
14636 vinfos[5].indices[0] = _ij11[0];
14637 vinfos[5].indices[1] = _ij11[1];
14638 vinfos[5].maxsolutions = _nj11;
14639 vinfos[6].jointtype = 1;
14640 vinfos[6].foffset = j12;
14641 vinfos[6].indices[0] = _ij12[0];
14642 vinfos[6].indices[1] = _ij12[1];
14643 vinfos[6].maxsolutions = _nj12;
14644 std::vector<int> vfree(0);
14645 solutions.AddSolution(vinfos,vfree);
14646 }
14647 }
14648 }
14649 
14650 }
14651 
14652 }
14653 
14654 } else
14655 {
14656 {
14657 IkReal j10array[1], cj10array[1], sj10array[1];
14658 bool j10valid[1]={false};
14659 _nj10 = 1;
14660 CheckValue<IkReal> x608=IKPowWithIntegerCheck(IKsign(cj9),-1);
14661 if(!x608.valid){
14662 continue;
14663 }
14664 CheckValue<IkReal> x609 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
14665 if(!x609.valid){
14666 continue;
14667 }
14668 j10array[0]=((-1.5707963267949)+(((1.5707963267949)*(x608.value)))+(x609.value));
14669 sj10array[0]=IKsin(j10array[0]);
14670 cj10array[0]=IKcos(j10array[0]);
14671 if( j10array[0] > IKPI )
14672 {
14673     j10array[0]-=IK2PI;
14674 }
14675 else if( j10array[0] < -IKPI )
14676 {    j10array[0]+=IK2PI;
14677 }
14678 j10valid[0] = true;
14679 for(int ij10 = 0; ij10 < 1; ++ij10)
14680 {
14681 if( !j10valid[ij10] )
14682 {
14683     continue;
14684 }
14685 _ij10[0] = ij10; _ij10[1] = -1;
14686 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14687 {
14688 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14689 {
14690     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14691 }
14692 }
14693 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14694 {
14695 IkReal evalcond[8];
14696 IkReal x610=IKsin(j10);
14697 IkReal x611=IKcos(j10);
14698 IkReal x612=((1.0)*sj9);
14699 IkReal x613=((1.0)*x611);
14700 evalcond[0]=(((sj9*x611))+new_r20);
14701 evalcond[1]=(((cj9*x610))+new_r11);
14702 evalcond[2]=((((-1.0)*x610*x612))+new_r21);
14703 evalcond[3]=((((-1.0)*cj9*x613))+new_r10);
14704 evalcond[4]=((((-1.0)*x610))+(((-1.0)*new_r00)));
14705 evalcond[5]=((((-1.0)*x613))+(((-1.0)*new_r01)));
14706 evalcond[6]=((((-1.0)*new_r21*x612))+((cj9*new_r11))+x610);
14707 evalcond[7]=(((cj9*new_r10))+(((-1.0)*x613))+(((-1.0)*new_r20*x612)));
14708 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
14709 {
14710 continue;
14711 }
14712 }
14713 
14714 {
14715 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14716 vinfos[0].jointtype = 1;
14717 vinfos[0].foffset = j6;
14718 vinfos[0].indices[0] = _ij6[0];
14719 vinfos[0].indices[1] = _ij6[1];
14720 vinfos[0].maxsolutions = _nj6;
14721 vinfos[1].jointtype = 1;
14722 vinfos[1].foffset = j7;
14723 vinfos[1].indices[0] = _ij7[0];
14724 vinfos[1].indices[1] = _ij7[1];
14725 vinfos[1].maxsolutions = _nj7;
14726 vinfos[2].jointtype = 1;
14727 vinfos[2].foffset = j8;
14728 vinfos[2].indices[0] = _ij8[0];
14729 vinfos[2].indices[1] = _ij8[1];
14730 vinfos[2].maxsolutions = _nj8;
14731 vinfos[3].jointtype = 1;
14732 vinfos[3].foffset = j9;
14733 vinfos[3].indices[0] = _ij9[0];
14734 vinfos[3].indices[1] = _ij9[1];
14735 vinfos[3].maxsolutions = _nj9;
14736 vinfos[4].jointtype = 1;
14737 vinfos[4].foffset = j10;
14738 vinfos[4].indices[0] = _ij10[0];
14739 vinfos[4].indices[1] = _ij10[1];
14740 vinfos[4].maxsolutions = _nj10;
14741 vinfos[5].jointtype = 1;
14742 vinfos[5].foffset = j11;
14743 vinfos[5].indices[0] = _ij11[0];
14744 vinfos[5].indices[1] = _ij11[1];
14745 vinfos[5].maxsolutions = _nj11;
14746 vinfos[6].jointtype = 1;
14747 vinfos[6].foffset = j12;
14748 vinfos[6].indices[0] = _ij12[0];
14749 vinfos[6].indices[1] = _ij12[1];
14750 vinfos[6].maxsolutions = _nj12;
14751 std::vector<int> vfree(0);
14752 solutions.AddSolution(vinfos,vfree);
14753 }
14754 }
14755 }
14756 
14757 }
14758 
14759 }
14760 
14761 } else
14762 {
14763 {
14764 IkReal j10array[1], cj10array[1], sj10array[1];
14765 bool j10valid[1]={false};
14766 _nj10 = 1;
14767 CheckValue<IkReal> x614 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
14768 if(!x614.valid){
14769 continue;
14770 }
14771 CheckValue<IkReal> x615=IKPowWithIntegerCheck(IKsign(sj9),-1);
14772 if(!x615.valid){
14773 continue;
14774 }
14775 j10array[0]=((-1.5707963267949)+(x614.value)+(((1.5707963267949)*(x615.value))));
14776 sj10array[0]=IKsin(j10array[0]);
14777 cj10array[0]=IKcos(j10array[0]);
14778 if( j10array[0] > IKPI )
14779 {
14780     j10array[0]-=IK2PI;
14781 }
14782 else if( j10array[0] < -IKPI )
14783 {    j10array[0]+=IK2PI;
14784 }
14785 j10valid[0] = true;
14786 for(int ij10 = 0; ij10 < 1; ++ij10)
14787 {
14788 if( !j10valid[ij10] )
14789 {
14790     continue;
14791 }
14792 _ij10[0] = ij10; _ij10[1] = -1;
14793 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14794 {
14795 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14796 {
14797     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14798 }
14799 }
14800 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14801 {
14802 IkReal evalcond[8];
14803 IkReal x616=IKsin(j10);
14804 IkReal x617=IKcos(j10);
14805 IkReal x618=((1.0)*sj9);
14806 IkReal x619=((1.0)*x617);
14807 evalcond[0]=(((sj9*x617))+new_r20);
14808 evalcond[1]=(((cj9*x616))+new_r11);
14809 evalcond[2]=(new_r21+(((-1.0)*x616*x618)));
14810 evalcond[3]=((((-1.0)*cj9*x619))+new_r10);
14811 evalcond[4]=((((-1.0)*x616))+(((-1.0)*new_r00)));
14812 evalcond[5]=((((-1.0)*x619))+(((-1.0)*new_r01)));
14813 evalcond[6]=((((-1.0)*new_r21*x618))+((cj9*new_r11))+x616);
14814 evalcond[7]=(((cj9*new_r10))+(((-1.0)*x619))+(((-1.0)*new_r20*x618)));
14815 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
14816 {
14817 continue;
14818 }
14819 }
14820 
14821 {
14822 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14823 vinfos[0].jointtype = 1;
14824 vinfos[0].foffset = j6;
14825 vinfos[0].indices[0] = _ij6[0];
14826 vinfos[0].indices[1] = _ij6[1];
14827 vinfos[0].maxsolutions = _nj6;
14828 vinfos[1].jointtype = 1;
14829 vinfos[1].foffset = j7;
14830 vinfos[1].indices[0] = _ij7[0];
14831 vinfos[1].indices[1] = _ij7[1];
14832 vinfos[1].maxsolutions = _nj7;
14833 vinfos[2].jointtype = 1;
14834 vinfos[2].foffset = j8;
14835 vinfos[2].indices[0] = _ij8[0];
14836 vinfos[2].indices[1] = _ij8[1];
14837 vinfos[2].maxsolutions = _nj8;
14838 vinfos[3].jointtype = 1;
14839 vinfos[3].foffset = j9;
14840 vinfos[3].indices[0] = _ij9[0];
14841 vinfos[3].indices[1] = _ij9[1];
14842 vinfos[3].maxsolutions = _nj9;
14843 vinfos[4].jointtype = 1;
14844 vinfos[4].foffset = j10;
14845 vinfos[4].indices[0] = _ij10[0];
14846 vinfos[4].indices[1] = _ij10[1];
14847 vinfos[4].maxsolutions = _nj10;
14848 vinfos[5].jointtype = 1;
14849 vinfos[5].foffset = j11;
14850 vinfos[5].indices[0] = _ij11[0];
14851 vinfos[5].indices[1] = _ij11[1];
14852 vinfos[5].maxsolutions = _nj11;
14853 vinfos[6].jointtype = 1;
14854 vinfos[6].foffset = j12;
14855 vinfos[6].indices[0] = _ij12[0];
14856 vinfos[6].indices[1] = _ij12[1];
14857 vinfos[6].maxsolutions = _nj12;
14858 std::vector<int> vfree(0);
14859 solutions.AddSolution(vinfos,vfree);
14860 }
14861 }
14862 }
14863 
14864 }
14865 
14866 }
14867 
14868 }
14869 } while(0);
14870 if( bgotonextstatement )
14871 {
14872 bool bgotonextstatement = true;
14873 do
14874 {
14875 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j8)))), 6.28318530717959)));
14876 evalcond[1]=new_r02;
14877 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
14878 {
14879 bgotonextstatement=false;
14880 {
14881 IkReal j10array[1], cj10array[1], sj10array[1];
14882 bool j10valid[1]={false};
14883 _nj10 = 1;
14884 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
14885     continue;
14886 j10array[0]=IKatan2(new_r00, new_r01);
14887 sj10array[0]=IKsin(j10array[0]);
14888 cj10array[0]=IKcos(j10array[0]);
14889 if( j10array[0] > IKPI )
14890 {
14891     j10array[0]-=IK2PI;
14892 }
14893 else if( j10array[0] < -IKPI )
14894 {    j10array[0]+=IK2PI;
14895 }
14896 j10valid[0] = true;
14897 for(int ij10 = 0; ij10 < 1; ++ij10)
14898 {
14899 if( !j10valid[ij10] )
14900 {
14901     continue;
14902 }
14903 _ij10[0] = ij10; _ij10[1] = -1;
14904 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14905 {
14906 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14907 {
14908     j10valid[iij10]=false; _ij10[1] = iij10; break; 
14909 }
14910 }
14911 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14912 {
14913 IkReal evalcond[8];
14914 IkReal x620=IKcos(j10);
14915 IkReal x621=IKsin(j10);
14916 IkReal x622=((1.0)*sj9);
14917 IkReal x623=((1.0)*new_r10);
14918 IkReal x624=((1.0)*new_r11);
14919 IkReal x625=((1.0)*x620);
14920 evalcond[0]=(((sj9*x620))+new_r20);
14921 evalcond[1]=((((-1.0)*x621))+new_r00);
14922 evalcond[2]=((((-1.0)*x625))+new_r01);
14923 evalcond[3]=((((-1.0)*x621*x622))+new_r21);
14924 evalcond[4]=((((-1.0)*x624))+((cj9*x621)));
14925 evalcond[5]=((((-1.0)*cj9*x625))+(((-1.0)*x623)));
14926 evalcond[6]=((((-1.0)*cj9*x624))+(((-1.0)*new_r21*x622))+x621);
14927 evalcond[7]=((((-1.0)*cj9*x623))+(((-1.0)*x625))+(((-1.0)*new_r20*x622)));
14928 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
14929 {
14930 continue;
14931 }
14932 }
14933 
14934 {
14935 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14936 vinfos[0].jointtype = 1;
14937 vinfos[0].foffset = j6;
14938 vinfos[0].indices[0] = _ij6[0];
14939 vinfos[0].indices[1] = _ij6[1];
14940 vinfos[0].maxsolutions = _nj6;
14941 vinfos[1].jointtype = 1;
14942 vinfos[1].foffset = j7;
14943 vinfos[1].indices[0] = _ij7[0];
14944 vinfos[1].indices[1] = _ij7[1];
14945 vinfos[1].maxsolutions = _nj7;
14946 vinfos[2].jointtype = 1;
14947 vinfos[2].foffset = j8;
14948 vinfos[2].indices[0] = _ij8[0];
14949 vinfos[2].indices[1] = _ij8[1];
14950 vinfos[2].maxsolutions = _nj8;
14951 vinfos[3].jointtype = 1;
14952 vinfos[3].foffset = j9;
14953 vinfos[3].indices[0] = _ij9[0];
14954 vinfos[3].indices[1] = _ij9[1];
14955 vinfos[3].maxsolutions = _nj9;
14956 vinfos[4].jointtype = 1;
14957 vinfos[4].foffset = j10;
14958 vinfos[4].indices[0] = _ij10[0];
14959 vinfos[4].indices[1] = _ij10[1];
14960 vinfos[4].maxsolutions = _nj10;
14961 vinfos[5].jointtype = 1;
14962 vinfos[5].foffset = j11;
14963 vinfos[5].indices[0] = _ij11[0];
14964 vinfos[5].indices[1] = _ij11[1];
14965 vinfos[5].maxsolutions = _nj11;
14966 vinfos[6].jointtype = 1;
14967 vinfos[6].foffset = j12;
14968 vinfos[6].indices[0] = _ij12[0];
14969 vinfos[6].indices[1] = _ij12[1];
14970 vinfos[6].maxsolutions = _nj12;
14971 std::vector<int> vfree(0);
14972 solutions.AddSolution(vinfos,vfree);
14973 }
14974 }
14975 }
14976 
14977 }
14978 } while(0);
14979 if( bgotonextstatement )
14980 {
14981 bool bgotonextstatement = true;
14982 do
14983 {
14984 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
14985 evalcond[1]=new_r22;
14986 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
14987 {
14988 bgotonextstatement=false;
14989 {
14990 IkReal j10array[1], cj10array[1], sj10array[1];
14991 bool j10valid[1]={false};
14992 _nj10 = 1;
14993 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
14994     continue;
14995 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
14996 sj10array[0]=IKsin(j10array[0]);
14997 cj10array[0]=IKcos(j10array[0]);
14998 if( j10array[0] > IKPI )
14999 {
15000     j10array[0]-=IK2PI;
15001 }
15002 else if( j10array[0] < -IKPI )
15003 {    j10array[0]+=IK2PI;
15004 }
15005 j10valid[0] = true;
15006 for(int ij10 = 0; ij10 < 1; ++ij10)
15007 {
15008 if( !j10valid[ij10] )
15009 {
15010     continue;
15011 }
15012 _ij10[0] = ij10; _ij10[1] = -1;
15013 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15014 {
15015 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15016 {
15017     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15018 }
15019 }
15020 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15021 {
15022 IkReal evalcond[8];
15023 IkReal x626=IKcos(j10);
15024 IkReal x627=IKsin(j10);
15025 IkReal x628=((1.0)*sj8);
15026 IkReal x629=((1.0)*x627);
15027 IkReal x630=((1.0)*x626);
15028 evalcond[0]=(x626+new_r20);
15029 evalcond[1]=((((-1.0)*x629))+new_r21);
15030 evalcond[2]=(((sj8*x626))+new_r01);
15031 evalcond[3]=(((sj8*x627))+new_r00);
15032 evalcond[4]=((((-1.0)*cj8*x630))+new_r11);
15033 evalcond[5]=((((-1.0)*cj8*x629))+new_r10);
15034 evalcond[6]=((((-1.0)*new_r00*x628))+(((-1.0)*x629))+((cj8*new_r10)));
15035 evalcond[7]=((((-1.0)*new_r01*x628))+(((-1.0)*x630))+((cj8*new_r11)));
15036 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
15037 {
15038 continue;
15039 }
15040 }
15041 
15042 {
15043 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15044 vinfos[0].jointtype = 1;
15045 vinfos[0].foffset = j6;
15046 vinfos[0].indices[0] = _ij6[0];
15047 vinfos[0].indices[1] = _ij6[1];
15048 vinfos[0].maxsolutions = _nj6;
15049 vinfos[1].jointtype = 1;
15050 vinfos[1].foffset = j7;
15051 vinfos[1].indices[0] = _ij7[0];
15052 vinfos[1].indices[1] = _ij7[1];
15053 vinfos[1].maxsolutions = _nj7;
15054 vinfos[2].jointtype = 1;
15055 vinfos[2].foffset = j8;
15056 vinfos[2].indices[0] = _ij8[0];
15057 vinfos[2].indices[1] = _ij8[1];
15058 vinfos[2].maxsolutions = _nj8;
15059 vinfos[3].jointtype = 1;
15060 vinfos[3].foffset = j9;
15061 vinfos[3].indices[0] = _ij9[0];
15062 vinfos[3].indices[1] = _ij9[1];
15063 vinfos[3].maxsolutions = _nj9;
15064 vinfos[4].jointtype = 1;
15065 vinfos[4].foffset = j10;
15066 vinfos[4].indices[0] = _ij10[0];
15067 vinfos[4].indices[1] = _ij10[1];
15068 vinfos[4].maxsolutions = _nj10;
15069 vinfos[5].jointtype = 1;
15070 vinfos[5].foffset = j11;
15071 vinfos[5].indices[0] = _ij11[0];
15072 vinfos[5].indices[1] = _ij11[1];
15073 vinfos[5].maxsolutions = _nj11;
15074 vinfos[6].jointtype = 1;
15075 vinfos[6].foffset = j12;
15076 vinfos[6].indices[0] = _ij12[0];
15077 vinfos[6].indices[1] = _ij12[1];
15078 vinfos[6].maxsolutions = _nj12;
15079 std::vector<int> vfree(0);
15080 solutions.AddSolution(vinfos,vfree);
15081 }
15082 }
15083 }
15084 
15085 }
15086 } while(0);
15087 if( bgotonextstatement )
15088 {
15089 bool bgotonextstatement = true;
15090 do
15091 {
15092 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
15093 evalcond[1]=new_r22;
15094 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
15095 {
15096 bgotonextstatement=false;
15097 {
15098 IkReal j10array[1], cj10array[1], sj10array[1];
15099 bool j10valid[1]={false};
15100 _nj10 = 1;
15101 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
15102     continue;
15103 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
15104 sj10array[0]=IKsin(j10array[0]);
15105 cj10array[0]=IKcos(j10array[0]);
15106 if( j10array[0] > IKPI )
15107 {
15108     j10array[0]-=IK2PI;
15109 }
15110 else if( j10array[0] < -IKPI )
15111 {    j10array[0]+=IK2PI;
15112 }
15113 j10valid[0] = true;
15114 for(int ij10 = 0; ij10 < 1; ++ij10)
15115 {
15116 if( !j10valid[ij10] )
15117 {
15118     continue;
15119 }
15120 _ij10[0] = ij10; _ij10[1] = -1;
15121 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15122 {
15123 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15124 {
15125     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15126 }
15127 }
15128 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15129 {
15130 IkReal evalcond[8];
15131 IkReal x631=IKcos(j10);
15132 IkReal x632=IKsin(j10);
15133 IkReal x633=((1.0)*sj8);
15134 IkReal x634=((1.0)*x631);
15135 IkReal x635=((1.0)*x632);
15136 evalcond[0]=(x632+new_r21);
15137 evalcond[1]=((((-1.0)*x634))+new_r20);
15138 evalcond[2]=(new_r01+((sj8*x631)));
15139 evalcond[3]=(new_r00+((sj8*x632)));
15140 evalcond[4]=((((-1.0)*cj8*x634))+new_r11);
15141 evalcond[5]=((((-1.0)*cj8*x635))+new_r10);
15142 evalcond[6]=((((-1.0)*new_r00*x633))+(((-1.0)*x635))+((cj8*new_r10)));
15143 evalcond[7]=((((-1.0)*new_r01*x633))+(((-1.0)*x634))+((cj8*new_r11)));
15144 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
15145 {
15146 continue;
15147 }
15148 }
15149 
15150 {
15151 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15152 vinfos[0].jointtype = 1;
15153 vinfos[0].foffset = j6;
15154 vinfos[0].indices[0] = _ij6[0];
15155 vinfos[0].indices[1] = _ij6[1];
15156 vinfos[0].maxsolutions = _nj6;
15157 vinfos[1].jointtype = 1;
15158 vinfos[1].foffset = j7;
15159 vinfos[1].indices[0] = _ij7[0];
15160 vinfos[1].indices[1] = _ij7[1];
15161 vinfos[1].maxsolutions = _nj7;
15162 vinfos[2].jointtype = 1;
15163 vinfos[2].foffset = j8;
15164 vinfos[2].indices[0] = _ij8[0];
15165 vinfos[2].indices[1] = _ij8[1];
15166 vinfos[2].maxsolutions = _nj8;
15167 vinfos[3].jointtype = 1;
15168 vinfos[3].foffset = j9;
15169 vinfos[3].indices[0] = _ij9[0];
15170 vinfos[3].indices[1] = _ij9[1];
15171 vinfos[3].maxsolutions = _nj9;
15172 vinfos[4].jointtype = 1;
15173 vinfos[4].foffset = j10;
15174 vinfos[4].indices[0] = _ij10[0];
15175 vinfos[4].indices[1] = _ij10[1];
15176 vinfos[4].maxsolutions = _nj10;
15177 vinfos[5].jointtype = 1;
15178 vinfos[5].foffset = j11;
15179 vinfos[5].indices[0] = _ij11[0];
15180 vinfos[5].indices[1] = _ij11[1];
15181 vinfos[5].maxsolutions = _nj11;
15182 vinfos[6].jointtype = 1;
15183 vinfos[6].foffset = j12;
15184 vinfos[6].indices[0] = _ij12[0];
15185 vinfos[6].indices[1] = _ij12[1];
15186 vinfos[6].maxsolutions = _nj12;
15187 std::vector<int> vfree(0);
15188 solutions.AddSolution(vinfos,vfree);
15189 }
15190 }
15191 }
15192 
15193 }
15194 } while(0);
15195 if( bgotonextstatement )
15196 {
15197 bool bgotonextstatement = true;
15198 do
15199 {
15200 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
15201 evalcond[1]=new_r20;
15202 evalcond[2]=new_r02;
15203 evalcond[3]=new_r12;
15204 evalcond[4]=new_r21;
15205 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
15206 {
15207 bgotonextstatement=false;
15208 {
15209 IkReal j10array[1], cj10array[1], sj10array[1];
15210 bool j10valid[1]={false};
15211 _nj10 = 1;
15212 IkReal x636=((1.0)*new_r01);
15213 if( IKabs(((((-1.0)*cj8*x636))+(((-1.0)*new_r00*sj8)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj8*x636))+((cj8*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj8*x636))+(((-1.0)*new_r00*sj8))))+IKsqr(((((-1.0)*sj8*x636))+((cj8*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
15214     continue;
15215 j10array[0]=IKatan2(((((-1.0)*cj8*x636))+(((-1.0)*new_r00*sj8))), ((((-1.0)*sj8*x636))+((cj8*new_r00))));
15216 sj10array[0]=IKsin(j10array[0]);
15217 cj10array[0]=IKcos(j10array[0]);
15218 if( j10array[0] > IKPI )
15219 {
15220     j10array[0]-=IK2PI;
15221 }
15222 else if( j10array[0] < -IKPI )
15223 {    j10array[0]+=IK2PI;
15224 }
15225 j10valid[0] = true;
15226 for(int ij10 = 0; ij10 < 1; ++ij10)
15227 {
15228 if( !j10valid[ij10] )
15229 {
15230     continue;
15231 }
15232 _ij10[0] = ij10; _ij10[1] = -1;
15233 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15234 {
15235 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15236 {
15237     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15238 }
15239 }
15240 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15241 {
15242 IkReal evalcond[8];
15243 IkReal x637=IKsin(j10);
15244 IkReal x638=IKcos(j10);
15245 IkReal x639=((1.0)*sj8);
15246 IkReal x640=((1.0)*x638);
15247 IkReal x641=(sj8*x637);
15248 IkReal x642=(cj8*x637);
15249 IkReal x643=(cj8*x640);
15250 evalcond[0]=(((new_r11*sj8))+((cj8*new_r01))+x637);
15251 evalcond[1]=(x642+new_r01+((sj8*x638)));
15252 evalcond[2]=(((new_r10*sj8))+(((-1.0)*x640))+((cj8*new_r00)));
15253 evalcond[3]=((((-1.0)*new_r00*x639))+((cj8*new_r10))+(((-1.0)*x637)));
15254 evalcond[4]=((((-1.0)*new_r01*x639))+((cj8*new_r11))+(((-1.0)*x640)));
15255 evalcond[5]=((((-1.0)*x643))+x641+new_r00);
15256 evalcond[6]=((((-1.0)*x643))+x641+new_r11);
15257 evalcond[7]=((((-1.0)*x642))+(((-1.0)*x638*x639))+new_r10);
15258 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
15259 {
15260 continue;
15261 }
15262 }
15263 
15264 {
15265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15266 vinfos[0].jointtype = 1;
15267 vinfos[0].foffset = j6;
15268 vinfos[0].indices[0] = _ij6[0];
15269 vinfos[0].indices[1] = _ij6[1];
15270 vinfos[0].maxsolutions = _nj6;
15271 vinfos[1].jointtype = 1;
15272 vinfos[1].foffset = j7;
15273 vinfos[1].indices[0] = _ij7[0];
15274 vinfos[1].indices[1] = _ij7[1];
15275 vinfos[1].maxsolutions = _nj7;
15276 vinfos[2].jointtype = 1;
15277 vinfos[2].foffset = j8;
15278 vinfos[2].indices[0] = _ij8[0];
15279 vinfos[2].indices[1] = _ij8[1];
15280 vinfos[2].maxsolutions = _nj8;
15281 vinfos[3].jointtype = 1;
15282 vinfos[3].foffset = j9;
15283 vinfos[3].indices[0] = _ij9[0];
15284 vinfos[3].indices[1] = _ij9[1];
15285 vinfos[3].maxsolutions = _nj9;
15286 vinfos[4].jointtype = 1;
15287 vinfos[4].foffset = j10;
15288 vinfos[4].indices[0] = _ij10[0];
15289 vinfos[4].indices[1] = _ij10[1];
15290 vinfos[4].maxsolutions = _nj10;
15291 vinfos[5].jointtype = 1;
15292 vinfos[5].foffset = j11;
15293 vinfos[5].indices[0] = _ij11[0];
15294 vinfos[5].indices[1] = _ij11[1];
15295 vinfos[5].maxsolutions = _nj11;
15296 vinfos[6].jointtype = 1;
15297 vinfos[6].foffset = j12;
15298 vinfos[6].indices[0] = _ij12[0];
15299 vinfos[6].indices[1] = _ij12[1];
15300 vinfos[6].maxsolutions = _nj12;
15301 std::vector<int> vfree(0);
15302 solutions.AddSolution(vinfos,vfree);
15303 }
15304 }
15305 }
15306 
15307 }
15308 } while(0);
15309 if( bgotonextstatement )
15310 {
15311 bool bgotonextstatement = true;
15312 do
15313 {
15314 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
15315 evalcond[1]=new_r20;
15316 evalcond[2]=new_r02;
15317 evalcond[3]=new_r12;
15318 evalcond[4]=new_r21;
15319 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  && IKabs(evalcond[4]) < 0.0000050000000000  )
15320 {
15321 bgotonextstatement=false;
15322 {
15323 IkReal j10array[1], cj10array[1], sj10array[1];
15324 bool j10valid[1]={false};
15325 _nj10 = 1;
15326 IkReal x644=((1.0)*new_r00);
15327 if( IKabs(((((-1.0)*sj8*x644))+((cj8*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x644)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*sj8*x644))+((cj8*new_r01))))+IKsqr(((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x644))))-1) <= IKFAST_SINCOS_THRESH )
15328     continue;
15329 j10array[0]=IKatan2(((((-1.0)*sj8*x644))+((cj8*new_r01))), ((((-1.0)*new_r01*sj8))+(((-1.0)*cj8*x644))));
15330 sj10array[0]=IKsin(j10array[0]);
15331 cj10array[0]=IKcos(j10array[0]);
15332 if( j10array[0] > IKPI )
15333 {
15334     j10array[0]-=IK2PI;
15335 }
15336 else if( j10array[0] < -IKPI )
15337 {    j10array[0]+=IK2PI;
15338 }
15339 j10valid[0] = true;
15340 for(int ij10 = 0; ij10 < 1; ++ij10)
15341 {
15342 if( !j10valid[ij10] )
15343 {
15344     continue;
15345 }
15346 _ij10[0] = ij10; _ij10[1] = -1;
15347 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15348 {
15349 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15350 {
15351     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15352 }
15353 }
15354 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15355 {
15356 IkReal evalcond[8];
15357 IkReal x645=IKsin(j10);
15358 IkReal x646=IKcos(j10);
15359 IkReal x647=((1.0)*sj8);
15360 IkReal x648=((1.0)*x645);
15361 IkReal x649=(sj8*x646);
15362 IkReal x650=((1.0)*x646);
15363 IkReal x651=(cj8*x648);
15364 evalcond[0]=(((new_r10*sj8))+((cj8*new_r00))+x646);
15365 evalcond[1]=(((new_r11*sj8))+(((-1.0)*x648))+((cj8*new_r01)));
15366 evalcond[2]=(((sj8*x645))+((cj8*x646))+new_r00);
15367 evalcond[3]=(((cj8*new_r10))+(((-1.0)*x648))+(((-1.0)*new_r00*x647)));
15368 evalcond[4]=(((cj8*new_r11))+(((-1.0)*x650))+(((-1.0)*new_r01*x647)));
15369 evalcond[5]=((((-1.0)*x651))+x649+new_r01);
15370 evalcond[6]=((((-1.0)*x651))+x649+new_r10);
15371 evalcond[7]=((((-1.0)*cj8*x650))+(((-1.0)*x645*x647))+new_r11);
15372 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
15373 {
15374 continue;
15375 }
15376 }
15377 
15378 {
15379 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15380 vinfos[0].jointtype = 1;
15381 vinfos[0].foffset = j6;
15382 vinfos[0].indices[0] = _ij6[0];
15383 vinfos[0].indices[1] = _ij6[1];
15384 vinfos[0].maxsolutions = _nj6;
15385 vinfos[1].jointtype = 1;
15386 vinfos[1].foffset = j7;
15387 vinfos[1].indices[0] = _ij7[0];
15388 vinfos[1].indices[1] = _ij7[1];
15389 vinfos[1].maxsolutions = _nj7;
15390 vinfos[2].jointtype = 1;
15391 vinfos[2].foffset = j8;
15392 vinfos[2].indices[0] = _ij8[0];
15393 vinfos[2].indices[1] = _ij8[1];
15394 vinfos[2].maxsolutions = _nj8;
15395 vinfos[3].jointtype = 1;
15396 vinfos[3].foffset = j9;
15397 vinfos[3].indices[0] = _ij9[0];
15398 vinfos[3].indices[1] = _ij9[1];
15399 vinfos[3].maxsolutions = _nj9;
15400 vinfos[4].jointtype = 1;
15401 vinfos[4].foffset = j10;
15402 vinfos[4].indices[0] = _ij10[0];
15403 vinfos[4].indices[1] = _ij10[1];
15404 vinfos[4].maxsolutions = _nj10;
15405 vinfos[5].jointtype = 1;
15406 vinfos[5].foffset = j11;
15407 vinfos[5].indices[0] = _ij11[0];
15408 vinfos[5].indices[1] = _ij11[1];
15409 vinfos[5].maxsolutions = _nj11;
15410 vinfos[6].jointtype = 1;
15411 vinfos[6].foffset = j12;
15412 vinfos[6].indices[0] = _ij12[0];
15413 vinfos[6].indices[1] = _ij12[1];
15414 vinfos[6].maxsolutions = _nj12;
15415 std::vector<int> vfree(0);
15416 solutions.AddSolution(vinfos,vfree);
15417 }
15418 }
15419 }
15420 
15421 }
15422 } while(0);
15423 if( bgotonextstatement )
15424 {
15425 bool bgotonextstatement = true;
15426 do
15427 {
15428 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j8))), 6.28318530717959)));
15429 evalcond[1]=new_r12;
15430 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
15431 {
15432 bgotonextstatement=false;
15433 {
15434 IkReal j10array[1], cj10array[1], sj10array[1];
15435 bool j10valid[1]={false};
15436 _nj10 = 1;
15437 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
15438     continue;
15439 j10array[0]=IKatan2(new_r10, new_r11);
15440 sj10array[0]=IKsin(j10array[0]);
15441 cj10array[0]=IKcos(j10array[0]);
15442 if( j10array[0] > IKPI )
15443 {
15444     j10array[0]-=IK2PI;
15445 }
15446 else if( j10array[0] < -IKPI )
15447 {    j10array[0]+=IK2PI;
15448 }
15449 j10valid[0] = true;
15450 for(int ij10 = 0; ij10 < 1; ++ij10)
15451 {
15452 if( !j10valid[ij10] )
15453 {
15454     continue;
15455 }
15456 _ij10[0] = ij10; _ij10[1] = -1;
15457 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15458 {
15459 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15460 {
15461     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15462 }
15463 }
15464 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15465 {
15466 IkReal evalcond[8];
15467 IkReal x652=IKcos(j10);
15468 IkReal x653=IKsin(j10);
15469 IkReal x654=((1.0)*sj9);
15470 IkReal x655=((1.0)*x652);
15471 evalcond[0]=(new_r20+((sj9*x652)));
15472 evalcond[1]=((((-1.0)*x653))+new_r10);
15473 evalcond[2]=((((-1.0)*x655))+new_r11);
15474 evalcond[3]=(((cj9*x653))+new_r01);
15475 evalcond[4]=(new_r21+(((-1.0)*x653*x654)));
15476 evalcond[5]=((((-1.0)*cj9*x655))+new_r00);
15477 evalcond[6]=(((cj9*new_r01))+x653+(((-1.0)*new_r21*x654)));
15478 evalcond[7]=(((cj9*new_r00))+(((-1.0)*x655))+(((-1.0)*new_r20*x654)));
15479 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
15480 {
15481 continue;
15482 }
15483 }
15484 
15485 {
15486 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15487 vinfos[0].jointtype = 1;
15488 vinfos[0].foffset = j6;
15489 vinfos[0].indices[0] = _ij6[0];
15490 vinfos[0].indices[1] = _ij6[1];
15491 vinfos[0].maxsolutions = _nj6;
15492 vinfos[1].jointtype = 1;
15493 vinfos[1].foffset = j7;
15494 vinfos[1].indices[0] = _ij7[0];
15495 vinfos[1].indices[1] = _ij7[1];
15496 vinfos[1].maxsolutions = _nj7;
15497 vinfos[2].jointtype = 1;
15498 vinfos[2].foffset = j8;
15499 vinfos[2].indices[0] = _ij8[0];
15500 vinfos[2].indices[1] = _ij8[1];
15501 vinfos[2].maxsolutions = _nj8;
15502 vinfos[3].jointtype = 1;
15503 vinfos[3].foffset = j9;
15504 vinfos[3].indices[0] = _ij9[0];
15505 vinfos[3].indices[1] = _ij9[1];
15506 vinfos[3].maxsolutions = _nj9;
15507 vinfos[4].jointtype = 1;
15508 vinfos[4].foffset = j10;
15509 vinfos[4].indices[0] = _ij10[0];
15510 vinfos[4].indices[1] = _ij10[1];
15511 vinfos[4].maxsolutions = _nj10;
15512 vinfos[5].jointtype = 1;
15513 vinfos[5].foffset = j11;
15514 vinfos[5].indices[0] = _ij11[0];
15515 vinfos[5].indices[1] = _ij11[1];
15516 vinfos[5].maxsolutions = _nj11;
15517 vinfos[6].jointtype = 1;
15518 vinfos[6].foffset = j12;
15519 vinfos[6].indices[0] = _ij12[0];
15520 vinfos[6].indices[1] = _ij12[1];
15521 vinfos[6].maxsolutions = _nj12;
15522 std::vector<int> vfree(0);
15523 solutions.AddSolution(vinfos,vfree);
15524 }
15525 }
15526 }
15527 
15528 }
15529 } while(0);
15530 if( bgotonextstatement )
15531 {
15532 bool bgotonextstatement = true;
15533 do
15534 {
15535 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j8)))), 6.28318530717959)));
15536 evalcond[1]=new_r12;
15537 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  )
15538 {
15539 bgotonextstatement=false;
15540 {
15541 IkReal j10eval[3];
15542 sj8=0;
15543 cj8=-1.0;
15544 j8=3.14159265358979;
15545 j10eval[0]=sj9;
15546 j10eval[1]=IKsign(sj9);
15547 j10eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
15548 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  || IKabs(j10eval[2]) < 0.0000010000000000  )
15549 {
15550 {
15551 IkReal j10eval[1];
15552 sj8=0;
15553 cj8=-1.0;
15554 j8=3.14159265358979;
15555 j10eval[0]=sj9;
15556 if( IKabs(j10eval[0]) < 0.0000010000000000  )
15557 {
15558 {
15559 IkReal j10eval[2];
15560 sj8=0;
15561 cj8=-1.0;
15562 j8=3.14159265358979;
15563 j10eval[0]=cj9;
15564 j10eval[1]=sj9;
15565 if( IKabs(j10eval[0]) < 0.0000010000000000  || IKabs(j10eval[1]) < 0.0000010000000000  )
15566 {
15567 {
15568 IkReal evalcond[4];
15569 bool bgotonextstatement = true;
15570 do
15571 {
15572 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j9)))), 6.28318530717959)));
15573 evalcond[1]=new_r22;
15574 evalcond[2]=new_r01;
15575 evalcond[3]=new_r00;
15576 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
15577 {
15578 bgotonextstatement=false;
15579 {
15580 IkReal j10array[1], cj10array[1], sj10array[1];
15581 bool j10valid[1]={false};
15582 _nj10 = 1;
15583 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
15584     continue;
15585 j10array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
15586 sj10array[0]=IKsin(j10array[0]);
15587 cj10array[0]=IKcos(j10array[0]);
15588 if( j10array[0] > IKPI )
15589 {
15590     j10array[0]-=IK2PI;
15591 }
15592 else if( j10array[0] < -IKPI )
15593 {    j10array[0]+=IK2PI;
15594 }
15595 j10valid[0] = true;
15596 for(int ij10 = 0; ij10 < 1; ++ij10)
15597 {
15598 if( !j10valid[ij10] )
15599 {
15600     continue;
15601 }
15602 _ij10[0] = ij10; _ij10[1] = -1;
15603 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15604 {
15605 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15606 {
15607     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15608 }
15609 }
15610 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15611 {
15612 IkReal evalcond[4];
15613 IkReal x656=IKcos(j10);
15614 IkReal x657=((1.0)*(IKsin(j10)));
15615 evalcond[0]=(x656+new_r20);
15616 evalcond[1]=((((-1.0)*x657))+new_r21);
15617 evalcond[2]=((((-1.0)*x657))+(((-1.0)*new_r10)));
15618 evalcond[3]=((((-1.0)*x656))+(((-1.0)*new_r11)));
15619 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
15620 {
15621 continue;
15622 }
15623 }
15624 
15625 {
15626 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15627 vinfos[0].jointtype = 1;
15628 vinfos[0].foffset = j6;
15629 vinfos[0].indices[0] = _ij6[0];
15630 vinfos[0].indices[1] = _ij6[1];
15631 vinfos[0].maxsolutions = _nj6;
15632 vinfos[1].jointtype = 1;
15633 vinfos[1].foffset = j7;
15634 vinfos[1].indices[0] = _ij7[0];
15635 vinfos[1].indices[1] = _ij7[1];
15636 vinfos[1].maxsolutions = _nj7;
15637 vinfos[2].jointtype = 1;
15638 vinfos[2].foffset = j8;
15639 vinfos[2].indices[0] = _ij8[0];
15640 vinfos[2].indices[1] = _ij8[1];
15641 vinfos[2].maxsolutions = _nj8;
15642 vinfos[3].jointtype = 1;
15643 vinfos[3].foffset = j9;
15644 vinfos[3].indices[0] = _ij9[0];
15645 vinfos[3].indices[1] = _ij9[1];
15646 vinfos[3].maxsolutions = _nj9;
15647 vinfos[4].jointtype = 1;
15648 vinfos[4].foffset = j10;
15649 vinfos[4].indices[0] = _ij10[0];
15650 vinfos[4].indices[1] = _ij10[1];
15651 vinfos[4].maxsolutions = _nj10;
15652 vinfos[5].jointtype = 1;
15653 vinfos[5].foffset = j11;
15654 vinfos[5].indices[0] = _ij11[0];
15655 vinfos[5].indices[1] = _ij11[1];
15656 vinfos[5].maxsolutions = _nj11;
15657 vinfos[6].jointtype = 1;
15658 vinfos[6].foffset = j12;
15659 vinfos[6].indices[0] = _ij12[0];
15660 vinfos[6].indices[1] = _ij12[1];
15661 vinfos[6].maxsolutions = _nj12;
15662 std::vector<int> vfree(0);
15663 solutions.AddSolution(vinfos,vfree);
15664 }
15665 }
15666 }
15667 
15668 }
15669 } while(0);
15670 if( bgotonextstatement )
15671 {
15672 bool bgotonextstatement = true;
15673 do
15674 {
15675 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j9)))), 6.28318530717959)));
15676 evalcond[1]=new_r22;
15677 evalcond[2]=new_r01;
15678 evalcond[3]=new_r00;
15679 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
15680 {
15681 bgotonextstatement=false;
15682 {
15683 IkReal j10array[1], cj10array[1], sj10array[1];
15684 bool j10valid[1]={false};
15685 _nj10 = 1;
15686 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
15687     continue;
15688 j10array[0]=IKatan2(((-1.0)*new_r21), new_r20);
15689 sj10array[0]=IKsin(j10array[0]);
15690 cj10array[0]=IKcos(j10array[0]);
15691 if( j10array[0] > IKPI )
15692 {
15693     j10array[0]-=IK2PI;
15694 }
15695 else if( j10array[0] < -IKPI )
15696 {    j10array[0]+=IK2PI;
15697 }
15698 j10valid[0] = true;
15699 for(int ij10 = 0; ij10 < 1; ++ij10)
15700 {
15701 if( !j10valid[ij10] )
15702 {
15703     continue;
15704 }
15705 _ij10[0] = ij10; _ij10[1] = -1;
15706 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15707 {
15708 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15709 {
15710     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15711 }
15712 }
15713 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15714 {
15715 IkReal evalcond[4];
15716 IkReal x658=IKsin(j10);
15717 IkReal x659=((1.0)*(IKcos(j10)));
15718 evalcond[0]=(x658+new_r21);
15719 evalcond[1]=((((-1.0)*x659))+new_r20);
15720 evalcond[2]=((((-1.0)*x658))+(((-1.0)*new_r10)));
15721 evalcond[3]=((((-1.0)*x659))+(((-1.0)*new_r11)));
15722 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
15723 {
15724 continue;
15725 }
15726 }
15727 
15728 {
15729 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15730 vinfos[0].jointtype = 1;
15731 vinfos[0].foffset = j6;
15732 vinfos[0].indices[0] = _ij6[0];
15733 vinfos[0].indices[1] = _ij6[1];
15734 vinfos[0].maxsolutions = _nj6;
15735 vinfos[1].jointtype = 1;
15736 vinfos[1].foffset = j7;
15737 vinfos[1].indices[0] = _ij7[0];
15738 vinfos[1].indices[1] = _ij7[1];
15739 vinfos[1].maxsolutions = _nj7;
15740 vinfos[2].jointtype = 1;
15741 vinfos[2].foffset = j8;
15742 vinfos[2].indices[0] = _ij8[0];
15743 vinfos[2].indices[1] = _ij8[1];
15744 vinfos[2].maxsolutions = _nj8;
15745 vinfos[3].jointtype = 1;
15746 vinfos[3].foffset = j9;
15747 vinfos[3].indices[0] = _ij9[0];
15748 vinfos[3].indices[1] = _ij9[1];
15749 vinfos[3].maxsolutions = _nj9;
15750 vinfos[4].jointtype = 1;
15751 vinfos[4].foffset = j10;
15752 vinfos[4].indices[0] = _ij10[0];
15753 vinfos[4].indices[1] = _ij10[1];
15754 vinfos[4].maxsolutions = _nj10;
15755 vinfos[5].jointtype = 1;
15756 vinfos[5].foffset = j11;
15757 vinfos[5].indices[0] = _ij11[0];
15758 vinfos[5].indices[1] = _ij11[1];
15759 vinfos[5].maxsolutions = _nj11;
15760 vinfos[6].jointtype = 1;
15761 vinfos[6].foffset = j12;
15762 vinfos[6].indices[0] = _ij12[0];
15763 vinfos[6].indices[1] = _ij12[1];
15764 vinfos[6].maxsolutions = _nj12;
15765 std::vector<int> vfree(0);
15766 solutions.AddSolution(vinfos,vfree);
15767 }
15768 }
15769 }
15770 
15771 }
15772 } while(0);
15773 if( bgotonextstatement )
15774 {
15775 bool bgotonextstatement = true;
15776 do
15777 {
15778 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j9))), 6.28318530717959)));
15779 evalcond[1]=new_r20;
15780 evalcond[2]=new_r02;
15781 evalcond[3]=new_r21;
15782 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
15783 {
15784 bgotonextstatement=false;
15785 {
15786 IkReal j10array[1], cj10array[1], sj10array[1];
15787 bool j10valid[1]={false};
15788 _nj10 = 1;
15789 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
15790     continue;
15791 j10array[0]=IKatan2(new_r01, ((-1.0)*new_r11));
15792 sj10array[0]=IKsin(j10array[0]);
15793 cj10array[0]=IKcos(j10array[0]);
15794 if( j10array[0] > IKPI )
15795 {
15796     j10array[0]-=IK2PI;
15797 }
15798 else if( j10array[0] < -IKPI )
15799 {    j10array[0]+=IK2PI;
15800 }
15801 j10valid[0] = true;
15802 for(int ij10 = 0; ij10 < 1; ++ij10)
15803 {
15804 if( !j10valid[ij10] )
15805 {
15806     continue;
15807 }
15808 _ij10[0] = ij10; _ij10[1] = -1;
15809 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15810 {
15811 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15812 {
15813     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15814 }
15815 }
15816 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15817 {
15818 IkReal evalcond[4];
15819 IkReal x660=IKsin(j10);
15820 IkReal x661=((1.0)*(IKcos(j10)));
15821 evalcond[0]=(x660+(((-1.0)*new_r01)));
15822 evalcond[1]=((((-1.0)*x660))+(((-1.0)*new_r10)));
15823 evalcond[2]=((((-1.0)*x661))+(((-1.0)*new_r11)));
15824 evalcond[3]=((((-1.0)*x661))+(((-1.0)*new_r00)));
15825 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
15826 {
15827 continue;
15828 }
15829 }
15830 
15831 {
15832 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15833 vinfos[0].jointtype = 1;
15834 vinfos[0].foffset = j6;
15835 vinfos[0].indices[0] = _ij6[0];
15836 vinfos[0].indices[1] = _ij6[1];
15837 vinfos[0].maxsolutions = _nj6;
15838 vinfos[1].jointtype = 1;
15839 vinfos[1].foffset = j7;
15840 vinfos[1].indices[0] = _ij7[0];
15841 vinfos[1].indices[1] = _ij7[1];
15842 vinfos[1].maxsolutions = _nj7;
15843 vinfos[2].jointtype = 1;
15844 vinfos[2].foffset = j8;
15845 vinfos[2].indices[0] = _ij8[0];
15846 vinfos[2].indices[1] = _ij8[1];
15847 vinfos[2].maxsolutions = _nj8;
15848 vinfos[3].jointtype = 1;
15849 vinfos[3].foffset = j9;
15850 vinfos[3].indices[0] = _ij9[0];
15851 vinfos[3].indices[1] = _ij9[1];
15852 vinfos[3].maxsolutions = _nj9;
15853 vinfos[4].jointtype = 1;
15854 vinfos[4].foffset = j10;
15855 vinfos[4].indices[0] = _ij10[0];
15856 vinfos[4].indices[1] = _ij10[1];
15857 vinfos[4].maxsolutions = _nj10;
15858 vinfos[5].jointtype = 1;
15859 vinfos[5].foffset = j11;
15860 vinfos[5].indices[0] = _ij11[0];
15861 vinfos[5].indices[1] = _ij11[1];
15862 vinfos[5].maxsolutions = _nj11;
15863 vinfos[6].jointtype = 1;
15864 vinfos[6].foffset = j12;
15865 vinfos[6].indices[0] = _ij12[0];
15866 vinfos[6].indices[1] = _ij12[1];
15867 vinfos[6].maxsolutions = _nj12;
15868 std::vector<int> vfree(0);
15869 solutions.AddSolution(vinfos,vfree);
15870 }
15871 }
15872 }
15873 
15874 }
15875 } while(0);
15876 if( bgotonextstatement )
15877 {
15878 bool bgotonextstatement = true;
15879 do
15880 {
15881 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j9)))), 6.28318530717959)));
15882 evalcond[1]=new_r20;
15883 evalcond[2]=new_r02;
15884 evalcond[3]=new_r21;
15885 if( IKabs(evalcond[0]) < 0.0000050000000000  && IKabs(evalcond[1]) < 0.0000050000000000  && IKabs(evalcond[2]) < 0.0000050000000000  && IKabs(evalcond[3]) < 0.0000050000000000  )
15886 {
15887 bgotonextstatement=false;
15888 {
15889 IkReal j10array[1], cj10array[1], sj10array[1];
15890 bool j10valid[1]={false};
15891 _nj10 = 1;
15892 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
15893     continue;
15894 j10array[0]=IKatan2(((-1.0)*new_r10), new_r00);
15895 sj10array[0]=IKsin(j10array[0]);
15896 cj10array[0]=IKcos(j10array[0]);
15897 if( j10array[0] > IKPI )
15898 {
15899     j10array[0]-=IK2PI;
15900 }
15901 else if( j10array[0] < -IKPI )
15902 {    j10array[0]+=IK2PI;
15903 }
15904 j10valid[0] = true;
15905 for(int ij10 = 0; ij10 < 1; ++ij10)
15906 {
15907 if( !j10valid[ij10] )
15908 {
15909     continue;
15910 }
15911 _ij10[0] = ij10; _ij10[1] = -1;
15912 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15913 {
15914 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15915 {
15916     j10valid[iij10]=false; _ij10[1] = iij10; break; 
15917 }
15918 }
15919 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15920 {
15921 IkReal evalcond[4];
15922 IkReal x662=IKcos(j10);
15923 IkReal x663=((1.0)*(IKsin(j10)));
15924 evalcond[0]=(x662+(((-1.0)*new_r00)));
15925 evalcond[1]=((((-1.0)*x663))+(((-1.0)*new_r10)));
15926 evalcond[2]=((((-1.0)*x662))+(((-1.0)*new_r11)));
15927 evalcond[3]=((((-1.0)*x663))+(((-1.0)*new_r01)));
15928 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  )
15929 {
15930 continue;
15931 }
15932 }
15933 
15934 {
15935 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15936 vinfos[0].jointtype = 1;
15937 vinfos[0].foffset = j6;
15938 vinfos[0].indices[0] = _ij6[0];
15939 vinfos[0].indices[1] = _ij6[1];
15940 vinfos[0].maxsolutions = _nj6;
15941 vinfos[1].jointtype = 1;
15942 vinfos[1].foffset = j7;
15943 vinfos[1].indices[0] = _ij7[0];
15944 vinfos[1].indices[1] = _ij7[1];
15945 vinfos[1].maxsolutions = _nj7;
15946 vinfos[2].jointtype = 1;
15947 vinfos[2].foffset = j8;
15948 vinfos[2].indices[0] = _ij8[0];
15949 vinfos[2].indices[1] = _ij8[1];
15950 vinfos[2].maxsolutions = _nj8;
15951 vinfos[3].jointtype = 1;
15952 vinfos[3].foffset = j9;
15953 vinfos[3].indices[0] = _ij9[0];
15954 vinfos[3].indices[1] = _ij9[1];
15955 vinfos[3].maxsolutions = _nj9;
15956 vinfos[4].jointtype = 1;
15957 vinfos[4].foffset = j10;
15958 vinfos[4].indices[0] = _ij10[0];
15959 vinfos[4].indices[1] = _ij10[1];
15960 vinfos[4].maxsolutions = _nj10;
15961 vinfos[5].jointtype = 1;
15962 vinfos[5].foffset = j11;
15963 vinfos[5].indices[0] = _ij11[0];
15964 vinfos[5].indices[1] = _ij11[1];
15965 vinfos[5].maxsolutions = _nj11;
15966 vinfos[6].jointtype = 1;
15967 vinfos[6].foffset = j12;
15968 vinfos[6].indices[0] = _ij12[0];
15969 vinfos[6].indices[1] = _ij12[1];
15970 vinfos[6].maxsolutions = _nj12;
15971 std::vector<int> vfree(0);
15972 solutions.AddSolution(vinfos,vfree);
15973 }
15974 }
15975 }
15976 
15977 }
15978 } while(0);
15979 if( bgotonextstatement )
15980 {
15981 bool bgotonextstatement = true;
15982 do
15983 {
15984 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
15985 if( IKabs(evalcond[0]) < 0.0000050000000000  )
15986 {
15987 bgotonextstatement=false;
15988 {
15989 IkReal j10array[1], cj10array[1], sj10array[1];
15990 bool j10valid[1]={false};
15991 _nj10 = 1;
15992 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
15993     continue;
15994 j10array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
15995 sj10array[0]=IKsin(j10array[0]);
15996 cj10array[0]=IKcos(j10array[0]);
15997 if( j10array[0] > IKPI )
15998 {
15999     j10array[0]-=IK2PI;
16000 }
16001 else if( j10array[0] < -IKPI )
16002 {    j10array[0]+=IK2PI;
16003 }
16004 j10valid[0] = true;
16005 for(int ij10 = 0; ij10 < 1; ++ij10)
16006 {
16007 if( !j10valid[ij10] )
16008 {
16009     continue;
16010 }
16011 _ij10[0] = ij10; _ij10[1] = -1;
16012 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16013 {
16014 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16015 {
16016     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16017 }
16018 }
16019 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16020 {
16021 IkReal evalcond[6];
16022 IkReal x664=IKsin(j10);
16023 IkReal x665=IKcos(j10);
16024 IkReal x666=((-1.0)*x665);
16025 evalcond[0]=x664;
16026 evalcond[1]=(new_r22*x664);
16027 evalcond[2]=x666;
16028 evalcond[3]=(new_r22*x666);
16029 evalcond[4]=((((-1.0)*x664))+(((-1.0)*new_r10)));
16030 evalcond[5]=((((-1.0)*x665))+(((-1.0)*new_r11)));
16031 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  )
16032 {
16033 continue;
16034 }
16035 }
16036 
16037 {
16038 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16039 vinfos[0].jointtype = 1;
16040 vinfos[0].foffset = j6;
16041 vinfos[0].indices[0] = _ij6[0];
16042 vinfos[0].indices[1] = _ij6[1];
16043 vinfos[0].maxsolutions = _nj6;
16044 vinfos[1].jointtype = 1;
16045 vinfos[1].foffset = j7;
16046 vinfos[1].indices[0] = _ij7[0];
16047 vinfos[1].indices[1] = _ij7[1];
16048 vinfos[1].maxsolutions = _nj7;
16049 vinfos[2].jointtype = 1;
16050 vinfos[2].foffset = j8;
16051 vinfos[2].indices[0] = _ij8[0];
16052 vinfos[2].indices[1] = _ij8[1];
16053 vinfos[2].maxsolutions = _nj8;
16054 vinfos[3].jointtype = 1;
16055 vinfos[3].foffset = j9;
16056 vinfos[3].indices[0] = _ij9[0];
16057 vinfos[3].indices[1] = _ij9[1];
16058 vinfos[3].maxsolutions = _nj9;
16059 vinfos[4].jointtype = 1;
16060 vinfos[4].foffset = j10;
16061 vinfos[4].indices[0] = _ij10[0];
16062 vinfos[4].indices[1] = _ij10[1];
16063 vinfos[4].maxsolutions = _nj10;
16064 vinfos[5].jointtype = 1;
16065 vinfos[5].foffset = j11;
16066 vinfos[5].indices[0] = _ij11[0];
16067 vinfos[5].indices[1] = _ij11[1];
16068 vinfos[5].maxsolutions = _nj11;
16069 vinfos[6].jointtype = 1;
16070 vinfos[6].foffset = j12;
16071 vinfos[6].indices[0] = _ij12[0];
16072 vinfos[6].indices[1] = _ij12[1];
16073 vinfos[6].maxsolutions = _nj12;
16074 std::vector<int> vfree(0);
16075 solutions.AddSolution(vinfos,vfree);
16076 }
16077 }
16078 }
16079 
16080 }
16081 } while(0);
16082 if( bgotonextstatement )
16083 {
16084 bool bgotonextstatement = true;
16085 do
16086 {
16087 if( 1 )
16088 {
16089 bgotonextstatement=false;
16090 continue; // branch miss [j10]
16091 
16092 }
16093 } while(0);
16094 if( bgotonextstatement )
16095 {
16096 }
16097 }
16098 }
16099 }
16100 }
16101 }
16102 }
16103 
16104 } else
16105 {
16106 {
16107 IkReal j10array[1], cj10array[1], sj10array[1];
16108 bool j10valid[1]={false};
16109 _nj10 = 1;
16110 CheckValue<IkReal> x667=IKPowWithIntegerCheck(cj9,-1);
16111 if(!x667.valid){
16112 continue;
16113 }
16114 CheckValue<IkReal> x668=IKPowWithIntegerCheck(sj9,-1);
16115 if(!x668.valid){
16116 continue;
16117 }
16118 if( IKabs((new_r01*(x667.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x668.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r01*(x667.value)))+IKsqr(((-1.0)*new_r20*(x668.value)))-1) <= IKFAST_SINCOS_THRESH )
16119     continue;
16120 j10array[0]=IKatan2((new_r01*(x667.value)), ((-1.0)*new_r20*(x668.value)));
16121 sj10array[0]=IKsin(j10array[0]);
16122 cj10array[0]=IKcos(j10array[0]);
16123 if( j10array[0] > IKPI )
16124 {
16125     j10array[0]-=IK2PI;
16126 }
16127 else if( j10array[0] < -IKPI )
16128 {    j10array[0]+=IK2PI;
16129 }
16130 j10valid[0] = true;
16131 for(int ij10 = 0; ij10 < 1; ++ij10)
16132 {
16133 if( !j10valid[ij10] )
16134 {
16135     continue;
16136 }
16137 _ij10[0] = ij10; _ij10[1] = -1;
16138 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16139 {
16140 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16141 {
16142     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16143 }
16144 }
16145 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16146 {
16147 IkReal evalcond[8];
16148 IkReal x669=IKsin(j10);
16149 IkReal x670=IKcos(j10);
16150 IkReal x671=((1.0)*sj9);
16151 IkReal x672=((1.0)*new_r01);
16152 IkReal x673=((1.0)*new_r00);
16153 IkReal x674=((1.0)*x670);
16154 evalcond[0]=(((sj9*x670))+new_r20);
16155 evalcond[1]=((((-1.0)*x669*x671))+new_r21);
16156 evalcond[2]=((((-1.0)*x669))+(((-1.0)*new_r10)));
16157 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x674)));
16158 evalcond[4]=(((cj9*x669))+(((-1.0)*x672)));
16159 evalcond[5]=((((-1.0)*cj9*x674))+(((-1.0)*x673)));
16160 evalcond[6]=((((-1.0)*new_r21*x671))+(((-1.0)*cj9*x672))+x669);
16161 evalcond[7]=((((-1.0)*new_r20*x671))+(((-1.0)*cj9*x673))+(((-1.0)*x674)));
16162 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
16163 {
16164 continue;
16165 }
16166 }
16167 
16168 {
16169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16170 vinfos[0].jointtype = 1;
16171 vinfos[0].foffset = j6;
16172 vinfos[0].indices[0] = _ij6[0];
16173 vinfos[0].indices[1] = _ij6[1];
16174 vinfos[0].maxsolutions = _nj6;
16175 vinfos[1].jointtype = 1;
16176 vinfos[1].foffset = j7;
16177 vinfos[1].indices[0] = _ij7[0];
16178 vinfos[1].indices[1] = _ij7[1];
16179 vinfos[1].maxsolutions = _nj7;
16180 vinfos[2].jointtype = 1;
16181 vinfos[2].foffset = j8;
16182 vinfos[2].indices[0] = _ij8[0];
16183 vinfos[2].indices[1] = _ij8[1];
16184 vinfos[2].maxsolutions = _nj8;
16185 vinfos[3].jointtype = 1;
16186 vinfos[3].foffset = j9;
16187 vinfos[3].indices[0] = _ij9[0];
16188 vinfos[3].indices[1] = _ij9[1];
16189 vinfos[3].maxsolutions = _nj9;
16190 vinfos[4].jointtype = 1;
16191 vinfos[4].foffset = j10;
16192 vinfos[4].indices[0] = _ij10[0];
16193 vinfos[4].indices[1] = _ij10[1];
16194 vinfos[4].maxsolutions = _nj10;
16195 vinfos[5].jointtype = 1;
16196 vinfos[5].foffset = j11;
16197 vinfos[5].indices[0] = _ij11[0];
16198 vinfos[5].indices[1] = _ij11[1];
16199 vinfos[5].maxsolutions = _nj11;
16200 vinfos[6].jointtype = 1;
16201 vinfos[6].foffset = j12;
16202 vinfos[6].indices[0] = _ij12[0];
16203 vinfos[6].indices[1] = _ij12[1];
16204 vinfos[6].maxsolutions = _nj12;
16205 std::vector<int> vfree(0);
16206 solutions.AddSolution(vinfos,vfree);
16207 }
16208 }
16209 }
16210 
16211 }
16212 
16213 }
16214 
16215 } else
16216 {
16217 {
16218 IkReal j10array[1], cj10array[1], sj10array[1];
16219 bool j10valid[1]={false};
16220 _nj10 = 1;
16221 CheckValue<IkReal> x675=IKPowWithIntegerCheck(sj9,-1);
16222 if(!x675.valid){
16223 continue;
16224 }
16225 if( IKabs((new_r21*(x675.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r21*(x675.value)))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
16226     continue;
16227 j10array[0]=IKatan2((new_r21*(x675.value)), ((-1.0)*new_r11));
16228 sj10array[0]=IKsin(j10array[0]);
16229 cj10array[0]=IKcos(j10array[0]);
16230 if( j10array[0] > IKPI )
16231 {
16232     j10array[0]-=IK2PI;
16233 }
16234 else if( j10array[0] < -IKPI )
16235 {    j10array[0]+=IK2PI;
16236 }
16237 j10valid[0] = true;
16238 for(int ij10 = 0; ij10 < 1; ++ij10)
16239 {
16240 if( !j10valid[ij10] )
16241 {
16242     continue;
16243 }
16244 _ij10[0] = ij10; _ij10[1] = -1;
16245 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16246 {
16247 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16248 {
16249     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16250 }
16251 }
16252 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16253 {
16254 IkReal evalcond[8];
16255 IkReal x676=IKsin(j10);
16256 IkReal x677=IKcos(j10);
16257 IkReal x678=((1.0)*sj9);
16258 IkReal x679=((1.0)*new_r01);
16259 IkReal x680=((1.0)*new_r00);
16260 IkReal x681=((1.0)*x677);
16261 evalcond[0]=(((sj9*x677))+new_r20);
16262 evalcond[1]=((((-1.0)*x676*x678))+new_r21);
16263 evalcond[2]=((((-1.0)*x676))+(((-1.0)*new_r10)));
16264 evalcond[3]=((((-1.0)*x681))+(((-1.0)*new_r11)));
16265 evalcond[4]=(((cj9*x676))+(((-1.0)*x679)));
16266 evalcond[5]=((((-1.0)*cj9*x681))+(((-1.0)*x680)));
16267 evalcond[6]=((((-1.0)*new_r21*x678))+(((-1.0)*cj9*x679))+x676);
16268 evalcond[7]=((((-1.0)*cj9*x680))+(((-1.0)*x681))+(((-1.0)*new_r20*x678)));
16269 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
16270 {
16271 continue;
16272 }
16273 }
16274 
16275 {
16276 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16277 vinfos[0].jointtype = 1;
16278 vinfos[0].foffset = j6;
16279 vinfos[0].indices[0] = _ij6[0];
16280 vinfos[0].indices[1] = _ij6[1];
16281 vinfos[0].maxsolutions = _nj6;
16282 vinfos[1].jointtype = 1;
16283 vinfos[1].foffset = j7;
16284 vinfos[1].indices[0] = _ij7[0];
16285 vinfos[1].indices[1] = _ij7[1];
16286 vinfos[1].maxsolutions = _nj7;
16287 vinfos[2].jointtype = 1;
16288 vinfos[2].foffset = j8;
16289 vinfos[2].indices[0] = _ij8[0];
16290 vinfos[2].indices[1] = _ij8[1];
16291 vinfos[2].maxsolutions = _nj8;
16292 vinfos[3].jointtype = 1;
16293 vinfos[3].foffset = j9;
16294 vinfos[3].indices[0] = _ij9[0];
16295 vinfos[3].indices[1] = _ij9[1];
16296 vinfos[3].maxsolutions = _nj9;
16297 vinfos[4].jointtype = 1;
16298 vinfos[4].foffset = j10;
16299 vinfos[4].indices[0] = _ij10[0];
16300 vinfos[4].indices[1] = _ij10[1];
16301 vinfos[4].maxsolutions = _nj10;
16302 vinfos[5].jointtype = 1;
16303 vinfos[5].foffset = j11;
16304 vinfos[5].indices[0] = _ij11[0];
16305 vinfos[5].indices[1] = _ij11[1];
16306 vinfos[5].maxsolutions = _nj11;
16307 vinfos[6].jointtype = 1;
16308 vinfos[6].foffset = j12;
16309 vinfos[6].indices[0] = _ij12[0];
16310 vinfos[6].indices[1] = _ij12[1];
16311 vinfos[6].maxsolutions = _nj12;
16312 std::vector<int> vfree(0);
16313 solutions.AddSolution(vinfos,vfree);
16314 }
16315 }
16316 }
16317 
16318 }
16319 
16320 }
16321 
16322 } else
16323 {
16324 {
16325 IkReal j10array[1], cj10array[1], sj10array[1];
16326 bool j10valid[1]={false};
16327 _nj10 = 1;
16328 CheckValue<IkReal> x682 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
16329 if(!x682.valid){
16330 continue;
16331 }
16332 CheckValue<IkReal> x683=IKPowWithIntegerCheck(IKsign(sj9),-1);
16333 if(!x683.valid){
16334 continue;
16335 }
16336 j10array[0]=((-1.5707963267949)+(x682.value)+(((1.5707963267949)*(x683.value))));
16337 sj10array[0]=IKsin(j10array[0]);
16338 cj10array[0]=IKcos(j10array[0]);
16339 if( j10array[0] > IKPI )
16340 {
16341     j10array[0]-=IK2PI;
16342 }
16343 else if( j10array[0] < -IKPI )
16344 {    j10array[0]+=IK2PI;
16345 }
16346 j10valid[0] = true;
16347 for(int ij10 = 0; ij10 < 1; ++ij10)
16348 {
16349 if( !j10valid[ij10] )
16350 {
16351     continue;
16352 }
16353 _ij10[0] = ij10; _ij10[1] = -1;
16354 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16355 {
16356 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16357 {
16358     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16359 }
16360 }
16361 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16362 {
16363 IkReal evalcond[8];
16364 IkReal x684=IKsin(j10);
16365 IkReal x685=IKcos(j10);
16366 IkReal x686=((1.0)*sj9);
16367 IkReal x687=((1.0)*new_r01);
16368 IkReal x688=((1.0)*new_r00);
16369 IkReal x689=((1.0)*x685);
16370 evalcond[0]=(((sj9*x685))+new_r20);
16371 evalcond[1]=((((-1.0)*x684*x686))+new_r21);
16372 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x684)));
16373 evalcond[3]=((((-1.0)*x689))+(((-1.0)*new_r11)));
16374 evalcond[4]=(((cj9*x684))+(((-1.0)*x687)));
16375 evalcond[5]=((((-1.0)*cj9*x689))+(((-1.0)*x688)));
16376 evalcond[6]=((((-1.0)*cj9*x687))+(((-1.0)*new_r21*x686))+x684);
16377 evalcond[7]=((((-1.0)*cj9*x688))+(((-1.0)*x689))+(((-1.0)*new_r20*x686)));
16378 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  )
16379 {
16380 continue;
16381 }
16382 }
16383 
16384 {
16385 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16386 vinfos[0].jointtype = 1;
16387 vinfos[0].foffset = j6;
16388 vinfos[0].indices[0] = _ij6[0];
16389 vinfos[0].indices[1] = _ij6[1];
16390 vinfos[0].maxsolutions = _nj6;
16391 vinfos[1].jointtype = 1;
16392 vinfos[1].foffset = j7;
16393 vinfos[1].indices[0] = _ij7[0];
16394 vinfos[1].indices[1] = _ij7[1];
16395 vinfos[1].maxsolutions = _nj7;
16396 vinfos[2].jointtype = 1;
16397 vinfos[2].foffset = j8;
16398 vinfos[2].indices[0] = _ij8[0];
16399 vinfos[2].indices[1] = _ij8[1];
16400 vinfos[2].maxsolutions = _nj8;
16401 vinfos[3].jointtype = 1;
16402 vinfos[3].foffset = j9;
16403 vinfos[3].indices[0] = _ij9[0];
16404 vinfos[3].indices[1] = _ij9[1];
16405 vinfos[3].maxsolutions = _nj9;
16406 vinfos[4].jointtype = 1;
16407 vinfos[4].foffset = j10;
16408 vinfos[4].indices[0] = _ij10[0];
16409 vinfos[4].indices[1] = _ij10[1];
16410 vinfos[4].maxsolutions = _nj10;
16411 vinfos[5].jointtype = 1;
16412 vinfos[5].foffset = j11;
16413 vinfos[5].indices[0] = _ij11[0];
16414 vinfos[5].indices[1] = _ij11[1];
16415 vinfos[5].maxsolutions = _nj11;
16416 vinfos[6].jointtype = 1;
16417 vinfos[6].foffset = j12;
16418 vinfos[6].indices[0] = _ij12[0];
16419 vinfos[6].indices[1] = _ij12[1];
16420 vinfos[6].maxsolutions = _nj12;
16421 std::vector<int> vfree(0);
16422 solutions.AddSolution(vinfos,vfree);
16423 }
16424 }
16425 }
16426 
16427 }
16428 
16429 }
16430 
16431 }
16432 } while(0);
16433 if( bgotonextstatement )
16434 {
16435 bool bgotonextstatement = true;
16436 do
16437 {
16438 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
16439 if( IKabs(evalcond[0]) < 0.0000050000000000  )
16440 {
16441 bgotonextstatement=false;
16442 {
16443 IkReal j10eval[1];
16444 new_r21=0;
16445 new_r20=0;
16446 new_r02=0;
16447 new_r12=0;
16448 j10eval[0]=IKabs(new_r22);
16449 if( IKabs(j10eval[0]) < 0.0000000100000000  )
16450 {
16451 continue; // no branches [j10]
16452 
16453 } else
16454 {
16455 IkReal op[2+1], zeror[2];
16456 int numroots;
16457 op[0]=new_r22;
16458 op[1]=0;
16459 op[2]=((-1.0)*new_r22);
16460 polyroots2(op,zeror,numroots);
16461 IkReal j10array[2], cj10array[2], sj10array[2], tempj10array[1];
16462 int numsolutions = 0;
16463 for(int ij10 = 0; ij10 < numroots; ++ij10)
16464 {
16465 IkReal htj10 = zeror[ij10];
16466 tempj10array[0]=((2.0)*(atan(htj10)));
16467 for(int kj10 = 0; kj10 < 1; ++kj10)
16468 {
16469 j10array[numsolutions] = tempj10array[kj10];
16470 if( j10array[numsolutions] > IKPI )
16471 {
16472     j10array[numsolutions]-=IK2PI;
16473 }
16474 else if( j10array[numsolutions] < -IKPI )
16475 {
16476     j10array[numsolutions]+=IK2PI;
16477 }
16478 sj10array[numsolutions] = IKsin(j10array[numsolutions]);
16479 cj10array[numsolutions] = IKcos(j10array[numsolutions]);
16480 numsolutions++;
16481 }
16482 }
16483 bool j10valid[2]={true,true};
16484 _nj10 = 2;
16485 for(int ij10 = 0; ij10 < numsolutions; ++ij10)
16486     {
16487 if( !j10valid[ij10] )
16488 {
16489     continue;
16490 }
16491     j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16492 htj10 = IKtan(j10/2);
16493 
16494 _ij10[0] = ij10; _ij10[1] = -1;
16495 for(int iij10 = ij10+1; iij10 < numsolutions; ++iij10)
16496 {
16497 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16498 {
16499     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16500 }
16501 }
16502 {
16503 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16504 vinfos[0].jointtype = 1;
16505 vinfos[0].foffset = j6;
16506 vinfos[0].indices[0] = _ij6[0];
16507 vinfos[0].indices[1] = _ij6[1];
16508 vinfos[0].maxsolutions = _nj6;
16509 vinfos[1].jointtype = 1;
16510 vinfos[1].foffset = j7;
16511 vinfos[1].indices[0] = _ij7[0];
16512 vinfos[1].indices[1] = _ij7[1];
16513 vinfos[1].maxsolutions = _nj7;
16514 vinfos[2].jointtype = 1;
16515 vinfos[2].foffset = j8;
16516 vinfos[2].indices[0] = _ij8[0];
16517 vinfos[2].indices[1] = _ij8[1];
16518 vinfos[2].maxsolutions = _nj8;
16519 vinfos[3].jointtype = 1;
16520 vinfos[3].foffset = j9;
16521 vinfos[3].indices[0] = _ij9[0];
16522 vinfos[3].indices[1] = _ij9[1];
16523 vinfos[3].maxsolutions = _nj9;
16524 vinfos[4].jointtype = 1;
16525 vinfos[4].foffset = j10;
16526 vinfos[4].indices[0] = _ij10[0];
16527 vinfos[4].indices[1] = _ij10[1];
16528 vinfos[4].maxsolutions = _nj10;
16529 vinfos[5].jointtype = 1;
16530 vinfos[5].foffset = j11;
16531 vinfos[5].indices[0] = _ij11[0];
16532 vinfos[5].indices[1] = _ij11[1];
16533 vinfos[5].maxsolutions = _nj11;
16534 vinfos[6].jointtype = 1;
16535 vinfos[6].foffset = j12;
16536 vinfos[6].indices[0] = _ij12[0];
16537 vinfos[6].indices[1] = _ij12[1];
16538 vinfos[6].maxsolutions = _nj12;
16539 std::vector<int> vfree(0);
16540 solutions.AddSolution(vinfos,vfree);
16541 }
16542     }
16543 
16544 }
16545 
16546 }
16547 
16548 }
16549 } while(0);
16550 if( bgotonextstatement )
16551 {
16552 bool bgotonextstatement = true;
16553 do
16554 {
16555 if( 1 )
16556 {
16557 bgotonextstatement=false;
16558 continue; // branch miss [j10]
16559 
16560 }
16561 } while(0);
16562 if( bgotonextstatement )
16563 {
16564 }
16565 }
16566 }
16567 }
16568 }
16569 }
16570 }
16571 }
16572 }
16573 }
16574 }
16575 
16576 } else
16577 {
16578 {
16579 IkReal j10array[1], cj10array[1], sj10array[1];
16580 bool j10valid[1]={false};
16581 _nj10 = 1;
16582 CheckValue<IkReal> x691=IKPowWithIntegerCheck(sj9,-1);
16583 if(!x691.valid){
16584 continue;
16585 }
16586 IkReal x690=x691.value;
16587 CheckValue<IkReal> x692=IKPowWithIntegerCheck(cj8,-1);
16588 if(!x692.valid){
16589 continue;
16590 }
16591 CheckValue<IkReal> x693=IKPowWithIntegerCheck(cj9,-1);
16592 if(!x693.valid){
16593 continue;
16594 }
16595 if( IKabs((x690*(x692.value)*(x693.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x690)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x690*(x692.value)*(x693.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9))))))+IKsqr(((-1.0)*new_r20*x690))-1) <= IKFAST_SINCOS_THRESH )
16596     continue;
16597 j10array[0]=IKatan2((x690*(x692.value)*(x693.value)*((((new_r20*sj8))+(((-1.0)*new_r01*sj9))))), ((-1.0)*new_r20*x690));
16598 sj10array[0]=IKsin(j10array[0]);
16599 cj10array[0]=IKcos(j10array[0]);
16600 if( j10array[0] > IKPI )
16601 {
16602     j10array[0]-=IK2PI;
16603 }
16604 else if( j10array[0] < -IKPI )
16605 {    j10array[0]+=IK2PI;
16606 }
16607 j10valid[0] = true;
16608 for(int ij10 = 0; ij10 < 1; ++ij10)
16609 {
16610 if( !j10valid[ij10] )
16611 {
16612     continue;
16613 }
16614 _ij10[0] = ij10; _ij10[1] = -1;
16615 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16616 {
16617 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16618 {
16619     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16620 }
16621 }
16622 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16623 {
16624 IkReal evalcond[12];
16625 IkReal x694=IKsin(j10);
16626 IkReal x695=IKcos(j10);
16627 IkReal x696=(cj9*sj8);
16628 IkReal x697=((1.0)*sj9);
16629 IkReal x698=((1.0)*sj8);
16630 IkReal x699=((1.0)*cj8);
16631 IkReal x700=(cj8*new_r00);
16632 IkReal x701=(cj8*new_r01);
16633 IkReal x702=((1.0)*x695);
16634 IkReal x703=(cj9*x694);
16635 IkReal x704=(cj9*x702);
16636 evalcond[0]=(new_r20+((sj9*x695)));
16637 evalcond[1]=((((-1.0)*x694*x697))+new_r21);
16638 evalcond[2]=(((new_r11*sj8))+x703+x701);
16639 evalcond[3]=((((-1.0)*new_r00*x698))+((cj8*new_r10))+(((-1.0)*x694)));
16640 evalcond[4]=((((-1.0)*new_r01*x698))+((cj8*new_r11))+(((-1.0)*x702)));
16641 evalcond[5]=(((cj8*x703))+((sj8*x695))+new_r01);
16642 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x704))+x700);
16643 evalcond[7]=((((-1.0)*cj9*x695*x699))+((sj8*x694))+new_r00);
16644 evalcond[8]=(((x694*x696))+(((-1.0)*x695*x699))+new_r11);
16645 evalcond[9]=((((-1.0)*x696*x702))+(((-1.0)*x694*x699))+new_r10);
16646 evalcond[10]=(((new_r11*x696))+x694+((cj9*x701))+(((-1.0)*new_r21*x697)));
16647 evalcond[11]=(((new_r10*x696))+(((-1.0)*x702))+((cj9*x700))+(((-1.0)*new_r20*x697)));
16648 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
16649 {
16650 continue;
16651 }
16652 }
16653 
16654 {
16655 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16656 vinfos[0].jointtype = 1;
16657 vinfos[0].foffset = j6;
16658 vinfos[0].indices[0] = _ij6[0];
16659 vinfos[0].indices[1] = _ij6[1];
16660 vinfos[0].maxsolutions = _nj6;
16661 vinfos[1].jointtype = 1;
16662 vinfos[1].foffset = j7;
16663 vinfos[1].indices[0] = _ij7[0];
16664 vinfos[1].indices[1] = _ij7[1];
16665 vinfos[1].maxsolutions = _nj7;
16666 vinfos[2].jointtype = 1;
16667 vinfos[2].foffset = j8;
16668 vinfos[2].indices[0] = _ij8[0];
16669 vinfos[2].indices[1] = _ij8[1];
16670 vinfos[2].maxsolutions = _nj8;
16671 vinfos[3].jointtype = 1;
16672 vinfos[3].foffset = j9;
16673 vinfos[3].indices[0] = _ij9[0];
16674 vinfos[3].indices[1] = _ij9[1];
16675 vinfos[3].maxsolutions = _nj9;
16676 vinfos[4].jointtype = 1;
16677 vinfos[4].foffset = j10;
16678 vinfos[4].indices[0] = _ij10[0];
16679 vinfos[4].indices[1] = _ij10[1];
16680 vinfos[4].maxsolutions = _nj10;
16681 vinfos[5].jointtype = 1;
16682 vinfos[5].foffset = j11;
16683 vinfos[5].indices[0] = _ij11[0];
16684 vinfos[5].indices[1] = _ij11[1];
16685 vinfos[5].maxsolutions = _nj11;
16686 vinfos[6].jointtype = 1;
16687 vinfos[6].foffset = j12;
16688 vinfos[6].indices[0] = _ij12[0];
16689 vinfos[6].indices[1] = _ij12[1];
16690 vinfos[6].maxsolutions = _nj12;
16691 std::vector<int> vfree(0);
16692 solutions.AddSolution(vinfos,vfree);
16693 }
16694 }
16695 }
16696 
16697 }
16698 
16699 }
16700 
16701 } else
16702 {
16703 {
16704 IkReal j10array[1], cj10array[1], sj10array[1];
16705 bool j10valid[1]={false};
16706 _nj10 = 1;
16707 CheckValue<IkReal> x706=IKPowWithIntegerCheck(sj9,-1);
16708 if(!x706.valid){
16709 continue;
16710 }
16711 IkReal x705=x706.value;
16712 CheckValue<IkReal> x707=IKPowWithIntegerCheck(sj8,-1);
16713 if(!x707.valid){
16714 continue;
16715 }
16716 if( IKabs((x705*(x707.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x705)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x705*(x707.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9))))))+IKsqr(((-1.0)*new_r20*x705))-1) <= IKFAST_SINCOS_THRESH )
16717     continue;
16718 j10array[0]=IKatan2((x705*(x707.value)*(((((-1.0)*cj8*cj9*new_r20))+(((-1.0)*new_r00*sj9))))), ((-1.0)*new_r20*x705));
16719 sj10array[0]=IKsin(j10array[0]);
16720 cj10array[0]=IKcos(j10array[0]);
16721 if( j10array[0] > IKPI )
16722 {
16723     j10array[0]-=IK2PI;
16724 }
16725 else if( j10array[0] < -IKPI )
16726 {    j10array[0]+=IK2PI;
16727 }
16728 j10valid[0] = true;
16729 for(int ij10 = 0; ij10 < 1; ++ij10)
16730 {
16731 if( !j10valid[ij10] )
16732 {
16733     continue;
16734 }
16735 _ij10[0] = ij10; _ij10[1] = -1;
16736 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16737 {
16738 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16739 {
16740     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16741 }
16742 }
16743 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16744 {
16745 IkReal evalcond[12];
16746 IkReal x708=IKsin(j10);
16747 IkReal x709=IKcos(j10);
16748 IkReal x710=(cj9*sj8);
16749 IkReal x711=((1.0)*sj9);
16750 IkReal x712=((1.0)*sj8);
16751 IkReal x713=((1.0)*cj8);
16752 IkReal x714=(cj8*new_r00);
16753 IkReal x715=(cj8*new_r01);
16754 IkReal x716=((1.0)*x709);
16755 IkReal x717=(cj9*x708);
16756 IkReal x718=(cj9*x716);
16757 evalcond[0]=(((sj9*x709))+new_r20);
16758 evalcond[1]=((((-1.0)*x708*x711))+new_r21);
16759 evalcond[2]=(((new_r11*sj8))+x715+x717);
16760 evalcond[3]=((((-1.0)*x708))+((cj8*new_r10))+(((-1.0)*new_r00*x712)));
16761 evalcond[4]=(((cj8*new_r11))+(((-1.0)*x716))+(((-1.0)*new_r01*x712)));
16762 evalcond[5]=(((cj8*x717))+new_r01+((sj8*x709)));
16763 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x718))+x714);
16764 evalcond[7]=((((-1.0)*cj9*x709*x713))+new_r00+((sj8*x708)));
16765 evalcond[8]=((((-1.0)*x709*x713))+new_r11+((x708*x710)));
16766 evalcond[9]=((((-1.0)*x710*x716))+(((-1.0)*x708*x713))+new_r10);
16767 evalcond[10]=((((-1.0)*new_r21*x711))+((new_r11*x710))+x708+((cj9*x715)));
16768 evalcond[11]=((((-1.0)*new_r20*x711))+(((-1.0)*x716))+((new_r10*x710))+((cj9*x714)));
16769 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
16770 {
16771 continue;
16772 }
16773 }
16774 
16775 {
16776 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16777 vinfos[0].jointtype = 1;
16778 vinfos[0].foffset = j6;
16779 vinfos[0].indices[0] = _ij6[0];
16780 vinfos[0].indices[1] = _ij6[1];
16781 vinfos[0].maxsolutions = _nj6;
16782 vinfos[1].jointtype = 1;
16783 vinfos[1].foffset = j7;
16784 vinfos[1].indices[0] = _ij7[0];
16785 vinfos[1].indices[1] = _ij7[1];
16786 vinfos[1].maxsolutions = _nj7;
16787 vinfos[2].jointtype = 1;
16788 vinfos[2].foffset = j8;
16789 vinfos[2].indices[0] = _ij8[0];
16790 vinfos[2].indices[1] = _ij8[1];
16791 vinfos[2].maxsolutions = _nj8;
16792 vinfos[3].jointtype = 1;
16793 vinfos[3].foffset = j9;
16794 vinfos[3].indices[0] = _ij9[0];
16795 vinfos[3].indices[1] = _ij9[1];
16796 vinfos[3].maxsolutions = _nj9;
16797 vinfos[4].jointtype = 1;
16798 vinfos[4].foffset = j10;
16799 vinfos[4].indices[0] = _ij10[0];
16800 vinfos[4].indices[1] = _ij10[1];
16801 vinfos[4].maxsolutions = _nj10;
16802 vinfos[5].jointtype = 1;
16803 vinfos[5].foffset = j11;
16804 vinfos[5].indices[0] = _ij11[0];
16805 vinfos[5].indices[1] = _ij11[1];
16806 vinfos[5].maxsolutions = _nj11;
16807 vinfos[6].jointtype = 1;
16808 vinfos[6].foffset = j12;
16809 vinfos[6].indices[0] = _ij12[0];
16810 vinfos[6].indices[1] = _ij12[1];
16811 vinfos[6].maxsolutions = _nj12;
16812 std::vector<int> vfree(0);
16813 solutions.AddSolution(vinfos,vfree);
16814 }
16815 }
16816 }
16817 
16818 }
16819 
16820 }
16821 
16822 } else
16823 {
16824 {
16825 IkReal j10array[1], cj10array[1], sj10array[1];
16826 bool j10valid[1]={false};
16827 _nj10 = 1;
16828 CheckValue<IkReal> x719 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
16829 if(!x719.valid){
16830 continue;
16831 }
16832 CheckValue<IkReal> x720=IKPowWithIntegerCheck(IKsign(sj9),-1);
16833 if(!x720.valid){
16834 continue;
16835 }
16836 j10array[0]=((-1.5707963267949)+(x719.value)+(((1.5707963267949)*(x720.value))));
16837 sj10array[0]=IKsin(j10array[0]);
16838 cj10array[0]=IKcos(j10array[0]);
16839 if( j10array[0] > IKPI )
16840 {
16841     j10array[0]-=IK2PI;
16842 }
16843 else if( j10array[0] < -IKPI )
16844 {    j10array[0]+=IK2PI;
16845 }
16846 j10valid[0] = true;
16847 for(int ij10 = 0; ij10 < 1; ++ij10)
16848 {
16849 if( !j10valid[ij10] )
16850 {
16851     continue;
16852 }
16853 _ij10[0] = ij10; _ij10[1] = -1;
16854 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16855 {
16856 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16857 {
16858     j10valid[iij10]=false; _ij10[1] = iij10; break; 
16859 }
16860 }
16861 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16862 {
16863 IkReal evalcond[12];
16864 IkReal x721=IKsin(j10);
16865 IkReal x722=IKcos(j10);
16866 IkReal x723=(cj9*sj8);
16867 IkReal x724=((1.0)*sj9);
16868 IkReal x725=((1.0)*sj8);
16869 IkReal x726=((1.0)*cj8);
16870 IkReal x727=(cj8*new_r00);
16871 IkReal x728=(cj8*new_r01);
16872 IkReal x729=((1.0)*x722);
16873 IkReal x730=(cj9*x721);
16874 IkReal x731=(cj9*x729);
16875 evalcond[0]=(((sj9*x722))+new_r20);
16876 evalcond[1]=((((-1.0)*x721*x724))+new_r21);
16877 evalcond[2]=(((new_r11*sj8))+x730+x728);
16878 evalcond[3]=(((cj8*new_r10))+(((-1.0)*x721))+(((-1.0)*new_r00*x725)));
16879 evalcond[4]=(((cj8*new_r11))+(((-1.0)*x729))+(((-1.0)*new_r01*x725)));
16880 evalcond[5]=(((sj8*x722))+((cj8*x730))+new_r01);
16881 evalcond[6]=(((new_r10*sj8))+(((-1.0)*x731))+x727);
16882 evalcond[7]=((((-1.0)*cj9*x722*x726))+((sj8*x721))+new_r00);
16883 evalcond[8]=(((x721*x723))+(((-1.0)*x722*x726))+new_r11);
16884 evalcond[9]=((((-1.0)*x723*x729))+(((-1.0)*x721*x726))+new_r10);
16885 evalcond[10]=((((-1.0)*new_r21*x724))+x721+((new_r11*x723))+((cj9*x728)));
16886 evalcond[11]=((((-1.0)*new_r20*x724))+(((-1.0)*x729))+((cj9*x727))+((new_r10*x723)));
16887 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH  )
16888 {
16889 continue;
16890 }
16891 }
16892 
16893 {
16894 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16895 vinfos[0].jointtype = 1;
16896 vinfos[0].foffset = j6;
16897 vinfos[0].indices[0] = _ij6[0];
16898 vinfos[0].indices[1] = _ij6[1];
16899 vinfos[0].maxsolutions = _nj6;
16900 vinfos[1].jointtype = 1;
16901 vinfos[1].foffset = j7;
16902 vinfos[1].indices[0] = _ij7[0];
16903 vinfos[1].indices[1] = _ij7[1];
16904 vinfos[1].maxsolutions = _nj7;
16905 vinfos[2].jointtype = 1;
16906 vinfos[2].foffset = j8;
16907 vinfos[2].indices[0] = _ij8[0];
16908 vinfos[2].indices[1] = _ij8[1];
16909 vinfos[2].maxsolutions = _nj8;
16910 vinfos[3].jointtype = 1;
16911 vinfos[3].foffset = j9;
16912 vinfos[3].indices[0] = _ij9[0];
16913 vinfos[3].indices[1] = _ij9[1];
16914 vinfos[3].maxsolutions = _nj9;
16915 vinfos[4].jointtype = 1;
16916 vinfos[4].foffset = j10;
16917 vinfos[4].indices[0] = _ij10[0];
16918 vinfos[4].indices[1] = _ij10[1];
16919 vinfos[4].maxsolutions = _nj10;
16920 vinfos[5].jointtype = 1;
16921 vinfos[5].foffset = j11;
16922 vinfos[5].indices[0] = _ij11[0];
16923 vinfos[5].indices[1] = _ij11[1];
16924 vinfos[5].maxsolutions = _nj11;
16925 vinfos[6].jointtype = 1;
16926 vinfos[6].foffset = j12;
16927 vinfos[6].indices[0] = _ij12[0];
16928 vinfos[6].indices[1] = _ij12[1];
16929 vinfos[6].maxsolutions = _nj12;
16930 std::vector<int> vfree(0);
16931 solutions.AddSolution(vinfos,vfree);
16932 }
16933 }
16934 }
16935 
16936 }
16937 
16938 }
16939 }
16940 }
16941 
16942 }
16943 
16944 }
16945 }
16946 }
16947 }
16948 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
16949 {
16950     using std::complex;
16951     if( rawcoeffs[0] == 0 ) {
16952         // solve with one reduced degree
16953         polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
16954         return;
16955     }
16956     IKFAST_ASSERT(rawcoeffs[0] != 0);
16957     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
16958     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
16959     complex<IkReal> coeffs[3];
16960     const int maxsteps = 110;
16961     for(int i = 0; i < 3; ++i) {
16962         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
16963     }
16964     complex<IkReal> roots[3];
16965     IkReal err[3];
16966     roots[0] = complex<IkReal>(1,0);
16967     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
16968     err[0] = 1.0;
16969     err[1] = 1.0;
16970     for(int i = 2; i < 3; ++i) {
16971         roots[i] = roots[i-1]*roots[1];
16972         err[i] = 1.0;
16973     }
16974     for(int step = 0; step < maxsteps; ++step) {
16975         bool changed = false;
16976         for(int i = 0; i < 3; ++i) {
16977             if ( err[i] >= tol ) {
16978                 changed = true;
16979                 // evaluate
16980                 complex<IkReal> x = roots[i] + coeffs[0];
16981                 for(int j = 1; j < 3; ++j) {
16982                     x = roots[i] * x + coeffs[j];
16983                 }
16984                 for(int j = 0; j < 3; ++j) {
16985                     if( i != j ) {
16986                         if( roots[i] != roots[j] ) {
16987                             x /= (roots[i] - roots[j]);
16988                         }
16989                     }
16990                 }
16991                 roots[i] -= x;
16992                 err[i] = abs(x);
16993             }
16994         }
16995         if( !changed ) {
16996             break;
16997         }
16998     }
16999 
17000     numroots = 0;
17001     bool visited[3] = {false};
17002     for(int i = 0; i < 3; ++i) {
17003         if( !visited[i] ) {
17004             // might be a multiple root, in which case it will have more error than the other roots
17005             // find any neighboring roots, and take the average
17006             complex<IkReal> newroot=roots[i];
17007             int n = 1;
17008             for(int j = i+1; j < 3; ++j) {
17009                 // care about error in real much more than imaginary
17010                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17011                     newroot += roots[j];
17012                     n += 1;
17013                     visited[j] = true;
17014                 }
17015             }
17016             if( n > 1 ) {
17017                 newroot /= n;
17018             }
17019             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17020             if( IKabs(imag(newroot)) < tolsqrt ) {
17021                 rawroots[numroots++] = real(newroot);
17022             }
17023         }
17024     }
17025 }
17026 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
17027     IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
17028     if( det < 0 ) {
17029         numroots=0;
17030     }
17031     else if( det == 0 ) {
17032         rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
17033         numroots = 1;
17034     }
17035     else {
17036         det = IKsqrt(det);
17037         rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
17038         rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
17039         numroots = 2;
17040     }
17041 }
17042 static inline void polyroots5(IkReal rawcoeffs[5+1], IkReal rawroots[5], int& numroots)
17043 {
17044     using std::complex;
17045     if( rawcoeffs[0] == 0 ) {
17046         // solve with one reduced degree
17047         polyroots4(&rawcoeffs[1], &rawroots[0], numroots);
17048         return;
17049     }
17050     IKFAST_ASSERT(rawcoeffs[0] != 0);
17051     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17052     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
17053     complex<IkReal> coeffs[5];
17054     const int maxsteps = 110;
17055     for(int i = 0; i < 5; ++i) {
17056         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
17057     }
17058     complex<IkReal> roots[5];
17059     IkReal err[5];
17060     roots[0] = complex<IkReal>(1,0);
17061     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
17062     err[0] = 1.0;
17063     err[1] = 1.0;
17064     for(int i = 2; i < 5; ++i) {
17065         roots[i] = roots[i-1]*roots[1];
17066         err[i] = 1.0;
17067     }
17068     for(int step = 0; step < maxsteps; ++step) {
17069         bool changed = false;
17070         for(int i = 0; i < 5; ++i) {
17071             if ( err[i] >= tol ) {
17072                 changed = true;
17073                 // evaluate
17074                 complex<IkReal> x = roots[i] + coeffs[0];
17075                 for(int j = 1; j < 5; ++j) {
17076                     x = roots[i] * x + coeffs[j];
17077                 }
17078                 for(int j = 0; j < 5; ++j) {
17079                     if( i != j ) {
17080                         if( roots[i] != roots[j] ) {
17081                             x /= (roots[i] - roots[j]);
17082                         }
17083                     }
17084                 }
17085                 roots[i] -= x;
17086                 err[i] = abs(x);
17087             }
17088         }
17089         if( !changed ) {
17090             break;
17091         }
17092     }
17093 
17094     numroots = 0;
17095     bool visited[5] = {false};
17096     for(int i = 0; i < 5; ++i) {
17097         if( !visited[i] ) {
17098             // might be a multiple root, in which case it will have more error than the other roots
17099             // find any neighboring roots, and take the average
17100             complex<IkReal> newroot=roots[i];
17101             int n = 1;
17102             for(int j = i+1; j < 5; ++j) {
17103                 // care about error in real much more than imaginary
17104                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17105                     newroot += roots[j];
17106                     n += 1;
17107                     visited[j] = true;
17108                 }
17109             }
17110             if( n > 1 ) {
17111                 newroot /= n;
17112             }
17113             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17114             if( IKabs(imag(newroot)) < tolsqrt ) {
17115                 rawroots[numroots++] = real(newroot);
17116             }
17117         }
17118     }
17119 }
17120 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
17121 {
17122     using std::complex;
17123     if( rawcoeffs[0] == 0 ) {
17124         // solve with one reduced degree
17125         polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
17126         return;
17127     }
17128     IKFAST_ASSERT(rawcoeffs[0] != 0);
17129     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17130     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
17131     complex<IkReal> coeffs[4];
17132     const int maxsteps = 110;
17133     for(int i = 0; i < 4; ++i) {
17134         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
17135     }
17136     complex<IkReal> roots[4];
17137     IkReal err[4];
17138     roots[0] = complex<IkReal>(1,0);
17139     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
17140     err[0] = 1.0;
17141     err[1] = 1.0;
17142     for(int i = 2; i < 4; ++i) {
17143         roots[i] = roots[i-1]*roots[1];
17144         err[i] = 1.0;
17145     }
17146     for(int step = 0; step < maxsteps; ++step) {
17147         bool changed = false;
17148         for(int i = 0; i < 4; ++i) {
17149             if ( err[i] >= tol ) {
17150                 changed = true;
17151                 // evaluate
17152                 complex<IkReal> x = roots[i] + coeffs[0];
17153                 for(int j = 1; j < 4; ++j) {
17154                     x = roots[i] * x + coeffs[j];
17155                 }
17156                 for(int j = 0; j < 4; ++j) {
17157                     if( i != j ) {
17158                         if( roots[i] != roots[j] ) {
17159                             x /= (roots[i] - roots[j]);
17160                         }
17161                     }
17162                 }
17163                 roots[i] -= x;
17164                 err[i] = abs(x);
17165             }
17166         }
17167         if( !changed ) {
17168             break;
17169         }
17170     }
17171 
17172     numroots = 0;
17173     bool visited[4] = {false};
17174     for(int i = 0; i < 4; ++i) {
17175         if( !visited[i] ) {
17176             // might be a multiple root, in which case it will have more error than the other roots
17177             // find any neighboring roots, and take the average
17178             complex<IkReal> newroot=roots[i];
17179             int n = 1;
17180             for(int j = i+1; j < 4; ++j) {
17181                 // care about error in real much more than imaginary
17182                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17183                     newroot += roots[j];
17184                     n += 1;
17185                     visited[j] = true;
17186                 }
17187             }
17188             if( n > 1 ) {
17189                 newroot /= n;
17190             }
17191             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17192             if( IKabs(imag(newroot)) < tolsqrt ) {
17193                 rawroots[numroots++] = real(newroot);
17194             }
17195         }
17196     }
17197 }
17198 static inline void polyroots7(IkReal rawcoeffs[7+1], IkReal rawroots[7], int& numroots)
17199 {
17200     using std::complex;
17201     if( rawcoeffs[0] == 0 ) {
17202         // solve with one reduced degree
17203         polyroots6(&rawcoeffs[1], &rawroots[0], numroots);
17204         return;
17205     }
17206     IKFAST_ASSERT(rawcoeffs[0] != 0);
17207     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17208     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
17209     complex<IkReal> coeffs[7];
17210     const int maxsteps = 110;
17211     for(int i = 0; i < 7; ++i) {
17212         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
17213     }
17214     complex<IkReal> roots[7];
17215     IkReal err[7];
17216     roots[0] = complex<IkReal>(1,0);
17217     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
17218     err[0] = 1.0;
17219     err[1] = 1.0;
17220     for(int i = 2; i < 7; ++i) {
17221         roots[i] = roots[i-1]*roots[1];
17222         err[i] = 1.0;
17223     }
17224     for(int step = 0; step < maxsteps; ++step) {
17225         bool changed = false;
17226         for(int i = 0; i < 7; ++i) {
17227             if ( err[i] >= tol ) {
17228                 changed = true;
17229                 // evaluate
17230                 complex<IkReal> x = roots[i] + coeffs[0];
17231                 for(int j = 1; j < 7; ++j) {
17232                     x = roots[i] * x + coeffs[j];
17233                 }
17234                 for(int j = 0; j < 7; ++j) {
17235                     if( i != j ) {
17236                         if( roots[i] != roots[j] ) {
17237                             x /= (roots[i] - roots[j]);
17238                         }
17239                     }
17240                 }
17241                 roots[i] -= x;
17242                 err[i] = abs(x);
17243             }
17244         }
17245         if( !changed ) {
17246             break;
17247         }
17248     }
17249 
17250     numroots = 0;
17251     bool visited[7] = {false};
17252     for(int i = 0; i < 7; ++i) {
17253         if( !visited[i] ) {
17254             // might be a multiple root, in which case it will have more error than the other roots
17255             // find any neighboring roots, and take the average
17256             complex<IkReal> newroot=roots[i];
17257             int n = 1;
17258             for(int j = i+1; j < 7; ++j) {
17259                 // care about error in real much more than imaginary
17260                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17261                     newroot += roots[j];
17262                     n += 1;
17263                     visited[j] = true;
17264                 }
17265             }
17266             if( n > 1 ) {
17267                 newroot /= n;
17268             }
17269             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17270             if( IKabs(imag(newroot)) < tolsqrt ) {
17271                 rawroots[numroots++] = real(newroot);
17272             }
17273         }
17274     }
17275 }
17276 static inline void polyroots6(IkReal rawcoeffs[6+1], IkReal rawroots[6], int& numroots)
17277 {
17278     using std::complex;
17279     if( rawcoeffs[0] == 0 ) {
17280         // solve with one reduced degree
17281         polyroots5(&rawcoeffs[1], &rawroots[0], numroots);
17282         return;
17283     }
17284     IKFAST_ASSERT(rawcoeffs[0] != 0);
17285     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17286     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
17287     complex<IkReal> coeffs[6];
17288     const int maxsteps = 110;
17289     for(int i = 0; i < 6; ++i) {
17290         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
17291     }
17292     complex<IkReal> roots[6];
17293     IkReal err[6];
17294     roots[0] = complex<IkReal>(1,0);
17295     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
17296     err[0] = 1.0;
17297     err[1] = 1.0;
17298     for(int i = 2; i < 6; ++i) {
17299         roots[i] = roots[i-1]*roots[1];
17300         err[i] = 1.0;
17301     }
17302     for(int step = 0; step < maxsteps; ++step) {
17303         bool changed = false;
17304         for(int i = 0; i < 6; ++i) {
17305             if ( err[i] >= tol ) {
17306                 changed = true;
17307                 // evaluate
17308                 complex<IkReal> x = roots[i] + coeffs[0];
17309                 for(int j = 1; j < 6; ++j) {
17310                     x = roots[i] * x + coeffs[j];
17311                 }
17312                 for(int j = 0; j < 6; ++j) {
17313                     if( i != j ) {
17314                         if( roots[i] != roots[j] ) {
17315                             x /= (roots[i] - roots[j]);
17316                         }
17317                     }
17318                 }
17319                 roots[i] -= x;
17320                 err[i] = abs(x);
17321             }
17322         }
17323         if( !changed ) {
17324             break;
17325         }
17326     }
17327 
17328     numroots = 0;
17329     bool visited[6] = {false};
17330     for(int i = 0; i < 6; ++i) {
17331         if( !visited[i] ) {
17332             // might be a multiple root, in which case it will have more error than the other roots
17333             // find any neighboring roots, and take the average
17334             complex<IkReal> newroot=roots[i];
17335             int n = 1;
17336             for(int j = i+1; j < 6; ++j) {
17337                 // care about error in real much more than imaginary
17338                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17339                     newroot += roots[j];
17340                     n += 1;
17341                     visited[j] = true;
17342                 }
17343             }
17344             if( n > 1 ) {
17345                 newroot /= n;
17346             }
17347             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17348             if( IKabs(imag(newroot)) < tolsqrt ) {
17349                 rawroots[numroots++] = real(newroot);
17350             }
17351         }
17352     }
17353 }
17354 static inline void polyroots8(IkReal rawcoeffs[8+1], IkReal rawroots[8], int& numroots)
17355 {
17356     using std::complex;
17357     if( rawcoeffs[0] == 0 ) {
17358         // solve with one reduced degree
17359         polyroots7(&rawcoeffs[1], &rawroots[0], numroots);
17360         return;
17361     }
17362     IKFAST_ASSERT(rawcoeffs[0] != 0);
17363     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17364     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
17365     complex<IkReal> coeffs[8];
17366     const int maxsteps = 110;
17367     for(int i = 0; i < 8; ++i) {
17368         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
17369     }
17370     complex<IkReal> roots[8];
17371     IkReal err[8];
17372     roots[0] = complex<IkReal>(1,0);
17373     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
17374     err[0] = 1.0;
17375     err[1] = 1.0;
17376     for(int i = 2; i < 8; ++i) {
17377         roots[i] = roots[i-1]*roots[1];
17378         err[i] = 1.0;
17379     }
17380     for(int step = 0; step < maxsteps; ++step) {
17381         bool changed = false;
17382         for(int i = 0; i < 8; ++i) {
17383             if ( err[i] >= tol ) {
17384                 changed = true;
17385                 // evaluate
17386                 complex<IkReal> x = roots[i] + coeffs[0];
17387                 for(int j = 1; j < 8; ++j) {
17388                     x = roots[i] * x + coeffs[j];
17389                 }
17390                 for(int j = 0; j < 8; ++j) {
17391                     if( i != j ) {
17392                         if( roots[i] != roots[j] ) {
17393                             x /= (roots[i] - roots[j]);
17394                         }
17395                     }
17396                 }
17397                 roots[i] -= x;
17398                 err[i] = abs(x);
17399             }
17400         }
17401         if( !changed ) {
17402             break;
17403         }
17404     }
17405 
17406     numroots = 0;
17407     bool visited[8] = {false};
17408     for(int i = 0; i < 8; ++i) {
17409         if( !visited[i] ) {
17410             // might be a multiple root, in which case it will have more error than the other roots
17411             // find any neighboring roots, and take the average
17412             complex<IkReal> newroot=roots[i];
17413             int n = 1;
17414             for(int j = i+1; j < 8; ++j) {
17415                 // care about error in real much more than imaginary
17416                 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
17417                     newroot += roots[j];
17418                     n += 1;
17419                     visited[j] = true;
17420                 }
17421             }
17422             if( n > 1 ) {
17423                 newroot /= n;
17424             }
17425             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
17426             if( IKabs(imag(newroot)) < tolsqrt ) {
17427                 rawroots[numroots++] = real(newroot);
17428             }
17429         }
17430     }
17431 }
17432 };
17433 
17434 
17437 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
17438 IKSolver solver;
17439 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
17440 }
17441 
17442 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
17443 IKSolver solver;
17444 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
17445 }
17446 
17447 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - fetch (a5ffee371c921db78a56e98f69ac61f1)>"; }
17448 
17449 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
17450 
17451 #ifdef IKFAST_NAMESPACE
17452 } // end namespace
17453 #endif
17454 
17455 #ifndef IKFAST_NO_MAIN
17456 #include <stdio.h>
17457 #include <stdlib.h>
17458 #ifdef IKFAST_NAMESPACE
17459 using namespace IKFAST_NAMESPACE;
17460 #endif
17461 int main(int argc, char** argv)
17462 {
17463     if( argc != 12+GetNumFreeParameters()+1 ) {
17464         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
17465                "Returns the ik solutions given the transformation of the end effector specified by\n"
17466                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
17467                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
17468         return 1;
17469     }
17470 
17471     IkSolutionList<IkReal> solutions;
17472     std::vector<IkReal> vfree(GetNumFreeParameters());
17473     IkReal eerot[9],eetrans[3];
17474     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
17475     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
17476     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
17477     for(std::size_t i = 0; i < vfree.size(); ++i)
17478         vfree[i] = atof(argv[13+i]);
17479     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
17480 
17481     if( !bSuccess ) {
17482         fprintf(stderr,"Failed to get ik solution\n");
17483         return -1;
17484     }
17485 
17486     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
17487     std::vector<IkReal> solvalues(GetNumJoints());
17488     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
17489         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
17490         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
17491         std::vector<IkReal> vsolfree(sol.GetFree().size());
17492         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
17493         for( std::size_t j = 0; j < solvalues.size(); ++j)
17494             printf("%.15f, ", solvalues[j]);
17495         printf("\n");
17496     }
17497     return 0;
17498 }
17499 
17500 #endif


fetch_ikfast_plugin
Author(s):
autogenerated on Sat Aug 5 2017 04:00:29