ikfast_lbr.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 // check if the included ikfast version matches what this file was compiled with
00025 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00026 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00027 
00028 #include <cmath>
00029 #include <vector>
00030 #include <limits>
00031 #include <algorithm>
00032 #include <complex>
00033 
00034 #define IKFAST_STRINGIZE2(s) #s
00035 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
00036 
00037 #ifndef IKFAST_ASSERT
00038 #include <stdexcept>
00039 #include <sstream>
00040 #include <iostream>
00041 
00042 #ifdef _MSC_VER
00043 #ifndef __PRETTY_FUNCTION__
00044 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00045 #endif
00046 #endif
00047 
00048 #ifndef __PRETTY_FUNCTION__
00049 #define __PRETTY_FUNCTION__ __func__
00050 #endif
00051 
00052 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
00053 
00054 #endif
00055 
00056 #if defined(_MSC_VER)
00057 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00058 #else
00059 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00060 #endif
00061 
00062 #define IK2PI  ((IkReal)6.28318530717959)
00063 #define IKPI  ((IkReal)3.14159265358979)
00064 #define IKPI_2  ((IkReal)1.57079632679490)
00065 
00066 #ifdef _MSC_VER
00067 #ifndef isnan
00068 #define isnan _isnan
00069 #endif
00070 #endif // _MSC_VER
00071 
00072 // lapack routines
00073 extern "C" {
00074   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00075   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00076   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00077   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00078   void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
00079   void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
00080 }
00081 
00082 using namespace std; // necessary to get std math routines
00083 
00084 #ifdef IKFAST_NAMESPACE
00085 namespace IKFAST_NAMESPACE {
00086 #endif
00087 
00088 inline float IKabs(float f) { return fabsf(f); }
00089 inline double IKabs(double f) { return fabs(f); }
00090 
00091 inline float IKsqr(float f) { return f*f; }
00092 inline double IKsqr(double f) { return f*f; }
00093 
00094 inline float IKlog(float f) { return logf(f); }
00095 inline double IKlog(double f) { return log(f); }
00096 
00097 // allows asin and acos to exceed 1
00098 #ifndef IKFAST_SINCOS_THRESH
00099 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00100 #endif
00101 
00102 // used to check input to atan2 for degenerate cases
00103 #ifndef IKFAST_ATAN2_MAGTHRESH
00104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00105 #endif
00106 
00107 // minimum distance of separate solutions
00108 #ifndef IKFAST_SOLUTION_THRESH
00109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00110 #endif
00111 
00112 inline float IKasin(float f)
00113 {
00114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00115 if( f <= -1 ) return float(-IKPI_2);
00116 else if( f >= 1 ) return float(IKPI_2);
00117 return asinf(f);
00118 }
00119 inline double IKasin(double f)
00120 {
00121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00122 if( f <= -1 ) return -IKPI_2;
00123 else if( f >= 1 ) return IKPI_2;
00124 return asin(f);
00125 }
00126 
00127 // return positive value in [0,y)
00128 inline float IKfmod(float x, float y)
00129 {
00130     while(x < 0) {
00131         x += y;
00132     }
00133     return fmodf(x,y);
00134 }
00135 
00136 // return positive value in [0,y)
00137 inline double IKfmod(double x, double y)
00138 {
00139     while(x < 0) {
00140         x += y;
00141     }
00142     return fmod(x,y);
00143 }
00144 
00145 inline float IKacos(float f)
00146 {
00147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00148 if( f <= -1 ) return float(IKPI);
00149 else if( f >= 1 ) return float(0);
00150 return acosf(f);
00151 }
00152 inline double IKacos(double f)
00153 {
00154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00155 if( f <= -1 ) return IKPI;
00156 else if( f >= 1 ) return 0;
00157 return acos(f);
00158 }
00159 inline float IKsin(float f) { return sinf(f); }
00160 inline double IKsin(double f) { return sin(f); }
00161 inline float IKcos(float f) { return cosf(f); }
00162 inline double IKcos(double f) { return cos(f); }
00163 inline float IKtan(float f) { return tanf(f); }
00164 inline double IKtan(double f) { return tan(f); }
00165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00167 inline float IKatan2(float fy, float fx) {
00168     if( isnan(fy) ) {
00169         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00170         return float(IKPI_2);
00171     }
00172     else if( isnan(fx) ) {
00173         return 0;
00174     }
00175     return atan2f(fy,fx);
00176 }
00177 inline double IKatan2(double fy, double fx) {
00178     if( isnan(fy) ) {
00179         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00180         return IKPI_2;
00181     }
00182     else if( isnan(fx) ) {
00183         return 0;
00184     }
00185     return atan2(fy,fx);
00186 }
00187 
00188 inline float IKsign(float f) {
00189     if( f > 0 ) {
00190         return float(1);
00191     }
00192     else if( f < 0 ) {
00193         return float(-1);
00194     }
00195     return 0;
00196 }
00197 
00198 inline double IKsign(double f) {
00199     if( f > 0 ) {
00200         return 1.0;
00201     }
00202     else if( f < 0 ) {
00203         return -1.0;
00204     }
00205     return 0;
00206 }
00207 
00210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00211 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65;
00212 x0=IKcos(j[1]);
00213 x1=IKsin(j[1]);
00214 x2=IKsin(j[3]);
00215 x3=IKcos(j[2]);
00216 x4=IKcos(j[3]);
00217 x5=IKcos(j[4]);
00218 x6=IKsin(j[2]);
00219 x7=IKsin(j[4]);
00220 x8=IKsin(j[6]);
00221 x9=IKcos(j[6]);
00222 x10=IKcos(j[5]);
00223 x11=IKsin(j[5]);
00224 x12=IKsin(j[0]);
00225 x13=IKcos(j[0]);
00226 x14=((IkReal(0.955072701792072))*(x13));
00227 x15=((IkReal(0.296371615192129))*(x12));
00228 x16=((IkReal(1.00000000000000))*(x7));
00229 x17=((IkReal(1.00000000000000))*(x10));
00230 x18=((IkReal(1.00000000000000))*(x5));
00231 x19=((IkReal(0.390000000000000))*(x4));
00232 x20=((IkReal(0.000500000000000000))*(x5));
00233 x21=((IkReal(0.0570000000000000))*(x7));
00234 x22=((IkReal(0.0570000000000000))*(x5));
00235 x23=((IkReal(1.00000000000000))*(x11));
00236 x24=((IkReal(0.390000000000000))*(x2));
00237 x25=((IkReal(0.000500000000000000))*(x4));
00238 x26=((x0)*(x3));
00239 x27=((x2)*(x6));
00240 x28=((x1)*(x6));
00241 x29=((x0)*(x6));
00242 x30=((x0)*(x2));
00243 x31=((x1)*(x4));
00244 x32=((x1)*(x2));
00245 x33=((x4)*(x6));
00246 x34=((x0)*(x4));
00247 x35=((((IkReal(-1.00000000000000))*(x14)))+(x15));
00248 x36=((((IkReal(-1.00000000000000))*(x15)))+(x14));
00249 x37=((IkReal(1.00000000000000))*(x30));
00250 x38=((IkReal(0.0570000000000000))*(x2)*(x3));
00251 x39=((((IkReal(-0.955072701792072))*(x12)))+(((IkReal(-0.296371615192129))*(x13))));
00252 x40=((x23)*(x27));
00253 x41=((x16)*(x29));
00254 x42=((x16)*(x28));
00255 x43=((((IkReal(-1.00000000000000))*(x18)*(x3)))+(((x33)*(x7))));
00256 x44=((((IkReal(-1.00000000000000))*(x31)))+(((x2)*(x26))));
00257 x45=((x32)+(((x26)*(x4))));
00258 x46=((((x3)*(x32)))+(x34));
00259 x47=((((IkReal(-1.00000000000000))*(x37)))+(((x3)*(x31))));
00260 x48=((((IkReal(-1.00000000000000))*(x18)*(x33)))+(((IkReal(-1.00000000000000))*(x16)*(x3))));
00261 x49=((((x18)*(x33)))+(((x16)*(x3))));
00262 x50=((x11)*(x44));
00263 x51=((x11)*(x46));
00264 x52=((x47)*(x5));
00265 x53=((((IkReal(-1.00000000000000))*(x41)))+(((x45)*(x5))));
00266 x54=((((IkReal(-1.00000000000000))*(x42)))+(x52));
00267 x55=((((x7)*(((((IkReal(-1.00000000000000))*(x3)*(x31)))+(x37)))))+(((IkReal(-1.00000000000000))*(x18)*(x28))));
00268 x56=((((IkReal(-1.00000000000000))*(x18)*(x29)))+(((x7)*(((((IkReal(-1.00000000000000))*(x32)))+(((IkReal(-1.00000000000000))*(x26)*(x4))))))));
00269 x57=((((x11)*(x49)))+(((IkReal(-1.00000000000000))*(x17)*(x27))));
00270 x58=((x10)*(x54));
00271 x59=((((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x45)))+(x41)))))+(((x10)*(x44))));
00272 x60=((((x9)*(((((IkReal(-1.00000000000000))*(x40)))+(((x10)*(x48)))))))+(((x43)*(x8))));
00273 x61=((((x8)*(((((IkReal(-1.00000000000000))*(x17)*(x48)))+(x40)))))+(((x43)*(x9))));
00274 x62=((((x3)*(((IkReal(-0.00200000000000000))+(((IkReal(-1.00000000000000))*(x20)))))))+(((IkReal(-0.0570000000000000))*(x10)*(x27)))+(((x11)*(((((x21)*(x3)))+(((x22)*(x33)))))))+(((x6)*(((((IkReal(-1.00000000000000))*(x24)))+(((x25)*(x7))))))));
00275 x63=((((x56)*(x8)))+(((x9)*(((((x10)*(x53)))+(x50))))));
00276 x64=((((x56)*(x9)))+(((x8)*(((((IkReal(-1.00000000000000))*(x17)*(x53)))+(((IkReal(-1.00000000000000))*(x23)*(x44))))))));
00277 x65=((((x7)*(((((IkReal(-1.00000000000000))*(x25)*(x26)))+(((IkReal(-0.000500000000000000))*(x32)))))))+(((x1)*(((IkReal(-0.400000000000000))+(((IkReal(-1.00000000000000))*(x19)))))))+(((IkReal(-0.00200000000000000))*(x29)))+(((x11)*(((((x21)*(x29)))+(((IkReal(-1.00000000000000))*(x22)*(x45)))))))+(((x10)*(((((IkReal(0.0570000000000000))*(x2)*(x26)))+(((IkReal(-0.0570000000000000))*(x31)))))))+(((x24)*(x26)))+(((IkReal(-1.00000000000000))*(x20)*(x29))));
00278 eerot[0]=((((x39)*(x60)))+(((x35)*(x63))));
00279 eerot[1]=((((x39)*(x61)))+(((x35)*(x64))));
00280 eerot[2]=((((x39)*(x57)))+(((x35)*(x59))));
00281 eetrans[0]=((((x39)*(x62)))+(((x35)*(x65)))+(((IkReal(-0.000592743230384259))*(x13)))+(((IkReal(-0.00191014540358414))*(x12))));
00282 eerot[3]=((((x39)*(x63)))+(((x36)*(x60))));
00283 eerot[4]=((((x39)*(x64)))+(((x36)*(x61))));
00284 eerot[5]=((((x39)*(x59)))+(((x36)*(x57))));
00285 eetrans[1]=((((x39)*(x65)))+(((x36)*(x62)))+(((IkReal(-0.000592743230384259))*(x12)))+(((IkReal(0.00191014540358414))*(x13))));
00286 eerot[6]=((((x9)*(((x58)+(x51)))))+(((x55)*(x8))));
00287 eerot[7]=((((x55)*(x9)))+(((x8)*(((((IkReal(-1.00000000000000))*(x17)*(x54)))+(((IkReal(-1.00000000000000))*(x23)*(x46))))))));
00288 eerot[8]=((((x10)*(x46)))+(((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x47)))+(x42))))));
00289 IkReal x66=((x1)*(x3));
00290 eetrans[2]=((IkReal(0.191500000000000))+(((x0)*(x19)))+(((IkReal(-0.00200000000000000))*(x28)))+(((x24)*(x66)))+(((IkReal(0.400000000000000))*(x0)))+(((x11)*(((((x21)*(x28)))+(((IkReal(-1.00000000000000))*(x22)*(x47)))))))+(((x7)*(((((IkReal(-1.00000000000000))*(x25)*(x66)))+(((IkReal(0.000500000000000000))*(x30)))))))+(((x10)*(((((IkReal(0.0570000000000000))*(x3)*(x32)))+(((IkReal(0.0570000000000000))*(x34)))))))+(((IkReal(-1.00000000000000))*(x20)*(x28))));
00291 }
00292 
00293 IKFAST_API int GetNumFreeParameters() { return 1; }
00294 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {0}; return freeparams; }
00295 IKFAST_API int GetNumJoints() { return 7; }
00296 
00297 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00298 
00299 IKFAST_API int GetIkType() { return 0x67000001; }
00300 
00301 class IKSolver {
00302 public:
00303 IkReal j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j0,cj0,sj0,htj0,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;
00304 unsigned char _ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij0[2], _nj0;
00305 
00306 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00307 j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1;  _ij0[0] = -1; _ij0[1] = -1; _nj0 = 0; 
00308 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00309     solutions.Clear();
00310 j0=pfree[0]; cj0=cos(pfree[0]); sj0=sin(pfree[0]);
00311 r00 = eerot[0*3+0];
00312 r01 = eerot[0*3+1];
00313 r02 = eerot[0*3+2];
00314 r10 = eerot[1*3+0];
00315 r11 = eerot[1*3+1];
00316 r12 = eerot[1*3+2];
00317 r20 = eerot[2*3+0];
00318 r21 = eerot[2*3+1];
00319 r22 = eerot[2*3+2];
00320 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00321 
00322 IkReal x67=IKsin(j0);
00323 IkReal x68=IKcos(j0);
00324 new_r00=((((IkReal(0.296371615192129))*(r00)*(x67)))+(((IkReal(-0.955072701792072))*(r00)*(x68)))+(((IkReal(-0.296371615192129))*(r10)*(x68)))+(((IkReal(-0.955072701792072))*(r10)*(x67))));
00325 IkReal x69=IKsin(j0);
00326 IkReal x70=IKcos(j0);
00327 new_r01=((((IkReal(0.296371615192129))*(r01)*(x69)))+(((IkReal(-0.955072701792072))*(r01)*(x70)))+(((IkReal(-0.955072701792072))*(r11)*(x69)))+(((IkReal(-0.296371615192129))*(r11)*(x70))));
00328 IkReal x71=IKsin(j0);
00329 IkReal x72=IKcos(j0);
00330 new_r02=((((IkReal(-0.955072701792072))*(r12)*(x71)))+(((IkReal(0.296371615192129))*(r02)*(x71)))+(((IkReal(-0.955072701792072))*(r02)*(x72)))+(((IkReal(-0.296371615192129))*(r12)*(x72))));
00331 IkReal x73=IKsin(j0);
00332 IkReal x74=IKcos(j0);
00333 new_px=((((IkReal(-0.0168931820659514))*(r02)*(x73)))+(((IkReal(0.0544391440021481))*(r02)*(x74)))+(((IkReal(0.296371615192129))*(px)*(x73)))+(((IkReal(0.0544391440021481))*(r12)*(x73)))+(((IkReal(0.0168931820659514))*(r12)*(x74)))+(((IkReal(-0.955072701792072))*(py)*(x73)))+(((IkReal(-0.955072701792072))*(px)*(x74)))+(((IkReal(-0.296371615192129))*(py)*(x74))));
00334 new_r10=r20;
00335 new_r11=r21;
00336 new_r12=r22;
00337 new_py=((IkReal(-0.191500000000000))+(pz)+(((IkReal(-0.0570000000000000))*(r22))));
00338 IkReal x75=IKcos(j0);
00339 IkReal x76=IKsin(j0);
00340 new_r20=((((IkReal(-0.955072701792072))*(r00)*(x76)))+(((IkReal(-0.296371615192129))*(r00)*(x75)))+(((IkReal(-0.296371615192129))*(r10)*(x76)))+(((IkReal(0.955072701792072))*(r10)*(x75))));
00341 IkReal x77=IKcos(j0);
00342 IkReal x78=IKsin(j0);
00343 new_r21=((((IkReal(-0.955072701792072))*(r01)*(x78)))+(((IkReal(0.955072701792072))*(r11)*(x77)))+(((IkReal(-0.296371615192129))*(r01)*(x77)))+(((IkReal(-0.296371615192129))*(r11)*(x78))));
00344 IkReal x79=IKsin(j0);
00345 IkReal x80=IKcos(j0);
00346 new_r22=((((IkReal(0.955072701792072))*(r12)*(x80)))+(((IkReal(-0.296371615192129))*(r02)*(x80)))+(((IkReal(-0.955072701792072))*(r02)*(x79)))+(((IkReal(-0.296371615192129))*(r12)*(x79))));
00347 IkReal x81=IKcos(j0);
00348 IkReal x82=IKsin(j0);
00349 new_pz=((IkReal(-0.00200000000000000))+(((IkReal(-0.0544391440021481))*(r12)*(x81)))+(((IkReal(0.0544391440021481))*(r02)*(x82)))+(((IkReal(0.955072701792072))*(py)*(x81)))+(((IkReal(-0.296371615192129))*(px)*(x81)))+(((IkReal(-0.955072701792072))*(px)*(x82)))+(((IkReal(0.0168931820659514))*(r02)*(x81)))+(((IkReal(0.0168931820659514))*(r12)*(x82)))+(((IkReal(-0.296371615192129))*(py)*(x82))));
00350 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;
00351 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00352 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00353 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00354 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00355 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00356 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00357 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00358 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00359 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00360 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00361 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00362 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00363 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00364 IkReal op[72], zeror[48];
00365 int numroots;
00366 IkReal x83=((IkReal(0.00100000000000000))*(npy));
00367 IkReal x84=((IkReal(0.780000000000000))*(npz));
00368 IkReal x85=((IkReal(0.00100000000000000))*(rxp0_0));
00369 IkReal x86=((IkReal(0.780000000000000))*(px));
00370 IkReal x87=((pp)*(r02));
00371 IkReal x88=((IkReal(0.00789625000000000))*(r02));
00372 IkReal x89=((IkReal(0.000400000000000000))*(r20));
00373 IkReal x90=((IkReal(0.000390000000000000))*(r01));
00374 IkReal x91=((IkReal(0.800000000000000))*(rxp2_2));
00375 IkReal x92=((IkReal(0.00100000000000000))*(rxp0_1));
00376 IkReal x93=((IkReal(0.780000000000000))*(py));
00377 IkReal x94=((IkReal(0.00789625000000000))*(r12));
00378 IkReal x95=((pp)*(r12));
00379 IkReal x96=((IkReal(0.000390000000000000))*(r11));
00380 IkReal x97=((IkReal(0.000780000000000000))*(r00));
00381 IkReal x98=((IkReal(0.000800000000000000))*(r21));
00382 IkReal x99=((IkReal(0.00200000000000000))*(rxp1_0));
00383 IkReal x100=((IkReal(3.12000000000000))*(npy));
00384 IkReal x101=((IkReal(1.56000000000000))*(npx));
00385 IkReal x102=((IkReal(1.60000000000000))*(rxp0_2));
00386 IkReal x103=((IkReal(0.00200000000000000))*(rxp2_0));
00387 IkReal x104=((IkReal(0.000800000000000000))*(r22));
00388 IkReal x105=((IkReal(0.0157925000000000))*(r00));
00389 IkReal x106=((IkReal(0.00200000000000000))*(rxp2_1));
00390 IkReal x107=((IkReal(0.0157925000000000))*(r10));
00391 IkReal x108=((IkReal(0.0315850000000000))*(r01));
00392 IkReal x109=((IkReal(3.20000000000000))*(rxp1_2));
00393 IkReal x110=((IkReal(0.000780000000000000))*(r10));
00394 IkReal x111=((IkReal(0.00200000000000000))*(rxp1_1));
00395 IkReal x112=((IkReal(-0.00790375000000000))+(pp));
00396 IkReal x113=((IkReal(8.00000000000000))*(npy));
00397 IkReal x114=((IkReal(2.00000000000000))*(pp));
00398 IkReal x115=((IkReal(4.00000000000000))*(npx));
00399 IkReal x116=((IkReal(4.00000000000000))*(pp));
00400 IkReal x117=((IkReal(2.00000000000000))*(npz));
00401 IkReal x118=((IkReal(-0.00200000000000000))*(npx));
00402 IkReal x119=((px)*(x117));
00403 IkReal x120=((py)*(x117));
00404 IkReal x121=((((IkReal(-1.00000000000000))*(x110)))+(x111));
00405 IkReal x122=((IkReal(1.00000000000000))*(x87));
00406 IkReal x123=((IkReal(-1.56000000000000))*(npx));
00407 IkReal x124=((px)*(x115));
00408 IkReal x125=((r00)*(x114));
00409 IkReal x126=((py)*(x115));
00410 IkReal x127=((r10)*(x114));
00411 IkReal x128=((r01)*(x116));
00412 IkReal x129=((px)*(x113));
00413 IkReal x130=((IkReal(1.00000000000000))*(x95));
00414 IkReal x131=((((IkReal(-1.00000000000000))*(x111)))+(((IkReal(-1.00000000000000))*(x110))));
00415 IkReal x132=((x99)+(x97));
00416 IkReal x133=((x89)+(x91));
00417 IkReal x134=((x92)+(x96));
00418 IkReal x135=((x89)+(x85));
00419 IkReal x136=((x83)+(x84));
00420 IkReal x137=((x86)+(x91));
00421 IkReal x138=((x102)+(x103));
00422 IkReal x139=((x88)+(x87));
00423 IkReal x140=((x112)+(((IkReal(-1.00000000000000))*(x136))));
00424 IkReal x141=((x112)+(x83)+(((IkReal(-1.00000000000000))*(x84))));
00425 IkReal x142=((x112)+(x84)+(((IkReal(-1.00000000000000))*(x83))));
00426 IkReal x143=((x119)+(x90));
00427 IkReal x144=((x122)+(x88));
00428 IkReal x145=((x124)+(x104));
00429 IkReal x146=((x125)+(x105));
00430 IkReal x147=((x127)+(x107));
00431 IkReal x148=((x128)+(x108));
00432 IkReal x149=((x130)+(x94));
00433 IkReal x150=((x120)+(x93));
00434 IkReal x151=((x136)+(x112));
00435 IkReal x152=((x93)+(x95)+(x94));
00436 IkReal x153=((((IkReal(-1.00000000000000))*(r11)*(x116)))+(((py)*(x113)))+(((IkReal(-0.0315850000000000))*(r11))));
00437 IkReal x154=((x119)+(x86)+(x85));
00438 IkReal x155=((((IkReal(-1.00000000000000))*(x126)))+(x147)+(x106));
00439 IkReal x156=((((IkReal(-1.00000000000000))*(x147)))+(x126)+(x106));
00440 IkReal x157=((((IkReal(-1.00000000000000))*(x120)))+(x152)+(((IkReal(-1.00000000000000))*(x134))));
00441 IkReal x158=((((IkReal(-1.00000000000000))*(x120)))+(x152)+(x134));
00442 IkReal x159=((((IkReal(-1.00000000000000))*(x149)))+(x150)+(x92)+(((IkReal(-1.00000000000000))*(x96))));
00443 IkReal x160=((((IkReal(-1.00000000000000))*(x149)))+(x150)+(x96)+(((IkReal(-1.00000000000000))*(x92))));
00444 op[0]=x140;
00445 op[1]=IkReal(0);
00446 op[2]=x118;
00447 op[3]=IkReal(0);
00448 op[4]=x141;
00449 op[5]=IkReal(0);
00450 op[6]=IkReal(0);
00451 op[7]=x140;
00452 op[8]=IkReal(0);
00453 op[9]=x118;
00454 op[10]=IkReal(0);
00455 op[11]=x141;
00456 op[12]=((((IkReal(-1.00000000000000))*(x143)))+(x139)+(((IkReal(-1.00000000000000))*(x133)))+(x86)+(((IkReal(-1.00000000000000))*(x85))));
00457 op[13]=x157;
00458 op[14]=((x99)+(x98)+(((IkReal(-1.00000000000000))*(x97))));
00459 op[15]=x121;
00460 op[16]=((((IkReal(-1.00000000000000))*(x119)))+(x135)+(x139)+(x86)+(x90)+(((IkReal(-1.00000000000000))*(x91))));
00461 op[17]=x158;
00462 op[18]=x157;
00463 op[19]=((((IkReal(-1.00000000000000))*(x144)))+(x143)+(((IkReal(-1.00000000000000))*(x133)))+(x85)+(((IkReal(-1.00000000000000))*(x86))));
00464 op[20]=x121;
00465 op[21]=((x98)+(x97)+(((IkReal(-1.00000000000000))*(x99))));
00466 op[22]=x158;
00467 op[23]=((((IkReal(-1.00000000000000))*(x144)))+(x119)+(((IkReal(-1.00000000000000))*(x137)))+(x89)+(((IkReal(-1.00000000000000))*(x90)))+(((IkReal(-1.00000000000000))*(x85))));
00468 op[24]=x123;
00469 op[25]=IkReal(0);
00470 op[26]=x100;
00471 op[27]=IkReal(0);
00472 op[28]=x101;
00473 op[29]=IkReal(0);
00474 op[30]=IkReal(0);
00475 op[31]=x123;
00476 op[32]=IkReal(0);
00477 op[33]=x100;
00478 op[34]=IkReal(0);
00479 op[35]=x101;
00480 op[36]=((((IkReal(-1.00000000000000))*(x102)))+(((IkReal(-1.00000000000000))*(x124)))+(x146)+(x104)+(x103));
00481 op[37]=x155;
00482 op[38]=((((IkReal(-1.00000000000000))*(x148)))+(x129)+(x109));
00483 op[39]=x153;
00484 op[40]=((((IkReal(-1.00000000000000))*(x146)))+(x145)+(x138));
00485 op[41]=x156;
00486 op[42]=x155;
00487 op[43]=((((IkReal(-1.00000000000000))*(x146)))+(x145)+(((IkReal(-1.00000000000000))*(x138))));
00488 op[44]=x153;
00489 op[45]=((((IkReal(-1.00000000000000))*(x129)))+(x148)+(x109));
00490 op[46]=x156;
00491 op[47]=((((IkReal(-1.00000000000000))*(x103)))+(((IkReal(-1.00000000000000))*(x124)))+(x146)+(x104)+(x102));
00492 op[48]=x142;
00493 op[49]=IkReal(0);
00494 op[50]=x118;
00495 op[51]=IkReal(0);
00496 op[52]=x151;
00497 op[53]=IkReal(0);
00498 op[54]=IkReal(0);
00499 op[55]=x142;
00500 op[56]=IkReal(0);
00501 op[57]=x118;
00502 op[58]=IkReal(0);
00503 op[59]=x151;
00504 op[60]=((((IkReal(-1.00000000000000))*(x144)))+(x154)+(x133)+(((IkReal(-1.00000000000000))*(x90))));
00505 op[61]=x159;
00506 op[62]=((((IkReal(-1.00000000000000))*(x132)))+(((IkReal(-1.00000000000000))*(x98))));
00507 op[63]=x131;
00508 op[64]=((((IkReal(-1.00000000000000))*(x144)))+(x143)+(x137)+(((IkReal(-1.00000000000000))*(x135))));
00509 op[65]=x160;
00510 op[66]=x159;
00511 op[67]=((((IkReal(-1.00000000000000))*(x154)))+(x133)+(x139)+(x90));
00512 op[68]=x131;
00513 op[69]=((x132)+(((IkReal(-1.00000000000000))*(x98))));
00514 op[70]=x160;
00515 op[71]=((((IkReal(-1.00000000000000))*(x143)))+(x139)+(x85)+(x91)+(((IkReal(-1.00000000000000))*(x89)))+(((IkReal(-1.00000000000000))*(x86))));
00516 solvedialyticpoly8qep(op,zeror,numroots);
00517 IkReal j5array[16], cj5array[16], sj5array[16], j6array[16], cj6array[16], sj6array[16], j1array[16], cj1array[16], sj1array[16];
00518 int numsolutions = 0;
00519 for(int ij5 = 0; ij5 < numroots; ij5 += 3)
00520 {
00521 IkReal htj5 = zeror[ij5+0], htj6 = zeror[ij5+1], htj1 = zeror[ij5+2];
00522 j5array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj5)));
00523 j6array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj6)));
00524 j1array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj1)));
00525 IkReal x161=(htj5)*(htj5);
00526 IkReal x162=(htj6)*(htj6);
00527 IkReal x163=(htj1)*(htj1);
00528 cj5array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x161))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x161)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x161))))));
00529 cj6array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x162))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x162)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x162))))));
00530 cj1array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x163))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x163)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x163))))));
00531 sj5array[numsolutions]=((IkReal(2.00000000000000))*(htj5)*(((IKabs(((IkReal(1.00000000000000))+((htj5)*(htj5)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj5)*(htj5))))):(IkReal)1.0e30)));
00532 sj6array[numsolutions]=((IkReal(2.00000000000000))*(htj6)*(((IKabs(((IkReal(1.00000000000000))+((htj6)*(htj6)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj6)*(htj6))))):(IkReal)1.0e30)));
00533 sj1array[numsolutions]=((IkReal(2.00000000000000))*(htj1)*(((IKabs(((IkReal(1.00000000000000))+((htj1)*(htj1)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj1)*(htj1))))):(IkReal)1.0e30)));
00534 if( j5array[numsolutions] > IKPI )
00535 {
00536     j5array[numsolutions]-=IK2PI;
00537 }
00538 else if( j5array[numsolutions] < -IKPI )
00539 {
00540     j5array[numsolutions]+=IK2PI;
00541 }
00542 if( j6array[numsolutions] > IKPI )
00543 {
00544     j6array[numsolutions]-=IK2PI;
00545 }
00546 else if( j6array[numsolutions] < -IKPI )
00547 {
00548     j6array[numsolutions]+=IK2PI;
00549 }
00550 if( j1array[numsolutions] > IKPI )
00551 {
00552     j1array[numsolutions]-=IK2PI;
00553 }
00554 else if( j1array[numsolutions] < -IKPI )
00555 {
00556     j1array[numsolutions]+=IK2PI;
00557 }
00558 numsolutions++;
00559 }
00560 bool j5valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
00561 _nj5 = 16;
00562 _nj6 = 1;
00563 _nj1 = 1;
00564 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
00565     {
00566 if( !j5valid[ij5] )
00567 {
00568     continue;
00569 }
00570 _ij5[0] = ij5; _ij5[1] = -1;
00571 _ij6[0] = 0; _ij6[1] = -1;
00572 _ij1[0] = 0; _ij1[1] = -1;
00573 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
00574 {
00575 if( !j5valid[iij5] ) { continue; }
00576 if( IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(cj6array[ij5]-cj6array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij5]-sj6array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(cj1array[ij5]-cj1array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij5]-sj1array[iij5]) < IKFAST_SOLUTION_THRESH &&  1 )
00577 {
00578     j5valid[iij5]=false; _ij5[1] = iij5; _ij6[1] = 0; _ij1[1] = 0;  break; 
00579 }
00580 }
00581     j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00582 
00583     j6 = j6array[ij5]; cj6 = cj6array[ij5]; sj6 = sj6array[ij5];
00584 
00585     j1 = j1array[ij5]; cj1 = cj1array[ij5]; sj1 = sj1array[ij5];
00586 
00587 {
00588 IkReal dummyeval[1];
00589 dummyeval[0]=cj1;
00590 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00591 {
00592 {
00593 IkReal dummyeval[1];
00594 dummyeval[0]=sj1;
00595 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00596 {
00597 {
00598 IkReal evalcond[2];
00599 IkReal x164=((IkReal(0.390000000000000))*(sj5));
00600 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959))));
00601 evalcond[1]=((IkReal(0.400000000000000))+(((IkReal(0.000500000000000000))*(cj6)*(r11)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.000500000000000000))*(r10)*(sj6)))+(((cj6)*(r10)*(x164)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x164)))+(((IkReal(0.390000000000000))*(cj5)*(r12))));
00602 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  )
00603 {
00604 {
00605 IkReal j2array[1], cj2array[1], sj2array[1];
00606 bool j2valid[1]={false};
00607 _nj2 = 1;
00608 IkReal x165=((IkReal(0.250000000000000))*(cj6));
00609 IkReal x166=((IkReal(195.000000000000))*(cj5));
00610 IkReal x167=((IkReal(0.250000000000000))*(sj6));
00611 IkReal x168=((IkReal(195.000000000000))*(sj5));
00612 if( IKabs(((((r01)*(x165)))+(((r00)*(x167)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x168)))+(((IkReal(-500.000000000000))*(px)))+(((r02)*(x166)))+(((cj6)*(r00)*(x168))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r22)*(x166)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x168)))+(((r20)*(x167)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x165)))+(((cj6)*(r20)*(x168))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r01)*(x165)))+(((r00)*(x167)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x168)))+(((IkReal(-500.000000000000))*(px)))+(((r02)*(x166)))+(((cj6)*(r00)*(x168)))))+IKsqr(((((r22)*(x166)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x168)))+(((r20)*(x167)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x165)))+(((cj6)*(r20)*(x168)))))-1) <= IKFAST_SINCOS_THRESH )
00613     continue;
00614 j2array[0]=IKatan2(((((r01)*(x165)))+(((r00)*(x167)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x168)))+(((IkReal(-500.000000000000))*(px)))+(((r02)*(x166)))+(((cj6)*(r00)*(x168)))), ((((r22)*(x166)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x168)))+(((r20)*(x167)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x165)))+(((cj6)*(r20)*(x168)))));
00615 sj2array[0]=IKsin(j2array[0]);
00616 cj2array[0]=IKcos(j2array[0]);
00617 if( j2array[0] > IKPI )
00618 {
00619     j2array[0]-=IK2PI;
00620 }
00621 else if( j2array[0] < -IKPI )
00622 {    j2array[0]+=IK2PI;
00623 }
00624 j2valid[0] = true;
00625 for(int ij2 = 0; ij2 < 1; ++ij2)
00626 {
00627 if( !j2valid[ij2] )
00628 {
00629     continue;
00630 }
00631 _ij2[0] = ij2; _ij2[1] = -1;
00632 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
00633 {
00634 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00635 {
00636     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00637 }
00638 }
00639 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00640 {
00641 IkReal evalcond[2];
00642 IkReal x169=((IkReal(0.000500000000000000))*(cj6));
00643 IkReal x170=((IkReal(0.390000000000000))*(sj5));
00644 IkReal x171=((IkReal(0.390000000000000))*(cj5));
00645 IkReal x172=((IkReal(0.000500000000000000))*(sj6));
00646 evalcond[0]=((((r22)*(x171)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x170)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((r20)*(x172)))+(((r21)*(x169)))+(((IkReal(-1.00000000000000))*(pz)))+(((cj6)*(r20)*(x170))));
00647 evalcond[1]=((((r01)*(x169)))+(((r00)*(x172)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x170)))+(((IkReal(-0.00200000000000000))*(IKsin(j2))))+(((IkReal(-1.00000000000000))*(px)))+(((cj6)*(r00)*(x170)))+(((r02)*(x171))));
00648 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00649 {
00650 continue;
00651 }
00652 }
00653 
00654 {
00655 IkReal dummyeval[1];
00656 IkReal gconst33;
00657 IkReal x173=(cj6)*(cj6);
00658 IkReal x174=(sj6)*(sj6);
00659 IkReal x175=((r11)*(r20));
00660 IkReal x176=((r12)*(sj5));
00661 IkReal x177=((IkReal(1.00000000000000))*(r10));
00662 IkReal x178=((r22)*(sj5));
00663 IkReal x179=((cj5)*(x174));
00664 IkReal x180=((cj5)*(x173));
00665 gconst33=IKsign(((((r20)*(sj6)*(x176)))+(((IkReal(-1.00000000000000))*(r21)*(x177)*(x179)))+(((x175)*(x179)))+(((x175)*(x180)))+(((cj6)*(r21)*(x176)))+(((IkReal(-1.00000000000000))*(sj6)*(x177)*(x178)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x178)))+(((IkReal(-1.00000000000000))*(r21)*(x177)*(x180)))));
00666 IkReal x181=(cj6)*(cj6);
00667 IkReal x182=(sj6)*(sj6);
00668 IkReal x183=((r11)*(r20));
00669 IkReal x184=((r12)*(sj5));
00670 IkReal x185=((IkReal(1.00000000000000))*(r10));
00671 IkReal x186=((r22)*(sj5));
00672 IkReal x187=((cj5)*(x182));
00673 IkReal x188=((cj5)*(x181));
00674 dummyeval[0]=((((IkReal(-1.00000000000000))*(sj6)*(x185)*(x186)))+(((r20)*(sj6)*(x184)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x186)))+(((cj6)*(r21)*(x184)))+(((IkReal(-1.00000000000000))*(r21)*(x185)*(x188)))+(((IkReal(-1.00000000000000))*(r21)*(x185)*(x187)))+(((x183)*(x187)))+(((x183)*(x188))));
00675 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00676 {
00677 {
00678 IkReal dummyeval[1];
00679 IkReal gconst34;
00680 IkReal x189=(sj6)*(sj6);
00681 IkReal x190=(cj6)*(cj6);
00682 IkReal x191=((cj6)*(sj5));
00683 IkReal x192=((r00)*(r11));
00684 IkReal x193=((IkReal(1.00000000000000))*(r02));
00685 IkReal x194=((sj5)*(sj6));
00686 IkReal x195=((cj5)*(x189));
00687 IkReal x196=((IkReal(1.00000000000000))*(r01)*(r10));
00688 IkReal x197=((cj5)*(x190));
00689 gconst34=IKsign(((((IkReal(-1.00000000000000))*(x196)*(x197)))+(((IkReal(-1.00000000000000))*(x195)*(x196)))+(((r00)*(r12)*(x194)))+(((r01)*(r12)*(x191)))+(((x192)*(x197)))+(((x192)*(x195)))+(((IkReal(-1.00000000000000))*(r10)*(x193)*(x194)))+(((IkReal(-1.00000000000000))*(r11)*(x191)*(x193)))));
00690 IkReal x198=(sj6)*(sj6);
00691 IkReal x199=(cj6)*(cj6);
00692 IkReal x200=((cj6)*(sj5));
00693 IkReal x201=((r00)*(r11));
00694 IkReal x202=((IkReal(1.00000000000000))*(r02));
00695 IkReal x203=((sj5)*(sj6));
00696 IkReal x204=((cj5)*(x198));
00697 IkReal x205=((IkReal(1.00000000000000))*(r01)*(r10));
00698 IkReal x206=((cj5)*(x199));
00699 dummyeval[0]=((((IkReal(-1.00000000000000))*(x204)*(x205)))+(((IkReal(-1.00000000000000))*(r10)*(x202)*(x203)))+(((x201)*(x204)))+(((x201)*(x206)))+(((r01)*(r12)*(x200)))+(((IkReal(-1.00000000000000))*(r11)*(x200)*(x202)))+(((r00)*(r12)*(x203)))+(((IkReal(-1.00000000000000))*(x205)*(x206))));
00700 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00701 {
00702 {
00703 IkReal dummyeval[1];
00704 dummyeval[0]=cj2;
00705 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00706 {
00707 {
00708 IkReal evalcond[5];
00709 IkReal x207=((IkReal(0.000500000000000000))*(cj6));
00710 IkReal x208=((IkReal(0.000500000000000000))*(sj6));
00711 IkReal x209=((sj5)*(sj6));
00712 IkReal x210=((cj5)*(r02));
00713 IkReal x211=((IkReal(0.390000000000000))*(cj5));
00714 IkReal x212=((IkReal(0.390000000000000))*(cj6)*(sj5));
00715 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
00716 evalcond[1]=((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(x209)))+(x210));
00717 evalcond[2]=((((IkReal(-0.390000000000000))*(r21)*(x209)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x208)))+(((r22)*(x211)))+(((r21)*(x207)))+(((r20)*(x212))));
00718 evalcond[3]=((IkReal(-0.00200000000000000))+(((IkReal(-0.390000000000000))*(r01)*(x209)))+(((IkReal(0.390000000000000))*(x210)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x207)))+(((r00)*(x208)))+(((r00)*(x212))));
00719 evalcond[4]=((IkReal(0.400000000000000))+(((r11)*(x207)))+(((r12)*(x211)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x208)))+(((r10)*(x212)))+(((IkReal(-0.390000000000000))*(r11)*(x209))));
00720 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
00721 {
00722 {
00723 IkReal j3array[1], cj3array[1], sj3array[1];
00724 bool j3valid[1]={false};
00725 _nj3 = 1;
00726 IkReal x213=((IkReal(1.00000000000000))*(sj5));
00727 if( IKabs(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x213)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj5)*(r12)))+(((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x213))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x213)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((cj5)*(r12)))+(((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x213)))))-1) <= IKFAST_SINCOS_THRESH )
00728     continue;
00729 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x213)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((cj5)*(r12)))+(((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x213)))));
00730 sj3array[0]=IKsin(j3array[0]);
00731 cj3array[0]=IKcos(j3array[0]);
00732 if( j3array[0] > IKPI )
00733 {
00734     j3array[0]-=IK2PI;
00735 }
00736 else if( j3array[0] < -IKPI )
00737 {    j3array[0]+=IK2PI;
00738 }
00739 j3valid[0] = true;
00740 for(int ij3 = 0; ij3 < 1; ++ij3)
00741 {
00742 if( !j3valid[ij3] )
00743 {
00744     continue;
00745 }
00746 _ij3[0] = ij3; _ij3[1] = -1;
00747 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
00748 {
00749 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00750 {
00751     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00752 }
00753 }
00754 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00755 {
00756 IkReal evalcond[2];
00757 IkReal x214=((cj6)*(sj5));
00758 IkReal x215=((IkReal(1.00000000000000))*(sj5)*(sj6));
00759 evalcond[0]=((((cj5)*(r22)))+(IKsin(j3))+(((r20)*(x214)))+(((IkReal(-1.00000000000000))*(r21)*(x215))));
00760 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x215)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((r10)*(x214))));
00761 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00762 {
00763 continue;
00764 }
00765 }
00766 
00767 {
00768 IkReal dummyeval[1];
00769 IkReal gconst39;
00770 IkReal x216=(cj6)*(cj6);
00771 IkReal x217=(sj6)*(sj6);
00772 IkReal x218=((r01)*(r20));
00773 IkReal x219=((r02)*(sj5));
00774 IkReal x220=((cj5)*(x217));
00775 IkReal x221=((IkReal(1.00000000000000))*(r22)*(sj5));
00776 IkReal x222=((IkReal(1.00000000000000))*(r00)*(r21));
00777 IkReal x223=((cj5)*(x216));
00778 gconst39=IKsign(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x221)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x221)))+(((IkReal(-1.00000000000000))*(x222)*(x223)))+(((IkReal(-1.00000000000000))*(x220)*(x222)))+(((x218)*(x220)))+(((x218)*(x223)))+(((r20)*(sj6)*(x219)))+(((cj6)*(r21)*(x219)))));
00779 IkReal x224=(cj6)*(cj6);
00780 IkReal x225=(sj6)*(sj6);
00781 IkReal x226=((r01)*(r20));
00782 IkReal x227=((r02)*(sj5));
00783 IkReal x228=((cj5)*(x225));
00784 IkReal x229=((IkReal(1.00000000000000))*(r22)*(sj5));
00785 IkReal x230=((IkReal(1.00000000000000))*(r00)*(r21));
00786 IkReal x231=((cj5)*(x224));
00787 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x229)))+(((r20)*(sj6)*(x227)))+(((IkReal(-1.00000000000000))*(x230)*(x231)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x229)))+(((cj6)*(r21)*(x227)))+(((x226)*(x231)))+(((x226)*(x228)))+(((IkReal(-1.00000000000000))*(x228)*(x230))));
00788 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00789 {
00790 {
00791 IkReal dummyeval[1];
00792 IkReal gconst40;
00793 IkReal x232=(cj6)*(cj6);
00794 IkReal x233=(cj5)*(cj5);
00795 IkReal x234=(r01)*(r01);
00796 IkReal x235=(r00)*(r00);
00797 IkReal x236=(sj6)*(sj6);
00798 IkReal x237=((cj6)*(r00));
00799 IkReal x238=((IkReal(2.00000000000000))*(r01)*(sj6));
00800 IkReal x239=((cj5)*(r02)*(sj5));
00801 gconst40=IKsign(((((IkReal(-2.00000000000000))*(x237)*(x239)))+(((x237)*(x238)))+(((x232)*(x234)))+((((r02)*(r02))*((sj5)*(sj5))))+(((x233)*(x234)*(x236)))+(((x235)*(x236)))+(((x238)*(x239)))+(((IkReal(-1.00000000000000))*(x233)*(x237)*(x238)))+(((x232)*(x233)*(x235)))));
00802 IkReal x240=(cj6)*(cj6);
00803 IkReal x241=(cj5)*(cj5);
00804 IkReal x242=(r01)*(r01);
00805 IkReal x243=(r00)*(r00);
00806 IkReal x244=(sj6)*(sj6);
00807 IkReal x245=((cj6)*(r00));
00808 IkReal x246=((IkReal(2.00000000000000))*(r01)*(sj6));
00809 IkReal x247=((cj5)*(r02)*(sj5));
00810 dummyeval[0]=((((x240)*(x241)*(x243)))+(((x240)*(x242)))+((((r02)*(r02))*((sj5)*(sj5))))+(((x245)*(x246)))+(((x241)*(x242)*(x244)))+(((x246)*(x247)))+(((x243)*(x244)))+(((IkReal(-1.00000000000000))*(x241)*(x245)*(x246)))+(((IkReal(-2.00000000000000))*(x245)*(x247))));
00811 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00812 {
00813 continue;
00814 
00815 } else
00816 {
00817 {
00818 IkReal j4array[1], cj4array[1], sj4array[1];
00819 bool j4valid[1]={false};
00820 _nj4 = 1;
00821 IkReal x248=((IkReal(1.00000000000000))*(r00));
00822 if( IKabs(((gconst40)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x248)))+(((cj5)*(r01)*(sj6)))+(((r02)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst40)*(((((IkReal(-1.00000000000000))*(sj6)*(x248)))+(((IkReal(-1.00000000000000))*(cj6)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH )
00823     continue;
00824 j4array[0]=IKatan2(((gconst40)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x248)))+(((cj5)*(r01)*(sj6)))+(((r02)*(sj5)))))), ((gconst40)*(((((IkReal(-1.00000000000000))*(sj6)*(x248)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)))))));
00825 sj4array[0]=IKsin(j4array[0]);
00826 cj4array[0]=IKcos(j4array[0]);
00827 if( j4array[0] > IKPI )
00828 {
00829     j4array[0]-=IK2PI;
00830 }
00831 else if( j4array[0] < -IKPI )
00832 {    j4array[0]+=IK2PI;
00833 }
00834 j4valid[0] = true;
00835 for(int ij4 = 0; ij4 < 1; ++ij4)
00836 {
00837 if( !j4valid[ij4] )
00838 {
00839     continue;
00840 }
00841 _ij4[0] = ij4; _ij4[1] = -1;
00842 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
00843 {
00844 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00845 {
00846     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00847 }
00848 }
00849 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00850 {
00851 IkReal evalcond[6];
00852 IkReal x249=IKsin(j4);
00853 IkReal x250=IKcos(j4);
00854 IkReal x251=((IkReal(1.00000000000000))*(r00));
00855 IkReal x252=((cj5)*(r01));
00856 IkReal x253=((cj5)*(r11));
00857 IkReal x254=((IkReal(1.00000000000000))*(cj5));
00858 IkReal x255=((cj5)*(r21));
00859 IkReal x256=((sj5)*(x249));
00860 IkReal x257=((cj6)*(x249));
00861 IkReal x258=((sj6)*(x250));
00862 IkReal x259=((sj6)*(x249));
00863 IkReal x260=((sj5)*(x250));
00864 IkReal x261=((cj6)*(x250));
00865 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x261)))+(((IkReal(-1.00000000000000))*(r20)*(x254)*(x257)))+(((IkReal(-1.00000000000000))*(r20)*(x258)))+(((r22)*(x256)))+(((x255)*(x259))));
00866 evalcond[1]=((((r21)*(x257)))+(((r20)*(x259)))+(((r22)*(x260)))+(((x255)*(x258)))+(((IkReal(-1.00000000000000))*(r20)*(x254)*(x261)))+(((IkReal(-1.00000000000000))*(cj3))));
00867 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x251)*(x258)))+(((IkReal(-1.00000000000000))*(cj5)*(x251)*(x257)))+(((IkReal(-1.00000000000000))*(r01)*(x261)))+(((x252)*(x259)))+(((r02)*(x256))));
00868 evalcond[3]=((((r12)*(x256)))+(((IkReal(-1.00000000000000))*(r10)*(x258)))+(((IkReal(-1.00000000000000))*(r11)*(x261)))+(((x253)*(x259)))+(((IkReal(-1.00000000000000))*(r10)*(x254)*(x257))));
00869 evalcond[4]=((((r00)*(x259)))+(((r02)*(x260)))+(((r01)*(x257)))+(((x252)*(x258)))+(((IkReal(-1.00000000000000))*(cj5)*(x251)*(x261))));
00870 evalcond[5]=((((IkReal(-1.00000000000000))*(sj3)))+(((r11)*(x257)))+(((r10)*(x259)))+(((IkReal(-1.00000000000000))*(r10)*(x254)*(x261)))+(((x253)*(x258)))+(((r12)*(x260))));
00871 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
00872 {
00873 continue;
00874 }
00875 }
00876 
00877 {
00878 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
00879 vinfos[0].jointtype = 1;
00880 vinfos[0].foffset = j0;
00881 vinfos[0].indices[0] = _ij0[0];
00882 vinfos[0].indices[1] = _ij0[1];
00883 vinfos[0].maxsolutions = _nj0;
00884 vinfos[1].jointtype = 1;
00885 vinfos[1].foffset = j1;
00886 vinfos[1].indices[0] = _ij1[0];
00887 vinfos[1].indices[1] = _ij1[1];
00888 vinfos[1].maxsolutions = _nj1;
00889 vinfos[2].jointtype = 1;
00890 vinfos[2].foffset = j2;
00891 vinfos[2].indices[0] = _ij2[0];
00892 vinfos[2].indices[1] = _ij2[1];
00893 vinfos[2].maxsolutions = _nj2;
00894 vinfos[3].jointtype = 1;
00895 vinfos[3].foffset = j3;
00896 vinfos[3].indices[0] = _ij3[0];
00897 vinfos[3].indices[1] = _ij3[1];
00898 vinfos[3].maxsolutions = _nj3;
00899 vinfos[4].jointtype = 1;
00900 vinfos[4].foffset = j4;
00901 vinfos[4].indices[0] = _ij4[0];
00902 vinfos[4].indices[1] = _ij4[1];
00903 vinfos[4].maxsolutions = _nj4;
00904 vinfos[5].jointtype = 1;
00905 vinfos[5].foffset = j5;
00906 vinfos[5].indices[0] = _ij5[0];
00907 vinfos[5].indices[1] = _ij5[1];
00908 vinfos[5].maxsolutions = _nj5;
00909 vinfos[6].jointtype = 1;
00910 vinfos[6].foffset = j6;
00911 vinfos[6].indices[0] = _ij6[0];
00912 vinfos[6].indices[1] = _ij6[1];
00913 vinfos[6].maxsolutions = _nj6;
00914 std::vector<int> vfree(0);
00915 solutions.AddSolution(vinfos,vfree);
00916 }
00917 }
00918 }
00919 
00920 }
00921 
00922 }
00923 
00924 } else
00925 {
00926 {
00927 IkReal j4array[1], cj4array[1], sj4array[1];
00928 bool j4valid[1]={false};
00929 _nj4 = 1;
00930 IkReal x262=((cj3)*(sj6));
00931 IkReal x263=((IkReal(1.00000000000000))*(r00));
00932 IkReal x264=((cj3)*(cj6));
00933 if( IKabs(((gconst39)*(((((IkReal(-1.00000000000000))*(cj5)*(x263)*(x264)))+(((cj5)*(r01)*(x262)))+(((cj3)*(r02)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst39)*(((((IkReal(-1.00000000000000))*(x262)*(x263)))+(((IkReal(-1.00000000000000))*(r01)*(x264))))))) < IKFAST_ATAN2_MAGTHRESH )
00934     continue;
00935 j4array[0]=IKatan2(((gconst39)*(((((IkReal(-1.00000000000000))*(cj5)*(x263)*(x264)))+(((cj5)*(r01)*(x262)))+(((cj3)*(r02)*(sj5)))))), ((gconst39)*(((((IkReal(-1.00000000000000))*(x262)*(x263)))+(((IkReal(-1.00000000000000))*(r01)*(x264)))))));
00936 sj4array[0]=IKsin(j4array[0]);
00937 cj4array[0]=IKcos(j4array[0]);
00938 if( j4array[0] > IKPI )
00939 {
00940     j4array[0]-=IK2PI;
00941 }
00942 else if( j4array[0] < -IKPI )
00943 {    j4array[0]+=IK2PI;
00944 }
00945 j4valid[0] = true;
00946 for(int ij4 = 0; ij4 < 1; ++ij4)
00947 {
00948 if( !j4valid[ij4] )
00949 {
00950     continue;
00951 }
00952 _ij4[0] = ij4; _ij4[1] = -1;
00953 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
00954 {
00955 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00956 {
00957     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00958 }
00959 }
00960 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00961 {
00962 IkReal evalcond[6];
00963 IkReal x265=IKsin(j4);
00964 IkReal x266=IKcos(j4);
00965 IkReal x267=((IkReal(1.00000000000000))*(r00));
00966 IkReal x268=((cj5)*(r01));
00967 IkReal x269=((cj5)*(r11));
00968 IkReal x270=((IkReal(1.00000000000000))*(cj5));
00969 IkReal x271=((cj5)*(r21));
00970 IkReal x272=((sj5)*(x265));
00971 IkReal x273=((cj6)*(x265));
00972 IkReal x274=((sj6)*(x266));
00973 IkReal x275=((sj6)*(x265));
00974 IkReal x276=((sj5)*(x266));
00975 IkReal x277=((cj6)*(x266));
00976 evalcond[0]=((((x271)*(x275)))+(((IkReal(-1.00000000000000))*(r20)*(x270)*(x273)))+(((IkReal(-1.00000000000000))*(r20)*(x274)))+(((r22)*(x272)))+(((IkReal(-1.00000000000000))*(r21)*(x277))));
00977 evalcond[1]=((((x271)*(x274)))+(((IkReal(-1.00000000000000))*(r20)*(x270)*(x277)))+(((r22)*(x276)))+(((r21)*(x273)))+(((r20)*(x275)))+(((IkReal(-1.00000000000000))*(cj3))));
00978 evalcond[2]=((IkReal(-1.00000000000000))+(((r02)*(x272)))+(((IkReal(-1.00000000000000))*(cj5)*(x267)*(x273)))+(((IkReal(-1.00000000000000))*(r01)*(x277)))+(((IkReal(-1.00000000000000))*(x267)*(x274)))+(((x268)*(x275))));
00979 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x270)*(x273)))+(((IkReal(-1.00000000000000))*(r11)*(x277)))+(((r12)*(x272)))+(((IkReal(-1.00000000000000))*(r10)*(x274)))+(((x269)*(x275))));
00980 evalcond[4]=((((r02)*(x276)))+(((IkReal(-1.00000000000000))*(cj5)*(x267)*(x277)))+(((r01)*(x273)))+(((r00)*(x275)))+(((x268)*(x274))));
00981 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x270)*(x277)))+(((IkReal(-1.00000000000000))*(sj3)))+(((r12)*(x276)))+(((r11)*(x273)))+(((x269)*(x274)))+(((r10)*(x275))));
00982 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
00983 {
00984 continue;
00985 }
00986 }
00987 
00988 {
00989 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
00990 vinfos[0].jointtype = 1;
00991 vinfos[0].foffset = j0;
00992 vinfos[0].indices[0] = _ij0[0];
00993 vinfos[0].indices[1] = _ij0[1];
00994 vinfos[0].maxsolutions = _nj0;
00995 vinfos[1].jointtype = 1;
00996 vinfos[1].foffset = j1;
00997 vinfos[1].indices[0] = _ij1[0];
00998 vinfos[1].indices[1] = _ij1[1];
00999 vinfos[1].maxsolutions = _nj1;
01000 vinfos[2].jointtype = 1;
01001 vinfos[2].foffset = j2;
01002 vinfos[2].indices[0] = _ij2[0];
01003 vinfos[2].indices[1] = _ij2[1];
01004 vinfos[2].maxsolutions = _nj2;
01005 vinfos[3].jointtype = 1;
01006 vinfos[3].foffset = j3;
01007 vinfos[3].indices[0] = _ij3[0];
01008 vinfos[3].indices[1] = _ij3[1];
01009 vinfos[3].maxsolutions = _nj3;
01010 vinfos[4].jointtype = 1;
01011 vinfos[4].foffset = j4;
01012 vinfos[4].indices[0] = _ij4[0];
01013 vinfos[4].indices[1] = _ij4[1];
01014 vinfos[4].maxsolutions = _nj4;
01015 vinfos[5].jointtype = 1;
01016 vinfos[5].foffset = j5;
01017 vinfos[5].indices[0] = _ij5[0];
01018 vinfos[5].indices[1] = _ij5[1];
01019 vinfos[5].maxsolutions = _nj5;
01020 vinfos[6].jointtype = 1;
01021 vinfos[6].foffset = j6;
01022 vinfos[6].indices[0] = _ij6[0];
01023 vinfos[6].indices[1] = _ij6[1];
01024 vinfos[6].maxsolutions = _nj6;
01025 std::vector<int> vfree(0);
01026 solutions.AddSolution(vinfos,vfree);
01027 }
01028 }
01029 }
01030 
01031 }
01032 
01033 }
01034 }
01035 }
01036 
01037 } else
01038 {
01039 IkReal x278=((IkReal(0.000500000000000000))*(cj6));
01040 IkReal x279=((IkReal(0.000500000000000000))*(sj6));
01041 IkReal x280=((sj5)*(sj6));
01042 IkReal x281=((cj5)*(r02));
01043 IkReal x282=((IkReal(0.390000000000000))*(cj5));
01044 IkReal x283=((IkReal(0.390000000000000))*(cj6)*(sj5));
01045 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
01046 evalcond[1]=((((cj6)*(r00)*(sj5)))+(x281)+(((IkReal(-1.00000000000000))*(r01)*(x280))));
01047 evalcond[2]=((((r22)*(x282)))+(((r21)*(x278)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.390000000000000))*(r21)*(x280)))+(((r20)*(x279)))+(((r20)*(x283))));
01048 evalcond[3]=((IkReal(0.00200000000000000))+(((r00)*(x283)))+(((IkReal(0.390000000000000))*(x281)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x278)))+(((r00)*(x279)))+(((IkReal(-0.390000000000000))*(r01)*(x280))));
01049 evalcond[4]=((IkReal(0.400000000000000))+(((r10)*(x283)))+(((IkReal(-0.390000000000000))*(r11)*(x280)))+(((r12)*(x282)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x278)))+(((r10)*(x279))));
01050 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
01051 {
01052 {
01053 IkReal j3array[1], cj3array[1], sj3array[1];
01054 bool j3valid[1]={false};
01055 _nj3 = 1;
01056 IkReal x284=((cj6)*(sj5));
01057 IkReal x285=((IkReal(1.00000000000000))*(sj5)*(sj6));
01058 if( IKabs(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x285)))+(((r20)*(x284))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj5)*(r12)))+(((r10)*(x284)))+(((IkReal(-1.00000000000000))*(r11)*(x285))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x285)))+(((r20)*(x284)))))+IKsqr(((((cj5)*(r12)))+(((r10)*(x284)))+(((IkReal(-1.00000000000000))*(r11)*(x285)))))-1) <= IKFAST_SINCOS_THRESH )
01059     continue;
01060 j3array[0]=IKatan2(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x285)))+(((r20)*(x284)))), ((((cj5)*(r12)))+(((r10)*(x284)))+(((IkReal(-1.00000000000000))*(r11)*(x285)))));
01061 sj3array[0]=IKsin(j3array[0]);
01062 cj3array[0]=IKcos(j3array[0]);
01063 if( j3array[0] > IKPI )
01064 {
01065     j3array[0]-=IK2PI;
01066 }
01067 else if( j3array[0] < -IKPI )
01068 {    j3array[0]+=IK2PI;
01069 }
01070 j3valid[0] = true;
01071 for(int ij3 = 0; ij3 < 1; ++ij3)
01072 {
01073 if( !j3valid[ij3] )
01074 {
01075     continue;
01076 }
01077 _ij3[0] = ij3; _ij3[1] = -1;
01078 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01079 {
01080 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01081 {
01082     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01083 }
01084 }
01085 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01086 {
01087 IkReal evalcond[2];
01088 IkReal x286=((cj6)*(sj5));
01089 IkReal x287=((IkReal(1.00000000000000))*(sj5)*(sj6));
01090 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x287)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((r20)*(x286))));
01091 evalcond[1]=((((cj5)*(r12)))+(((r10)*(x286)))+(((IkReal(-1.00000000000000))*(r11)*(x287)))+(((IkReal(-1.00000000000000))*(IKcos(j3)))));
01092 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
01093 {
01094 continue;
01095 }
01096 }
01097 
01098 {
01099 IkReal dummyeval[1];
01100 IkReal gconst43;
01101 IkReal x288=(sj6)*(sj6);
01102 IkReal x289=(cj6)*(cj6);
01103 IkReal x290=((IkReal(1.00000000000000))*(r20));
01104 IkReal x291=((r00)*(r21));
01105 IkReal x292=((r22)*(sj5));
01106 IkReal x293=((r02)*(sj5));
01107 IkReal x294=((cj5)*(x288));
01108 IkReal x295=((cj5)*(x289));
01109 gconst43=IKsign(((((x291)*(x294)))+(((x291)*(x295)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x293)))+(((IkReal(-1.00000000000000))*(sj6)*(x290)*(x293)))+(((IkReal(-1.00000000000000))*(r01)*(x290)*(x295)))+(((IkReal(-1.00000000000000))*(r01)*(x290)*(x294)))+(((cj6)*(r01)*(x292)))+(((r00)*(sj6)*(x292)))));
01110 IkReal x296=(sj6)*(sj6);
01111 IkReal x297=(cj6)*(cj6);
01112 IkReal x298=((IkReal(1.00000000000000))*(r20));
01113 IkReal x299=((r00)*(r21));
01114 IkReal x300=((r22)*(sj5));
01115 IkReal x301=((r02)*(sj5));
01116 IkReal x302=((cj5)*(x296));
01117 IkReal x303=((cj5)*(x297));
01118 dummyeval[0]=((((IkReal(-1.00000000000000))*(r01)*(x298)*(x303)))+(((IkReal(-1.00000000000000))*(r01)*(x298)*(x302)))+(((r00)*(sj6)*(x300)))+(((x299)*(x302)))+(((x299)*(x303)))+(((cj6)*(r01)*(x300)))+(((IkReal(-1.00000000000000))*(sj6)*(x298)*(x301)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x301))));
01119 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01120 {
01121 {
01122 IkReal dummyeval[1];
01123 IkReal gconst44;
01124 IkReal x304=(r20)*(r20);
01125 IkReal x305=(cj5)*(cj5);
01126 IkReal x306=(sj6)*(sj6);
01127 IkReal x307=(cj6)*(cj6);
01128 IkReal x308=(r21)*(r21);
01129 IkReal x309=((cj6)*(r20));
01130 IkReal x310=((IkReal(2.00000000000000))*(r21)*(sj6));
01131 IkReal x311=((cj5)*(r22)*(sj5));
01132 gconst44=IKsign(((((x310)*(x311)))+(((IkReal(-2.00000000000000))*(x309)*(x311)))+(((x309)*(x310)))+(((IkReal(-1.00000000000000))*(x305)*(x309)*(x310)))+(((x305)*(x306)*(x308)))+(((x304)*(x306)))+(((x304)*(x305)*(x307)))+(((x307)*(x308)))+((((r22)*(r22))*((sj5)*(sj5))))));
01133 IkReal x312=(r20)*(r20);
01134 IkReal x313=(cj5)*(cj5);
01135 IkReal x314=(sj6)*(sj6);
01136 IkReal x315=(cj6)*(cj6);
01137 IkReal x316=(r21)*(r21);
01138 IkReal x317=((cj6)*(r20));
01139 IkReal x318=((IkReal(2.00000000000000))*(r21)*(sj6));
01140 IkReal x319=((cj5)*(r22)*(sj5));
01141 dummyeval[0]=((((x317)*(x318)))+(((x313)*(x314)*(x316)))+(((x312)*(x314)))+(((x315)*(x316)))+(((IkReal(-1.00000000000000))*(x313)*(x317)*(x318)))+(((x312)*(x313)*(x315)))+(((IkReal(-2.00000000000000))*(x317)*(x319)))+(((x318)*(x319)))+((((r22)*(r22))*((sj5)*(sj5)))));
01142 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01143 {
01144 continue;
01145 
01146 } else
01147 {
01148 {
01149 IkReal j4array[1], cj4array[1], sj4array[1];
01150 bool j4valid[1]={false};
01151 _nj4 = 1;
01152 IkReal x320=((cj3)*(cj6));
01153 IkReal x321=((IkReal(1.00000000000000))*(r21));
01154 IkReal x322=((cj3)*(sj6));
01155 if( IKabs(((gconst44)*(((((IkReal(-1.00000000000000))*(x320)*(x321)))+(((IkReal(-1.00000000000000))*(r20)*(x322))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x320)))+(((IkReal(-1.00000000000000))*(cj5)*(x321)*(x322))))))) < IKFAST_ATAN2_MAGTHRESH )
01156     continue;
01157 j4array[0]=IKatan2(((gconst44)*(((((IkReal(-1.00000000000000))*(x320)*(x321)))+(((IkReal(-1.00000000000000))*(r20)*(x322)))))), ((gconst44)*(((((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x320)))+(((IkReal(-1.00000000000000))*(cj5)*(x321)*(x322)))))));
01158 sj4array[0]=IKsin(j4array[0]);
01159 cj4array[0]=IKcos(j4array[0]);
01160 if( j4array[0] > IKPI )
01161 {
01162     j4array[0]-=IK2PI;
01163 }
01164 else if( j4array[0] < -IKPI )
01165 {    j4array[0]+=IK2PI;
01166 }
01167 j4valid[0] = true;
01168 for(int ij4 = 0; ij4 < 1; ++ij4)
01169 {
01170 if( !j4valid[ij4] )
01171 {
01172     continue;
01173 }
01174 _ij4[0] = ij4; _ij4[1] = -1;
01175 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01176 {
01177 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01178 {
01179     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01180 }
01181 }
01182 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01183 {
01184 IkReal evalcond[6];
01185 IkReal x323=IKsin(j4);
01186 IkReal x324=IKcos(j4);
01187 IkReal x325=((IkReal(1.00000000000000))*(r00));
01188 IkReal x326=((cj5)*(r01));
01189 IkReal x327=((cj5)*(r11));
01190 IkReal x328=((IkReal(1.00000000000000))*(cj5));
01191 IkReal x329=((cj5)*(r21));
01192 IkReal x330=((sj5)*(x323));
01193 IkReal x331=((cj6)*(x323));
01194 IkReal x332=((sj6)*(x324));
01195 IkReal x333=((sj6)*(x323));
01196 IkReal x334=((sj5)*(x324));
01197 IkReal x335=((cj6)*(x324));
01198 evalcond[0]=((((r22)*(x330)))+(((IkReal(-1.00000000000000))*(r20)*(x332)))+(((IkReal(-1.00000000000000))*(r20)*(x328)*(x331)))+(((IkReal(-1.00000000000000))*(r21)*(x335)))+(((x329)*(x333))));
01199 evalcond[1]=((((r22)*(x334)))+(cj3)+(((r21)*(x331)))+(((IkReal(-1.00000000000000))*(r20)*(x328)*(x335)))+(((r20)*(x333)))+(((x329)*(x332))));
01200 evalcond[2]=((IkReal(1.00000000000000))+(((r02)*(x330)))+(((IkReal(-1.00000000000000))*(r01)*(x335)))+(((x326)*(x333)))+(((IkReal(-1.00000000000000))*(x325)*(x332)))+(((IkReal(-1.00000000000000))*(cj5)*(x325)*(x331))));
01201 evalcond[3]=((((r12)*(x330)))+(((IkReal(-1.00000000000000))*(r11)*(x335)))+(((IkReal(-1.00000000000000))*(r10)*(x328)*(x331)))+(((x327)*(x333)))+(((IkReal(-1.00000000000000))*(r10)*(x332))));
01202 evalcond[4]=((((r01)*(x331)))+(((r00)*(x333)))+(((r02)*(x334)))+(((x326)*(x332)))+(((IkReal(-1.00000000000000))*(cj5)*(x325)*(x335))));
01203 evalcond[5]=((((r11)*(x331)))+(((IkReal(-1.00000000000000))*(sj3)))+(((r12)*(x334)))+(((IkReal(-1.00000000000000))*(r10)*(x328)*(x335)))+(((x327)*(x332)))+(((r10)*(x333))));
01204 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01205 {
01206 continue;
01207 }
01208 }
01209 
01210 {
01211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
01212 vinfos[0].jointtype = 1;
01213 vinfos[0].foffset = j0;
01214 vinfos[0].indices[0] = _ij0[0];
01215 vinfos[0].indices[1] = _ij0[1];
01216 vinfos[0].maxsolutions = _nj0;
01217 vinfos[1].jointtype = 1;
01218 vinfos[1].foffset = j1;
01219 vinfos[1].indices[0] = _ij1[0];
01220 vinfos[1].indices[1] = _ij1[1];
01221 vinfos[1].maxsolutions = _nj1;
01222 vinfos[2].jointtype = 1;
01223 vinfos[2].foffset = j2;
01224 vinfos[2].indices[0] = _ij2[0];
01225 vinfos[2].indices[1] = _ij2[1];
01226 vinfos[2].maxsolutions = _nj2;
01227 vinfos[3].jointtype = 1;
01228 vinfos[3].foffset = j3;
01229 vinfos[3].indices[0] = _ij3[0];
01230 vinfos[3].indices[1] = _ij3[1];
01231 vinfos[3].maxsolutions = _nj3;
01232 vinfos[4].jointtype = 1;
01233 vinfos[4].foffset = j4;
01234 vinfos[4].indices[0] = _ij4[0];
01235 vinfos[4].indices[1] = _ij4[1];
01236 vinfos[4].maxsolutions = _nj4;
01237 vinfos[5].jointtype = 1;
01238 vinfos[5].foffset = j5;
01239 vinfos[5].indices[0] = _ij5[0];
01240 vinfos[5].indices[1] = _ij5[1];
01241 vinfos[5].maxsolutions = _nj5;
01242 vinfos[6].jointtype = 1;
01243 vinfos[6].foffset = j6;
01244 vinfos[6].indices[0] = _ij6[0];
01245 vinfos[6].indices[1] = _ij6[1];
01246 vinfos[6].maxsolutions = _nj6;
01247 std::vector<int> vfree(0);
01248 solutions.AddSolution(vinfos,vfree);
01249 }
01250 }
01251 }
01252 
01253 }
01254 
01255 }
01256 
01257 } else
01258 {
01259 {
01260 IkReal j4array[1], cj4array[1], sj4array[1];
01261 bool j4valid[1]={false};
01262 _nj4 = 1;
01263 IkReal x336=((cj3)*(sj6));
01264 IkReal x337=((IkReal(1.00000000000000))*(r00));
01265 IkReal x338=((cj3)*(cj6));
01266 if( IKabs(((gconst43)*(((((cj5)*(r01)*(x336)))+(((cj3)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x337)*(x338))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((IkReal(-1.00000000000000))*(r01)*(x338)))+(((IkReal(-1.00000000000000))*(x336)*(x337))))))) < IKFAST_ATAN2_MAGTHRESH )
01267     continue;
01268 j4array[0]=IKatan2(((gconst43)*(((((cj5)*(r01)*(x336)))+(((cj3)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x337)*(x338)))))), ((gconst43)*(((((IkReal(-1.00000000000000))*(r01)*(x338)))+(((IkReal(-1.00000000000000))*(x336)*(x337)))))));
01269 sj4array[0]=IKsin(j4array[0]);
01270 cj4array[0]=IKcos(j4array[0]);
01271 if( j4array[0] > IKPI )
01272 {
01273     j4array[0]-=IK2PI;
01274 }
01275 else if( j4array[0] < -IKPI )
01276 {    j4array[0]+=IK2PI;
01277 }
01278 j4valid[0] = true;
01279 for(int ij4 = 0; ij4 < 1; ++ij4)
01280 {
01281 if( !j4valid[ij4] )
01282 {
01283     continue;
01284 }
01285 _ij4[0] = ij4; _ij4[1] = -1;
01286 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01287 {
01288 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01289 {
01290     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01291 }
01292 }
01293 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01294 {
01295 IkReal evalcond[6];
01296 IkReal x339=IKsin(j4);
01297 IkReal x340=IKcos(j4);
01298 IkReal x341=((IkReal(1.00000000000000))*(r00));
01299 IkReal x342=((cj5)*(r01));
01300 IkReal x343=((cj5)*(r11));
01301 IkReal x344=((IkReal(1.00000000000000))*(cj5));
01302 IkReal x345=((cj5)*(r21));
01303 IkReal x346=((sj5)*(x339));
01304 IkReal x347=((cj6)*(x339));
01305 IkReal x348=((sj6)*(x340));
01306 IkReal x349=((sj6)*(x339));
01307 IkReal x350=((sj5)*(x340));
01308 IkReal x351=((cj6)*(x340));
01309 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x344)*(x347)))+(((r22)*(x346)))+(((x345)*(x349)))+(((IkReal(-1.00000000000000))*(r21)*(x351)))+(((IkReal(-1.00000000000000))*(r20)*(x348))));
01310 evalcond[1]=((((r21)*(x347)))+(cj3)+(((x345)*(x348)))+(((IkReal(-1.00000000000000))*(r20)*(x344)*(x351)))+(((r22)*(x350)))+(((r20)*(x349))));
01311 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj5)*(x341)*(x347)))+(((IkReal(-1.00000000000000))*(r01)*(x351)))+(((r02)*(x346)))+(((IkReal(-1.00000000000000))*(x341)*(x348)))+(((x342)*(x349))));
01312 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x344)*(x347)))+(((IkReal(-1.00000000000000))*(r10)*(x348)))+(((IkReal(-1.00000000000000))*(r11)*(x351)))+(((x343)*(x349)))+(((r12)*(x346))));
01313 evalcond[4]=((((r01)*(x347)))+(((r00)*(x349)))+(((IkReal(-1.00000000000000))*(cj5)*(x341)*(x351)))+(((r02)*(x350)))+(((x342)*(x348))));
01314 evalcond[5]=((((IkReal(-1.00000000000000))*(sj3)))+(((r12)*(x350)))+(((IkReal(-1.00000000000000))*(r10)*(x344)*(x351)))+(((r11)*(x347)))+(((x343)*(x348)))+(((r10)*(x349))));
01315 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01316 {
01317 continue;
01318 }
01319 }
01320 
01321 {
01322 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
01323 vinfos[0].jointtype = 1;
01324 vinfos[0].foffset = j0;
01325 vinfos[0].indices[0] = _ij0[0];
01326 vinfos[0].indices[1] = _ij0[1];
01327 vinfos[0].maxsolutions = _nj0;
01328 vinfos[1].jointtype = 1;
01329 vinfos[1].foffset = j1;
01330 vinfos[1].indices[0] = _ij1[0];
01331 vinfos[1].indices[1] = _ij1[1];
01332 vinfos[1].maxsolutions = _nj1;
01333 vinfos[2].jointtype = 1;
01334 vinfos[2].foffset = j2;
01335 vinfos[2].indices[0] = _ij2[0];
01336 vinfos[2].indices[1] = _ij2[1];
01337 vinfos[2].maxsolutions = _nj2;
01338 vinfos[3].jointtype = 1;
01339 vinfos[3].foffset = j3;
01340 vinfos[3].indices[0] = _ij3[0];
01341 vinfos[3].indices[1] = _ij3[1];
01342 vinfos[3].maxsolutions = _nj3;
01343 vinfos[4].jointtype = 1;
01344 vinfos[4].foffset = j4;
01345 vinfos[4].indices[0] = _ij4[0];
01346 vinfos[4].indices[1] = _ij4[1];
01347 vinfos[4].maxsolutions = _nj4;
01348 vinfos[5].jointtype = 1;
01349 vinfos[5].foffset = j5;
01350 vinfos[5].indices[0] = _ij5[0];
01351 vinfos[5].indices[1] = _ij5[1];
01352 vinfos[5].maxsolutions = _nj5;
01353 vinfos[6].jointtype = 1;
01354 vinfos[6].foffset = j6;
01355 vinfos[6].indices[0] = _ij6[0];
01356 vinfos[6].indices[1] = _ij6[1];
01357 vinfos[6].maxsolutions = _nj6;
01358 std::vector<int> vfree(0);
01359 solutions.AddSolution(vinfos,vfree);
01360 }
01361 }
01362 }
01363 
01364 }
01365 
01366 }
01367 }
01368 }
01369 
01370 } else
01371 {
01372 if( 1 )
01373 {
01374 continue;
01375 
01376 } else
01377 {
01378 }
01379 }
01380 }
01381 }
01382 
01383 } else
01384 {
01385 {
01386 IkReal j3array[1], cj3array[1], sj3array[1];
01387 bool j3valid[1]={false};
01388 _nj3 = 1;
01389 IkReal x352=((cj6)*(sj5));
01390 IkReal x353=((IkReal(1.00000000000000))*(sj5)*(sj6));
01391 if( IKabs(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((r00)*(x352)))+(((IkReal(-1.00000000000000))*(r01)*(x353)))+(((cj5)*(r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r11)*(x353)))+(((cj5)*(r12)))+(((r10)*(x352))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((r00)*(x352)))+(((IkReal(-1.00000000000000))*(r01)*(x353)))+(((cj5)*(r02)))))))+IKsqr(((((IkReal(-1.00000000000000))*(r11)*(x353)))+(((cj5)*(r12)))+(((r10)*(x352)))))-1) <= IKFAST_SINCOS_THRESH )
01392     continue;
01393 j3array[0]=IKatan2(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((r00)*(x352)))+(((IkReal(-1.00000000000000))*(r01)*(x353)))+(((cj5)*(r02)))))), ((((IkReal(-1.00000000000000))*(r11)*(x353)))+(((cj5)*(r12)))+(((r10)*(x352)))));
01394 sj3array[0]=IKsin(j3array[0]);
01395 cj3array[0]=IKcos(j3array[0]);
01396 if( j3array[0] > IKPI )
01397 {
01398     j3array[0]-=IK2PI;
01399 }
01400 else if( j3array[0] < -IKPI )
01401 {    j3array[0]+=IK2PI;
01402 }
01403 j3valid[0] = true;
01404 for(int ij3 = 0; ij3 < 1; ++ij3)
01405 {
01406 if( !j3valid[ij3] )
01407 {
01408     continue;
01409 }
01410 _ij3[0] = ij3; _ij3[1] = -1;
01411 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01412 {
01413 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01414 {
01415     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01416 }
01417 }
01418 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01419 {
01420 IkReal evalcond[3];
01421 IkReal x354=IKsin(j3);
01422 IkReal x355=((cj6)*(sj5));
01423 IkReal x356=((IkReal(1.00000000000000))*(sj5)*(sj6));
01424 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x354)))+(((IkReal(-1.00000000000000))*(r21)*(x356)))+(((r20)*(x355))));
01425 evalcond[1]=((((IkReal(-1.00000000000000))*(cj2)*(x354)))+(((r00)*(x355)))+(((IkReal(-1.00000000000000))*(r01)*(x356)))+(((cj5)*(r02))));
01426 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x356)))+(((cj5)*(r12)))+(((r10)*(x355)))+(((IkReal(-1.00000000000000))*(IKcos(j3)))));
01427 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01428 {
01429 continue;
01430 }
01431 }
01432 
01433 {
01434 IkReal dummyeval[1];
01435 IkReal gconst36;
01436 IkReal x357=(cj5)*(cj5);
01437 IkReal x358=(r11)*(r11);
01438 IkReal x359=(sj6)*(sj6);
01439 IkReal x360=(cj6)*(cj6);
01440 IkReal x361=(r10)*(r10);
01441 IkReal x362=((r11)*(sj6));
01442 IkReal x363=((IkReal(1.00000000000000))*(x359));
01443 IkReal x364=((IkReal(1.00000000000000))*(x360));
01444 IkReal x365=((IkReal(2.00000000000000))*(cj6)*(r10));
01445 IkReal x366=((cj5)*(r12)*(sj5));
01446 gconst36=IKsign(((((IkReal(-1.00000000000000))*(x357)*(x358)*(x363)))+(((IkReal(-1.00000000000000))*(x362)*(x365)))+(((IkReal(-1.00000000000000))*(x361)*(x363)))+(((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x358)*(x364)))+(((IkReal(-2.00000000000000))*(x362)*(x366)))+(((x365)*(x366)))+(((IkReal(-1.00000000000000))*(x357)*(x361)*(x364)))+(((x357)*(x362)*(x365)))));
01447 IkReal x367=(cj5)*(cj5);
01448 IkReal x368=(r11)*(r11);
01449 IkReal x369=(sj6)*(sj6);
01450 IkReal x370=(cj6)*(cj6);
01451 IkReal x371=(r10)*(r10);
01452 IkReal x372=((r11)*(sj6));
01453 IkReal x373=((IkReal(1.00000000000000))*(x369));
01454 IkReal x374=((IkReal(1.00000000000000))*(x370));
01455 IkReal x375=((IkReal(2.00000000000000))*(cj6)*(r10));
01456 IkReal x376=((cj5)*(r12)*(sj5));
01457 dummyeval[0]=((((IkReal(-1.00000000000000))*(x368)*(x374)))+(((x375)*(x376)))+(((IkReal(-1.00000000000000))*(x372)*(x375)))+(((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x367)*(x371)*(x374)))+(((IkReal(-2.00000000000000))*(x372)*(x376)))+(((x367)*(x372)*(x375)))+(((IkReal(-1.00000000000000))*(x371)*(x373)))+(((IkReal(-1.00000000000000))*(x367)*(x368)*(x373))));
01458 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01459 {
01460 {
01461 IkReal dummyeval[1];
01462 IkReal gconst35;
01463 IkReal x377=(sj6)*(sj6);
01464 IkReal x378=(cj5)*(cj5);
01465 IkReal x379=(cj6)*(cj6);
01466 IkReal x380=((cj5)*(sj5));
01467 IkReal x381=((cj6)*(r00));
01468 IkReal x382=((r11)*(sj6));
01469 IkReal x383=((r01)*(r11));
01470 IkReal x384=((r01)*(sj6));
01471 IkReal x385=((r00)*(r10));
01472 IkReal x386=((IkReal(1.00000000000000))*(cj6)*(r10));
01473 gconst35=IKsign(((((r02)*(x380)*(x382)))+(((x377)*(x385)))+(((IkReal(-1.00000000000000))*(x378)*(x384)*(x386)))+(((IkReal(-1.00000000000000))*(x378)*(x381)*(x382)))+(((cj6)*(r10)*(x384)))+(((x377)*(x378)*(x383)))+(((x378)*(x379)*(x385)))+(((r02)*(r12)*((sj5)*(sj5))))+(((x379)*(x383)))+(((r12)*(x380)*(x384)))+(((x381)*(x382)))+(((IkReal(-1.00000000000000))*(r02)*(x380)*(x386)))+(((IkReal(-1.00000000000000))*(r12)*(x380)*(x381)))));
01474 IkReal x387=(sj6)*(sj6);
01475 IkReal x388=(cj5)*(cj5);
01476 IkReal x389=(cj6)*(cj6);
01477 IkReal x390=((cj5)*(sj5));
01478 IkReal x391=((cj6)*(r00));
01479 IkReal x392=((r11)*(sj6));
01480 IkReal x393=((r01)*(r11));
01481 IkReal x394=((r01)*(sj6));
01482 IkReal x395=((r00)*(r10));
01483 IkReal x396=((IkReal(1.00000000000000))*(cj6)*(r10));
01484 dummyeval[0]=((((IkReal(-1.00000000000000))*(x388)*(x394)*(x396)))+(((IkReal(-1.00000000000000))*(r02)*(x390)*(x396)))+(((IkReal(-1.00000000000000))*(r12)*(x390)*(x391)))+(((x387)*(x388)*(x393)))+(((r02)*(r12)*((sj5)*(sj5))))+(((r02)*(x390)*(x392)))+(((x391)*(x392)))+(((cj6)*(r10)*(x394)))+(((x387)*(x395)))+(((r12)*(x390)*(x394)))+(((x389)*(x393)))+(((x388)*(x389)*(x395)))+(((IkReal(-1.00000000000000))*(x388)*(x391)*(x392))));
01485 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01486 {
01487 continue;
01488 
01489 } else
01490 {
01491 {
01492 IkReal j4array[1], cj4array[1], sj4array[1];
01493 bool j4valid[1]={false};
01494 _nj4 = 1;
01495 IkReal x397=((IkReal(1.00000000000000))*(r11));
01496 IkReal x398=((cj2)*(cj3)*(cj6));
01497 IkReal x399=((cj2)*(cj3)*(sj6));
01498 if( IKabs(((gconst35)*(((((IkReal(-1.00000000000000))*(x397)*(x398)))+(((IkReal(-1.00000000000000))*(r10)*(x399))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((IkReal(-1.00000000000000))*(cj2)*(cj3)*(r12)*(sj5)))+(((cj5)*(r10)*(x398)))+(((IkReal(-1.00000000000000))*(cj5)*(x397)*(x399))))))) < IKFAST_ATAN2_MAGTHRESH )
01499     continue;
01500 j4array[0]=IKatan2(((gconst35)*(((((IkReal(-1.00000000000000))*(x397)*(x398)))+(((IkReal(-1.00000000000000))*(r10)*(x399)))))), ((gconst35)*(((((IkReal(-1.00000000000000))*(cj2)*(cj3)*(r12)*(sj5)))+(((cj5)*(r10)*(x398)))+(((IkReal(-1.00000000000000))*(cj5)*(x397)*(x399)))))));
01501 sj4array[0]=IKsin(j4array[0]);
01502 cj4array[0]=IKcos(j4array[0]);
01503 if( j4array[0] > IKPI )
01504 {
01505     j4array[0]-=IK2PI;
01506 }
01507 else if( j4array[0] < -IKPI )
01508 {    j4array[0]+=IK2PI;
01509 }
01510 j4valid[0] = true;
01511 for(int ij4 = 0; ij4 < 1; ++ij4)
01512 {
01513 if( !j4valid[ij4] )
01514 {
01515     continue;
01516 }
01517 _ij4[0] = ij4; _ij4[1] = -1;
01518 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01519 {
01520 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01521 {
01522     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01523 }
01524 }
01525 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01526 {
01527 IkReal evalcond[6];
01528 IkReal x400=IKsin(j4);
01529 IkReal x401=IKcos(j4);
01530 IkReal x402=((IkReal(1.00000000000000))*(r00));
01531 IkReal x403=((cj5)*(r01));
01532 IkReal x404=((cj5)*(r11));
01533 IkReal x405=((IkReal(1.00000000000000))*(sj2));
01534 IkReal x406=((IkReal(1.00000000000000))*(cj5));
01535 IkReal x407=((cj5)*(r21));
01536 IkReal x408=((sj5)*(x400));
01537 IkReal x409=((cj6)*(x400));
01538 IkReal x410=((sj6)*(x401));
01539 IkReal x411=((sj6)*(x400));
01540 IkReal x412=((sj5)*(x401));
01541 IkReal x413=((cj6)*(x401));
01542 evalcond[0]=((((x407)*(x411)))+(((r22)*(x408)))+(((IkReal(-1.00000000000000))*(r20)*(x410)))+(((IkReal(-1.00000000000000))*(r21)*(x413)))+(((IkReal(-1.00000000000000))*(r20)*(x406)*(x409)))+(((IkReal(-1.00000000000000))*(cj2))));
01543 evalcond[1]=((((x407)*(x410)))+(((IkReal(-1.00000000000000))*(cj3)*(x405)))+(((r22)*(x412)))+(((r21)*(x409)))+(((r20)*(x411)))+(((IkReal(-1.00000000000000))*(r20)*(x406)*(x413))));
01544 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x413)))+(((IkReal(-1.00000000000000))*(cj5)*(x402)*(x409)))+(((IkReal(-1.00000000000000))*(x402)*(x410)))+(((x403)*(x411)))+(((IkReal(-1.00000000000000))*(x405)))+(((r02)*(x408))));
01545 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x410)))+(((x404)*(x411)))+(((r12)*(x408)))+(((IkReal(-1.00000000000000))*(r11)*(x413)))+(((IkReal(-1.00000000000000))*(r10)*(x406)*(x409))));
01546 evalcond[4]=((((cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x402)*(x413)))+(((r01)*(x409)))+(((r00)*(x411)))+(((x403)*(x410)))+(((r02)*(x412))));
01547 evalcond[5]=((((r11)*(x409)))+(((IkReal(-1.00000000000000))*(sj3)))+(((x404)*(x410)))+(((r12)*(x412)))+(((IkReal(-1.00000000000000))*(r10)*(x406)*(x413)))+(((r10)*(x411))));
01548 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01549 {
01550 continue;
01551 }
01552 }
01553 
01554 {
01555 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
01556 vinfos[0].jointtype = 1;
01557 vinfos[0].foffset = j0;
01558 vinfos[0].indices[0] = _ij0[0];
01559 vinfos[0].indices[1] = _ij0[1];
01560 vinfos[0].maxsolutions = _nj0;
01561 vinfos[1].jointtype = 1;
01562 vinfos[1].foffset = j1;
01563 vinfos[1].indices[0] = _ij1[0];
01564 vinfos[1].indices[1] = _ij1[1];
01565 vinfos[1].maxsolutions = _nj1;
01566 vinfos[2].jointtype = 1;
01567 vinfos[2].foffset = j2;
01568 vinfos[2].indices[0] = _ij2[0];
01569 vinfos[2].indices[1] = _ij2[1];
01570 vinfos[2].maxsolutions = _nj2;
01571 vinfos[3].jointtype = 1;
01572 vinfos[3].foffset = j3;
01573 vinfos[3].indices[0] = _ij3[0];
01574 vinfos[3].indices[1] = _ij3[1];
01575 vinfos[3].maxsolutions = _nj3;
01576 vinfos[4].jointtype = 1;
01577 vinfos[4].foffset = j4;
01578 vinfos[4].indices[0] = _ij4[0];
01579 vinfos[4].indices[1] = _ij4[1];
01580 vinfos[4].maxsolutions = _nj4;
01581 vinfos[5].jointtype = 1;
01582 vinfos[5].foffset = j5;
01583 vinfos[5].indices[0] = _ij5[0];
01584 vinfos[5].indices[1] = _ij5[1];
01585 vinfos[5].maxsolutions = _nj5;
01586 vinfos[6].jointtype = 1;
01587 vinfos[6].foffset = j6;
01588 vinfos[6].indices[0] = _ij6[0];
01589 vinfos[6].indices[1] = _ij6[1];
01590 vinfos[6].maxsolutions = _nj6;
01591 std::vector<int> vfree(0);
01592 solutions.AddSolution(vinfos,vfree);
01593 }
01594 }
01595 }
01596 
01597 }
01598 
01599 }
01600 
01601 } else
01602 {
01603 {
01604 IkReal j4array[1], cj4array[1], sj4array[1];
01605 bool j4valid[1]={false};
01606 _nj4 = 1;
01607 IkReal x414=((IkReal(1.00000000000000))*(sj3));
01608 if( IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x414)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x414))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(r12)*(sj5)*(x414)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj6)*(x414)))+(((cj5)*(cj6)*(r10)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
01609     continue;
01610 j4array[0]=IKatan2(((gconst36)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x414)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x414)))))), ((gconst36)*(((((IkReal(-1.00000000000000))*(r12)*(sj5)*(x414)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj6)*(x414)))+(((cj5)*(cj6)*(r10)*(sj3)))))));
01611 sj4array[0]=IKsin(j4array[0]);
01612 cj4array[0]=IKcos(j4array[0]);
01613 if( j4array[0] > IKPI )
01614 {
01615     j4array[0]-=IK2PI;
01616 }
01617 else if( j4array[0] < -IKPI )
01618 {    j4array[0]+=IK2PI;
01619 }
01620 j4valid[0] = true;
01621 for(int ij4 = 0; ij4 < 1; ++ij4)
01622 {
01623 if( !j4valid[ij4] )
01624 {
01625     continue;
01626 }
01627 _ij4[0] = ij4; _ij4[1] = -1;
01628 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01629 {
01630 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01631 {
01632     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01633 }
01634 }
01635 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01636 {
01637 IkReal evalcond[6];
01638 IkReal x415=IKsin(j4);
01639 IkReal x416=IKcos(j4);
01640 IkReal x417=((IkReal(1.00000000000000))*(r00));
01641 IkReal x418=((cj5)*(r01));
01642 IkReal x419=((cj5)*(r11));
01643 IkReal x420=((IkReal(1.00000000000000))*(sj2));
01644 IkReal x421=((IkReal(1.00000000000000))*(cj5));
01645 IkReal x422=((cj5)*(r21));
01646 IkReal x423=((sj5)*(x415));
01647 IkReal x424=((cj6)*(x415));
01648 IkReal x425=((sj6)*(x416));
01649 IkReal x426=((sj6)*(x415));
01650 IkReal x427=((sj5)*(x416));
01651 IkReal x428=((cj6)*(x416));
01652 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x428)))+(((IkReal(-1.00000000000000))*(r20)*(x425)))+(((r22)*(x423)))+(((IkReal(-1.00000000000000))*(r20)*(x421)*(x424)))+(((x422)*(x426)))+(((IkReal(-1.00000000000000))*(cj2))));
01653 evalcond[1]=((((r20)*(x426)))+(((IkReal(-1.00000000000000))*(cj3)*(x420)))+(((r21)*(x424)))+(((r22)*(x427)))+(((IkReal(-1.00000000000000))*(r20)*(x421)*(x428)))+(((x422)*(x425))));
01654 evalcond[2]=((((IkReal(-1.00000000000000))*(x420)))+(((r02)*(x423)))+(((IkReal(-1.00000000000000))*(r01)*(x428)))+(((IkReal(-1.00000000000000))*(cj5)*(x417)*(x424)))+(((x418)*(x426)))+(((IkReal(-1.00000000000000))*(x417)*(x425))));
01655 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x428)))+(((x419)*(x426)))+(((IkReal(-1.00000000000000))*(r10)*(x421)*(x424)))+(((IkReal(-1.00000000000000))*(r10)*(x425)))+(((r12)*(x423))));
01656 evalcond[4]=((((cj2)*(cj3)))+(((r02)*(x427)))+(((IkReal(-1.00000000000000))*(cj5)*(x417)*(x428)))+(((x418)*(x425)))+(((r00)*(x426)))+(((r01)*(x424))));
01657 evalcond[5]=((((IkReal(-1.00000000000000))*(sj3)))+(((x419)*(x425)))+(((r10)*(x426)))+(((IkReal(-1.00000000000000))*(r10)*(x421)*(x428)))+(((r12)*(x427)))+(((r11)*(x424))));
01658 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01659 {
01660 continue;
01661 }
01662 }
01663 
01664 {
01665 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
01666 vinfos[0].jointtype = 1;
01667 vinfos[0].foffset = j0;
01668 vinfos[0].indices[0] = _ij0[0];
01669 vinfos[0].indices[1] = _ij0[1];
01670 vinfos[0].maxsolutions = _nj0;
01671 vinfos[1].jointtype = 1;
01672 vinfos[1].foffset = j1;
01673 vinfos[1].indices[0] = _ij1[0];
01674 vinfos[1].indices[1] = _ij1[1];
01675 vinfos[1].maxsolutions = _nj1;
01676 vinfos[2].jointtype = 1;
01677 vinfos[2].foffset = j2;
01678 vinfos[2].indices[0] = _ij2[0];
01679 vinfos[2].indices[1] = _ij2[1];
01680 vinfos[2].maxsolutions = _nj2;
01681 vinfos[3].jointtype = 1;
01682 vinfos[3].foffset = j3;
01683 vinfos[3].indices[0] = _ij3[0];
01684 vinfos[3].indices[1] = _ij3[1];
01685 vinfos[3].maxsolutions = _nj3;
01686 vinfos[4].jointtype = 1;
01687 vinfos[4].foffset = j4;
01688 vinfos[4].indices[0] = _ij4[0];
01689 vinfos[4].indices[1] = _ij4[1];
01690 vinfos[4].maxsolutions = _nj4;
01691 vinfos[5].jointtype = 1;
01692 vinfos[5].foffset = j5;
01693 vinfos[5].indices[0] = _ij5[0];
01694 vinfos[5].indices[1] = _ij5[1];
01695 vinfos[5].maxsolutions = _nj5;
01696 vinfos[6].jointtype = 1;
01697 vinfos[6].foffset = j6;
01698 vinfos[6].indices[0] = _ij6[0];
01699 vinfos[6].indices[1] = _ij6[1];
01700 vinfos[6].maxsolutions = _nj6;
01701 std::vector<int> vfree(0);
01702 solutions.AddSolution(vinfos,vfree);
01703 }
01704 }
01705 }
01706 
01707 }
01708 
01709 }
01710 }
01711 }
01712 
01713 }
01714 
01715 }
01716 
01717 } else
01718 {
01719 {
01720 IkReal j4array[1], cj4array[1], sj4array[1];
01721 bool j4valid[1]={false};
01722 _nj4 = 1;
01723 IkReal x429=((r10)*(sj2));
01724 IkReal x430=((IkReal(1.00000000000000))*(sj6));
01725 IkReal x431=((IkReal(1.00000000000000))*(sj2));
01726 if( IKabs(((gconst34)*(((((IkReal(-1.00000000000000))*(x429)*(x430)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x431))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst34)*(((((IkReal(-1.00000000000000))*(r12)*(sj5)*(x431)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj2)*(x430)))+(((cj5)*(cj6)*(x429))))))) < IKFAST_ATAN2_MAGTHRESH )
01727     continue;
01728 j4array[0]=IKatan2(((gconst34)*(((((IkReal(-1.00000000000000))*(x429)*(x430)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x431)))))), ((gconst34)*(((((IkReal(-1.00000000000000))*(r12)*(sj5)*(x431)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj2)*(x430)))+(((cj5)*(cj6)*(x429)))))));
01729 sj4array[0]=IKsin(j4array[0]);
01730 cj4array[0]=IKcos(j4array[0]);
01731 if( j4array[0] > IKPI )
01732 {
01733     j4array[0]-=IK2PI;
01734 }
01735 else if( j4array[0] < -IKPI )
01736 {    j4array[0]+=IK2PI;
01737 }
01738 j4valid[0] = true;
01739 for(int ij4 = 0; ij4 < 1; ++ij4)
01740 {
01741 if( !j4valid[ij4] )
01742 {
01743     continue;
01744 }
01745 _ij4[0] = ij4; _ij4[1] = -1;
01746 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01747 {
01748 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01749 {
01750     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01751 }
01752 }
01753 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01754 {
01755 IkReal evalcond[3];
01756 IkReal x432=IKsin(j4);
01757 IkReal x433=IKcos(j4);
01758 IkReal x434=((IkReal(1.00000000000000))*(cj6));
01759 IkReal x435=((sj5)*(x432));
01760 IkReal x436=((cj5)*(x432));
01761 IkReal x437=((IkReal(1.00000000000000))*(sj6)*(x433));
01762 evalcond[0]=((((r21)*(sj6)*(x436)))+(((IkReal(-1.00000000000000))*(r20)*(x434)*(x436)))+(((r22)*(x435)))+(((IkReal(-1.00000000000000))*(r20)*(x437)))+(((IkReal(-1.00000000000000))*(r21)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(cj2))));
01763 evalcond[1]=((((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(r01)*(x433)*(x434)))+(((r02)*(x435)))+(((IkReal(-1.00000000000000))*(r00)*(x434)*(x436)))+(((r01)*(sj6)*(x436)))+(((IkReal(-1.00000000000000))*(r00)*(x437))));
01764 evalcond[2]=((((r12)*(x435)))+(((IkReal(-1.00000000000000))*(r10)*(x437)))+(((IkReal(-1.00000000000000))*(r11)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(r10)*(x434)*(x436)))+(((r11)*(sj6)*(x436))));
01765 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01766 {
01767 continue;
01768 }
01769 }
01770 
01771 {
01772 IkReal j3array[1], cj3array[1], sj3array[1];
01773 bool j3valid[1]={false};
01774 _nj3 = 1;
01775 IkReal x438=((cj4)*(cj5));
01776 IkReal x439=((r11)*(sj6));
01777 IkReal x440=((cj6)*(r10));
01778 if( IKabs(((((r10)*(sj4)*(sj6)))+(((cj6)*(r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x438)*(x440)))+(((x438)*(x439)))+(((cj4)*(r12)*(sj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x439)))+(((cj5)*(r12)))+(((sj5)*(x440))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r10)*(sj4)*(sj6)))+(((cj6)*(r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x438)*(x440)))+(((x438)*(x439)))+(((cj4)*(r12)*(sj5)))))+IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x439)))+(((cj5)*(r12)))+(((sj5)*(x440)))))-1) <= IKFAST_SINCOS_THRESH )
01779     continue;
01780 j3array[0]=IKatan2(((((r10)*(sj4)*(sj6)))+(((cj6)*(r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x438)*(x440)))+(((x438)*(x439)))+(((cj4)*(r12)*(sj5)))), ((((IkReal(-1.00000000000000))*(sj5)*(x439)))+(((cj5)*(r12)))+(((sj5)*(x440)))));
01781 sj3array[0]=IKsin(j3array[0]);
01782 cj3array[0]=IKcos(j3array[0]);
01783 if( j3array[0] > IKPI )
01784 {
01785     j3array[0]-=IK2PI;
01786 }
01787 else if( j3array[0] < -IKPI )
01788 {    j3array[0]+=IK2PI;
01789 }
01790 j3valid[0] = true;
01791 for(int ij3 = 0; ij3 < 1; ++ij3)
01792 {
01793 if( !j3valid[ij3] )
01794 {
01795     continue;
01796 }
01797 _ij3[0] = ij3; _ij3[1] = -1;
01798 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01799 {
01800 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01801 {
01802     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01803 }
01804 }
01805 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01806 {
01807 IkReal evalcond[6];
01808 IkReal x441=IKsin(j3);
01809 IkReal x442=IKcos(j3);
01810 IkReal x443=((r11)*(sj6));
01811 IkReal x444=((IkReal(1.00000000000000))*(sj5));
01812 IkReal x445=((cj6)*(sj4));
01813 IkReal x446=((sj4)*(sj6));
01814 IkReal x447=((cj4)*(sj5));
01815 IkReal x448=((cj4)*(cj5));
01816 IkReal x449=((r01)*(sj6));
01817 IkReal x450=((r21)*(sj6));
01818 IkReal x451=((cj6)*(sj5));
01819 IkReal x452=((IkReal(1.00000000000000))*(x442));
01820 IkReal x453=((IkReal(1.00000000000000))*(x441));
01821 IkReal x454=((IkReal(1.00000000000000))*(cj6)*(x448));
01822 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x451)))+(((sj2)*(x441)))+(((IkReal(-1.00000000000000))*(x444)*(x450))));
01823 evalcond[1]=((((IkReal(-1.00000000000000))*(x444)*(x449)))+(((r00)*(x451)))+(((IkReal(-1.00000000000000))*(cj2)*(x453)))+(((cj5)*(r02))));
01824 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x443)*(x444)))+(((IkReal(-1.00000000000000))*(x452)))+(((r10)*(x451))));
01825 evalcond[3]=((((r21)*(x445)))+(((IkReal(-1.00000000000000))*(sj2)*(x452)))+(((r22)*(x447)))+(((x448)*(x450)))+(((IkReal(-1.00000000000000))*(r20)*(x454)))+(((r20)*(x446))));
01826 evalcond[4]=((((r02)*(x447)))+(((IkReal(-1.00000000000000))*(r00)*(x454)))+(((cj2)*(x442)))+(((r01)*(x445)))+(((r00)*(x446)))+(((x448)*(x449))));
01827 evalcond[5]=((((IkReal(-1.00000000000000))*(x453)))+(((IkReal(-1.00000000000000))*(r10)*(x454)))+(((x443)*(x448)))+(((r10)*(x446)))+(((r11)*(x445)))+(((r12)*(x447))));
01828 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01829 {
01830 continue;
01831 }
01832 }
01833 
01834 {
01835 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
01836 vinfos[0].jointtype = 1;
01837 vinfos[0].foffset = j0;
01838 vinfos[0].indices[0] = _ij0[0];
01839 vinfos[0].indices[1] = _ij0[1];
01840 vinfos[0].maxsolutions = _nj0;
01841 vinfos[1].jointtype = 1;
01842 vinfos[1].foffset = j1;
01843 vinfos[1].indices[0] = _ij1[0];
01844 vinfos[1].indices[1] = _ij1[1];
01845 vinfos[1].maxsolutions = _nj1;
01846 vinfos[2].jointtype = 1;
01847 vinfos[2].foffset = j2;
01848 vinfos[2].indices[0] = _ij2[0];
01849 vinfos[2].indices[1] = _ij2[1];
01850 vinfos[2].maxsolutions = _nj2;
01851 vinfos[3].jointtype = 1;
01852 vinfos[3].foffset = j3;
01853 vinfos[3].indices[0] = _ij3[0];
01854 vinfos[3].indices[1] = _ij3[1];
01855 vinfos[3].maxsolutions = _nj3;
01856 vinfos[4].jointtype = 1;
01857 vinfos[4].foffset = j4;
01858 vinfos[4].indices[0] = _ij4[0];
01859 vinfos[4].indices[1] = _ij4[1];
01860 vinfos[4].maxsolutions = _nj4;
01861 vinfos[5].jointtype = 1;
01862 vinfos[5].foffset = j5;
01863 vinfos[5].indices[0] = _ij5[0];
01864 vinfos[5].indices[1] = _ij5[1];
01865 vinfos[5].maxsolutions = _nj5;
01866 vinfos[6].jointtype = 1;
01867 vinfos[6].foffset = j6;
01868 vinfos[6].indices[0] = _ij6[0];
01869 vinfos[6].indices[1] = _ij6[1];
01870 vinfos[6].maxsolutions = _nj6;
01871 std::vector<int> vfree(0);
01872 solutions.AddSolution(vinfos,vfree);
01873 }
01874 }
01875 }
01876 }
01877 }
01878 
01879 }
01880 
01881 }
01882 
01883 } else
01884 {
01885 {
01886 IkReal j4array[1], cj4array[1], sj4array[1];
01887 bool j4valid[1]={false};
01888 _nj4 = 1;
01889 IkReal x455=((cj2)*(cj6));
01890 IkReal x456=((IkReal(1.00000000000000))*(r11));
01891 IkReal x457=((IkReal(1.00000000000000))*(cj2));
01892 if( IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x457))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst33)*(((((cj5)*(r10)*(x455)))+(((IkReal(-1.00000000000000))*(cj2)*(cj5)*(sj6)*(x456)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x457))))))) < IKFAST_ATAN2_MAGTHRESH )
01893     continue;
01894 j4array[0]=IKatan2(((gconst33)*(((((IkReal(-1.00000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x457)))))), ((gconst33)*(((((cj5)*(r10)*(x455)))+(((IkReal(-1.00000000000000))*(cj2)*(cj5)*(sj6)*(x456)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x457)))))));
01895 sj4array[0]=IKsin(j4array[0]);
01896 cj4array[0]=IKcos(j4array[0]);
01897 if( j4array[0] > IKPI )
01898 {
01899     j4array[0]-=IK2PI;
01900 }
01901 else if( j4array[0] < -IKPI )
01902 {    j4array[0]+=IK2PI;
01903 }
01904 j4valid[0] = true;
01905 for(int ij4 = 0; ij4 < 1; ++ij4)
01906 {
01907 if( !j4valid[ij4] )
01908 {
01909     continue;
01910 }
01911 _ij4[0] = ij4; _ij4[1] = -1;
01912 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01913 {
01914 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01915 {
01916     j4valid[iij4]=false; _ij4[1] = iij4; break; 
01917 }
01918 }
01919 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01920 {
01921 IkReal evalcond[3];
01922 IkReal x458=IKsin(j4);
01923 IkReal x459=IKcos(j4);
01924 IkReal x460=((IkReal(1.00000000000000))*(cj6));
01925 IkReal x461=((sj5)*(x458));
01926 IkReal x462=((cj5)*(x458));
01927 IkReal x463=((IkReal(1.00000000000000))*(sj6)*(x459));
01928 evalcond[0]=((((r22)*(x461)))+(((r21)*(sj6)*(x462)))+(((IkReal(-1.00000000000000))*(r20)*(x460)*(x462)))+(((IkReal(-1.00000000000000))*(r20)*(x463)))+(((IkReal(-1.00000000000000))*(r21)*(x459)*(x460)))+(((IkReal(-1.00000000000000))*(cj2))));
01929 evalcond[1]=((((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(r00)*(x463)))+(((r01)*(sj6)*(x462)))+(((IkReal(-1.00000000000000))*(r00)*(x460)*(x462)))+(((r02)*(x461)))+(((IkReal(-1.00000000000000))*(r01)*(x459)*(x460))));
01930 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x459)*(x460)))+(((r11)*(sj6)*(x462)))+(((r12)*(x461)))+(((IkReal(-1.00000000000000))*(r10)*(x463)))+(((IkReal(-1.00000000000000))*(r10)*(x460)*(x462))));
01931 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01932 {
01933 continue;
01934 }
01935 }
01936 
01937 {
01938 IkReal j3array[1], cj3array[1], sj3array[1];
01939 bool j3valid[1]={false};
01940 _nj3 = 1;
01941 IkReal x464=((cj4)*(cj5));
01942 IkReal x465=((r11)*(sj6));
01943 IkReal x466=((cj6)*(r10));
01944 if( IKabs(((((r10)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x464)*(x466)))+(((cj6)*(r11)*(sj4)))+(((x464)*(x465)))+(((cj4)*(r12)*(sj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj5)*(x465)))+(((sj5)*(x466))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r10)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x464)*(x466)))+(((cj6)*(r11)*(sj4)))+(((x464)*(x465)))+(((cj4)*(r12)*(sj5)))))+IKsqr(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj5)*(x465)))+(((sj5)*(x466)))))-1) <= IKFAST_SINCOS_THRESH )
01945     continue;
01946 j3array[0]=IKatan2(((((r10)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x464)*(x466)))+(((cj6)*(r11)*(sj4)))+(((x464)*(x465)))+(((cj4)*(r12)*(sj5)))), ((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj5)*(x465)))+(((sj5)*(x466)))));
01947 sj3array[0]=IKsin(j3array[0]);
01948 cj3array[0]=IKcos(j3array[0]);
01949 if( j3array[0] > IKPI )
01950 {
01951     j3array[0]-=IK2PI;
01952 }
01953 else if( j3array[0] < -IKPI )
01954 {    j3array[0]+=IK2PI;
01955 }
01956 j3valid[0] = true;
01957 for(int ij3 = 0; ij3 < 1; ++ij3)
01958 {
01959 if( !j3valid[ij3] )
01960 {
01961     continue;
01962 }
01963 _ij3[0] = ij3; _ij3[1] = -1;
01964 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01965 {
01966 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01967 {
01968     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01969 }
01970 }
01971 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01972 {
01973 IkReal evalcond[6];
01974 IkReal x467=IKsin(j3);
01975 IkReal x468=IKcos(j3);
01976 IkReal x469=((r11)*(sj6));
01977 IkReal x470=((IkReal(1.00000000000000))*(sj5));
01978 IkReal x471=((cj6)*(sj4));
01979 IkReal x472=((sj4)*(sj6));
01980 IkReal x473=((cj4)*(sj5));
01981 IkReal x474=((cj4)*(cj5));
01982 IkReal x475=((r01)*(sj6));
01983 IkReal x476=((r21)*(sj6));
01984 IkReal x477=((cj6)*(sj5));
01985 IkReal x478=((IkReal(1.00000000000000))*(x468));
01986 IkReal x479=((IkReal(1.00000000000000))*(x467));
01987 IkReal x480=((IkReal(1.00000000000000))*(cj6)*(x474));
01988 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x477)))+(((IkReal(-1.00000000000000))*(x470)*(x476)))+(((sj2)*(x467))));
01989 evalcond[1]=((((IkReal(-1.00000000000000))*(x470)*(x475)))+(((IkReal(-1.00000000000000))*(cj2)*(x479)))+(((r00)*(x477)))+(((cj5)*(r02))));
01990 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x477)))+(((IkReal(-1.00000000000000))*(x469)*(x470)))+(((IkReal(-1.00000000000000))*(x478))));
01991 evalcond[3]=((((r21)*(x471)))+(((r20)*(x472)))+(((IkReal(-1.00000000000000))*(sj2)*(x478)))+(((x474)*(x476)))+(((r22)*(x473)))+(((IkReal(-1.00000000000000))*(r20)*(x480))));
01992 evalcond[4]=((((cj2)*(x468)))+(((r02)*(x473)))+(((x474)*(x475)))+(((r00)*(x472)))+(((r01)*(x471)))+(((IkReal(-1.00000000000000))*(r00)*(x480))));
01993 evalcond[5]=((((r10)*(x472)))+(((x469)*(x474)))+(((IkReal(-1.00000000000000))*(x479)))+(((r11)*(x471)))+(((r12)*(x473)))+(((IkReal(-1.00000000000000))*(r10)*(x480))));
01994 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
01995 {
01996 continue;
01997 }
01998 }
01999 
02000 {
02001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02002 vinfos[0].jointtype = 1;
02003 vinfos[0].foffset = j0;
02004 vinfos[0].indices[0] = _ij0[0];
02005 vinfos[0].indices[1] = _ij0[1];
02006 vinfos[0].maxsolutions = _nj0;
02007 vinfos[1].jointtype = 1;
02008 vinfos[1].foffset = j1;
02009 vinfos[1].indices[0] = _ij1[0];
02010 vinfos[1].indices[1] = _ij1[1];
02011 vinfos[1].maxsolutions = _nj1;
02012 vinfos[2].jointtype = 1;
02013 vinfos[2].foffset = j2;
02014 vinfos[2].indices[0] = _ij2[0];
02015 vinfos[2].indices[1] = _ij2[1];
02016 vinfos[2].maxsolutions = _nj2;
02017 vinfos[3].jointtype = 1;
02018 vinfos[3].foffset = j3;
02019 vinfos[3].indices[0] = _ij3[0];
02020 vinfos[3].indices[1] = _ij3[1];
02021 vinfos[3].maxsolutions = _nj3;
02022 vinfos[4].jointtype = 1;
02023 vinfos[4].foffset = j4;
02024 vinfos[4].indices[0] = _ij4[0];
02025 vinfos[4].indices[1] = _ij4[1];
02026 vinfos[4].maxsolutions = _nj4;
02027 vinfos[5].jointtype = 1;
02028 vinfos[5].foffset = j5;
02029 vinfos[5].indices[0] = _ij5[0];
02030 vinfos[5].indices[1] = _ij5[1];
02031 vinfos[5].maxsolutions = _nj5;
02032 vinfos[6].jointtype = 1;
02033 vinfos[6].foffset = j6;
02034 vinfos[6].indices[0] = _ij6[0];
02035 vinfos[6].indices[1] = _ij6[1];
02036 vinfos[6].maxsolutions = _nj6;
02037 std::vector<int> vfree(0);
02038 solutions.AddSolution(vinfos,vfree);
02039 }
02040 }
02041 }
02042 }
02043 }
02044 
02045 }
02046 
02047 }
02048 }
02049 }
02050 
02051 } else
02052 {
02053 IkReal x481=((IkReal(0.390000000000000))*(sj5));
02054 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959))));
02055 evalcond[1]=((IkReal(-0.400000000000000))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x481)))+(((IkReal(0.000500000000000000))*(cj6)*(r11)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.000500000000000000))*(r10)*(sj6)))+(((cj6)*(r10)*(x481)))+(((IkReal(0.390000000000000))*(cj5)*(r12))));
02056 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  )
02057 {
02058 {
02059 IkReal j2array[1], cj2array[1], sj2array[1];
02060 bool j2valid[1]={false};
02061 _nj2 = 1;
02062 IkReal x482=((IkReal(0.250000000000000))*(cj6));
02063 IkReal x483=((IkReal(195.000000000000))*(cj5));
02064 IkReal x484=((IkReal(0.250000000000000))*(sj6));
02065 IkReal x485=((IkReal(195.000000000000))*(sj5));
02066 if( IKabs(((((r01)*(sj6)*(x485)))+(((IkReal(500.000000000000))*(px)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x485)))+(((IkReal(-1.00000000000000))*(r01)*(x482)))+(((IkReal(-1.00000000000000))*(r02)*(x483)))+(((IkReal(-1.00000000000000))*(r00)*(x484))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj6)*(r20)*(x485)))+(((r20)*(x484)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x483)))+(((r21)*(x482)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x485))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r01)*(sj6)*(x485)))+(((IkReal(500.000000000000))*(px)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x485)))+(((IkReal(-1.00000000000000))*(r01)*(x482)))+(((IkReal(-1.00000000000000))*(r02)*(x483)))+(((IkReal(-1.00000000000000))*(r00)*(x484)))))+IKsqr(((((cj6)*(r20)*(x485)))+(((r20)*(x484)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x483)))+(((r21)*(x482)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x485)))))-1) <= IKFAST_SINCOS_THRESH )
02067     continue;
02068 j2array[0]=IKatan2(((((r01)*(sj6)*(x485)))+(((IkReal(500.000000000000))*(px)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x485)))+(((IkReal(-1.00000000000000))*(r01)*(x482)))+(((IkReal(-1.00000000000000))*(r02)*(x483)))+(((IkReal(-1.00000000000000))*(r00)*(x484)))), ((((cj6)*(r20)*(x485)))+(((r20)*(x484)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x483)))+(((r21)*(x482)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x485)))));
02069 sj2array[0]=IKsin(j2array[0]);
02070 cj2array[0]=IKcos(j2array[0]);
02071 if( j2array[0] > IKPI )
02072 {
02073     j2array[0]-=IK2PI;
02074 }
02075 else if( j2array[0] < -IKPI )
02076 {    j2array[0]+=IK2PI;
02077 }
02078 j2valid[0] = true;
02079 for(int ij2 = 0; ij2 < 1; ++ij2)
02080 {
02081 if( !j2valid[ij2] )
02082 {
02083     continue;
02084 }
02085 _ij2[0] = ij2; _ij2[1] = -1;
02086 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02087 {
02088 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02089 {
02090     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02091 }
02092 }
02093 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02094 {
02095 IkReal evalcond[2];
02096 IkReal x486=((IkReal(0.000500000000000000))*(cj6));
02097 IkReal x487=((IkReal(0.390000000000000))*(sj5));
02098 IkReal x488=((IkReal(0.390000000000000))*(cj5));
02099 IkReal x489=((IkReal(0.000500000000000000))*(sj6));
02100 evalcond[0]=((((cj6)*(r20)*(x487)))+(((r20)*(x489)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x488)))+(((r21)*(x486)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x487))));
02101 evalcond[1]=((((r02)*(x488)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(0.00200000000000000))*(IKsin(j2))))+(((r01)*(x486)))+(((r00)*(x489)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x487)))+(((cj6)*(r00)*(x487))));
02102 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02103 {
02104 continue;
02105 }
02106 }
02107 
02108 {
02109 IkReal dummyeval[1];
02110 IkReal gconst45;
02111 IkReal x490=(sj6)*(sj6);
02112 IkReal x491=(cj6)*(cj6);
02113 IkReal x492=((r01)*(r10));
02114 IkReal x493=((IkReal(1.00000000000000))*(r00));
02115 IkReal x494=((r02)*(sj5));
02116 IkReal x495=((r12)*(sj5));
02117 IkReal x496=((cj5)*(x490));
02118 IkReal x497=((cj5)*(x491));
02119 gconst45=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x493)*(x495)))+(((IkReal(-1.00000000000000))*(r11)*(x493)*(x497)))+(((IkReal(-1.00000000000000))*(r11)*(x493)*(x496)))+(((r10)*(sj6)*(x494)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x495)))+(((cj6)*(r11)*(x494)))+(((x492)*(x497)))+(((x492)*(x496)))));
02120 IkReal x498=(sj6)*(sj6);
02121 IkReal x499=(cj6)*(cj6);
02122 IkReal x500=((r01)*(r10));
02123 IkReal x501=((IkReal(1.00000000000000))*(r00));
02124 IkReal x502=((r02)*(sj5));
02125 IkReal x503=((r12)*(sj5));
02126 IkReal x504=((cj5)*(x498));
02127 IkReal x505=((cj5)*(x499));
02128 dummyeval[0]=((((x500)*(x505)))+(((x500)*(x504)))+(((r10)*(sj6)*(x502)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x503)))+(((IkReal(-1.00000000000000))*(sj6)*(x501)*(x503)))+(((IkReal(-1.00000000000000))*(r11)*(x501)*(x505)))+(((IkReal(-1.00000000000000))*(r11)*(x501)*(x504)))+(((cj6)*(r11)*(x502))));
02129 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02130 {
02131 {
02132 IkReal dummyeval[1];
02133 IkReal gconst46;
02134 IkReal x506=(cj6)*(cj6);
02135 IkReal x507=(sj6)*(sj6);
02136 IkReal x508=((r11)*(r20));
02137 IkReal x509=((r12)*(sj5));
02138 IkReal x510=((IkReal(1.00000000000000))*(r10));
02139 IkReal x511=((r22)*(sj5));
02140 IkReal x512=((cj5)*(x507));
02141 IkReal x513=((cj5)*(x506));
02142 gconst46=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x510)*(x511)))+(((cj6)*(r21)*(x509)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x511)))+(((IkReal(-1.00000000000000))*(r21)*(x510)*(x512)))+(((IkReal(-1.00000000000000))*(r21)*(x510)*(x513)))+(((x508)*(x512)))+(((x508)*(x513)))+(((r20)*(sj6)*(x509)))));
02143 IkReal x514=(cj6)*(cj6);
02144 IkReal x515=(sj6)*(sj6);
02145 IkReal x516=((r11)*(r20));
02146 IkReal x517=((r12)*(sj5));
02147 IkReal x518=((IkReal(1.00000000000000))*(r10));
02148 IkReal x519=((r22)*(sj5));
02149 IkReal x520=((cj5)*(x515));
02150 IkReal x521=((cj5)*(x514));
02151 dummyeval[0]=((((IkReal(-1.00000000000000))*(r21)*(x518)*(x520)))+(((IkReal(-1.00000000000000))*(r21)*(x518)*(x521)))+(((x516)*(x520)))+(((x516)*(x521)))+(((r20)*(sj6)*(x517)))+(((IkReal(-1.00000000000000))*(sj6)*(x518)*(x519)))+(((cj6)*(r21)*(x517)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x519))));
02152 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02153 {
02154 {
02155 IkReal dummyeval[1];
02156 dummyeval[0]=sj2;
02157 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02158 {
02159 {
02160 IkReal evalcond[5];
02161 IkReal x522=((IkReal(0.000500000000000000))*(cj6));
02162 IkReal x523=((IkReal(0.390000000000000))*(sj5));
02163 IkReal x524=((cj6)*(r20));
02164 IkReal x525=((IkReal(0.000500000000000000))*(sj6));
02165 IkReal x526=((cj5)*(r22));
02166 IkReal x527=((r21)*(sj6));
02167 IkReal x528=((IkReal(0.390000000000000))*(cj5));
02168 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
02169 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x527)))+(((sj5)*(x524)))+(x526));
02170 evalcond[2]=((IkReal(-0.00200000000000000))+(((r21)*(x522)))+(((r20)*(x525)))+(((IkReal(-1.00000000000000))*(x523)*(x527)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(0.390000000000000))*(x526)))+(((x523)*(x524))));
02171 evalcond[3]=((((r02)*(x528)))+(((cj6)*(r00)*(x523)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x523)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x522)))+(((r00)*(x525))));
02172 evalcond[4]=((IkReal(-0.400000000000000))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x523)))+(((r11)*(x522)))+(((r12)*(x528)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x525)))+(((cj6)*(r10)*(x523))));
02173 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
02174 {
02175 {
02176 IkReal j3array[1], cj3array[1], sj3array[1];
02177 bool j3valid[1]={false};
02178 _nj3 = 1;
02179 IkReal x529=((IkReal(1.00000000000000))*(cj5));
02180 IkReal x530=((sj5)*(sj6));
02181 IkReal x531=((IkReal(1.00000000000000))*(cj6)*(sj5));
02182 if( IKabs(((((IkReal(-1.00000000000000))*(r00)*(x531)))+(((IkReal(-1.00000000000000))*(r02)*(x529)))+(((r01)*(x530))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r11)*(x530)))+(((IkReal(-1.00000000000000))*(r10)*(x531)))+(((IkReal(-1.00000000000000))*(r12)*(x529))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r00)*(x531)))+(((IkReal(-1.00000000000000))*(r02)*(x529)))+(((r01)*(x530)))))+IKsqr(((((r11)*(x530)))+(((IkReal(-1.00000000000000))*(r10)*(x531)))+(((IkReal(-1.00000000000000))*(r12)*(x529)))))-1) <= IKFAST_SINCOS_THRESH )
02183     continue;
02184 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r00)*(x531)))+(((IkReal(-1.00000000000000))*(r02)*(x529)))+(((r01)*(x530)))), ((((r11)*(x530)))+(((IkReal(-1.00000000000000))*(r10)*(x531)))+(((IkReal(-1.00000000000000))*(r12)*(x529)))));
02185 sj3array[0]=IKsin(j3array[0]);
02186 cj3array[0]=IKcos(j3array[0]);
02187 if( j3array[0] > IKPI )
02188 {
02189     j3array[0]-=IK2PI;
02190 }
02191 else if( j3array[0] < -IKPI )
02192 {    j3array[0]+=IK2PI;
02193 }
02194 j3valid[0] = true;
02195 for(int ij3 = 0; ij3 < 1; ++ij3)
02196 {
02197 if( !j3valid[ij3] )
02198 {
02199     continue;
02200 }
02201 _ij3[0] = ij3; _ij3[1] = -1;
02202 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02203 {
02204 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02205 {
02206     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02207 }
02208 }
02209 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02210 {
02211 IkReal evalcond[2];
02212 IkReal x532=((cj6)*(sj5));
02213 IkReal x533=((IkReal(1.00000000000000))*(sj5)*(sj6));
02214 evalcond[0]=((((IkReal(-1.00000000000000))*(r01)*(x533)))+(IKsin(j3))+(((r00)*(x532)))+(((cj5)*(r02))));
02215 evalcond[1]=((((cj5)*(r12)))+(IKcos(j3))+(((r10)*(x532)))+(((IkReal(-1.00000000000000))*(r11)*(x533))));
02216 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02217 {
02218 continue;
02219 }
02220 }
02221 
02222 {
02223 IkReal dummyeval[1];
02224 IkReal gconst51;
02225 IkReal x534=(cj6)*(cj6);
02226 IkReal x535=(sj6)*(sj6);
02227 IkReal x536=((r11)*(r20));
02228 IkReal x537=((r12)*(sj5));
02229 IkReal x538=((IkReal(1.00000000000000))*(r10));
02230 IkReal x539=((r22)*(sj5));
02231 IkReal x540=((cj5)*(x535));
02232 IkReal x541=((cj5)*(x534));
02233 gconst51=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x538)*(x539)))+(((r20)*(sj6)*(x537)))+(((IkReal(-1.00000000000000))*(r21)*(x538)*(x541)))+(((IkReal(-1.00000000000000))*(r21)*(x538)*(x540)))+(((x536)*(x541)))+(((x536)*(x540)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x539)))+(((cj6)*(r21)*(x537)))));
02234 IkReal x542=(cj6)*(cj6);
02235 IkReal x543=(sj6)*(sj6);
02236 IkReal x544=((r11)*(r20));
02237 IkReal x545=((r12)*(sj5));
02238 IkReal x546=((IkReal(1.00000000000000))*(r10));
02239 IkReal x547=((r22)*(sj5));
02240 IkReal x548=((cj5)*(x543));
02241 IkReal x549=((cj5)*(x542));
02242 dummyeval[0]=((((x544)*(x549)))+(((x544)*(x548)))+(((IkReal(-1.00000000000000))*(r21)*(x546)*(x548)))+(((IkReal(-1.00000000000000))*(r21)*(x546)*(x549)))+(((r20)*(sj6)*(x545)))+(((IkReal(-1.00000000000000))*(sj6)*(x546)*(x547)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x547)))+(((cj6)*(r21)*(x545))));
02243 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02244 {
02245 {
02246 IkReal dummyeval[1];
02247 IkReal gconst52;
02248 IkReal x550=(r20)*(r20);
02249 IkReal x551=(cj5)*(cj5);
02250 IkReal x552=(sj6)*(sj6);
02251 IkReal x553=(cj6)*(cj6);
02252 IkReal x554=(r21)*(r21);
02253 IkReal x555=((cj6)*(r20));
02254 IkReal x556=((IkReal(2.00000000000000))*(r21)*(sj6));
02255 IkReal x557=((cj5)*(r22)*(sj5));
02256 gconst52=IKsign(((((x551)*(x552)*(x554)))+(((x550)*(x551)*(x553)))+(((IkReal(-1.00000000000000))*(x551)*(x555)*(x556)))+(((x555)*(x556)))+(((x550)*(x552)))+(((x556)*(x557)))+(((x553)*(x554)))+(((IkReal(-2.00000000000000))*(x555)*(x557)))+((((r22)*(r22))*((sj5)*(sj5))))));
02257 IkReal x558=(r20)*(r20);
02258 IkReal x559=(cj5)*(cj5);
02259 IkReal x560=(sj6)*(sj6);
02260 IkReal x561=(cj6)*(cj6);
02261 IkReal x562=(r21)*(r21);
02262 IkReal x563=((cj6)*(r20));
02263 IkReal x564=((IkReal(2.00000000000000))*(r21)*(sj6));
02264 IkReal x565=((cj5)*(r22)*(sj5));
02265 dummyeval[0]=((((x558)*(x560)))+(((IkReal(-1.00000000000000))*(x559)*(x563)*(x564)))+(((x564)*(x565)))+(((x559)*(x560)*(x562)))+(((x563)*(x564)))+(((x561)*(x562)))+(((IkReal(-2.00000000000000))*(x563)*(x565)))+(((x558)*(x559)*(x561)))+((((r22)*(r22))*((sj5)*(sj5)))));
02266 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02267 {
02268 continue;
02269 
02270 } else
02271 {
02272 {
02273 IkReal j4array[1], cj4array[1], sj4array[1];
02274 bool j4valid[1]={false};
02275 _nj4 = 1;
02276 IkReal x566=((IkReal(1.00000000000000))*(r20));
02277 if( IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x566)))+(((cj5)*(r21)*(sj6)))+(((r22)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(sj6)*(x566)))+(((IkReal(-1.00000000000000))*(cj6)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH )
02278     continue;
02279 j4array[0]=IKatan2(((gconst52)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x566)))+(((cj5)*(r21)*(sj6)))+(((r22)*(sj5)))))), ((gconst52)*(((((IkReal(-1.00000000000000))*(sj6)*(x566)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)))))));
02280 sj4array[0]=IKsin(j4array[0]);
02281 cj4array[0]=IKcos(j4array[0]);
02282 if( j4array[0] > IKPI )
02283 {
02284     j4array[0]-=IK2PI;
02285 }
02286 else if( j4array[0] < -IKPI )
02287 {    j4array[0]+=IK2PI;
02288 }
02289 j4valid[0] = true;
02290 for(int ij4 = 0; ij4 < 1; ++ij4)
02291 {
02292 if( !j4valid[ij4] )
02293 {
02294     continue;
02295 }
02296 _ij4[0] = ij4; _ij4[1] = -1;
02297 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02298 {
02299 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02300 {
02301     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02302 }
02303 }
02304 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02305 {
02306 IkReal evalcond[6];
02307 IkReal x567=IKsin(j4);
02308 IkReal x568=IKcos(j4);
02309 IkReal x569=((IkReal(1.00000000000000))*(r00));
02310 IkReal x570=((cj5)*(r01));
02311 IkReal x571=((cj5)*(r11));
02312 IkReal x572=((IkReal(1.00000000000000))*(cj5));
02313 IkReal x573=((cj5)*(r21));
02314 IkReal x574=((sj5)*(x567));
02315 IkReal x575=((cj6)*(x567));
02316 IkReal x576=((sj6)*(x568));
02317 IkReal x577=((sj6)*(x567));
02318 IkReal x578=((sj5)*(x568));
02319 IkReal x579=((cj6)*(x568));
02320 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r20)*(x576)))+(((x573)*(x577)))+(((IkReal(-1.00000000000000))*(r20)*(x572)*(x575)))+(((IkReal(-1.00000000000000))*(r21)*(x579)))+(((r22)*(x574))));
02321 evalcond[1]=((((r20)*(x577)))+(((x573)*(x576)))+(((IkReal(-1.00000000000000))*(r20)*(x572)*(x579)))+(((r21)*(x575)))+(((r22)*(x578))));
02322 evalcond[2]=((((IkReal(-1.00000000000000))*(x569)*(x576)))+(((IkReal(-1.00000000000000))*(cj5)*(x569)*(x575)))+(((r02)*(x574)))+(((x570)*(x577)))+(((IkReal(-1.00000000000000))*(r01)*(x579))));
02323 evalcond[3]=((((x571)*(x577)))+(((r12)*(x574)))+(((IkReal(-1.00000000000000))*(r11)*(x579)))+(((IkReal(-1.00000000000000))*(r10)*(x572)*(x575)))+(((IkReal(-1.00000000000000))*(r10)*(x576))));
02324 evalcond[4]=((((r01)*(x575)))+(((r00)*(x577)))+(((IkReal(-1.00000000000000))*(cj5)*(x569)*(x579)))+(((r02)*(x578)))+(((IkReal(-1.00000000000000))*(cj3)))+(((x570)*(x576))));
02325 evalcond[5]=((((x571)*(x576)))+(((r12)*(x578)))+(sj3)+(((IkReal(-1.00000000000000))*(r10)*(x572)*(x579)))+(((r10)*(x577)))+(((r11)*(x575))));
02326 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02327 {
02328 continue;
02329 }
02330 }
02331 
02332 {
02333 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02334 vinfos[0].jointtype = 1;
02335 vinfos[0].foffset = j0;
02336 vinfos[0].indices[0] = _ij0[0];
02337 vinfos[0].indices[1] = _ij0[1];
02338 vinfos[0].maxsolutions = _nj0;
02339 vinfos[1].jointtype = 1;
02340 vinfos[1].foffset = j1;
02341 vinfos[1].indices[0] = _ij1[0];
02342 vinfos[1].indices[1] = _ij1[1];
02343 vinfos[1].maxsolutions = _nj1;
02344 vinfos[2].jointtype = 1;
02345 vinfos[2].foffset = j2;
02346 vinfos[2].indices[0] = _ij2[0];
02347 vinfos[2].indices[1] = _ij2[1];
02348 vinfos[2].maxsolutions = _nj2;
02349 vinfos[3].jointtype = 1;
02350 vinfos[3].foffset = j3;
02351 vinfos[3].indices[0] = _ij3[0];
02352 vinfos[3].indices[1] = _ij3[1];
02353 vinfos[3].maxsolutions = _nj3;
02354 vinfos[4].jointtype = 1;
02355 vinfos[4].foffset = j4;
02356 vinfos[4].indices[0] = _ij4[0];
02357 vinfos[4].indices[1] = _ij4[1];
02358 vinfos[4].maxsolutions = _nj4;
02359 vinfos[5].jointtype = 1;
02360 vinfos[5].foffset = j5;
02361 vinfos[5].indices[0] = _ij5[0];
02362 vinfos[5].indices[1] = _ij5[1];
02363 vinfos[5].maxsolutions = _nj5;
02364 vinfos[6].jointtype = 1;
02365 vinfos[6].foffset = j6;
02366 vinfos[6].indices[0] = _ij6[0];
02367 vinfos[6].indices[1] = _ij6[1];
02368 vinfos[6].maxsolutions = _nj6;
02369 std::vector<int> vfree(0);
02370 solutions.AddSolution(vinfos,vfree);
02371 }
02372 }
02373 }
02374 
02375 }
02376 
02377 }
02378 
02379 } else
02380 {
02381 {
02382 IkReal j4array[1], cj4array[1], sj4array[1];
02383 bool j4valid[1]={false};
02384 _nj4 = 1;
02385 IkReal x580=((IkReal(1.00000000000000))*(sj3));
02386 if( IKabs(((gconst51)*(((((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x580))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x580)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x580))))))) < IKFAST_ATAN2_MAGTHRESH )
02387     continue;
02388 j4array[0]=IKatan2(((gconst51)*(((((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x580)))))), ((gconst51)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x580)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x580)))))));
02389 sj4array[0]=IKsin(j4array[0]);
02390 cj4array[0]=IKcos(j4array[0]);
02391 if( j4array[0] > IKPI )
02392 {
02393     j4array[0]-=IK2PI;
02394 }
02395 else if( j4array[0] < -IKPI )
02396 {    j4array[0]+=IK2PI;
02397 }
02398 j4valid[0] = true;
02399 for(int ij4 = 0; ij4 < 1; ++ij4)
02400 {
02401 if( !j4valid[ij4] )
02402 {
02403     continue;
02404 }
02405 _ij4[0] = ij4; _ij4[1] = -1;
02406 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02407 {
02408 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02409 {
02410     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02411 }
02412 }
02413 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02414 {
02415 IkReal evalcond[6];
02416 IkReal x581=IKsin(j4);
02417 IkReal x582=IKcos(j4);
02418 IkReal x583=((IkReal(1.00000000000000))*(r00));
02419 IkReal x584=((cj5)*(r01));
02420 IkReal x585=((cj5)*(r11));
02421 IkReal x586=((IkReal(1.00000000000000))*(cj5));
02422 IkReal x587=((cj5)*(r21));
02423 IkReal x588=((sj5)*(x581));
02424 IkReal x589=((cj6)*(x581));
02425 IkReal x590=((sj6)*(x582));
02426 IkReal x591=((sj6)*(x581));
02427 IkReal x592=((sj5)*(x582));
02428 IkReal x593=((cj6)*(x582));
02429 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r20)*(x590)))+(((IkReal(-1.00000000000000))*(r20)*(x586)*(x589)))+(((IkReal(-1.00000000000000))*(r21)*(x593)))+(((r22)*(x588)))+(((x587)*(x591))));
02430 evalcond[1]=((((r22)*(x592)))+(((IkReal(-1.00000000000000))*(r20)*(x586)*(x593)))+(((r20)*(x591)))+(((r21)*(x589)))+(((x587)*(x590))));
02431 evalcond[2]=((((r02)*(x588)))+(((IkReal(-1.00000000000000))*(r01)*(x593)))+(((IkReal(-1.00000000000000))*(cj5)*(x583)*(x589)))+(((IkReal(-1.00000000000000))*(x583)*(x590)))+(((x584)*(x591))));
02432 evalcond[3]=((((x585)*(x591)))+(((IkReal(-1.00000000000000))*(r10)*(x590)))+(((r12)*(x588)))+(((IkReal(-1.00000000000000))*(r10)*(x586)*(x589)))+(((IkReal(-1.00000000000000))*(r11)*(x593))));
02433 evalcond[4]=((((r00)*(x591)))+(((r02)*(x592)))+(((x584)*(x590)))+(((r01)*(x589)))+(((IkReal(-1.00000000000000))*(cj5)*(x583)*(x593)))+(((IkReal(-1.00000000000000))*(cj3))));
02434 evalcond[5]=((((x585)*(x590)))+(sj3)+(((r11)*(x589)))+(((IkReal(-1.00000000000000))*(r10)*(x586)*(x593)))+(((r12)*(x592)))+(((r10)*(x591))));
02435 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02436 {
02437 continue;
02438 }
02439 }
02440 
02441 {
02442 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02443 vinfos[0].jointtype = 1;
02444 vinfos[0].foffset = j0;
02445 vinfos[0].indices[0] = _ij0[0];
02446 vinfos[0].indices[1] = _ij0[1];
02447 vinfos[0].maxsolutions = _nj0;
02448 vinfos[1].jointtype = 1;
02449 vinfos[1].foffset = j1;
02450 vinfos[1].indices[0] = _ij1[0];
02451 vinfos[1].indices[1] = _ij1[1];
02452 vinfos[1].maxsolutions = _nj1;
02453 vinfos[2].jointtype = 1;
02454 vinfos[2].foffset = j2;
02455 vinfos[2].indices[0] = _ij2[0];
02456 vinfos[2].indices[1] = _ij2[1];
02457 vinfos[2].maxsolutions = _nj2;
02458 vinfos[3].jointtype = 1;
02459 vinfos[3].foffset = j3;
02460 vinfos[3].indices[0] = _ij3[0];
02461 vinfos[3].indices[1] = _ij3[1];
02462 vinfos[3].maxsolutions = _nj3;
02463 vinfos[4].jointtype = 1;
02464 vinfos[4].foffset = j4;
02465 vinfos[4].indices[0] = _ij4[0];
02466 vinfos[4].indices[1] = _ij4[1];
02467 vinfos[4].maxsolutions = _nj4;
02468 vinfos[5].jointtype = 1;
02469 vinfos[5].foffset = j5;
02470 vinfos[5].indices[0] = _ij5[0];
02471 vinfos[5].indices[1] = _ij5[1];
02472 vinfos[5].maxsolutions = _nj5;
02473 vinfos[6].jointtype = 1;
02474 vinfos[6].foffset = j6;
02475 vinfos[6].indices[0] = _ij6[0];
02476 vinfos[6].indices[1] = _ij6[1];
02477 vinfos[6].maxsolutions = _nj6;
02478 std::vector<int> vfree(0);
02479 solutions.AddSolution(vinfos,vfree);
02480 }
02481 }
02482 }
02483 
02484 }
02485 
02486 }
02487 }
02488 }
02489 
02490 } else
02491 {
02492 IkReal x594=((IkReal(0.000500000000000000))*(cj6));
02493 IkReal x595=((IkReal(0.390000000000000))*(sj5));
02494 IkReal x596=((cj6)*(r20));
02495 IkReal x597=((IkReal(0.000500000000000000))*(sj6));
02496 IkReal x598=((cj5)*(r22));
02497 IkReal x599=((r21)*(sj6));
02498 IkReal x600=((IkReal(0.390000000000000))*(cj5));
02499 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
02500 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x599)))+(x598)+(((sj5)*(x596))));
02501 evalcond[2]=((IkReal(0.00200000000000000))+(((x595)*(x596)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x594)))+(((IkReal(-1.00000000000000))*(x595)*(x599)))+(((r20)*(x597)))+(((IkReal(0.390000000000000))*(x598))));
02502 evalcond[3]=((((r01)*(x594)))+(((cj6)*(r00)*(x595)))+(((r00)*(x597)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x595)))+(((r02)*(x600))));
02503 evalcond[4]=((IkReal(-0.400000000000000))+(((cj6)*(r10)*(x595)))+(((r12)*(x600)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x595)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x594)))+(((r10)*(x597))));
02504 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
02505 {
02506 {
02507 IkReal j3array[1], cj3array[1], sj3array[1];
02508 bool j3valid[1]={false};
02509 _nj3 = 1;
02510 IkReal x601=((IkReal(1.00000000000000))*(sj5));
02511 if( IKabs(((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x601)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x601)))+(((r11)*(sj5)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x601)))+(((cj5)*(r02)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x601)))+(((r11)*(sj5)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
02512     continue;
02513 j3array[0]=IKatan2(((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x601)))+(((cj5)*(r02)))), ((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x601)))+(((r11)*(sj5)*(sj6)))));
02514 sj3array[0]=IKsin(j3array[0]);
02515 cj3array[0]=IKcos(j3array[0]);
02516 if( j3array[0] > IKPI )
02517 {
02518     j3array[0]-=IK2PI;
02519 }
02520 else if( j3array[0] < -IKPI )
02521 {    j3array[0]+=IK2PI;
02522 }
02523 j3valid[0] = true;
02524 for(int ij3 = 0; ij3 < 1; ++ij3)
02525 {
02526 if( !j3valid[ij3] )
02527 {
02528     continue;
02529 }
02530 _ij3[0] = ij3; _ij3[1] = -1;
02531 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02532 {
02533 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02534 {
02535     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02536 }
02537 }
02538 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02539 {
02540 IkReal evalcond[2];
02541 IkReal x602=((cj6)*(sj5));
02542 IkReal x603=((IkReal(1.00000000000000))*(sj5)*(sj6));
02543 evalcond[0]=((((r00)*(x602)))+(((IkReal(-1.00000000000000))*(r01)*(x603)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((cj5)*(r02))));
02544 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x603)))+(IKcos(j3))+(((r10)*(x602))));
02545 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02546 {
02547 continue;
02548 }
02549 }
02550 
02551 {
02552 IkReal dummyeval[1];
02553 IkReal gconst55;
02554 IkReal x604=(cj6)*(cj6);
02555 IkReal x605=(sj6)*(sj6);
02556 IkReal x606=((r01)*(r20));
02557 IkReal x607=((r02)*(sj5));
02558 IkReal x608=((cj5)*(x605));
02559 IkReal x609=((IkReal(1.00000000000000))*(r22)*(sj5));
02560 IkReal x610=((IkReal(1.00000000000000))*(r00)*(r21));
02561 IkReal x611=((cj5)*(x604));
02562 gconst55=IKsign(((((IkReal(-1.00000000000000))*(x610)*(x611)))+(((r20)*(sj6)*(x607)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x609)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x609)))+(((x606)*(x608)))+(((x606)*(x611)))+(((cj6)*(r21)*(x607)))+(((IkReal(-1.00000000000000))*(x608)*(x610)))));
02563 IkReal x612=(cj6)*(cj6);
02564 IkReal x613=(sj6)*(sj6);
02565 IkReal x614=((r01)*(r20));
02566 IkReal x615=((r02)*(sj5));
02567 IkReal x616=((cj5)*(x613));
02568 IkReal x617=((IkReal(1.00000000000000))*(r22)*(sj5));
02569 IkReal x618=((IkReal(1.00000000000000))*(r00)*(r21));
02570 IkReal x619=((cj5)*(x612));
02571 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x617)))+(((IkReal(-1.00000000000000))*(x618)*(x619)))+(((r20)*(sj6)*(x615)))+(((cj6)*(r21)*(x615)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x617)))+(((x614)*(x619)))+(((x614)*(x616)))+(((IkReal(-1.00000000000000))*(x616)*(x618))));
02572 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02573 {
02574 {
02575 IkReal dummyeval[1];
02576 IkReal gconst56;
02577 IkReal x620=(cj6)*(cj6);
02578 IkReal x621=(sj6)*(sj6);
02579 IkReal x622=((r11)*(r20));
02580 IkReal x623=((r12)*(sj5));
02581 IkReal x624=((IkReal(1.00000000000000))*(r10));
02582 IkReal x625=((r22)*(sj5));
02583 IkReal x626=((cj5)*(x621));
02584 IkReal x627=((cj5)*(x620));
02585 gconst56=IKsign(((((cj6)*(r21)*(x623)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x625)))+(((IkReal(-1.00000000000000))*(r21)*(x624)*(x627)))+(((IkReal(-1.00000000000000))*(r21)*(x624)*(x626)))+(((r20)*(sj6)*(x623)))+(((IkReal(-1.00000000000000))*(sj6)*(x624)*(x625)))+(((x622)*(x627)))+(((x622)*(x626)))));
02586 IkReal x628=(cj6)*(cj6);
02587 IkReal x629=(sj6)*(sj6);
02588 IkReal x630=((r11)*(r20));
02589 IkReal x631=((r12)*(sj5));
02590 IkReal x632=((IkReal(1.00000000000000))*(r10));
02591 IkReal x633=((r22)*(sj5));
02592 IkReal x634=((cj5)*(x629));
02593 IkReal x635=((cj5)*(x628));
02594 dummyeval[0]=((((cj6)*(r21)*(x631)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x633)))+(((IkReal(-1.00000000000000))*(sj6)*(x632)*(x633)))+(((IkReal(-1.00000000000000))*(r21)*(x632)*(x634)))+(((IkReal(-1.00000000000000))*(r21)*(x632)*(x635)))+(((r20)*(sj6)*(x631)))+(((x630)*(x635)))+(((x630)*(x634))));
02595 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02596 {
02597 continue;
02598 
02599 } else
02600 {
02601 {
02602 IkReal j4array[1], cj4array[1], sj4array[1];
02603 bool j4valid[1]={false};
02604 _nj4 = 1;
02605 IkReal x636=((IkReal(1.00000000000000))*(sj3));
02606 if( IKabs(((gconst56)*(((((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x636))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst56)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x636)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x636))))))) < IKFAST_ATAN2_MAGTHRESH )
02607     continue;
02608 j4array[0]=IKatan2(((gconst56)*(((((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x636)))))), ((gconst56)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x636)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x636)))))));
02609 sj4array[0]=IKsin(j4array[0]);
02610 cj4array[0]=IKcos(j4array[0]);
02611 if( j4array[0] > IKPI )
02612 {
02613     j4array[0]-=IK2PI;
02614 }
02615 else if( j4array[0] < -IKPI )
02616 {    j4array[0]+=IK2PI;
02617 }
02618 j4valid[0] = true;
02619 for(int ij4 = 0; ij4 < 1; ++ij4)
02620 {
02621 if( !j4valid[ij4] )
02622 {
02623     continue;
02624 }
02625 _ij4[0] = ij4; _ij4[1] = -1;
02626 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02627 {
02628 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02629 {
02630     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02631 }
02632 }
02633 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02634 {
02635 IkReal evalcond[6];
02636 IkReal x637=IKsin(j4);
02637 IkReal x638=IKcos(j4);
02638 IkReal x639=((IkReal(1.00000000000000))*(r00));
02639 IkReal x640=((cj5)*(r01));
02640 IkReal x641=((cj5)*(r11));
02641 IkReal x642=((IkReal(1.00000000000000))*(cj5));
02642 IkReal x643=((cj5)*(r21));
02643 IkReal x644=((sj5)*(x637));
02644 IkReal x645=((cj6)*(x637));
02645 IkReal x646=((sj6)*(x638));
02646 IkReal x647=((sj6)*(x637));
02647 IkReal x648=((sj5)*(x638));
02648 IkReal x649=((cj6)*(x638));
02649 evalcond[0]=((IkReal(1.00000000000000))+(((x643)*(x647)))+(((IkReal(-1.00000000000000))*(r20)*(x642)*(x645)))+(((IkReal(-1.00000000000000))*(r20)*(x646)))+(((r22)*(x644)))+(((IkReal(-1.00000000000000))*(r21)*(x649))));
02650 evalcond[1]=((((r20)*(x647)))+(((x643)*(x646)))+(((IkReal(-1.00000000000000))*(r20)*(x642)*(x649)))+(((r21)*(x645)))+(((r22)*(x648))));
02651 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x639)*(x645)))+(((IkReal(-1.00000000000000))*(x639)*(x646)))+(((x640)*(x647)))+(((r02)*(x644)))+(((IkReal(-1.00000000000000))*(r01)*(x649))));
02652 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x646)))+(((x641)*(x647)))+(((r12)*(x644)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x645)))+(((IkReal(-1.00000000000000))*(r11)*(x649))));
02653 evalcond[4]=((cj3)+(((IkReal(-1.00000000000000))*(cj5)*(x639)*(x649)))+(((x640)*(x646)))+(((r02)*(x648)))+(((r01)*(x645)))+(((r00)*(x647))));
02654 evalcond[5]=((sj3)+(((x641)*(x646)))+(((r10)*(x647)))+(((r12)*(x648)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x649)))+(((r11)*(x645))));
02655 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02656 {
02657 continue;
02658 }
02659 }
02660 
02661 {
02662 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02663 vinfos[0].jointtype = 1;
02664 vinfos[0].foffset = j0;
02665 vinfos[0].indices[0] = _ij0[0];
02666 vinfos[0].indices[1] = _ij0[1];
02667 vinfos[0].maxsolutions = _nj0;
02668 vinfos[1].jointtype = 1;
02669 vinfos[1].foffset = j1;
02670 vinfos[1].indices[0] = _ij1[0];
02671 vinfos[1].indices[1] = _ij1[1];
02672 vinfos[1].maxsolutions = _nj1;
02673 vinfos[2].jointtype = 1;
02674 vinfos[2].foffset = j2;
02675 vinfos[2].indices[0] = _ij2[0];
02676 vinfos[2].indices[1] = _ij2[1];
02677 vinfos[2].maxsolutions = _nj2;
02678 vinfos[3].jointtype = 1;
02679 vinfos[3].foffset = j3;
02680 vinfos[3].indices[0] = _ij3[0];
02681 vinfos[3].indices[1] = _ij3[1];
02682 vinfos[3].maxsolutions = _nj3;
02683 vinfos[4].jointtype = 1;
02684 vinfos[4].foffset = j4;
02685 vinfos[4].indices[0] = _ij4[0];
02686 vinfos[4].indices[1] = _ij4[1];
02687 vinfos[4].maxsolutions = _nj4;
02688 vinfos[5].jointtype = 1;
02689 vinfos[5].foffset = j5;
02690 vinfos[5].indices[0] = _ij5[0];
02691 vinfos[5].indices[1] = _ij5[1];
02692 vinfos[5].maxsolutions = _nj5;
02693 vinfos[6].jointtype = 1;
02694 vinfos[6].foffset = j6;
02695 vinfos[6].indices[0] = _ij6[0];
02696 vinfos[6].indices[1] = _ij6[1];
02697 vinfos[6].maxsolutions = _nj6;
02698 std::vector<int> vfree(0);
02699 solutions.AddSolution(vinfos,vfree);
02700 }
02701 }
02702 }
02703 
02704 }
02705 
02706 }
02707 
02708 } else
02709 {
02710 {
02711 IkReal j4array[1], cj4array[1], sj4array[1];
02712 bool j4valid[1]={false};
02713 _nj4 = 1;
02714 IkReal x650=((cj3)*(r21));
02715 IkReal x651=((IkReal(1.00000000000000))*(cj6));
02716 IkReal x652=((cj3)*(r20));
02717 if( IKabs(((gconst55)*(((((cj3)*(r22)*(sj5)))+(((cj5)*(sj6)*(x650)))+(((IkReal(-1.00000000000000))*(cj5)*(x651)*(x652))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst55)*(((((IkReal(-1.00000000000000))*(sj6)*(x652)))+(((IkReal(-1.00000000000000))*(x650)*(x651))))))) < IKFAST_ATAN2_MAGTHRESH )
02718     continue;
02719 j4array[0]=IKatan2(((gconst55)*(((((cj3)*(r22)*(sj5)))+(((cj5)*(sj6)*(x650)))+(((IkReal(-1.00000000000000))*(cj5)*(x651)*(x652)))))), ((gconst55)*(((((IkReal(-1.00000000000000))*(sj6)*(x652)))+(((IkReal(-1.00000000000000))*(x650)*(x651)))))));
02720 sj4array[0]=IKsin(j4array[0]);
02721 cj4array[0]=IKcos(j4array[0]);
02722 if( j4array[0] > IKPI )
02723 {
02724     j4array[0]-=IK2PI;
02725 }
02726 else if( j4array[0] < -IKPI )
02727 {    j4array[0]+=IK2PI;
02728 }
02729 j4valid[0] = true;
02730 for(int ij4 = 0; ij4 < 1; ++ij4)
02731 {
02732 if( !j4valid[ij4] )
02733 {
02734     continue;
02735 }
02736 _ij4[0] = ij4; _ij4[1] = -1;
02737 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02738 {
02739 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02740 {
02741     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02742 }
02743 }
02744 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02745 {
02746 IkReal evalcond[6];
02747 IkReal x653=IKsin(j4);
02748 IkReal x654=IKcos(j4);
02749 IkReal x655=((IkReal(1.00000000000000))*(r00));
02750 IkReal x656=((cj5)*(r01));
02751 IkReal x657=((cj5)*(r11));
02752 IkReal x658=((IkReal(1.00000000000000))*(cj5));
02753 IkReal x659=((cj5)*(r21));
02754 IkReal x660=((sj5)*(x653));
02755 IkReal x661=((cj6)*(x653));
02756 IkReal x662=((sj6)*(x654));
02757 IkReal x663=((sj6)*(x653));
02758 IkReal x664=((sj5)*(x654));
02759 IkReal x665=((cj6)*(x654));
02760 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r21)*(x665)))+(((IkReal(-1.00000000000000))*(r20)*(x662)))+(((x659)*(x663)))+(((IkReal(-1.00000000000000))*(r20)*(x658)*(x661)))+(((r22)*(x660))));
02761 evalcond[1]=((((r20)*(x663)))+(((r21)*(x661)))+(((x659)*(x662)))+(((IkReal(-1.00000000000000))*(r20)*(x658)*(x665)))+(((r22)*(x664))));
02762 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x655)*(x661)))+(((x656)*(x663)))+(((r02)*(x660)))+(((IkReal(-1.00000000000000))*(x655)*(x662)))+(((IkReal(-1.00000000000000))*(r01)*(x665))));
02763 evalcond[3]=((((x657)*(x663)))+(((IkReal(-1.00000000000000))*(r10)*(x662)))+(((IkReal(-1.00000000000000))*(r11)*(x665)))+(((IkReal(-1.00000000000000))*(r10)*(x658)*(x661)))+(((r12)*(x660))));
02764 evalcond[4]=((((r01)*(x661)))+(((r00)*(x663)))+(cj3)+(((IkReal(-1.00000000000000))*(cj5)*(x655)*(x665)))+(((x656)*(x662)))+(((r02)*(x664))));
02765 evalcond[5]=((sj3)+(((x657)*(x662)))+(((r11)*(x661)))+(((IkReal(-1.00000000000000))*(r10)*(x658)*(x665)))+(((r12)*(x664)))+(((r10)*(x663))));
02766 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02767 {
02768 continue;
02769 }
02770 }
02771 
02772 {
02773 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02774 vinfos[0].jointtype = 1;
02775 vinfos[0].foffset = j0;
02776 vinfos[0].indices[0] = _ij0[0];
02777 vinfos[0].indices[1] = _ij0[1];
02778 vinfos[0].maxsolutions = _nj0;
02779 vinfos[1].jointtype = 1;
02780 vinfos[1].foffset = j1;
02781 vinfos[1].indices[0] = _ij1[0];
02782 vinfos[1].indices[1] = _ij1[1];
02783 vinfos[1].maxsolutions = _nj1;
02784 vinfos[2].jointtype = 1;
02785 vinfos[2].foffset = j2;
02786 vinfos[2].indices[0] = _ij2[0];
02787 vinfos[2].indices[1] = _ij2[1];
02788 vinfos[2].maxsolutions = _nj2;
02789 vinfos[3].jointtype = 1;
02790 vinfos[3].foffset = j3;
02791 vinfos[3].indices[0] = _ij3[0];
02792 vinfos[3].indices[1] = _ij3[1];
02793 vinfos[3].maxsolutions = _nj3;
02794 vinfos[4].jointtype = 1;
02795 vinfos[4].foffset = j4;
02796 vinfos[4].indices[0] = _ij4[0];
02797 vinfos[4].indices[1] = _ij4[1];
02798 vinfos[4].maxsolutions = _nj4;
02799 vinfos[5].jointtype = 1;
02800 vinfos[5].foffset = j5;
02801 vinfos[5].indices[0] = _ij5[0];
02802 vinfos[5].indices[1] = _ij5[1];
02803 vinfos[5].maxsolutions = _nj5;
02804 vinfos[6].jointtype = 1;
02805 vinfos[6].foffset = j6;
02806 vinfos[6].indices[0] = _ij6[0];
02807 vinfos[6].indices[1] = _ij6[1];
02808 vinfos[6].maxsolutions = _nj6;
02809 std::vector<int> vfree(0);
02810 solutions.AddSolution(vinfos,vfree);
02811 }
02812 }
02813 }
02814 
02815 }
02816 
02817 }
02818 }
02819 }
02820 
02821 } else
02822 {
02823 if( 1 )
02824 {
02825 continue;
02826 
02827 } else
02828 {
02829 }
02830 }
02831 }
02832 }
02833 
02834 } else
02835 {
02836 {
02837 IkReal j3array[1], cj3array[1], sj3array[1];
02838 bool j3valid[1]={false};
02839 _nj3 = 1;
02840 IkReal x666=((IkReal(1.00000000000000))*(cj5));
02841 IkReal x667=((sj5)*(sj6));
02842 IkReal x668=((IkReal(1.00000000000000))*(cj6)*(sj5));
02843 if( IKabs(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r20)*(x668)))+(((IkReal(-1.00000000000000))*(r22)*(x666)))+(((r21)*(x667))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r10)*(x668)))+(((r11)*(x667)))+(((IkReal(-1.00000000000000))*(r12)*(x666))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r20)*(x668)))+(((IkReal(-1.00000000000000))*(r22)*(x666)))+(((r21)*(x667)))))))+IKsqr(((((IkReal(-1.00000000000000))*(r10)*(x668)))+(((r11)*(x667)))+(((IkReal(-1.00000000000000))*(r12)*(x666)))))-1) <= IKFAST_SINCOS_THRESH )
02844     continue;
02845 j3array[0]=IKatan2(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r20)*(x668)))+(((IkReal(-1.00000000000000))*(r22)*(x666)))+(((r21)*(x667)))))), ((((IkReal(-1.00000000000000))*(r10)*(x668)))+(((r11)*(x667)))+(((IkReal(-1.00000000000000))*(r12)*(x666)))));
02846 sj3array[0]=IKsin(j3array[0]);
02847 cj3array[0]=IKcos(j3array[0]);
02848 if( j3array[0] > IKPI )
02849 {
02850     j3array[0]-=IK2PI;
02851 }
02852 else if( j3array[0] < -IKPI )
02853 {    j3array[0]+=IK2PI;
02854 }
02855 j3valid[0] = true;
02856 for(int ij3 = 0; ij3 < 1; ++ij3)
02857 {
02858 if( !j3valid[ij3] )
02859 {
02860     continue;
02861 }
02862 _ij3[0] = ij3; _ij3[1] = -1;
02863 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02864 {
02865 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02866 {
02867     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02868 }
02869 }
02870 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02871 {
02872 IkReal evalcond[3];
02873 IkReal x669=IKsin(j3);
02874 IkReal x670=((cj6)*(sj5));
02875 IkReal x671=((IkReal(1.00000000000000))*(sj5)*(sj6));
02876 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x671)))+(((r20)*(x670)))+(((sj2)*(x669))));
02877 evalcond[1]=((((cj2)*(x669)))+(((r00)*(x670)))+(((IkReal(-1.00000000000000))*(r01)*(x671)))+(((cj5)*(r02))));
02878 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x670)))+(IKcos(j3))+(((IkReal(-1.00000000000000))*(r11)*(x671))));
02879 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
02880 {
02881 continue;
02882 }
02883 }
02884 
02885 {
02886 IkReal dummyeval[1];
02887 IkReal gconst47;
02888 IkReal x672=(r11)*(r11);
02889 IkReal x673=(cj5)*(cj5);
02890 IkReal x674=(cj6)*(cj6);
02891 IkReal x675=(r10)*(r10);
02892 IkReal x676=(sj6)*(sj6);
02893 IkReal x677=((IkReal(2.00000000000000))*(cj5)*(r12)*(sj5));
02894 IkReal x678=((IkReal(2.00000000000000))*(cj6)*(r10)*(r11)*(sj6));
02895 gconst47=IKsign(((((x673)*(x674)*(x675)))+(((x672)*(x674)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x677)))+(((r11)*(sj6)*(x677)))+(((x672)*(x673)*(x676)))+((((r12)*(r12))*((sj5)*(sj5))))+(x678)+(((x675)*(x676)))+(((IkReal(-1.00000000000000))*(x673)*(x678)))));
02896 IkReal x679=(r11)*(r11);
02897 IkReal x680=(cj5)*(cj5);
02898 IkReal x681=(cj6)*(cj6);
02899 IkReal x682=(r10)*(r10);
02900 IkReal x683=(sj6)*(sj6);
02901 IkReal x684=x677;
02902 IkReal x685=x678;
02903 dummyeval[0]=((((IkReal(-1.00000000000000))*(x680)*(x685)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x684)))+(((x682)*(x683)))+(((x679)*(x681)))+((((r12)*(r12))*((sj5)*(sj5))))+(((r11)*(sj6)*(x684)))+(x685)+(((x679)*(x680)*(x683)))+(((x680)*(x681)*(x682))));
02904 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02905 {
02906 {
02907 IkReal dummyeval[1];
02908 IkReal gconst48;
02909 IkReal x686=(cj5)*(cj5);
02910 IkReal x687=(sj6)*(sj6);
02911 IkReal x688=(cj6)*(cj6);
02912 IkReal x689=((IkReal(1.00000000000000))*(r00));
02913 IkReal x690=((IkReal(1.00000000000000))*(r12));
02914 IkReal x691=((cj6)*(sj6));
02915 IkReal x692=((IkReal(1.00000000000000))*(r01));
02916 IkReal x693=((cj5)*(sj5));
02917 IkReal x694=((r10)*(x686));
02918 IkReal x695=((r11)*(x686));
02919 gconst48=IKsign(((((IkReal(-1.00000000000000))*(r11)*(x689)*(x691)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x690)*(x693)))+(((IkReal(-1.00000000000000))*(r10)*(x691)*(x692)))+(((IkReal(-1.00000000000000))*(r10)*(x687)*(x689)))+(((cj6)*(r00)*(r12)*(x693)))+(((r01)*(x691)*(x694)))+(((IkReal(-1.00000000000000))*(r11)*(x688)*(x692)))+(((IkReal(-1.00000000000000))*(r02)*(x690)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(sj6)*(x693)))+(((cj6)*(r02)*(r10)*(x693)))+(((IkReal(-1.00000000000000))*(x687)*(x692)*(x695)))+(((IkReal(-1.00000000000000))*(x688)*(x689)*(x694)))+(((r00)*(x691)*(x695)))));
02920 IkReal x696=(cj5)*(cj5);
02921 IkReal x697=(sj6)*(sj6);
02922 IkReal x698=(cj6)*(cj6);
02923 IkReal x699=((IkReal(1.00000000000000))*(r00));
02924 IkReal x700=((IkReal(1.00000000000000))*(r12));
02925 IkReal x701=((cj6)*(sj6));
02926 IkReal x702=((IkReal(1.00000000000000))*(r01));
02927 IkReal x703=((cj5)*(sj5));
02928 IkReal x704=((r10)*(x696));
02929 IkReal x705=((r11)*(x696));
02930 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(x697)*(x699)))+(((IkReal(-1.00000000000000))*(r10)*(x701)*(x702)))+(((IkReal(-1.00000000000000))*(x697)*(x702)*(x705)))+(((cj6)*(r00)*(r12)*(x703)))+(((IkReal(-1.00000000000000))*(r11)*(x699)*(x701)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(sj6)*(x703)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x700)*(x703)))+(((cj6)*(r02)*(r10)*(x703)))+(((r01)*(x701)*(x704)))+(((IkReal(-1.00000000000000))*(r11)*(x698)*(x702)))+(((IkReal(-1.00000000000000))*(r02)*(x700)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x698)*(x699)*(x704)))+(((r00)*(x701)*(x705))));
02931 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02932 {
02933 continue;
02934 
02935 } else
02936 {
02937 {
02938 IkReal j4array[1], cj4array[1], sj4array[1];
02939 bool j4valid[1]={false};
02940 _nj4 = 1;
02941 IkReal x706=((r00)*(sj3));
02942 IkReal x707=((cj5)*(cj6));
02943 IkReal x708=((r01)*(sj3));
02944 IkReal x709=((cj5)*(sj6));
02945 IkReal x710=((r11)*(sj2));
02946 IkReal x711=((IkReal(1.00000000000000))*(r10)*(sj2));
02947 if( IKabs(((gconst48)*(((((IkReal(-1.00000000000000))*(x707)*(x711)))+(((r12)*(sj2)*(sj5)))+(((x709)*(x710)))+(((cj6)*(x708)))+(((sj6)*(x706))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((IkReal(-1.00000000000000))*(x706)*(x707)))+(((r02)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(x710)))+(((IkReal(-1.00000000000000))*(sj6)*(x711)))+(((x708)*(x709))))))) < IKFAST_ATAN2_MAGTHRESH )
02948     continue;
02949 j4array[0]=IKatan2(((gconst48)*(((((IkReal(-1.00000000000000))*(x707)*(x711)))+(((r12)*(sj2)*(sj5)))+(((x709)*(x710)))+(((cj6)*(x708)))+(((sj6)*(x706)))))), ((gconst48)*(((((IkReal(-1.00000000000000))*(x706)*(x707)))+(((r02)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(x710)))+(((IkReal(-1.00000000000000))*(sj6)*(x711)))+(((x708)*(x709)))))));
02950 sj4array[0]=IKsin(j4array[0]);
02951 cj4array[0]=IKcos(j4array[0]);
02952 if( j4array[0] > IKPI )
02953 {
02954     j4array[0]-=IK2PI;
02955 }
02956 else if( j4array[0] < -IKPI )
02957 {    j4array[0]+=IK2PI;
02958 }
02959 j4valid[0] = true;
02960 for(int ij4 = 0; ij4 < 1; ++ij4)
02961 {
02962 if( !j4valid[ij4] )
02963 {
02964     continue;
02965 }
02966 _ij4[0] = ij4; _ij4[1] = -1;
02967 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02968 {
02969 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02970 {
02971     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02972 }
02973 }
02974 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02975 {
02976 IkReal evalcond[6];
02977 IkReal x712=IKsin(j4);
02978 IkReal x713=IKcos(j4);
02979 IkReal x714=((IkReal(1.00000000000000))*(r00));
02980 IkReal x715=((cj5)*(r01));
02981 IkReal x716=((cj5)*(r11));
02982 IkReal x717=((IkReal(1.00000000000000))*(cj3));
02983 IkReal x718=((IkReal(1.00000000000000))*(cj5));
02984 IkReal x719=((cj5)*(r21));
02985 IkReal x720=((sj5)*(x712));
02986 IkReal x721=((cj6)*(x712));
02987 IkReal x722=((sj6)*(x713));
02988 IkReal x723=((sj6)*(x712));
02989 IkReal x724=((sj5)*(x713));
02990 IkReal x725=((cj6)*(x713));
02991 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x725)))+(((IkReal(-1.00000000000000))*(r20)*(x722)))+(((x719)*(x723)))+(((IkReal(-1.00000000000000))*(r20)*(x718)*(x721)))+(((r22)*(x720)))+(((IkReal(-1.00000000000000))*(cj2))));
02992 evalcond[1]=((((r20)*(x723)))+(((x719)*(x722)))+(((r21)*(x721)))+(((IkReal(-1.00000000000000))*(r20)*(x718)*(x725)))+(((r22)*(x724)))+(((IkReal(-1.00000000000000))*(sj2)*(x717))));
02993 evalcond[2]=((sj2)+(((IkReal(-1.00000000000000))*(cj5)*(x714)*(x721)))+(((IkReal(-1.00000000000000))*(r01)*(x725)))+(((r02)*(x720)))+(((IkReal(-1.00000000000000))*(x714)*(x722)))+(((x715)*(x723))));
02994 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x725)))+(((IkReal(-1.00000000000000))*(r10)*(x718)*(x721)))+(((IkReal(-1.00000000000000))*(r10)*(x722)))+(((x716)*(x723)))+(((r12)*(x720))));
02995 evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x714)*(x725)))+(((r01)*(x721)))+(((IkReal(-1.00000000000000))*(cj2)*(x717)))+(((r00)*(x723)))+(((r02)*(x724)))+(((x715)*(x722))));
02996 evalcond[5]=((((r10)*(x723)))+(sj3)+(((IkReal(-1.00000000000000))*(r10)*(x718)*(x725)))+(((r11)*(x721)))+(((x716)*(x722)))+(((r12)*(x724))));
02997 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02998 {
02999 continue;
03000 }
03001 }
03002 
03003 {
03004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03005 vinfos[0].jointtype = 1;
03006 vinfos[0].foffset = j0;
03007 vinfos[0].indices[0] = _ij0[0];
03008 vinfos[0].indices[1] = _ij0[1];
03009 vinfos[0].maxsolutions = _nj0;
03010 vinfos[1].jointtype = 1;
03011 vinfos[1].foffset = j1;
03012 vinfos[1].indices[0] = _ij1[0];
03013 vinfos[1].indices[1] = _ij1[1];
03014 vinfos[1].maxsolutions = _nj1;
03015 vinfos[2].jointtype = 1;
03016 vinfos[2].foffset = j2;
03017 vinfos[2].indices[0] = _ij2[0];
03018 vinfos[2].indices[1] = _ij2[1];
03019 vinfos[2].maxsolutions = _nj2;
03020 vinfos[3].jointtype = 1;
03021 vinfos[3].foffset = j3;
03022 vinfos[3].indices[0] = _ij3[0];
03023 vinfos[3].indices[1] = _ij3[1];
03024 vinfos[3].maxsolutions = _nj3;
03025 vinfos[4].jointtype = 1;
03026 vinfos[4].foffset = j4;
03027 vinfos[4].indices[0] = _ij4[0];
03028 vinfos[4].indices[1] = _ij4[1];
03029 vinfos[4].maxsolutions = _nj4;
03030 vinfos[5].jointtype = 1;
03031 vinfos[5].foffset = j5;
03032 vinfos[5].indices[0] = _ij5[0];
03033 vinfos[5].indices[1] = _ij5[1];
03034 vinfos[5].maxsolutions = _nj5;
03035 vinfos[6].jointtype = 1;
03036 vinfos[6].foffset = j6;
03037 vinfos[6].indices[0] = _ij6[0];
03038 vinfos[6].indices[1] = _ij6[1];
03039 vinfos[6].maxsolutions = _nj6;
03040 std::vector<int> vfree(0);
03041 solutions.AddSolution(vinfos,vfree);
03042 }
03043 }
03044 }
03045 
03046 }
03047 
03048 }
03049 
03050 } else
03051 {
03052 {
03053 IkReal j4array[1], cj4array[1], sj4array[1];
03054 bool j4valid[1]={false};
03055 _nj4 = 1;
03056 IkReal x726=((IkReal(1.00000000000000))*(sj3));
03057 if( IKabs(((gconst47)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x726)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x726))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst47)*(((((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj6)*(x726)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x726)))+(((cj5)*(cj6)*(r10)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
03058     continue;
03059 j4array[0]=IKatan2(((gconst47)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x726)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x726)))))), ((gconst47)*(((((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj6)*(x726)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x726)))+(((cj5)*(cj6)*(r10)*(sj3)))))));
03060 sj4array[0]=IKsin(j4array[0]);
03061 cj4array[0]=IKcos(j4array[0]);
03062 if( j4array[0] > IKPI )
03063 {
03064     j4array[0]-=IK2PI;
03065 }
03066 else if( j4array[0] < -IKPI )
03067 {    j4array[0]+=IK2PI;
03068 }
03069 j4valid[0] = true;
03070 for(int ij4 = 0; ij4 < 1; ++ij4)
03071 {
03072 if( !j4valid[ij4] )
03073 {
03074     continue;
03075 }
03076 _ij4[0] = ij4; _ij4[1] = -1;
03077 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03078 {
03079 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03080 {
03081     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03082 }
03083 }
03084 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03085 {
03086 IkReal evalcond[6];
03087 IkReal x727=IKsin(j4);
03088 IkReal x728=IKcos(j4);
03089 IkReal x729=((IkReal(1.00000000000000))*(r00));
03090 IkReal x730=((cj5)*(r01));
03091 IkReal x731=((cj5)*(r11));
03092 IkReal x732=((IkReal(1.00000000000000))*(cj3));
03093 IkReal x733=((IkReal(1.00000000000000))*(cj5));
03094 IkReal x734=((cj5)*(r21));
03095 IkReal x735=((sj5)*(x727));
03096 IkReal x736=((cj6)*(x727));
03097 IkReal x737=((sj6)*(x728));
03098 IkReal x738=((sj6)*(x727));
03099 IkReal x739=((sj5)*(x728));
03100 IkReal x740=((cj6)*(x728));
03101 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x740)))+(((IkReal(-1.00000000000000))*(r20)*(x733)*(x736)))+(((IkReal(-1.00000000000000))*(r20)*(x737)))+(((x734)*(x738)))+(((r22)*(x735)))+(((IkReal(-1.00000000000000))*(cj2))));
03102 evalcond[1]=((((x734)*(x737)))+(((r20)*(x738)))+(((IkReal(-1.00000000000000))*(sj2)*(x732)))+(((r22)*(x739)))+(((r21)*(x736)))+(((IkReal(-1.00000000000000))*(r20)*(x733)*(x740))));
03103 evalcond[2]=((sj2)+(((IkReal(-1.00000000000000))*(cj5)*(x729)*(x736)))+(((IkReal(-1.00000000000000))*(r01)*(x740)))+(((r02)*(x735)))+(((IkReal(-1.00000000000000))*(x729)*(x737)))+(((x730)*(x738))));
03104 evalcond[3]=((((r12)*(x735)))+(((IkReal(-1.00000000000000))*(r10)*(x733)*(x736)))+(((x731)*(x738)))+(((IkReal(-1.00000000000000))*(r10)*(x737)))+(((IkReal(-1.00000000000000))*(r11)*(x740))));
03105 evalcond[4]=((((IkReal(-1.00000000000000))*(cj2)*(x732)))+(((r00)*(x738)))+(((r01)*(x736)))+(((r02)*(x739)))+(((x730)*(x737)))+(((IkReal(-1.00000000000000))*(cj5)*(x729)*(x740))));
03106 evalcond[5]=((((r12)*(x739)))+(((r10)*(x738)))+(((x731)*(x737)))+(sj3)+(((IkReal(-1.00000000000000))*(r10)*(x733)*(x740)))+(((r11)*(x736))));
03107 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03108 {
03109 continue;
03110 }
03111 }
03112 
03113 {
03114 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03115 vinfos[0].jointtype = 1;
03116 vinfos[0].foffset = j0;
03117 vinfos[0].indices[0] = _ij0[0];
03118 vinfos[0].indices[1] = _ij0[1];
03119 vinfos[0].maxsolutions = _nj0;
03120 vinfos[1].jointtype = 1;
03121 vinfos[1].foffset = j1;
03122 vinfos[1].indices[0] = _ij1[0];
03123 vinfos[1].indices[1] = _ij1[1];
03124 vinfos[1].maxsolutions = _nj1;
03125 vinfos[2].jointtype = 1;
03126 vinfos[2].foffset = j2;
03127 vinfos[2].indices[0] = _ij2[0];
03128 vinfos[2].indices[1] = _ij2[1];
03129 vinfos[2].maxsolutions = _nj2;
03130 vinfos[3].jointtype = 1;
03131 vinfos[3].foffset = j3;
03132 vinfos[3].indices[0] = _ij3[0];
03133 vinfos[3].indices[1] = _ij3[1];
03134 vinfos[3].maxsolutions = _nj3;
03135 vinfos[4].jointtype = 1;
03136 vinfos[4].foffset = j4;
03137 vinfos[4].indices[0] = _ij4[0];
03138 vinfos[4].indices[1] = _ij4[1];
03139 vinfos[4].maxsolutions = _nj4;
03140 vinfos[5].jointtype = 1;
03141 vinfos[5].foffset = j5;
03142 vinfos[5].indices[0] = _ij5[0];
03143 vinfos[5].indices[1] = _ij5[1];
03144 vinfos[5].maxsolutions = _nj5;
03145 vinfos[6].jointtype = 1;
03146 vinfos[6].foffset = j6;
03147 vinfos[6].indices[0] = _ij6[0];
03148 vinfos[6].indices[1] = _ij6[1];
03149 vinfos[6].maxsolutions = _nj6;
03150 std::vector<int> vfree(0);
03151 solutions.AddSolution(vinfos,vfree);
03152 }
03153 }
03154 }
03155 
03156 }
03157 
03158 }
03159 }
03160 }
03161 
03162 }
03163 
03164 }
03165 
03166 } else
03167 {
03168 {
03169 IkReal j4array[1], cj4array[1], sj4array[1];
03170 bool j4valid[1]={false};
03171 _nj4 = 1;
03172 IkReal x741=((cj2)*(cj6));
03173 IkReal x742=((IkReal(1.00000000000000))*(r11));
03174 IkReal x743=((IkReal(1.00000000000000))*(cj2));
03175 if( IKabs(((gconst46)*(((((IkReal(-1.00000000000000))*(x741)*(x742)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x743))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((cj5)*(r10)*(x741)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x743)))+(((IkReal(-1.00000000000000))*(cj2)*(cj5)*(sj6)*(x742))))))) < IKFAST_ATAN2_MAGTHRESH )
03176     continue;
03177 j4array[0]=IKatan2(((gconst46)*(((((IkReal(-1.00000000000000))*(x741)*(x742)))+(((IkReal(-1.00000000000000))*(r10)*(sj6)*(x743)))))), ((gconst46)*(((((cj5)*(r10)*(x741)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x743)))+(((IkReal(-1.00000000000000))*(cj2)*(cj5)*(sj6)*(x742)))))));
03178 sj4array[0]=IKsin(j4array[0]);
03179 cj4array[0]=IKcos(j4array[0]);
03180 if( j4array[0] > IKPI )
03181 {
03182     j4array[0]-=IK2PI;
03183 }
03184 else if( j4array[0] < -IKPI )
03185 {    j4array[0]+=IK2PI;
03186 }
03187 j4valid[0] = true;
03188 for(int ij4 = 0; ij4 < 1; ++ij4)
03189 {
03190 if( !j4valid[ij4] )
03191 {
03192     continue;
03193 }
03194 _ij4[0] = ij4; _ij4[1] = -1;
03195 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03196 {
03197 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03198 {
03199     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03200 }
03201 }
03202 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03203 {
03204 IkReal evalcond[3];
03205 IkReal x744=IKsin(j4);
03206 IkReal x745=IKcos(j4);
03207 IkReal x746=((IkReal(1.00000000000000))*(cj6));
03208 IkReal x747=((sj5)*(x744));
03209 IkReal x748=((cj5)*(x744));
03210 IkReal x749=((IkReal(1.00000000000000))*(sj6)*(x745));
03211 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x749)))+(((r22)*(x747)))+(((r21)*(sj6)*(x748)))+(((IkReal(-1.00000000000000))*(r20)*(x746)*(x748)))+(((IkReal(-1.00000000000000))*(r21)*(x745)*(x746)))+(((IkReal(-1.00000000000000))*(cj2))));
03212 evalcond[1]=((((r02)*(x747)))+(sj2)+(((IkReal(-1.00000000000000))*(r01)*(x745)*(x746)))+(((IkReal(-1.00000000000000))*(r00)*(x746)*(x748)))+(((IkReal(-1.00000000000000))*(r00)*(x749)))+(((r01)*(sj6)*(x748))));
03213 evalcond[2]=((((r11)*(sj6)*(x748)))+(((IkReal(-1.00000000000000))*(r11)*(x745)*(x746)))+(((IkReal(-1.00000000000000))*(r10)*(x749)))+(((IkReal(-1.00000000000000))*(r10)*(x746)*(x748)))+(((r12)*(x747))));
03214 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
03215 {
03216 continue;
03217 }
03218 }
03219 
03220 {
03221 IkReal j3array[1], cj3array[1], sj3array[1];
03222 bool j3valid[1]={false};
03223 _nj3 = 1;
03224 IkReal x750=((r11)*(sj6));
03225 IkReal x751=((IkReal(1.00000000000000))*(r12));
03226 IkReal x752=((IkReal(1.00000000000000))*(sj4));
03227 IkReal x753=((cj6)*(r10));
03228 IkReal x754=((cj4)*(cj5));
03229 if( IKabs(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x752)))+(((IkReal(-1.00000000000000))*(x750)*(x754)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x752)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x751)))+(((x753)*(x754))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj5)*(x751)))+(((sj5)*(x750)))+(((IkReal(-1.00000000000000))*(sj5)*(x753))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x752)))+(((IkReal(-1.00000000000000))*(x750)*(x754)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x752)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x751)))+(((x753)*(x754)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj5)*(x751)))+(((sj5)*(x750)))+(((IkReal(-1.00000000000000))*(sj5)*(x753)))))-1) <= IKFAST_SINCOS_THRESH )
03230     continue;
03231 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x752)))+(((IkReal(-1.00000000000000))*(x750)*(x754)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x752)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x751)))+(((x753)*(x754)))), ((((IkReal(-1.00000000000000))*(cj5)*(x751)))+(((sj5)*(x750)))+(((IkReal(-1.00000000000000))*(sj5)*(x753)))));
03232 sj3array[0]=IKsin(j3array[0]);
03233 cj3array[0]=IKcos(j3array[0]);
03234 if( j3array[0] > IKPI )
03235 {
03236     j3array[0]-=IK2PI;
03237 }
03238 else if( j3array[0] < -IKPI )
03239 {    j3array[0]+=IK2PI;
03240 }
03241 j3valid[0] = true;
03242 for(int ij3 = 0; ij3 < 1; ++ij3)
03243 {
03244 if( !j3valid[ij3] )
03245 {
03246     continue;
03247 }
03248 _ij3[0] = ij3; _ij3[1] = -1;
03249 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03250 {
03251 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03252 {
03253     j3valid[iij3]=false; _ij3[1] = iij3; break; 
03254 }
03255 }
03256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03257 {
03258 IkReal evalcond[6];
03259 IkReal x755=IKsin(j3);
03260 IkReal x756=IKcos(j3);
03261 IkReal x757=((r11)*(sj6));
03262 IkReal x758=((IkReal(1.00000000000000))*(sj5));
03263 IkReal x759=((sj4)*(sj6));
03264 IkReal x760=((cj4)*(sj5));
03265 IkReal x761=((cj6)*(sj4));
03266 IkReal x762=((cj4)*(cj5));
03267 IkReal x763=((r01)*(sj6));
03268 IkReal x764=((r21)*(sj6));
03269 IkReal x765=((cj6)*(sj5));
03270 IkReal x766=((IkReal(1.00000000000000))*(x756));
03271 IkReal x767=((IkReal(1.00000000000000))*(cj6)*(x762));
03272 evalcond[0]=((((IkReal(-1.00000000000000))*(x758)*(x764)))+(((cj5)*(r22)))+(((r20)*(x765)))+(((sj2)*(x755))));
03273 evalcond[1]=((((IkReal(-1.00000000000000))*(x758)*(x763)))+(((cj2)*(x755)))+(((r00)*(x765)))+(((cj5)*(r02))));
03274 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x765)))+(x756)+(((IkReal(-1.00000000000000))*(x757)*(x758))));
03275 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x767)))+(((r21)*(x761)))+(((x762)*(x764)))+(((r22)*(x760)))+(((r20)*(x759)))+(((IkReal(-1.00000000000000))*(sj2)*(x766))));
03276 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x767)))+(((IkReal(-1.00000000000000))*(cj2)*(x766)))+(((x762)*(x763)))+(((r00)*(x759)))+(((r02)*(x760)))+(((r01)*(x761))));
03277 evalcond[5]=((((x757)*(x762)))+(((r10)*(x759)))+(((IkReal(-1.00000000000000))*(r10)*(x767)))+(((r12)*(x760)))+(x755)+(((r11)*(x761))));
03278 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03279 {
03280 continue;
03281 }
03282 }
03283 
03284 {
03285 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03286 vinfos[0].jointtype = 1;
03287 vinfos[0].foffset = j0;
03288 vinfos[0].indices[0] = _ij0[0];
03289 vinfos[0].indices[1] = _ij0[1];
03290 vinfos[0].maxsolutions = _nj0;
03291 vinfos[1].jointtype = 1;
03292 vinfos[1].foffset = j1;
03293 vinfos[1].indices[0] = _ij1[0];
03294 vinfos[1].indices[1] = _ij1[1];
03295 vinfos[1].maxsolutions = _nj1;
03296 vinfos[2].jointtype = 1;
03297 vinfos[2].foffset = j2;
03298 vinfos[2].indices[0] = _ij2[0];
03299 vinfos[2].indices[1] = _ij2[1];
03300 vinfos[2].maxsolutions = _nj2;
03301 vinfos[3].jointtype = 1;
03302 vinfos[3].foffset = j3;
03303 vinfos[3].indices[0] = _ij3[0];
03304 vinfos[3].indices[1] = _ij3[1];
03305 vinfos[3].maxsolutions = _nj3;
03306 vinfos[4].jointtype = 1;
03307 vinfos[4].foffset = j4;
03308 vinfos[4].indices[0] = _ij4[0];
03309 vinfos[4].indices[1] = _ij4[1];
03310 vinfos[4].maxsolutions = _nj4;
03311 vinfos[5].jointtype = 1;
03312 vinfos[5].foffset = j5;
03313 vinfos[5].indices[0] = _ij5[0];
03314 vinfos[5].indices[1] = _ij5[1];
03315 vinfos[5].maxsolutions = _nj5;
03316 vinfos[6].jointtype = 1;
03317 vinfos[6].foffset = j6;
03318 vinfos[6].indices[0] = _ij6[0];
03319 vinfos[6].indices[1] = _ij6[1];
03320 vinfos[6].maxsolutions = _nj6;
03321 std::vector<int> vfree(0);
03322 solutions.AddSolution(vinfos,vfree);
03323 }
03324 }
03325 }
03326 }
03327 }
03328 
03329 }
03330 
03331 }
03332 
03333 } else
03334 {
03335 {
03336 IkReal j4array[1], cj4array[1], sj4array[1];
03337 bool j4valid[1]={false};
03338 _nj4 = 1;
03339 IkReal x768=((r10)*(sj2));
03340 IkReal x769=((IkReal(1.00000000000000))*(sj6));
03341 IkReal x770=((IkReal(1.00000000000000))*(sj2));
03342 if( IKabs(((gconst45)*(((((IkReal(-1.00000000000000))*(x768)*(x769)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x770))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst45)*(((((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj2)*(x769)))+(((cj5)*(cj6)*(x768)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x770))))))) < IKFAST_ATAN2_MAGTHRESH )
03343     continue;
03344 j4array[0]=IKatan2(((gconst45)*(((((IkReal(-1.00000000000000))*(x768)*(x769)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x770)))))), ((gconst45)*(((((IkReal(-1.00000000000000))*(cj5)*(r11)*(sj2)*(x769)))+(((cj5)*(cj6)*(x768)))+(((IkReal(-1.00000000000000))*(r12)*(sj5)*(x770)))))));
03345 sj4array[0]=IKsin(j4array[0]);
03346 cj4array[0]=IKcos(j4array[0]);
03347 if( j4array[0] > IKPI )
03348 {
03349     j4array[0]-=IK2PI;
03350 }
03351 else if( j4array[0] < -IKPI )
03352 {    j4array[0]+=IK2PI;
03353 }
03354 j4valid[0] = true;
03355 for(int ij4 = 0; ij4 < 1; ++ij4)
03356 {
03357 if( !j4valid[ij4] )
03358 {
03359     continue;
03360 }
03361 _ij4[0] = ij4; _ij4[1] = -1;
03362 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03363 {
03364 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03365 {
03366     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03367 }
03368 }
03369 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03370 {
03371 IkReal evalcond[3];
03372 IkReal x771=IKsin(j4);
03373 IkReal x772=IKcos(j4);
03374 IkReal x773=((IkReal(1.00000000000000))*(cj6));
03375 IkReal x774=((sj5)*(x771));
03376 IkReal x775=((cj5)*(x771));
03377 IkReal x776=((IkReal(1.00000000000000))*(sj6)*(x772));
03378 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x773)*(x775)))+(((IkReal(-1.00000000000000))*(r20)*(x776)))+(((r22)*(x774)))+(((r21)*(sj6)*(x775)))+(((IkReal(-1.00000000000000))*(r21)*(x772)*(x773)))+(((IkReal(-1.00000000000000))*(cj2))));
03379 evalcond[1]=((sj2)+(((IkReal(-1.00000000000000))*(r00)*(x776)))+(((IkReal(-1.00000000000000))*(r00)*(x773)*(x775)))+(((IkReal(-1.00000000000000))*(r01)*(x772)*(x773)))+(((r01)*(sj6)*(x775)))+(((r02)*(x774))));
03380 evalcond[2]=((((r11)*(sj6)*(x775)))+(((IkReal(-1.00000000000000))*(r11)*(x772)*(x773)))+(((IkReal(-1.00000000000000))*(r10)*(x776)))+(((r12)*(x774)))+(((IkReal(-1.00000000000000))*(r10)*(x773)*(x775))));
03381 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
03382 {
03383 continue;
03384 }
03385 }
03386 
03387 {
03388 IkReal j3array[1], cj3array[1], sj3array[1];
03389 bool j3valid[1]={false};
03390 _nj3 = 1;
03391 IkReal x777=((r11)*(sj6));
03392 IkReal x778=((IkReal(1.00000000000000))*(r12));
03393 IkReal x779=((IkReal(1.00000000000000))*(sj4));
03394 IkReal x780=((cj6)*(r10));
03395 IkReal x781=((cj4)*(cj5));
03396 if( IKabs(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x779)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x778)))+(((IkReal(-1.00000000000000))*(x777)*(x781)))+(((x780)*(x781)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x779))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj5)*(x778)))+(((IkReal(-1.00000000000000))*(sj5)*(x780)))+(((sj5)*(x777))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x779)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x778)))+(((IkReal(-1.00000000000000))*(x777)*(x781)))+(((x780)*(x781)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x779)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj5)*(x778)))+(((IkReal(-1.00000000000000))*(sj5)*(x780)))+(((sj5)*(x777)))))-1) <= IKFAST_SINCOS_THRESH )
03397     continue;
03398 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r10)*(sj6)*(x779)))+(((IkReal(-1.00000000000000))*(cj4)*(sj5)*(x778)))+(((IkReal(-1.00000000000000))*(x777)*(x781)))+(((x780)*(x781)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x779)))), ((((IkReal(-1.00000000000000))*(cj5)*(x778)))+(((IkReal(-1.00000000000000))*(sj5)*(x780)))+(((sj5)*(x777)))));
03399 sj3array[0]=IKsin(j3array[0]);
03400 cj3array[0]=IKcos(j3array[0]);
03401 if( j3array[0] > IKPI )
03402 {
03403     j3array[0]-=IK2PI;
03404 }
03405 else if( j3array[0] < -IKPI )
03406 {    j3array[0]+=IK2PI;
03407 }
03408 j3valid[0] = true;
03409 for(int ij3 = 0; ij3 < 1; ++ij3)
03410 {
03411 if( !j3valid[ij3] )
03412 {
03413     continue;
03414 }
03415 _ij3[0] = ij3; _ij3[1] = -1;
03416 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03417 {
03418 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03419 {
03420     j3valid[iij3]=false; _ij3[1] = iij3; break; 
03421 }
03422 }
03423 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03424 {
03425 IkReal evalcond[6];
03426 IkReal x782=IKsin(j3);
03427 IkReal x783=IKcos(j3);
03428 IkReal x784=((r11)*(sj6));
03429 IkReal x785=((IkReal(1.00000000000000))*(sj5));
03430 IkReal x786=((sj4)*(sj6));
03431 IkReal x787=((cj4)*(sj5));
03432 IkReal x788=((cj6)*(sj4));
03433 IkReal x789=((cj4)*(cj5));
03434 IkReal x790=((r01)*(sj6));
03435 IkReal x791=((r21)*(sj6));
03436 IkReal x792=((cj6)*(sj5));
03437 IkReal x793=((IkReal(1.00000000000000))*(x783));
03438 IkReal x794=((IkReal(1.00000000000000))*(cj6)*(x789));
03439 evalcond[0]=((((sj2)*(x782)))+(((cj5)*(r22)))+(((r20)*(x792)))+(((IkReal(-1.00000000000000))*(x785)*(x791))));
03440 evalcond[1]=((((r00)*(x792)))+(((IkReal(-1.00000000000000))*(x785)*(x790)))+(((cj2)*(x782)))+(((cj5)*(r02))));
03441 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x792)))+(((IkReal(-1.00000000000000))*(x784)*(x785)))+(x783));
03442 evalcond[3]=((((r20)*(x786)))+(((IkReal(-1.00000000000000))*(r20)*(x794)))+(((r21)*(x788)))+(((r22)*(x787)))+(((IkReal(-1.00000000000000))*(sj2)*(x793)))+(((x789)*(x791))));
03443 evalcond[4]=((((r00)*(x786)))+(((r01)*(x788)))+(((IkReal(-1.00000000000000))*(r00)*(x794)))+(((IkReal(-1.00000000000000))*(cj2)*(x793)))+(((r02)*(x787)))+(((x789)*(x790))));
03444 evalcond[5]=((((r10)*(x786)))+(((x784)*(x789)))+(((IkReal(-1.00000000000000))*(r10)*(x794)))+(((r11)*(x788)))+(x782)+(((r12)*(x787))));
03445 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03446 {
03447 continue;
03448 }
03449 }
03450 
03451 {
03452 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03453 vinfos[0].jointtype = 1;
03454 vinfos[0].foffset = j0;
03455 vinfos[0].indices[0] = _ij0[0];
03456 vinfos[0].indices[1] = _ij0[1];
03457 vinfos[0].maxsolutions = _nj0;
03458 vinfos[1].jointtype = 1;
03459 vinfos[1].foffset = j1;
03460 vinfos[1].indices[0] = _ij1[0];
03461 vinfos[1].indices[1] = _ij1[1];
03462 vinfos[1].maxsolutions = _nj1;
03463 vinfos[2].jointtype = 1;
03464 vinfos[2].foffset = j2;
03465 vinfos[2].indices[0] = _ij2[0];
03466 vinfos[2].indices[1] = _ij2[1];
03467 vinfos[2].maxsolutions = _nj2;
03468 vinfos[3].jointtype = 1;
03469 vinfos[3].foffset = j3;
03470 vinfos[3].indices[0] = _ij3[0];
03471 vinfos[3].indices[1] = _ij3[1];
03472 vinfos[3].maxsolutions = _nj3;
03473 vinfos[4].jointtype = 1;
03474 vinfos[4].foffset = j4;
03475 vinfos[4].indices[0] = _ij4[0];
03476 vinfos[4].indices[1] = _ij4[1];
03477 vinfos[4].maxsolutions = _nj4;
03478 vinfos[5].jointtype = 1;
03479 vinfos[5].foffset = j5;
03480 vinfos[5].indices[0] = _ij5[0];
03481 vinfos[5].indices[1] = _ij5[1];
03482 vinfos[5].maxsolutions = _nj5;
03483 vinfos[6].jointtype = 1;
03484 vinfos[6].foffset = j6;
03485 vinfos[6].indices[0] = _ij6[0];
03486 vinfos[6].indices[1] = _ij6[1];
03487 vinfos[6].maxsolutions = _nj6;
03488 std::vector<int> vfree(0);
03489 solutions.AddSolution(vinfos,vfree);
03490 }
03491 }
03492 }
03493 }
03494 }
03495 
03496 }
03497 
03498 }
03499 }
03500 }
03501 
03502 } else
03503 {
03504 IkReal x795=((IkReal(0.390000000000000))*(sj5));
03505 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
03506 evalcond[1]=((IkReal(-0.400000000000000))+(((cj6)*(r00)*(x795)))+(((IkReal(0.000500000000000000))*(cj6)*(r01)))+(((IkReal(0.000500000000000000))*(r00)*(sj6)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x795)))+(((IkReal(0.390000000000000))*(cj5)*(r02))));
03507 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  )
03508 {
03509 {
03510 IkReal j2array[1], cj2array[1], sj2array[1];
03511 bool j2valid[1]={false};
03512 _nj2 = 1;
03513 IkReal x796=((IkReal(195.000000000000))*(sj5));
03514 IkReal x797=((IkReal(195.000000000000))*(cj5));
03515 IkReal x798=((IkReal(0.250000000000000))*(sj6));
03516 IkReal x799=((IkReal(0.250000000000000))*(cj6));
03517 if( IKabs(((((r10)*(x798)))+(((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x796)))+(((r11)*(x799)))+(((cj6)*(r10)*(x796)))+(((r12)*(x797))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r20)*(x798)))+(((cj6)*(r20)*(x796)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x799)))+(((r22)*(x797)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x796))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r10)*(x798)))+(((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x796)))+(((r11)*(x799)))+(((cj6)*(r10)*(x796)))+(((r12)*(x797)))))+IKsqr(((((r20)*(x798)))+(((cj6)*(r20)*(x796)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x799)))+(((r22)*(x797)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x796)))))-1) <= IKFAST_SINCOS_THRESH )
03518     continue;
03519 j2array[0]=IKatan2(((((r10)*(x798)))+(((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x796)))+(((r11)*(x799)))+(((cj6)*(r10)*(x796)))+(((r12)*(x797)))), ((((r20)*(x798)))+(((cj6)*(r20)*(x796)))+(((IkReal(-500.000000000000))*(pz)))+(((r21)*(x799)))+(((r22)*(x797)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x796)))));
03520 sj2array[0]=IKsin(j2array[0]);
03521 cj2array[0]=IKcos(j2array[0]);
03522 if( j2array[0] > IKPI )
03523 {
03524     j2array[0]-=IK2PI;
03525 }
03526 else if( j2array[0] < -IKPI )
03527 {    j2array[0]+=IK2PI;
03528 }
03529 j2valid[0] = true;
03530 for(int ij2 = 0; ij2 < 1; ++ij2)
03531 {
03532 if( !j2valid[ij2] )
03533 {
03534     continue;
03535 }
03536 _ij2[0] = ij2; _ij2[1] = -1;
03537 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03538 {
03539 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03540 {
03541     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03542 }
03543 }
03544 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03545 {
03546 IkReal evalcond[2];
03547 IkReal x800=((IkReal(0.000500000000000000))*(cj6));
03548 IkReal x801=((IkReal(0.000500000000000000))*(sj6));
03549 IkReal x802=((IkReal(0.390000000000000))*(cj5));
03550 IkReal x803=((IkReal(0.390000000000000))*(sj5)*(sj6));
03551 IkReal x804=((IkReal(0.390000000000000))*(cj6)*(sj5));
03552 evalcond[0]=((((r21)*(x800)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((r22)*(x802)))+(((IkReal(-1.00000000000000))*(r21)*(x803)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x804)))+(((r20)*(x801))));
03553 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x803)))+(((r12)*(x802)))+(((IkReal(-0.00200000000000000))*(IKsin(j2))))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x800)))+(((r10)*(x804)))+(((r10)*(x801))));
03554 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
03555 {
03556 continue;
03557 }
03558 }
03559 
03560 {
03561 IkReal dummyeval[1];
03562 IkReal gconst57;
03563 IkReal x805=(cj6)*(cj6);
03564 IkReal x806=(sj6)*(sj6);
03565 IkReal x807=((r01)*(r20));
03566 IkReal x808=((r02)*(sj5));
03567 IkReal x809=((cj5)*(x806));
03568 IkReal x810=((IkReal(1.00000000000000))*(r22)*(sj5));
03569 IkReal x811=((IkReal(1.00000000000000))*(r00)*(r21));
03570 IkReal x812=((cj5)*(x805));
03571 gconst57=IKsign(((((cj6)*(r21)*(x808)))+(((IkReal(-1.00000000000000))*(x809)*(x811)))+(((IkReal(-1.00000000000000))*(x811)*(x812)))+(((r20)*(sj6)*(x808)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x810)))+(((x807)*(x812)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x810)))+(((x807)*(x809)))));
03572 IkReal x813=(cj6)*(cj6);
03573 IkReal x814=(sj6)*(sj6);
03574 IkReal x815=((r01)*(r20));
03575 IkReal x816=((r02)*(sj5));
03576 IkReal x817=((cj5)*(x814));
03577 IkReal x818=((IkReal(1.00000000000000))*(r22)*(sj5));
03578 IkReal x819=((IkReal(1.00000000000000))*(r00)*(r21));
03579 IkReal x820=((cj5)*(x813));
03580 dummyeval[0]=((((x815)*(x817)))+(((x815)*(x820)))+(((IkReal(-1.00000000000000))*(x819)*(x820)))+(((IkReal(-1.00000000000000))*(x817)*(x819)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x818)))+(((cj6)*(r21)*(x816)))+(((r20)*(sj6)*(x816)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x818))));
03581 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03582 {
03583 {
03584 IkReal dummyeval[1];
03585 IkReal gconst58;
03586 IkReal x821=(sj6)*(sj6);
03587 IkReal x822=(cj6)*(cj6);
03588 IkReal x823=((r01)*(r10));
03589 IkReal x824=((IkReal(1.00000000000000))*(r00));
03590 IkReal x825=((r02)*(sj5));
03591 IkReal x826=((r12)*(sj5));
03592 IkReal x827=((cj5)*(x821));
03593 IkReal x828=((cj5)*(x822));
03594 gconst58=IKsign(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x826)))+(((IkReal(-1.00000000000000))*(r11)*(x824)*(x827)))+(((IkReal(-1.00000000000000))*(r11)*(x824)*(x828)))+(((IkReal(-1.00000000000000))*(sj6)*(x824)*(x826)))+(((x823)*(x828)))+(((x823)*(x827)))+(((cj6)*(r11)*(x825)))+(((r10)*(sj6)*(x825)))));
03595 IkReal x829=(sj6)*(sj6);
03596 IkReal x830=(cj6)*(cj6);
03597 IkReal x831=((r01)*(r10));
03598 IkReal x832=((IkReal(1.00000000000000))*(r00));
03599 IkReal x833=((r02)*(sj5));
03600 IkReal x834=((r12)*(sj5));
03601 IkReal x835=((cj5)*(x829));
03602 IkReal x836=((cj5)*(x830));
03603 dummyeval[0]=((((x831)*(x835)))+(((x831)*(x836)))+(((r10)*(sj6)*(x833)))+(((IkReal(-1.00000000000000))*(sj6)*(x832)*(x834)))+(((IkReal(-1.00000000000000))*(r11)*(x832)*(x836)))+(((IkReal(-1.00000000000000))*(r11)*(x832)*(x835)))+(((cj6)*(r11)*(x833)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x834))));
03604 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03605 {
03606 {
03607 IkReal dummyeval[1];
03608 dummyeval[0]=cj2;
03609 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03610 {
03611 {
03612 IkReal evalcond[5];
03613 IkReal x837=((IkReal(0.000500000000000000))*(cj6));
03614 IkReal x838=((IkReal(0.000500000000000000))*(sj6));
03615 IkReal x839=((sj5)*(sj6));
03616 IkReal x840=((cj5)*(r12));
03617 IkReal x841=((IkReal(0.390000000000000))*(cj5));
03618 IkReal x842=((IkReal(0.390000000000000))*(cj6)*(sj5));
03619 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
03620 evalcond[1]=((((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x839)))+(x840));
03621 evalcond[2]=((((r22)*(x841)))+(((IkReal(-0.390000000000000))*(r21)*(x839)))+(((r20)*(x842)))+(((r20)*(x838)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x837))));
03622 evalcond[3]=((IkReal(-0.400000000000000))+(((r02)*(x841)))+(((IkReal(-0.390000000000000))*(r01)*(x839)))+(((r00)*(x838)))+(((r00)*(x842)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x837))));
03623 evalcond[4]=((IkReal(-0.00200000000000000))+(((IkReal(0.390000000000000))*(x840)))+(((r10)*(x838)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x842)))+(((r11)*(x837)))+(((IkReal(-0.390000000000000))*(r11)*(x839))));
03624 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
03625 {
03626 {
03627 IkReal j3array[1], cj3array[1], sj3array[1];
03628 bool j3valid[1]={false};
03629 _nj3 = 1;
03630 IkReal x843=((sj5)*(sj6));
03631 IkReal x844=((IkReal(1.00000000000000))*(cj5));
03632 IkReal x845=((IkReal(1.00000000000000))*(cj6)*(sj5));
03633 if( IKabs(((((IkReal(-1.00000000000000))*(r20)*(x845)))+(((r21)*(x843)))+(((IkReal(-1.00000000000000))*(r22)*(x844))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x844)))+(((r01)*(x843)))+(((IkReal(-1.00000000000000))*(r00)*(x845))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r20)*(x845)))+(((r21)*(x843)))+(((IkReal(-1.00000000000000))*(r22)*(x844)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x844)))+(((r01)*(x843)))+(((IkReal(-1.00000000000000))*(r00)*(x845)))))-1) <= IKFAST_SINCOS_THRESH )
03634     continue;
03635 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r20)*(x845)))+(((r21)*(x843)))+(((IkReal(-1.00000000000000))*(r22)*(x844)))), ((((IkReal(-1.00000000000000))*(r02)*(x844)))+(((r01)*(x843)))+(((IkReal(-1.00000000000000))*(r00)*(x845)))));
03636 sj3array[0]=IKsin(j3array[0]);
03637 cj3array[0]=IKcos(j3array[0]);
03638 if( j3array[0] > IKPI )
03639 {
03640     j3array[0]-=IK2PI;
03641 }
03642 else if( j3array[0] < -IKPI )
03643 {    j3array[0]+=IK2PI;
03644 }
03645 j3valid[0] = true;
03646 for(int ij3 = 0; ij3 < 1; ++ij3)
03647 {
03648 if( !j3valid[ij3] )
03649 {
03650     continue;
03651 }
03652 _ij3[0] = ij3; _ij3[1] = -1;
03653 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03654 {
03655 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03656 {
03657     j3valid[iij3]=false; _ij3[1] = iij3; break; 
03658 }
03659 }
03660 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03661 {
03662 IkReal evalcond[2];
03663 IkReal x846=((cj6)*(sj5));
03664 IkReal x847=((IkReal(1.00000000000000))*(sj5)*(sj6));
03665 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x847)))+(((r20)*(x846)))+(IKsin(j3)));
03666 evalcond[1]=((((r00)*(x846)))+(IKcos(j3))+(((IkReal(-1.00000000000000))*(r01)*(x847)))+(((cj5)*(r02))));
03667 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
03668 {
03669 continue;
03670 }
03671 }
03672 
03673 {
03674 IkReal dummyeval[1];
03675 IkReal gconst63;
03676 IkReal x848=(sj6)*(sj6);
03677 IkReal x849=(cj6)*(cj6);
03678 IkReal x850=((r01)*(r10));
03679 IkReal x851=((IkReal(1.00000000000000))*(r00));
03680 IkReal x852=((r02)*(sj5));
03681 IkReal x853=((r12)*(sj5));
03682 IkReal x854=((cj5)*(x848));
03683 IkReal x855=((cj5)*(x849));
03684 gconst63=IKsign(((((x850)*(x855)))+(((x850)*(x854)))+(((IkReal(-1.00000000000000))*(sj6)*(x851)*(x853)))+(((cj6)*(r11)*(x852)))+(((IkReal(-1.00000000000000))*(r11)*(x851)*(x855)))+(((IkReal(-1.00000000000000))*(r11)*(x851)*(x854)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x853)))+(((r10)*(sj6)*(x852)))));
03685 IkReal x856=(sj6)*(sj6);
03686 IkReal x857=(cj6)*(cj6);
03687 IkReal x858=((r01)*(r10));
03688 IkReal x859=((IkReal(1.00000000000000))*(r00));
03689 IkReal x860=((r02)*(sj5));
03690 IkReal x861=((r12)*(sj5));
03691 IkReal x862=((cj5)*(x856));
03692 IkReal x863=((cj5)*(x857));
03693 dummyeval[0]=((((cj6)*(r11)*(x860)))+(((IkReal(-1.00000000000000))*(r11)*(x859)*(x863)))+(((IkReal(-1.00000000000000))*(r11)*(x859)*(x862)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x861)))+(((x858)*(x863)))+(((x858)*(x862)))+(((r10)*(sj6)*(x860)))+(((IkReal(-1.00000000000000))*(sj6)*(x859)*(x861))));
03694 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03695 {
03696 {
03697 IkReal dummyeval[1];
03698 IkReal gconst64;
03699 IkReal x864=(cj5)*(cj5);
03700 IkReal x865=(sj6)*(sj6);
03701 IkReal x866=(cj6)*(cj6);
03702 IkReal x867=((r00)*(r20));
03703 IkReal x868=((cj5)*(sj5));
03704 IkReal x869=((r01)*(r21));
03705 IkReal x870=((r21)*(sj6));
03706 IkReal x871=((sj6)*(x868));
03707 IkReal x872=((IkReal(1.00000000000000))*(cj6)*(r00));
03708 IkReal x873=((cj6)*(r01)*(r20)*(sj6));
03709 gconst64=IKsign(((((r02)*(x868)*(x870)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x868)))+(((IkReal(-1.00000000000000))*(x864)*(x870)*(x872)))+(((x864)*(x866)*(x867)))+(((x866)*(x869)))+(((x864)*(x865)*(x869)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x864)*(x873)))+(((cj6)*(r00)*(x870)))+(x873)+(((IkReal(-1.00000000000000))*(r22)*(x868)*(x872)))+(((x865)*(x867)))+(((r01)*(r22)*(x871)))));
03710 IkReal x874=(cj5)*(cj5);
03711 IkReal x875=(sj6)*(sj6);
03712 IkReal x876=(cj6)*(cj6);
03713 IkReal x877=((r00)*(r20));
03714 IkReal x878=((cj5)*(sj5));
03715 IkReal x879=((r01)*(r21));
03716 IkReal x880=((r21)*(sj6));
03717 IkReal x881=((sj6)*(x878));
03718 IkReal x882=((IkReal(1.00000000000000))*(cj6)*(r00));
03719 IkReal x883=x873;
03720 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x878)))+(((x876)*(x879)))+(((x874)*(x876)*(x877)))+(((cj6)*(r00)*(x880)))+(((r02)*(x878)*(x880)))+(((x874)*(x875)*(x879)))+(((IkReal(-1.00000000000000))*(r22)*(x878)*(x882)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x874)*(x883)))+(((x875)*(x877)))+(x883)+(((r01)*(r22)*(x881)))+(((IkReal(-1.00000000000000))*(x874)*(x880)*(x882))));
03721 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03722 {
03723 continue;
03724 
03725 } else
03726 {
03727 {
03728 IkReal j4array[1], cj4array[1], sj4array[1];
03729 bool j4valid[1]={false};
03730 _nj4 = 1;
03731 IkReal x884=((IkReal(1.00000000000000))*(sj3));
03732 if( IKabs(((gconst64)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x884)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x884))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst64)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x884)))+(((cj5)*(cj6)*(r20)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x884))))))) < IKFAST_ATAN2_MAGTHRESH )
03733     continue;
03734 j4array[0]=IKatan2(((gconst64)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x884)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x884)))))), ((gconst64)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x884)))+(((cj5)*(cj6)*(r20)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x884)))))));
03735 sj4array[0]=IKsin(j4array[0]);
03736 cj4array[0]=IKcos(j4array[0]);
03737 if( j4array[0] > IKPI )
03738 {
03739     j4array[0]-=IK2PI;
03740 }
03741 else if( j4array[0] < -IKPI )
03742 {    j4array[0]+=IK2PI;
03743 }
03744 j4valid[0] = true;
03745 for(int ij4 = 0; ij4 < 1; ++ij4)
03746 {
03747 if( !j4valid[ij4] )
03748 {
03749     continue;
03750 }
03751 _ij4[0] = ij4; _ij4[1] = -1;
03752 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03753 {
03754 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03755 {
03756     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03757 }
03758 }
03759 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03760 {
03761 IkReal evalcond[6];
03762 IkReal x885=IKsin(j4);
03763 IkReal x886=IKcos(j4);
03764 IkReal x887=((IkReal(1.00000000000000))*(r00));
03765 IkReal x888=((cj5)*(r01));
03766 IkReal x889=((cj5)*(r11));
03767 IkReal x890=((IkReal(1.00000000000000))*(cj5));
03768 IkReal x891=((cj5)*(r21));
03769 IkReal x892=((sj5)*(x885));
03770 IkReal x893=((cj6)*(x885));
03771 IkReal x894=((sj6)*(x886));
03772 IkReal x895=((sj6)*(x885));
03773 IkReal x896=((sj5)*(x886));
03774 IkReal x897=((cj6)*(x886));
03775 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x894)))+(((IkReal(-1.00000000000000))*(r21)*(x897)))+(((x891)*(x895)))+(((r22)*(x892)))+(((IkReal(-1.00000000000000))*(r20)*(x890)*(x893))));
03776 evalcond[1]=((((r20)*(x895)))+(((r21)*(x893)))+(((x891)*(x894)))+(((r22)*(x896)))+(((IkReal(-1.00000000000000))*(r20)*(x890)*(x897)))+(((IkReal(-1.00000000000000))*(cj3))));
03777 evalcond[2]=((((r02)*(x892)))+(((IkReal(-1.00000000000000))*(cj5)*(x887)*(x893)))+(((IkReal(-1.00000000000000))*(r01)*(x897)))+(((x888)*(x895)))+(((IkReal(-1.00000000000000))*(x887)*(x894))));
03778 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x897)))+(((IkReal(-1.00000000000000))*(r10)*(x890)*(x893)))+(((IkReal(-1.00000000000000))*(r10)*(x894)))+(((x889)*(x895)))+(((r12)*(x892))));
03779 evalcond[4]=((((r02)*(x896)))+(sj3)+(((r01)*(x893)))+(((r00)*(x895)))+(((IkReal(-1.00000000000000))*(cj5)*(x887)*(x897)))+(((x888)*(x894))));
03780 evalcond[5]=((((r11)*(x893)))+(((r10)*(x895)))+(((IkReal(-1.00000000000000))*(r10)*(x890)*(x897)))+(((x889)*(x894)))+(((r12)*(x896))));
03781 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03782 {
03783 continue;
03784 }
03785 }
03786 
03787 {
03788 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03789 vinfos[0].jointtype = 1;
03790 vinfos[0].foffset = j0;
03791 vinfos[0].indices[0] = _ij0[0];
03792 vinfos[0].indices[1] = _ij0[1];
03793 vinfos[0].maxsolutions = _nj0;
03794 vinfos[1].jointtype = 1;
03795 vinfos[1].foffset = j1;
03796 vinfos[1].indices[0] = _ij1[0];
03797 vinfos[1].indices[1] = _ij1[1];
03798 vinfos[1].maxsolutions = _nj1;
03799 vinfos[2].jointtype = 1;
03800 vinfos[2].foffset = j2;
03801 vinfos[2].indices[0] = _ij2[0];
03802 vinfos[2].indices[1] = _ij2[1];
03803 vinfos[2].maxsolutions = _nj2;
03804 vinfos[3].jointtype = 1;
03805 vinfos[3].foffset = j3;
03806 vinfos[3].indices[0] = _ij3[0];
03807 vinfos[3].indices[1] = _ij3[1];
03808 vinfos[3].maxsolutions = _nj3;
03809 vinfos[4].jointtype = 1;
03810 vinfos[4].foffset = j4;
03811 vinfos[4].indices[0] = _ij4[0];
03812 vinfos[4].indices[1] = _ij4[1];
03813 vinfos[4].maxsolutions = _nj4;
03814 vinfos[5].jointtype = 1;
03815 vinfos[5].foffset = j5;
03816 vinfos[5].indices[0] = _ij5[0];
03817 vinfos[5].indices[1] = _ij5[1];
03818 vinfos[5].maxsolutions = _nj5;
03819 vinfos[6].jointtype = 1;
03820 vinfos[6].foffset = j6;
03821 vinfos[6].indices[0] = _ij6[0];
03822 vinfos[6].indices[1] = _ij6[1];
03823 vinfos[6].maxsolutions = _nj6;
03824 std::vector<int> vfree(0);
03825 solutions.AddSolution(vinfos,vfree);
03826 }
03827 }
03828 }
03829 
03830 }
03831 
03832 }
03833 
03834 } else
03835 {
03836 {
03837 IkReal j4array[1], cj4array[1], sj4array[1];
03838 bool j4valid[1]={false};
03839 _nj4 = 1;
03840 IkReal x898=((sj3)*(sj6));
03841 IkReal x899=((IkReal(1.00000000000000))*(r10));
03842 IkReal x900=((cj6)*(sj3));
03843 if( IKabs(((gconst63)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x899)*(x900)))+(((cj5)*(r11)*(x898))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst63)*(((((IkReal(-1.00000000000000))*(x898)*(x899)))+(((IkReal(-1.00000000000000))*(r11)*(x900))))))) < IKFAST_ATAN2_MAGTHRESH )
03844     continue;
03845 j4array[0]=IKatan2(((gconst63)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x899)*(x900)))+(((cj5)*(r11)*(x898)))))), ((gconst63)*(((((IkReal(-1.00000000000000))*(x898)*(x899)))+(((IkReal(-1.00000000000000))*(r11)*(x900)))))));
03846 sj4array[0]=IKsin(j4array[0]);
03847 cj4array[0]=IKcos(j4array[0]);
03848 if( j4array[0] > IKPI )
03849 {
03850     j4array[0]-=IK2PI;
03851 }
03852 else if( j4array[0] < -IKPI )
03853 {    j4array[0]+=IK2PI;
03854 }
03855 j4valid[0] = true;
03856 for(int ij4 = 0; ij4 < 1; ++ij4)
03857 {
03858 if( !j4valid[ij4] )
03859 {
03860     continue;
03861 }
03862 _ij4[0] = ij4; _ij4[1] = -1;
03863 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03864 {
03865 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03866 {
03867     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03868 }
03869 }
03870 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03871 {
03872 IkReal evalcond[6];
03873 IkReal x901=IKsin(j4);
03874 IkReal x902=IKcos(j4);
03875 IkReal x903=((IkReal(1.00000000000000))*(r00));
03876 IkReal x904=((cj5)*(r01));
03877 IkReal x905=((cj5)*(r11));
03878 IkReal x906=((IkReal(1.00000000000000))*(cj5));
03879 IkReal x907=((cj5)*(r21));
03880 IkReal x908=((sj5)*(x901));
03881 IkReal x909=((cj6)*(x901));
03882 IkReal x910=((sj6)*(x902));
03883 IkReal x911=((sj6)*(x901));
03884 IkReal x912=((sj5)*(x902));
03885 IkReal x913=((cj6)*(x902));
03886 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x906)*(x909)))+(((IkReal(-1.00000000000000))*(r20)*(x910)))+(((r22)*(x908)))+(((x907)*(x911)))+(((IkReal(-1.00000000000000))*(r21)*(x913))));
03887 evalcond[1]=((((r21)*(x909)))+(((r20)*(x911)))+(((x907)*(x910)))+(((r22)*(x912)))+(((IkReal(-1.00000000000000))*(r20)*(x906)*(x913)))+(((IkReal(-1.00000000000000))*(cj3))));
03888 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x913)))+(((IkReal(-1.00000000000000))*(cj5)*(x903)*(x909)))+(((r02)*(x908)))+(((x904)*(x911)))+(((IkReal(-1.00000000000000))*(x903)*(x910))));
03889 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x906)*(x909)))+(((IkReal(-1.00000000000000))*(r11)*(x913)))+(((IkReal(-1.00000000000000))*(r10)*(x910)))+(((r12)*(x908)))+(((x905)*(x911))));
03890 evalcond[4]=((sj3)+(((r01)*(x909)))+(((IkReal(-1.00000000000000))*(cj5)*(x903)*(x913)))+(((r02)*(x912)))+(((x904)*(x910)))+(((r00)*(x911))));
03891 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x906)*(x913)))+(((r11)*(x909)))+(((r12)*(x912)))+(((r10)*(x911)))+(((x905)*(x910))));
03892 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03893 {
03894 continue;
03895 }
03896 }
03897 
03898 {
03899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03900 vinfos[0].jointtype = 1;
03901 vinfos[0].foffset = j0;
03902 vinfos[0].indices[0] = _ij0[0];
03903 vinfos[0].indices[1] = _ij0[1];
03904 vinfos[0].maxsolutions = _nj0;
03905 vinfos[1].jointtype = 1;
03906 vinfos[1].foffset = j1;
03907 vinfos[1].indices[0] = _ij1[0];
03908 vinfos[1].indices[1] = _ij1[1];
03909 vinfos[1].maxsolutions = _nj1;
03910 vinfos[2].jointtype = 1;
03911 vinfos[2].foffset = j2;
03912 vinfos[2].indices[0] = _ij2[0];
03913 vinfos[2].indices[1] = _ij2[1];
03914 vinfos[2].maxsolutions = _nj2;
03915 vinfos[3].jointtype = 1;
03916 vinfos[3].foffset = j3;
03917 vinfos[3].indices[0] = _ij3[0];
03918 vinfos[3].indices[1] = _ij3[1];
03919 vinfos[3].maxsolutions = _nj3;
03920 vinfos[4].jointtype = 1;
03921 vinfos[4].foffset = j4;
03922 vinfos[4].indices[0] = _ij4[0];
03923 vinfos[4].indices[1] = _ij4[1];
03924 vinfos[4].maxsolutions = _nj4;
03925 vinfos[5].jointtype = 1;
03926 vinfos[5].foffset = j5;
03927 vinfos[5].indices[0] = _ij5[0];
03928 vinfos[5].indices[1] = _ij5[1];
03929 vinfos[5].maxsolutions = _nj5;
03930 vinfos[6].jointtype = 1;
03931 vinfos[6].foffset = j6;
03932 vinfos[6].indices[0] = _ij6[0];
03933 vinfos[6].indices[1] = _ij6[1];
03934 vinfos[6].maxsolutions = _nj6;
03935 std::vector<int> vfree(0);
03936 solutions.AddSolution(vinfos,vfree);
03937 }
03938 }
03939 }
03940 
03941 }
03942 
03943 }
03944 }
03945 }
03946 
03947 } else
03948 {
03949 IkReal x914=((IkReal(0.000500000000000000))*(cj6));
03950 IkReal x915=((IkReal(0.000500000000000000))*(sj6));
03951 IkReal x916=((sj5)*(sj6));
03952 IkReal x917=((cj5)*(r12));
03953 IkReal x918=((IkReal(0.390000000000000))*(cj5));
03954 IkReal x919=((IkReal(0.390000000000000))*(cj6)*(sj5));
03955 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
03956 evalcond[1]=((((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x916)))+(x917));
03957 evalcond[2]=((((r20)*(x919)))+(((r20)*(x915)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x918)))+(((r21)*(x914)))+(((IkReal(-0.390000000000000))*(r21)*(x916))));
03958 evalcond[3]=((IkReal(-0.400000000000000))+(((IkReal(-0.390000000000000))*(r01)*(x916)))+(((r02)*(x918)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x914)))+(((r00)*(x915)))+(((r00)*(x919))));
03959 evalcond[4]=((IkReal(0.00200000000000000))+(((r10)*(x915)))+(((r10)*(x919)))+(((r11)*(x914)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.390000000000000))*(x917)))+(((IkReal(-0.390000000000000))*(r11)*(x916))));
03960 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
03961 {
03962 {
03963 IkReal j3array[1], cj3array[1], sj3array[1];
03964 bool j3valid[1]={false};
03965 _nj3 = 1;
03966 IkReal x920=((cj6)*(sj5));
03967 IkReal x921=((sj5)*(sj6));
03968 if( IKabs(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x921)))+(((r20)*(x920))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r01)*(x921)))+(((IkReal(-1.00000000000000))*(r00)*(x920)))+(((IkReal(-1.00000000000000))*(cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x921)))+(((r20)*(x920)))))+IKsqr(((((r01)*(x921)))+(((IkReal(-1.00000000000000))*(r00)*(x920)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
03969     continue;
03970 j3array[0]=IKatan2(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x921)))+(((r20)*(x920)))), ((((r01)*(x921)))+(((IkReal(-1.00000000000000))*(r00)*(x920)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))));
03971 sj3array[0]=IKsin(j3array[0]);
03972 cj3array[0]=IKcos(j3array[0]);
03973 if( j3array[0] > IKPI )
03974 {
03975     j3array[0]-=IK2PI;
03976 }
03977 else if( j3array[0] < -IKPI )
03978 {    j3array[0]+=IK2PI;
03979 }
03980 j3valid[0] = true;
03981 for(int ij3 = 0; ij3 < 1; ++ij3)
03982 {
03983 if( !j3valid[ij3] )
03984 {
03985     continue;
03986 }
03987 _ij3[0] = ij3; _ij3[1] = -1;
03988 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03989 {
03990 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03991 {
03992     j3valid[iij3]=false; _ij3[1] = iij3; break; 
03993 }
03994 }
03995 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03996 {
03997 IkReal evalcond[2];
03998 IkReal x922=((cj6)*(sj5));
03999 IkReal x923=((IkReal(1.00000000000000))*(sj5)*(sj6));
04000 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x923)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((r20)*(x922))));
04001 evalcond[1]=((IKcos(j3))+(((IkReal(-1.00000000000000))*(r01)*(x923)))+(((r00)*(x922)))+(((cj5)*(r02))));
04002 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
04003 {
04004 continue;
04005 }
04006 }
04007 
04008 {
04009 IkReal dummyeval[1];
04010 IkReal gconst67;
04011 IkReal x924=(sj6)*(sj6);
04012 IkReal x925=(cj6)*(cj6);
04013 IkReal x926=((cj6)*(sj5));
04014 IkReal x927=((r10)*(r21));
04015 IkReal x928=((IkReal(1.00000000000000))*(r12));
04016 IkReal x929=((sj5)*(sj6));
04017 IkReal x930=((cj5)*(x925));
04018 IkReal x931=((IkReal(1.00000000000000))*(r11)*(r20));
04019 IkReal x932=((cj5)*(x924));
04020 gconst67=IKsign(((((r11)*(r22)*(x926)))+(((IkReal(-1.00000000000000))*(x930)*(x931)))+(((r10)*(r22)*(x929)))+(((IkReal(-1.00000000000000))*(x931)*(x932)))+(((x927)*(x932)))+(((x927)*(x930)))+(((IkReal(-1.00000000000000))*(r21)*(x926)*(x928)))+(((IkReal(-1.00000000000000))*(r20)*(x928)*(x929)))));
04021 IkReal x933=(sj6)*(sj6);
04022 IkReal x934=(cj6)*(cj6);
04023 IkReal x935=((cj6)*(sj5));
04024 IkReal x936=((r10)*(r21));
04025 IkReal x937=((IkReal(1.00000000000000))*(r12));
04026 IkReal x938=((sj5)*(sj6));
04027 IkReal x939=((cj5)*(x934));
04028 IkReal x940=((IkReal(1.00000000000000))*(r11)*(r20));
04029 IkReal x941=((cj5)*(x933));
04030 dummyeval[0]=((((x936)*(x939)))+(((r11)*(r22)*(x935)))+(((r10)*(r22)*(x938)))+(((x936)*(x941)))+(((IkReal(-1.00000000000000))*(x939)*(x940)))+(((IkReal(-1.00000000000000))*(r20)*(x937)*(x938)))+(((IkReal(-1.00000000000000))*(r21)*(x935)*(x937)))+(((IkReal(-1.00000000000000))*(x940)*(x941))));
04031 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04032 {
04033 {
04034 IkReal dummyeval[1];
04035 IkReal gconst68;
04036 IkReal x942=(sj6)*(sj6);
04037 IkReal x943=(cj6)*(cj6);
04038 IkReal x944=((r01)*(r10));
04039 IkReal x945=((IkReal(1.00000000000000))*(r00));
04040 IkReal x946=((r02)*(sj5));
04041 IkReal x947=((r12)*(sj5));
04042 IkReal x948=((cj5)*(x942));
04043 IkReal x949=((cj5)*(x943));
04044 gconst68=IKsign(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x947)))+(((IkReal(-1.00000000000000))*(r11)*(x945)*(x948)))+(((IkReal(-1.00000000000000))*(r11)*(x945)*(x949)))+(((r10)*(sj6)*(x946)))+(((IkReal(-1.00000000000000))*(sj6)*(x945)*(x947)))+(((cj6)*(r11)*(x946)))+(((x944)*(x949)))+(((x944)*(x948)))));
04045 IkReal x950=(sj6)*(sj6);
04046 IkReal x951=(cj6)*(cj6);
04047 IkReal x952=((r01)*(r10));
04048 IkReal x953=((IkReal(1.00000000000000))*(r00));
04049 IkReal x954=((r02)*(sj5));
04050 IkReal x955=((r12)*(sj5));
04051 IkReal x956=((cj5)*(x950));
04052 IkReal x957=((cj5)*(x951));
04053 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x953)*(x956)))+(((IkReal(-1.00000000000000))*(r11)*(x953)*(x957)))+(((x952)*(x957)))+(((x952)*(x956)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x955)))+(((IkReal(-1.00000000000000))*(sj6)*(x953)*(x955)))+(((r10)*(sj6)*(x954)))+(((cj6)*(r11)*(x954))));
04054 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04055 {
04056 continue;
04057 
04058 } else
04059 {
04060 {
04061 IkReal j4array[1], cj4array[1], sj4array[1];
04062 bool j4valid[1]={false};
04063 _nj4 = 1;
04064 IkReal x958=((sj3)*(sj6));
04065 IkReal x959=((IkReal(1.00000000000000))*(r10));
04066 IkReal x960=((cj6)*(sj3));
04067 if( IKabs(((gconst68)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x959)*(x960)))+(((cj5)*(r11)*(x958))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst68)*(((((IkReal(-1.00000000000000))*(r11)*(x960)))+(((IkReal(-1.00000000000000))*(x958)*(x959))))))) < IKFAST_ATAN2_MAGTHRESH )
04068     continue;
04069 j4array[0]=IKatan2(((gconst68)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x959)*(x960)))+(((cj5)*(r11)*(x958)))))), ((gconst68)*(((((IkReal(-1.00000000000000))*(r11)*(x960)))+(((IkReal(-1.00000000000000))*(x958)*(x959)))))));
04070 sj4array[0]=IKsin(j4array[0]);
04071 cj4array[0]=IKcos(j4array[0]);
04072 if( j4array[0] > IKPI )
04073 {
04074     j4array[0]-=IK2PI;
04075 }
04076 else if( j4array[0] < -IKPI )
04077 {    j4array[0]+=IK2PI;
04078 }
04079 j4valid[0] = true;
04080 for(int ij4 = 0; ij4 < 1; ++ij4)
04081 {
04082 if( !j4valid[ij4] )
04083 {
04084     continue;
04085 }
04086 _ij4[0] = ij4; _ij4[1] = -1;
04087 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04088 {
04089 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04090 {
04091     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04092 }
04093 }
04094 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04095 {
04096 IkReal evalcond[6];
04097 IkReal x961=IKsin(j4);
04098 IkReal x962=IKcos(j4);
04099 IkReal x963=((IkReal(1.00000000000000))*(r00));
04100 IkReal x964=((cj5)*(r01));
04101 IkReal x965=((cj5)*(r11));
04102 IkReal x966=((IkReal(1.00000000000000))*(cj5));
04103 IkReal x967=((cj5)*(r21));
04104 IkReal x968=((sj5)*(x961));
04105 IkReal x969=((cj6)*(x961));
04106 IkReal x970=((sj6)*(x962));
04107 IkReal x971=((sj6)*(x961));
04108 IkReal x972=((sj5)*(x962));
04109 IkReal x973=((cj6)*(x962));
04110 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x970)))+(((x967)*(x971)))+(((IkReal(-1.00000000000000))*(r21)*(x973)))+(((r22)*(x968)))+(((IkReal(-1.00000000000000))*(r20)*(x966)*(x969))));
04111 evalcond[1]=((cj3)+(((r22)*(x972)))+(((IkReal(-1.00000000000000))*(r20)*(x966)*(x973)))+(((r21)*(x969)))+(((r20)*(x971)))+(((x967)*(x970))));
04112 evalcond[2]=((((IkReal(-1.00000000000000))*(x963)*(x970)))+(((IkReal(-1.00000000000000))*(cj5)*(x963)*(x969)))+(((r02)*(x968)))+(((x964)*(x971)))+(((IkReal(-1.00000000000000))*(r01)*(x973))));
04113 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x966)*(x969)))+(((x965)*(x971)))+(((IkReal(-1.00000000000000))*(r11)*(x973)))+(((r12)*(x968)))+(((IkReal(-1.00000000000000))*(r10)*(x970))));
04114 evalcond[4]=((sj3)+(((r00)*(x971)))+(((r01)*(x969)))+(((IkReal(-1.00000000000000))*(cj5)*(x963)*(x973)))+(((x964)*(x970)))+(((r02)*(x972))));
04115 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x966)*(x973)))+(((x965)*(x970)))+(((r11)*(x969)))+(((r12)*(x972)))+(((r10)*(x971))));
04116 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04117 {
04118 continue;
04119 }
04120 }
04121 
04122 {
04123 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04124 vinfos[0].jointtype = 1;
04125 vinfos[0].foffset = j0;
04126 vinfos[0].indices[0] = _ij0[0];
04127 vinfos[0].indices[1] = _ij0[1];
04128 vinfos[0].maxsolutions = _nj0;
04129 vinfos[1].jointtype = 1;
04130 vinfos[1].foffset = j1;
04131 vinfos[1].indices[0] = _ij1[0];
04132 vinfos[1].indices[1] = _ij1[1];
04133 vinfos[1].maxsolutions = _nj1;
04134 vinfos[2].jointtype = 1;
04135 vinfos[2].foffset = j2;
04136 vinfos[2].indices[0] = _ij2[0];
04137 vinfos[2].indices[1] = _ij2[1];
04138 vinfos[2].maxsolutions = _nj2;
04139 vinfos[3].jointtype = 1;
04140 vinfos[3].foffset = j3;
04141 vinfos[3].indices[0] = _ij3[0];
04142 vinfos[3].indices[1] = _ij3[1];
04143 vinfos[3].maxsolutions = _nj3;
04144 vinfos[4].jointtype = 1;
04145 vinfos[4].foffset = j4;
04146 vinfos[4].indices[0] = _ij4[0];
04147 vinfos[4].indices[1] = _ij4[1];
04148 vinfos[4].maxsolutions = _nj4;
04149 vinfos[5].jointtype = 1;
04150 vinfos[5].foffset = j5;
04151 vinfos[5].indices[0] = _ij5[0];
04152 vinfos[5].indices[1] = _ij5[1];
04153 vinfos[5].maxsolutions = _nj5;
04154 vinfos[6].jointtype = 1;
04155 vinfos[6].foffset = j6;
04156 vinfos[6].indices[0] = _ij6[0];
04157 vinfos[6].indices[1] = _ij6[1];
04158 vinfos[6].maxsolutions = _nj6;
04159 std::vector<int> vfree(0);
04160 solutions.AddSolution(vinfos,vfree);
04161 }
04162 }
04163 }
04164 
04165 }
04166 
04167 }
04168 
04169 } else
04170 {
04171 {
04172 IkReal j4array[1], cj4array[1], sj4array[1];
04173 bool j4valid[1]={false};
04174 _nj4 = 1;
04175 IkReal x974=((cj3)*(sj6));
04176 IkReal x975=((IkReal(1.00000000000000))*(r10));
04177 IkReal x976=((cj3)*(cj6));
04178 if( IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(cj5)*(x975)*(x976)))+(((cj3)*(r12)*(sj5)))+(((cj5)*(r11)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(x974)*(x975)))+(((IkReal(-1.00000000000000))*(r11)*(x976))))))) < IKFAST_ATAN2_MAGTHRESH )
04179     continue;
04180 j4array[0]=IKatan2(((gconst67)*(((((IkReal(-1.00000000000000))*(cj5)*(x975)*(x976)))+(((cj3)*(r12)*(sj5)))+(((cj5)*(r11)*(x974)))))), ((gconst67)*(((((IkReal(-1.00000000000000))*(x974)*(x975)))+(((IkReal(-1.00000000000000))*(r11)*(x976)))))));
04181 sj4array[0]=IKsin(j4array[0]);
04182 cj4array[0]=IKcos(j4array[0]);
04183 if( j4array[0] > IKPI )
04184 {
04185     j4array[0]-=IK2PI;
04186 }
04187 else if( j4array[0] < -IKPI )
04188 {    j4array[0]+=IK2PI;
04189 }
04190 j4valid[0] = true;
04191 for(int ij4 = 0; ij4 < 1; ++ij4)
04192 {
04193 if( !j4valid[ij4] )
04194 {
04195     continue;
04196 }
04197 _ij4[0] = ij4; _ij4[1] = -1;
04198 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04199 {
04200 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04201 {
04202     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04203 }
04204 }
04205 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04206 {
04207 IkReal evalcond[6];
04208 IkReal x977=IKsin(j4);
04209 IkReal x978=IKcos(j4);
04210 IkReal x979=((IkReal(1.00000000000000))*(r00));
04211 IkReal x980=((cj5)*(r01));
04212 IkReal x981=((cj5)*(r11));
04213 IkReal x982=((IkReal(1.00000000000000))*(cj5));
04214 IkReal x983=((cj5)*(r21));
04215 IkReal x984=((sj5)*(x977));
04216 IkReal x985=((cj6)*(x977));
04217 IkReal x986=((sj6)*(x978));
04218 IkReal x987=((sj6)*(x977));
04219 IkReal x988=((sj5)*(x978));
04220 IkReal x989=((cj6)*(x978));
04221 evalcond[0]=((((x983)*(x987)))+(((IkReal(-1.00000000000000))*(r20)*(x982)*(x985)))+(((r22)*(x984)))+(((IkReal(-1.00000000000000))*(r20)*(x986)))+(((IkReal(-1.00000000000000))*(r21)*(x989))));
04222 evalcond[1]=((((x983)*(x986)))+(((IkReal(-1.00000000000000))*(r20)*(x982)*(x989)))+(((r22)*(x988)))+(cj3)+(((r21)*(x985)))+(((r20)*(x987))));
04223 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x979)*(x985)))+(((IkReal(-1.00000000000000))*(x979)*(x986)))+(((x980)*(x987)))+(((IkReal(-1.00000000000000))*(r01)*(x989)))+(((r02)*(x984))));
04224 evalcond[3]=((IkReal(1.00000000000000))+(((x981)*(x987)))+(((IkReal(-1.00000000000000))*(r10)*(x982)*(x985)))+(((IkReal(-1.00000000000000))*(r11)*(x989)))+(((r12)*(x984)))+(((IkReal(-1.00000000000000))*(r10)*(x986))));
04225 evalcond[4]=((sj3)+(((r00)*(x987)))+(((r01)*(x985)))+(((IkReal(-1.00000000000000))*(cj5)*(x979)*(x989)))+(((x980)*(x986)))+(((r02)*(x988))));
04226 evalcond[5]=((((x981)*(x986)))+(((IkReal(-1.00000000000000))*(r10)*(x982)*(x989)))+(((r12)*(x988)))+(((r11)*(x985)))+(((r10)*(x987))));
04227 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04228 {
04229 continue;
04230 }
04231 }
04232 
04233 {
04234 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04235 vinfos[0].jointtype = 1;
04236 vinfos[0].foffset = j0;
04237 vinfos[0].indices[0] = _ij0[0];
04238 vinfos[0].indices[1] = _ij0[1];
04239 vinfos[0].maxsolutions = _nj0;
04240 vinfos[1].jointtype = 1;
04241 vinfos[1].foffset = j1;
04242 vinfos[1].indices[0] = _ij1[0];
04243 vinfos[1].indices[1] = _ij1[1];
04244 vinfos[1].maxsolutions = _nj1;
04245 vinfos[2].jointtype = 1;
04246 vinfos[2].foffset = j2;
04247 vinfos[2].indices[0] = _ij2[0];
04248 vinfos[2].indices[1] = _ij2[1];
04249 vinfos[2].maxsolutions = _nj2;
04250 vinfos[3].jointtype = 1;
04251 vinfos[3].foffset = j3;
04252 vinfos[3].indices[0] = _ij3[0];
04253 vinfos[3].indices[1] = _ij3[1];
04254 vinfos[3].maxsolutions = _nj3;
04255 vinfos[4].jointtype = 1;
04256 vinfos[4].foffset = j4;
04257 vinfos[4].indices[0] = _ij4[0];
04258 vinfos[4].indices[1] = _ij4[1];
04259 vinfos[4].maxsolutions = _nj4;
04260 vinfos[5].jointtype = 1;
04261 vinfos[5].foffset = j5;
04262 vinfos[5].indices[0] = _ij5[0];
04263 vinfos[5].indices[1] = _ij5[1];
04264 vinfos[5].maxsolutions = _nj5;
04265 vinfos[6].jointtype = 1;
04266 vinfos[6].foffset = j6;
04267 vinfos[6].indices[0] = _ij6[0];
04268 vinfos[6].indices[1] = _ij6[1];
04269 vinfos[6].maxsolutions = _nj6;
04270 std::vector<int> vfree(0);
04271 solutions.AddSolution(vinfos,vfree);
04272 }
04273 }
04274 }
04275 
04276 }
04277 
04278 }
04279 }
04280 }
04281 
04282 } else
04283 {
04284 if( 1 )
04285 {
04286 continue;
04287 
04288 } else
04289 {
04290 }
04291 }
04292 }
04293 }
04294 
04295 } else
04296 {
04297 {
04298 IkReal j3array[1], cj3array[1], sj3array[1];
04299 bool j3valid[1]={false};
04300 _nj3 = 1;
04301 IkReal x990=((IkReal(1.00000000000000))*(sj5));
04302 if( IKabs(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r11)*(sj6)*(x990)))+(((cj5)*(r12)))+(((cj6)*(r10)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r01)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x990)))+(((IkReal(-1.00000000000000))*(cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r11)*(sj6)*(x990)))+(((cj5)*(r12)))+(((cj6)*(r10)*(sj5)))))))+IKsqr(((((r01)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x990)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
04303     continue;
04304 j3array[0]=IKatan2(((((IKabs(cj2) != 0)?((IkReal)1/(cj2)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r11)*(sj6)*(x990)))+(((cj5)*(r12)))+(((cj6)*(r10)*(sj5)))))), ((((r01)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj6)*(r00)*(x990)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))));
04305 sj3array[0]=IKsin(j3array[0]);
04306 cj3array[0]=IKcos(j3array[0]);
04307 if( j3array[0] > IKPI )
04308 {
04309     j3array[0]-=IK2PI;
04310 }
04311 else if( j3array[0] < -IKPI )
04312 {    j3array[0]+=IK2PI;
04313 }
04314 j3valid[0] = true;
04315 for(int ij3 = 0; ij3 < 1; ++ij3)
04316 {
04317 if( !j3valid[ij3] )
04318 {
04319     continue;
04320 }
04321 _ij3[0] = ij3; _ij3[1] = -1;
04322 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04323 {
04324 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04325 {
04326     j3valid[iij3]=false; _ij3[1] = iij3; break; 
04327 }
04328 }
04329 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04330 {
04331 IkReal evalcond[3];
04332 IkReal x991=IKsin(j3);
04333 IkReal x992=((cj6)*(sj5));
04334 IkReal x993=((IkReal(1.00000000000000))*(sj5)*(sj6));
04335 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x992)))+(((sj2)*(x991)))+(((IkReal(-1.00000000000000))*(r21)*(x993))));
04336 evalcond[1]=((((r00)*(x992)))+(IKcos(j3))+(((IkReal(-1.00000000000000))*(r01)*(x993)))+(((cj5)*(r02))));
04337 evalcond[2]=((((IkReal(-1.00000000000000))*(cj2)*(x991)))+(((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x993)))+(((r10)*(x992))));
04338 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
04339 {
04340 continue;
04341 }
04342 }
04343 
04344 {
04345 IkReal dummyeval[1];
04346 IkReal gconst59;
04347 IkReal x994=(cj6)*(cj6);
04348 IkReal x995=(cj5)*(cj5);
04349 IkReal x996=(r01)*(r01);
04350 IkReal x997=(r00)*(r00);
04351 IkReal x998=(sj6)*(sj6);
04352 IkReal x999=((cj6)*(r00));
04353 IkReal x1000=((IkReal(2.00000000000000))*(r01)*(sj6));
04354 IkReal x1001=((cj5)*(r02)*(sj5));
04355 gconst59=IKsign(((((x1000)*(x999)))+(((x994)*(x996)))+(((x1000)*(x1001)))+((((r02)*(r02))*((sj5)*(sj5))))+(((x997)*(x998)))+(((x995)*(x996)*(x998)))+(((x994)*(x995)*(x997)))+(((IkReal(-1.00000000000000))*(x1000)*(x995)*(x999)))+(((IkReal(-2.00000000000000))*(x1001)*(x999)))));
04356 IkReal x1002=(cj6)*(cj6);
04357 IkReal x1003=(cj5)*(cj5);
04358 IkReal x1004=(r01)*(r01);
04359 IkReal x1005=(r00)*(r00);
04360 IkReal x1006=(sj6)*(sj6);
04361 IkReal x1007=((cj6)*(r00));
04362 IkReal x1008=((IkReal(2.00000000000000))*(r01)*(sj6));
04363 IkReal x1009=((cj5)*(r02)*(sj5));
04364 dummyeval[0]=((((x1008)*(x1009)))+((((r02)*(r02))*((sj5)*(sj5))))+(((x1003)*(x1004)*(x1006)))+(((IkReal(-2.00000000000000))*(x1007)*(x1009)))+(((x1005)*(x1006)))+(((x1007)*(x1008)))+(((x1002)*(x1004)))+(((IkReal(-1.00000000000000))*(x1003)*(x1007)*(x1008)))+(((x1002)*(x1003)*(x1005))));
04365 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04366 {
04367 {
04368 IkReal dummyeval[1];
04369 IkReal gconst60;
04370 IkReal x1010=(sj6)*(sj6);
04371 IkReal x1011=(cj6)*(cj6);
04372 IkReal x1012=((r01)*(r10));
04373 IkReal x1013=((IkReal(1.00000000000000))*(r00));
04374 IkReal x1014=((r02)*(sj5));
04375 IkReal x1015=((r12)*(sj5));
04376 IkReal x1016=((cj5)*(x1010));
04377 IkReal x1017=((cj5)*(x1011));
04378 gconst60=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x1013)*(x1015)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1015)))+(((IkReal(-1.00000000000000))*(r11)*(x1013)*(x1017)))+(((IkReal(-1.00000000000000))*(r11)*(x1013)*(x1016)))+(((r10)*(sj6)*(x1014)))+(((x1012)*(x1016)))+(((x1012)*(x1017)))+(((cj6)*(r11)*(x1014)))));
04379 IkReal x1018=(sj6)*(sj6);
04380 IkReal x1019=(cj6)*(cj6);
04381 IkReal x1020=((r01)*(r10));
04382 IkReal x1021=((IkReal(1.00000000000000))*(r00));
04383 IkReal x1022=((r02)*(sj5));
04384 IkReal x1023=((r12)*(sj5));
04385 IkReal x1024=((cj5)*(x1018));
04386 IkReal x1025=((cj5)*(x1019));
04387 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x1021)*(x1024)))+(((IkReal(-1.00000000000000))*(r11)*(x1021)*(x1025)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1023)))+(((IkReal(-1.00000000000000))*(sj6)*(x1021)*(x1023)))+(((r10)*(sj6)*(x1022)))+(((cj6)*(r11)*(x1022)))+(((x1020)*(x1025)))+(((x1020)*(x1024))));
04388 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04389 {
04390 continue;
04391 
04392 } else
04393 {
04394 {
04395 IkReal j4array[1], cj4array[1], sj4array[1];
04396 bool j4valid[1]={false};
04397 _nj4 = 1;
04398 IkReal x1026=((cj2)*(cj3));
04399 IkReal x1027=((sj3)*(sj6));
04400 IkReal x1028=((IkReal(1.00000000000000))*(r10));
04401 IkReal x1029=((cj5)*(cj6));
04402 if( IKabs(((gconst60)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1026)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1026)))+(((IkReal(-1.00000000000000))*(sj3)*(x1028)*(x1029)))+(((cj5)*(r11)*(x1027)))+(((r00)*(x1026)*(x1029))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst60)*(((((r00)*(sj6)*(x1026)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3)))+(((cj6)*(r01)*(x1026)))+(((IkReal(-1.00000000000000))*(x1027)*(x1028))))))) < IKFAST_ATAN2_MAGTHRESH )
04403     continue;
04404 j4array[0]=IKatan2(((gconst60)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1026)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1026)))+(((IkReal(-1.00000000000000))*(sj3)*(x1028)*(x1029)))+(((cj5)*(r11)*(x1027)))+(((r00)*(x1026)*(x1029)))))), ((gconst60)*(((((r00)*(sj6)*(x1026)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3)))+(((cj6)*(r01)*(x1026)))+(((IkReal(-1.00000000000000))*(x1027)*(x1028)))))));
04405 sj4array[0]=IKsin(j4array[0]);
04406 cj4array[0]=IKcos(j4array[0]);
04407 if( j4array[0] > IKPI )
04408 {
04409     j4array[0]-=IK2PI;
04410 }
04411 else if( j4array[0] < -IKPI )
04412 {    j4array[0]+=IK2PI;
04413 }
04414 j4valid[0] = true;
04415 for(int ij4 = 0; ij4 < 1; ++ij4)
04416 {
04417 if( !j4valid[ij4] )
04418 {
04419     continue;
04420 }
04421 _ij4[0] = ij4; _ij4[1] = -1;
04422 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04423 {
04424 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04425 {
04426     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04427 }
04428 }
04429 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04430 {
04431 IkReal evalcond[6];
04432 IkReal x1030=IKsin(j4);
04433 IkReal x1031=IKcos(j4);
04434 IkReal x1032=((IkReal(1.00000000000000))*(r00));
04435 IkReal x1033=((cj5)*(r01));
04436 IkReal x1034=((cj5)*(r11));
04437 IkReal x1035=((IkReal(1.00000000000000))*(sj2));
04438 IkReal x1036=((IkReal(1.00000000000000))*(cj5));
04439 IkReal x1037=((cj5)*(r21));
04440 IkReal x1038=((sj5)*(x1030));
04441 IkReal x1039=((cj6)*(x1030));
04442 IkReal x1040=((sj6)*(x1031));
04443 IkReal x1041=((sj6)*(x1030));
04444 IkReal x1042=((sj5)*(x1031));
04445 IkReal x1043=((cj6)*(x1031));
04446 evalcond[0]=((((r22)*(x1038)))+(((x1037)*(x1041)))+(((IkReal(-1.00000000000000))*(r20)*(x1036)*(x1039)))+(((IkReal(-1.00000000000000))*(r21)*(x1043)))+(((IkReal(-1.00000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(r20)*(x1040))));
04447 evalcond[1]=((((x1037)*(x1040)))+(((r20)*(x1041)))+(((r21)*(x1039)))+(((IkReal(-1.00000000000000))*(r20)*(x1036)*(x1043)))+(((IkReal(-1.00000000000000))*(cj3)*(x1035)))+(((r22)*(x1042))));
04448 evalcond[2]=((((IkReal(-1.00000000000000))*(x1032)*(x1040)))+(((IkReal(-1.00000000000000))*(r01)*(x1043)))+(((r02)*(x1038)))+(((IkReal(-1.00000000000000))*(cj5)*(x1032)*(x1039)))+(((x1033)*(x1041))));
04449 evalcond[3]=((((IkReal(-1.00000000000000))*(x1035)))+(((x1034)*(x1041)))+(((IkReal(-1.00000000000000))*(r11)*(x1043)))+(((IkReal(-1.00000000000000))*(r10)*(x1040)))+(((r12)*(x1038)))+(((IkReal(-1.00000000000000))*(r10)*(x1036)*(x1039))));
04450 evalcond[4]=((((r02)*(x1042)))+(sj3)+(((IkReal(-1.00000000000000))*(cj5)*(x1032)*(x1043)))+(((r00)*(x1041)))+(((r01)*(x1039)))+(((x1033)*(x1040))));
04451 evalcond[5]=((((x1034)*(x1040)))+(((cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(r10)*(x1036)*(x1043)))+(((r10)*(x1041)))+(((r11)*(x1039)))+(((r12)*(x1042))));
04452 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04453 {
04454 continue;
04455 }
04456 }
04457 
04458 {
04459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04460 vinfos[0].jointtype = 1;
04461 vinfos[0].foffset = j0;
04462 vinfos[0].indices[0] = _ij0[0];
04463 vinfos[0].indices[1] = _ij0[1];
04464 vinfos[0].maxsolutions = _nj0;
04465 vinfos[1].jointtype = 1;
04466 vinfos[1].foffset = j1;
04467 vinfos[1].indices[0] = _ij1[0];
04468 vinfos[1].indices[1] = _ij1[1];
04469 vinfos[1].maxsolutions = _nj1;
04470 vinfos[2].jointtype = 1;
04471 vinfos[2].foffset = j2;
04472 vinfos[2].indices[0] = _ij2[0];
04473 vinfos[2].indices[1] = _ij2[1];
04474 vinfos[2].maxsolutions = _nj2;
04475 vinfos[3].jointtype = 1;
04476 vinfos[3].foffset = j3;
04477 vinfos[3].indices[0] = _ij3[0];
04478 vinfos[3].indices[1] = _ij3[1];
04479 vinfos[3].maxsolutions = _nj3;
04480 vinfos[4].jointtype = 1;
04481 vinfos[4].foffset = j4;
04482 vinfos[4].indices[0] = _ij4[0];
04483 vinfos[4].indices[1] = _ij4[1];
04484 vinfos[4].maxsolutions = _nj4;
04485 vinfos[5].jointtype = 1;
04486 vinfos[5].foffset = j5;
04487 vinfos[5].indices[0] = _ij5[0];
04488 vinfos[5].indices[1] = _ij5[1];
04489 vinfos[5].maxsolutions = _nj5;
04490 vinfos[6].jointtype = 1;
04491 vinfos[6].foffset = j6;
04492 vinfos[6].indices[0] = _ij6[0];
04493 vinfos[6].indices[1] = _ij6[1];
04494 vinfos[6].maxsolutions = _nj6;
04495 std::vector<int> vfree(0);
04496 solutions.AddSolution(vinfos,vfree);
04497 }
04498 }
04499 }
04500 
04501 }
04502 
04503 }
04504 
04505 } else
04506 {
04507 {
04508 IkReal j4array[1], cj4array[1], sj4array[1];
04509 bool j4valid[1]={false};
04510 _nj4 = 1;
04511 IkReal x1044=((cj5)*(sj3));
04512 IkReal x1045=((IkReal(1.00000000000000))*(sj3));
04513 if( IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1045)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1045))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1045)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1044)))+(((cj6)*(r00)*(x1044))))))) < IKFAST_ATAN2_MAGTHRESH )
04514     continue;
04515 j4array[0]=IKatan2(((gconst59)*(((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1045)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1045)))))), ((gconst59)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1045)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1044)))+(((cj6)*(r00)*(x1044)))))));
04516 sj4array[0]=IKsin(j4array[0]);
04517 cj4array[0]=IKcos(j4array[0]);
04518 if( j4array[0] > IKPI )
04519 {
04520     j4array[0]-=IK2PI;
04521 }
04522 else if( j4array[0] < -IKPI )
04523 {    j4array[0]+=IK2PI;
04524 }
04525 j4valid[0] = true;
04526 for(int ij4 = 0; ij4 < 1; ++ij4)
04527 {
04528 if( !j4valid[ij4] )
04529 {
04530     continue;
04531 }
04532 _ij4[0] = ij4; _ij4[1] = -1;
04533 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04534 {
04535 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04536 {
04537     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04538 }
04539 }
04540 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04541 {
04542 IkReal evalcond[6];
04543 IkReal x1046=IKsin(j4);
04544 IkReal x1047=IKcos(j4);
04545 IkReal x1048=((IkReal(1.00000000000000))*(r00));
04546 IkReal x1049=((cj5)*(r01));
04547 IkReal x1050=((cj5)*(r11));
04548 IkReal x1051=((IkReal(1.00000000000000))*(sj2));
04549 IkReal x1052=((IkReal(1.00000000000000))*(cj5));
04550 IkReal x1053=((cj5)*(r21));
04551 IkReal x1054=((sj5)*(x1046));
04552 IkReal x1055=((cj6)*(x1046));
04553 IkReal x1056=((sj6)*(x1047));
04554 IkReal x1057=((sj6)*(x1046));
04555 IkReal x1058=((sj5)*(x1047));
04556 IkReal x1059=((cj6)*(x1047));
04557 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1052)*(x1055)))+(((x1053)*(x1057)))+(((IkReal(-1.00000000000000))*(r21)*(x1059)))+(((r22)*(x1054)))+(((IkReal(-1.00000000000000))*(r20)*(x1056)))+(((IkReal(-1.00000000000000))*(cj2))));
04558 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1052)*(x1059)))+(((x1053)*(x1056)))+(((r20)*(x1057)))+(((r21)*(x1055)))+(((r22)*(x1058)))+(((IkReal(-1.00000000000000))*(cj3)*(x1051))));
04559 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1059)))+(((r02)*(x1054)))+(((x1049)*(x1057)))+(((IkReal(-1.00000000000000))*(x1048)*(x1056)))+(((IkReal(-1.00000000000000))*(cj5)*(x1048)*(x1055))));
04560 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x1052)*(x1055)))+(((IkReal(-1.00000000000000))*(x1051)))+(((r12)*(x1054)))+(((IkReal(-1.00000000000000))*(r11)*(x1059)))+(((IkReal(-1.00000000000000))*(r10)*(x1056)))+(((x1050)*(x1057))));
04561 evalcond[4]=((((r02)*(x1058)))+(sj3)+(((r00)*(x1057)))+(((r01)*(x1055)))+(((x1049)*(x1056)))+(((IkReal(-1.00000000000000))*(cj5)*(x1048)*(x1059))));
04562 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1052)*(x1059)))+(((r12)*(x1058)))+(((r11)*(x1055)))+(((cj2)*(cj3)))+(((x1050)*(x1056)))+(((r10)*(x1057))));
04563 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04564 {
04565 continue;
04566 }
04567 }
04568 
04569 {
04570 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04571 vinfos[0].jointtype = 1;
04572 vinfos[0].foffset = j0;
04573 vinfos[0].indices[0] = _ij0[0];
04574 vinfos[0].indices[1] = _ij0[1];
04575 vinfos[0].maxsolutions = _nj0;
04576 vinfos[1].jointtype = 1;
04577 vinfos[1].foffset = j1;
04578 vinfos[1].indices[0] = _ij1[0];
04579 vinfos[1].indices[1] = _ij1[1];
04580 vinfos[1].maxsolutions = _nj1;
04581 vinfos[2].jointtype = 1;
04582 vinfos[2].foffset = j2;
04583 vinfos[2].indices[0] = _ij2[0];
04584 vinfos[2].indices[1] = _ij2[1];
04585 vinfos[2].maxsolutions = _nj2;
04586 vinfos[3].jointtype = 1;
04587 vinfos[3].foffset = j3;
04588 vinfos[3].indices[0] = _ij3[0];
04589 vinfos[3].indices[1] = _ij3[1];
04590 vinfos[3].maxsolutions = _nj3;
04591 vinfos[4].jointtype = 1;
04592 vinfos[4].foffset = j4;
04593 vinfos[4].indices[0] = _ij4[0];
04594 vinfos[4].indices[1] = _ij4[1];
04595 vinfos[4].maxsolutions = _nj4;
04596 vinfos[5].jointtype = 1;
04597 vinfos[5].foffset = j5;
04598 vinfos[5].indices[0] = _ij5[0];
04599 vinfos[5].indices[1] = _ij5[1];
04600 vinfos[5].maxsolutions = _nj5;
04601 vinfos[6].jointtype = 1;
04602 vinfos[6].foffset = j6;
04603 vinfos[6].indices[0] = _ij6[0];
04604 vinfos[6].indices[1] = _ij6[1];
04605 vinfos[6].maxsolutions = _nj6;
04606 std::vector<int> vfree(0);
04607 solutions.AddSolution(vinfos,vfree);
04608 }
04609 }
04610 }
04611 
04612 }
04613 
04614 }
04615 }
04616 }
04617 
04618 }
04619 
04620 }
04621 
04622 } else
04623 {
04624 {
04625 IkReal j4array[1], cj4array[1], sj4array[1];
04626 bool j4valid[1]={false};
04627 _nj4 = 1;
04628 IkReal x1060=((IkReal(1.00000000000000))*(sj2));
04629 if( IKabs(((gconst58)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1060)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1060))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst58)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1060)))+(((cj5)*(cj6)*(r00)*(sj2)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1060))))))) < IKFAST_ATAN2_MAGTHRESH )
04630     continue;
04631 j4array[0]=IKatan2(((gconst58)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1060)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1060)))))), ((gconst58)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1060)))+(((cj5)*(cj6)*(r00)*(sj2)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1060)))))));
04632 sj4array[0]=IKsin(j4array[0]);
04633 cj4array[0]=IKcos(j4array[0]);
04634 if( j4array[0] > IKPI )
04635 {
04636     j4array[0]-=IK2PI;
04637 }
04638 else if( j4array[0] < -IKPI )
04639 {    j4array[0]+=IK2PI;
04640 }
04641 j4valid[0] = true;
04642 for(int ij4 = 0; ij4 < 1; ++ij4)
04643 {
04644 if( !j4valid[ij4] )
04645 {
04646     continue;
04647 }
04648 _ij4[0] = ij4; _ij4[1] = -1;
04649 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04650 {
04651 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04652 {
04653     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04654 }
04655 }
04656 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04657 {
04658 IkReal evalcond[3];
04659 IkReal x1061=IKsin(j4);
04660 IkReal x1062=IKcos(j4);
04661 IkReal x1063=((IkReal(1.00000000000000))*(cj6));
04662 IkReal x1064=((sj5)*(x1061));
04663 IkReal x1065=((cj5)*(x1061));
04664 IkReal x1066=((IkReal(1.00000000000000))*(sj6)*(x1062));
04665 evalcond[0]=((((r21)*(sj6)*(x1065)))+(((IkReal(-1.00000000000000))*(r20)*(x1066)))+(((IkReal(-1.00000000000000))*(r21)*(x1062)*(x1063)))+(((IkReal(-1.00000000000000))*(r20)*(x1063)*(x1065)))+(((r22)*(x1064)))+(((IkReal(-1.00000000000000))*(cj2))));
04666 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x1066)))+(((IkReal(-1.00000000000000))*(r00)*(x1063)*(x1065)))+(((r01)*(sj6)*(x1065)))+(((IkReal(-1.00000000000000))*(r01)*(x1062)*(x1063)))+(((r02)*(x1064))));
04667 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x1063)*(x1065)))+(((r12)*(x1064)))+(((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(r10)*(x1066)))+(((IkReal(-1.00000000000000))*(r11)*(x1062)*(x1063)))+(((r11)*(sj6)*(x1065))));
04668 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
04669 {
04670 continue;
04671 }
04672 }
04673 
04674 {
04675 IkReal j3array[1], cj3array[1], sj3array[1];
04676 bool j3valid[1]={false};
04677 _nj3 = 1;
04678 IkReal x1067=((IkReal(1.00000000000000))*(cj6));
04679 IkReal x1068=((IkReal(1.00000000000000))*(cj5));
04680 IkReal x1069=((r01)*(sj6));
04681 if( IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1067)))+(((IkReal(-1.00000000000000))*(cj4)*(x1068)*(x1069)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x1068)))+(((sj5)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1067))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1067)))+(((IkReal(-1.00000000000000))*(cj4)*(x1068)*(x1069)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x1068)))+(((sj5)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1067)))))-1) <= IKFAST_SINCOS_THRESH )
04682     continue;
04683 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1067)))+(((IkReal(-1.00000000000000))*(cj4)*(x1068)*(x1069)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))), ((((IkReal(-1.00000000000000))*(r02)*(x1068)))+(((sj5)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1067)))));
04684 sj3array[0]=IKsin(j3array[0]);
04685 cj3array[0]=IKcos(j3array[0]);
04686 if( j3array[0] > IKPI )
04687 {
04688     j3array[0]-=IK2PI;
04689 }
04690 else if( j3array[0] < -IKPI )
04691 {    j3array[0]+=IK2PI;
04692 }
04693 j3valid[0] = true;
04694 for(int ij3 = 0; ij3 < 1; ++ij3)
04695 {
04696 if( !j3valid[ij3] )
04697 {
04698     continue;
04699 }
04700 _ij3[0] = ij3; _ij3[1] = -1;
04701 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04702 {
04703 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04704 {
04705     j3valid[iij3]=false; _ij3[1] = iij3; break; 
04706 }
04707 }
04708 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04709 {
04710 IkReal evalcond[6];
04711 IkReal x1070=IKsin(j3);
04712 IkReal x1071=IKcos(j3);
04713 IkReal x1072=((r11)*(sj6));
04714 IkReal x1073=((IkReal(1.00000000000000))*(sj5));
04715 IkReal x1074=((sj4)*(sj6));
04716 IkReal x1075=((cj4)*(sj5));
04717 IkReal x1076=((cj6)*(sj4));
04718 IkReal x1077=((cj4)*(cj5));
04719 IkReal x1078=((r01)*(sj6));
04720 IkReal x1079=((r21)*(sj6));
04721 IkReal x1080=((cj6)*(sj5));
04722 IkReal x1081=((IkReal(1.00000000000000))*(cj6)*(x1077));
04723 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x1073)*(x1079)))+(((sj2)*(x1070)))+(((r20)*(x1080))));
04724 evalcond[1]=((x1071)+(((IkReal(-1.00000000000000))*(x1073)*(x1078)))+(((r00)*(x1080)))+(((cj5)*(r02))));
04725 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj2)*(x1070)))+(((r10)*(x1080)))+(((IkReal(-1.00000000000000))*(x1072)*(x1073))));
04726 evalcond[3]=((((x1077)*(x1079)))+(((IkReal(-1.00000000000000))*(sj2)*(x1071)))+(((r20)*(x1074)))+(((r22)*(x1075)))+(((r21)*(x1076)))+(((IkReal(-1.00000000000000))*(r20)*(x1081))));
04727 evalcond[4]=((((x1077)*(x1078)))+(x1070)+(((IkReal(-1.00000000000000))*(r00)*(x1081)))+(((r00)*(x1074)))+(((r01)*(x1076)))+(((r02)*(x1075))));
04728 evalcond[5]=((((r12)*(x1075)))+(((IkReal(-1.00000000000000))*(r10)*(x1081)))+(((cj2)*(x1071)))+(((r10)*(x1074)))+(((x1072)*(x1077)))+(((r11)*(x1076))));
04729 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04730 {
04731 continue;
04732 }
04733 }
04734 
04735 {
04736 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04737 vinfos[0].jointtype = 1;
04738 vinfos[0].foffset = j0;
04739 vinfos[0].indices[0] = _ij0[0];
04740 vinfos[0].indices[1] = _ij0[1];
04741 vinfos[0].maxsolutions = _nj0;
04742 vinfos[1].jointtype = 1;
04743 vinfos[1].foffset = j1;
04744 vinfos[1].indices[0] = _ij1[0];
04745 vinfos[1].indices[1] = _ij1[1];
04746 vinfos[1].maxsolutions = _nj1;
04747 vinfos[2].jointtype = 1;
04748 vinfos[2].foffset = j2;
04749 vinfos[2].indices[0] = _ij2[0];
04750 vinfos[2].indices[1] = _ij2[1];
04751 vinfos[2].maxsolutions = _nj2;
04752 vinfos[3].jointtype = 1;
04753 vinfos[3].foffset = j3;
04754 vinfos[3].indices[0] = _ij3[0];
04755 vinfos[3].indices[1] = _ij3[1];
04756 vinfos[3].maxsolutions = _nj3;
04757 vinfos[4].jointtype = 1;
04758 vinfos[4].foffset = j4;
04759 vinfos[4].indices[0] = _ij4[0];
04760 vinfos[4].indices[1] = _ij4[1];
04761 vinfos[4].maxsolutions = _nj4;
04762 vinfos[5].jointtype = 1;
04763 vinfos[5].foffset = j5;
04764 vinfos[5].indices[0] = _ij5[0];
04765 vinfos[5].indices[1] = _ij5[1];
04766 vinfos[5].maxsolutions = _nj5;
04767 vinfos[6].jointtype = 1;
04768 vinfos[6].foffset = j6;
04769 vinfos[6].indices[0] = _ij6[0];
04770 vinfos[6].indices[1] = _ij6[1];
04771 vinfos[6].maxsolutions = _nj6;
04772 std::vector<int> vfree(0);
04773 solutions.AddSolution(vinfos,vfree);
04774 }
04775 }
04776 }
04777 }
04778 }
04779 
04780 }
04781 
04782 }
04783 
04784 } else
04785 {
04786 {
04787 IkReal j4array[1], cj4array[1], sj4array[1];
04788 bool j4valid[1]={false};
04789 _nj4 = 1;
04790 IkReal x1082=((IkReal(1.00000000000000))*(cj2));
04791 IkReal x1083=((sj6)*(x1082));
04792 if( IKabs(((gconst57)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1082)))+(((IkReal(-1.00000000000000))*(r00)*(x1083))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst57)*(((((cj2)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1083)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1082))))))) < IKFAST_ATAN2_MAGTHRESH )
04793     continue;
04794 j4array[0]=IKatan2(((gconst57)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1082)))+(((IkReal(-1.00000000000000))*(r00)*(x1083)))))), ((gconst57)*(((((cj2)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1083)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1082)))))));
04795 sj4array[0]=IKsin(j4array[0]);
04796 cj4array[0]=IKcos(j4array[0]);
04797 if( j4array[0] > IKPI )
04798 {
04799     j4array[0]-=IK2PI;
04800 }
04801 else if( j4array[0] < -IKPI )
04802 {    j4array[0]+=IK2PI;
04803 }
04804 j4valid[0] = true;
04805 for(int ij4 = 0; ij4 < 1; ++ij4)
04806 {
04807 if( !j4valid[ij4] )
04808 {
04809     continue;
04810 }
04811 _ij4[0] = ij4; _ij4[1] = -1;
04812 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04813 {
04814 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04815 {
04816     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04817 }
04818 }
04819 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04820 {
04821 IkReal evalcond[3];
04822 IkReal x1084=IKsin(j4);
04823 IkReal x1085=IKcos(j4);
04824 IkReal x1086=((IkReal(1.00000000000000))*(cj6));
04825 IkReal x1087=((sj5)*(x1084));
04826 IkReal x1088=((cj5)*(x1084));
04827 IkReal x1089=((IkReal(1.00000000000000))*(sj6)*(x1085));
04828 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x1085)*(x1086)))+(((r21)*(sj6)*(x1088)))+(((IkReal(-1.00000000000000))*(r20)*(x1086)*(x1088)))+(((r22)*(x1087)))+(((IkReal(-1.00000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(r20)*(x1089))));
04829 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x1086)*(x1088)))+(((IkReal(-1.00000000000000))*(r00)*(x1089)))+(((IkReal(-1.00000000000000))*(r01)*(x1085)*(x1086)))+(((r02)*(x1087)))+(((r01)*(sj6)*(x1088))));
04830 evalcond[2]=((((IkReal(-1.00000000000000))*(sj2)))+(((r11)*(sj6)*(x1088)))+(((IkReal(-1.00000000000000))*(r10)*(x1086)*(x1088)))+(((IkReal(-1.00000000000000))*(r10)*(x1089)))+(((r12)*(x1087)))+(((IkReal(-1.00000000000000))*(r11)*(x1085)*(x1086))));
04831 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
04832 {
04833 continue;
04834 }
04835 }
04836 
04837 {
04838 IkReal j3array[1], cj3array[1], sj3array[1];
04839 bool j3valid[1]={false};
04840 _nj3 = 1;
04841 IkReal x1090=((IkReal(1.00000000000000))*(cj6));
04842 IkReal x1091=((IkReal(1.00000000000000))*(cj5));
04843 IkReal x1092=((r01)*(sj6));
04844 if( IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1090)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x1091)*(x1092)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x1091)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1090)))+(((sj5)*(x1092))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1090)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x1091)*(x1092)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x1091)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1090)))+(((sj5)*(x1092)))))-1) <= IKFAST_SINCOS_THRESH )
04845     continue;
04846 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x1090)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x1091)*(x1092)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))), ((((IkReal(-1.00000000000000))*(r02)*(x1091)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1090)))+(((sj5)*(x1092)))));
04847 sj3array[0]=IKsin(j3array[0]);
04848 cj3array[0]=IKcos(j3array[0]);
04849 if( j3array[0] > IKPI )
04850 {
04851     j3array[0]-=IK2PI;
04852 }
04853 else if( j3array[0] < -IKPI )
04854 {    j3array[0]+=IK2PI;
04855 }
04856 j3valid[0] = true;
04857 for(int ij3 = 0; ij3 < 1; ++ij3)
04858 {
04859 if( !j3valid[ij3] )
04860 {
04861     continue;
04862 }
04863 _ij3[0] = ij3; _ij3[1] = -1;
04864 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04865 {
04866 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04867 {
04868     j3valid[iij3]=false; _ij3[1] = iij3; break; 
04869 }
04870 }
04871 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04872 {
04873 IkReal evalcond[6];
04874 IkReal x1093=IKsin(j3);
04875 IkReal x1094=IKcos(j3);
04876 IkReal x1095=((r11)*(sj6));
04877 IkReal x1096=((IkReal(1.00000000000000))*(sj5));
04878 IkReal x1097=((sj4)*(sj6));
04879 IkReal x1098=((cj4)*(sj5));
04880 IkReal x1099=((cj6)*(sj4));
04881 IkReal x1100=((cj4)*(cj5));
04882 IkReal x1101=((r01)*(sj6));
04883 IkReal x1102=((r21)*(sj6));
04884 IkReal x1103=((cj6)*(sj5));
04885 IkReal x1104=((IkReal(1.00000000000000))*(cj6)*(x1100));
04886 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1103)))+(((sj2)*(x1093)))+(((IkReal(-1.00000000000000))*(x1096)*(x1102))));
04887 evalcond[1]=((x1094)+(((r00)*(x1103)))+(((IkReal(-1.00000000000000))*(x1096)*(x1101)))+(((cj5)*(r02))));
04888 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj2)*(x1093)))+(((IkReal(-1.00000000000000))*(x1095)*(x1096)))+(((r10)*(x1103))));
04889 evalcond[3]=((((r21)*(x1099)))+(((r22)*(x1098)))+(((IkReal(-1.00000000000000))*(r20)*(x1104)))+(((IkReal(-1.00000000000000))*(sj2)*(x1094)))+(((x1100)*(x1102)))+(((r20)*(x1097))));
04890 evalcond[4]=((x1093)+(((r01)*(x1099)))+(((IkReal(-1.00000000000000))*(r00)*(x1104)))+(((r00)*(x1097)))+(((r02)*(x1098)))+(((x1100)*(x1101))));
04891 evalcond[5]=((((r10)*(x1097)))+(((r11)*(x1099)))+(((r12)*(x1098)))+(((cj2)*(x1094)))+(((IkReal(-1.00000000000000))*(r10)*(x1104)))+(((x1095)*(x1100))));
04892 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
04893 {
04894 continue;
04895 }
04896 }
04897 
04898 {
04899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04900 vinfos[0].jointtype = 1;
04901 vinfos[0].foffset = j0;
04902 vinfos[0].indices[0] = _ij0[0];
04903 vinfos[0].indices[1] = _ij0[1];
04904 vinfos[0].maxsolutions = _nj0;
04905 vinfos[1].jointtype = 1;
04906 vinfos[1].foffset = j1;
04907 vinfos[1].indices[0] = _ij1[0];
04908 vinfos[1].indices[1] = _ij1[1];
04909 vinfos[1].maxsolutions = _nj1;
04910 vinfos[2].jointtype = 1;
04911 vinfos[2].foffset = j2;
04912 vinfos[2].indices[0] = _ij2[0];
04913 vinfos[2].indices[1] = _ij2[1];
04914 vinfos[2].maxsolutions = _nj2;
04915 vinfos[3].jointtype = 1;
04916 vinfos[3].foffset = j3;
04917 vinfos[3].indices[0] = _ij3[0];
04918 vinfos[3].indices[1] = _ij3[1];
04919 vinfos[3].maxsolutions = _nj3;
04920 vinfos[4].jointtype = 1;
04921 vinfos[4].foffset = j4;
04922 vinfos[4].indices[0] = _ij4[0];
04923 vinfos[4].indices[1] = _ij4[1];
04924 vinfos[4].maxsolutions = _nj4;
04925 vinfos[5].jointtype = 1;
04926 vinfos[5].foffset = j5;
04927 vinfos[5].indices[0] = _ij5[0];
04928 vinfos[5].indices[1] = _ij5[1];
04929 vinfos[5].maxsolutions = _nj5;
04930 vinfos[6].jointtype = 1;
04931 vinfos[6].foffset = j6;
04932 vinfos[6].indices[0] = _ij6[0];
04933 vinfos[6].indices[1] = _ij6[1];
04934 vinfos[6].maxsolutions = _nj6;
04935 std::vector<int> vfree(0);
04936 solutions.AddSolution(vinfos,vfree);
04937 }
04938 }
04939 }
04940 }
04941 }
04942 
04943 }
04944 
04945 }
04946 }
04947 }
04948 
04949 } else
04950 {
04951 IkReal x1105=((IkReal(0.390000000000000))*(sj5));
04952 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
04953 evalcond[1]=((IkReal(0.400000000000000))+(((cj6)*(r00)*(x1105)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1105)))+(((IkReal(0.000500000000000000))*(cj6)*(r01)))+(((IkReal(0.000500000000000000))*(r00)*(sj6)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(0.390000000000000))*(cj5)*(r02))));
04954 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  )
04955 {
04956 {
04957 IkReal j2array[1], cj2array[1], sj2array[1];
04958 bool j2valid[1]={false};
04959 _nj2 = 1;
04960 IkReal x1106=((IkReal(195.000000000000))*(cj5));
04961 IkReal x1107=((IkReal(0.250000000000000))*(cj6));
04962 IkReal x1108=((IkReal(0.250000000000000))*(sj6));
04963 IkReal x1109=((IkReal(195.000000000000))*(cj6)*(sj5));
04964 IkReal x1110=((IkReal(195.000000000000))*(sj5)*(sj6));
04965 if( IKabs(((((IkReal(-1.00000000000000))*(r11)*(x1107)))+(((IkReal(-1.00000000000000))*(r12)*(x1106)))+(((IkReal(500.000000000000))*(py)))+(((r11)*(x1110)))+(((IkReal(-1.00000000000000))*(r10)*(x1109)))+(((IkReal(-1.00000000000000))*(r10)*(x1108))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r20)*(x1109)))+(((r20)*(x1108)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(x1110)))+(((r22)*(x1106)))+(((r21)*(x1107))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r11)*(x1107)))+(((IkReal(-1.00000000000000))*(r12)*(x1106)))+(((IkReal(500.000000000000))*(py)))+(((r11)*(x1110)))+(((IkReal(-1.00000000000000))*(r10)*(x1109)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)))))+IKsqr(((((r20)*(x1109)))+(((r20)*(x1108)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(x1110)))+(((r22)*(x1106)))+(((r21)*(x1107)))))-1) <= IKFAST_SINCOS_THRESH )
04966     continue;
04967 j2array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r11)*(x1107)))+(((IkReal(-1.00000000000000))*(r12)*(x1106)))+(((IkReal(500.000000000000))*(py)))+(((r11)*(x1110)))+(((IkReal(-1.00000000000000))*(r10)*(x1109)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)))), ((((r20)*(x1109)))+(((r20)*(x1108)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(x1110)))+(((r22)*(x1106)))+(((r21)*(x1107)))));
04968 sj2array[0]=IKsin(j2array[0]);
04969 cj2array[0]=IKcos(j2array[0]);
04970 if( j2array[0] > IKPI )
04971 {
04972     j2array[0]-=IK2PI;
04973 }
04974 else if( j2array[0] < -IKPI )
04975 {    j2array[0]+=IK2PI;
04976 }
04977 j2valid[0] = true;
04978 for(int ij2 = 0; ij2 < 1; ++ij2)
04979 {
04980 if( !j2valid[ij2] )
04981 {
04982     continue;
04983 }
04984 _ij2[0] = ij2; _ij2[1] = -1;
04985 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
04986 {
04987 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
04988 {
04989     j2valid[iij2]=false; _ij2[1] = iij2; break; 
04990 }
04991 }
04992 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
04993 {
04994 IkReal evalcond[2];
04995 IkReal x1111=((IkReal(0.000500000000000000))*(cj6));
04996 IkReal x1112=((IkReal(0.000500000000000000))*(sj6));
04997 IkReal x1113=((IkReal(0.390000000000000))*(cj5));
04998 IkReal x1114=((IkReal(0.390000000000000))*(sj5)*(sj6));
04999 IkReal x1115=((IkReal(0.390000000000000))*(cj6)*(sj5));
05000 evalcond[0]=((((r20)*(x1115)))+(((r20)*(x1112)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((IkReal(-1.00000000000000))*(r21)*(x1114)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x1113)))+(((r21)*(x1111))));
05001 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x1114)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.00200000000000000))*(IKsin(j2))))+(((r12)*(x1113)))+(((r11)*(x1111)))+(((r10)*(x1115)))+(((r10)*(x1112))));
05002 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
05003 {
05004 continue;
05005 }
05006 }
05007 
05008 {
05009 IkReal dummyeval[1];
05010 IkReal gconst69;
05011 IkReal x1116=(sj6)*(sj6);
05012 IkReal x1117=(cj6)*(cj6);
05013 IkReal x1118=((cj6)*(sj5));
05014 IkReal x1119=((r00)*(r11));
05015 IkReal x1120=((IkReal(1.00000000000000))*(r02));
05016 IkReal x1121=((sj5)*(sj6));
05017 IkReal x1122=((cj5)*(x1116));
05018 IkReal x1123=((IkReal(1.00000000000000))*(r01)*(r10));
05019 IkReal x1124=((cj5)*(x1117));
05020 gconst69=IKsign(((((r01)*(r12)*(x1118)))+(((IkReal(-1.00000000000000))*(r11)*(x1118)*(x1120)))+(((IkReal(-1.00000000000000))*(x1122)*(x1123)))+(((r00)*(r12)*(x1121)))+(((x1119)*(x1122)))+(((x1119)*(x1124)))+(((IkReal(-1.00000000000000))*(x1123)*(x1124)))+(((IkReal(-1.00000000000000))*(r10)*(x1120)*(x1121)))));
05021 IkReal x1125=(sj6)*(sj6);
05022 IkReal x1126=(cj6)*(cj6);
05023 IkReal x1127=((cj6)*(sj5));
05024 IkReal x1128=((r00)*(r11));
05025 IkReal x1129=((IkReal(1.00000000000000))*(r02));
05026 IkReal x1130=((sj5)*(sj6));
05027 IkReal x1131=((cj5)*(x1125));
05028 IkReal x1132=((IkReal(1.00000000000000))*(r01)*(r10));
05029 IkReal x1133=((cj5)*(x1126));
05030 dummyeval[0]=((((r00)*(r12)*(x1130)))+(((IkReal(-1.00000000000000))*(x1131)*(x1132)))+(((IkReal(-1.00000000000000))*(r10)*(x1129)*(x1130)))+(((IkReal(-1.00000000000000))*(r11)*(x1127)*(x1129)))+(((IkReal(-1.00000000000000))*(x1132)*(x1133)))+(((x1128)*(x1133)))+(((x1128)*(x1131)))+(((r01)*(r12)*(x1127))));
05031 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05032 {
05033 {
05034 IkReal dummyeval[1];
05035 IkReal gconst70;
05036 IkReal x1134=(cj6)*(cj6);
05037 IkReal x1135=(sj6)*(sj6);
05038 IkReal x1136=((r01)*(r20));
05039 IkReal x1137=((r02)*(sj5));
05040 IkReal x1138=((cj5)*(x1135));
05041 IkReal x1139=((IkReal(1.00000000000000))*(r22)*(sj5));
05042 IkReal x1140=((IkReal(1.00000000000000))*(r00)*(r21));
05043 IkReal x1141=((cj5)*(x1134));
05044 gconst70=IKsign(((((IkReal(-1.00000000000000))*(x1140)*(x1141)))+(((r20)*(sj6)*(x1137)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1139)))+(((x1136)*(x1138)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1139)))+(((cj6)*(r21)*(x1137)))+(((x1136)*(x1141)))+(((IkReal(-1.00000000000000))*(x1138)*(x1140)))));
05045 IkReal x1142=(cj6)*(cj6);
05046 IkReal x1143=(sj6)*(sj6);
05047 IkReal x1144=((r01)*(r20));
05048 IkReal x1145=((r02)*(sj5));
05049 IkReal x1146=((cj5)*(x1143));
05050 IkReal x1147=((IkReal(1.00000000000000))*(r22)*(sj5));
05051 IkReal x1148=((IkReal(1.00000000000000))*(r00)*(r21));
05052 IkReal x1149=((cj5)*(x1142));
05053 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1146)*(x1148)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1147)))+(((IkReal(-1.00000000000000))*(x1148)*(x1149)))+(((x1144)*(x1149)))+(((x1144)*(x1146)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1147)))+(((cj6)*(r21)*(x1145)))+(((r20)*(sj6)*(x1145))));
05054 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05055 {
05056 {
05057 IkReal dummyeval[1];
05058 dummyeval[0]=sj2;
05059 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05060 {
05061 {
05062 IkReal evalcond[5];
05063 IkReal x1150=((IkReal(0.000500000000000000))*(cj6));
05064 IkReal x1151=((IkReal(0.390000000000000))*(sj5));
05065 IkReal x1152=((cj6)*(r20));
05066 IkReal x1153=((IkReal(0.000500000000000000))*(sj6));
05067 IkReal x1154=((cj5)*(r22));
05068 IkReal x1155=((r21)*(sj6));
05069 IkReal x1156=((IkReal(0.390000000000000))*(cj5));
05070 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
05071 evalcond[1]=((x1154)+(((IkReal(-1.00000000000000))*(sj5)*(x1155)))+(((sj5)*(x1152))));
05072 evalcond[2]=((IkReal(-0.00200000000000000))+(((r20)*(x1153)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(0.390000000000000))*(x1154)))+(((x1151)*(x1152)))+(((r21)*(x1150)))+(((IkReal(-1.00000000000000))*(x1151)*(x1155))));
05073 evalcond[3]=((IkReal(0.400000000000000))+(((r00)*(x1153)))+(((r02)*(x1156)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1151)))+(((cj6)*(r00)*(x1151)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x1150))));
05074 evalcond[4]=((((r11)*(x1150)))+(((r12)*(x1156)))+(((cj6)*(r10)*(x1151)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x1151)))+(((r10)*(x1153))));
05075 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
05076 {
05077 {
05078 IkReal j3array[1], cj3array[1], sj3array[1];
05079 bool j3valid[1]={false};
05080 _nj3 = 1;
05081 IkReal x1157=((IkReal(1.00000000000000))*(sj5));
05082 if( IKabs(((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x1157)))+(((r11)*(sj5)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1157)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x1157)))+(((r11)*(sj5)*(sj6)))))+IKsqr(((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1157)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
05083     continue;
05084 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj6)*(r10)*(x1157)))+(((r11)*(sj5)*(sj6)))), ((((cj6)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1157)))+(((cj5)*(r02)))));
05085 sj3array[0]=IKsin(j3array[0]);
05086 cj3array[0]=IKcos(j3array[0]);
05087 if( j3array[0] > IKPI )
05088 {
05089     j3array[0]-=IK2PI;
05090 }
05091 else if( j3array[0] < -IKPI )
05092 {    j3array[0]+=IK2PI;
05093 }
05094 j3valid[0] = true;
05095 for(int ij3 = 0; ij3 < 1; ++ij3)
05096 {
05097 if( !j3valid[ij3] )
05098 {
05099     continue;
05100 }
05101 _ij3[0] = ij3; _ij3[1] = -1;
05102 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05103 {
05104 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05105 {
05106     j3valid[iij3]=false; _ij3[1] = iij3; break; 
05107 }
05108 }
05109 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05110 {
05111 IkReal evalcond[2];
05112 IkReal x1158=((cj6)*(sj5));
05113 IkReal x1159=((IkReal(1.00000000000000))*(sj5)*(sj6));
05114 evalcond[0]=((((r00)*(x1158)))+(((IkReal(-1.00000000000000))*(r01)*(x1159)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((cj5)*(r02))));
05115 evalcond[1]=((((cj5)*(r12)))+(IKsin(j3))+(((IkReal(-1.00000000000000))*(r11)*(x1159)))+(((r10)*(x1158))));
05116 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
05117 {
05118 continue;
05119 }
05120 }
05121 
05122 {
05123 IkReal dummyeval[1];
05124 IkReal gconst76;
05125 IkReal x1160=(sj6)*(sj6);
05126 IkReal x1161=(cj6)*(cj6);
05127 IkReal x1162=((IkReal(1.00000000000000))*(r20));
05128 IkReal x1163=((r00)*(r21));
05129 IkReal x1164=((r22)*(sj5));
05130 IkReal x1165=((r02)*(sj5));
05131 IkReal x1166=((cj5)*(x1160));
05132 IkReal x1167=((cj5)*(x1161));
05133 gconst76=IKsign(((((IkReal(-1.00000000000000))*(r01)*(x1162)*(x1167)))+(((IkReal(-1.00000000000000))*(r01)*(x1162)*(x1166)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1165)))+(((x1163)*(x1167)))+(((x1163)*(x1166)))+(((cj6)*(r01)*(x1164)))+(((r00)*(sj6)*(x1164)))+(((IkReal(-1.00000000000000))*(sj6)*(x1162)*(x1165)))));
05134 IkReal x1168=(sj6)*(sj6);
05135 IkReal x1169=(cj6)*(cj6);
05136 IkReal x1170=((IkReal(1.00000000000000))*(r20));
05137 IkReal x1171=((r00)*(r21));
05138 IkReal x1172=((r22)*(sj5));
05139 IkReal x1173=((r02)*(sj5));
05140 IkReal x1174=((cj5)*(x1168));
05141 IkReal x1175=((cj5)*(x1169));
05142 dummyeval[0]=((((IkReal(-1.00000000000000))*(r01)*(x1170)*(x1175)))+(((IkReal(-1.00000000000000))*(r01)*(x1170)*(x1174)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1173)))+(((cj6)*(r01)*(x1172)))+(((r00)*(sj6)*(x1172)))+(((IkReal(-1.00000000000000))*(sj6)*(x1170)*(x1173)))+(((x1171)*(x1175)))+(((x1171)*(x1174))));
05143 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05144 {
05145 {
05146 IkReal dummyeval[1];
05147 IkReal gconst75;
05148 IkReal x1176=(r20)*(r20);
05149 IkReal x1177=(cj5)*(cj5);
05150 IkReal x1178=(sj6)*(sj6);
05151 IkReal x1179=(cj6)*(cj6);
05152 IkReal x1180=(r21)*(r21);
05153 IkReal x1181=((cj6)*(r20));
05154 IkReal x1182=((IkReal(2.00000000000000))*(r21)*(sj6));
05155 IkReal x1183=((cj5)*(r22)*(sj5));
05156 gconst75=IKsign(((((IkReal(-1.00000000000000))*(x1177)*(x1181)*(x1182)))+(((x1177)*(x1178)*(x1180)))+(((x1182)*(x1183)))+(((x1179)*(x1180)))+(((x1176)*(x1178)))+(((x1181)*(x1182)))+(((IkReal(-2.00000000000000))*(x1181)*(x1183)))+(((x1176)*(x1177)*(x1179)))+((((r22)*(r22))*((sj5)*(sj5))))));
05157 IkReal x1184=(r20)*(r20);
05158 IkReal x1185=(cj5)*(cj5);
05159 IkReal x1186=(sj6)*(sj6);
05160 IkReal x1187=(cj6)*(cj6);
05161 IkReal x1188=(r21)*(r21);
05162 IkReal x1189=((cj6)*(r20));
05163 IkReal x1190=((IkReal(2.00000000000000))*(r21)*(sj6));
05164 IkReal x1191=((cj5)*(r22)*(sj5));
05165 dummyeval[0]=((((x1184)*(x1185)*(x1187)))+(((x1187)*(x1188)))+(((x1184)*(x1186)))+(((IkReal(-1.00000000000000))*(x1185)*(x1189)*(x1190)))+(((IkReal(-2.00000000000000))*(x1189)*(x1191)))+(((x1189)*(x1190)))+(((x1185)*(x1186)*(x1188)))+(((x1190)*(x1191)))+((((r22)*(r22))*((sj5)*(sj5)))));
05166 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05167 {
05168 continue;
05169 
05170 } else
05171 {
05172 {
05173 IkReal j4array[1], cj4array[1], sj4array[1];
05174 bool j4valid[1]={false};
05175 _nj4 = 1;
05176 IkReal x1192=((IkReal(1.00000000000000))*(r20));
05177 if( IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1192)))+(((cj5)*(r21)*(sj6)))+(((r22)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(sj6)*(x1192)))+(((IkReal(-1.00000000000000))*(cj6)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH )
05178     continue;
05179 j4array[0]=IKatan2(((gconst75)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1192)))+(((cj5)*(r21)*(sj6)))+(((r22)*(sj5)))))), ((gconst75)*(((((IkReal(-1.00000000000000))*(sj6)*(x1192)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)))))));
05180 sj4array[0]=IKsin(j4array[0]);
05181 cj4array[0]=IKcos(j4array[0]);
05182 if( j4array[0] > IKPI )
05183 {
05184     j4array[0]-=IK2PI;
05185 }
05186 else if( j4array[0] < -IKPI )
05187 {    j4array[0]+=IK2PI;
05188 }
05189 j4valid[0] = true;
05190 for(int ij4 = 0; ij4 < 1; ++ij4)
05191 {
05192 if( !j4valid[ij4] )
05193 {
05194     continue;
05195 }
05196 _ij4[0] = ij4; _ij4[1] = -1;
05197 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05198 {
05199 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05200 {
05201     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05202 }
05203 }
05204 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05205 {
05206 IkReal evalcond[6];
05207 IkReal x1193=IKsin(j4);
05208 IkReal x1194=IKcos(j4);
05209 IkReal x1195=((IkReal(1.00000000000000))*(r00));
05210 IkReal x1196=((cj5)*(r01));
05211 IkReal x1197=((cj5)*(r11));
05212 IkReal x1198=((IkReal(1.00000000000000))*(cj5));
05213 IkReal x1199=((cj5)*(r21));
05214 IkReal x1200=((sj5)*(x1193));
05215 IkReal x1201=((cj6)*(x1193));
05216 IkReal x1202=((sj6)*(x1194));
05217 IkReal x1203=((sj6)*(x1193));
05218 IkReal x1204=((sj5)*(x1194));
05219 IkReal x1205=((cj6)*(x1194));
05220 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r21)*(x1205)))+(((x1199)*(x1203)))+(((IkReal(-1.00000000000000))*(r20)*(x1198)*(x1201)))+(((IkReal(-1.00000000000000))*(r20)*(x1202)))+(((r22)*(x1200))));
05221 evalcond[1]=((((x1199)*(x1202)))+(((r21)*(x1201)))+(((r20)*(x1203)))+(((IkReal(-1.00000000000000))*(r20)*(x1198)*(x1205)))+(((r22)*(x1204))));
05222 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x1195)*(x1201)))+(((r02)*(x1200)))+(((IkReal(-1.00000000000000))*(x1195)*(x1202)))+(((x1196)*(x1203)))+(((IkReal(-1.00000000000000))*(r01)*(x1205))));
05223 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x1202)))+(((r12)*(x1200)))+(((IkReal(-1.00000000000000))*(r10)*(x1198)*(x1201)))+(((x1197)*(x1203)))+(((IkReal(-1.00000000000000))*(r11)*(x1205))));
05224 evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x1195)*(x1205)))+(((IkReal(-1.00000000000000))*(sj3)))+(((r01)*(x1201)))+(((r00)*(x1203)))+(((r02)*(x1204)))+(((x1196)*(x1202))));
05225 evalcond[5]=((((r10)*(x1203)))+(((r11)*(x1201)))+(((r12)*(x1204)))+(((IkReal(-1.00000000000000))*(r10)*(x1198)*(x1205)))+(((x1197)*(x1202)))+(((IkReal(-1.00000000000000))*(cj3))));
05226 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
05227 {
05228 continue;
05229 }
05230 }
05231 
05232 {
05233 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
05234 vinfos[0].jointtype = 1;
05235 vinfos[0].foffset = j0;
05236 vinfos[0].indices[0] = _ij0[0];
05237 vinfos[0].indices[1] = _ij0[1];
05238 vinfos[0].maxsolutions = _nj0;
05239 vinfos[1].jointtype = 1;
05240 vinfos[1].foffset = j1;
05241 vinfos[1].indices[0] = _ij1[0];
05242 vinfos[1].indices[1] = _ij1[1];
05243 vinfos[1].maxsolutions = _nj1;
05244 vinfos[2].jointtype = 1;
05245 vinfos[2].foffset = j2;
05246 vinfos[2].indices[0] = _ij2[0];
05247 vinfos[2].indices[1] = _ij2[1];
05248 vinfos[2].maxsolutions = _nj2;
05249 vinfos[3].jointtype = 1;
05250 vinfos[3].foffset = j3;
05251 vinfos[3].indices[0] = _ij3[0];
05252 vinfos[3].indices[1] = _ij3[1];
05253 vinfos[3].maxsolutions = _nj3;
05254 vinfos[4].jointtype = 1;
05255 vinfos[4].foffset = j4;
05256 vinfos[4].indices[0] = _ij4[0];
05257 vinfos[4].indices[1] = _ij4[1];
05258 vinfos[4].maxsolutions = _nj4;
05259 vinfos[5].jointtype = 1;
05260 vinfos[5].foffset = j5;
05261 vinfos[5].indices[0] = _ij5[0];
05262 vinfos[5].indices[1] = _ij5[1];
05263 vinfos[5].maxsolutions = _nj5;
05264 vinfos[6].jointtype = 1;
05265 vinfos[6].foffset = j6;
05266 vinfos[6].indices[0] = _ij6[0];
05267 vinfos[6].indices[1] = _ij6[1];
05268 vinfos[6].maxsolutions = _nj6;
05269 std::vector<int> vfree(0);
05270 solutions.AddSolution(vinfos,vfree);
05271 }
05272 }
05273 }
05274 
05275 }
05276 
05277 }
05278 
05279 } else
05280 {
05281 {
05282 IkReal j4array[1], cj4array[1], sj4array[1];
05283 bool j4valid[1]={false};
05284 _nj4 = 1;
05285 IkReal x1206=((IkReal(1.00000000000000))*(sj3));
05286 if( IKabs(((gconst76)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x1206)))+(((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst76)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1206)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1206))))))) < IKFAST_ATAN2_MAGTHRESH )
05287     continue;
05288 j4array[0]=IKatan2(((gconst76)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(r20)*(x1206)))+(((cj5)*(r21)*(sj3)*(sj6)))+(((r22)*(sj3)*(sj5)))))), ((gconst76)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1206)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1206)))))));
05289 sj4array[0]=IKsin(j4array[0]);
05290 cj4array[0]=IKcos(j4array[0]);
05291 if( j4array[0] > IKPI )
05292 {
05293     j4array[0]-=IK2PI;
05294 }
05295 else if( j4array[0] < -IKPI )
05296 {    j4array[0]+=IK2PI;
05297 }
05298 j4valid[0] = true;
05299 for(int ij4 = 0; ij4 < 1; ++ij4)
05300 {
05301 if( !j4valid[ij4] )
05302 {
05303     continue;
05304 }
05305 _ij4[0] = ij4; _ij4[1] = -1;
05306 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05307 {
05308 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05309 {
05310     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05311 }
05312 }
05313 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05314 {
05315 IkReal evalcond[6];
05316 IkReal x1207=IKsin(j4);
05317 IkReal x1208=IKcos(j4);
05318 IkReal x1209=((IkReal(1.00000000000000))*(r00));
05319 IkReal x1210=((cj5)*(r01));
05320 IkReal x1211=((cj5)*(r11));
05321 IkReal x1212=((IkReal(1.00000000000000))*(cj5));
05322 IkReal x1213=((cj5)*(r21));
05323 IkReal x1214=((sj5)*(x1207));
05324 IkReal x1215=((cj6)*(x1207));
05325 IkReal x1216=((sj6)*(x1208));
05326 IkReal x1217=((sj6)*(x1207));
05327 IkReal x1218=((sj5)*(x1208));
05328 IkReal x1219=((cj6)*(x1208));
05329 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r20)*(x1216)))+(((IkReal(-1.00000000000000))*(r20)*(x1212)*(x1215)))+(((r22)*(x1214)))+(((x1213)*(x1217)))+(((IkReal(-1.00000000000000))*(r21)*(x1219))));
05330 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1212)*(x1219)))+(((r22)*(x1218)))+(((x1213)*(x1216)))+(((r20)*(x1217)))+(((r21)*(x1215))));
05331 evalcond[2]=((((IkReal(-1.00000000000000))*(x1209)*(x1216)))+(((x1210)*(x1217)))+(((r02)*(x1214)))+(((IkReal(-1.00000000000000))*(cj5)*(x1209)*(x1215)))+(((IkReal(-1.00000000000000))*(r01)*(x1219))));
05332 evalcond[3]=((((r12)*(x1214)))+(((IkReal(-1.00000000000000))*(r10)*(x1212)*(x1215)))+(((IkReal(-1.00000000000000))*(r11)*(x1219)))+(((x1211)*(x1217)))+(((IkReal(-1.00000000000000))*(r10)*(x1216))));
05333 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((x1210)*(x1216)))+(((r02)*(x1218)))+(((r00)*(x1217)))+(((r01)*(x1215)))+(((IkReal(-1.00000000000000))*(cj5)*(x1209)*(x1219))));
05334 evalcond[5]=((((r12)*(x1218)))+(((r11)*(x1215)))+(((IkReal(-1.00000000000000))*(r10)*(x1212)*(x1219)))+(((r10)*(x1217)))+(((x1211)*(x1216)))+(((IkReal(-1.00000000000000))*(cj3))));
05335 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
05336 {
05337 continue;
05338 }
05339 }
05340 
05341 {
05342 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
05343 vinfos[0].jointtype = 1;
05344 vinfos[0].foffset = j0;
05345 vinfos[0].indices[0] = _ij0[0];
05346 vinfos[0].indices[1] = _ij0[1];
05347 vinfos[0].maxsolutions = _nj0;
05348 vinfos[1].jointtype = 1;
05349 vinfos[1].foffset = j1;
05350 vinfos[1].indices[0] = _ij1[0];
05351 vinfos[1].indices[1] = _ij1[1];
05352 vinfos[1].maxsolutions = _nj1;
05353 vinfos[2].jointtype = 1;
05354 vinfos[2].foffset = j2;
05355 vinfos[2].indices[0] = _ij2[0];
05356 vinfos[2].indices[1] = _ij2[1];
05357 vinfos[2].maxsolutions = _nj2;
05358 vinfos[3].jointtype = 1;
05359 vinfos[3].foffset = j3;
05360 vinfos[3].indices[0] = _ij3[0];
05361 vinfos[3].indices[1] = _ij3[1];
05362 vinfos[3].maxsolutions = _nj3;
05363 vinfos[4].jointtype = 1;
05364 vinfos[4].foffset = j4;
05365 vinfos[4].indices[0] = _ij4[0];
05366 vinfos[4].indices[1] = _ij4[1];
05367 vinfos[4].maxsolutions = _nj4;
05368 vinfos[5].jointtype = 1;
05369 vinfos[5].foffset = j5;
05370 vinfos[5].indices[0] = _ij5[0];
05371 vinfos[5].indices[1] = _ij5[1];
05372 vinfos[5].maxsolutions = _nj5;
05373 vinfos[6].jointtype = 1;
05374 vinfos[6].foffset = j6;
05375 vinfos[6].indices[0] = _ij6[0];
05376 vinfos[6].indices[1] = _ij6[1];
05377 vinfos[6].maxsolutions = _nj6;
05378 std::vector<int> vfree(0);
05379 solutions.AddSolution(vinfos,vfree);
05380 }
05381 }
05382 }
05383 
05384 }
05385 
05386 }
05387 }
05388 }
05389 
05390 } else
05391 {
05392 IkReal x1220=((IkReal(0.000500000000000000))*(cj6));
05393 IkReal x1221=((IkReal(0.390000000000000))*(sj5));
05394 IkReal x1222=((cj6)*(r20));
05395 IkReal x1223=((IkReal(0.000500000000000000))*(sj6));
05396 IkReal x1224=((cj5)*(r22));
05397 IkReal x1225=((r21)*(sj6));
05398 IkReal x1226=((IkReal(0.390000000000000))*(cj5));
05399 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
05400 evalcond[1]=((x1224)+(((sj5)*(x1222)))+(((IkReal(-1.00000000000000))*(sj5)*(x1225))));
05401 evalcond[2]=((IkReal(0.00200000000000000))+(((r20)*(x1223)))+(((IkReal(-1.00000000000000))*(x1221)*(x1225)))+(((IkReal(-1.00000000000000))*(pz)))+(((x1221)*(x1222)))+(((IkReal(0.390000000000000))*(x1224)))+(((r21)*(x1220))));
05402 evalcond[3]=((IkReal(0.400000000000000))+(((r01)*(x1220)))+(((r00)*(x1223)))+(((cj6)*(r00)*(x1221)))+(((r02)*(x1226)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1221))));
05403 evalcond[4]=((((IkReal(-1.00000000000000))*(py)))+(((r10)*(x1223)))+(((cj6)*(r10)*(x1221)))+(((r11)*(x1220)))+(((r12)*(x1226)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x1221))));
05404 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
05405 {
05406 {
05407 IkReal j3array[1], cj3array[1], sj3array[1];
05408 bool j3valid[1]={false};
05409 _nj3 = 1;
05410 IkReal x1227=((cj6)*(sj5));
05411 IkReal x1228=((IkReal(1.00000000000000))*(sj5)*(sj6));
05412 if( IKabs(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x1228)))+(((r10)*(x1227))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r01)*(x1228)))+(((r00)*(x1227)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x1228)))+(((r10)*(x1227)))))+IKsqr(((((IkReal(-1.00000000000000))*(r01)*(x1228)))+(((r00)*(x1227)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
05413     continue;
05414 j3array[0]=IKatan2(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x1228)))+(((r10)*(x1227)))), ((((IkReal(-1.00000000000000))*(r01)*(x1228)))+(((r00)*(x1227)))+(((cj5)*(r02)))));
05415 sj3array[0]=IKsin(j3array[0]);
05416 cj3array[0]=IKcos(j3array[0]);
05417 if( j3array[0] > IKPI )
05418 {
05419     j3array[0]-=IK2PI;
05420 }
05421 else if( j3array[0] < -IKPI )
05422 {    j3array[0]+=IK2PI;
05423 }
05424 j3valid[0] = true;
05425 for(int ij3 = 0; ij3 < 1; ++ij3)
05426 {
05427 if( !j3valid[ij3] )
05428 {
05429     continue;
05430 }
05431 _ij3[0] = ij3; _ij3[1] = -1;
05432 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05433 {
05434 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05435 {
05436     j3valid[iij3]=false; _ij3[1] = iij3; break; 
05437 }
05438 }
05439 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05440 {
05441 IkReal evalcond[2];
05442 IkReal x1229=((cj6)*(sj5));
05443 IkReal x1230=((IkReal(1.00000000000000))*(sj5)*(sj6));
05444 evalcond[0]=((((IkReal(-1.00000000000000))*(r01)*(x1230)))+(((r00)*(x1229)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((cj5)*(r02))));
05445 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x1230)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((r10)*(x1229))));
05446 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
05447 {
05448 continue;
05449 }
05450 }
05451 
05452 {
05453 IkReal dummyeval[1];
05454 IkReal gconst79;
05455 IkReal x1231=(cj6)*(cj6);
05456 IkReal x1232=(sj6)*(sj6);
05457 IkReal x1233=((r11)*(r20));
05458 IkReal x1234=((r12)*(sj5));
05459 IkReal x1235=((IkReal(1.00000000000000))*(r10));
05460 IkReal x1236=((r22)*(sj5));
05461 IkReal x1237=((cj5)*(x1232));
05462 IkReal x1238=((cj5)*(x1231));
05463 gconst79=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x1235)*(x1236)))+(((cj6)*(r21)*(x1234)))+(((r20)*(sj6)*(x1234)))+(((x1233)*(x1237)))+(((x1233)*(x1238)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1236)))+(((IkReal(-1.00000000000000))*(r21)*(x1235)*(x1237)))+(((IkReal(-1.00000000000000))*(r21)*(x1235)*(x1238)))));
05464 IkReal x1239=(cj6)*(cj6);
05465 IkReal x1240=(sj6)*(sj6);
05466 IkReal x1241=((r11)*(r20));
05467 IkReal x1242=((r12)*(sj5));
05468 IkReal x1243=((IkReal(1.00000000000000))*(r10));
05469 IkReal x1244=((r22)*(sj5));
05470 IkReal x1245=((cj5)*(x1240));
05471 IkReal x1246=((cj5)*(x1239));
05472 dummyeval[0]=((((IkReal(-1.00000000000000))*(r21)*(x1243)*(x1246)))+(((IkReal(-1.00000000000000))*(r21)*(x1243)*(x1245)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1244)))+(((cj6)*(r21)*(x1242)))+(((r20)*(sj6)*(x1242)))+(((x1241)*(x1246)))+(((x1241)*(x1245)))+(((IkReal(-1.00000000000000))*(sj6)*(x1243)*(x1244))));
05473 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05474 {
05475 {
05476 IkReal dummyeval[1];
05477 IkReal gconst80;
05478 IkReal x1247=(cj5)*(cj5);
05479 IkReal x1248=(r21)*(r21);
05480 IkReal x1249=(sj6)*(sj6);
05481 IkReal x1250=(cj6)*(cj6);
05482 IkReal x1251=(r20)*(r20);
05483 IkReal x1252=((cj6)*(r20));
05484 IkReal x1253=((IkReal(1.00000000000000))*(x1250));
05485 IkReal x1254=((IkReal(2.00000000000000))*(r21)*(sj6));
05486 IkReal x1255=((cj5)*(r22)*(sj5));
05487 IkReal x1256=((IkReal(1.00000000000000))*(x1249));
05488 gconst80=IKsign(((((x1247)*(x1252)*(x1254)))+(((IkReal(-1.00000000000000))*(x1247)*(x1251)*(x1253)))+(((IkReal(-1.00000000000000))*(x1248)*(x1253)))+(((IkReal(-1.00000000000000))*(x1252)*(x1254)))+(((IkReal(-1.00000000000000))*(x1254)*(x1255)))+(((IkReal(-1.00000000000000))*(x1251)*(x1256)))+(((IkReal(2.00000000000000))*(x1252)*(x1255)))+(((IkReal(-1.00000000000000))*(x1247)*(x1248)*(x1256)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))));
05489 IkReal x1257=(cj5)*(cj5);
05490 IkReal x1258=(r21)*(r21);
05491 IkReal x1259=(sj6)*(sj6);
05492 IkReal x1260=(cj6)*(cj6);
05493 IkReal x1261=(r20)*(r20);
05494 IkReal x1262=((cj6)*(r20));
05495 IkReal x1263=((IkReal(1.00000000000000))*(x1260));
05496 IkReal x1264=((IkReal(2.00000000000000))*(r21)*(sj6));
05497 IkReal x1265=((cj5)*(r22)*(sj5));
05498 IkReal x1266=((IkReal(1.00000000000000))*(x1259));
05499 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1258)*(x1263)))+(((IkReal(-1.00000000000000))*(x1264)*(x1265)))+(((IkReal(-1.00000000000000))*(x1257)*(x1258)*(x1266)))+(((IkReal(-1.00000000000000))*(x1261)*(x1266)))+(((IkReal(2.00000000000000))*(x1262)*(x1265)))+(((IkReal(-1.00000000000000))*(x1262)*(x1264)))+(((x1257)*(x1262)*(x1264)))+(((IkReal(-1.00000000000000))*(x1257)*(x1261)*(x1263)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5)))));
05500 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05501 {
05502 continue;
05503 
05504 } else
05505 {
05506 {
05507 IkReal j4array[1], cj4array[1], sj4array[1];
05508 bool j4valid[1]={false};
05509 _nj4 = 1;
05510 IkReal x1267=((IkReal(1.00000000000000))*(r20));
05511 if( IKabs(((gconst80)*(((((cj5)*(r21)*(sj6)))+(((r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1267))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst80)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)))+(((IkReal(-1.00000000000000))*(sj6)*(x1267))))))) < IKFAST_ATAN2_MAGTHRESH )
05512     continue;
05513 j4array[0]=IKatan2(((gconst80)*(((((cj5)*(r21)*(sj6)))+(((r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1267)))))), ((gconst80)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)))+(((IkReal(-1.00000000000000))*(sj6)*(x1267)))))));
05514 sj4array[0]=IKsin(j4array[0]);
05515 cj4array[0]=IKcos(j4array[0]);
05516 if( j4array[0] > IKPI )
05517 {
05518     j4array[0]-=IK2PI;
05519 }
05520 else if( j4array[0] < -IKPI )
05521 {    j4array[0]+=IK2PI;
05522 }
05523 j4valid[0] = true;
05524 for(int ij4 = 0; ij4 < 1; ++ij4)
05525 {
05526 if( !j4valid[ij4] )
05527 {
05528     continue;
05529 }
05530 _ij4[0] = ij4; _ij4[1] = -1;
05531 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05532 {
05533 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05534 {
05535     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05536 }
05537 }
05538 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05539 {
05540 IkReal evalcond[6];
05541 IkReal x1268=IKsin(j4);
05542 IkReal x1269=IKcos(j4);
05543 IkReal x1270=((IkReal(1.00000000000000))*(r00));
05544 IkReal x1271=((cj5)*(r01));
05545 IkReal x1272=((cj5)*(r11));
05546 IkReal x1273=((IkReal(1.00000000000000))*(cj5));
05547 IkReal x1274=((cj5)*(r21));
05548 IkReal x1275=((sj5)*(x1268));
05549 IkReal x1276=((cj6)*(x1268));
05550 IkReal x1277=((sj6)*(x1269));
05551 IkReal x1278=((sj6)*(x1268));
05552 IkReal x1279=((sj5)*(x1269));
05553 IkReal x1280=((cj6)*(x1269));
05554 evalcond[0]=((IkReal(1.00000000000000))+(((r22)*(x1275)))+(((x1274)*(x1278)))+(((IkReal(-1.00000000000000))*(r20)*(x1273)*(x1276)))+(((IkReal(-1.00000000000000))*(r21)*(x1280)))+(((IkReal(-1.00000000000000))*(r20)*(x1277))));
05555 evalcond[1]=((((r21)*(x1276)))+(((IkReal(-1.00000000000000))*(r20)*(x1273)*(x1280)))+(((r22)*(x1279)))+(((x1274)*(x1277)))+(((r20)*(x1278))));
05556 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1280)))+(((IkReal(-1.00000000000000))*(x1270)*(x1277)))+(((IkReal(-1.00000000000000))*(cj5)*(x1270)*(x1276)))+(((r02)*(x1275)))+(((x1271)*(x1278))));
05557 evalcond[3]=((((r12)*(x1275)))+(((x1272)*(x1278)))+(((IkReal(-1.00000000000000))*(r11)*(x1280)))+(((IkReal(-1.00000000000000))*(r10)*(x1273)*(x1276)))+(((IkReal(-1.00000000000000))*(r10)*(x1277))));
05558 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x1270)*(x1280)))+(((r02)*(x1279)))+(((x1271)*(x1277)))+(((r00)*(x1278)))+(((r01)*(x1276))));
05559 evalcond[5]=((((r12)*(x1279)))+(((r10)*(x1278)))+(cj3)+(((r11)*(x1276)))+(((x1272)*(x1277)))+(((IkReal(-1.00000000000000))*(r10)*(x1273)*(x1280))));
05560 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
05561 {
05562 continue;
05563 }
05564 }
05565 
05566 {
05567 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
05568 vinfos[0].jointtype = 1;
05569 vinfos[0].foffset = j0;
05570 vinfos[0].indices[0] = _ij0[0];
05571 vinfos[0].indices[1] = _ij0[1];
05572 vinfos[0].maxsolutions = _nj0;
05573 vinfos[1].jointtype = 1;
05574 vinfos[1].foffset = j1;
05575 vinfos[1].indices[0] = _ij1[0];
05576 vinfos[1].indices[1] = _ij1[1];
05577 vinfos[1].maxsolutions = _nj1;
05578 vinfos[2].jointtype = 1;
05579 vinfos[2].foffset = j2;
05580 vinfos[2].indices[0] = _ij2[0];
05581 vinfos[2].indices[1] = _ij2[1];
05582 vinfos[2].maxsolutions = _nj2;
05583 vinfos[3].jointtype = 1;
05584 vinfos[3].foffset = j3;
05585 vinfos[3].indices[0] = _ij3[0];
05586 vinfos[3].indices[1] = _ij3[1];
05587 vinfos[3].maxsolutions = _nj3;
05588 vinfos[4].jointtype = 1;
05589 vinfos[4].foffset = j4;
05590 vinfos[4].indices[0] = _ij4[0];
05591 vinfos[4].indices[1] = _ij4[1];
05592 vinfos[4].maxsolutions = _nj4;
05593 vinfos[5].jointtype = 1;
05594 vinfos[5].foffset = j5;
05595 vinfos[5].indices[0] = _ij5[0];
05596 vinfos[5].indices[1] = _ij5[1];
05597 vinfos[5].maxsolutions = _nj5;
05598 vinfos[6].jointtype = 1;
05599 vinfos[6].foffset = j6;
05600 vinfos[6].indices[0] = _ij6[0];
05601 vinfos[6].indices[1] = _ij6[1];
05602 vinfos[6].maxsolutions = _nj6;
05603 std::vector<int> vfree(0);
05604 solutions.AddSolution(vinfos,vfree);
05605 }
05606 }
05607 }
05608 
05609 }
05610 
05611 }
05612 
05613 } else
05614 {
05615 {
05616 IkReal j4array[1], cj4array[1], sj4array[1];
05617 bool j4valid[1]={false};
05618 _nj4 = 1;
05619 IkReal x1281=((cj3)*(sj6));
05620 IkReal x1282=((IkReal(1.00000000000000))*(cj3)*(cj6));
05621 if( IKabs(((gconst79)*(((((cj3)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1282)))+(((cj5)*(r21)*(x1281))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(r20)*(x1281)))+(((IkReal(-1.00000000000000))*(r21)*(x1282))))))) < IKFAST_ATAN2_MAGTHRESH )
05622     continue;
05623 j4array[0]=IKatan2(((gconst79)*(((((cj3)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1282)))+(((cj5)*(r21)*(x1281)))))), ((gconst79)*(((((IkReal(-1.00000000000000))*(r20)*(x1281)))+(((IkReal(-1.00000000000000))*(r21)*(x1282)))))));
05624 sj4array[0]=IKsin(j4array[0]);
05625 cj4array[0]=IKcos(j4array[0]);
05626 if( j4array[0] > IKPI )
05627 {
05628     j4array[0]-=IK2PI;
05629 }
05630 else if( j4array[0] < -IKPI )
05631 {    j4array[0]+=IK2PI;
05632 }
05633 j4valid[0] = true;
05634 for(int ij4 = 0; ij4 < 1; ++ij4)
05635 {
05636 if( !j4valid[ij4] )
05637 {
05638     continue;
05639 }
05640 _ij4[0] = ij4; _ij4[1] = -1;
05641 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05642 {
05643 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05644 {
05645     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05646 }
05647 }
05648 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05649 {
05650 IkReal evalcond[6];
05651 IkReal x1283=IKsin(j4);
05652 IkReal x1284=IKcos(j4);
05653 IkReal x1285=((IkReal(1.00000000000000))*(r00));
05654 IkReal x1286=((cj5)*(r01));
05655 IkReal x1287=((cj5)*(r11));
05656 IkReal x1288=((IkReal(1.00000000000000))*(cj5));
05657 IkReal x1289=((cj5)*(r21));
05658 IkReal x1290=((sj5)*(x1283));
05659 IkReal x1291=((cj6)*(x1283));
05660 IkReal x1292=((sj6)*(x1284));
05661 IkReal x1293=((sj6)*(x1283));
05662 IkReal x1294=((sj5)*(x1284));
05663 IkReal x1295=((cj6)*(x1284));
05664 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r21)*(x1295)))+(((IkReal(-1.00000000000000))*(r20)*(x1288)*(x1291)))+(((IkReal(-1.00000000000000))*(r20)*(x1292)))+(((r22)*(x1290)))+(((x1289)*(x1293))));
05665 evalcond[1]=((((r20)*(x1293)))+(((r21)*(x1291)))+(((IkReal(-1.00000000000000))*(r20)*(x1288)*(x1295)))+(((r22)*(x1294)))+(((x1289)*(x1292))));
05666 evalcond[2]=((((x1286)*(x1293)))+(((IkReal(-1.00000000000000))*(r01)*(x1295)))+(((r02)*(x1290)))+(((IkReal(-1.00000000000000))*(cj5)*(x1285)*(x1291)))+(((IkReal(-1.00000000000000))*(x1285)*(x1292))));
05667 evalcond[3]=((((x1287)*(x1293)))+(((r12)*(x1290)))+(((IkReal(-1.00000000000000))*(r10)*(x1292)))+(((IkReal(-1.00000000000000))*(r11)*(x1295)))+(((IkReal(-1.00000000000000))*(r10)*(x1288)*(x1291))));
05668 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((x1286)*(x1292)))+(((r02)*(x1294)))+(((IkReal(-1.00000000000000))*(cj5)*(x1285)*(x1295)))+(((r01)*(x1291)))+(((r00)*(x1293))));
05669 evalcond[5]=((cj3)+(((x1287)*(x1292)))+(((r12)*(x1294)))+(((r11)*(x1291)))+(((r10)*(x1293)))+(((IkReal(-1.00000000000000))*(r10)*(x1288)*(x1295))));
05670 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
05671 {
05672 continue;
05673 }
05674 }
05675 
05676 {
05677 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
05678 vinfos[0].jointtype = 1;
05679 vinfos[0].foffset = j0;
05680 vinfos[0].indices[0] = _ij0[0];
05681 vinfos[0].indices[1] = _ij0[1];
05682 vinfos[0].maxsolutions = _nj0;
05683 vinfos[1].jointtype = 1;
05684 vinfos[1].foffset = j1;
05685 vinfos[1].indices[0] = _ij1[0];
05686 vinfos[1].indices[1] = _ij1[1];
05687 vinfos[1].maxsolutions = _nj1;
05688 vinfos[2].jointtype = 1;
05689 vinfos[2].foffset = j2;
05690 vinfos[2].indices[0] = _ij2[0];
05691 vinfos[2].indices[1] = _ij2[1];
05692 vinfos[2].maxsolutions = _nj2;
05693 vinfos[3].jointtype = 1;
05694 vinfos[3].foffset = j3;
05695 vinfos[3].indices[0] = _ij3[0];
05696 vinfos[3].indices[1] = _ij3[1];
05697 vinfos[3].maxsolutions = _nj3;
05698 vinfos[4].jointtype = 1;
05699 vinfos[4].foffset = j4;
05700 vinfos[4].indices[0] = _ij4[0];
05701 vinfos[4].indices[1] = _ij4[1];
05702 vinfos[4].maxsolutions = _nj4;
05703 vinfos[5].jointtype = 1;
05704 vinfos[5].foffset = j5;
05705 vinfos[5].indices[0] = _ij5[0];
05706 vinfos[5].indices[1] = _ij5[1];
05707 vinfos[5].maxsolutions = _nj5;
05708 vinfos[6].jointtype = 1;
05709 vinfos[6].foffset = j6;
05710 vinfos[6].indices[0] = _ij6[0];
05711 vinfos[6].indices[1] = _ij6[1];
05712 vinfos[6].maxsolutions = _nj6;
05713 std::vector<int> vfree(0);
05714 solutions.AddSolution(vinfos,vfree);
05715 }
05716 }
05717 }
05718 
05719 }
05720 
05721 }
05722 }
05723 }
05724 
05725 } else
05726 {
05727 if( 1 )
05728 {
05729 continue;
05730 
05731 } else
05732 {
05733 }
05734 }
05735 }
05736 }
05737 
05738 } else
05739 {
05740 {
05741 IkReal j3array[1], cj3array[1], sj3array[1];
05742 bool j3valid[1]={false};
05743 _nj3 = 1;
05744 IkReal x1296=((IkReal(1.00000000000000))*(sj5));
05745 if( IKabs(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1296))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1296)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1296)))))))+IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1296)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
05746     continue;
05747 j3array[0]=IKatan2(((((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30))*(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1296)))))), ((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1296)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))));
05748 sj3array[0]=IKsin(j3array[0]);
05749 cj3array[0]=IKcos(j3array[0]);
05750 if( j3array[0] > IKPI )
05751 {
05752     j3array[0]-=IK2PI;
05753 }
05754 else if( j3array[0] < -IKPI )
05755 {    j3array[0]+=IK2PI;
05756 }
05757 j3valid[0] = true;
05758 for(int ij3 = 0; ij3 < 1; ++ij3)
05759 {
05760 if( !j3valid[ij3] )
05761 {
05762     continue;
05763 }
05764 _ij3[0] = ij3; _ij3[1] = -1;
05765 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05766 {
05767 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05768 {
05769     j3valid[iij3]=false; _ij3[1] = iij3; break; 
05770 }
05771 }
05772 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05773 {
05774 IkReal evalcond[3];
05775 IkReal x1297=IKsin(j3);
05776 IkReal x1298=((cj6)*(sj5));
05777 IkReal x1299=((IkReal(1.00000000000000))*(sj5)*(sj6));
05778 evalcond[0]=((((r20)*(x1298)))+(((cj5)*(r22)))+(((sj2)*(x1297)))+(((IkReal(-1.00000000000000))*(r21)*(x1299))));
05779 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1299)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((r00)*(x1298)))+(((cj5)*(r02))));
05780 evalcond[2]=((((cj5)*(r12)))+(((cj2)*(x1297)))+(((IkReal(-1.00000000000000))*(r11)*(x1299)))+(((r10)*(x1298))));
05781 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
05782 {
05783 continue;
05784 }
05785 }
05786 
05787 {
05788 IkReal dummyeval[1];
05789 IkReal gconst71;
05790 IkReal x1300=(sj6)*(sj6);
05791 IkReal x1301=(cj6)*(cj6);
05792 IkReal x1302=((cj6)*(sj5));
05793 IkReal x1303=((r00)*(r11));
05794 IkReal x1304=((IkReal(1.00000000000000))*(r02));
05795 IkReal x1305=((sj5)*(sj6));
05796 IkReal x1306=((cj5)*(x1300));
05797 IkReal x1307=((IkReal(1.00000000000000))*(r01)*(r10));
05798 IkReal x1308=((cj5)*(x1301));
05799 gconst71=IKsign(((((IkReal(-1.00000000000000))*(x1307)*(x1308)))+(((IkReal(-1.00000000000000))*(r10)*(x1304)*(x1305)))+(((IkReal(-1.00000000000000))*(x1306)*(x1307)))+(((IkReal(-1.00000000000000))*(r11)*(x1302)*(x1304)))+(((r01)*(r12)*(x1302)))+(((x1303)*(x1308)))+(((x1303)*(x1306)))+(((r00)*(r12)*(x1305)))));
05800 IkReal x1309=(sj6)*(sj6);
05801 IkReal x1310=(cj6)*(cj6);
05802 IkReal x1311=((cj6)*(sj5));
05803 IkReal x1312=((r00)*(r11));
05804 IkReal x1313=((IkReal(1.00000000000000))*(r02));
05805 IkReal x1314=((sj5)*(sj6));
05806 IkReal x1315=((cj5)*(x1309));
05807 IkReal x1316=((IkReal(1.00000000000000))*(r01)*(r10));
05808 IkReal x1317=((cj5)*(x1310));
05809 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1315)*(x1316)))+(((r01)*(r12)*(x1311)))+(((IkReal(-1.00000000000000))*(r10)*(x1313)*(x1314)))+(((IkReal(-1.00000000000000))*(r11)*(x1311)*(x1313)))+(((r00)*(r12)*(x1314)))+(((IkReal(-1.00000000000000))*(x1316)*(x1317)))+(((x1312)*(x1315)))+(((x1312)*(x1317))));
05810 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05811 {
05812 {
05813 IkReal dummyeval[1];
05814 IkReal gconst72;
05815 IkReal x1318=(cj5)*(cj5);
05816 IkReal x1319=(sj6)*(sj6);
05817 IkReal x1320=(r01)*(r01);
05818 IkReal x1321=(cj6)*(cj6);
05819 IkReal x1322=(r00)*(r00);
05820 IkReal x1323=((cj6)*(r00));
05821 IkReal x1324=((IkReal(2.00000000000000))*(r01)*(sj6));
05822 IkReal x1325=((cj5)*(r02)*(sj5));
05823 IkReal x1326=((IkReal(1.00000000000000))*(x1319));
05824 IkReal x1327=((IkReal(1.00000000000000))*(x1321));
05825 gconst72=IKsign(((((IkReal(-1.00000000000000))*(x1323)*(x1324)))+(((IkReal(-1.00000000000000))*(x1324)*(x1325)))+(((IkReal(-1.00000000000000))*(x1318)*(x1322)*(x1327)))+(((x1318)*(x1323)*(x1324)))+(((IkReal(-1.00000000000000))*(x1322)*(x1326)))+(((IkReal(-1.00000000000000))*(x1318)*(x1320)*(x1326)))+(((IkReal(2.00000000000000))*(x1323)*(x1325)))+(((IkReal(-1.00000000000000))*((r02)*(r02))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1320)*(x1327)))));
05826 IkReal x1328=(cj5)*(cj5);
05827 IkReal x1329=(sj6)*(sj6);
05828 IkReal x1330=(r01)*(r01);
05829 IkReal x1331=(cj6)*(cj6);
05830 IkReal x1332=(r00)*(r00);
05831 IkReal x1333=((cj6)*(r00));
05832 IkReal x1334=((IkReal(2.00000000000000))*(r01)*(sj6));
05833 IkReal x1335=((cj5)*(r02)*(sj5));
05834 IkReal x1336=((IkReal(1.00000000000000))*(x1329));
05835 IkReal x1337=((IkReal(1.00000000000000))*(x1331));
05836 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1334)*(x1335)))+(((IkReal(2.00000000000000))*(x1333)*(x1335)))+(((IkReal(-1.00000000000000))*(x1330)*(x1337)))+(((IkReal(-1.00000000000000))*(x1333)*(x1334)))+(((IkReal(-1.00000000000000))*(x1332)*(x1336)))+(((x1328)*(x1333)*(x1334)))+(((IkReal(-1.00000000000000))*(x1328)*(x1330)*(x1336)))+(((IkReal(-1.00000000000000))*((r02)*(r02))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1328)*(x1332)*(x1337))));
05837 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
05838 {
05839 continue;
05840 
05841 } else
05842 {
05843 {
05844 IkReal j4array[1], cj4array[1], sj4array[1];
05845 bool j4valid[1]={false};
05846 _nj4 = 1;
05847 IkReal x1338=((cj5)*(sj3));
05848 IkReal x1339=((IkReal(1.00000000000000))*(sj3));
05849 if( IKabs(((gconst72)*(((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1339)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1339))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst72)*(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1338)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1339)))+(((cj6)*(r00)*(x1338))))))) < IKFAST_ATAN2_MAGTHRESH )
05850     continue;
05851 j4array[0]=IKatan2(((gconst72)*(((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1339)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1339)))))), ((gconst72)*(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1338)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1339)))+(((cj6)*(r00)*(x1338)))))));
05852 sj4array[0]=IKsin(j4array[0]);
05853 cj4array[0]=IKcos(j4array[0]);
05854 if( j4array[0] > IKPI )
05855 {
05856     j4array[0]-=IK2PI;
05857 }
05858 else if( j4array[0] < -IKPI )
05859 {    j4array[0]+=IK2PI;
05860 }
05861 j4valid[0] = true;
05862 for(int ij4 = 0; ij4 < 1; ++ij4)
05863 {
05864 if( !j4valid[ij4] )
05865 {
05866     continue;
05867 }
05868 _ij4[0] = ij4; _ij4[1] = -1;
05869 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05870 {
05871 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05872 {
05873     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05874 }
05875 }
05876 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05877 {
05878 IkReal evalcond[6];
05879 IkReal x1340=IKsin(j4);
05880 IkReal x1341=IKcos(j4);
05881 IkReal x1342=((IkReal(1.00000000000000))*(r00));
05882 IkReal x1343=((cj5)*(r01));
05883 IkReal x1344=((cj5)*(r11));
05884 IkReal x1345=((IkReal(1.00000000000000))*(cj3));
05885 IkReal x1346=((IkReal(1.00000000000000))*(cj5));
05886 IkReal x1347=((cj5)*(r21));
05887 IkReal x1348=((sj5)*(x1340));
05888 IkReal x1349=((cj6)*(x1340));
05889 IkReal x1350=((sj6)*(x1341));
05890 IkReal x1351=((sj6)*(x1340));
05891 IkReal x1352=((sj5)*(x1341));
05892 IkReal x1353=((cj6)*(x1341));
05893 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x1353)))+(((IkReal(-1.00000000000000))*(r20)*(x1346)*(x1349)))+(((r22)*(x1348)))+(((IkReal(-1.00000000000000))*(r20)*(x1350)))+(((x1347)*(x1351)))+(((IkReal(-1.00000000000000))*(cj2))));
05894 evalcond[1]=((((r22)*(x1352)))+(((IkReal(-1.00000000000000))*(sj2)*(x1345)))+(((IkReal(-1.00000000000000))*(r20)*(x1346)*(x1353)))+(((r20)*(x1351)))+(((x1347)*(x1350)))+(((r21)*(x1349))));
05895 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x1342)*(x1349)))+(((r02)*(x1348)))+(((x1343)*(x1351)))+(((IkReal(-1.00000000000000))*(r01)*(x1353)))+(((IkReal(-1.00000000000000))*(x1342)*(x1350))));
05896 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x1346)*(x1349)))+(sj2)+(((IkReal(-1.00000000000000))*(r10)*(x1350)))+(((x1344)*(x1351)))+(((IkReal(-1.00000000000000))*(r11)*(x1353)))+(((r12)*(x1348))));
05897 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((r02)*(x1352)))+(((IkReal(-1.00000000000000))*(cj5)*(x1342)*(x1353)))+(((r01)*(x1349)))+(((r00)*(x1351)))+(((x1343)*(x1350))));
05898 evalcond[5]=((((r10)*(x1351)))+(((IkReal(-1.00000000000000))*(r10)*(x1346)*(x1353)))+(((x1344)*(x1350)))+(((IkReal(-1.00000000000000))*(cj2)*(x1345)))+(((r11)*(x1349)))+(((r12)*(x1352))));
05899 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
05900 {
05901 continue;
05902 }
05903 }
05904 
05905 {
05906 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
05907 vinfos[0].jointtype = 1;
05908 vinfos[0].foffset = j0;
05909 vinfos[0].indices[0] = _ij0[0];
05910 vinfos[0].indices[1] = _ij0[1];
05911 vinfos[0].maxsolutions = _nj0;
05912 vinfos[1].jointtype = 1;
05913 vinfos[1].foffset = j1;
05914 vinfos[1].indices[0] = _ij1[0];
05915 vinfos[1].indices[1] = _ij1[1];
05916 vinfos[1].maxsolutions = _nj1;
05917 vinfos[2].jointtype = 1;
05918 vinfos[2].foffset = j2;
05919 vinfos[2].indices[0] = _ij2[0];
05920 vinfos[2].indices[1] = _ij2[1];
05921 vinfos[2].maxsolutions = _nj2;
05922 vinfos[3].jointtype = 1;
05923 vinfos[3].foffset = j3;
05924 vinfos[3].indices[0] = _ij3[0];
05925 vinfos[3].indices[1] = _ij3[1];
05926 vinfos[3].maxsolutions = _nj3;
05927 vinfos[4].jointtype = 1;
05928 vinfos[4].foffset = j4;
05929 vinfos[4].indices[0] = _ij4[0];
05930 vinfos[4].indices[1] = _ij4[1];
05931 vinfos[4].maxsolutions = _nj4;
05932 vinfos[5].jointtype = 1;
05933 vinfos[5].foffset = j5;
05934 vinfos[5].indices[0] = _ij5[0];
05935 vinfos[5].indices[1] = _ij5[1];
05936 vinfos[5].maxsolutions = _nj5;
05937 vinfos[6].jointtype = 1;
05938 vinfos[6].foffset = j6;
05939 vinfos[6].indices[0] = _ij6[0];
05940 vinfos[6].indices[1] = _ij6[1];
05941 vinfos[6].maxsolutions = _nj6;
05942 std::vector<int> vfree(0);
05943 solutions.AddSolution(vinfos,vfree);
05944 }
05945 }
05946 }
05947 
05948 }
05949 
05950 }
05951 
05952 } else
05953 {
05954 {
05955 IkReal j4array[1], cj4array[1], sj4array[1];
05956 bool j4valid[1]={false};
05957 _nj4 = 1;
05958 IkReal x1354=((IkReal(1.00000000000000))*(sj2));
05959 if( IKabs(((gconst71)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1354)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1354))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst71)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1354)))+(((cj5)*(cj6)*(r00)*(sj2)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1354))))))) < IKFAST_ATAN2_MAGTHRESH )
05960     continue;
05961 j4array[0]=IKatan2(((gconst71)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1354)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1354)))))), ((gconst71)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1354)))+(((cj5)*(cj6)*(r00)*(sj2)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1354)))))));
05962 sj4array[0]=IKsin(j4array[0]);
05963 cj4array[0]=IKcos(j4array[0]);
05964 if( j4array[0] > IKPI )
05965 {
05966     j4array[0]-=IK2PI;
05967 }
05968 else if( j4array[0] < -IKPI )
05969 {    j4array[0]+=IK2PI;
05970 }
05971 j4valid[0] = true;
05972 for(int ij4 = 0; ij4 < 1; ++ij4)
05973 {
05974 if( !j4valid[ij4] )
05975 {
05976     continue;
05977 }
05978 _ij4[0] = ij4; _ij4[1] = -1;
05979 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05980 {
05981 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05982 {
05983     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05984 }
05985 }
05986 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05987 {
05988 IkReal evalcond[6];
05989 IkReal x1355=IKsin(j4);
05990 IkReal x1356=IKcos(j4);
05991 IkReal x1357=((IkReal(1.00000000000000))*(r00));
05992 IkReal x1358=((cj5)*(r01));
05993 IkReal x1359=((cj5)*(r11));
05994 IkReal x1360=((IkReal(1.00000000000000))*(cj3));
05995 IkReal x1361=((IkReal(1.00000000000000))*(cj5));
05996 IkReal x1362=((cj5)*(r21));
05997 IkReal x1363=((sj5)*(x1355));
05998 IkReal x1364=((cj6)*(x1355));
05999 IkReal x1365=((sj6)*(x1356));
06000 IkReal x1366=((sj6)*(x1355));
06001 IkReal x1367=((sj5)*(x1356));
06002 IkReal x1368=((cj6)*(x1356));
06003 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1361)*(x1364)))+(((IkReal(-1.00000000000000))*(r21)*(x1368)))+(((r22)*(x1363)))+(((IkReal(-1.00000000000000))*(r20)*(x1365)))+(((IkReal(-1.00000000000000))*(cj2)))+(((x1362)*(x1366))));
06004 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1361)*(x1368)))+(((r22)*(x1367)))+(((IkReal(-1.00000000000000))*(sj2)*(x1360)))+(((r20)*(x1366)))+(((r21)*(x1364)))+(((x1362)*(x1365))));
06005 evalcond[2]=((((r02)*(x1363)))+(((x1358)*(x1366)))+(((IkReal(-1.00000000000000))*(cj5)*(x1357)*(x1364)))+(((IkReal(-1.00000000000000))*(x1357)*(x1365)))+(((IkReal(-1.00000000000000))*(r01)*(x1368))));
06006 evalcond[3]=((sj2)+(((x1359)*(x1366)))+(((IkReal(-1.00000000000000))*(r10)*(x1365)))+(((IkReal(-1.00000000000000))*(r10)*(x1361)*(x1364)))+(((IkReal(-1.00000000000000))*(r11)*(x1368)))+(((r12)*(x1363))));
06007 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((r02)*(x1367)))+(((x1358)*(x1365)))+(((IkReal(-1.00000000000000))*(cj5)*(x1357)*(x1368)))+(((r01)*(x1364)))+(((r00)*(x1366))));
06008 evalcond[5]=((((r10)*(x1366)))+(((IkReal(-1.00000000000000))*(cj2)*(x1360)))+(((x1359)*(x1365)))+(((IkReal(-1.00000000000000))*(r10)*(x1361)*(x1368)))+(((r11)*(x1364)))+(((r12)*(x1367))));
06009 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
06010 {
06011 continue;
06012 }
06013 }
06014 
06015 {
06016 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
06017 vinfos[0].jointtype = 1;
06018 vinfos[0].foffset = j0;
06019 vinfos[0].indices[0] = _ij0[0];
06020 vinfos[0].indices[1] = _ij0[1];
06021 vinfos[0].maxsolutions = _nj0;
06022 vinfos[1].jointtype = 1;
06023 vinfos[1].foffset = j1;
06024 vinfos[1].indices[0] = _ij1[0];
06025 vinfos[1].indices[1] = _ij1[1];
06026 vinfos[1].maxsolutions = _nj1;
06027 vinfos[2].jointtype = 1;
06028 vinfos[2].foffset = j2;
06029 vinfos[2].indices[0] = _ij2[0];
06030 vinfos[2].indices[1] = _ij2[1];
06031 vinfos[2].maxsolutions = _nj2;
06032 vinfos[3].jointtype = 1;
06033 vinfos[3].foffset = j3;
06034 vinfos[3].indices[0] = _ij3[0];
06035 vinfos[3].indices[1] = _ij3[1];
06036 vinfos[3].maxsolutions = _nj3;
06037 vinfos[4].jointtype = 1;
06038 vinfos[4].foffset = j4;
06039 vinfos[4].indices[0] = _ij4[0];
06040 vinfos[4].indices[1] = _ij4[1];
06041 vinfos[4].maxsolutions = _nj4;
06042 vinfos[5].jointtype = 1;
06043 vinfos[5].foffset = j5;
06044 vinfos[5].indices[0] = _ij5[0];
06045 vinfos[5].indices[1] = _ij5[1];
06046 vinfos[5].maxsolutions = _nj5;
06047 vinfos[6].jointtype = 1;
06048 vinfos[6].foffset = j6;
06049 vinfos[6].indices[0] = _ij6[0];
06050 vinfos[6].indices[1] = _ij6[1];
06051 vinfos[6].maxsolutions = _nj6;
06052 std::vector<int> vfree(0);
06053 solutions.AddSolution(vinfos,vfree);
06054 }
06055 }
06056 }
06057 
06058 }
06059 
06060 }
06061 }
06062 }
06063 
06064 }
06065 
06066 }
06067 
06068 } else
06069 {
06070 {
06071 IkReal j4array[1], cj4array[1], sj4array[1];
06072 bool j4valid[1]={false};
06073 _nj4 = 1;
06074 IkReal x1369=((IkReal(1.00000000000000))*(cj2));
06075 IkReal x1370=((sj6)*(x1369));
06076 if( IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(r00)*(x1370)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1369))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1369)))+(((cj2)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1370))))))) < IKFAST_ATAN2_MAGTHRESH )
06077     continue;
06078 j4array[0]=IKatan2(((gconst70)*(((((IkReal(-1.00000000000000))*(r00)*(x1370)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1369)))))), ((gconst70)*(((((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1369)))+(((cj2)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1370)))))));
06079 sj4array[0]=IKsin(j4array[0]);
06080 cj4array[0]=IKcos(j4array[0]);
06081 if( j4array[0] > IKPI )
06082 {
06083     j4array[0]-=IK2PI;
06084 }
06085 else if( j4array[0] < -IKPI )
06086 {    j4array[0]+=IK2PI;
06087 }
06088 j4valid[0] = true;
06089 for(int ij4 = 0; ij4 < 1; ++ij4)
06090 {
06091 if( !j4valid[ij4] )
06092 {
06093     continue;
06094 }
06095 _ij4[0] = ij4; _ij4[1] = -1;
06096 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06097 {
06098 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06099 {
06100     j4valid[iij4]=false; _ij4[1] = iij4; break; 
06101 }
06102 }
06103 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06104 {
06105 IkReal evalcond[3];
06106 IkReal x1371=IKsin(j4);
06107 IkReal x1372=IKcos(j4);
06108 IkReal x1373=((IkReal(1.00000000000000))*(cj6));
06109 IkReal x1374=((sj5)*(x1371));
06110 IkReal x1375=((cj5)*(x1371));
06111 IkReal x1376=((IkReal(1.00000000000000))*(sj6)*(x1372));
06112 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1373)*(x1375)))+(((IkReal(-1.00000000000000))*(r20)*(x1376)))+(((r22)*(x1374)))+(((r21)*(sj6)*(x1375)))+(((IkReal(-1.00000000000000))*(r21)*(x1372)*(x1373)))+(((IkReal(-1.00000000000000))*(cj2))));
06113 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x1376)))+(((r01)*(sj6)*(x1375)))+(((IkReal(-1.00000000000000))*(r00)*(x1373)*(x1375)))+(((r02)*(x1374)))+(((IkReal(-1.00000000000000))*(r01)*(x1372)*(x1373))));
06114 evalcond[2]=((sj2)+(((IkReal(-1.00000000000000))*(r10)*(x1373)*(x1375)))+(((IkReal(-1.00000000000000))*(r11)*(x1372)*(x1373)))+(((r12)*(x1374)))+(((r11)*(sj6)*(x1375)))+(((IkReal(-1.00000000000000))*(r10)*(x1376))));
06115 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
06116 {
06117 continue;
06118 }
06119 }
06120 
06121 {
06122 IkReal j3array[1], cj3array[1], sj3array[1];
06123 bool j3valid[1]={false};
06124 _nj3 = 1;
06125 IkReal x1377=((cj4)*(cj5));
06126 IkReal x1378=((r01)*(sj6));
06127 IkReal x1379=((cj6)*(r00));
06128 if( IKabs(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x1377)*(x1378)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x1377)*(x1379))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj5)*(x1379)))+(((IkReal(-1.00000000000000))*(sj5)*(x1378)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x1377)*(x1378)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x1377)*(x1379)))))+IKsqr(((((sj5)*(x1379)))+(((IkReal(-1.00000000000000))*(sj5)*(x1378)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
06129     continue;
06130 j3array[0]=IKatan2(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x1377)*(x1378)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x1377)*(x1379)))), ((((sj5)*(x1379)))+(((IkReal(-1.00000000000000))*(sj5)*(x1378)))+(((cj5)*(r02)))));
06131 sj3array[0]=IKsin(j3array[0]);
06132 cj3array[0]=IKcos(j3array[0]);
06133 if( j3array[0] > IKPI )
06134 {
06135     j3array[0]-=IK2PI;
06136 }
06137 else if( j3array[0] < -IKPI )
06138 {    j3array[0]+=IK2PI;
06139 }
06140 j3valid[0] = true;
06141 for(int ij3 = 0; ij3 < 1; ++ij3)
06142 {
06143 if( !j3valid[ij3] )
06144 {
06145     continue;
06146 }
06147 _ij3[0] = ij3; _ij3[1] = -1;
06148 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06149 {
06150 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06151 {
06152     j3valid[iij3]=false; _ij3[1] = iij3; break; 
06153 }
06154 }
06155 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06156 {
06157 IkReal evalcond[6];
06158 IkReal x1380=IKsin(j3);
06159 IkReal x1381=IKcos(j3);
06160 IkReal x1382=((r11)*(sj6));
06161 IkReal x1383=((IkReal(1.00000000000000))*(sj5));
06162 IkReal x1384=((cj6)*(sj4));
06163 IkReal x1385=((sj4)*(sj6));
06164 IkReal x1386=((cj4)*(sj5));
06165 IkReal x1387=((cj4)*(cj5));
06166 IkReal x1388=((r01)*(sj6));
06167 IkReal x1389=((r21)*(sj6));
06168 IkReal x1390=((cj6)*(sj5));
06169 IkReal x1391=((IkReal(1.00000000000000))*(x1381));
06170 IkReal x1392=((IkReal(1.00000000000000))*(cj6)*(x1387));
06171 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x1383)*(x1389)))+(((r20)*(x1390)))+(((sj2)*(x1380))));
06172 evalcond[1]=((((IkReal(-1.00000000000000))*(x1383)*(x1388)))+(((r00)*(x1390)))+(((IkReal(-1.00000000000000))*(x1391)))+(((cj5)*(r02))));
06173 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x1390)))+(((cj2)*(x1380)))+(((IkReal(-1.00000000000000))*(x1382)*(x1383))));
06174 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x1392)))+(((r21)*(x1384)))+(((r20)*(x1385)))+(((x1387)*(x1389)))+(((r22)*(x1386)))+(((IkReal(-1.00000000000000))*(sj2)*(x1391))));
06175 evalcond[4]=((((x1387)*(x1388)))+(((IkReal(-1.00000000000000))*(r00)*(x1392)))+(((IkReal(-1.00000000000000))*(x1380)))+(((r00)*(x1385)))+(((r01)*(x1384)))+(((r02)*(x1386))));
06176 evalcond[5]=((((r10)*(x1385)))+(((IkReal(-1.00000000000000))*(r10)*(x1392)))+(((IkReal(-1.00000000000000))*(cj2)*(x1391)))+(((r11)*(x1384)))+(((r12)*(x1386)))+(((x1382)*(x1387))));
06177 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
06178 {
06179 continue;
06180 }
06181 }
06182 
06183 {
06184 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
06185 vinfos[0].jointtype = 1;
06186 vinfos[0].foffset = j0;
06187 vinfos[0].indices[0] = _ij0[0];
06188 vinfos[0].indices[1] = _ij0[1];
06189 vinfos[0].maxsolutions = _nj0;
06190 vinfos[1].jointtype = 1;
06191 vinfos[1].foffset = j1;
06192 vinfos[1].indices[0] = _ij1[0];
06193 vinfos[1].indices[1] = _ij1[1];
06194 vinfos[1].maxsolutions = _nj1;
06195 vinfos[2].jointtype = 1;
06196 vinfos[2].foffset = j2;
06197 vinfos[2].indices[0] = _ij2[0];
06198 vinfos[2].indices[1] = _ij2[1];
06199 vinfos[2].maxsolutions = _nj2;
06200 vinfos[3].jointtype = 1;
06201 vinfos[3].foffset = j3;
06202 vinfos[3].indices[0] = _ij3[0];
06203 vinfos[3].indices[1] = _ij3[1];
06204 vinfos[3].maxsolutions = _nj3;
06205 vinfos[4].jointtype = 1;
06206 vinfos[4].foffset = j4;
06207 vinfos[4].indices[0] = _ij4[0];
06208 vinfos[4].indices[1] = _ij4[1];
06209 vinfos[4].maxsolutions = _nj4;
06210 vinfos[5].jointtype = 1;
06211 vinfos[5].foffset = j5;
06212 vinfos[5].indices[0] = _ij5[0];
06213 vinfos[5].indices[1] = _ij5[1];
06214 vinfos[5].maxsolutions = _nj5;
06215 vinfos[6].jointtype = 1;
06216 vinfos[6].foffset = j6;
06217 vinfos[6].indices[0] = _ij6[0];
06218 vinfos[6].indices[1] = _ij6[1];
06219 vinfos[6].maxsolutions = _nj6;
06220 std::vector<int> vfree(0);
06221 solutions.AddSolution(vinfos,vfree);
06222 }
06223 }
06224 }
06225 }
06226 }
06227 
06228 }
06229 
06230 }
06231 
06232 } else
06233 {
06234 {
06235 IkReal j4array[1], cj4array[1], sj4array[1];
06236 bool j4valid[1]={false};
06237 _nj4 = 1;
06238 IkReal x1393=((IkReal(1.00000000000000))*(sj2));
06239 if( IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1393)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1393))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1393)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1393)))+(((cj5)*(cj6)*(r00)*(sj2))))))) < IKFAST_ATAN2_MAGTHRESH )
06240     continue;
06241 j4array[0]=IKatan2(((gconst69)*(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1393)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1393)))))), ((gconst69)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(sj6)*(x1393)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x1393)))+(((cj5)*(cj6)*(r00)*(sj2)))))));
06242 sj4array[0]=IKsin(j4array[0]);
06243 cj4array[0]=IKcos(j4array[0]);
06244 if( j4array[0] > IKPI )
06245 {
06246     j4array[0]-=IK2PI;
06247 }
06248 else if( j4array[0] < -IKPI )
06249 {    j4array[0]+=IK2PI;
06250 }
06251 j4valid[0] = true;
06252 for(int ij4 = 0; ij4 < 1; ++ij4)
06253 {
06254 if( !j4valid[ij4] )
06255 {
06256     continue;
06257 }
06258 _ij4[0] = ij4; _ij4[1] = -1;
06259 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06260 {
06261 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06262 {
06263     j4valid[iij4]=false; _ij4[1] = iij4; break; 
06264 }
06265 }
06266 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06267 {
06268 IkReal evalcond[3];
06269 IkReal x1394=IKsin(j4);
06270 IkReal x1395=IKcos(j4);
06271 IkReal x1396=((IkReal(1.00000000000000))*(cj6));
06272 IkReal x1397=((sj5)*(x1394));
06273 IkReal x1398=((cj5)*(x1394));
06274 IkReal x1399=((IkReal(1.00000000000000))*(sj6)*(x1395));
06275 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x1395)*(x1396)))+(((IkReal(-1.00000000000000))*(r20)*(x1399)))+(((r21)*(sj6)*(x1398)))+(((r22)*(x1397)))+(((IkReal(-1.00000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(r20)*(x1396)*(x1398))));
06276 evalcond[1]=((((r01)*(sj6)*(x1398)))+(((IkReal(-1.00000000000000))*(r01)*(x1395)*(x1396)))+(((IkReal(-1.00000000000000))*(r00)*(x1399)))+(((IkReal(-1.00000000000000))*(r00)*(x1396)*(x1398)))+(((r02)*(x1397))));
06277 evalcond[2]=((sj2)+(((r11)*(sj6)*(x1398)))+(((IkReal(-1.00000000000000))*(r10)*(x1399)))+(((IkReal(-1.00000000000000))*(r10)*(x1396)*(x1398)))+(((r12)*(x1397)))+(((IkReal(-1.00000000000000))*(r11)*(x1395)*(x1396))));
06278 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
06279 {
06280 continue;
06281 }
06282 }
06283 
06284 {
06285 IkReal j3array[1], cj3array[1], sj3array[1];
06286 bool j3valid[1]={false};
06287 _nj3 = 1;
06288 IkReal x1400=((cj4)*(cj5));
06289 IkReal x1401=((r01)*(sj6));
06290 IkReal x1402=((cj6)*(r00));
06291 if( IKabs(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x1400)*(x1402)))+(((cj4)*(r02)*(sj5)))+(((x1400)*(x1401))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x1401)))+(((sj5)*(x1402)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x1400)*(x1402)))+(((cj4)*(r02)*(sj5)))+(((x1400)*(x1401)))))+IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x1401)))+(((sj5)*(x1402)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
06292     continue;
06293 j3array[0]=IKatan2(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x1400)*(x1402)))+(((cj4)*(r02)*(sj5)))+(((x1400)*(x1401)))), ((((IkReal(-1.00000000000000))*(sj5)*(x1401)))+(((sj5)*(x1402)))+(((cj5)*(r02)))));
06294 sj3array[0]=IKsin(j3array[0]);
06295 cj3array[0]=IKcos(j3array[0]);
06296 if( j3array[0] > IKPI )
06297 {
06298     j3array[0]-=IK2PI;
06299 }
06300 else if( j3array[0] < -IKPI )
06301 {    j3array[0]+=IK2PI;
06302 }
06303 j3valid[0] = true;
06304 for(int ij3 = 0; ij3 < 1; ++ij3)
06305 {
06306 if( !j3valid[ij3] )
06307 {
06308     continue;
06309 }
06310 _ij3[0] = ij3; _ij3[1] = -1;
06311 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06312 {
06313 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06314 {
06315     j3valid[iij3]=false; _ij3[1] = iij3; break; 
06316 }
06317 }
06318 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06319 {
06320 IkReal evalcond[6];
06321 IkReal x1403=IKsin(j3);
06322 IkReal x1404=IKcos(j3);
06323 IkReal x1405=((r11)*(sj6));
06324 IkReal x1406=((IkReal(1.00000000000000))*(sj5));
06325 IkReal x1407=((cj6)*(sj4));
06326 IkReal x1408=((sj4)*(sj6));
06327 IkReal x1409=((cj4)*(sj5));
06328 IkReal x1410=((cj4)*(cj5));
06329 IkReal x1411=((r01)*(sj6));
06330 IkReal x1412=((r21)*(sj6));
06331 IkReal x1413=((cj6)*(sj5));
06332 IkReal x1414=((IkReal(1.00000000000000))*(x1404));
06333 IkReal x1415=((IkReal(1.00000000000000))*(cj6)*(x1410));
06334 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1413)))+(((sj2)*(x1403)))+(((IkReal(-1.00000000000000))*(x1406)*(x1412))));
06335 evalcond[1]=((((IkReal(-1.00000000000000))*(x1414)))+(((r00)*(x1413)))+(((IkReal(-1.00000000000000))*(x1406)*(x1411)))+(((cj5)*(r02))));
06336 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x1405)*(x1406)))+(((r10)*(x1413)))+(((cj2)*(x1403))));
06337 evalcond[3]=((((r20)*(x1408)))+(((r21)*(x1407)))+(((x1410)*(x1412)))+(((IkReal(-1.00000000000000))*(sj2)*(x1414)))+(((IkReal(-1.00000000000000))*(r20)*(x1415)))+(((r22)*(x1409))));
06338 evalcond[4]=((((IkReal(-1.00000000000000))*(x1403)))+(((r02)*(x1409)))+(((r00)*(x1408)))+(((r01)*(x1407)))+(((x1410)*(x1411)))+(((IkReal(-1.00000000000000))*(r00)*(x1415))));
06339 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1415)))+(((IkReal(-1.00000000000000))*(cj2)*(x1414)))+(((r12)*(x1409)))+(((r11)*(x1407)))+(((x1405)*(x1410)))+(((r10)*(x1408))));
06340 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
06341 {
06342 continue;
06343 }
06344 }
06345 
06346 {
06347 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
06348 vinfos[0].jointtype = 1;
06349 vinfos[0].foffset = j0;
06350 vinfos[0].indices[0] = _ij0[0];
06351 vinfos[0].indices[1] = _ij0[1];
06352 vinfos[0].maxsolutions = _nj0;
06353 vinfos[1].jointtype = 1;
06354 vinfos[1].foffset = j1;
06355 vinfos[1].indices[0] = _ij1[0];
06356 vinfos[1].indices[1] = _ij1[1];
06357 vinfos[1].maxsolutions = _nj1;
06358 vinfos[2].jointtype = 1;
06359 vinfos[2].foffset = j2;
06360 vinfos[2].indices[0] = _ij2[0];
06361 vinfos[2].indices[1] = _ij2[1];
06362 vinfos[2].maxsolutions = _nj2;
06363 vinfos[3].jointtype = 1;
06364 vinfos[3].foffset = j3;
06365 vinfos[3].indices[0] = _ij3[0];
06366 vinfos[3].indices[1] = _ij3[1];
06367 vinfos[3].maxsolutions = _nj3;
06368 vinfos[4].jointtype = 1;
06369 vinfos[4].foffset = j4;
06370 vinfos[4].indices[0] = _ij4[0];
06371 vinfos[4].indices[1] = _ij4[1];
06372 vinfos[4].maxsolutions = _nj4;
06373 vinfos[5].jointtype = 1;
06374 vinfos[5].foffset = j5;
06375 vinfos[5].indices[0] = _ij5[0];
06376 vinfos[5].indices[1] = _ij5[1];
06377 vinfos[5].maxsolutions = _nj5;
06378 vinfos[6].jointtype = 1;
06379 vinfos[6].foffset = j6;
06380 vinfos[6].indices[0] = _ij6[0];
06381 vinfos[6].indices[1] = _ij6[1];
06382 vinfos[6].maxsolutions = _nj6;
06383 std::vector<int> vfree(0);
06384 solutions.AddSolution(vinfos,vfree);
06385 }
06386 }
06387 }
06388 }
06389 }
06390 
06391 }
06392 
06393 }
06394 }
06395 }
06396 
06397 } else
06398 {
06399 if( 1 )
06400 {
06401 continue;
06402 
06403 } else
06404 {
06405 }
06406 }
06407 }
06408 }
06409 }
06410 }
06411 
06412 } else
06413 {
06414 {
06415 IkReal j2array[1], cj2array[1], sj2array[1];
06416 bool j2valid[1]={false};
06417 _nj2 = 1;
06418 IkReal x1416=((IkReal(195.000000000000))*(cj5));
06419 IkReal x1417=((IkReal(0.250000000000000))*(cj6));
06420 IkReal x1418=((IkReal(0.250000000000000))*(sj6));
06421 IkReal x1419=((IkReal(195.000000000000))*(cj6)*(sj5));
06422 IkReal x1420=((IkReal(195.000000000000))*(sj5)*(sj6));
06423 if( IKabs(((((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))*(((((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(x1420)))+(((r11)*(x1417)))+(((r12)*(x1416)))+(((r10)*(x1418)))+(((r10)*(x1419)))+(((IkReal(200.000000000000))*(cj1))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r21)*(x1417)))+(((r20)*(x1418)))+(((r20)*(x1419)))+(((IkReal(-1.00000000000000))*(r21)*(x1420)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x1416))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))*(((((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(x1420)))+(((r11)*(x1417)))+(((r12)*(x1416)))+(((r10)*(x1418)))+(((r10)*(x1419)))+(((IkReal(200.000000000000))*(cj1)))))))+IKsqr(((((r21)*(x1417)))+(((r20)*(x1418)))+(((r20)*(x1419)))+(((IkReal(-1.00000000000000))*(r21)*(x1420)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x1416)))))-1) <= IKFAST_SINCOS_THRESH )
06424     continue;
06425 j2array[0]=IKatan2(((((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))*(((((IkReal(-500.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(x1420)))+(((r11)*(x1417)))+(((r12)*(x1416)))+(((r10)*(x1418)))+(((r10)*(x1419)))+(((IkReal(200.000000000000))*(cj1)))))), ((((r21)*(x1417)))+(((r20)*(x1418)))+(((r20)*(x1419)))+(((IkReal(-1.00000000000000))*(r21)*(x1420)))+(((IkReal(-500.000000000000))*(pz)))+(((r22)*(x1416)))));
06426 sj2array[0]=IKsin(j2array[0]);
06427 cj2array[0]=IKcos(j2array[0]);
06428 if( j2array[0] > IKPI )
06429 {
06430     j2array[0]-=IK2PI;
06431 }
06432 else if( j2array[0] < -IKPI )
06433 {    j2array[0]+=IK2PI;
06434 }
06435 j2valid[0] = true;
06436 for(int ij2 = 0; ij2 < 1; ++ij2)
06437 {
06438 if( !j2valid[ij2] )
06439 {
06440     continue;
06441 }
06442 _ij2[0] = ij2; _ij2[1] = -1;
06443 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
06444 {
06445 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
06446 {
06447     j2valid[iij2]=false; _ij2[1] = iij2; break; 
06448 }
06449 }
06450 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
06451 {
06452 IkReal evalcond[3];
06453 IkReal x1421=IKsin(j2);
06454 IkReal x1422=((IkReal(0.390000000000000))*(sj5));
06455 IkReal x1423=((IkReal(0.390000000000000))*(cj5));
06456 IkReal x1424=((IkReal(0.000500000000000000))*(cj6));
06457 IkReal x1425=((IkReal(0.000500000000000000))*(sj6));
06458 IkReal x1426=((IkReal(0.00200000000000000))*(x1421));
06459 evalcond[0]=((((r21)*(x1424)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((r22)*(x1423)))+(((IkReal(-1.00000000000000))*(pz)))+(((cj6)*(r20)*(x1422)))+(((r20)*(x1425)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x1422))));
06460 evalcond[1]=((((r01)*(x1424)))+(((cj6)*(r00)*(x1422)))+(((r00)*(x1425)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1422)))+(((r02)*(x1423)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj1)*(x1426))));
06461 evalcond[2]=((((r11)*(x1424)))+(((IkReal(0.400000000000000))*(cj1)))+(((r10)*(x1425)))+(((IkReal(-1.00000000000000))*(sj1)*(x1426)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x1422)))+(((cj6)*(r10)*(x1422)))+(((r12)*(x1423))));
06462 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
06463 {
06464 continue;
06465 }
06466 }
06467 
06468 {
06469 IkReal dummyeval[1];
06470 IkReal gconst0;
06471 IkReal x1427=(sj6)*(sj6);
06472 IkReal x1428=(cj6)*(cj6);
06473 IkReal x1429=((IkReal(1.00000000000000))*(r20));
06474 IkReal x1430=((r00)*(r21));
06475 IkReal x1431=((r22)*(sj5));
06476 IkReal x1432=((r02)*(sj5));
06477 IkReal x1433=((cj5)*(x1427));
06478 IkReal x1434=((cj5)*(x1428));
06479 gconst0=IKsign(((((IkReal(-1.00000000000000))*(r01)*(x1429)*(x1433)))+(((IkReal(-1.00000000000000))*(r01)*(x1429)*(x1434)))+(((x1430)*(x1434)))+(((x1430)*(x1433)))+(((IkReal(-1.00000000000000))*(sj6)*(x1429)*(x1432)))+(((r00)*(sj6)*(x1431)))+(((cj6)*(r01)*(x1431)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1432)))));
06480 IkReal x1435=(sj6)*(sj6);
06481 IkReal x1436=(cj6)*(cj6);
06482 IkReal x1437=((IkReal(1.00000000000000))*(r20));
06483 IkReal x1438=((r00)*(r21));
06484 IkReal x1439=((r22)*(sj5));
06485 IkReal x1440=((r02)*(sj5));
06486 IkReal x1441=((cj5)*(x1435));
06487 IkReal x1442=((cj5)*(x1436));
06488 dummyeval[0]=((((x1438)*(x1442)))+(((x1438)*(x1441)))+(((IkReal(-1.00000000000000))*(r01)*(x1437)*(x1442)))+(((IkReal(-1.00000000000000))*(r01)*(x1437)*(x1441)))+(((r00)*(sj6)*(x1439)))+(((IkReal(-1.00000000000000))*(sj6)*(x1437)*(x1440)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1440)))+(((cj6)*(r01)*(x1439))));
06489 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06490 {
06491 {
06492 IkReal dummyeval[1];
06493 IkReal gconst1;
06494 IkReal x1443=(sj6)*(sj6);
06495 IkReal x1444=(cj6)*(cj6);
06496 IkReal x1445=((cj6)*(sj5));
06497 IkReal x1446=((r10)*(r21));
06498 IkReal x1447=((IkReal(1.00000000000000))*(r12));
06499 IkReal x1448=((sj5)*(sj6));
06500 IkReal x1449=((cj5)*(x1444));
06501 IkReal x1450=((IkReal(1.00000000000000))*(r11)*(r20));
06502 IkReal x1451=((cj5)*(x1443));
06503 gconst1=IKsign(((((x1446)*(x1451)))+(((IkReal(-1.00000000000000))*(r21)*(x1445)*(x1447)))+(((IkReal(-1.00000000000000))*(r20)*(x1447)*(x1448)))+(((r10)*(r22)*(x1448)))+(((r11)*(r22)*(x1445)))+(((IkReal(-1.00000000000000))*(x1449)*(x1450)))+(((x1446)*(x1449)))+(((IkReal(-1.00000000000000))*(x1450)*(x1451)))));
06504 IkReal x1452=(sj6)*(sj6);
06505 IkReal x1453=(cj6)*(cj6);
06506 IkReal x1454=((cj6)*(sj5));
06507 IkReal x1455=((r10)*(r21));
06508 IkReal x1456=((IkReal(1.00000000000000))*(r12));
06509 IkReal x1457=((sj5)*(sj6));
06510 IkReal x1458=((cj5)*(x1453));
06511 IkReal x1459=((IkReal(1.00000000000000))*(r11)*(r20));
06512 IkReal x1460=((cj5)*(x1452));
06513 dummyeval[0]=((((IkReal(-1.00000000000000))*(r20)*(x1456)*(x1457)))+(((x1455)*(x1458)))+(((IkReal(-1.00000000000000))*(x1458)*(x1459)))+(((r11)*(r22)*(x1454)))+(((IkReal(-1.00000000000000))*(x1459)*(x1460)))+(((x1455)*(x1460)))+(((IkReal(-1.00000000000000))*(r21)*(x1454)*(x1456)))+(((r10)*(r22)*(x1457))));
06514 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06515 {
06516 {
06517 IkReal dummyeval[2];
06518 IkReal x1461=(sj1)*(sj1);
06519 IkReal x1462=(cj1)*(cj1);
06520 dummyeval[0]=((((cj2)*(x1462)))+(((cj2)*(x1461))));
06521 dummyeval[1]=((x1461)+(x1462));
06522 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
06523 {
06524 {
06525 IkReal evalcond[4];
06526 IkReal x1463=((IkReal(0.390000000000000))*(sj5));
06527 IkReal x1464=((IkReal(0.390000000000000))*(cj5));
06528 IkReal x1465=((IkReal(0.000500000000000000))*(cj6));
06529 IkReal x1466=((IkReal(0.000500000000000000))*(sj6));
06530 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
06531 evalcond[1]=((((cj6)*(r20)*(x1463)))+(((r20)*(x1466)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x1463)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x1464)))+(((r21)*(x1465))));
06532 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1463)))+(((r01)*(x1465)))+(((r00)*(x1466)))+(((cj6)*(r00)*(x1463)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x1464)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1))));
06533 evalcond[3]=((((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x1464)))+(((IkReal(-0.00200000000000000))*(sj1)))+(((r11)*(x1465)))+(((r10)*(x1466)))+(((cj6)*(r10)*(x1463)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x1463))));
06534 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
06535 {
06536 {
06537 IkReal dummyeval[1];
06538 IkReal gconst9;
06539 IkReal x1467=(sj6)*(sj6);
06540 IkReal x1468=(cj6)*(cj6);
06541 IkReal x1469=((IkReal(1.00000000000000))*(r20));
06542 IkReal x1470=((r00)*(r21));
06543 IkReal x1471=((r22)*(sj5));
06544 IkReal x1472=((r02)*(sj5));
06545 IkReal x1473=((cj5)*(x1467));
06546 IkReal x1474=((cj5)*(x1468));
06547 gconst9=IKsign(((((r00)*(sj6)*(x1471)))+(((cj6)*(r01)*(x1471)))+(((IkReal(-1.00000000000000))*(sj6)*(x1469)*(x1472)))+(((x1470)*(x1474)))+(((x1470)*(x1473)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1472)))+(((IkReal(-1.00000000000000))*(r01)*(x1469)*(x1473)))+(((IkReal(-1.00000000000000))*(r01)*(x1469)*(x1474)))));
06548 IkReal x1475=(sj6)*(sj6);
06549 IkReal x1476=(cj6)*(cj6);
06550 IkReal x1477=((IkReal(1.00000000000000))*(r20));
06551 IkReal x1478=((r00)*(r21));
06552 IkReal x1479=((r22)*(sj5));
06553 IkReal x1480=((r02)*(sj5));
06554 IkReal x1481=((cj5)*(x1475));
06555 IkReal x1482=((cj5)*(x1476));
06556 dummyeval[0]=((((r00)*(sj6)*(x1479)))+(((cj6)*(r01)*(x1479)))+(((IkReal(-1.00000000000000))*(sj6)*(x1477)*(x1480)))+(((IkReal(-1.00000000000000))*(r01)*(x1477)*(x1481)))+(((IkReal(-1.00000000000000))*(r01)*(x1477)*(x1482)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1480)))+(((x1478)*(x1482)))+(((x1478)*(x1481))));
06557 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06558 {
06559 {
06560 IkReal dummyeval[1];
06561 IkReal gconst10;
06562 IkReal x1483=(sj6)*(sj6);
06563 IkReal x1484=(cj6)*(cj6);
06564 IkReal x1485=((cj6)*(sj5));
06565 IkReal x1486=((r10)*(r21));
06566 IkReal x1487=((IkReal(1.00000000000000))*(r12));
06567 IkReal x1488=((sj5)*(sj6));
06568 IkReal x1489=((cj5)*(x1484));
06569 IkReal x1490=((IkReal(1.00000000000000))*(r11)*(r20));
06570 IkReal x1491=((cj5)*(x1483));
06571 gconst10=IKsign(((((IkReal(-1.00000000000000))*(r20)*(x1487)*(x1488)))+(((x1486)*(x1491)))+(((r10)*(r22)*(x1488)))+(((x1486)*(x1489)))+(((r11)*(r22)*(x1485)))+(((IkReal(-1.00000000000000))*(r21)*(x1485)*(x1487)))+(((IkReal(-1.00000000000000))*(x1490)*(x1491)))+(((IkReal(-1.00000000000000))*(x1489)*(x1490)))));
06572 IkReal x1492=(sj6)*(sj6);
06573 IkReal x1493=(cj6)*(cj6);
06574 IkReal x1494=((cj6)*(sj5));
06575 IkReal x1495=((r10)*(r21));
06576 IkReal x1496=((IkReal(1.00000000000000))*(r12));
06577 IkReal x1497=((sj5)*(sj6));
06578 IkReal x1498=((cj5)*(x1493));
06579 IkReal x1499=((IkReal(1.00000000000000))*(r11)*(r20));
06580 IkReal x1500=((cj5)*(x1492));
06581 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1499)*(x1500)))+(((x1495)*(x1498)))+(((r11)*(r22)*(x1494)))+(((r10)*(r22)*(x1497)))+(((x1495)*(x1500)))+(((IkReal(-1.00000000000000))*(r20)*(x1496)*(x1497)))+(((IkReal(-1.00000000000000))*(x1498)*(x1499)))+(((IkReal(-1.00000000000000))*(r21)*(x1494)*(x1496))));
06582 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06583 {
06584 {
06585 IkReal dummyeval[1];
06586 dummyeval[0]=cj1;
06587 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06588 {
06589 {
06590 IkReal evalcond[5];
06591 IkReal x1501=((IkReal(0.000500000000000000))*(cj6));
06592 IkReal x1502=((IkReal(0.000500000000000000))*(sj6));
06593 IkReal x1503=((sj5)*(sj6));
06594 IkReal x1504=((cj5)*(r12));
06595 IkReal x1505=((IkReal(0.390000000000000))*(cj5));
06596 IkReal x1506=((IkReal(0.390000000000000))*(cj6)*(sj5));
06597 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
06598 evalcond[1]=((((cj6)*(r10)*(sj5)))+(x1504)+(((IkReal(-1.00000000000000))*(r11)*(x1503))));
06599 evalcond[2]=((((r20)*(x1506)))+(((r20)*(x1502)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.390000000000000))*(r21)*(x1503)))+(((r21)*(x1501)))+(((r22)*(x1505))));
06600 evalcond[3]=((IkReal(-0.400000000000000))+(((r01)*(x1501)))+(((r00)*(x1506)))+(((r00)*(x1502)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.390000000000000))*(r01)*(x1503)))+(((r02)*(x1505))));
06601 evalcond[4]=((IkReal(-0.00200000000000000))+(((IkReal(0.390000000000000))*(x1504)))+(((r11)*(x1501)))+(((r10)*(x1506)))+(((r10)*(x1502)))+(((IkReal(-0.390000000000000))*(r11)*(x1503)))+(((IkReal(-1.00000000000000))*(py))));
06602 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
06603 {
06604 {
06605 IkReal j3array[1], cj3array[1], sj3array[1];
06606 bool j3valid[1]={false};
06607 _nj3 = 1;
06608 IkReal x1507=((sj5)*(sj6));
06609 IkReal x1508=((IkReal(1.00000000000000))*(cj5));
06610 IkReal x1509=((IkReal(1.00000000000000))*(cj6)*(sj5));
06611 if( IKabs(((((IkReal(-1.00000000000000))*(r20)*(x1509)))+(((IkReal(-1.00000000000000))*(r22)*(x1508)))+(((r21)*(x1507))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r00)*(x1509)))+(((r01)*(x1507)))+(((IkReal(-1.00000000000000))*(r02)*(x1508))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r20)*(x1509)))+(((IkReal(-1.00000000000000))*(r22)*(x1508)))+(((r21)*(x1507)))))+IKsqr(((((IkReal(-1.00000000000000))*(r00)*(x1509)))+(((r01)*(x1507)))+(((IkReal(-1.00000000000000))*(r02)*(x1508)))))-1) <= IKFAST_SINCOS_THRESH )
06612     continue;
06613 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r20)*(x1509)))+(((IkReal(-1.00000000000000))*(r22)*(x1508)))+(((r21)*(x1507)))), ((((IkReal(-1.00000000000000))*(r00)*(x1509)))+(((r01)*(x1507)))+(((IkReal(-1.00000000000000))*(r02)*(x1508)))));
06614 sj3array[0]=IKsin(j3array[0]);
06615 cj3array[0]=IKcos(j3array[0]);
06616 if( j3array[0] > IKPI )
06617 {
06618     j3array[0]-=IK2PI;
06619 }
06620 else if( j3array[0] < -IKPI )
06621 {    j3array[0]+=IK2PI;
06622 }
06623 j3valid[0] = true;
06624 for(int ij3 = 0; ij3 < 1; ++ij3)
06625 {
06626 if( !j3valid[ij3] )
06627 {
06628     continue;
06629 }
06630 _ij3[0] = ij3; _ij3[1] = -1;
06631 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06632 {
06633 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06634 {
06635     j3valid[iij3]=false; _ij3[1] = iij3; break; 
06636 }
06637 }
06638 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06639 {
06640 IkReal evalcond[2];
06641 IkReal x1510=((cj6)*(sj5));
06642 IkReal x1511=((IkReal(1.00000000000000))*(sj5)*(sj6));
06643 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1510)))+(IKsin(j3))+(((IkReal(-1.00000000000000))*(r21)*(x1511))));
06644 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1511)))+(((r00)*(x1510)))+(IKcos(j3))+(((cj5)*(r02))));
06645 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
06646 {
06647 continue;
06648 }
06649 }
06650 
06651 {
06652 IkReal dummyeval[1];
06653 IkReal gconst15;
06654 IkReal x1512=(sj6)*(sj6);
06655 IkReal x1513=(cj6)*(cj6);
06656 IkReal x1514=((r01)*(r10));
06657 IkReal x1515=((IkReal(1.00000000000000))*(r00));
06658 IkReal x1516=((r02)*(sj5));
06659 IkReal x1517=((r12)*(sj5));
06660 IkReal x1518=((cj5)*(x1512));
06661 IkReal x1519=((cj5)*(x1513));
06662 gconst15=IKsign(((((IkReal(-1.00000000000000))*(r11)*(x1515)*(x1518)))+(((IkReal(-1.00000000000000))*(r11)*(x1515)*(x1519)))+(((x1514)*(x1518)))+(((x1514)*(x1519)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1517)))+(((r10)*(sj6)*(x1516)))+(((IkReal(-1.00000000000000))*(sj6)*(x1515)*(x1517)))+(((cj6)*(r11)*(x1516)))));
06663 IkReal x1520=(sj6)*(sj6);
06664 IkReal x1521=(cj6)*(cj6);
06665 IkReal x1522=((r01)*(r10));
06666 IkReal x1523=((IkReal(1.00000000000000))*(r00));
06667 IkReal x1524=((r02)*(sj5));
06668 IkReal x1525=((r12)*(sj5));
06669 IkReal x1526=((cj5)*(x1520));
06670 IkReal x1527=((cj5)*(x1521));
06671 dummyeval[0]=((((cj6)*(r11)*(x1524)))+(((x1522)*(x1527)))+(((x1522)*(x1526)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1525)))+(((r10)*(sj6)*(x1524)))+(((IkReal(-1.00000000000000))*(sj6)*(x1523)*(x1525)))+(((IkReal(-1.00000000000000))*(r11)*(x1523)*(x1526)))+(((IkReal(-1.00000000000000))*(r11)*(x1523)*(x1527))));
06672 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06673 {
06674 {
06675 IkReal dummyeval[1];
06676 IkReal gconst16;
06677 IkReal x1528=(cj5)*(cj5);
06678 IkReal x1529=(sj6)*(sj6);
06679 IkReal x1530=(cj6)*(cj6);
06680 IkReal x1531=((r00)*(r20));
06681 IkReal x1532=((cj5)*(sj5));
06682 IkReal x1533=((r01)*(r21));
06683 IkReal x1534=((r21)*(sj6));
06684 IkReal x1535=((sj6)*(x1532));
06685 IkReal x1536=((IkReal(1.00000000000000))*(cj6)*(r00));
06686 IkReal x1537=((cj6)*(r01)*(r20)*(sj6));
06687 gconst16=IKsign(((((r01)*(r22)*(x1535)))+(((r02)*(x1532)*(x1534)))+(((x1528)*(x1530)*(x1531)))+(((cj6)*(r00)*(x1534)))+(((IkReal(-1.00000000000000))*(x1528)*(x1534)*(x1536)))+(((IkReal(-1.00000000000000))*(r22)*(x1532)*(x1536)))+(((x1530)*(x1533)))+(((IkReal(-1.00000000000000))*(x1528)*(x1537)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1532)))+(x1537)+(((x1529)*(x1531)))+(((r02)*(r22)*((sj5)*(sj5))))+(((x1528)*(x1529)*(x1533)))));
06688 IkReal x1538=(cj5)*(cj5);
06689 IkReal x1539=(sj6)*(sj6);
06690 IkReal x1540=(cj6)*(cj6);
06691 IkReal x1541=((r00)*(r20));
06692 IkReal x1542=((cj5)*(sj5));
06693 IkReal x1543=((r01)*(r21));
06694 IkReal x1544=((r21)*(sj6));
06695 IkReal x1545=((sj6)*(x1542));
06696 IkReal x1546=((IkReal(1.00000000000000))*(cj6)*(r00));
06697 IkReal x1547=x1537;
06698 dummyeval[0]=((((r02)*(x1542)*(x1544)))+(((x1539)*(x1541)))+(((IkReal(-1.00000000000000))*(r22)*(x1542)*(x1546)))+(((x1538)*(x1540)*(x1541)))+(x1547)+(((r01)*(r22)*(x1545)))+(((x1538)*(x1539)*(x1543)))+(((r02)*(r22)*((sj5)*(sj5))))+(((x1540)*(x1543)))+(((IkReal(-1.00000000000000))*(x1538)*(x1544)*(x1546)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1542)))+(((cj6)*(r00)*(x1544)))+(((IkReal(-1.00000000000000))*(x1538)*(x1547))));
06699 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
06700 {
06701 continue;
06702 
06703 } else
06704 {
06705 {
06706 IkReal j4array[1], cj4array[1], sj4array[1];
06707 bool j4valid[1]={false};
06708 _nj4 = 1;
06709 IkReal x1548=((IkReal(1.00000000000000))*(sj3));
06710 if( IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1548)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1548))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1548)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1548)))+(((cj5)*(cj6)*(r20)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
06711     continue;
06712 j4array[0]=IKatan2(((gconst16)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1548)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1548)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1548)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1548)))+(((cj5)*(cj6)*(r20)*(sj3)))))));
06713 sj4array[0]=IKsin(j4array[0]);
06714 cj4array[0]=IKcos(j4array[0]);
06715 if( j4array[0] > IKPI )
06716 {
06717     j4array[0]-=IK2PI;
06718 }
06719 else if( j4array[0] < -IKPI )
06720 {    j4array[0]+=IK2PI;
06721 }
06722 j4valid[0] = true;
06723 for(int ij4 = 0; ij4 < 1; ++ij4)
06724 {
06725 if( !j4valid[ij4] )
06726 {
06727     continue;
06728 }
06729 _ij4[0] = ij4; _ij4[1] = -1;
06730 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06731 {
06732 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06733 {
06734     j4valid[iij4]=false; _ij4[1] = iij4; break; 
06735 }
06736 }
06737 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06738 {
06739 IkReal evalcond[6];
06740 IkReal x1549=IKsin(j4);
06741 IkReal x1550=IKcos(j4);
06742 IkReal x1551=((IkReal(1.00000000000000))*(r00));
06743 IkReal x1552=((cj5)*(r01));
06744 IkReal x1553=((cj5)*(r11));
06745 IkReal x1554=((IkReal(1.00000000000000))*(cj5));
06746 IkReal x1555=((cj5)*(r21));
06747 IkReal x1556=((sj5)*(x1549));
06748 IkReal x1557=((cj6)*(x1549));
06749 IkReal x1558=((sj6)*(x1550));
06750 IkReal x1559=((sj6)*(x1549));
06751 IkReal x1560=((sj5)*(x1550));
06752 IkReal x1561=((cj6)*(x1550));
06753 evalcond[0]=((((x1555)*(x1559)))+(((IkReal(-1.00000000000000))*(r21)*(x1561)))+(((r22)*(x1556)))+(((IkReal(-1.00000000000000))*(r20)*(x1554)*(x1557)))+(((IkReal(-1.00000000000000))*(r20)*(x1558))));
06754 evalcond[1]=((((x1555)*(x1558)))+(((r22)*(x1560)))+(((r21)*(x1557)))+(((r20)*(x1559)))+(((IkReal(-1.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(r20)*(x1554)*(x1561))));
06755 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1561)))+(((x1552)*(x1559)))+(((IkReal(-1.00000000000000))*(cj5)*(x1551)*(x1557)))+(((r02)*(x1556)))+(((IkReal(-1.00000000000000))*(x1551)*(x1558))));
06756 evalcond[3]=((IkReal(-1.00000000000000))+(((x1553)*(x1559)))+(((IkReal(-1.00000000000000))*(r10)*(x1554)*(x1557)))+(((r12)*(x1556)))+(((IkReal(-1.00000000000000))*(r10)*(x1558)))+(((IkReal(-1.00000000000000))*(r11)*(x1561))));
06757 evalcond[4]=((((r00)*(x1559)))+(((r01)*(x1557)))+(((IkReal(-1.00000000000000))*(cj5)*(x1551)*(x1561)))+(sj3)+(((x1552)*(x1558)))+(((r02)*(x1560))));
06758 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1554)*(x1561)))+(((x1553)*(x1558)))+(((r11)*(x1557)))+(((r12)*(x1560)))+(((r10)*(x1559))));
06759 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
06760 {
06761 continue;
06762 }
06763 }
06764 
06765 {
06766 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
06767 vinfos[0].jointtype = 1;
06768 vinfos[0].foffset = j0;
06769 vinfos[0].indices[0] = _ij0[0];
06770 vinfos[0].indices[1] = _ij0[1];
06771 vinfos[0].maxsolutions = _nj0;
06772 vinfos[1].jointtype = 1;
06773 vinfos[1].foffset = j1;
06774 vinfos[1].indices[0] = _ij1[0];
06775 vinfos[1].indices[1] = _ij1[1];
06776 vinfos[1].maxsolutions = _nj1;
06777 vinfos[2].jointtype = 1;
06778 vinfos[2].foffset = j2;
06779 vinfos[2].indices[0] = _ij2[0];
06780 vinfos[2].indices[1] = _ij2[1];
06781 vinfos[2].maxsolutions = _nj2;
06782 vinfos[3].jointtype = 1;
06783 vinfos[3].foffset = j3;
06784 vinfos[3].indices[0] = _ij3[0];
06785 vinfos[3].indices[1] = _ij3[1];
06786 vinfos[3].maxsolutions = _nj3;
06787 vinfos[4].jointtype = 1;
06788 vinfos[4].foffset = j4;
06789 vinfos[4].indices[0] = _ij4[0];
06790 vinfos[4].indices[1] = _ij4[1];
06791 vinfos[4].maxsolutions = _nj4;
06792 vinfos[5].jointtype = 1;
06793 vinfos[5].foffset = j5;
06794 vinfos[5].indices[0] = _ij5[0];
06795 vinfos[5].indices[1] = _ij5[1];
06796 vinfos[5].maxsolutions = _nj5;
06797 vinfos[6].jointtype = 1;
06798 vinfos[6].foffset = j6;
06799 vinfos[6].indices[0] = _ij6[0];
06800 vinfos[6].indices[1] = _ij6[1];
06801 vinfos[6].maxsolutions = _nj6;
06802 std::vector<int> vfree(0);
06803 solutions.AddSolution(vinfos,vfree);
06804 }
06805 }
06806 }
06807 
06808 }
06809 
06810 }
06811 
06812 } else
06813 {
06814 {
06815 IkReal j4array[1], cj4array[1], sj4array[1];
06816 bool j4valid[1]={false};
06817 _nj4 = 1;
06818 IkReal x1562=((cj5)*(sj3));
06819 IkReal x1563=((IkReal(1.00000000000000))*(r10));
06820 if( IKabs(((gconst15)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(x1562)*(x1563)))+(((r11)*(sj6)*(x1562))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((IkReal(-1.00000000000000))*(sj3)*(sj6)*(x1563)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
06821     continue;
06822 j4array[0]=IKatan2(((gconst15)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(x1562)*(x1563)))+(((r11)*(sj6)*(x1562)))))), ((gconst15)*(((((IkReal(-1.00000000000000))*(sj3)*(sj6)*(x1563)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3)))))));
06823 sj4array[0]=IKsin(j4array[0]);
06824 cj4array[0]=IKcos(j4array[0]);
06825 if( j4array[0] > IKPI )
06826 {
06827     j4array[0]-=IK2PI;
06828 }
06829 else if( j4array[0] < -IKPI )
06830 {    j4array[0]+=IK2PI;
06831 }
06832 j4valid[0] = true;
06833 for(int ij4 = 0; ij4 < 1; ++ij4)
06834 {
06835 if( !j4valid[ij4] )
06836 {
06837     continue;
06838 }
06839 _ij4[0] = ij4; _ij4[1] = -1;
06840 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06841 {
06842 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06843 {
06844     j4valid[iij4]=false; _ij4[1] = iij4; break; 
06845 }
06846 }
06847 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06848 {
06849 IkReal evalcond[6];
06850 IkReal x1564=IKsin(j4);
06851 IkReal x1565=IKcos(j4);
06852 IkReal x1566=((IkReal(1.00000000000000))*(r00));
06853 IkReal x1567=((cj5)*(r01));
06854 IkReal x1568=((cj5)*(r11));
06855 IkReal x1569=((IkReal(1.00000000000000))*(cj5));
06856 IkReal x1570=((cj5)*(r21));
06857 IkReal x1571=((sj5)*(x1564));
06858 IkReal x1572=((cj6)*(x1564));
06859 IkReal x1573=((sj6)*(x1565));
06860 IkReal x1574=((sj6)*(x1564));
06861 IkReal x1575=((sj5)*(x1565));
06862 IkReal x1576=((cj6)*(x1565));
06863 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1573)))+(((x1570)*(x1574)))+(((IkReal(-1.00000000000000))*(r20)*(x1569)*(x1572)))+(((IkReal(-1.00000000000000))*(r21)*(x1576)))+(((r22)*(x1571))));
06864 evalcond[1]=((((r20)*(x1574)))+(((x1570)*(x1573)))+(((IkReal(-1.00000000000000))*(r20)*(x1569)*(x1576)))+(((r21)*(x1572)))+(((r22)*(x1575)))+(((IkReal(-1.00000000000000))*(cj3))));
06865 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1576)))+(((x1567)*(x1574)))+(((IkReal(-1.00000000000000))*(x1566)*(x1573)))+(((IkReal(-1.00000000000000))*(cj5)*(x1566)*(x1572)))+(((r02)*(x1571))));
06866 evalcond[3]=((IkReal(-1.00000000000000))+(((r12)*(x1571)))+(((x1568)*(x1574)))+(((IkReal(-1.00000000000000))*(r10)*(x1573)))+(((IkReal(-1.00000000000000))*(r10)*(x1569)*(x1572)))+(((IkReal(-1.00000000000000))*(r11)*(x1576))));
06867 evalcond[4]=((((x1567)*(x1573)))+(sj3)+(((IkReal(-1.00000000000000))*(cj5)*(x1566)*(x1576)))+(((r02)*(x1575)))+(((r01)*(x1572)))+(((r00)*(x1574))));
06868 evalcond[5]=((((r12)*(x1575)))+(((x1568)*(x1573)))+(((r11)*(x1572)))+(((r10)*(x1574)))+(((IkReal(-1.00000000000000))*(r10)*(x1569)*(x1576))));
06869 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
06870 {
06871 continue;
06872 }
06873 }
06874 
06875 {
06876 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
06877 vinfos[0].jointtype = 1;
06878 vinfos[0].foffset = j0;
06879 vinfos[0].indices[0] = _ij0[0];
06880 vinfos[0].indices[1] = _ij0[1];
06881 vinfos[0].maxsolutions = _nj0;
06882 vinfos[1].jointtype = 1;
06883 vinfos[1].foffset = j1;
06884 vinfos[1].indices[0] = _ij1[0];
06885 vinfos[1].indices[1] = _ij1[1];
06886 vinfos[1].maxsolutions = _nj1;
06887 vinfos[2].jointtype = 1;
06888 vinfos[2].foffset = j2;
06889 vinfos[2].indices[0] = _ij2[0];
06890 vinfos[2].indices[1] = _ij2[1];
06891 vinfos[2].maxsolutions = _nj2;
06892 vinfos[3].jointtype = 1;
06893 vinfos[3].foffset = j3;
06894 vinfos[3].indices[0] = _ij3[0];
06895 vinfos[3].indices[1] = _ij3[1];
06896 vinfos[3].maxsolutions = _nj3;
06897 vinfos[4].jointtype = 1;
06898 vinfos[4].foffset = j4;
06899 vinfos[4].indices[0] = _ij4[0];
06900 vinfos[4].indices[1] = _ij4[1];
06901 vinfos[4].maxsolutions = _nj4;
06902 vinfos[5].jointtype = 1;
06903 vinfos[5].foffset = j5;
06904 vinfos[5].indices[0] = _ij5[0];
06905 vinfos[5].indices[1] = _ij5[1];
06906 vinfos[5].maxsolutions = _nj5;
06907 vinfos[6].jointtype = 1;
06908 vinfos[6].foffset = j6;
06909 vinfos[6].indices[0] = _ij6[0];
06910 vinfos[6].indices[1] = _ij6[1];
06911 vinfos[6].maxsolutions = _nj6;
06912 std::vector<int> vfree(0);
06913 solutions.AddSolution(vinfos,vfree);
06914 }
06915 }
06916 }
06917 
06918 }
06919 
06920 }
06921 }
06922 }
06923 
06924 } else
06925 {
06926 IkReal x1577=((IkReal(0.000500000000000000))*(cj6));
06927 IkReal x1578=((IkReal(0.000500000000000000))*(sj6));
06928 IkReal x1579=((sj5)*(sj6));
06929 IkReal x1580=((cj5)*(r12));
06930 IkReal x1581=((IkReal(0.390000000000000))*(cj5));
06931 IkReal x1582=((IkReal(0.390000000000000))*(cj6)*(sj5));
06932 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
06933 evalcond[1]=((((cj6)*(r10)*(sj5)))+(x1580)+(((IkReal(-1.00000000000000))*(r11)*(x1579))));
06934 evalcond[2]=((((r20)*(x1578)))+(((r20)*(x1582)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x1581)))+(((r21)*(x1577)))+(((IkReal(-0.390000000000000))*(r21)*(x1579))));
06935 evalcond[3]=((IkReal(0.400000000000000))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x1581)))+(((IkReal(-0.390000000000000))*(r01)*(x1579)))+(((r01)*(x1577)))+(((r00)*(x1578)))+(((r00)*(x1582))));
06936 evalcond[4]=((IkReal(0.00200000000000000))+(((IkReal(0.390000000000000))*(x1580)))+(((IkReal(-0.390000000000000))*(r11)*(x1579)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x1577)))+(((r10)*(x1578)))+(((r10)*(x1582))));
06937 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
06938 {
06939 {
06940 IkReal j3array[1], cj3array[1], sj3array[1];
06941 bool j3valid[1]={false};
06942 _nj3 = 1;
06943 IkReal x1583=((IkReal(1.00000000000000))*(sj5));
06944 if( IKabs(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1583)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1583)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1583)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1583)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
06945     continue;
06946 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj6)*(r20)*(x1583)))+(((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1583)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))));
06947 sj3array[0]=IKsin(j3array[0]);
06948 cj3array[0]=IKcos(j3array[0]);
06949 if( j3array[0] > IKPI )
06950 {
06951     j3array[0]-=IK2PI;
06952 }
06953 else if( j3array[0] < -IKPI )
06954 {    j3array[0]+=IK2PI;
06955 }
06956 j3valid[0] = true;
06957 for(int ij3 = 0; ij3 < 1; ++ij3)
06958 {
06959 if( !j3valid[ij3] )
06960 {
06961     continue;
06962 }
06963 _ij3[0] = ij3; _ij3[1] = -1;
06964 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06965 {
06966 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06967 {
06968     j3valid[iij3]=false; _ij3[1] = iij3; break; 
06969 }
06970 }
06971 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06972 {
06973 IkReal evalcond[2];
06974 IkReal x1584=((cj6)*(sj5));
06975 IkReal x1585=((IkReal(1.00000000000000))*(sj5)*(sj6));
06976 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1584)))+(IKsin(j3))+(((IkReal(-1.00000000000000))*(r21)*(x1585))));
06977 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1585)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((r00)*(x1584)))+(((cj5)*(r02))));
06978 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
06979 {
06980 continue;
06981 }
06982 }
06983 
06984 {
06985 IkReal dummyeval[1];
06986 IkReal gconst19;
06987 IkReal x1586=(cj6)*(cj6);
06988 IkReal x1587=(sj6)*(sj6);
06989 IkReal x1588=((r11)*(r20));
06990 IkReal x1589=((r12)*(sj5));
06991 IkReal x1590=((IkReal(1.00000000000000))*(r10));
06992 IkReal x1591=((r22)*(sj5));
06993 IkReal x1592=((cj5)*(x1587));
06994 IkReal x1593=((cj5)*(x1586));
06995 gconst19=IKsign(((((IkReal(-1.00000000000000))*(r21)*(x1590)*(x1592)))+(((IkReal(-1.00000000000000))*(r21)*(x1590)*(x1593)))+(((x1588)*(x1592)))+(((x1588)*(x1593)))+(((r20)*(sj6)*(x1589)))+(((IkReal(-1.00000000000000))*(sj6)*(x1590)*(x1591)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1591)))+(((cj6)*(r21)*(x1589)))));
06996 IkReal x1594=(cj6)*(cj6);
06997 IkReal x1595=(sj6)*(sj6);
06998 IkReal x1596=((r11)*(r20));
06999 IkReal x1597=((r12)*(sj5));
07000 IkReal x1598=((IkReal(1.00000000000000))*(r10));
07001 IkReal x1599=((r22)*(sj5));
07002 IkReal x1600=((cj5)*(x1595));
07003 IkReal x1601=((cj5)*(x1594));
07004 dummyeval[0]=((((IkReal(-1.00000000000000))*(sj6)*(x1598)*(x1599)))+(((r20)*(sj6)*(x1597)))+(((IkReal(-1.00000000000000))*(r21)*(x1598)*(x1601)))+(((IkReal(-1.00000000000000))*(r21)*(x1598)*(x1600)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1599)))+(((cj6)*(r21)*(x1597)))+(((x1596)*(x1601)))+(((x1596)*(x1600))));
07005 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07006 {
07007 {
07008 IkReal dummyeval[1];
07009 IkReal gconst20;
07010 IkReal x1602=(cj5)*(cj5);
07011 IkReal x1603=(r11)*(r11);
07012 IkReal x1604=(sj6)*(sj6);
07013 IkReal x1605=(cj6)*(cj6);
07014 IkReal x1606=(r10)*(r10);
07015 IkReal x1607=((r11)*(sj6));
07016 IkReal x1608=((IkReal(1.00000000000000))*(x1604));
07017 IkReal x1609=((IkReal(1.00000000000000))*(x1605));
07018 IkReal x1610=((IkReal(2.00000000000000))*(cj6)*(r10));
07019 IkReal x1611=((cj5)*(r12)*(sj5));
07020 gconst20=IKsign(((((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1602)*(x1606)*(x1609)))+(((IkReal(-1.00000000000000))*(x1603)*(x1609)))+(((x1602)*(x1607)*(x1610)))+(((IkReal(-2.00000000000000))*(x1607)*(x1611)))+(((IkReal(-1.00000000000000))*(x1606)*(x1608)))+(((IkReal(-1.00000000000000))*(x1607)*(x1610)))+(((IkReal(-1.00000000000000))*(x1602)*(x1603)*(x1608)))+(((x1610)*(x1611)))));
07021 IkReal x1612=(cj5)*(cj5);
07022 IkReal x1613=(r11)*(r11);
07023 IkReal x1614=(sj6)*(sj6);
07024 IkReal x1615=(cj6)*(cj6);
07025 IkReal x1616=(r10)*(r10);
07026 IkReal x1617=((r11)*(sj6));
07027 IkReal x1618=((IkReal(1.00000000000000))*(x1614));
07028 IkReal x1619=((IkReal(1.00000000000000))*(x1615));
07029 IkReal x1620=((IkReal(2.00000000000000))*(cj6)*(r10));
07030 IkReal x1621=((cj5)*(r12)*(sj5));
07031 dummyeval[0]=((((x1620)*(x1621)))+(((IkReal(-1.00000000000000))*(x1612)*(x1616)*(x1619)))+(((IkReal(-1.00000000000000))*(x1616)*(x1618)))+(((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1617)*(x1620)))+(((x1612)*(x1617)*(x1620)))+(((IkReal(-1.00000000000000))*(x1612)*(x1613)*(x1618)))+(((IkReal(-2.00000000000000))*(x1617)*(x1621)))+(((IkReal(-1.00000000000000))*(x1613)*(x1619))));
07032 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07033 {
07034 continue;
07035 
07036 } else
07037 {
07038 {
07039 IkReal j4array[1], cj4array[1], sj4array[1];
07040 bool j4valid[1]={false};
07041 _nj4 = 1;
07042 IkReal x1622=((IkReal(1.00000000000000))*(r10));
07043 if( IKabs(((gconst20)*(((((cj5)*(r11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1622)))+(((r12)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)))+(((IkReal(-1.00000000000000))*(sj6)*(x1622))))))) < IKFAST_ATAN2_MAGTHRESH )
07044     continue;
07045 j4array[0]=IKatan2(((gconst20)*(((((cj5)*(r11)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x1622)))+(((r12)*(sj5)))))), ((gconst20)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)))+(((IkReal(-1.00000000000000))*(sj6)*(x1622)))))));
07046 sj4array[0]=IKsin(j4array[0]);
07047 cj4array[0]=IKcos(j4array[0]);
07048 if( j4array[0] > IKPI )
07049 {
07050     j4array[0]-=IK2PI;
07051 }
07052 else if( j4array[0] < -IKPI )
07053 {    j4array[0]+=IK2PI;
07054 }
07055 j4valid[0] = true;
07056 for(int ij4 = 0; ij4 < 1; ++ij4)
07057 {
07058 if( !j4valid[ij4] )
07059 {
07060     continue;
07061 }
07062 _ij4[0] = ij4; _ij4[1] = -1;
07063 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07064 {
07065 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07066 {
07067     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07068 }
07069 }
07070 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07071 {
07072 IkReal evalcond[6];
07073 IkReal x1623=IKsin(j4);
07074 IkReal x1624=IKcos(j4);
07075 IkReal x1625=((IkReal(1.00000000000000))*(r00));
07076 IkReal x1626=((cj5)*(r01));
07077 IkReal x1627=((cj5)*(r11));
07078 IkReal x1628=((IkReal(1.00000000000000))*(cj5));
07079 IkReal x1629=((cj5)*(r21));
07080 IkReal x1630=((sj5)*(x1623));
07081 IkReal x1631=((cj6)*(x1623));
07082 IkReal x1632=((sj6)*(x1624));
07083 IkReal x1633=((sj6)*(x1623));
07084 IkReal x1634=((sj5)*(x1624));
07085 IkReal x1635=((cj6)*(x1624));
07086 evalcond[0]=((((x1629)*(x1633)))+(((IkReal(-1.00000000000000))*(r20)*(x1632)))+(((IkReal(-1.00000000000000))*(r20)*(x1628)*(x1631)))+(((r22)*(x1630)))+(((IkReal(-1.00000000000000))*(r21)*(x1635))));
07087 evalcond[1]=((((r21)*(x1631)))+(((r20)*(x1633)))+(((x1629)*(x1632)))+(((IkReal(-1.00000000000000))*(r20)*(x1628)*(x1635)))+(((r22)*(x1634)))+(((IkReal(-1.00000000000000))*(cj3))));
07088 evalcond[2]=((((r02)*(x1630)))+(((IkReal(-1.00000000000000))*(r01)*(x1635)))+(((IkReal(-1.00000000000000))*(cj5)*(x1625)*(x1631)))+(((IkReal(-1.00000000000000))*(x1625)*(x1632)))+(((x1626)*(x1633))));
07089 evalcond[3]=((IkReal(1.00000000000000))+(((x1627)*(x1633)))+(((r12)*(x1630)))+(((IkReal(-1.00000000000000))*(r10)*(x1628)*(x1631)))+(((IkReal(-1.00000000000000))*(r10)*(x1632)))+(((IkReal(-1.00000000000000))*(r11)*(x1635))));
07090 evalcond[4]=((((r02)*(x1634)))+(((IkReal(-1.00000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x1625)*(x1635)))+(((r01)*(x1631)))+(((r00)*(x1633)))+(((x1626)*(x1632))));
07091 evalcond[5]=((((x1627)*(x1632)))+(((r11)*(x1631)))+(((r10)*(x1633)))+(((r12)*(x1634)))+(((IkReal(-1.00000000000000))*(r10)*(x1628)*(x1635))));
07092 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07093 {
07094 continue;
07095 }
07096 }
07097 
07098 {
07099 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07100 vinfos[0].jointtype = 1;
07101 vinfos[0].foffset = j0;
07102 vinfos[0].indices[0] = _ij0[0];
07103 vinfos[0].indices[1] = _ij0[1];
07104 vinfos[0].maxsolutions = _nj0;
07105 vinfos[1].jointtype = 1;
07106 vinfos[1].foffset = j1;
07107 vinfos[1].indices[0] = _ij1[0];
07108 vinfos[1].indices[1] = _ij1[1];
07109 vinfos[1].maxsolutions = _nj1;
07110 vinfos[2].jointtype = 1;
07111 vinfos[2].foffset = j2;
07112 vinfos[2].indices[0] = _ij2[0];
07113 vinfos[2].indices[1] = _ij2[1];
07114 vinfos[2].maxsolutions = _nj2;
07115 vinfos[3].jointtype = 1;
07116 vinfos[3].foffset = j3;
07117 vinfos[3].indices[0] = _ij3[0];
07118 vinfos[3].indices[1] = _ij3[1];
07119 vinfos[3].maxsolutions = _nj3;
07120 vinfos[4].jointtype = 1;
07121 vinfos[4].foffset = j4;
07122 vinfos[4].indices[0] = _ij4[0];
07123 vinfos[4].indices[1] = _ij4[1];
07124 vinfos[4].maxsolutions = _nj4;
07125 vinfos[5].jointtype = 1;
07126 vinfos[5].foffset = j5;
07127 vinfos[5].indices[0] = _ij5[0];
07128 vinfos[5].indices[1] = _ij5[1];
07129 vinfos[5].maxsolutions = _nj5;
07130 vinfos[6].jointtype = 1;
07131 vinfos[6].foffset = j6;
07132 vinfos[6].indices[0] = _ij6[0];
07133 vinfos[6].indices[1] = _ij6[1];
07134 vinfos[6].maxsolutions = _nj6;
07135 std::vector<int> vfree(0);
07136 solutions.AddSolution(vinfos,vfree);
07137 }
07138 }
07139 }
07140 
07141 }
07142 
07143 }
07144 
07145 } else
07146 {
07147 {
07148 IkReal j4array[1], cj4array[1], sj4array[1];
07149 bool j4valid[1]={false};
07150 _nj4 = 1;
07151 IkReal x1636=((cj3)*(sj6));
07152 IkReal x1637=((IkReal(1.00000000000000))*(r10));
07153 IkReal x1638=((cj3)*(cj6));
07154 if( IKabs(((gconst19)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x1637)*(x1638)))+(((cj5)*(r11)*(x1636))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((IkReal(-1.00000000000000))*(x1636)*(x1637)))+(((IkReal(-1.00000000000000))*(r11)*(x1638))))))) < IKFAST_ATAN2_MAGTHRESH )
07155     continue;
07156 j4array[0]=IKatan2(((gconst19)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x1637)*(x1638)))+(((cj5)*(r11)*(x1636)))))), ((gconst19)*(((((IkReal(-1.00000000000000))*(x1636)*(x1637)))+(((IkReal(-1.00000000000000))*(r11)*(x1638)))))));
07157 sj4array[0]=IKsin(j4array[0]);
07158 cj4array[0]=IKcos(j4array[0]);
07159 if( j4array[0] > IKPI )
07160 {
07161     j4array[0]-=IK2PI;
07162 }
07163 else if( j4array[0] < -IKPI )
07164 {    j4array[0]+=IK2PI;
07165 }
07166 j4valid[0] = true;
07167 for(int ij4 = 0; ij4 < 1; ++ij4)
07168 {
07169 if( !j4valid[ij4] )
07170 {
07171     continue;
07172 }
07173 _ij4[0] = ij4; _ij4[1] = -1;
07174 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07175 {
07176 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07177 {
07178     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07179 }
07180 }
07181 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07182 {
07183 IkReal evalcond[6];
07184 IkReal x1639=IKsin(j4);
07185 IkReal x1640=IKcos(j4);
07186 IkReal x1641=((IkReal(1.00000000000000))*(r00));
07187 IkReal x1642=((cj5)*(r01));
07188 IkReal x1643=((cj5)*(r11));
07189 IkReal x1644=((IkReal(1.00000000000000))*(cj5));
07190 IkReal x1645=((cj5)*(r21));
07191 IkReal x1646=((sj5)*(x1639));
07192 IkReal x1647=((cj6)*(x1639));
07193 IkReal x1648=((sj6)*(x1640));
07194 IkReal x1649=((sj6)*(x1639));
07195 IkReal x1650=((sj5)*(x1640));
07196 IkReal x1651=((cj6)*(x1640));
07197 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x1651)))+(((x1645)*(x1649)))+(((IkReal(-1.00000000000000))*(r20)*(x1648)))+(((r22)*(x1646)))+(((IkReal(-1.00000000000000))*(r20)*(x1644)*(x1647))));
07198 evalcond[1]=((((r22)*(x1650)))+(((r21)*(x1647)))+(((r20)*(x1649)))+(((x1645)*(x1648)))+(((IkReal(-1.00000000000000))*(r20)*(x1644)*(x1651)))+(((IkReal(-1.00000000000000))*(cj3))));
07199 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x1641)*(x1647)))+(((IkReal(-1.00000000000000))*(x1641)*(x1648)))+(((r02)*(x1646)))+(((IkReal(-1.00000000000000))*(r01)*(x1651)))+(((x1642)*(x1649))));
07200 evalcond[3]=((IkReal(1.00000000000000))+(((r12)*(x1646)))+(((IkReal(-1.00000000000000))*(r10)*(x1644)*(x1647)))+(((IkReal(-1.00000000000000))*(r10)*(x1648)))+(((x1643)*(x1649)))+(((IkReal(-1.00000000000000))*(r11)*(x1651))));
07201 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x1641)*(x1651)))+(((r02)*(x1650)))+(((r00)*(x1649)))+(((r01)*(x1647)))+(((x1642)*(x1648))));
07202 evalcond[5]=((((r11)*(x1647)))+(((r12)*(x1650)))+(((r10)*(x1649)))+(((IkReal(-1.00000000000000))*(r10)*(x1644)*(x1651)))+(((x1643)*(x1648))));
07203 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07204 {
07205 continue;
07206 }
07207 }
07208 
07209 {
07210 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07211 vinfos[0].jointtype = 1;
07212 vinfos[0].foffset = j0;
07213 vinfos[0].indices[0] = _ij0[0];
07214 vinfos[0].indices[1] = _ij0[1];
07215 vinfos[0].maxsolutions = _nj0;
07216 vinfos[1].jointtype = 1;
07217 vinfos[1].foffset = j1;
07218 vinfos[1].indices[0] = _ij1[0];
07219 vinfos[1].indices[1] = _ij1[1];
07220 vinfos[1].maxsolutions = _nj1;
07221 vinfos[2].jointtype = 1;
07222 vinfos[2].foffset = j2;
07223 vinfos[2].indices[0] = _ij2[0];
07224 vinfos[2].indices[1] = _ij2[1];
07225 vinfos[2].maxsolutions = _nj2;
07226 vinfos[3].jointtype = 1;
07227 vinfos[3].foffset = j3;
07228 vinfos[3].indices[0] = _ij3[0];
07229 vinfos[3].indices[1] = _ij3[1];
07230 vinfos[3].maxsolutions = _nj3;
07231 vinfos[4].jointtype = 1;
07232 vinfos[4].foffset = j4;
07233 vinfos[4].indices[0] = _ij4[0];
07234 vinfos[4].indices[1] = _ij4[1];
07235 vinfos[4].maxsolutions = _nj4;
07236 vinfos[5].jointtype = 1;
07237 vinfos[5].foffset = j5;
07238 vinfos[5].indices[0] = _ij5[0];
07239 vinfos[5].indices[1] = _ij5[1];
07240 vinfos[5].maxsolutions = _nj5;
07241 vinfos[6].jointtype = 1;
07242 vinfos[6].foffset = j6;
07243 vinfos[6].indices[0] = _ij6[0];
07244 vinfos[6].indices[1] = _ij6[1];
07245 vinfos[6].maxsolutions = _nj6;
07246 std::vector<int> vfree(0);
07247 solutions.AddSolution(vinfos,vfree);
07248 }
07249 }
07250 }
07251 
07252 }
07253 
07254 }
07255 }
07256 }
07257 
07258 } else
07259 {
07260 if( 1 )
07261 {
07262 continue;
07263 
07264 } else
07265 {
07266 }
07267 }
07268 }
07269 }
07270 
07271 } else
07272 {
07273 {
07274 IkReal j3array[1], cj3array[1], sj3array[1];
07275 bool j3valid[1]={false};
07276 _nj3 = 1;
07277 IkReal x1652=((cj6)*(sj5));
07278 IkReal x1653=((sj5)*(sj6));
07279 if( IKabs(((((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((r21)*(x1653))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1652)))+(((IkReal(-1.00000000000000))*(r11)*(x1653))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((r21)*(x1653)))))+IKsqr(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1652)))+(((IkReal(-1.00000000000000))*(r11)*(x1653)))))))-1) <= IKFAST_SINCOS_THRESH )
07280     continue;
07281 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(x1652)))+(((r21)*(x1653)))), ((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1652)))+(((IkReal(-1.00000000000000))*(r11)*(x1653)))))));
07282 sj3array[0]=IKsin(j3array[0]);
07283 cj3array[0]=IKcos(j3array[0]);
07284 if( j3array[0] > IKPI )
07285 {
07286     j3array[0]-=IK2PI;
07287 }
07288 else if( j3array[0] < -IKPI )
07289 {    j3array[0]+=IK2PI;
07290 }
07291 j3valid[0] = true;
07292 for(int ij3 = 0; ij3 < 1; ++ij3)
07293 {
07294 if( !j3valid[ij3] )
07295 {
07296     continue;
07297 }
07298 _ij3[0] = ij3; _ij3[1] = -1;
07299 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
07300 {
07301 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
07302 {
07303     j3valid[iij3]=false; _ij3[1] = iij3; break; 
07304 }
07305 }
07306 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
07307 {
07308 IkReal evalcond[3];
07309 IkReal x1654=IKcos(j3);
07310 IkReal x1655=((cj6)*(sj5));
07311 IkReal x1656=((IkReal(1.00000000000000))*(sj5)*(sj6));
07312 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1655)))+(((IkReal(-1.00000000000000))*(r21)*(x1656)))+(IKsin(j3)));
07313 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1656)))+(((sj1)*(x1654)))+(((r00)*(x1655)))+(((cj5)*(r02))));
07314 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x1655)))+(((IkReal(-1.00000000000000))*(r11)*(x1656)))+(((IkReal(-1.00000000000000))*(cj1)*(x1654))));
07315 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
07316 {
07317 continue;
07318 }
07319 }
07320 
07321 {
07322 IkReal dummyeval[1];
07323 IkReal gconst11;
07324 IkReal x1657=(cj5)*(cj5);
07325 IkReal x1658=(r21)*(r21);
07326 IkReal x1659=(sj6)*(sj6);
07327 IkReal x1660=(cj6)*(cj6);
07328 IkReal x1661=(r20)*(r20);
07329 IkReal x1662=((cj6)*(r20));
07330 IkReal x1663=((IkReal(1.00000000000000))*(x1660));
07331 IkReal x1664=((IkReal(2.00000000000000))*(r21)*(sj6));
07332 IkReal x1665=((cj5)*(r22)*(sj5));
07333 IkReal x1666=((IkReal(1.00000000000000))*(x1659));
07334 gconst11=IKsign(((((IkReal(-1.00000000000000))*(x1657)*(x1658)*(x1666)))+(((IkReal(-1.00000000000000))*(x1662)*(x1664)))+(((IkReal(2.00000000000000))*(x1662)*(x1665)))+(((IkReal(-1.00000000000000))*(x1661)*(x1666)))+(((x1657)*(x1662)*(x1664)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1658)*(x1663)))+(((IkReal(-1.00000000000000))*(x1657)*(x1661)*(x1663)))+(((IkReal(-1.00000000000000))*(x1664)*(x1665)))));
07335 IkReal x1667=(cj5)*(cj5);
07336 IkReal x1668=(r21)*(r21);
07337 IkReal x1669=(sj6)*(sj6);
07338 IkReal x1670=(cj6)*(cj6);
07339 IkReal x1671=(r20)*(r20);
07340 IkReal x1672=((cj6)*(r20));
07341 IkReal x1673=((IkReal(1.00000000000000))*(x1670));
07342 IkReal x1674=((IkReal(2.00000000000000))*(r21)*(sj6));
07343 IkReal x1675=((cj5)*(r22)*(sj5));
07344 IkReal x1676=((IkReal(1.00000000000000))*(x1669));
07345 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1672)*(x1674)))+(((x1667)*(x1672)*(x1674)))+(((IkReal(-1.00000000000000))*(x1668)*(x1673)))+(((IkReal(2.00000000000000))*(x1672)*(x1675)))+(((IkReal(-1.00000000000000))*(x1671)*(x1676)))+(((IkReal(-1.00000000000000))*(x1667)*(x1671)*(x1673)))+(((IkReal(-1.00000000000000))*(x1667)*(x1668)*(x1676)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x1674)*(x1675))));
07346 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07347 {
07348 {
07349 IkReal dummyeval[1];
07350 IkReal gconst12;
07351 IkReal x1677=(cj5)*(cj5);
07352 IkReal x1678=(sj6)*(sj6);
07353 IkReal x1679=(cj6)*(cj6);
07354 IkReal x1680=((r00)*(r20));
07355 IkReal x1681=((cj5)*(sj5));
07356 IkReal x1682=((r01)*(r21));
07357 IkReal x1683=((r21)*(sj6));
07358 IkReal x1684=((sj6)*(x1681));
07359 IkReal x1685=((IkReal(1.00000000000000))*(cj6)*(r00));
07360 IkReal x1686=((cj6)*(r01)*(r20)*(sj6));
07361 gconst12=IKsign(((x1686)+(((r02)*(x1681)*(x1683)))+(((r01)*(r22)*(x1684)))+(((IkReal(-1.00000000000000))*(x1677)*(x1683)*(x1685)))+(((x1677)*(x1678)*(x1682)))+(((IkReal(-1.00000000000000))*(r22)*(x1681)*(x1685)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1681)))+(((x1677)*(x1679)*(x1680)))+(((r02)*(r22)*((sj5)*(sj5))))+(((cj6)*(r00)*(x1683)))+(((x1679)*(x1682)))+(((IkReal(-1.00000000000000))*(x1677)*(x1686)))+(((x1678)*(x1680)))));
07362 IkReal x1687=(cj5)*(cj5);
07363 IkReal x1688=(sj6)*(sj6);
07364 IkReal x1689=(cj6)*(cj6);
07365 IkReal x1690=((r00)*(r20));
07366 IkReal x1691=((cj5)*(sj5));
07367 IkReal x1692=((r01)*(r21));
07368 IkReal x1693=((r21)*(sj6));
07369 IkReal x1694=((sj6)*(x1691));
07370 IkReal x1695=((IkReal(1.00000000000000))*(cj6)*(r00));
07371 IkReal x1696=x1686;
07372 dummyeval[0]=((x1696)+(((r01)*(r22)*(x1694)))+(((x1689)*(x1692)))+(((IkReal(-1.00000000000000))*(x1687)*(x1693)*(x1695)))+(((IkReal(-1.00000000000000))*(x1687)*(x1696)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1691)))+(((r02)*(x1691)*(x1693)))+(((r02)*(r22)*((sj5)*(sj5))))+(((x1688)*(x1690)))+(((cj6)*(r00)*(x1693)))+(((x1687)*(x1688)*(x1692)))+(((x1687)*(x1689)*(x1690)))+(((IkReal(-1.00000000000000))*(r22)*(x1691)*(x1695))));
07373 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07374 {
07375 continue;
07376 
07377 } else
07378 {
07379 {
07380 IkReal j4array[1], cj4array[1], sj4array[1];
07381 bool j4valid[1]={false};
07382 _nj4 = 1;
07383 IkReal x1697=((IkReal(1.00000000000000))*(sj1)*(sj3));
07384 if( IKabs(((gconst12)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1697)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1697))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1697)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1697))))))) < IKFAST_ATAN2_MAGTHRESH )
07385     continue;
07386 j4array[0]=IKatan2(((gconst12)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1697)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x1697)))))), ((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1697)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1697)))))));
07387 sj4array[0]=IKsin(j4array[0]);
07388 cj4array[0]=IKcos(j4array[0]);
07389 if( j4array[0] > IKPI )
07390 {
07391     j4array[0]-=IK2PI;
07392 }
07393 else if( j4array[0] < -IKPI )
07394 {    j4array[0]+=IK2PI;
07395 }
07396 j4valid[0] = true;
07397 for(int ij4 = 0; ij4 < 1; ++ij4)
07398 {
07399 if( !j4valid[ij4] )
07400 {
07401     continue;
07402 }
07403 _ij4[0] = ij4; _ij4[1] = -1;
07404 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07405 {
07406 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07407 {
07408     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07409 }
07410 }
07411 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07412 {
07413 IkReal evalcond[6];
07414 IkReal x1698=IKsin(j4);
07415 IkReal x1699=IKcos(j4);
07416 IkReal x1700=((IkReal(1.00000000000000))*(r00));
07417 IkReal x1701=((cj5)*(r01));
07418 IkReal x1702=((cj5)*(r11));
07419 IkReal x1703=((IkReal(1.00000000000000))*(cj5));
07420 IkReal x1704=((IkReal(1.00000000000000))*(cj1));
07421 IkReal x1705=((cj5)*(r21));
07422 IkReal x1706=((sj5)*(x1698));
07423 IkReal x1707=((cj6)*(x1698));
07424 IkReal x1708=((sj6)*(x1699));
07425 IkReal x1709=((sj6)*(x1698));
07426 IkReal x1710=((sj5)*(x1699));
07427 IkReal x1711=((cj6)*(x1699));
07428 evalcond[0]=((((r22)*(x1706)))+(((IkReal(-1.00000000000000))*(r21)*(x1711)))+(((x1705)*(x1709)))+(((IkReal(-1.00000000000000))*(r20)*(x1708)))+(((IkReal(-1.00000000000000))*(r20)*(x1703)*(x1707))));
07429 evalcond[1]=((((r22)*(x1710)))+(((r20)*(x1709)))+(((x1705)*(x1708)))+(((r21)*(x1707)))+(((IkReal(-1.00000000000000))*(r20)*(x1703)*(x1711)))+(((IkReal(-1.00000000000000))*(cj3))));
07430 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1711)))+(((r02)*(x1706)))+(((IkReal(-1.00000000000000))*(cj5)*(x1700)*(x1707)))+(((IkReal(-1.00000000000000))*(x1704)))+(((x1701)*(x1709)))+(((IkReal(-1.00000000000000))*(x1700)*(x1708))));
07431 evalcond[3]=((((r12)*(x1706)))+(((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r10)*(x1703)*(x1707)))+(((IkReal(-1.00000000000000))*(r10)*(x1708)))+(((IkReal(-1.00000000000000))*(r11)*(x1711)))+(((x1702)*(x1709))));
07432 evalcond[4]=((((r02)*(x1710)))+(((IkReal(-1.00000000000000))*(cj5)*(x1700)*(x1711)))+(((r01)*(x1707)))+(((sj1)*(sj3)))+(((r00)*(x1709)))+(((x1701)*(x1708))));
07433 evalcond[5]=((((r12)*(x1710)))+(((IkReal(-1.00000000000000))*(r10)*(x1703)*(x1711)))+(((r11)*(x1707)))+(((IkReal(-1.00000000000000))*(sj3)*(x1704)))+(((r10)*(x1709)))+(((x1702)*(x1708))));
07434 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07435 {
07436 continue;
07437 }
07438 }
07439 
07440 {
07441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07442 vinfos[0].jointtype = 1;
07443 vinfos[0].foffset = j0;
07444 vinfos[0].indices[0] = _ij0[0];
07445 vinfos[0].indices[1] = _ij0[1];
07446 vinfos[0].maxsolutions = _nj0;
07447 vinfos[1].jointtype = 1;
07448 vinfos[1].foffset = j1;
07449 vinfos[1].indices[0] = _ij1[0];
07450 vinfos[1].indices[1] = _ij1[1];
07451 vinfos[1].maxsolutions = _nj1;
07452 vinfos[2].jointtype = 1;
07453 vinfos[2].foffset = j2;
07454 vinfos[2].indices[0] = _ij2[0];
07455 vinfos[2].indices[1] = _ij2[1];
07456 vinfos[2].maxsolutions = _nj2;
07457 vinfos[3].jointtype = 1;
07458 vinfos[3].foffset = j3;
07459 vinfos[3].indices[0] = _ij3[0];
07460 vinfos[3].indices[1] = _ij3[1];
07461 vinfos[3].maxsolutions = _nj3;
07462 vinfos[4].jointtype = 1;
07463 vinfos[4].foffset = j4;
07464 vinfos[4].indices[0] = _ij4[0];
07465 vinfos[4].indices[1] = _ij4[1];
07466 vinfos[4].maxsolutions = _nj4;
07467 vinfos[5].jointtype = 1;
07468 vinfos[5].foffset = j5;
07469 vinfos[5].indices[0] = _ij5[0];
07470 vinfos[5].indices[1] = _ij5[1];
07471 vinfos[5].maxsolutions = _nj5;
07472 vinfos[6].jointtype = 1;
07473 vinfos[6].foffset = j6;
07474 vinfos[6].indices[0] = _ij6[0];
07475 vinfos[6].indices[1] = _ij6[1];
07476 vinfos[6].maxsolutions = _nj6;
07477 std::vector<int> vfree(0);
07478 solutions.AddSolution(vinfos,vfree);
07479 }
07480 }
07481 }
07482 
07483 }
07484 
07485 }
07486 
07487 } else
07488 {
07489 {
07490 IkReal j4array[1], cj4array[1], sj4array[1];
07491 bool j4valid[1]={false};
07492 _nj4 = 1;
07493 IkReal x1712=((cj3)*(cj6));
07494 IkReal x1713=((IkReal(1.00000000000000))*(r21));
07495 IkReal x1714=((cj3)*(sj6));
07496 if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(x1712)*(x1713)))+(((IkReal(-1.00000000000000))*(r20)*(x1714))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj5)*(r20)*(x1712)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x1713)*(x1714))))))) < IKFAST_ATAN2_MAGTHRESH )
07497     continue;
07498 j4array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(x1712)*(x1713)))+(((IkReal(-1.00000000000000))*(r20)*(x1714)))))), ((gconst11)*(((((cj5)*(r20)*(x1712)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x1713)*(x1714)))))));
07499 sj4array[0]=IKsin(j4array[0]);
07500 cj4array[0]=IKcos(j4array[0]);
07501 if( j4array[0] > IKPI )
07502 {
07503     j4array[0]-=IK2PI;
07504 }
07505 else if( j4array[0] < -IKPI )
07506 {    j4array[0]+=IK2PI;
07507 }
07508 j4valid[0] = true;
07509 for(int ij4 = 0; ij4 < 1; ++ij4)
07510 {
07511 if( !j4valid[ij4] )
07512 {
07513     continue;
07514 }
07515 _ij4[0] = ij4; _ij4[1] = -1;
07516 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07517 {
07518 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07519 {
07520     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07521 }
07522 }
07523 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07524 {
07525 IkReal evalcond[6];
07526 IkReal x1715=IKsin(j4);
07527 IkReal x1716=IKcos(j4);
07528 IkReal x1717=((IkReal(1.00000000000000))*(r00));
07529 IkReal x1718=((cj5)*(r01));
07530 IkReal x1719=((cj5)*(r11));
07531 IkReal x1720=((IkReal(1.00000000000000))*(cj5));
07532 IkReal x1721=((IkReal(1.00000000000000))*(cj1));
07533 IkReal x1722=((cj5)*(r21));
07534 IkReal x1723=((sj5)*(x1715));
07535 IkReal x1724=((cj6)*(x1715));
07536 IkReal x1725=((sj6)*(x1716));
07537 IkReal x1726=((sj6)*(x1715));
07538 IkReal x1727=((sj5)*(x1716));
07539 IkReal x1728=((cj6)*(x1716));
07540 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1725)))+(((x1722)*(x1726)))+(((r22)*(x1723)))+(((IkReal(-1.00000000000000))*(r21)*(x1728)))+(((IkReal(-1.00000000000000))*(r20)*(x1720)*(x1724))));
07541 evalcond[1]=((((x1722)*(x1725)))+(((r21)*(x1724)))+(((r22)*(x1727)))+(((r20)*(x1726)))+(((IkReal(-1.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(r20)*(x1720)*(x1728))));
07542 evalcond[2]=((((x1718)*(x1726)))+(((r02)*(x1723)))+(((IkReal(-1.00000000000000))*(x1717)*(x1725)))+(((IkReal(-1.00000000000000))*(r01)*(x1728)))+(((IkReal(-1.00000000000000))*(cj5)*(x1717)*(x1724)))+(((IkReal(-1.00000000000000))*(x1721))));
07543 evalcond[3]=((((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r11)*(x1728)))+(((IkReal(-1.00000000000000))*(r10)*(x1720)*(x1724)))+(((r12)*(x1723)))+(((IkReal(-1.00000000000000))*(r10)*(x1725)))+(((x1719)*(x1726))));
07544 evalcond[4]=((((r01)*(x1724)))+(((r00)*(x1726)))+(((x1718)*(x1725)))+(((sj1)*(sj3)))+(((r02)*(x1727)))+(((IkReal(-1.00000000000000))*(cj5)*(x1717)*(x1728))));
07545 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1720)*(x1728)))+(((r10)*(x1726)))+(((r11)*(x1724)))+(((r12)*(x1727)))+(((x1719)*(x1725)))+(((IkReal(-1.00000000000000))*(sj3)*(x1721))));
07546 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07547 {
07548 continue;
07549 }
07550 }
07551 
07552 {
07553 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07554 vinfos[0].jointtype = 1;
07555 vinfos[0].foffset = j0;
07556 vinfos[0].indices[0] = _ij0[0];
07557 vinfos[0].indices[1] = _ij0[1];
07558 vinfos[0].maxsolutions = _nj0;
07559 vinfos[1].jointtype = 1;
07560 vinfos[1].foffset = j1;
07561 vinfos[1].indices[0] = _ij1[0];
07562 vinfos[1].indices[1] = _ij1[1];
07563 vinfos[1].maxsolutions = _nj1;
07564 vinfos[2].jointtype = 1;
07565 vinfos[2].foffset = j2;
07566 vinfos[2].indices[0] = _ij2[0];
07567 vinfos[2].indices[1] = _ij2[1];
07568 vinfos[2].maxsolutions = _nj2;
07569 vinfos[3].jointtype = 1;
07570 vinfos[3].foffset = j3;
07571 vinfos[3].indices[0] = _ij3[0];
07572 vinfos[3].indices[1] = _ij3[1];
07573 vinfos[3].maxsolutions = _nj3;
07574 vinfos[4].jointtype = 1;
07575 vinfos[4].foffset = j4;
07576 vinfos[4].indices[0] = _ij4[0];
07577 vinfos[4].indices[1] = _ij4[1];
07578 vinfos[4].maxsolutions = _nj4;
07579 vinfos[5].jointtype = 1;
07580 vinfos[5].foffset = j5;
07581 vinfos[5].indices[0] = _ij5[0];
07582 vinfos[5].indices[1] = _ij5[1];
07583 vinfos[5].maxsolutions = _nj5;
07584 vinfos[6].jointtype = 1;
07585 vinfos[6].foffset = j6;
07586 vinfos[6].indices[0] = _ij6[0];
07587 vinfos[6].indices[1] = _ij6[1];
07588 vinfos[6].maxsolutions = _nj6;
07589 std::vector<int> vfree(0);
07590 solutions.AddSolution(vinfos,vfree);
07591 }
07592 }
07593 }
07594 
07595 }
07596 
07597 }
07598 }
07599 }
07600 
07601 }
07602 
07603 }
07604 
07605 } else
07606 {
07607 {
07608 IkReal j4array[1], cj4array[1], sj4array[1];
07609 bool j4valid[1]={false};
07610 _nj4 = 1;
07611 IkReal x1729=((cj6)*(sj1));
07612 IkReal x1730=((IkReal(1.00000000000000))*(sj1)*(sj6));
07613 if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(r21)*(x1729)))+(((IkReal(-1.00000000000000))*(r20)*(x1730))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((cj5)*(r20)*(x1729)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1730))))))) < IKFAST_ATAN2_MAGTHRESH )
07614     continue;
07615 j4array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(r21)*(x1729)))+(((IkReal(-1.00000000000000))*(r20)*(x1730)))))), ((gconst10)*(((((cj5)*(r20)*(x1729)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1730)))))));
07616 sj4array[0]=IKsin(j4array[0]);
07617 cj4array[0]=IKcos(j4array[0]);
07618 if( j4array[0] > IKPI )
07619 {
07620     j4array[0]-=IK2PI;
07621 }
07622 else if( j4array[0] < -IKPI )
07623 {    j4array[0]+=IK2PI;
07624 }
07625 j4valid[0] = true;
07626 for(int ij4 = 0; ij4 < 1; ++ij4)
07627 {
07628 if( !j4valid[ij4] )
07629 {
07630     continue;
07631 }
07632 _ij4[0] = ij4; _ij4[1] = -1;
07633 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07634 {
07635 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07636 {
07637     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07638 }
07639 }
07640 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07641 {
07642 IkReal evalcond[3];
07643 IkReal x1731=IKsin(j4);
07644 IkReal x1732=IKcos(j4);
07645 IkReal x1733=((IkReal(1.00000000000000))*(cj6));
07646 IkReal x1734=((sj5)*(x1731));
07647 IkReal x1735=((cj5)*(x1731));
07648 IkReal x1736=((IkReal(1.00000000000000))*(sj6)*(x1732));
07649 evalcond[0]=((((r21)*(sj6)*(x1735)))+(((IkReal(-1.00000000000000))*(r20)*(x1733)*(x1735)))+(((r22)*(x1734)))+(((IkReal(-1.00000000000000))*(r21)*(x1732)*(x1733)))+(((IkReal(-1.00000000000000))*(r20)*(x1736))));
07650 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1732)*(x1733)))+(((IkReal(-1.00000000000000))*(r00)*(x1736)))+(((r02)*(x1734)))+(((r01)*(sj6)*(x1735)))+(((IkReal(-1.00000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(r00)*(x1733)*(x1735))));
07651 evalcond[2]=((((r11)*(sj6)*(x1735)))+(((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r10)*(x1736)))+(((IkReal(-1.00000000000000))*(r10)*(x1733)*(x1735)))+(((IkReal(-1.00000000000000))*(r11)*(x1732)*(x1733)))+(((r12)*(x1734))));
07652 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
07653 {
07654 continue;
07655 }
07656 }
07657 
07658 {
07659 IkReal j3array[1], cj3array[1], sj3array[1];
07660 bool j3valid[1]={false};
07661 _nj3 = 1;
07662 IkReal x1737=((cj4)*(cj5));
07663 IkReal x1738=((r21)*(sj6));
07664 IkReal x1739=((IkReal(1.00000000000000))*(cj6)*(r20));
07665 if( IKabs(((((sj5)*(x1738)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x1739))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((x1737)*(x1738)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x1737)*(x1739)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x1738)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x1739)))))+IKsqr(((((x1737)*(x1738)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x1737)*(x1739)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
07666     continue;
07667 j3array[0]=IKatan2(((((sj5)*(x1738)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x1739)))), ((((x1737)*(x1738)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x1737)*(x1739)))+(((r20)*(sj4)*(sj6)))));
07668 sj3array[0]=IKsin(j3array[0]);
07669 cj3array[0]=IKcos(j3array[0]);
07670 if( j3array[0] > IKPI )
07671 {
07672     j3array[0]-=IK2PI;
07673 }
07674 else if( j3array[0] < -IKPI )
07675 {    j3array[0]+=IK2PI;
07676 }
07677 j3valid[0] = true;
07678 for(int ij3 = 0; ij3 < 1; ++ij3)
07679 {
07680 if( !j3valid[ij3] )
07681 {
07682     continue;
07683 }
07684 _ij3[0] = ij3; _ij3[1] = -1;
07685 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
07686 {
07687 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
07688 {
07689     j3valid[iij3]=false; _ij3[1] = iij3; break; 
07690 }
07691 }
07692 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
07693 {
07694 IkReal evalcond[6];
07695 IkReal x1740=IKcos(j3);
07696 IkReal x1741=IKsin(j3);
07697 IkReal x1742=((r11)*(sj6));
07698 IkReal x1743=((IkReal(1.00000000000000))*(sj5));
07699 IkReal x1744=((IkReal(1.00000000000000))*(cj1));
07700 IkReal x1745=((cj6)*(sj4));
07701 IkReal x1746=((sj4)*(sj6));
07702 IkReal x1747=((cj4)*(sj5));
07703 IkReal x1748=((cj4)*(cj5));
07704 IkReal x1749=((r01)*(sj6));
07705 IkReal x1750=((r21)*(sj6));
07706 IkReal x1751=((cj6)*(sj5));
07707 IkReal x1752=((IkReal(1.00000000000000))*(cj6)*(x1748));
07708 evalcond[0]=((((cj5)*(r22)))+(x1741)+(((r20)*(x1751)))+(((IkReal(-1.00000000000000))*(x1743)*(x1750))));
07709 evalcond[1]=((((sj1)*(x1740)))+(((r00)*(x1751)))+(((IkReal(-1.00000000000000))*(x1743)*(x1749)))+(((cj5)*(r02))));
07710 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x1740)*(x1744)))+(((IkReal(-1.00000000000000))*(x1742)*(x1743)))+(((r10)*(x1751))));
07711 evalcond[3]=((((r22)*(x1747)))+(((r21)*(x1745)))+(((IkReal(-1.00000000000000))*(x1740)))+(((x1748)*(x1750)))+(((r20)*(x1746)))+(((IkReal(-1.00000000000000))*(r20)*(x1752))));
07712 evalcond[4]=((((r02)*(x1747)))+(((sj1)*(x1741)))+(((IkReal(-1.00000000000000))*(r00)*(x1752)))+(((x1748)*(x1749)))+(((r00)*(x1746)))+(((r01)*(x1745))));
07713 evalcond[5]=((((r12)*(x1747)))+(((IkReal(-1.00000000000000))*(x1741)*(x1744)))+(((IkReal(-1.00000000000000))*(r10)*(x1752)))+(((x1742)*(x1748)))+(((r11)*(x1745)))+(((r10)*(x1746))));
07714 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07715 {
07716 continue;
07717 }
07718 }
07719 
07720 {
07721 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07722 vinfos[0].jointtype = 1;
07723 vinfos[0].foffset = j0;
07724 vinfos[0].indices[0] = _ij0[0];
07725 vinfos[0].indices[1] = _ij0[1];
07726 vinfos[0].maxsolutions = _nj0;
07727 vinfos[1].jointtype = 1;
07728 vinfos[1].foffset = j1;
07729 vinfos[1].indices[0] = _ij1[0];
07730 vinfos[1].indices[1] = _ij1[1];
07731 vinfos[1].maxsolutions = _nj1;
07732 vinfos[2].jointtype = 1;
07733 vinfos[2].foffset = j2;
07734 vinfos[2].indices[0] = _ij2[0];
07735 vinfos[2].indices[1] = _ij2[1];
07736 vinfos[2].maxsolutions = _nj2;
07737 vinfos[3].jointtype = 1;
07738 vinfos[3].foffset = j3;
07739 vinfos[3].indices[0] = _ij3[0];
07740 vinfos[3].indices[1] = _ij3[1];
07741 vinfos[3].maxsolutions = _nj3;
07742 vinfos[4].jointtype = 1;
07743 vinfos[4].foffset = j4;
07744 vinfos[4].indices[0] = _ij4[0];
07745 vinfos[4].indices[1] = _ij4[1];
07746 vinfos[4].maxsolutions = _nj4;
07747 vinfos[5].jointtype = 1;
07748 vinfos[5].foffset = j5;
07749 vinfos[5].indices[0] = _ij5[0];
07750 vinfos[5].indices[1] = _ij5[1];
07751 vinfos[5].maxsolutions = _nj5;
07752 vinfos[6].jointtype = 1;
07753 vinfos[6].foffset = j6;
07754 vinfos[6].indices[0] = _ij6[0];
07755 vinfos[6].indices[1] = _ij6[1];
07756 vinfos[6].maxsolutions = _nj6;
07757 std::vector<int> vfree(0);
07758 solutions.AddSolution(vinfos,vfree);
07759 }
07760 }
07761 }
07762 }
07763 }
07764 
07765 }
07766 
07767 }
07768 
07769 } else
07770 {
07771 {
07772 IkReal j4array[1], cj4array[1], sj4array[1];
07773 bool j4valid[1]={false};
07774 _nj4 = 1;
07775 IkReal x1753=((cj1)*(cj6));
07776 IkReal x1754=((IkReal(1.00000000000000))*(cj1));
07777 if( IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(r21)*(x1753)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1754))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1754)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1754)))+(((cj5)*(r20)*(x1753))))))) < IKFAST_ATAN2_MAGTHRESH )
07778     continue;
07779 j4array[0]=IKatan2(((gconst9)*(((((IkReal(-1.00000000000000))*(r21)*(x1753)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x1754)))))), ((gconst9)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x1754)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x1754)))+(((cj5)*(r20)*(x1753)))))));
07780 sj4array[0]=IKsin(j4array[0]);
07781 cj4array[0]=IKcos(j4array[0]);
07782 if( j4array[0] > IKPI )
07783 {
07784     j4array[0]-=IK2PI;
07785 }
07786 else if( j4array[0] < -IKPI )
07787 {    j4array[0]+=IK2PI;
07788 }
07789 j4valid[0] = true;
07790 for(int ij4 = 0; ij4 < 1; ++ij4)
07791 {
07792 if( !j4valid[ij4] )
07793 {
07794     continue;
07795 }
07796 _ij4[0] = ij4; _ij4[1] = -1;
07797 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07798 {
07799 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07800 {
07801     j4valid[iij4]=false; _ij4[1] = iij4; break; 
07802 }
07803 }
07804 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07805 {
07806 IkReal evalcond[3];
07807 IkReal x1755=IKsin(j4);
07808 IkReal x1756=IKcos(j4);
07809 IkReal x1757=((IkReal(1.00000000000000))*(cj6));
07810 IkReal x1758=((sj5)*(x1755));
07811 IkReal x1759=((cj5)*(x1755));
07812 IkReal x1760=((IkReal(1.00000000000000))*(sj6)*(x1756));
07813 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1760)))+(((IkReal(-1.00000000000000))*(r20)*(x1757)*(x1759)))+(((r22)*(x1758)))+(((IkReal(-1.00000000000000))*(r21)*(x1756)*(x1757)))+(((r21)*(sj6)*(x1759))));
07814 evalcond[1]=((((r01)*(sj6)*(x1759)))+(((IkReal(-1.00000000000000))*(r00)*(x1760)))+(((IkReal(-1.00000000000000))*(r01)*(x1756)*(x1757)))+(((r02)*(x1758)))+(((IkReal(-1.00000000000000))*(r00)*(x1757)*(x1759)))+(((IkReal(-1.00000000000000))*(cj1))));
07815 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)))+(((r11)*(sj6)*(x1759)))+(((IkReal(-1.00000000000000))*(r10)*(x1757)*(x1759)))+(((IkReal(-1.00000000000000))*(r10)*(x1760)))+(((r12)*(x1758)))+(((IkReal(-1.00000000000000))*(r11)*(x1756)*(x1757))));
07816 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
07817 {
07818 continue;
07819 }
07820 }
07821 
07822 {
07823 IkReal j3array[1], cj3array[1], sj3array[1];
07824 bool j3valid[1]={false};
07825 _nj3 = 1;
07826 IkReal x1761=((cj4)*(cj5));
07827 IkReal x1762=((r21)*(sj6));
07828 IkReal x1763=((IkReal(1.00000000000000))*(cj6)*(r20));
07829 if( IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x1763)))+(((sj5)*(x1762)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x1761)*(x1763)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((x1761)*(x1762))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x1763)))+(((sj5)*(x1762)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x1761)*(x1763)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((x1761)*(x1762)))))-1) <= IKFAST_SINCOS_THRESH )
07830     continue;
07831 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(sj5)*(x1763)))+(((sj5)*(x1762)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x1761)*(x1763)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((x1761)*(x1762)))));
07832 sj3array[0]=IKsin(j3array[0]);
07833 cj3array[0]=IKcos(j3array[0]);
07834 if( j3array[0] > IKPI )
07835 {
07836     j3array[0]-=IK2PI;
07837 }
07838 else if( j3array[0] < -IKPI )
07839 {    j3array[0]+=IK2PI;
07840 }
07841 j3valid[0] = true;
07842 for(int ij3 = 0; ij3 < 1; ++ij3)
07843 {
07844 if( !j3valid[ij3] )
07845 {
07846     continue;
07847 }
07848 _ij3[0] = ij3; _ij3[1] = -1;
07849 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
07850 {
07851 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
07852 {
07853     j3valid[iij3]=false; _ij3[1] = iij3; break; 
07854 }
07855 }
07856 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
07857 {
07858 IkReal evalcond[6];
07859 IkReal x1764=IKcos(j3);
07860 IkReal x1765=IKsin(j3);
07861 IkReal x1766=((r11)*(sj6));
07862 IkReal x1767=((IkReal(1.00000000000000))*(sj5));
07863 IkReal x1768=((IkReal(1.00000000000000))*(cj1));
07864 IkReal x1769=((cj6)*(sj4));
07865 IkReal x1770=((sj4)*(sj6));
07866 IkReal x1771=((cj4)*(sj5));
07867 IkReal x1772=((cj4)*(cj5));
07868 IkReal x1773=((r01)*(sj6));
07869 IkReal x1774=((r21)*(sj6));
07870 IkReal x1775=((cj6)*(sj5));
07871 IkReal x1776=((IkReal(1.00000000000000))*(cj6)*(x1772));
07872 evalcond[0]=((((cj5)*(r22)))+(x1765)+(((IkReal(-1.00000000000000))*(x1767)*(x1774)))+(((r20)*(x1775))));
07873 evalcond[1]=((((sj1)*(x1764)))+(((IkReal(-1.00000000000000))*(x1767)*(x1773)))+(((r00)*(x1775)))+(((cj5)*(r02))));
07874 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x1766)*(x1767)))+(((r10)*(x1775)))+(((IkReal(-1.00000000000000))*(x1764)*(x1768))));
07875 evalcond[3]=((((r21)*(x1769)))+(((r22)*(x1771)))+(((IkReal(-1.00000000000000))*(x1764)))+(((IkReal(-1.00000000000000))*(r20)*(x1776)))+(((r20)*(x1770)))+(((x1772)*(x1774))));
07876 evalcond[4]=((((sj1)*(x1765)))+(((r02)*(x1771)))+(((r01)*(x1769)))+(((r00)*(x1770)))+(((IkReal(-1.00000000000000))*(r00)*(x1776)))+(((x1772)*(x1773))));
07877 evalcond[5]=((((r12)*(x1771)))+(((r11)*(x1769)))+(((x1766)*(x1772)))+(((IkReal(-1.00000000000000))*(r10)*(x1776)))+(((r10)*(x1770)))+(((IkReal(-1.00000000000000))*(x1765)*(x1768))));
07878 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
07879 {
07880 continue;
07881 }
07882 }
07883 
07884 {
07885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
07886 vinfos[0].jointtype = 1;
07887 vinfos[0].foffset = j0;
07888 vinfos[0].indices[0] = _ij0[0];
07889 vinfos[0].indices[1] = _ij0[1];
07890 vinfos[0].maxsolutions = _nj0;
07891 vinfos[1].jointtype = 1;
07892 vinfos[1].foffset = j1;
07893 vinfos[1].indices[0] = _ij1[0];
07894 vinfos[1].indices[1] = _ij1[1];
07895 vinfos[1].maxsolutions = _nj1;
07896 vinfos[2].jointtype = 1;
07897 vinfos[2].foffset = j2;
07898 vinfos[2].indices[0] = _ij2[0];
07899 vinfos[2].indices[1] = _ij2[1];
07900 vinfos[2].maxsolutions = _nj2;
07901 vinfos[3].jointtype = 1;
07902 vinfos[3].foffset = j3;
07903 vinfos[3].indices[0] = _ij3[0];
07904 vinfos[3].indices[1] = _ij3[1];
07905 vinfos[3].maxsolutions = _nj3;
07906 vinfos[4].jointtype = 1;
07907 vinfos[4].foffset = j4;
07908 vinfos[4].indices[0] = _ij4[0];
07909 vinfos[4].indices[1] = _ij4[1];
07910 vinfos[4].maxsolutions = _nj4;
07911 vinfos[5].jointtype = 1;
07912 vinfos[5].foffset = j5;
07913 vinfos[5].indices[0] = _ij5[0];
07914 vinfos[5].indices[1] = _ij5[1];
07915 vinfos[5].maxsolutions = _nj5;
07916 vinfos[6].jointtype = 1;
07917 vinfos[6].foffset = j6;
07918 vinfos[6].indices[0] = _ij6[0];
07919 vinfos[6].indices[1] = _ij6[1];
07920 vinfos[6].maxsolutions = _nj6;
07921 std::vector<int> vfree(0);
07922 solutions.AddSolution(vinfos,vfree);
07923 }
07924 }
07925 }
07926 }
07927 }
07928 
07929 }
07930 
07931 }
07932 
07933 } else
07934 {
07935 IkReal x1777=((IkReal(0.390000000000000))*(sj5));
07936 IkReal x1778=((IkReal(0.390000000000000))*(cj5));
07937 IkReal x1779=((IkReal(0.000500000000000000))*(cj6));
07938 IkReal x1780=((IkReal(0.000500000000000000))*(sj6));
07939 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
07940 evalcond[1]=((((r20)*(x1780)))+(((r22)*(x1778)))+(((IkReal(-1.00000000000000))*(pz)))+(((cj6)*(r20)*(x1777)))+(((r21)*(x1779)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x1777))));
07941 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x1777)))+(((r02)*(x1778)))+(((cj6)*(r00)*(x1777)))+(((IkReal(0.00200000000000000))*(cj1)))+(((r01)*(x1779)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.400000000000000))*(sj1)))+(((r00)*(x1780))));
07942 evalcond[3]=((((r10)*(x1780)))+(((r12)*(x1778)))+(((r11)*(x1779)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x1777)))+(((cj6)*(r10)*(x1777))));
07943 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
07944 {
07945 {
07946 IkReal dummyeval[1];
07947 IkReal gconst21;
07948 IkReal x1781=(cj6)*(cj6);
07949 IkReal x1782=(sj6)*(sj6);
07950 IkReal x1783=((r01)*(r20));
07951 IkReal x1784=((r02)*(sj5));
07952 IkReal x1785=((cj5)*(x1782));
07953 IkReal x1786=((IkReal(1.00000000000000))*(r22)*(sj5));
07954 IkReal x1787=((IkReal(1.00000000000000))*(r00)*(r21));
07955 IkReal x1788=((cj5)*(x1781));
07956 gconst21=IKsign(((((cj6)*(r21)*(x1784)))+(((x1783)*(x1785)))+(((x1783)*(x1788)))+(((IkReal(-1.00000000000000))*(x1787)*(x1788)))+(((r20)*(sj6)*(x1784)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1786)))+(((IkReal(-1.00000000000000))*(x1785)*(x1787)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1786)))));
07957 IkReal x1789=(cj6)*(cj6);
07958 IkReal x1790=(sj6)*(sj6);
07959 IkReal x1791=((r01)*(r20));
07960 IkReal x1792=((r02)*(sj5));
07961 IkReal x1793=((cj5)*(x1790));
07962 IkReal x1794=((IkReal(1.00000000000000))*(r22)*(sj5));
07963 IkReal x1795=((IkReal(1.00000000000000))*(r00)*(r21));
07964 IkReal x1796=((cj5)*(x1789));
07965 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x1794)))+(((IkReal(-1.00000000000000))*(x1795)*(x1796)))+(((cj6)*(r21)*(x1792)))+(((r20)*(sj6)*(x1792)))+(((IkReal(-1.00000000000000))*(x1793)*(x1795)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1794)))+(((x1791)*(x1793)))+(((x1791)*(x1796))));
07966 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07967 {
07968 {
07969 IkReal dummyeval[1];
07970 IkReal gconst22;
07971 IkReal x1797=(cj6)*(cj6);
07972 IkReal x1798=(sj6)*(sj6);
07973 IkReal x1799=((r11)*(r20));
07974 IkReal x1800=((r12)*(sj5));
07975 IkReal x1801=((IkReal(1.00000000000000))*(r10));
07976 IkReal x1802=((r22)*(sj5));
07977 IkReal x1803=((cj5)*(x1798));
07978 IkReal x1804=((cj5)*(x1797));
07979 gconst22=IKsign(((((IkReal(-1.00000000000000))*(r21)*(x1801)*(x1804)))+(((IkReal(-1.00000000000000))*(r21)*(x1801)*(x1803)))+(((x1799)*(x1803)))+(((x1799)*(x1804)))+(((IkReal(-1.00000000000000))*(sj6)*(x1801)*(x1802)))+(((r20)*(sj6)*(x1800)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1802)))+(((cj6)*(r21)*(x1800)))));
07980 IkReal x1805=(cj6)*(cj6);
07981 IkReal x1806=(sj6)*(sj6);
07982 IkReal x1807=((r11)*(r20));
07983 IkReal x1808=((r12)*(sj5));
07984 IkReal x1809=((IkReal(1.00000000000000))*(r10));
07985 IkReal x1810=((r22)*(sj5));
07986 IkReal x1811=((cj5)*(x1806));
07987 IkReal x1812=((cj5)*(x1805));
07988 dummyeval[0]=((((IkReal(-1.00000000000000))*(r21)*(x1809)*(x1812)))+(((IkReal(-1.00000000000000))*(r21)*(x1809)*(x1811)))+(((r20)*(sj6)*(x1808)))+(((x1807)*(x1811)))+(((x1807)*(x1812)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x1810)))+(((IkReal(-1.00000000000000))*(sj6)*(x1809)*(x1810)))+(((cj6)*(r21)*(x1808))));
07989 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07990 {
07991 {
07992 IkReal dummyeval[1];
07993 dummyeval[0]=cj1;
07994 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
07995 {
07996 {
07997 IkReal evalcond[5];
07998 IkReal x1813=((IkReal(0.000500000000000000))*(cj6));
07999 IkReal x1814=((IkReal(0.000500000000000000))*(sj6));
08000 IkReal x1815=((sj5)*(sj6));
08001 IkReal x1816=((cj5)*(r12));
08002 IkReal x1817=((IkReal(0.390000000000000))*(cj5));
08003 IkReal x1818=((IkReal(0.390000000000000))*(cj6)*(sj5));
08004 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
08005 evalcond[1]=((((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x1815)))+(x1816));
08006 evalcond[2]=((((r22)*(x1817)))+(((r20)*(x1818)))+(((r20)*(x1814)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x1813)))+(((IkReal(-0.390000000000000))*(r21)*(x1815))));
08007 evalcond[3]=((IkReal(-0.400000000000000))+(((r02)*(x1817)))+(((IkReal(-0.390000000000000))*(r01)*(x1815)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x1813)))+(((r00)*(x1818)))+(((r00)*(x1814))));
08008 evalcond[4]=((IkReal(0.00200000000000000))+(((IkReal(-0.390000000000000))*(r11)*(x1815)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.390000000000000))*(x1816)))+(((r11)*(x1813)))+(((r10)*(x1818)))+(((r10)*(x1814))));
08009 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
08010 {
08011 {
08012 IkReal j3array[1], cj3array[1], sj3array[1];
08013 bool j3valid[1]={false};
08014 _nj3 = 1;
08015 IkReal x1819=((cj6)*(sj5));
08016 IkReal x1820=((sj5)*(sj6));
08017 if( IKabs(((((cj5)*(r22)))+(((r20)*(x1819)))+(((IkReal(-1.00000000000000))*(r21)*(x1820))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r00)*(x1819)))+(((r01)*(x1820)))+(((IkReal(-1.00000000000000))*(cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((r20)*(x1819)))+(((IkReal(-1.00000000000000))*(r21)*(x1820)))))+IKsqr(((((IkReal(-1.00000000000000))*(r00)*(x1819)))+(((r01)*(x1820)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
08018     continue;
08019 j3array[0]=IKatan2(((((cj5)*(r22)))+(((r20)*(x1819)))+(((IkReal(-1.00000000000000))*(r21)*(x1820)))), ((((IkReal(-1.00000000000000))*(r00)*(x1819)))+(((r01)*(x1820)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))));
08020 sj3array[0]=IKsin(j3array[0]);
08021 cj3array[0]=IKcos(j3array[0]);
08022 if( j3array[0] > IKPI )
08023 {
08024     j3array[0]-=IK2PI;
08025 }
08026 else if( j3array[0] < -IKPI )
08027 {    j3array[0]+=IK2PI;
08028 }
08029 j3valid[0] = true;
08030 for(int ij3 = 0; ij3 < 1; ++ij3)
08031 {
08032 if( !j3valid[ij3] )
08033 {
08034     continue;
08035 }
08036 _ij3[0] = ij3; _ij3[1] = -1;
08037 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08038 {
08039 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08040 {
08041     j3valid[iij3]=false; _ij3[1] = iij3; break; 
08042 }
08043 }
08044 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08045 {
08046 IkReal evalcond[2];
08047 IkReal x1821=((cj6)*(sj5));
08048 IkReal x1822=((IkReal(1.00000000000000))*(sj5)*(sj6));
08049 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(r21)*(x1822)))+(((r20)*(x1821))));
08050 evalcond[1]=((((r00)*(x1821)))+(IKcos(j3))+(((IkReal(-1.00000000000000))*(r01)*(x1822)))+(((cj5)*(r02))));
08051 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
08052 {
08053 continue;
08054 }
08055 }
08056 
08057 {
08058 IkReal dummyeval[1];
08059 IkReal gconst27;
08060 IkReal x1823=(sj6)*(sj6);
08061 IkReal x1824=(cj6)*(cj6);
08062 IkReal x1825=((cj6)*(sj5));
08063 IkReal x1826=((r10)*(r21));
08064 IkReal x1827=((IkReal(1.00000000000000))*(r12));
08065 IkReal x1828=((sj5)*(sj6));
08066 IkReal x1829=((cj5)*(x1824));
08067 IkReal x1830=((IkReal(1.00000000000000))*(r11)*(r20));
08068 IkReal x1831=((cj5)*(x1823));
08069 gconst27=IKsign(((((x1826)*(x1831)))+(((x1826)*(x1829)))+(((r10)*(r22)*(x1828)))+(((IkReal(-1.00000000000000))*(r20)*(x1827)*(x1828)))+(((r11)*(r22)*(x1825)))+(((IkReal(-1.00000000000000))*(x1829)*(x1830)))+(((IkReal(-1.00000000000000))*(r21)*(x1825)*(x1827)))+(((IkReal(-1.00000000000000))*(x1830)*(x1831)))));
08070 IkReal x1832=(sj6)*(sj6);
08071 IkReal x1833=(cj6)*(cj6);
08072 IkReal x1834=((cj6)*(sj5));
08073 IkReal x1835=((r10)*(r21));
08074 IkReal x1836=((IkReal(1.00000000000000))*(r12));
08075 IkReal x1837=((sj5)*(sj6));
08076 IkReal x1838=((cj5)*(x1833));
08077 IkReal x1839=((IkReal(1.00000000000000))*(r11)*(r20));
08078 IkReal x1840=((cj5)*(x1832));
08079 dummyeval[0]=((((IkReal(-1.00000000000000))*(r20)*(x1836)*(x1837)))+(((r10)*(r22)*(x1837)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1835)*(x1838)))+(((IkReal(-1.00000000000000))*(x1838)*(x1839)))+(((IkReal(-1.00000000000000))*(r21)*(x1834)*(x1836)))+(((r11)*(r22)*(x1834)))+(((x1835)*(x1840))));
08080 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08081 {
08082 {
08083 IkReal dummyeval[1];
08084 IkReal gconst28;
08085 IkReal x1841=(sj6)*(sj6);
08086 IkReal x1842=(cj6)*(cj6);
08087 IkReal x1843=((r01)*(r10));
08088 IkReal x1844=((IkReal(1.00000000000000))*(r00));
08089 IkReal x1845=((r02)*(sj5));
08090 IkReal x1846=((r12)*(sj5));
08091 IkReal x1847=((cj5)*(x1841));
08092 IkReal x1848=((cj5)*(x1842));
08093 gconst28=IKsign(((((r10)*(sj6)*(x1845)))+(((cj6)*(r11)*(x1845)))+(((IkReal(-1.00000000000000))*(r11)*(x1844)*(x1848)))+(((IkReal(-1.00000000000000))*(r11)*(x1844)*(x1847)))+(((IkReal(-1.00000000000000))*(sj6)*(x1844)*(x1846)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1846)))+(((x1843)*(x1848)))+(((x1843)*(x1847)))));
08094 IkReal x1849=(sj6)*(sj6);
08095 IkReal x1850=(cj6)*(cj6);
08096 IkReal x1851=((r01)*(r10));
08097 IkReal x1852=((IkReal(1.00000000000000))*(r00));
08098 IkReal x1853=((r02)*(sj5));
08099 IkReal x1854=((r12)*(sj5));
08100 IkReal x1855=((cj5)*(x1849));
08101 IkReal x1856=((cj5)*(x1850));
08102 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x1852)*(x1855)))+(((IkReal(-1.00000000000000))*(r11)*(x1852)*(x1856)))+(((x1851)*(x1855)))+(((x1851)*(x1856)))+(((cj6)*(r11)*(x1853)))+(((IkReal(-1.00000000000000))*(sj6)*(x1852)*(x1854)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x1854)))+(((r10)*(sj6)*(x1853))));
08103 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08104 {
08105 continue;
08106 
08107 } else
08108 {
08109 {
08110 IkReal j4array[1], cj4array[1], sj4array[1];
08111 bool j4valid[1]={false};
08112 _nj4 = 1;
08113 IkReal x1857=((sj3)*(sj6));
08114 IkReal x1858=((IkReal(1.00000000000000))*(r10));
08115 IkReal x1859=((cj6)*(sj3));
08116 if( IKabs(((gconst28)*(((((r12)*(sj3)*(sj5)))+(((cj5)*(r11)*(x1857)))+(((IkReal(-1.00000000000000))*(cj5)*(x1858)*(x1859))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(x1857)*(x1858)))+(((IkReal(-1.00000000000000))*(r11)*(x1859))))))) < IKFAST_ATAN2_MAGTHRESH )
08117     continue;
08118 j4array[0]=IKatan2(((gconst28)*(((((r12)*(sj3)*(sj5)))+(((cj5)*(r11)*(x1857)))+(((IkReal(-1.00000000000000))*(cj5)*(x1858)*(x1859)))))), ((gconst28)*(((((IkReal(-1.00000000000000))*(x1857)*(x1858)))+(((IkReal(-1.00000000000000))*(r11)*(x1859)))))));
08119 sj4array[0]=IKsin(j4array[0]);
08120 cj4array[0]=IKcos(j4array[0]);
08121 if( j4array[0] > IKPI )
08122 {
08123     j4array[0]-=IK2PI;
08124 }
08125 else if( j4array[0] < -IKPI )
08126 {    j4array[0]+=IK2PI;
08127 }
08128 j4valid[0] = true;
08129 for(int ij4 = 0; ij4 < 1; ++ij4)
08130 {
08131 if( !j4valid[ij4] )
08132 {
08133     continue;
08134 }
08135 _ij4[0] = ij4; _ij4[1] = -1;
08136 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08137 {
08138 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08139 {
08140     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08141 }
08142 }
08143 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08144 {
08145 IkReal evalcond[6];
08146 IkReal x1860=IKsin(j4);
08147 IkReal x1861=IKcos(j4);
08148 IkReal x1862=((IkReal(1.00000000000000))*(r00));
08149 IkReal x1863=((cj5)*(r01));
08150 IkReal x1864=((cj5)*(r11));
08151 IkReal x1865=((IkReal(1.00000000000000))*(cj5));
08152 IkReal x1866=((cj5)*(r21));
08153 IkReal x1867=((sj5)*(x1860));
08154 IkReal x1868=((cj6)*(x1860));
08155 IkReal x1869=((sj6)*(x1861));
08156 IkReal x1870=((sj6)*(x1860));
08157 IkReal x1871=((sj5)*(x1861));
08158 IkReal x1872=((cj6)*(x1861));
08159 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1865)*(x1868)))+(((x1866)*(x1870)))+(((r22)*(x1867)))+(((IkReal(-1.00000000000000))*(r21)*(x1872)))+(((IkReal(-1.00000000000000))*(r20)*(x1869))));
08160 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1865)*(x1872)))+(((r22)*(x1871)))+(cj3)+(((x1866)*(x1869)))+(((r20)*(x1870)))+(((r21)*(x1868))));
08161 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1872)))+(((x1863)*(x1870)))+(((IkReal(-1.00000000000000))*(cj5)*(x1862)*(x1868)))+(((r02)*(x1867)))+(((IkReal(-1.00000000000000))*(x1862)*(x1869))));
08162 evalcond[3]=((IkReal(1.00000000000000))+(((r12)*(x1867)))+(((IkReal(-1.00000000000000))*(r11)*(x1872)))+(((IkReal(-1.00000000000000))*(r10)*(x1869)))+(((IkReal(-1.00000000000000))*(r10)*(x1865)*(x1868)))+(((x1864)*(x1870))));
08163 evalcond[4]=((sj3)+(((r02)*(x1871)))+(((x1863)*(x1869)))+(((IkReal(-1.00000000000000))*(cj5)*(x1862)*(x1872)))+(((r00)*(x1870)))+(((r01)*(x1868))));
08164 evalcond[5]=((((x1864)*(x1869)))+(((r11)*(x1868)))+(((IkReal(-1.00000000000000))*(r10)*(x1865)*(x1872)))+(((r10)*(x1870)))+(((r12)*(x1871))));
08165 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08166 {
08167 continue;
08168 }
08169 }
08170 
08171 {
08172 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08173 vinfos[0].jointtype = 1;
08174 vinfos[0].foffset = j0;
08175 vinfos[0].indices[0] = _ij0[0];
08176 vinfos[0].indices[1] = _ij0[1];
08177 vinfos[0].maxsolutions = _nj0;
08178 vinfos[1].jointtype = 1;
08179 vinfos[1].foffset = j1;
08180 vinfos[1].indices[0] = _ij1[0];
08181 vinfos[1].indices[1] = _ij1[1];
08182 vinfos[1].maxsolutions = _nj1;
08183 vinfos[2].jointtype = 1;
08184 vinfos[2].foffset = j2;
08185 vinfos[2].indices[0] = _ij2[0];
08186 vinfos[2].indices[1] = _ij2[1];
08187 vinfos[2].maxsolutions = _nj2;
08188 vinfos[3].jointtype = 1;
08189 vinfos[3].foffset = j3;
08190 vinfos[3].indices[0] = _ij3[0];
08191 vinfos[3].indices[1] = _ij3[1];
08192 vinfos[3].maxsolutions = _nj3;
08193 vinfos[4].jointtype = 1;
08194 vinfos[4].foffset = j4;
08195 vinfos[4].indices[0] = _ij4[0];
08196 vinfos[4].indices[1] = _ij4[1];
08197 vinfos[4].maxsolutions = _nj4;
08198 vinfos[5].jointtype = 1;
08199 vinfos[5].foffset = j5;
08200 vinfos[5].indices[0] = _ij5[0];
08201 vinfos[5].indices[1] = _ij5[1];
08202 vinfos[5].maxsolutions = _nj5;
08203 vinfos[6].jointtype = 1;
08204 vinfos[6].foffset = j6;
08205 vinfos[6].indices[0] = _ij6[0];
08206 vinfos[6].indices[1] = _ij6[1];
08207 vinfos[6].maxsolutions = _nj6;
08208 std::vector<int> vfree(0);
08209 solutions.AddSolution(vinfos,vfree);
08210 }
08211 }
08212 }
08213 
08214 }
08215 
08216 }
08217 
08218 } else
08219 {
08220 {
08221 IkReal j4array[1], cj4array[1], sj4array[1];
08222 bool j4valid[1]={false};
08223 _nj4 = 1;
08224 IkReal x1873=((cj3)*(cj5));
08225 IkReal x1874=((IkReal(1.00000000000000))*(cj6));
08226 if( IKabs(((gconst27)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(r10)*(x1873)*(x1874)))+(((r11)*(sj6)*(x1873))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(cj3)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(x1874))))))) < IKFAST_ATAN2_MAGTHRESH )
08227     continue;
08228 j4array[0]=IKatan2(((gconst27)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(r10)*(x1873)*(x1874)))+(((r11)*(sj6)*(x1873)))))), ((gconst27)*(((((IkReal(-1.00000000000000))*(cj3)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(x1874)))))));
08229 sj4array[0]=IKsin(j4array[0]);
08230 cj4array[0]=IKcos(j4array[0]);
08231 if( j4array[0] > IKPI )
08232 {
08233     j4array[0]-=IK2PI;
08234 }
08235 else if( j4array[0] < -IKPI )
08236 {    j4array[0]+=IK2PI;
08237 }
08238 j4valid[0] = true;
08239 for(int ij4 = 0; ij4 < 1; ++ij4)
08240 {
08241 if( !j4valid[ij4] )
08242 {
08243     continue;
08244 }
08245 _ij4[0] = ij4; _ij4[1] = -1;
08246 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08247 {
08248 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08249 {
08250     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08251 }
08252 }
08253 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08254 {
08255 IkReal evalcond[6];
08256 IkReal x1875=IKsin(j4);
08257 IkReal x1876=IKcos(j4);
08258 IkReal x1877=((IkReal(1.00000000000000))*(r00));
08259 IkReal x1878=((cj5)*(r01));
08260 IkReal x1879=((cj5)*(r11));
08261 IkReal x1880=((IkReal(1.00000000000000))*(cj5));
08262 IkReal x1881=((cj5)*(r21));
08263 IkReal x1882=((sj5)*(x1875));
08264 IkReal x1883=((cj6)*(x1875));
08265 IkReal x1884=((sj6)*(x1876));
08266 IkReal x1885=((sj6)*(x1875));
08267 IkReal x1886=((sj5)*(x1876));
08268 IkReal x1887=((cj6)*(x1876));
08269 evalcond[0]=((((x1881)*(x1885)))+(((IkReal(-1.00000000000000))*(r20)*(x1884)))+(((IkReal(-1.00000000000000))*(r20)*(x1880)*(x1883)))+(((IkReal(-1.00000000000000))*(r21)*(x1887)))+(((r22)*(x1882))));
08270 evalcond[1]=((((r21)*(x1883)))+(((x1881)*(x1884)))+(cj3)+(((r20)*(x1885)))+(((IkReal(-1.00000000000000))*(r20)*(x1880)*(x1887)))+(((r22)*(x1886))));
08271 evalcond[2]=((((x1878)*(x1885)))+(((IkReal(-1.00000000000000))*(cj5)*(x1877)*(x1883)))+(((IkReal(-1.00000000000000))*(r01)*(x1887)))+(((IkReal(-1.00000000000000))*(x1877)*(x1884)))+(((r02)*(x1882))));
08272 evalcond[3]=((IkReal(1.00000000000000))+(((x1879)*(x1885)))+(((r12)*(x1882)))+(((IkReal(-1.00000000000000))*(r10)*(x1880)*(x1883)))+(((IkReal(-1.00000000000000))*(r10)*(x1884)))+(((IkReal(-1.00000000000000))*(r11)*(x1887))));
08273 evalcond[4]=((sj3)+(((x1878)*(x1884)))+(((r00)*(x1885)))+(((r01)*(x1883)))+(((IkReal(-1.00000000000000))*(cj5)*(x1877)*(x1887)))+(((r02)*(x1886))));
08274 evalcond[5]=((((x1879)*(x1884)))+(((r12)*(x1886)))+(((IkReal(-1.00000000000000))*(r10)*(x1880)*(x1887)))+(((r10)*(x1885)))+(((r11)*(x1883))));
08275 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08276 {
08277 continue;
08278 }
08279 }
08280 
08281 {
08282 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08283 vinfos[0].jointtype = 1;
08284 vinfos[0].foffset = j0;
08285 vinfos[0].indices[0] = _ij0[0];
08286 vinfos[0].indices[1] = _ij0[1];
08287 vinfos[0].maxsolutions = _nj0;
08288 vinfos[1].jointtype = 1;
08289 vinfos[1].foffset = j1;
08290 vinfos[1].indices[0] = _ij1[0];
08291 vinfos[1].indices[1] = _ij1[1];
08292 vinfos[1].maxsolutions = _nj1;
08293 vinfos[2].jointtype = 1;
08294 vinfos[2].foffset = j2;
08295 vinfos[2].indices[0] = _ij2[0];
08296 vinfos[2].indices[1] = _ij2[1];
08297 vinfos[2].maxsolutions = _nj2;
08298 vinfos[3].jointtype = 1;
08299 vinfos[3].foffset = j3;
08300 vinfos[3].indices[0] = _ij3[0];
08301 vinfos[3].indices[1] = _ij3[1];
08302 vinfos[3].maxsolutions = _nj3;
08303 vinfos[4].jointtype = 1;
08304 vinfos[4].foffset = j4;
08305 vinfos[4].indices[0] = _ij4[0];
08306 vinfos[4].indices[1] = _ij4[1];
08307 vinfos[4].maxsolutions = _nj4;
08308 vinfos[5].jointtype = 1;
08309 vinfos[5].foffset = j5;
08310 vinfos[5].indices[0] = _ij5[0];
08311 vinfos[5].indices[1] = _ij5[1];
08312 vinfos[5].maxsolutions = _nj5;
08313 vinfos[6].jointtype = 1;
08314 vinfos[6].foffset = j6;
08315 vinfos[6].indices[0] = _ij6[0];
08316 vinfos[6].indices[1] = _ij6[1];
08317 vinfos[6].maxsolutions = _nj6;
08318 std::vector<int> vfree(0);
08319 solutions.AddSolution(vinfos,vfree);
08320 }
08321 }
08322 }
08323 
08324 }
08325 
08326 }
08327 }
08328 }
08329 
08330 } else
08331 {
08332 IkReal x1888=((IkReal(0.000500000000000000))*(cj6));
08333 IkReal x1889=((IkReal(0.000500000000000000))*(sj6));
08334 IkReal x1890=((sj5)*(sj6));
08335 IkReal x1891=((cj5)*(r12));
08336 IkReal x1892=((IkReal(0.390000000000000))*(cj5));
08337 IkReal x1893=((IkReal(0.390000000000000))*(cj6)*(sj5));
08338 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
08339 evalcond[1]=((((cj6)*(r10)*(sj5)))+(x1891)+(((IkReal(-1.00000000000000))*(r11)*(x1890))));
08340 evalcond[2]=((((r22)*(x1892)))+(((r21)*(x1888)))+(((r20)*(x1889)))+(((r20)*(x1893)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.390000000000000))*(r21)*(x1890))));
08341 evalcond[3]=((IkReal(0.400000000000000))+(((r02)*(x1892)))+(((r00)*(x1893)))+(((r00)*(x1889)))+(((r01)*(x1888)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.390000000000000))*(r01)*(x1890))));
08342 evalcond[4]=((IkReal(-0.00200000000000000))+(((r10)*(x1893)))+(((r10)*(x1889)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.390000000000000))*(r11)*(x1890)))+(((IkReal(0.390000000000000))*(x1891)))+(((r11)*(x1888))));
08343 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
08344 {
08345 {
08346 IkReal j3array[1], cj3array[1], sj3array[1];
08347 bool j3valid[1]={false};
08348 _nj3 = 1;
08349 IkReal x1894=((cj6)*(sj5));
08350 IkReal x1895=((IkReal(1.00000000000000))*(sj5)*(sj6));
08351 if( IKabs(((((cj5)*(r22)))+(((r20)*(x1894)))+(((IkReal(-1.00000000000000))*(r21)*(x1895))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r00)*(x1894)))+(((IkReal(-1.00000000000000))*(r01)*(x1895)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((r20)*(x1894)))+(((IkReal(-1.00000000000000))*(r21)*(x1895)))))+IKsqr(((((r00)*(x1894)))+(((IkReal(-1.00000000000000))*(r01)*(x1895)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
08352     continue;
08353 j3array[0]=IKatan2(((((cj5)*(r22)))+(((r20)*(x1894)))+(((IkReal(-1.00000000000000))*(r21)*(x1895)))), ((((r00)*(x1894)))+(((IkReal(-1.00000000000000))*(r01)*(x1895)))+(((cj5)*(r02)))));
08354 sj3array[0]=IKsin(j3array[0]);
08355 cj3array[0]=IKcos(j3array[0]);
08356 if( j3array[0] > IKPI )
08357 {
08358     j3array[0]-=IK2PI;
08359 }
08360 else if( j3array[0] < -IKPI )
08361 {    j3array[0]+=IK2PI;
08362 }
08363 j3valid[0] = true;
08364 for(int ij3 = 0; ij3 < 1; ++ij3)
08365 {
08366 if( !j3valid[ij3] )
08367 {
08368     continue;
08369 }
08370 _ij3[0] = ij3; _ij3[1] = -1;
08371 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08372 {
08373 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08374 {
08375     j3valid[iij3]=false; _ij3[1] = iij3; break; 
08376 }
08377 }
08378 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08379 {
08380 IkReal evalcond[2];
08381 IkReal x1896=((cj6)*(sj5));
08382 IkReal x1897=((IkReal(1.00000000000000))*(sj5)*(sj6));
08383 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1896)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(r21)*(x1897))));
08384 evalcond[1]=((((r00)*(x1896)))+(((IkReal(-1.00000000000000))*(r01)*(x1897)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((cj5)*(r02))));
08385 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
08386 {
08387 continue;
08388 }
08389 }
08390 
08391 {
08392 IkReal dummyeval[1];
08393 IkReal gconst31;
08394 IkReal x1898=(sj6)*(sj6);
08395 IkReal x1899=(cj6)*(cj6);
08396 IkReal x1900=((cj6)*(sj5));
08397 IkReal x1901=((r10)*(r21));
08398 IkReal x1902=((IkReal(1.00000000000000))*(r12));
08399 IkReal x1903=((sj5)*(sj6));
08400 IkReal x1904=((cj5)*(x1899));
08401 IkReal x1905=((IkReal(1.00000000000000))*(r11)*(r20));
08402 IkReal x1906=((cj5)*(x1898));
08403 gconst31=IKsign(((((x1901)*(x1904)))+(((x1901)*(x1906)))+(((r11)*(r22)*(x1900)))+(((IkReal(-1.00000000000000))*(x1905)*(x1906)))+(((r10)*(r22)*(x1903)))+(((IkReal(-1.00000000000000))*(x1904)*(x1905)))+(((IkReal(-1.00000000000000))*(r21)*(x1900)*(x1902)))+(((IkReal(-1.00000000000000))*(r20)*(x1902)*(x1903)))));
08404 IkReal x1907=(sj6)*(sj6);
08405 IkReal x1908=(cj6)*(cj6);
08406 IkReal x1909=((cj6)*(sj5));
08407 IkReal x1910=((r10)*(r21));
08408 IkReal x1911=((IkReal(1.00000000000000))*(r12));
08409 IkReal x1912=((sj5)*(sj6));
08410 IkReal x1913=((cj5)*(x1908));
08411 IkReal x1914=((IkReal(1.00000000000000))*(r11)*(r20));
08412 IkReal x1915=((cj5)*(x1907));
08413 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1914)*(x1915)))+(((IkReal(-1.00000000000000))*(r20)*(x1911)*(x1912)))+(((r11)*(r22)*(x1909)))+(((x1910)*(x1913)))+(((x1910)*(x1915)))+(((r10)*(r22)*(x1912)))+(((IkReal(-1.00000000000000))*(r21)*(x1909)*(x1911)))+(((IkReal(-1.00000000000000))*(x1913)*(x1914))));
08414 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08415 {
08416 {
08417 IkReal dummyeval[1];
08418 IkReal gconst32;
08419 IkReal x1916=(r20)*(r20);
08420 IkReal x1917=(cj5)*(cj5);
08421 IkReal x1918=(sj6)*(sj6);
08422 IkReal x1919=(cj6)*(cj6);
08423 IkReal x1920=(r21)*(r21);
08424 IkReal x1921=((cj6)*(r20));
08425 IkReal x1922=((IkReal(2.00000000000000))*(r21)*(sj6));
08426 IkReal x1923=((cj5)*(r22)*(sj5));
08427 gconst32=IKsign(((((IkReal(-1.00000000000000))*(x1917)*(x1921)*(x1922)))+(((x1921)*(x1922)))+(((x1916)*(x1918)))+(((x1916)*(x1917)*(x1919)))+(((x1917)*(x1918)*(x1920)))+(((x1919)*(x1920)))+(((IkReal(-2.00000000000000))*(x1921)*(x1923)))+((((r22)*(r22))*((sj5)*(sj5))))+(((x1922)*(x1923)))));
08428 IkReal x1924=(r20)*(r20);
08429 IkReal x1925=(cj5)*(cj5);
08430 IkReal x1926=(sj6)*(sj6);
08431 IkReal x1927=(cj6)*(cj6);
08432 IkReal x1928=(r21)*(r21);
08433 IkReal x1929=((cj6)*(r20));
08434 IkReal x1930=((IkReal(2.00000000000000))*(r21)*(sj6));
08435 IkReal x1931=((cj5)*(r22)*(sj5));
08436 dummyeval[0]=((((x1927)*(x1928)))+(((x1930)*(x1931)))+(((IkReal(-1.00000000000000))*(x1925)*(x1929)*(x1930)))+(((x1924)*(x1925)*(x1927)))+(((IkReal(-2.00000000000000))*(x1929)*(x1931)))+(((x1929)*(x1930)))+(((x1925)*(x1926)*(x1928)))+((((r22)*(r22))*((sj5)*(sj5))))+(((x1924)*(x1926))));
08437 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08438 {
08439 continue;
08440 
08441 } else
08442 {
08443 {
08444 IkReal j4array[1], cj4array[1], sj4array[1];
08445 bool j4valid[1]={false};
08446 _nj4 = 1;
08447 IkReal x1932=((cj3)*(cj6));
08448 IkReal x1933=((IkReal(1.00000000000000))*(r21));
08449 IkReal x1934=((cj3)*(sj6));
08450 if( IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(x1932)*(x1933)))+(((IkReal(-1.00000000000000))*(r20)*(x1934))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(cj5)*(x1933)*(x1934)))+(((cj5)*(r20)*(x1932)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
08451     continue;
08452 j4array[0]=IKatan2(((gconst32)*(((((IkReal(-1.00000000000000))*(x1932)*(x1933)))+(((IkReal(-1.00000000000000))*(r20)*(x1934)))))), ((gconst32)*(((((IkReal(-1.00000000000000))*(cj5)*(x1933)*(x1934)))+(((cj5)*(r20)*(x1932)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))))));
08453 sj4array[0]=IKsin(j4array[0]);
08454 cj4array[0]=IKcos(j4array[0]);
08455 if( j4array[0] > IKPI )
08456 {
08457     j4array[0]-=IK2PI;
08458 }
08459 else if( j4array[0] < -IKPI )
08460 {    j4array[0]+=IK2PI;
08461 }
08462 j4valid[0] = true;
08463 for(int ij4 = 0; ij4 < 1; ++ij4)
08464 {
08465 if( !j4valid[ij4] )
08466 {
08467     continue;
08468 }
08469 _ij4[0] = ij4; _ij4[1] = -1;
08470 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08471 {
08472 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08473 {
08474     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08475 }
08476 }
08477 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08478 {
08479 IkReal evalcond[6];
08480 IkReal x1935=IKsin(j4);
08481 IkReal x1936=IKcos(j4);
08482 IkReal x1937=((IkReal(1.00000000000000))*(r00));
08483 IkReal x1938=((cj5)*(r01));
08484 IkReal x1939=((cj5)*(r11));
08485 IkReal x1940=((IkReal(1.00000000000000))*(cj5));
08486 IkReal x1941=((cj5)*(r21));
08487 IkReal x1942=((sj5)*(x1935));
08488 IkReal x1943=((cj6)*(x1935));
08489 IkReal x1944=((sj6)*(x1936));
08490 IkReal x1945=((sj6)*(x1935));
08491 IkReal x1946=((sj5)*(x1936));
08492 IkReal x1947=((cj6)*(x1936));
08493 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1944)))+(((IkReal(-1.00000000000000))*(r20)*(x1940)*(x1943)))+(((r22)*(x1942)))+(((x1941)*(x1945)))+(((IkReal(-1.00000000000000))*(r21)*(x1947))));
08494 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1940)*(x1947)))+(cj3)+(((r20)*(x1945)))+(((r22)*(x1946)))+(((x1941)*(x1944)))+(((r21)*(x1943))));
08495 evalcond[2]=((((x1938)*(x1945)))+(((IkReal(-1.00000000000000))*(x1937)*(x1944)))+(((IkReal(-1.00000000000000))*(r01)*(x1947)))+(((IkReal(-1.00000000000000))*(cj5)*(x1937)*(x1943)))+(((r02)*(x1942))));
08496 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x1940)*(x1943)))+(((x1939)*(x1945)))+(((r12)*(x1942)))+(((IkReal(-1.00000000000000))*(r11)*(x1947)))+(((IkReal(-1.00000000000000))*(r10)*(x1944))));
08497 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((x1938)*(x1944)))+(((r00)*(x1945)))+(((r01)*(x1943)))+(((IkReal(-1.00000000000000))*(cj5)*(x1937)*(x1947)))+(((r02)*(x1946))));
08498 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1940)*(x1947)))+(((r11)*(x1943)))+(((x1939)*(x1944)))+(((r10)*(x1945)))+(((r12)*(x1946))));
08499 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08500 {
08501 continue;
08502 }
08503 }
08504 
08505 {
08506 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08507 vinfos[0].jointtype = 1;
08508 vinfos[0].foffset = j0;
08509 vinfos[0].indices[0] = _ij0[0];
08510 vinfos[0].indices[1] = _ij0[1];
08511 vinfos[0].maxsolutions = _nj0;
08512 vinfos[1].jointtype = 1;
08513 vinfos[1].foffset = j1;
08514 vinfos[1].indices[0] = _ij1[0];
08515 vinfos[1].indices[1] = _ij1[1];
08516 vinfos[1].maxsolutions = _nj1;
08517 vinfos[2].jointtype = 1;
08518 vinfos[2].foffset = j2;
08519 vinfos[2].indices[0] = _ij2[0];
08520 vinfos[2].indices[1] = _ij2[1];
08521 vinfos[2].maxsolutions = _nj2;
08522 vinfos[3].jointtype = 1;
08523 vinfos[3].foffset = j3;
08524 vinfos[3].indices[0] = _ij3[0];
08525 vinfos[3].indices[1] = _ij3[1];
08526 vinfos[3].maxsolutions = _nj3;
08527 vinfos[4].jointtype = 1;
08528 vinfos[4].foffset = j4;
08529 vinfos[4].indices[0] = _ij4[0];
08530 vinfos[4].indices[1] = _ij4[1];
08531 vinfos[4].maxsolutions = _nj4;
08532 vinfos[5].jointtype = 1;
08533 vinfos[5].foffset = j5;
08534 vinfos[5].indices[0] = _ij5[0];
08535 vinfos[5].indices[1] = _ij5[1];
08536 vinfos[5].maxsolutions = _nj5;
08537 vinfos[6].jointtype = 1;
08538 vinfos[6].foffset = j6;
08539 vinfos[6].indices[0] = _ij6[0];
08540 vinfos[6].indices[1] = _ij6[1];
08541 vinfos[6].maxsolutions = _nj6;
08542 std::vector<int> vfree(0);
08543 solutions.AddSolution(vinfos,vfree);
08544 }
08545 }
08546 }
08547 
08548 }
08549 
08550 }
08551 
08552 } else
08553 {
08554 {
08555 IkReal j4array[1], cj4array[1], sj4array[1];
08556 bool j4valid[1]={false};
08557 _nj4 = 1;
08558 IkReal x1948=((cj3)*(sj6));
08559 IkReal x1949=((IkReal(1.00000000000000))*(r10));
08560 IkReal x1950=((cj3)*(cj6));
08561 if( IKabs(((gconst31)*(((((cj3)*(r12)*(sj5)))+(((cj5)*(r11)*(x1948)))+(((IkReal(-1.00000000000000))*(cj5)*(x1949)*(x1950))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1950)))+(((IkReal(-1.00000000000000))*(x1948)*(x1949))))))) < IKFAST_ATAN2_MAGTHRESH )
08562     continue;
08563 j4array[0]=IKatan2(((gconst31)*(((((cj3)*(r12)*(sj5)))+(((cj5)*(r11)*(x1948)))+(((IkReal(-1.00000000000000))*(cj5)*(x1949)*(x1950)))))), ((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1950)))+(((IkReal(-1.00000000000000))*(x1948)*(x1949)))))));
08564 sj4array[0]=IKsin(j4array[0]);
08565 cj4array[0]=IKcos(j4array[0]);
08566 if( j4array[0] > IKPI )
08567 {
08568     j4array[0]-=IK2PI;
08569 }
08570 else if( j4array[0] < -IKPI )
08571 {    j4array[0]+=IK2PI;
08572 }
08573 j4valid[0] = true;
08574 for(int ij4 = 0; ij4 < 1; ++ij4)
08575 {
08576 if( !j4valid[ij4] )
08577 {
08578     continue;
08579 }
08580 _ij4[0] = ij4; _ij4[1] = -1;
08581 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08582 {
08583 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08584 {
08585     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08586 }
08587 }
08588 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08589 {
08590 IkReal evalcond[6];
08591 IkReal x1951=IKsin(j4);
08592 IkReal x1952=IKcos(j4);
08593 IkReal x1953=((IkReal(1.00000000000000))*(r00));
08594 IkReal x1954=((cj5)*(r01));
08595 IkReal x1955=((cj5)*(r11));
08596 IkReal x1956=((IkReal(1.00000000000000))*(cj5));
08597 IkReal x1957=((cj5)*(r21));
08598 IkReal x1958=((sj5)*(x1951));
08599 IkReal x1959=((cj6)*(x1951));
08600 IkReal x1960=((sj6)*(x1952));
08601 IkReal x1961=((sj6)*(x1951));
08602 IkReal x1962=((sj5)*(x1952));
08603 IkReal x1963=((cj6)*(x1952));
08604 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x1960)))+(((IkReal(-1.00000000000000))*(r20)*(x1956)*(x1959)))+(((x1957)*(x1961)))+(((r22)*(x1958)))+(((IkReal(-1.00000000000000))*(r21)*(x1963))));
08605 evalcond[1]=((cj3)+(((IkReal(-1.00000000000000))*(r20)*(x1956)*(x1963)))+(((r20)*(x1961)))+(((r21)*(x1959)))+(((x1957)*(x1960)))+(((r22)*(x1962))));
08606 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x1953)*(x1959)))+(((x1954)*(x1961)))+(((IkReal(-1.00000000000000))*(r01)*(x1963)))+(((IkReal(-1.00000000000000))*(x1953)*(x1960)))+(((r02)*(x1958))));
08607 evalcond[3]=((IkReal(-1.00000000000000))+(((r12)*(x1958)))+(((x1955)*(x1961)))+(((IkReal(-1.00000000000000))*(r11)*(x1963)))+(((IkReal(-1.00000000000000))*(r10)*(x1960)))+(((IkReal(-1.00000000000000))*(r10)*(x1956)*(x1959))));
08608 evalcond[4]=((((r02)*(x1962)))+(((IkReal(-1.00000000000000))*(sj3)))+(((x1954)*(x1960)))+(((IkReal(-1.00000000000000))*(cj5)*(x1953)*(x1963)))+(((r00)*(x1961)))+(((r01)*(x1959))));
08609 evalcond[5]=((((r10)*(x1961)))+(((r11)*(x1959)))+(((r12)*(x1962)))+(((IkReal(-1.00000000000000))*(r10)*(x1956)*(x1963)))+(((x1955)*(x1960))));
08610 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08611 {
08612 continue;
08613 }
08614 }
08615 
08616 {
08617 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08618 vinfos[0].jointtype = 1;
08619 vinfos[0].foffset = j0;
08620 vinfos[0].indices[0] = _ij0[0];
08621 vinfos[0].indices[1] = _ij0[1];
08622 vinfos[0].maxsolutions = _nj0;
08623 vinfos[1].jointtype = 1;
08624 vinfos[1].foffset = j1;
08625 vinfos[1].indices[0] = _ij1[0];
08626 vinfos[1].indices[1] = _ij1[1];
08627 vinfos[1].maxsolutions = _nj1;
08628 vinfos[2].jointtype = 1;
08629 vinfos[2].foffset = j2;
08630 vinfos[2].indices[0] = _ij2[0];
08631 vinfos[2].indices[1] = _ij2[1];
08632 vinfos[2].maxsolutions = _nj2;
08633 vinfos[3].jointtype = 1;
08634 vinfos[3].foffset = j3;
08635 vinfos[3].indices[0] = _ij3[0];
08636 vinfos[3].indices[1] = _ij3[1];
08637 vinfos[3].maxsolutions = _nj3;
08638 vinfos[4].jointtype = 1;
08639 vinfos[4].foffset = j4;
08640 vinfos[4].indices[0] = _ij4[0];
08641 vinfos[4].indices[1] = _ij4[1];
08642 vinfos[4].maxsolutions = _nj4;
08643 vinfos[5].jointtype = 1;
08644 vinfos[5].foffset = j5;
08645 vinfos[5].indices[0] = _ij5[0];
08646 vinfos[5].indices[1] = _ij5[1];
08647 vinfos[5].maxsolutions = _nj5;
08648 vinfos[6].jointtype = 1;
08649 vinfos[6].foffset = j6;
08650 vinfos[6].indices[0] = _ij6[0];
08651 vinfos[6].indices[1] = _ij6[1];
08652 vinfos[6].maxsolutions = _nj6;
08653 std::vector<int> vfree(0);
08654 solutions.AddSolution(vinfos,vfree);
08655 }
08656 }
08657 }
08658 
08659 }
08660 
08661 }
08662 }
08663 }
08664 
08665 } else
08666 {
08667 if( 1 )
08668 {
08669 continue;
08670 
08671 } else
08672 {
08673 }
08674 }
08675 }
08676 }
08677 
08678 } else
08679 {
08680 {
08681 IkReal j3array[1], cj3array[1], sj3array[1];
08682 bool j3valid[1]={false};
08683 _nj3 = 1;
08684 IkReal x1964=((cj6)*(sj5));
08685 IkReal x1965=((IkReal(1.00000000000000))*(sj5)*(sj6));
08686 if( IKabs(((((cj5)*(r22)))+(((r20)*(x1964)))+(((IkReal(-1.00000000000000))*(r21)*(x1965))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1964)))+(((IkReal(-1.00000000000000))*(r11)*(x1965))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((r20)*(x1964)))+(((IkReal(-1.00000000000000))*(r21)*(x1965)))))+IKsqr(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1964)))+(((IkReal(-1.00000000000000))*(r11)*(x1965)))))))-1) <= IKFAST_SINCOS_THRESH )
08687     continue;
08688 j3array[0]=IKatan2(((((cj5)*(r22)))+(((r20)*(x1964)))+(((IkReal(-1.00000000000000))*(r21)*(x1965)))), ((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x1964)))+(((IkReal(-1.00000000000000))*(r11)*(x1965)))))));
08689 sj3array[0]=IKsin(j3array[0]);
08690 cj3array[0]=IKcos(j3array[0]);
08691 if( j3array[0] > IKPI )
08692 {
08693     j3array[0]-=IK2PI;
08694 }
08695 else if( j3array[0] < -IKPI )
08696 {    j3array[0]+=IK2PI;
08697 }
08698 j3valid[0] = true;
08699 for(int ij3 = 0; ij3 < 1; ++ij3)
08700 {
08701 if( !j3valid[ij3] )
08702 {
08703     continue;
08704 }
08705 _ij3[0] = ij3; _ij3[1] = -1;
08706 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08707 {
08708 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08709 {
08710     j3valid[iij3]=false; _ij3[1] = iij3; break; 
08711 }
08712 }
08713 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08714 {
08715 IkReal evalcond[3];
08716 IkReal x1966=IKcos(j3);
08717 IkReal x1967=((cj6)*(sj5));
08718 IkReal x1968=((IkReal(1.00000000000000))*(sj5)*(sj6));
08719 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x1967)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(r21)*(x1968))));
08720 evalcond[1]=((((sj1)*(x1966)))+(((r00)*(x1967)))+(((IkReal(-1.00000000000000))*(r01)*(x1968)))+(((cj5)*(r02))));
08721 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x1967)))+(((IkReal(-1.00000000000000))*(cj1)*(x1966)))+(((IkReal(-1.00000000000000))*(r11)*(x1968))));
08722 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
08723 {
08724 continue;
08725 }
08726 }
08727 
08728 {
08729 IkReal dummyeval[1];
08730 IkReal gconst23;
08731 IkReal x1969=(r20)*(r20);
08732 IkReal x1970=(cj5)*(cj5);
08733 IkReal x1971=(sj6)*(sj6);
08734 IkReal x1972=(cj6)*(cj6);
08735 IkReal x1973=(r21)*(r21);
08736 IkReal x1974=((cj6)*(r20));
08737 IkReal x1975=((IkReal(2.00000000000000))*(r21)*(sj6));
08738 IkReal x1976=((cj5)*(r22)*(sj5));
08739 gconst23=IKsign(((((x1969)*(x1971)))+(((x1970)*(x1971)*(x1973)))+(((x1972)*(x1973)))+(((x1975)*(x1976)))+(((x1969)*(x1970)*(x1972)))+(((x1974)*(x1975)))+(((IkReal(-2.00000000000000))*(x1974)*(x1976)))+(((IkReal(-1.00000000000000))*(x1970)*(x1974)*(x1975)))+((((r22)*(r22))*((sj5)*(sj5))))));
08740 IkReal x1977=(r20)*(r20);
08741 IkReal x1978=(cj5)*(cj5);
08742 IkReal x1979=(sj6)*(sj6);
08743 IkReal x1980=(cj6)*(cj6);
08744 IkReal x1981=(r21)*(r21);
08745 IkReal x1982=((cj6)*(r20));
08746 IkReal x1983=((IkReal(2.00000000000000))*(r21)*(sj6));
08747 IkReal x1984=((cj5)*(r22)*(sj5));
08748 dummyeval[0]=((((x1980)*(x1981)))+(((IkReal(-1.00000000000000))*(x1978)*(x1982)*(x1983)))+(((x1982)*(x1983)))+(((IkReal(-2.00000000000000))*(x1982)*(x1984)))+(((x1977)*(x1979)))+(((x1977)*(x1978)*(x1980)))+((((r22)*(r22))*((sj5)*(sj5))))+(((x1983)*(x1984)))+(((x1978)*(x1979)*(x1981))));
08749 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08750 {
08751 {
08752 IkReal dummyeval[1];
08753 IkReal gconst24;
08754 IkReal x1985=(cj5)*(cj5);
08755 IkReal x1986=(sj6)*(sj6);
08756 IkReal x1987=(cj6)*(cj6);
08757 IkReal x1988=((r00)*(r20));
08758 IkReal x1989=((cj5)*(sj5));
08759 IkReal x1990=((r01)*(r21));
08760 IkReal x1991=((r21)*(sj6));
08761 IkReal x1992=((sj6)*(x1989));
08762 IkReal x1993=((IkReal(1.00000000000000))*(cj6)*(r00));
08763 IkReal x1994=((cj6)*(r01)*(r20)*(sj6));
08764 gconst24=IKsign(((((r02)*(x1989)*(x1991)))+(((IkReal(-1.00000000000000))*(r22)*(x1989)*(x1993)))+(((cj6)*(r00)*(x1991)))+(((IkReal(-1.00000000000000))*(x1985)*(x1994)))+(((x1985)*(x1986)*(x1990)))+(((r01)*(r22)*(x1992)))+(x1994)+(((r02)*(r22)*((sj5)*(sj5))))+(((x1987)*(x1990)))+(((x1985)*(x1987)*(x1988)))+(((x1986)*(x1988)))+(((IkReal(-1.00000000000000))*(x1985)*(x1991)*(x1993)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1989)))));
08765 IkReal x1995=(cj5)*(cj5);
08766 IkReal x1996=(sj6)*(sj6);
08767 IkReal x1997=(cj6)*(cj6);
08768 IkReal x1998=((r00)*(r20));
08769 IkReal x1999=((cj5)*(sj5));
08770 IkReal x2000=((r01)*(r21));
08771 IkReal x2001=((r21)*(sj6));
08772 IkReal x2002=((sj6)*(x1999));
08773 IkReal x2003=((IkReal(1.00000000000000))*(cj6)*(r00));
08774 IkReal x2004=x1994;
08775 dummyeval[0]=((((x1997)*(x2000)))+(((x1995)*(x1997)*(x1998)))+(((x1996)*(x1998)))+(((r01)*(r22)*(x2002)))+(((x1995)*(x1996)*(x2000)))+(x2004)+(((IkReal(-1.00000000000000))*(r22)*(x1999)*(x2003)))+(((IkReal(-1.00000000000000))*(x1995)*(x2001)*(x2003)))+(((IkReal(-1.00000000000000))*(x1995)*(x2004)))+(((r02)*(x1999)*(x2001)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x1999)))+(((cj6)*(r00)*(x2001)))+(((r02)*(r22)*((sj5)*(sj5)))));
08776 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
08777 {
08778 continue;
08779 
08780 } else
08781 {
08782 {
08783 IkReal j4array[1], cj4array[1], sj4array[1];
08784 bool j4valid[1]={false};
08785 _nj4 = 1;
08786 IkReal x2005=((cj1)*(r20));
08787 IkReal x2006=((cj5)*(cj6));
08788 IkReal x2007=((IkReal(1.00000000000000))*(cj1));
08789 IkReal x2008=((IkReal(1.00000000000000))*(cj3));
08790 IkReal x2009=((sj6)*(x2008));
08791 if( IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2007)))+(((IkReal(-1.00000000000000))*(r00)*(x2009)))+(((x2005)*(x2006)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2007)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2008))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(x2009)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x2008)))+(((cj1)*(cj6)*(r21)))+(((cj3)*(r00)*(x2006)))+(((sj6)*(x2005))))))) < IKFAST_ATAN2_MAGTHRESH )
08792     continue;
08793 j4array[0]=IKatan2(((gconst24)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2007)))+(((IkReal(-1.00000000000000))*(r00)*(x2009)))+(((x2005)*(x2006)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2007)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2008)))))), ((gconst24)*(((((IkReal(-1.00000000000000))*(cj5)*(r01)*(x2009)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x2008)))+(((cj1)*(cj6)*(r21)))+(((cj3)*(r00)*(x2006)))+(((sj6)*(x2005)))))));
08794 sj4array[0]=IKsin(j4array[0]);
08795 cj4array[0]=IKcos(j4array[0]);
08796 if( j4array[0] > IKPI )
08797 {
08798     j4array[0]-=IK2PI;
08799 }
08800 else if( j4array[0] < -IKPI )
08801 {    j4array[0]+=IK2PI;
08802 }
08803 j4valid[0] = true;
08804 for(int ij4 = 0; ij4 < 1; ++ij4)
08805 {
08806 if( !j4valid[ij4] )
08807 {
08808     continue;
08809 }
08810 _ij4[0] = ij4; _ij4[1] = -1;
08811 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08812 {
08813 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08814 {
08815     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08816 }
08817 }
08818 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08819 {
08820 IkReal evalcond[6];
08821 IkReal x2010=IKsin(j4);
08822 IkReal x2011=IKcos(j4);
08823 IkReal x2012=((IkReal(1.00000000000000))*(r00));
08824 IkReal x2013=((cj5)*(r01));
08825 IkReal x2014=((cj5)*(r11));
08826 IkReal x2015=((IkReal(1.00000000000000))*(cj5));
08827 IkReal x2016=((cj5)*(r21));
08828 IkReal x2017=((sj5)*(x2010));
08829 IkReal x2018=((cj6)*(x2010));
08830 IkReal x2019=((sj6)*(x2011));
08831 IkReal x2020=((sj6)*(x2010));
08832 IkReal x2021=((sj5)*(x2011));
08833 IkReal x2022=((cj6)*(x2011));
08834 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x2019)))+(((IkReal(-1.00000000000000))*(r21)*(x2022)))+(((r22)*(x2017)))+(((x2016)*(x2020)))+(((IkReal(-1.00000000000000))*(r20)*(x2015)*(x2018))));
08835 evalcond[1]=((cj3)+(((r22)*(x2021)))+(((x2016)*(x2019)))+(((IkReal(-1.00000000000000))*(r20)*(x2015)*(x2022)))+(((r21)*(x2018)))+(((r20)*(x2020))));
08836 evalcond[2]=((cj1)+(((IkReal(-1.00000000000000))*(x2012)*(x2019)))+(((x2013)*(x2020)))+(((IkReal(-1.00000000000000))*(cj5)*(x2012)*(x2018)))+(((r02)*(x2017)))+(((IkReal(-1.00000000000000))*(r01)*(x2022))));
08837 evalcond[3]=((sj1)+(((IkReal(-1.00000000000000))*(r10)*(x2015)*(x2018)))+(((IkReal(-1.00000000000000))*(r10)*(x2019)))+(((r12)*(x2017)))+(((x2014)*(x2020)))+(((IkReal(-1.00000000000000))*(r11)*(x2022))));
08838 evalcond[4]=((((r00)*(x2020)))+(((IkReal(-1.00000000000000))*(cj5)*(x2012)*(x2022)))+(((sj1)*(sj3)))+(((r01)*(x2018)))+(((r02)*(x2021)))+(((x2013)*(x2019))));
08839 evalcond[5]=((((r10)*(x2020)))+(((x2014)*(x2019)))+(((IkReal(-1.00000000000000))*(r10)*(x2015)*(x2022)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((r11)*(x2018)))+(((r12)*(x2021))));
08840 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08841 {
08842 continue;
08843 }
08844 }
08845 
08846 {
08847 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08848 vinfos[0].jointtype = 1;
08849 vinfos[0].foffset = j0;
08850 vinfos[0].indices[0] = _ij0[0];
08851 vinfos[0].indices[1] = _ij0[1];
08852 vinfos[0].maxsolutions = _nj0;
08853 vinfos[1].jointtype = 1;
08854 vinfos[1].foffset = j1;
08855 vinfos[1].indices[0] = _ij1[0];
08856 vinfos[1].indices[1] = _ij1[1];
08857 vinfos[1].maxsolutions = _nj1;
08858 vinfos[2].jointtype = 1;
08859 vinfos[2].foffset = j2;
08860 vinfos[2].indices[0] = _ij2[0];
08861 vinfos[2].indices[1] = _ij2[1];
08862 vinfos[2].maxsolutions = _nj2;
08863 vinfos[3].jointtype = 1;
08864 vinfos[3].foffset = j3;
08865 vinfos[3].indices[0] = _ij3[0];
08866 vinfos[3].indices[1] = _ij3[1];
08867 vinfos[3].maxsolutions = _nj3;
08868 vinfos[4].jointtype = 1;
08869 vinfos[4].foffset = j4;
08870 vinfos[4].indices[0] = _ij4[0];
08871 vinfos[4].indices[1] = _ij4[1];
08872 vinfos[4].maxsolutions = _nj4;
08873 vinfos[5].jointtype = 1;
08874 vinfos[5].foffset = j5;
08875 vinfos[5].indices[0] = _ij5[0];
08876 vinfos[5].indices[1] = _ij5[1];
08877 vinfos[5].maxsolutions = _nj5;
08878 vinfos[6].jointtype = 1;
08879 vinfos[6].foffset = j6;
08880 vinfos[6].indices[0] = _ij6[0];
08881 vinfos[6].indices[1] = _ij6[1];
08882 vinfos[6].maxsolutions = _nj6;
08883 std::vector<int> vfree(0);
08884 solutions.AddSolution(vinfos,vfree);
08885 }
08886 }
08887 }
08888 
08889 }
08890 
08891 }
08892 
08893 } else
08894 {
08895 {
08896 IkReal j4array[1], cj4array[1], sj4array[1];
08897 bool j4valid[1]={false};
08898 _nj4 = 1;
08899 IkReal x2023=((cj3)*(cj6));
08900 IkReal x2024=((IkReal(1.00000000000000))*(r21));
08901 IkReal x2025=((cj3)*(sj6));
08902 if( IKabs(((gconst23)*(((((IkReal(-1.00000000000000))*(x2023)*(x2024)))+(((IkReal(-1.00000000000000))*(r20)*(x2025))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst23)*(((((IkReal(-1.00000000000000))*(cj5)*(x2024)*(x2025)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x2023))))))) < IKFAST_ATAN2_MAGTHRESH )
08903     continue;
08904 j4array[0]=IKatan2(((gconst23)*(((((IkReal(-1.00000000000000))*(x2023)*(x2024)))+(((IkReal(-1.00000000000000))*(r20)*(x2025)))))), ((gconst23)*(((((IkReal(-1.00000000000000))*(cj5)*(x2024)*(x2025)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x2023)))))));
08905 sj4array[0]=IKsin(j4array[0]);
08906 cj4array[0]=IKcos(j4array[0]);
08907 if( j4array[0] > IKPI )
08908 {
08909     j4array[0]-=IK2PI;
08910 }
08911 else if( j4array[0] < -IKPI )
08912 {    j4array[0]+=IK2PI;
08913 }
08914 j4valid[0] = true;
08915 for(int ij4 = 0; ij4 < 1; ++ij4)
08916 {
08917 if( !j4valid[ij4] )
08918 {
08919     continue;
08920 }
08921 _ij4[0] = ij4; _ij4[1] = -1;
08922 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08923 {
08924 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08925 {
08926     j4valid[iij4]=false; _ij4[1] = iij4; break; 
08927 }
08928 }
08929 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08930 {
08931 IkReal evalcond[6];
08932 IkReal x2026=IKsin(j4);
08933 IkReal x2027=IKcos(j4);
08934 IkReal x2028=((IkReal(1.00000000000000))*(r00));
08935 IkReal x2029=((cj5)*(r01));
08936 IkReal x2030=((cj5)*(r11));
08937 IkReal x2031=((IkReal(1.00000000000000))*(cj5));
08938 IkReal x2032=((cj5)*(r21));
08939 IkReal x2033=((sj5)*(x2026));
08940 IkReal x2034=((cj6)*(x2026));
08941 IkReal x2035=((sj6)*(x2027));
08942 IkReal x2036=((sj6)*(x2026));
08943 IkReal x2037=((sj5)*(x2027));
08944 IkReal x2038=((cj6)*(x2027));
08945 evalcond[0]=((((r22)*(x2033)))+(((IkReal(-1.00000000000000))*(r20)*(x2035)))+(((x2032)*(x2036)))+(((IkReal(-1.00000000000000))*(r20)*(x2031)*(x2034)))+(((IkReal(-1.00000000000000))*(r21)*(x2038))));
08946 evalcond[1]=((((r22)*(x2037)))+(((r21)*(x2034)))+(cj3)+(((x2032)*(x2035)))+(((IkReal(-1.00000000000000))*(r20)*(x2031)*(x2038)))+(((r20)*(x2036))));
08947 evalcond[2]=((cj1)+(((IkReal(-1.00000000000000))*(cj5)*(x2028)*(x2034)))+(((r02)*(x2033)))+(((IkReal(-1.00000000000000))*(r01)*(x2038)))+(((x2029)*(x2036)))+(((IkReal(-1.00000000000000))*(x2028)*(x2035))));
08948 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x2035)))+(sj1)+(((r12)*(x2033)))+(((x2030)*(x2036)))+(((IkReal(-1.00000000000000))*(r11)*(x2038)))+(((IkReal(-1.00000000000000))*(r10)*(x2031)*(x2034))));
08949 evalcond[4]=((((r00)*(x2036)))+(((r01)*(x2034)))+(((IkReal(-1.00000000000000))*(cj5)*(x2028)*(x2038)))+(((sj1)*(sj3)))+(((r02)*(x2037)))+(((x2029)*(x2035))));
08950 evalcond[5]=((((r10)*(x2036)))+(((r12)*(x2037)))+(((x2030)*(x2035)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((IkReal(-1.00000000000000))*(r10)*(x2031)*(x2038)))+(((r11)*(x2034))));
08951 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
08952 {
08953 continue;
08954 }
08955 }
08956 
08957 {
08958 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
08959 vinfos[0].jointtype = 1;
08960 vinfos[0].foffset = j0;
08961 vinfos[0].indices[0] = _ij0[0];
08962 vinfos[0].indices[1] = _ij0[1];
08963 vinfos[0].maxsolutions = _nj0;
08964 vinfos[1].jointtype = 1;
08965 vinfos[1].foffset = j1;
08966 vinfos[1].indices[0] = _ij1[0];
08967 vinfos[1].indices[1] = _ij1[1];
08968 vinfos[1].maxsolutions = _nj1;
08969 vinfos[2].jointtype = 1;
08970 vinfos[2].foffset = j2;
08971 vinfos[2].indices[0] = _ij2[0];
08972 vinfos[2].indices[1] = _ij2[1];
08973 vinfos[2].maxsolutions = _nj2;
08974 vinfos[3].jointtype = 1;
08975 vinfos[3].foffset = j3;
08976 vinfos[3].indices[0] = _ij3[0];
08977 vinfos[3].indices[1] = _ij3[1];
08978 vinfos[3].maxsolutions = _nj3;
08979 vinfos[4].jointtype = 1;
08980 vinfos[4].foffset = j4;
08981 vinfos[4].indices[0] = _ij4[0];
08982 vinfos[4].indices[1] = _ij4[1];
08983 vinfos[4].maxsolutions = _nj4;
08984 vinfos[5].jointtype = 1;
08985 vinfos[5].foffset = j5;
08986 vinfos[5].indices[0] = _ij5[0];
08987 vinfos[5].indices[1] = _ij5[1];
08988 vinfos[5].maxsolutions = _nj5;
08989 vinfos[6].jointtype = 1;
08990 vinfos[6].foffset = j6;
08991 vinfos[6].indices[0] = _ij6[0];
08992 vinfos[6].indices[1] = _ij6[1];
08993 vinfos[6].maxsolutions = _nj6;
08994 std::vector<int> vfree(0);
08995 solutions.AddSolution(vinfos,vfree);
08996 }
08997 }
08998 }
08999 
09000 }
09001 
09002 }
09003 }
09004 }
09005 
09006 }
09007 
09008 }
09009 
09010 } else
09011 {
09012 {
09013 IkReal j4array[1], cj4array[1], sj4array[1];
09014 bool j4valid[1]={false};
09015 _nj4 = 1;
09016 IkReal x2039=((cj6)*(sj1));
09017 IkReal x2040=((IkReal(1.00000000000000))*(sj1)*(sj6));
09018 if( IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(r20)*(x2040)))+(((IkReal(-1.00000000000000))*(r21)*(x2039))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((cj5)*(r20)*(x2039)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2040)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
09019     continue;
09020 j4array[0]=IKatan2(((gconst22)*(((((IkReal(-1.00000000000000))*(r20)*(x2040)))+(((IkReal(-1.00000000000000))*(r21)*(x2039)))))), ((gconst22)*(((((cj5)*(r20)*(x2039)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2040)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))))));
09021 sj4array[0]=IKsin(j4array[0]);
09022 cj4array[0]=IKcos(j4array[0]);
09023 if( j4array[0] > IKPI )
09024 {
09025     j4array[0]-=IK2PI;
09026 }
09027 else if( j4array[0] < -IKPI )
09028 {    j4array[0]+=IK2PI;
09029 }
09030 j4valid[0] = true;
09031 for(int ij4 = 0; ij4 < 1; ++ij4)
09032 {
09033 if( !j4valid[ij4] )
09034 {
09035     continue;
09036 }
09037 _ij4[0] = ij4; _ij4[1] = -1;
09038 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09039 {
09040 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09041 {
09042     j4valid[iij4]=false; _ij4[1] = iij4; break; 
09043 }
09044 }
09045 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09046 {
09047 IkReal evalcond[3];
09048 IkReal x2041=IKsin(j4);
09049 IkReal x2042=IKcos(j4);
09050 IkReal x2043=((IkReal(1.00000000000000))*(cj6));
09051 IkReal x2044=((sj5)*(x2041));
09052 IkReal x2045=((cj5)*(x2041));
09053 IkReal x2046=((IkReal(1.00000000000000))*(sj6)*(x2042));
09054 evalcond[0]=((((r22)*(x2044)))+(((IkReal(-1.00000000000000))*(r21)*(x2042)*(x2043)))+(((r21)*(sj6)*(x2045)))+(((IkReal(-1.00000000000000))*(r20)*(x2046)))+(((IkReal(-1.00000000000000))*(r20)*(x2043)*(x2045))));
09055 evalcond[1]=((cj1)+(((r01)*(sj6)*(x2045)))+(((r02)*(x2044)))+(((IkReal(-1.00000000000000))*(r01)*(x2042)*(x2043)))+(((IkReal(-1.00000000000000))*(r00)*(x2046)))+(((IkReal(-1.00000000000000))*(r00)*(x2043)*(x2045))));
09056 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x2043)*(x2045)))+(((r11)*(sj6)*(x2045)))+(sj1)+(((IkReal(-1.00000000000000))*(r11)*(x2042)*(x2043)))+(((IkReal(-1.00000000000000))*(r10)*(x2046)))+(((r12)*(x2044))));
09057 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
09058 {
09059 continue;
09060 }
09061 }
09062 
09063 {
09064 IkReal j3array[1], cj3array[1], sj3array[1];
09065 bool j3valid[1]={false};
09066 _nj3 = 1;
09067 IkReal x2047=((cj4)*(cj5));
09068 IkReal x2048=((cj6)*(r20));
09069 IkReal x2049=((IkReal(1.00000000000000))*(sj4));
09070 IkReal x2050=((IkReal(1.00000000000000))*(r21)*(sj6));
09071 if( IKabs(((((sj5)*(x2048)))+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2050))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2049)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x2047)*(x2048)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2049)))+(((IkReal(-1.00000000000000))*(x2047)*(x2050))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x2048)))+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2050)))))+IKsqr(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2049)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x2047)*(x2048)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2049)))+(((IkReal(-1.00000000000000))*(x2047)*(x2050)))))-1) <= IKFAST_SINCOS_THRESH )
09072     continue;
09073 j3array[0]=IKatan2(((((sj5)*(x2048)))+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2050)))), ((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2049)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x2047)*(x2048)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2049)))+(((IkReal(-1.00000000000000))*(x2047)*(x2050)))));
09074 sj3array[0]=IKsin(j3array[0]);
09075 cj3array[0]=IKcos(j3array[0]);
09076 if( j3array[0] > IKPI )
09077 {
09078     j3array[0]-=IK2PI;
09079 }
09080 else if( j3array[0] < -IKPI )
09081 {    j3array[0]+=IK2PI;
09082 }
09083 j3valid[0] = true;
09084 for(int ij3 = 0; ij3 < 1; ++ij3)
09085 {
09086 if( !j3valid[ij3] )
09087 {
09088     continue;
09089 }
09090 _ij3[0] = ij3; _ij3[1] = -1;
09091 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
09092 {
09093 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
09094 {
09095     j3valid[iij3]=false; _ij3[1] = iij3; break; 
09096 }
09097 }
09098 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
09099 {
09100 IkReal evalcond[6];
09101 IkReal x2051=IKcos(j3);
09102 IkReal x2052=IKsin(j3);
09103 IkReal x2053=((r11)*(sj6));
09104 IkReal x2054=((IkReal(1.00000000000000))*(sj5));
09105 IkReal x2055=((IkReal(1.00000000000000))*(cj1));
09106 IkReal x2056=((sj4)*(sj6));
09107 IkReal x2057=((cj4)*(sj5));
09108 IkReal x2058=((cj6)*(sj4));
09109 IkReal x2059=((cj4)*(cj5));
09110 IkReal x2060=((r01)*(sj6));
09111 IkReal x2061=((r21)*(sj6));
09112 IkReal x2062=((cj6)*(sj5));
09113 IkReal x2063=((IkReal(1.00000000000000))*(cj6)*(x2059));
09114 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2052)))+(((r20)*(x2062)))+(((IkReal(-1.00000000000000))*(x2054)*(x2061))));
09115 evalcond[1]=((((sj1)*(x2051)))+(((IkReal(-1.00000000000000))*(x2054)*(x2060)))+(((r00)*(x2062)))+(((cj5)*(r02))));
09116 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2051)*(x2055)))+(((r10)*(x2062)))+(((IkReal(-1.00000000000000))*(x2053)*(x2054))));
09117 evalcond[3]=((((r22)*(x2057)))+(x2051)+(((x2059)*(x2061)))+(((IkReal(-1.00000000000000))*(r20)*(x2063)))+(((r20)*(x2056)))+(((r21)*(x2058))));
09118 evalcond[4]=((((r01)*(x2058)))+(((x2059)*(x2060)))+(((sj1)*(x2052)))+(((r02)*(x2057)))+(((IkReal(-1.00000000000000))*(r00)*(x2063)))+(((r00)*(x2056))));
09119 evalcond[5]=((((r10)*(x2056)))+(((r12)*(x2057)))+(((IkReal(-1.00000000000000))*(r10)*(x2063)))+(((x2053)*(x2059)))+(((r11)*(x2058)))+(((IkReal(-1.00000000000000))*(x2052)*(x2055))));
09120 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
09121 {
09122 continue;
09123 }
09124 }
09125 
09126 {
09127 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09128 vinfos[0].jointtype = 1;
09129 vinfos[0].foffset = j0;
09130 vinfos[0].indices[0] = _ij0[0];
09131 vinfos[0].indices[1] = _ij0[1];
09132 vinfos[0].maxsolutions = _nj0;
09133 vinfos[1].jointtype = 1;
09134 vinfos[1].foffset = j1;
09135 vinfos[1].indices[0] = _ij1[0];
09136 vinfos[1].indices[1] = _ij1[1];
09137 vinfos[1].maxsolutions = _nj1;
09138 vinfos[2].jointtype = 1;
09139 vinfos[2].foffset = j2;
09140 vinfos[2].indices[0] = _ij2[0];
09141 vinfos[2].indices[1] = _ij2[1];
09142 vinfos[2].maxsolutions = _nj2;
09143 vinfos[3].jointtype = 1;
09144 vinfos[3].foffset = j3;
09145 vinfos[3].indices[0] = _ij3[0];
09146 vinfos[3].indices[1] = _ij3[1];
09147 vinfos[3].maxsolutions = _nj3;
09148 vinfos[4].jointtype = 1;
09149 vinfos[4].foffset = j4;
09150 vinfos[4].indices[0] = _ij4[0];
09151 vinfos[4].indices[1] = _ij4[1];
09152 vinfos[4].maxsolutions = _nj4;
09153 vinfos[5].jointtype = 1;
09154 vinfos[5].foffset = j5;
09155 vinfos[5].indices[0] = _ij5[0];
09156 vinfos[5].indices[1] = _ij5[1];
09157 vinfos[5].maxsolutions = _nj5;
09158 vinfos[6].jointtype = 1;
09159 vinfos[6].foffset = j6;
09160 vinfos[6].indices[0] = _ij6[0];
09161 vinfos[6].indices[1] = _ij6[1];
09162 vinfos[6].maxsolutions = _nj6;
09163 std::vector<int> vfree(0);
09164 solutions.AddSolution(vinfos,vfree);
09165 }
09166 }
09167 }
09168 }
09169 }
09170 
09171 }
09172 
09173 }
09174 
09175 } else
09176 {
09177 {
09178 IkReal j4array[1], cj4array[1], sj4array[1];
09179 bool j4valid[1]={false};
09180 _nj4 = 1;
09181 IkReal x2064=((cj1)*(cj6));
09182 IkReal x2065=((IkReal(1.00000000000000))*(cj1));
09183 if( IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x2064)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2065))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2065)))+(((cj5)*(r20)*(x2064)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2065))))))) < IKFAST_ATAN2_MAGTHRESH )
09184     continue;
09185 j4array[0]=IKatan2(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x2064)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2065)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2065)))+(((cj5)*(r20)*(x2064)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2065)))))));
09186 sj4array[0]=IKsin(j4array[0]);
09187 cj4array[0]=IKcos(j4array[0]);
09188 if( j4array[0] > IKPI )
09189 {
09190     j4array[0]-=IK2PI;
09191 }
09192 else if( j4array[0] < -IKPI )
09193 {    j4array[0]+=IK2PI;
09194 }
09195 j4valid[0] = true;
09196 for(int ij4 = 0; ij4 < 1; ++ij4)
09197 {
09198 if( !j4valid[ij4] )
09199 {
09200     continue;
09201 }
09202 _ij4[0] = ij4; _ij4[1] = -1;
09203 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09204 {
09205 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09206 {
09207     j4valid[iij4]=false; _ij4[1] = iij4; break; 
09208 }
09209 }
09210 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09211 {
09212 IkReal evalcond[3];
09213 IkReal x2066=IKsin(j4);
09214 IkReal x2067=IKcos(j4);
09215 IkReal x2068=((IkReal(1.00000000000000))*(cj6));
09216 IkReal x2069=((sj5)*(x2066));
09217 IkReal x2070=((cj5)*(x2066));
09218 IkReal x2071=((IkReal(1.00000000000000))*(sj6)*(x2067));
09219 evalcond[0]=((((r22)*(x2069)))+(((r21)*(sj6)*(x2070)))+(((IkReal(-1.00000000000000))*(r21)*(x2067)*(x2068)))+(((IkReal(-1.00000000000000))*(r20)*(x2068)*(x2070)))+(((IkReal(-1.00000000000000))*(r20)*(x2071))));
09220 evalcond[1]=((cj1)+(((IkReal(-1.00000000000000))*(r00)*(x2068)*(x2070)))+(((IkReal(-1.00000000000000))*(r01)*(x2067)*(x2068)))+(((r01)*(sj6)*(x2070)))+(((r02)*(x2069)))+(((IkReal(-1.00000000000000))*(r00)*(x2071))));
09221 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x2071)))+(sj1)+(((r12)*(x2069)))+(((r11)*(sj6)*(x2070)))+(((IkReal(-1.00000000000000))*(r11)*(x2067)*(x2068)))+(((IkReal(-1.00000000000000))*(r10)*(x2068)*(x2070))));
09222 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
09223 {
09224 continue;
09225 }
09226 }
09227 
09228 {
09229 IkReal j3array[1], cj3array[1], sj3array[1];
09230 bool j3valid[1]={false};
09231 _nj3 = 1;
09232 IkReal x2072=((cj4)*(cj5));
09233 IkReal x2073=((cj6)*(r20));
09234 IkReal x2074=((IkReal(1.00000000000000))*(sj4));
09235 IkReal x2075=((IkReal(1.00000000000000))*(r21)*(sj6));
09236 if( IKabs(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2075)))+(((sj5)*(x2073))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2074)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2074)))+(((x2072)*(x2073)))+(((IkReal(-1.00000000000000))*(x2072)*(x2075))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2075)))+(((sj5)*(x2073)))))+IKsqr(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2074)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2074)))+(((x2072)*(x2073)))+(((IkReal(-1.00000000000000))*(x2072)*(x2075)))))-1) <= IKFAST_SINCOS_THRESH )
09237     continue;
09238 j3array[0]=IKatan2(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2075)))+(((sj5)*(x2073)))), ((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2074)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2074)))+(((x2072)*(x2073)))+(((IkReal(-1.00000000000000))*(x2072)*(x2075)))));
09239 sj3array[0]=IKsin(j3array[0]);
09240 cj3array[0]=IKcos(j3array[0]);
09241 if( j3array[0] > IKPI )
09242 {
09243     j3array[0]-=IK2PI;
09244 }
09245 else if( j3array[0] < -IKPI )
09246 {    j3array[0]+=IK2PI;
09247 }
09248 j3valid[0] = true;
09249 for(int ij3 = 0; ij3 < 1; ++ij3)
09250 {
09251 if( !j3valid[ij3] )
09252 {
09253     continue;
09254 }
09255 _ij3[0] = ij3; _ij3[1] = -1;
09256 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
09257 {
09258 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
09259 {
09260     j3valid[iij3]=false; _ij3[1] = iij3; break; 
09261 }
09262 }
09263 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
09264 {
09265 IkReal evalcond[6];
09266 IkReal x2076=IKcos(j3);
09267 IkReal x2077=IKsin(j3);
09268 IkReal x2078=((r11)*(sj6));
09269 IkReal x2079=((IkReal(1.00000000000000))*(sj5));
09270 IkReal x2080=((IkReal(1.00000000000000))*(cj1));
09271 IkReal x2081=((sj4)*(sj6));
09272 IkReal x2082=((cj4)*(sj5));
09273 IkReal x2083=((cj6)*(sj4));
09274 IkReal x2084=((cj4)*(cj5));
09275 IkReal x2085=((r01)*(sj6));
09276 IkReal x2086=((r21)*(sj6));
09277 IkReal x2087=((cj6)*(sj5));
09278 IkReal x2088=((IkReal(1.00000000000000))*(cj6)*(x2084));
09279 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x2087)))+(((IkReal(-1.00000000000000))*(x2077)))+(((IkReal(-1.00000000000000))*(x2079)*(x2086))));
09280 evalcond[1]=((((sj1)*(x2076)))+(((IkReal(-1.00000000000000))*(x2079)*(x2085)))+(((r00)*(x2087)))+(((cj5)*(r02))));
09281 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2076)*(x2080)))+(((r10)*(x2087)))+(((IkReal(-1.00000000000000))*(x2078)*(x2079))));
09282 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x2088)))+(((x2084)*(x2086)))+(x2076)+(((r20)*(x2081)))+(((r21)*(x2083)))+(((r22)*(x2082))));
09283 evalcond[4]=((((r02)*(x2082)))+(((x2084)*(x2085)))+(((sj1)*(x2077)))+(((r01)*(x2083)))+(((r00)*(x2081)))+(((IkReal(-1.00000000000000))*(r00)*(x2088))));
09284 evalcond[5]=((((x2078)*(x2084)))+(((IkReal(-1.00000000000000))*(x2077)*(x2080)))+(((r10)*(x2081)))+(((IkReal(-1.00000000000000))*(r10)*(x2088)))+(((r11)*(x2083)))+(((r12)*(x2082))));
09285 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
09286 {
09287 continue;
09288 }
09289 }
09290 
09291 {
09292 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09293 vinfos[0].jointtype = 1;
09294 vinfos[0].foffset = j0;
09295 vinfos[0].indices[0] = _ij0[0];
09296 vinfos[0].indices[1] = _ij0[1];
09297 vinfos[0].maxsolutions = _nj0;
09298 vinfos[1].jointtype = 1;
09299 vinfos[1].foffset = j1;
09300 vinfos[1].indices[0] = _ij1[0];
09301 vinfos[1].indices[1] = _ij1[1];
09302 vinfos[1].maxsolutions = _nj1;
09303 vinfos[2].jointtype = 1;
09304 vinfos[2].foffset = j2;
09305 vinfos[2].indices[0] = _ij2[0];
09306 vinfos[2].indices[1] = _ij2[1];
09307 vinfos[2].maxsolutions = _nj2;
09308 vinfos[3].jointtype = 1;
09309 vinfos[3].foffset = j3;
09310 vinfos[3].indices[0] = _ij3[0];
09311 vinfos[3].indices[1] = _ij3[1];
09312 vinfos[3].maxsolutions = _nj3;
09313 vinfos[4].jointtype = 1;
09314 vinfos[4].foffset = j4;
09315 vinfos[4].indices[0] = _ij4[0];
09316 vinfos[4].indices[1] = _ij4[1];
09317 vinfos[4].maxsolutions = _nj4;
09318 vinfos[5].jointtype = 1;
09319 vinfos[5].foffset = j5;
09320 vinfos[5].indices[0] = _ij5[0];
09321 vinfos[5].indices[1] = _ij5[1];
09322 vinfos[5].maxsolutions = _nj5;
09323 vinfos[6].jointtype = 1;
09324 vinfos[6].foffset = j6;
09325 vinfos[6].indices[0] = _ij6[0];
09326 vinfos[6].indices[1] = _ij6[1];
09327 vinfos[6].maxsolutions = _nj6;
09328 std::vector<int> vfree(0);
09329 solutions.AddSolution(vinfos,vfree);
09330 }
09331 }
09332 }
09333 }
09334 }
09335 
09336 }
09337 
09338 }
09339 
09340 } else
09341 {
09342 if( 1 )
09343 {
09344 continue;
09345 
09346 } else
09347 {
09348 }
09349 }
09350 }
09351 }
09352 
09353 } else
09354 {
09355 {
09356 IkReal j3array[1], cj3array[1], sj3array[1];
09357 bool j3valid[1]={false};
09358 _nj3 = 1;
09359 IkReal x2089=(sj1)*(sj1);
09360 IkReal x2090=(cj1)*(cj1);
09361 IkReal x2091=((cj5)*(r12));
09362 IkReal x2092=((cj6)*(sj5));
09363 IkReal x2093=((IkReal(1.00000000000000))*(sj1));
09364 IkReal x2094=((cj5)*(r02));
09365 IkReal x2095=((IkReal(1.00000000000000))*(cj1));
09366 IkReal x2096=((r01)*(sj5)*(sj6));
09367 IkReal x2097=((r11)*(sj5)*(sj6));
09368 if( IKabs(((((IKabs(((((cj2)*(x2090)))+(((cj2)*(x2089))))) != 0)?((IkReal)1/(((((cj2)*(x2090)))+(((cj2)*(x2089)))))):(IkReal)1.0e30))*(((((cj1)*(x2094)))+(((cj1)*(r00)*(x2092)))+(((sj1)*(x2091)))+(((IkReal(-1.00000000000000))*(x2095)*(x2096)))+(((IkReal(-1.00000000000000))*(x2093)*(x2097)))+(((r10)*(sj1)*(x2092))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x2090)+(x2089))) != 0)?((IkReal)1/(((x2090)+(x2089)))):(IkReal)1.0e30))*(((((cj1)*(x2091)))+(((sj1)*(x2096)))+(((IkReal(-1.00000000000000))*(x2095)*(x2097)))+(((IkReal(-1.00000000000000))*(x2093)*(x2094)))+(((cj1)*(r10)*(x2092)))+(((IkReal(-1.00000000000000))*(r00)*(x2092)*(x2093))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x2090)))+(((cj2)*(x2089))))) != 0)?((IkReal)1/(((((cj2)*(x2090)))+(((cj2)*(x2089)))))):(IkReal)1.0e30))*(((((cj1)*(x2094)))+(((cj1)*(r00)*(x2092)))+(((sj1)*(x2091)))+(((IkReal(-1.00000000000000))*(x2095)*(x2096)))+(((IkReal(-1.00000000000000))*(x2093)*(x2097)))+(((r10)*(sj1)*(x2092)))))))+IKsqr(((((IKabs(((x2090)+(x2089))) != 0)?((IkReal)1/(((x2090)+(x2089)))):(IkReal)1.0e30))*(((((cj1)*(x2091)))+(((sj1)*(x2096)))+(((IkReal(-1.00000000000000))*(x2095)*(x2097)))+(((IkReal(-1.00000000000000))*(x2093)*(x2094)))+(((cj1)*(r10)*(x2092)))+(((IkReal(-1.00000000000000))*(r00)*(x2092)*(x2093)))))))-1) <= IKFAST_SINCOS_THRESH )
09369     continue;
09370 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x2090)))+(((cj2)*(x2089))))) != 0)?((IkReal)1/(((((cj2)*(x2090)))+(((cj2)*(x2089)))))):(IkReal)1.0e30))*(((((cj1)*(x2094)))+(((cj1)*(r00)*(x2092)))+(((sj1)*(x2091)))+(((IkReal(-1.00000000000000))*(x2095)*(x2096)))+(((IkReal(-1.00000000000000))*(x2093)*(x2097)))+(((r10)*(sj1)*(x2092)))))), ((((IKabs(((x2090)+(x2089))) != 0)?((IkReal)1/(((x2090)+(x2089)))):(IkReal)1.0e30))*(((((cj1)*(x2091)))+(((sj1)*(x2096)))+(((IkReal(-1.00000000000000))*(x2095)*(x2097)))+(((IkReal(-1.00000000000000))*(x2093)*(x2094)))+(((cj1)*(r10)*(x2092)))+(((IkReal(-1.00000000000000))*(r00)*(x2092)*(x2093)))))));
09371 sj3array[0]=IKsin(j3array[0]);
09372 cj3array[0]=IKcos(j3array[0]);
09373 if( j3array[0] > IKPI )
09374 {
09375     j3array[0]-=IK2PI;
09376 }
09377 else if( j3array[0] < -IKPI )
09378 {    j3array[0]+=IK2PI;
09379 }
09380 j3valid[0] = true;
09381 for(int ij3 = 0; ij3 < 1; ++ij3)
09382 {
09383 if( !j3valid[ij3] )
09384 {
09385     continue;
09386 }
09387 _ij3[0] = ij3; _ij3[1] = -1;
09388 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
09389 {
09390 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
09391 {
09392     j3valid[iij3]=false; _ij3[1] = iij3; break; 
09393 }
09394 }
09395 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
09396 {
09397 IkReal evalcond[3];
09398 IkReal x2098=IKsin(j3);
09399 IkReal x2099=IKcos(j3);
09400 IkReal x2100=((IkReal(1.00000000000000))*(cj1));
09401 IkReal x2101=((cj6)*(sj5));
09402 IkReal x2102=((IkReal(1.00000000000000))*(sj5)*(sj6));
09403 IkReal x2103=((cj2)*(x2098));
09404 evalcond[0]=((((r20)*(x2101)))+(((cj5)*(r22)))+(((sj2)*(x2098)))+(((IkReal(-1.00000000000000))*(r21)*(x2102))));
09405 evalcond[1]=((((IkReal(-1.00000000000000))*(x2100)*(x2103)))+(((sj1)*(x2099)))+(((r00)*(x2101)))+(((IkReal(-1.00000000000000))*(r01)*(x2102)))+(((cj5)*(r02))));
09406 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x2103)))+(((IkReal(-1.00000000000000))*(r11)*(x2102)))+(((r10)*(x2101)))+(((IkReal(-1.00000000000000))*(x2099)*(x2100))));
09407 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
09408 {
09409 continue;
09410 }
09411 }
09412 
09413 {
09414 IkReal dummyeval[1];
09415 IkReal gconst2;
09416 IkReal x2104=(cj5)*(cj5);
09417 IkReal x2105=(r21)*(r21);
09418 IkReal x2106=(sj6)*(sj6);
09419 IkReal x2107=(cj6)*(cj6);
09420 IkReal x2108=(r20)*(r20);
09421 IkReal x2109=((cj6)*(r20));
09422 IkReal x2110=((IkReal(1.00000000000000))*(x2107));
09423 IkReal x2111=((IkReal(2.00000000000000))*(r21)*(sj6));
09424 IkReal x2112=((cj5)*(r22)*(sj5));
09425 IkReal x2113=((IkReal(1.00000000000000))*(x2106));
09426 gconst2=IKsign(((((x2104)*(x2109)*(x2111)))+(((IkReal(2.00000000000000))*(x2109)*(x2112)))+(((IkReal(-1.00000000000000))*(x2108)*(x2113)))+(((IkReal(-1.00000000000000))*(x2104)*(x2105)*(x2113)))+(((IkReal(-1.00000000000000))*(x2111)*(x2112)))+(((IkReal(-1.00000000000000))*(x2109)*(x2111)))+(((IkReal(-1.00000000000000))*(x2104)*(x2108)*(x2110)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x2105)*(x2110)))));
09427 IkReal x2114=(cj5)*(cj5);
09428 IkReal x2115=(r21)*(r21);
09429 IkReal x2116=(sj6)*(sj6);
09430 IkReal x2117=(cj6)*(cj6);
09431 IkReal x2118=(r20)*(r20);
09432 IkReal x2119=((cj6)*(r20));
09433 IkReal x2120=((IkReal(1.00000000000000))*(x2117));
09434 IkReal x2121=((IkReal(2.00000000000000))*(r21)*(sj6));
09435 IkReal x2122=((cj5)*(r22)*(sj5));
09436 IkReal x2123=((IkReal(1.00000000000000))*(x2116));
09437 dummyeval[0]=((((IkReal(-1.00000000000000))*(x2119)*(x2121)))+(((IkReal(-1.00000000000000))*(x2118)*(x2123)))+(((IkReal(-1.00000000000000))*(x2114)*(x2115)*(x2123)))+(((IkReal(2.00000000000000))*(x2119)*(x2122)))+(((IkReal(-1.00000000000000))*(x2121)*(x2122)))+(((IkReal(-1.00000000000000))*(x2115)*(x2120)))+(((IkReal(-1.00000000000000))*(x2114)*(x2118)*(x2120)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((x2114)*(x2119)*(x2121))));
09438 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
09439 {
09440 {
09441 IkReal dummyeval[1];
09442 IkReal gconst3;
09443 IkReal x2124=(cj5)*(cj5);
09444 IkReal x2125=(sj6)*(sj6);
09445 IkReal x2126=(cj6)*(cj6);
09446 IkReal x2127=((r00)*(r20));
09447 IkReal x2128=((cj5)*(sj5));
09448 IkReal x2129=((r01)*(r21));
09449 IkReal x2130=((r21)*(sj6));
09450 IkReal x2131=((sj6)*(x2128));
09451 IkReal x2132=((IkReal(1.00000000000000))*(cj6)*(r00));
09452 IkReal x2133=((cj6)*(r01)*(r20)*(sj6));
09453 gconst3=IKsign(((((x2125)*(x2127)))+(((cj6)*(r00)*(x2130)))+(((r01)*(r22)*(x2131)))+(((r02)*(x2128)*(x2130)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x2128)))+(((IkReal(-1.00000000000000))*(r22)*(x2128)*(x2132)))+(x2133)+(((x2124)*(x2126)*(x2127)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x2124)*(x2130)*(x2132)))+(((IkReal(-1.00000000000000))*(x2124)*(x2133)))+(((x2126)*(x2129)))+(((x2124)*(x2125)*(x2129)))));
09454 IkReal x2134=(cj5)*(cj5);
09455 IkReal x2135=(sj6)*(sj6);
09456 IkReal x2136=(cj6)*(cj6);
09457 IkReal x2137=((r00)*(r20));
09458 IkReal x2138=((cj5)*(sj5));
09459 IkReal x2139=((r01)*(r21));
09460 IkReal x2140=((r21)*(sj6));
09461 IkReal x2141=((sj6)*(x2138));
09462 IkReal x2142=((IkReal(1.00000000000000))*(cj6)*(r00));
09463 IkReal x2143=x2133;
09464 dummyeval[0]=((((x2134)*(x2135)*(x2139)))+(((x2136)*(x2139)))+(((r02)*(x2138)*(x2140)))+(((IkReal(-1.00000000000000))*(x2134)*(x2143)))+(x2143)+(((cj6)*(r00)*(x2140)))+(((IkReal(-1.00000000000000))*(x2134)*(x2140)*(x2142)))+(((x2135)*(x2137)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x2138)))+(((r02)*(r22)*((sj5)*(sj5))))+(((r01)*(r22)*(x2141)))+(((IkReal(-1.00000000000000))*(r22)*(x2138)*(x2142)))+(((x2134)*(x2136)*(x2137))));
09465 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
09466 {
09467 continue;
09468 
09469 } else
09470 {
09471 {
09472 IkReal j4array[1], cj4array[1], sj4array[1];
09473 bool j4valid[1]={false};
09474 _nj4 = 1;
09475 IkReal x2144=((sj2)*(sj5));
09476 IkReal x2145=((cj5)*(sj6));
09477 IkReal x2146=((sj2)*(sj6));
09478 IkReal x2147=((cj3)*(r00));
09479 IkReal x2148=((cj3)*(r01)*(sj2));
09480 IkReal x2149=((IkReal(1.00000000000000))*(cj1)*(cj6)*(sj2));
09481 if( IKabs(((gconst3)*(((((x2146)*(x2147)))+(((cj6)*(x2148)))+(((cj1)*(r22)*(x2144)))+(((cj1)*(r21)*(sj2)*(x2145)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x2149))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(sj2)*(x2147)))+(((x2145)*(x2148)))+(((IkReal(-1.00000000000000))*(cj1)*(r20)*(x2146)))+(((cj3)*(r02)*(x2144)))+(((IkReal(-1.00000000000000))*(r21)*(x2149))))))) < IKFAST_ATAN2_MAGTHRESH )
09482     continue;
09483 j4array[0]=IKatan2(((gconst3)*(((((x2146)*(x2147)))+(((cj6)*(x2148)))+(((cj1)*(r22)*(x2144)))+(((cj1)*(r21)*(sj2)*(x2145)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x2149)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(sj2)*(x2147)))+(((x2145)*(x2148)))+(((IkReal(-1.00000000000000))*(cj1)*(r20)*(x2146)))+(((cj3)*(r02)*(x2144)))+(((IkReal(-1.00000000000000))*(r21)*(x2149)))))));
09484 sj4array[0]=IKsin(j4array[0]);
09485 cj4array[0]=IKcos(j4array[0]);
09486 if( j4array[0] > IKPI )
09487 {
09488     j4array[0]-=IK2PI;
09489 }
09490 else if( j4array[0] < -IKPI )
09491 {    j4array[0]+=IK2PI;
09492 }
09493 j4valid[0] = true;
09494 for(int ij4 = 0; ij4 < 1; ++ij4)
09495 {
09496 if( !j4valid[ij4] )
09497 {
09498     continue;
09499 }
09500 _ij4[0] = ij4; _ij4[1] = -1;
09501 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09502 {
09503 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09504 {
09505     j4valid[iij4]=false; _ij4[1] = iij4; break; 
09506 }
09507 }
09508 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09509 {
09510 IkReal evalcond[6];
09511 IkReal x2150=IKsin(j4);
09512 IkReal x2151=IKcos(j4);
09513 IkReal x2152=((IkReal(1.00000000000000))*(r00));
09514 IkReal x2153=((cj5)*(r01));
09515 IkReal x2154=((cj5)*(r11));
09516 IkReal x2155=((cj2)*(cj3));
09517 IkReal x2156=((IkReal(1.00000000000000))*(sj2));
09518 IkReal x2157=((IkReal(1.00000000000000))*(cj5));
09519 IkReal x2158=((cj5)*(r21));
09520 IkReal x2159=((sj5)*(x2150));
09521 IkReal x2160=((cj6)*(x2150));
09522 IkReal x2161=((sj6)*(x2151));
09523 IkReal x2162=((sj6)*(x2150));
09524 IkReal x2163=((sj5)*(x2151));
09525 IkReal x2164=((cj6)*(x2151));
09526 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x2164)))+(((IkReal(-1.00000000000000))*(r20)*(x2157)*(x2160)))+(((r22)*(x2159)))+(((x2158)*(x2162)))+(((IkReal(-1.00000000000000))*(r20)*(x2161)))+(((IkReal(-1.00000000000000))*(cj2))));
09527 evalcond[1]=((((r21)*(x2160)))+(((r20)*(x2162)))+(((IkReal(-1.00000000000000))*(r20)*(x2157)*(x2164)))+(((r22)*(x2163)))+(((x2158)*(x2161)))+(((IkReal(-1.00000000000000))*(cj3)*(x2156))));
09528 evalcond[2]=((((IkReal(-1.00000000000000))*(x2152)*(x2161)))+(((r02)*(x2159)))+(((x2153)*(x2162)))+(((IkReal(-1.00000000000000))*(cj1)*(x2156)))+(((IkReal(-1.00000000000000))*(cj5)*(x2152)*(x2160)))+(((IkReal(-1.00000000000000))*(r01)*(x2164))));
09529 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x2164)))+(((IkReal(-1.00000000000000))*(sj1)*(x2156)))+(((x2154)*(x2162)))+(((IkReal(-1.00000000000000))*(r10)*(x2161)))+(((r12)*(x2159)))+(((IkReal(-1.00000000000000))*(r10)*(x2157)*(x2160))));
09530 evalcond[4]=((((r00)*(x2162)))+(((r01)*(x2160)))+(((sj1)*(sj3)))+(((r02)*(x2163)))+(((x2153)*(x2161)))+(((IkReal(-1.00000000000000))*(cj5)*(x2152)*(x2164)))+(((cj1)*(x2155))));
09531 evalcond[5]=((((r12)*(x2163)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((sj1)*(x2155)))+(((x2154)*(x2161)))+(((r10)*(x2162)))+(((r11)*(x2160)))+(((IkReal(-1.00000000000000))*(r10)*(x2157)*(x2164))));
09532 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
09533 {
09534 continue;
09535 }
09536 }
09537 
09538 {
09539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09540 vinfos[0].jointtype = 1;
09541 vinfos[0].foffset = j0;
09542 vinfos[0].indices[0] = _ij0[0];
09543 vinfos[0].indices[1] = _ij0[1];
09544 vinfos[0].maxsolutions = _nj0;
09545 vinfos[1].jointtype = 1;
09546 vinfos[1].foffset = j1;
09547 vinfos[1].indices[0] = _ij1[0];
09548 vinfos[1].indices[1] = _ij1[1];
09549 vinfos[1].maxsolutions = _nj1;
09550 vinfos[2].jointtype = 1;
09551 vinfos[2].foffset = j2;
09552 vinfos[2].indices[0] = _ij2[0];
09553 vinfos[2].indices[1] = _ij2[1];
09554 vinfos[2].maxsolutions = _nj2;
09555 vinfos[3].jointtype = 1;
09556 vinfos[3].foffset = j3;
09557 vinfos[3].indices[0] = _ij3[0];
09558 vinfos[3].indices[1] = _ij3[1];
09559 vinfos[3].maxsolutions = _nj3;
09560 vinfos[4].jointtype = 1;
09561 vinfos[4].foffset = j4;
09562 vinfos[4].indices[0] = _ij4[0];
09563 vinfos[4].indices[1] = _ij4[1];
09564 vinfos[4].maxsolutions = _nj4;
09565 vinfos[5].jointtype = 1;
09566 vinfos[5].foffset = j5;
09567 vinfos[5].indices[0] = _ij5[0];
09568 vinfos[5].indices[1] = _ij5[1];
09569 vinfos[5].maxsolutions = _nj5;
09570 vinfos[6].jointtype = 1;
09571 vinfos[6].foffset = j6;
09572 vinfos[6].indices[0] = _ij6[0];
09573 vinfos[6].indices[1] = _ij6[1];
09574 vinfos[6].maxsolutions = _nj6;
09575 std::vector<int> vfree(0);
09576 solutions.AddSolution(vinfos,vfree);
09577 }
09578 }
09579 }
09580 
09581 }
09582 
09583 }
09584 
09585 } else
09586 {
09587 {
09588 IkReal j4array[1], cj4array[1], sj4array[1];
09589 bool j4valid[1]={false};
09590 _nj4 = 1;
09591 IkReal x2165=((r22)*(sj5));
09592 IkReal x2166=((cj2)*(cj5));
09593 IkReal x2167=((r21)*(sj6));
09594 IkReal x2168=((cj6)*(r20));
09595 IkReal x2169=((cj6)*(r21));
09596 IkReal x2170=((r20)*(sj6));
09597 IkReal x2171=((IkReal(1.00000000000000))*(cj3)*(sj2));
09598 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x2170)*(x2171)))+(((IkReal(-1.00000000000000))*(x2169)*(x2171)))+(((IkReal(-1.00000000000000))*(cj2)*(x2165)))+(((x2166)*(x2168)))+(((IkReal(-1.00000000000000))*(x2166)*(x2167))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj2)*(x2170)))+(((IkReal(-1.00000000000000))*(cj5)*(x2167)*(x2171)))+(((cj3)*(cj5)*(sj2)*(x2168)))+(((IkReal(-1.00000000000000))*(x2165)*(x2171)))+(((cj2)*(x2169))))))) < IKFAST_ATAN2_MAGTHRESH )
09599     continue;
09600 j4array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(x2170)*(x2171)))+(((IkReal(-1.00000000000000))*(x2169)*(x2171)))+(((IkReal(-1.00000000000000))*(cj2)*(x2165)))+(((x2166)*(x2168)))+(((IkReal(-1.00000000000000))*(x2166)*(x2167)))))), ((gconst2)*(((((cj2)*(x2170)))+(((IkReal(-1.00000000000000))*(cj5)*(x2167)*(x2171)))+(((cj3)*(cj5)*(sj2)*(x2168)))+(((IkReal(-1.00000000000000))*(x2165)*(x2171)))+(((cj2)*(x2169)))))));
09601 sj4array[0]=IKsin(j4array[0]);
09602 cj4array[0]=IKcos(j4array[0]);
09603 if( j4array[0] > IKPI )
09604 {
09605     j4array[0]-=IK2PI;
09606 }
09607 else if( j4array[0] < -IKPI )
09608 {    j4array[0]+=IK2PI;
09609 }
09610 j4valid[0] = true;
09611 for(int ij4 = 0; ij4 < 1; ++ij4)
09612 {
09613 if( !j4valid[ij4] )
09614 {
09615     continue;
09616 }
09617 _ij4[0] = ij4; _ij4[1] = -1;
09618 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09619 {
09620 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09621 {
09622     j4valid[iij4]=false; _ij4[1] = iij4; break; 
09623 }
09624 }
09625 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09626 {
09627 IkReal evalcond[6];
09628 IkReal x2172=IKsin(j4);
09629 IkReal x2173=IKcos(j4);
09630 IkReal x2174=((IkReal(1.00000000000000))*(r00));
09631 IkReal x2175=((cj5)*(r01));
09632 IkReal x2176=((cj5)*(r11));
09633 IkReal x2177=((cj2)*(cj3));
09634 IkReal x2178=((IkReal(1.00000000000000))*(sj2));
09635 IkReal x2179=((IkReal(1.00000000000000))*(cj5));
09636 IkReal x2180=((cj5)*(r21));
09637 IkReal x2181=((sj5)*(x2172));
09638 IkReal x2182=((cj6)*(x2172));
09639 IkReal x2183=((sj6)*(x2173));
09640 IkReal x2184=((sj6)*(x2172));
09641 IkReal x2185=((sj5)*(x2173));
09642 IkReal x2186=((cj6)*(x2173));
09643 evalcond[0]=((((r22)*(x2181)))+(((IkReal(-1.00000000000000))*(r20)*(x2183)))+(((x2180)*(x2184)))+(((IkReal(-1.00000000000000))*(r20)*(x2179)*(x2182)))+(((IkReal(-1.00000000000000))*(r21)*(x2186)))+(((IkReal(-1.00000000000000))*(cj2))));
09644 evalcond[1]=((((r22)*(x2185)))+(((IkReal(-1.00000000000000))*(cj3)*(x2178)))+(((x2180)*(x2183)))+(((r20)*(x2184)))+(((r21)*(x2182)))+(((IkReal(-1.00000000000000))*(r20)*(x2179)*(x2186))));
09645 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x2186)))+(((IkReal(-1.00000000000000))*(cj5)*(x2174)*(x2182)))+(((r02)*(x2181)))+(((x2175)*(x2184)))+(((IkReal(-1.00000000000000))*(cj1)*(x2178)))+(((IkReal(-1.00000000000000))*(x2174)*(x2183))));
09646 evalcond[3]=((((IkReal(-1.00000000000000))*(sj1)*(x2178)))+(((IkReal(-1.00000000000000))*(r10)*(x2179)*(x2182)))+(((IkReal(-1.00000000000000))*(r10)*(x2183)))+(((r12)*(x2181)))+(((x2176)*(x2184)))+(((IkReal(-1.00000000000000))*(r11)*(x2186))));
09647 evalcond[4]=((((sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x2174)*(x2186)))+(((r02)*(x2185)))+(((x2175)*(x2183)))+(((cj1)*(x2177)))+(((r01)*(x2182)))+(((r00)*(x2184))));
09648 evalcond[5]=((((sj1)*(x2177)))+(((IkReal(-1.00000000000000))*(r10)*(x2179)*(x2186)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((r12)*(x2185)))+(((x2176)*(x2183)))+(((r11)*(x2182)))+(((r10)*(x2184))));
09649 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
09650 {
09651 continue;
09652 }
09653 }
09654 
09655 {
09656 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09657 vinfos[0].jointtype = 1;
09658 vinfos[0].foffset = j0;
09659 vinfos[0].indices[0] = _ij0[0];
09660 vinfos[0].indices[1] = _ij0[1];
09661 vinfos[0].maxsolutions = _nj0;
09662 vinfos[1].jointtype = 1;
09663 vinfos[1].foffset = j1;
09664 vinfos[1].indices[0] = _ij1[0];
09665 vinfos[1].indices[1] = _ij1[1];
09666 vinfos[1].maxsolutions = _nj1;
09667 vinfos[2].jointtype = 1;
09668 vinfos[2].foffset = j2;
09669 vinfos[2].indices[0] = _ij2[0];
09670 vinfos[2].indices[1] = _ij2[1];
09671 vinfos[2].maxsolutions = _nj2;
09672 vinfos[3].jointtype = 1;
09673 vinfos[3].foffset = j3;
09674 vinfos[3].indices[0] = _ij3[0];
09675 vinfos[3].indices[1] = _ij3[1];
09676 vinfos[3].maxsolutions = _nj3;
09677 vinfos[4].jointtype = 1;
09678 vinfos[4].foffset = j4;
09679 vinfos[4].indices[0] = _ij4[0];
09680 vinfos[4].indices[1] = _ij4[1];
09681 vinfos[4].maxsolutions = _nj4;
09682 vinfos[5].jointtype = 1;
09683 vinfos[5].foffset = j5;
09684 vinfos[5].indices[0] = _ij5[0];
09685 vinfos[5].indices[1] = _ij5[1];
09686 vinfos[5].maxsolutions = _nj5;
09687 vinfos[6].jointtype = 1;
09688 vinfos[6].foffset = j6;
09689 vinfos[6].indices[0] = _ij6[0];
09690 vinfos[6].indices[1] = _ij6[1];
09691 vinfos[6].maxsolutions = _nj6;
09692 std::vector<int> vfree(0);
09693 solutions.AddSolution(vinfos,vfree);
09694 }
09695 }
09696 }
09697 
09698 }
09699 
09700 }
09701 }
09702 }
09703 
09704 }
09705 
09706 }
09707 
09708 } else
09709 {
09710 {
09711 IkReal j4array[1], cj4array[1], sj4array[1];
09712 bool j4valid[1]={false};
09713 _nj4 = 1;
09714 IkReal x2187=((cj2)*(cj6));
09715 IkReal x2188=((cj5)*(sj6));
09716 IkReal x2189=((IkReal(1.00000000000000))*(sj1)*(sj2));
09717 IkReal x2190=((r21)*(x2189));
09718 if( IKabs(((gconst1)*(((((cj2)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2189)))+(((IkReal(-1.00000000000000))*(cj6)*(x2190)))+(((r11)*(x2187))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(cj5)*(r10)*(x2187)))+(((IkReal(-1.00000000000000))*(x2188)*(x2190)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2189)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj2)))+(((cj2)*(r11)*(x2188)))+(((cj2)*(r12)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
09719     continue;
09720 j4array[0]=IKatan2(((gconst1)*(((((cj2)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2189)))+(((IkReal(-1.00000000000000))*(cj6)*(x2190)))+(((r11)*(x2187)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(cj5)*(r10)*(x2187)))+(((IkReal(-1.00000000000000))*(x2188)*(x2190)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2189)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj2)))+(((cj2)*(r11)*(x2188)))+(((cj2)*(r12)*(sj5)))))));
09721 sj4array[0]=IKsin(j4array[0]);
09722 cj4array[0]=IKcos(j4array[0]);
09723 if( j4array[0] > IKPI )
09724 {
09725     j4array[0]-=IK2PI;
09726 }
09727 else if( j4array[0] < -IKPI )
09728 {    j4array[0]+=IK2PI;
09729 }
09730 j4valid[0] = true;
09731 for(int ij4 = 0; ij4 < 1; ++ij4)
09732 {
09733 if( !j4valid[ij4] )
09734 {
09735     continue;
09736 }
09737 _ij4[0] = ij4; _ij4[1] = -1;
09738 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09739 {
09740 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09741 {
09742     j4valid[iij4]=false; _ij4[1] = iij4; break; 
09743 }
09744 }
09745 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09746 {
09747 IkReal evalcond[3];
09748 IkReal x2191=IKsin(j4);
09749 IkReal x2192=IKcos(j4);
09750 IkReal x2193=((IkReal(1.00000000000000))*(sj2));
09751 IkReal x2194=((IkReal(1.00000000000000))*(cj6));
09752 IkReal x2195=((sj5)*(x2191));
09753 IkReal x2196=((cj5)*(x2191));
09754 IkReal x2197=((IkReal(1.00000000000000))*(sj6)*(x2192));
09755 evalcond[0]=((((r22)*(x2195)))+(((IkReal(-1.00000000000000))*(r20)*(x2197)))+(((r21)*(sj6)*(x2196)))+(((IkReal(-1.00000000000000))*(r20)*(x2194)*(x2196)))+(((IkReal(-1.00000000000000))*(r21)*(x2192)*(x2194)))+(((IkReal(-1.00000000000000))*(cj2))));
09756 evalcond[1]=((((r01)*(sj6)*(x2196)))+(((IkReal(-1.00000000000000))*(r00)*(x2194)*(x2196)))+(((IkReal(-1.00000000000000))*(r01)*(x2192)*(x2194)))+(((r02)*(x2195)))+(((IkReal(-1.00000000000000))*(cj1)*(x2193)))+(((IkReal(-1.00000000000000))*(r00)*(x2197))));
09757 evalcond[2]=((((r11)*(sj6)*(x2196)))+(((IkReal(-1.00000000000000))*(r10)*(x2197)))+(((IkReal(-1.00000000000000))*(sj1)*(x2193)))+(((IkReal(-1.00000000000000))*(r10)*(x2194)*(x2196)))+(((r12)*(x2195)))+(((IkReal(-1.00000000000000))*(r11)*(x2192)*(x2194))));
09758 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
09759 {
09760 continue;
09761 }
09762 }
09763 
09764 {
09765 IkReal dummyeval[1];
09766 IkReal gconst4;
09767 gconst4=IKsign(sj2);
09768 dummyeval[0]=sj2;
09769 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
09770 {
09771 {
09772 IkReal dummyeval[2];
09773 IkReal x2198=(sj1)*(sj1);
09774 IkReal x2199=(cj1)*(cj1);
09775 dummyeval[0]=((((cj2)*(x2198)))+(((cj2)*(x2199))));
09776 dummyeval[1]=((x2198)+(x2199));
09777 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
09778 {
09779 {
09780 IkReal dummyeval[2];
09781 dummyeval[0]=sj2;
09782 dummyeval[1]=cj1;
09783 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
09784 {
09785 {
09786 IkReal evalcond[9];
09787 IkReal x2200=((sj4)*(sj5));
09788 IkReal x2201=((cj6)*(r20));
09789 IkReal x2202=((IkReal(1.00000000000000))*(cj4));
09790 IkReal x2203=((IkReal(0.390000000000000))*(sj5));
09791 IkReal x2204=((r21)*(sj6));
09792 IkReal x2205=((IkReal(0.390000000000000))*(cj5));
09793 IkReal x2206=((cj6)*(r00));
09794 IkReal x2207=((IkReal(0.000500000000000000))*(cj6));
09795 IkReal x2208=((r11)*(sj6));
09796 IkReal x2209=((cj5)*(sj4));
09797 IkReal x2210=((r01)*(sj6));
09798 IkReal x2211=((r20)*(sj6));
09799 IkReal x2212=((r10)*(sj6));
09800 IkReal x2213=((cj6)*(r10));
09801 IkReal x2214=((cj6)*(r21));
09802 IkReal x2215=((r00)*(sj6));
09803 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
09804 evalcond[1]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2204)))+(((sj5)*(x2201))));
09805 evalcond[2]=((IkReal(-0.00200000000000000))+(((IkReal(-1.00000000000000))*(x2203)*(x2204)))+(((x2201)*(x2203)))+(((IkReal(0.000500000000000000))*(x2211)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x2205)))+(((r21)*(x2207))));
09806 evalcond[3]=((((r02)*(x2205)))+(((r01)*(x2207)))+(((x2203)*(x2206)))+(((IkReal(0.000500000000000000))*(x2215)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x2203)*(x2210))));
09807 evalcond[4]=((((IkReal(-1.00000000000000))*(x2203)*(x2208)))+(((r12)*(x2205)))+(((x2203)*(x2213)))+(((IkReal(0.400000000000000))*(cj1)))+(((r11)*(x2207)))+(((IkReal(0.000500000000000000))*(x2212)))+(((IkReal(-1.00000000000000))*(py))));
09808 evalcond[5]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x2202)*(x2211)))+(((IkReal(-1.00000000000000))*(x2202)*(x2214)))+(((x2204)*(x2209)))+(((IkReal(-1.00000000000000))*(x2201)*(x2209)))+(((r22)*(x2200))));
09809 evalcond[6]=((((cj4)*(r22)*(sj5)))+(((sj4)*(x2211)))+(((sj4)*(x2214)))+(((cj4)*(cj5)*(x2204)))+(((IkReal(-1.00000000000000))*(cj5)*(x2201)*(x2202))));
09810 evalcond[7]=((((r02)*(x2200)))+(((IkReal(-1.00000000000000))*(x2206)*(x2209)))+(((IkReal(-1.00000000000000))*(x2202)*(x2215)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2202)))+(((x2209)*(x2210))));
09811 evalcond[8]=((((r12)*(x2200)))+(((IkReal(-1.00000000000000))*(x2202)*(x2212)))+(((IkReal(-1.00000000000000))*(x2209)*(x2213)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x2202)))+(((x2208)*(x2209))));
09812 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
09813 {
09814 {
09815 IkReal dummyeval[1];
09816 IkReal gconst5;
09817 gconst5=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
09818 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
09819 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
09820 {
09821 {
09822 IkReal dummyeval[1];
09823 IkReal gconst6;
09824 gconst6=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
09825 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
09826 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
09827 {
09828 continue;
09829 
09830 } else
09831 {
09832 {
09833 IkReal j3array[1], cj3array[1], sj3array[1];
09834 bool j3valid[1]={false};
09835 _nj3 = 1;
09836 IkReal x2216=((IkReal(1.00000000000000))*(sj4));
09837 IkReal x2217=((r00)*(sj6));
09838 IkReal x2218=((cj1)*(cj6));
09839 IkReal x2219=((cj4)*(sj5));
09840 IkReal x2220=((cj4)*(cj5));
09841 IkReal x2221=((r01)*(sj1));
09842 IkReal x2222=((IkReal(1.00000000000000))*(sj6));
09843 IkReal x2223=((cj1)*(r02));
09844 IkReal x2224=((cj1)*(r01));
09845 IkReal x2225=((IkReal(1.00000000000000))*(r02)*(sj1));
09846 IkReal x2226=((cj6)*(r00)*(sj1));
09847 if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x2219)*(x2225)))+(((r00)*(sj5)*(x2218)))+(((cj5)*(x2223)))+(((IkReal(-1.00000000000000))*(x2220)*(x2221)*(x2222)))+(((IkReal(-1.00000000000000))*(sj1)*(x2216)*(x2217)))+(((IkReal(-1.00000000000000))*(sj5)*(x2222)*(x2224)))+(((IkReal(-1.00000000000000))*(cj6)*(x2216)*(x2221)))+(((x2220)*(x2226))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x2219)*(x2223)))+(((IkReal(-1.00000000000000))*(r01)*(x2216)*(x2218)))+(((r00)*(x2218)*(x2220)))+(((IkReal(-1.00000000000000))*(cj5)*(x2225)))+(((IkReal(-1.00000000000000))*(sj5)*(x2226)))+(((IkReal(-1.00000000000000))*(cj1)*(x2216)*(x2217)))+(((IkReal(-1.00000000000000))*(x2220)*(x2222)*(x2224)))+(((sj5)*(sj6)*(x2221))))))) < IKFAST_ATAN2_MAGTHRESH )
09848     continue;
09849 j3array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(x2219)*(x2225)))+(((r00)*(sj5)*(x2218)))+(((cj5)*(x2223)))+(((IkReal(-1.00000000000000))*(x2220)*(x2221)*(x2222)))+(((IkReal(-1.00000000000000))*(sj1)*(x2216)*(x2217)))+(((IkReal(-1.00000000000000))*(sj5)*(x2222)*(x2224)))+(((IkReal(-1.00000000000000))*(cj6)*(x2216)*(x2221)))+(((x2220)*(x2226)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(x2219)*(x2223)))+(((IkReal(-1.00000000000000))*(r01)*(x2216)*(x2218)))+(((r00)*(x2218)*(x2220)))+(((IkReal(-1.00000000000000))*(cj5)*(x2225)))+(((IkReal(-1.00000000000000))*(sj5)*(x2226)))+(((IkReal(-1.00000000000000))*(cj1)*(x2216)*(x2217)))+(((IkReal(-1.00000000000000))*(x2220)*(x2222)*(x2224)))+(((sj5)*(sj6)*(x2221)))))));
09850 sj3array[0]=IKsin(j3array[0]);
09851 cj3array[0]=IKcos(j3array[0]);
09852 if( j3array[0] > IKPI )
09853 {
09854     j3array[0]-=IK2PI;
09855 }
09856 else if( j3array[0] < -IKPI )
09857 {    j3array[0]+=IK2PI;
09858 }
09859 j3valid[0] = true;
09860 for(int ij3 = 0; ij3 < 1; ++ij3)
09861 {
09862 if( !j3valid[ij3] )
09863 {
09864     continue;
09865 }
09866 _ij3[0] = ij3; _ij3[1] = -1;
09867 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
09868 {
09869 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
09870 {
09871     j3valid[iij3]=false; _ij3[1] = iij3; break; 
09872 }
09873 }
09874 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
09875 {
09876 IkReal evalcond[4];
09877 IkReal x2227=IKcos(j3);
09878 IkReal x2228=IKsin(j3);
09879 IkReal x2229=((r11)*(sj6));
09880 IkReal x2230=((IkReal(1.00000000000000))*(sj5));
09881 IkReal x2231=((sj4)*(sj6));
09882 IkReal x2232=((cj4)*(sj5));
09883 IkReal x2233=((cj6)*(sj4));
09884 IkReal x2234=((cj4)*(cj5));
09885 IkReal x2235=((r01)*(sj6));
09886 IkReal x2236=((cj6)*(sj5));
09887 IkReal x2237=((sj1)*(x2227));
09888 IkReal x2238=((cj1)*(x2227));
09889 IkReal x2239=((IkReal(1.00000000000000))*(x2228));
09890 IkReal x2240=((cj1)*(x2239));
09891 IkReal x2241=((IkReal(1.00000000000000))*(cj6)*(x2234));
09892 evalcond[0]=((x2237)+(((IkReal(-1.00000000000000))*(x2240)))+(((IkReal(-1.00000000000000))*(x2230)*(x2235)))+(((r00)*(x2236)))+(((cj5)*(r02))));
09893 evalcond[1]=((((cj5)*(r12)))+(((r10)*(x2236)))+(((IkReal(-1.00000000000000))*(x2229)*(x2230)))+(((IkReal(-1.00000000000000))*(sj1)*(x2239)))+(((IkReal(-1.00000000000000))*(x2238))));
09894 evalcond[2]=((x2238)+(((sj1)*(x2228)))+(((r02)*(x2232)))+(((IkReal(-1.00000000000000))*(r00)*(x2241)))+(((r01)*(x2233)))+(((r00)*(x2231)))+(((x2234)*(x2235))));
09895 evalcond[3]=((x2237)+(((x2229)*(x2234)))+(((IkReal(-1.00000000000000))*(r10)*(x2241)))+(((r10)*(x2231)))+(((IkReal(-1.00000000000000))*(x2240)))+(((r11)*(x2233)))+(((r12)*(x2232))));
09896 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
09897 {
09898 continue;
09899 }
09900 }
09901 
09902 {
09903 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
09904 vinfos[0].jointtype = 1;
09905 vinfos[0].foffset = j0;
09906 vinfos[0].indices[0] = _ij0[0];
09907 vinfos[0].indices[1] = _ij0[1];
09908 vinfos[0].maxsolutions = _nj0;
09909 vinfos[1].jointtype = 1;
09910 vinfos[1].foffset = j1;
09911 vinfos[1].indices[0] = _ij1[0];
09912 vinfos[1].indices[1] = _ij1[1];
09913 vinfos[1].maxsolutions = _nj1;
09914 vinfos[2].jointtype = 1;
09915 vinfos[2].foffset = j2;
09916 vinfos[2].indices[0] = _ij2[0];
09917 vinfos[2].indices[1] = _ij2[1];
09918 vinfos[2].maxsolutions = _nj2;
09919 vinfos[3].jointtype = 1;
09920 vinfos[3].foffset = j3;
09921 vinfos[3].indices[0] = _ij3[0];
09922 vinfos[3].indices[1] = _ij3[1];
09923 vinfos[3].maxsolutions = _nj3;
09924 vinfos[4].jointtype = 1;
09925 vinfos[4].foffset = j4;
09926 vinfos[4].indices[0] = _ij4[0];
09927 vinfos[4].indices[1] = _ij4[1];
09928 vinfos[4].maxsolutions = _nj4;
09929 vinfos[5].jointtype = 1;
09930 vinfos[5].foffset = j5;
09931 vinfos[5].indices[0] = _ij5[0];
09932 vinfos[5].indices[1] = _ij5[1];
09933 vinfos[5].maxsolutions = _nj5;
09934 vinfos[6].jointtype = 1;
09935 vinfos[6].foffset = j6;
09936 vinfos[6].indices[0] = _ij6[0];
09937 vinfos[6].indices[1] = _ij6[1];
09938 vinfos[6].maxsolutions = _nj6;
09939 std::vector<int> vfree(0);
09940 solutions.AddSolution(vinfos,vfree);
09941 }
09942 }
09943 }
09944 
09945 }
09946 
09947 }
09948 
09949 } else
09950 {
09951 {
09952 IkReal j3array[1], cj3array[1], sj3array[1];
09953 bool j3valid[1]={false};
09954 _nj3 = 1;
09955 IkReal x2242=((cj5)*(r12));
09956 IkReal x2243=((cj6)*(sj5));
09957 IkReal x2244=((IkReal(1.00000000000000))*(sj1));
09958 IkReal x2245=((cj5)*(r02));
09959 IkReal x2246=((IkReal(1.00000000000000))*(cj1));
09960 IkReal x2247=((r01)*(sj5)*(sj6));
09961 IkReal x2248=((r11)*(sj5)*(sj6));
09962 if( IKabs(((gconst5)*(((((cj1)*(x2245)))+(((r10)*(sj1)*(x2243)))+(((IkReal(-1.00000000000000))*(x2244)*(x2248)))+(((cj1)*(r00)*(x2243)))+(((sj1)*(x2242)))+(((IkReal(-1.00000000000000))*(x2246)*(x2247))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((cj1)*(x2242)))+(((IkReal(-1.00000000000000))*(r00)*(x2243)*(x2244)))+(((cj1)*(r10)*(x2243)))+(((IkReal(-1.00000000000000))*(x2244)*(x2245)))+(((sj1)*(x2247)))+(((IkReal(-1.00000000000000))*(x2246)*(x2248))))))) < IKFAST_ATAN2_MAGTHRESH )
09963     continue;
09964 j3array[0]=IKatan2(((gconst5)*(((((cj1)*(x2245)))+(((r10)*(sj1)*(x2243)))+(((IkReal(-1.00000000000000))*(x2244)*(x2248)))+(((cj1)*(r00)*(x2243)))+(((sj1)*(x2242)))+(((IkReal(-1.00000000000000))*(x2246)*(x2247)))))), ((gconst5)*(((((cj1)*(x2242)))+(((IkReal(-1.00000000000000))*(r00)*(x2243)*(x2244)))+(((cj1)*(r10)*(x2243)))+(((IkReal(-1.00000000000000))*(x2244)*(x2245)))+(((sj1)*(x2247)))+(((IkReal(-1.00000000000000))*(x2246)*(x2248)))))));
09965 sj3array[0]=IKsin(j3array[0]);
09966 cj3array[0]=IKcos(j3array[0]);
09967 if( j3array[0] > IKPI )
09968 {
09969     j3array[0]-=IK2PI;
09970 }
09971 else if( j3array[0] < -IKPI )
09972 {    j3array[0]+=IK2PI;
09973 }
09974 j3valid[0] = true;
09975 for(int ij3 = 0; ij3 < 1; ++ij3)
09976 {
09977 if( !j3valid[ij3] )
09978 {
09979     continue;
09980 }
09981 _ij3[0] = ij3; _ij3[1] = -1;
09982 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
09983 {
09984 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
09985 {
09986     j3valid[iij3]=false; _ij3[1] = iij3; break; 
09987 }
09988 }
09989 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
09990 {
09991 IkReal evalcond[4];
09992 IkReal x2249=IKcos(j3);
09993 IkReal x2250=IKsin(j3);
09994 IkReal x2251=((r11)*(sj6));
09995 IkReal x2252=((IkReal(1.00000000000000))*(sj5));
09996 IkReal x2253=((sj4)*(sj6));
09997 IkReal x2254=((cj4)*(sj5));
09998 IkReal x2255=((cj6)*(sj4));
09999 IkReal x2256=((cj4)*(cj5));
10000 IkReal x2257=((r01)*(sj6));
10001 IkReal x2258=((cj6)*(sj5));
10002 IkReal x2259=((sj1)*(x2249));
10003 IkReal x2260=((cj1)*(x2249));
10004 IkReal x2261=((IkReal(1.00000000000000))*(x2250));
10005 IkReal x2262=((cj1)*(x2261));
10006 IkReal x2263=((IkReal(1.00000000000000))*(cj6)*(x2256));
10007 evalcond[0]=((x2259)+(((IkReal(-1.00000000000000))*(x2252)*(x2257)))+(((r00)*(x2258)))+(((IkReal(-1.00000000000000))*(x2262)))+(((cj5)*(r02))));
10008 evalcond[1]=((((cj5)*(r12)))+(((r10)*(x2258)))+(((IkReal(-1.00000000000000))*(sj1)*(x2261)))+(((IkReal(-1.00000000000000))*(x2251)*(x2252)))+(((IkReal(-1.00000000000000))*(x2260))));
10009 evalcond[2]=((x2260)+(((IkReal(-1.00000000000000))*(r00)*(x2263)))+(((x2256)*(x2257)))+(((sj1)*(x2250)))+(((r00)*(x2253)))+(((r01)*(x2255)))+(((r02)*(x2254))));
10010 evalcond[3]=((((r10)*(x2253)))+(x2259)+(((r12)*(x2254)))+(((IkReal(-1.00000000000000))*(r10)*(x2263)))+(((x2251)*(x2256)))+(((IkReal(-1.00000000000000))*(x2262)))+(((r11)*(x2255))));
10011 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
10012 {
10013 continue;
10014 }
10015 }
10016 
10017 {
10018 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10019 vinfos[0].jointtype = 1;
10020 vinfos[0].foffset = j0;
10021 vinfos[0].indices[0] = _ij0[0];
10022 vinfos[0].indices[1] = _ij0[1];
10023 vinfos[0].maxsolutions = _nj0;
10024 vinfos[1].jointtype = 1;
10025 vinfos[1].foffset = j1;
10026 vinfos[1].indices[0] = _ij1[0];
10027 vinfos[1].indices[1] = _ij1[1];
10028 vinfos[1].maxsolutions = _nj1;
10029 vinfos[2].jointtype = 1;
10030 vinfos[2].foffset = j2;
10031 vinfos[2].indices[0] = _ij2[0];
10032 vinfos[2].indices[1] = _ij2[1];
10033 vinfos[2].maxsolutions = _nj2;
10034 vinfos[3].jointtype = 1;
10035 vinfos[3].foffset = j3;
10036 vinfos[3].indices[0] = _ij3[0];
10037 vinfos[3].indices[1] = _ij3[1];
10038 vinfos[3].maxsolutions = _nj3;
10039 vinfos[4].jointtype = 1;
10040 vinfos[4].foffset = j4;
10041 vinfos[4].indices[0] = _ij4[0];
10042 vinfos[4].indices[1] = _ij4[1];
10043 vinfos[4].maxsolutions = _nj4;
10044 vinfos[5].jointtype = 1;
10045 vinfos[5].foffset = j5;
10046 vinfos[5].indices[0] = _ij5[0];
10047 vinfos[5].indices[1] = _ij5[1];
10048 vinfos[5].maxsolutions = _nj5;
10049 vinfos[6].jointtype = 1;
10050 vinfos[6].foffset = j6;
10051 vinfos[6].indices[0] = _ij6[0];
10052 vinfos[6].indices[1] = _ij6[1];
10053 vinfos[6].maxsolutions = _nj6;
10054 std::vector<int> vfree(0);
10055 solutions.AddSolution(vinfos,vfree);
10056 }
10057 }
10058 }
10059 
10060 }
10061 
10062 }
10063 
10064 } else
10065 {
10066 IkReal x2264=((sj4)*(sj5));
10067 IkReal x2265=((cj6)*(r20));
10068 IkReal x2266=((IkReal(1.00000000000000))*(cj4));
10069 IkReal x2267=((IkReal(0.390000000000000))*(sj5));
10070 IkReal x2268=((r21)*(sj6));
10071 IkReal x2269=((IkReal(0.390000000000000))*(cj5));
10072 IkReal x2270=((cj6)*(r00));
10073 IkReal x2271=((IkReal(0.000500000000000000))*(cj6));
10074 IkReal x2272=((r11)*(sj6));
10075 IkReal x2273=((cj5)*(sj4));
10076 IkReal x2274=((r01)*(sj6));
10077 IkReal x2275=((r20)*(sj6));
10078 IkReal x2276=((r10)*(sj6));
10079 IkReal x2277=((cj6)*(r10));
10080 IkReal x2278=((cj6)*(r21));
10081 IkReal x2279=((r00)*(sj6));
10082 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
10083 evalcond[1]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2268)))+(((sj5)*(x2265))));
10084 evalcond[2]=((IkReal(0.00200000000000000))+(((r21)*(x2271)))+(((r22)*(x2269)))+(((IkReal(-1.00000000000000))*(pz)))+(((x2265)*(x2267)))+(((IkReal(-1.00000000000000))*(x2267)*(x2268)))+(((IkReal(0.000500000000000000))*(x2275))));
10085 evalcond[3]=((((r01)*(x2271)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x2269)))+(((x2267)*(x2270)))+(((IkReal(-1.00000000000000))*(x2267)*(x2274)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(0.000500000000000000))*(x2279))));
10086 evalcond[4]=((((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x2271)))+(((r12)*(x2269)))+(((x2267)*(x2277)))+(((IkReal(-1.00000000000000))*(x2267)*(x2272)))+(((IkReal(0.000500000000000000))*(x2276))));
10087 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x2266)*(x2275)))+(((IkReal(-1.00000000000000))*(x2266)*(x2278)))+(((r22)*(x2264)))+(((x2268)*(x2273)))+(((IkReal(-1.00000000000000))*(x2265)*(x2273))));
10088 evalcond[6]=((((cj4)*(cj5)*(x2268)))+(((sj4)*(x2275)))+(((sj4)*(x2278)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x2265)*(x2266))));
10089 evalcond[7]=((((IkReal(-1.00000000000000))*(x2270)*(x2273)))+(((IkReal(-1.00000000000000))*(x2266)*(x2279)))+(((x2273)*(x2274)))+(((r02)*(x2264)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2266))));
10090 evalcond[8]=((((IkReal(-1.00000000000000))*(x2266)*(x2276)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x2266)))+(((IkReal(-1.00000000000000))*(x2273)*(x2277)))+(((x2272)*(x2273)))+(((r12)*(x2264))));
10091 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
10092 {
10093 {
10094 IkReal dummyeval[1];
10095 IkReal gconst7;
10096 gconst7=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
10097 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
10098 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
10099 {
10100 {
10101 IkReal dummyeval[1];
10102 IkReal gconst8;
10103 gconst8=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
10104 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
10105 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
10106 {
10107 continue;
10108 
10109 } else
10110 {
10111 {
10112 IkReal j3array[1], cj3array[1], sj3array[1];
10113 bool j3valid[1]={false};
10114 _nj3 = 1;
10115 IkReal x2280=((sj1)*(sj6));
10116 IkReal x2281=((r00)*(sj4));
10117 IkReal x2282=((IkReal(1.00000000000000))*(r01));
10118 IkReal x2283=((cj1)*(cj6));
10119 IkReal x2284=((cj1)*(sj6));
10120 IkReal x2285=((cj4)*(cj5));
10121 IkReal x2286=((r02)*(sj1));
10122 IkReal x2287=((cj4)*(sj5));
10123 IkReal x2288=((cj1)*(r02));
10124 IkReal x2289=((cj6)*(r00)*(sj1));
10125 if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(x2285)*(x2289)))+(((r01)*(x2280)*(x2285)))+(((r00)*(sj5)*(x2283)))+(((IkReal(-1.00000000000000))*(sj5)*(x2282)*(x2284)))+(((x2280)*(x2281)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((cj5)*(x2288)))+(((x2286)*(x2287))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(sj4)*(x2282)*(x2283)))+(((sj5)*(x2289)))+(((IkReal(-1.00000000000000))*(x2282)*(x2284)*(x2285)))+(((IkReal(-1.00000000000000))*(x2287)*(x2288)))+(((r00)*(x2283)*(x2285)))+(((cj5)*(x2286)))+(((IkReal(-1.00000000000000))*(sj5)*(x2280)*(x2282)))+(((IkReal(-1.00000000000000))*(x2281)*(x2284))))))) < IKFAST_ATAN2_MAGTHRESH )
10126     continue;
10127 j3array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(x2285)*(x2289)))+(((r01)*(x2280)*(x2285)))+(((r00)*(sj5)*(x2283)))+(((IkReal(-1.00000000000000))*(sj5)*(x2282)*(x2284)))+(((x2280)*(x2281)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((cj5)*(x2288)))+(((x2286)*(x2287)))))), ((gconst8)*(((((IkReal(-1.00000000000000))*(sj4)*(x2282)*(x2283)))+(((sj5)*(x2289)))+(((IkReal(-1.00000000000000))*(x2282)*(x2284)*(x2285)))+(((IkReal(-1.00000000000000))*(x2287)*(x2288)))+(((r00)*(x2283)*(x2285)))+(((cj5)*(x2286)))+(((IkReal(-1.00000000000000))*(sj5)*(x2280)*(x2282)))+(((IkReal(-1.00000000000000))*(x2281)*(x2284)))))));
10128 sj3array[0]=IKsin(j3array[0]);
10129 cj3array[0]=IKcos(j3array[0]);
10130 if( j3array[0] > IKPI )
10131 {
10132     j3array[0]-=IK2PI;
10133 }
10134 else if( j3array[0] < -IKPI )
10135 {    j3array[0]+=IK2PI;
10136 }
10137 j3valid[0] = true;
10138 for(int ij3 = 0; ij3 < 1; ++ij3)
10139 {
10140 if( !j3valid[ij3] )
10141 {
10142     continue;
10143 }
10144 _ij3[0] = ij3; _ij3[1] = -1;
10145 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10146 {
10147 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10148 {
10149     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10150 }
10151 }
10152 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10153 {
10154 IkReal evalcond[4];
10155 IkReal x2290=IKsin(j3);
10156 IkReal x2291=IKcos(j3);
10157 IkReal x2292=((r11)*(sj6));
10158 IkReal x2293=((IkReal(1.00000000000000))*(sj5));
10159 IkReal x2294=((sj4)*(sj6));
10160 IkReal x2295=((cj4)*(sj5));
10161 IkReal x2296=((cj6)*(sj4));
10162 IkReal x2297=((cj4)*(cj5));
10163 IkReal x2298=((r01)*(sj6));
10164 IkReal x2299=((cj6)*(sj5));
10165 IkReal x2300=((sj1)*(x2290));
10166 IkReal x2301=((cj1)*(x2290));
10167 IkReal x2302=((IkReal(1.00000000000000))*(x2291));
10168 IkReal x2303=((cj1)*(x2302));
10169 IkReal x2304=((IkReal(1.00000000000000))*(cj6)*(x2297));
10170 evalcond[0]=((((sj1)*(x2291)))+(x2301)+(((IkReal(-1.00000000000000))*(x2293)*(x2298)))+(((r00)*(x2299)))+(((cj5)*(r02))));
10171 evalcond[1]=((((cj5)*(r12)))+(((r10)*(x2299)))+(x2300)+(((IkReal(-1.00000000000000))*(x2292)*(x2293)))+(((IkReal(-1.00000000000000))*(x2303))));
10172 evalcond[2]=((x2300)+(((IkReal(-1.00000000000000))*(r00)*(x2304)))+(((IkReal(-1.00000000000000))*(x2303)))+(((r02)*(x2295)))+(((r00)*(x2294)))+(((r01)*(x2296)))+(((x2297)*(x2298))));
10173 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x2304)))+(((IkReal(-1.00000000000000))*(x2301)))+(((IkReal(-1.00000000000000))*(sj1)*(x2302)))+(((r10)*(x2294)))+(((x2292)*(x2297)))+(((r12)*(x2295)))+(((r11)*(x2296))));
10174 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
10175 {
10176 continue;
10177 }
10178 }
10179 
10180 {
10181 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10182 vinfos[0].jointtype = 1;
10183 vinfos[0].foffset = j0;
10184 vinfos[0].indices[0] = _ij0[0];
10185 vinfos[0].indices[1] = _ij0[1];
10186 vinfos[0].maxsolutions = _nj0;
10187 vinfos[1].jointtype = 1;
10188 vinfos[1].foffset = j1;
10189 vinfos[1].indices[0] = _ij1[0];
10190 vinfos[1].indices[1] = _ij1[1];
10191 vinfos[1].maxsolutions = _nj1;
10192 vinfos[2].jointtype = 1;
10193 vinfos[2].foffset = j2;
10194 vinfos[2].indices[0] = _ij2[0];
10195 vinfos[2].indices[1] = _ij2[1];
10196 vinfos[2].maxsolutions = _nj2;
10197 vinfos[3].jointtype = 1;
10198 vinfos[3].foffset = j3;
10199 vinfos[3].indices[0] = _ij3[0];
10200 vinfos[3].indices[1] = _ij3[1];
10201 vinfos[3].maxsolutions = _nj3;
10202 vinfos[4].jointtype = 1;
10203 vinfos[4].foffset = j4;
10204 vinfos[4].indices[0] = _ij4[0];
10205 vinfos[4].indices[1] = _ij4[1];
10206 vinfos[4].maxsolutions = _nj4;
10207 vinfos[5].jointtype = 1;
10208 vinfos[5].foffset = j5;
10209 vinfos[5].indices[0] = _ij5[0];
10210 vinfos[5].indices[1] = _ij5[1];
10211 vinfos[5].maxsolutions = _nj5;
10212 vinfos[6].jointtype = 1;
10213 vinfos[6].foffset = j6;
10214 vinfos[6].indices[0] = _ij6[0];
10215 vinfos[6].indices[1] = _ij6[1];
10216 vinfos[6].maxsolutions = _nj6;
10217 std::vector<int> vfree(0);
10218 solutions.AddSolution(vinfos,vfree);
10219 }
10220 }
10221 }
10222 
10223 }
10224 
10225 }
10226 
10227 } else
10228 {
10229 {
10230 IkReal j3array[1], cj3array[1], sj3array[1];
10231 bool j3valid[1]={false};
10232 _nj3 = 1;
10233 IkReal x2305=((sj1)*(sj5));
10234 IkReal x2306=((cj6)*(r10));
10235 IkReal x2307=((cj5)*(sj1));
10236 IkReal x2308=((cj1)*(cj5));
10237 IkReal x2309=((cj1)*(sj5));
10238 IkReal x2310=((cj6)*(r00));
10239 IkReal x2311=((r11)*(sj6));
10240 IkReal x2312=((IkReal(1.00000000000000))*(r01)*(sj6));
10241 if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x2305)*(x2311)))+(((r12)*(x2307)))+(((x2305)*(x2306)))+(((IkReal(-1.00000000000000))*(x2309)*(x2312)))+(((x2309)*(x2310)))+(((r02)*(x2308))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x2308)))+(((x2305)*(x2310)))+(((x2309)*(x2311)))+(((r02)*(x2307)))+(((IkReal(-1.00000000000000))*(x2305)*(x2312)))+(((IkReal(-1.00000000000000))*(x2306)*(x2309))))))) < IKFAST_ATAN2_MAGTHRESH )
10242     continue;
10243 j3array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x2305)*(x2311)))+(((r12)*(x2307)))+(((x2305)*(x2306)))+(((IkReal(-1.00000000000000))*(x2309)*(x2312)))+(((x2309)*(x2310)))+(((r02)*(x2308)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x2308)))+(((x2305)*(x2310)))+(((x2309)*(x2311)))+(((r02)*(x2307)))+(((IkReal(-1.00000000000000))*(x2305)*(x2312)))+(((IkReal(-1.00000000000000))*(x2306)*(x2309)))))));
10244 sj3array[0]=IKsin(j3array[0]);
10245 cj3array[0]=IKcos(j3array[0]);
10246 if( j3array[0] > IKPI )
10247 {
10248     j3array[0]-=IK2PI;
10249 }
10250 else if( j3array[0] < -IKPI )
10251 {    j3array[0]+=IK2PI;
10252 }
10253 j3valid[0] = true;
10254 for(int ij3 = 0; ij3 < 1; ++ij3)
10255 {
10256 if( !j3valid[ij3] )
10257 {
10258     continue;
10259 }
10260 _ij3[0] = ij3; _ij3[1] = -1;
10261 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10262 {
10263 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10264 {
10265     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10266 }
10267 }
10268 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10269 {
10270 IkReal evalcond[4];
10271 IkReal x2313=IKsin(j3);
10272 IkReal x2314=IKcos(j3);
10273 IkReal x2315=((r11)*(sj6));
10274 IkReal x2316=((IkReal(1.00000000000000))*(sj5));
10275 IkReal x2317=((sj4)*(sj6));
10276 IkReal x2318=((cj4)*(sj5));
10277 IkReal x2319=((cj6)*(sj4));
10278 IkReal x2320=((cj4)*(cj5));
10279 IkReal x2321=((r01)*(sj6));
10280 IkReal x2322=((cj6)*(sj5));
10281 IkReal x2323=((sj1)*(x2313));
10282 IkReal x2324=((cj1)*(x2313));
10283 IkReal x2325=((IkReal(1.00000000000000))*(x2314));
10284 IkReal x2326=((cj1)*(x2325));
10285 IkReal x2327=((IkReal(1.00000000000000))*(cj6)*(x2320));
10286 evalcond[0]=((x2324)+(((sj1)*(x2314)))+(((r00)*(x2322)))+(((IkReal(-1.00000000000000))*(x2316)*(x2321)))+(((cj5)*(r02))));
10287 evalcond[1]=((((r10)*(x2322)))+(((cj5)*(r12)))+(x2323)+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(-1.00000000000000))*(x2326))));
10288 evalcond[2]=((x2323)+(((r01)*(x2319)))+(((x2320)*(x2321)))+(((r00)*(x2317)))+(((IkReal(-1.00000000000000))*(r00)*(x2327)))+(((r02)*(x2318)))+(((IkReal(-1.00000000000000))*(x2326))));
10289 evalcond[3]=((((x2315)*(x2320)))+(((r10)*(x2317)))+(((r11)*(x2319)))+(((r12)*(x2318)))+(((IkReal(-1.00000000000000))*(r10)*(x2327)))+(((IkReal(-1.00000000000000))*(sj1)*(x2325)))+(((IkReal(-1.00000000000000))*(x2324))));
10290 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
10291 {
10292 continue;
10293 }
10294 }
10295 
10296 {
10297 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10298 vinfos[0].jointtype = 1;
10299 vinfos[0].foffset = j0;
10300 vinfos[0].indices[0] = _ij0[0];
10301 vinfos[0].indices[1] = _ij0[1];
10302 vinfos[0].maxsolutions = _nj0;
10303 vinfos[1].jointtype = 1;
10304 vinfos[1].foffset = j1;
10305 vinfos[1].indices[0] = _ij1[0];
10306 vinfos[1].indices[1] = _ij1[1];
10307 vinfos[1].maxsolutions = _nj1;
10308 vinfos[2].jointtype = 1;
10309 vinfos[2].foffset = j2;
10310 vinfos[2].indices[0] = _ij2[0];
10311 vinfos[2].indices[1] = _ij2[1];
10312 vinfos[2].maxsolutions = _nj2;
10313 vinfos[3].jointtype = 1;
10314 vinfos[3].foffset = j3;
10315 vinfos[3].indices[0] = _ij3[0];
10316 vinfos[3].indices[1] = _ij3[1];
10317 vinfos[3].maxsolutions = _nj3;
10318 vinfos[4].jointtype = 1;
10319 vinfos[4].foffset = j4;
10320 vinfos[4].indices[0] = _ij4[0];
10321 vinfos[4].indices[1] = _ij4[1];
10322 vinfos[4].maxsolutions = _nj4;
10323 vinfos[5].jointtype = 1;
10324 vinfos[5].foffset = j5;
10325 vinfos[5].indices[0] = _ij5[0];
10326 vinfos[5].indices[1] = _ij5[1];
10327 vinfos[5].maxsolutions = _nj5;
10328 vinfos[6].jointtype = 1;
10329 vinfos[6].foffset = j6;
10330 vinfos[6].indices[0] = _ij6[0];
10331 vinfos[6].indices[1] = _ij6[1];
10332 vinfos[6].maxsolutions = _nj6;
10333 std::vector<int> vfree(0);
10334 solutions.AddSolution(vinfos,vfree);
10335 }
10336 }
10337 }
10338 
10339 }
10340 
10341 }
10342 
10343 } else
10344 {
10345 IkReal x2328=((sj4)*(sj5));
10346 IkReal x2329=((IkReal(0.390000000000000))*(sj5));
10347 IkReal x2330=((r21)*(sj6));
10348 IkReal x2331=((IkReal(0.390000000000000))*(cj5));
10349 IkReal x2332=((cj6)*(r00));
10350 IkReal x2333=((IkReal(0.000500000000000000))*(cj6));
10351 IkReal x2334=((r11)*(sj6));
10352 IkReal x2335=((cj5)*(sj4));
10353 IkReal x2336=((IkReal(1.00000000000000))*(r20));
10354 IkReal x2337=((cj4)*(sj6));
10355 IkReal x2338=((r01)*(sj6));
10356 IkReal x2339=((IkReal(0.000500000000000000))*(sj6));
10357 IkReal x2340=((IkReal(1.00000000000000))*(r10));
10358 IkReal x2341=((IkReal(1.00000000000000))*(cj4)*(cj6));
10359 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
10360 evalcond[1]=((((r21)*(x2333)))+(((cj6)*(r20)*(x2329)))+(((r22)*(x2331)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x2329)*(x2330)))+(((IkReal(-0.00200000000000000))*(cj2)))+(((r20)*(x2339))));
10361 evalcond[2]=((IkReal(-0.400000000000000))+(((r02)*(x2331)))+(((x2329)*(x2332)))+(((r01)*(x2333)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x2339)))+(((IkReal(-1.00000000000000))*(x2329)*(x2338))));
10362 evalcond[3]=((((r12)*(x2331)))+(((r11)*(x2333)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(x2329)*(x2334)))+(((cj6)*(r10)*(x2329)))+(((r10)*(x2339))));
10363 evalcond[4]=((((IkReal(-1.00000000000000))*(r21)*(x2341)))+(((r22)*(x2328)))+(((IkReal(-1.00000000000000))*(cj6)*(x2335)*(x2336)))+(((IkReal(-1.00000000000000))*(x2336)*(x2337)))+(((x2330)*(x2335)))+(((IkReal(-1.00000000000000))*(cj2))));
10364 evalcond[5]=((((r02)*(x2328)))+(((IkReal(-1.00000000000000))*(r01)*(x2341)))+(((IkReal(-1.00000000000000))*(x2332)*(x2335)))+(((x2335)*(x2338)))+(((IkReal(-1.00000000000000))*(r00)*(x2337))));
10365 evalcond[6]=((((IkReal(-1.00000000000000))*(x2337)*(x2340)))+(((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj6)*(x2335)*(x2340)))+(((x2334)*(x2335)))+(((r12)*(x2328)))+(((IkReal(-1.00000000000000))*(r11)*(x2341))));
10366 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
10367 {
10368 {
10369 IkReal j3array[1], cj3array[1], sj3array[1];
10370 bool j3valid[1]={false};
10371 _nj3 = 1;
10372 IkReal x2342=((IkReal(1.00000000000000))*(cj6));
10373 IkReal x2343=((IkReal(1.00000000000000))*(cj5));
10374 IkReal x2344=((r01)*(sj6));
10375 if( IKabs(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x2343)*(x2344)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2342))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2342)))+(((IkReal(-1.00000000000000))*(r02)*(x2343)))+(((sj5)*(x2344))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x2343)*(x2344)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2342)))))+IKsqr(((((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2342)))+(((IkReal(-1.00000000000000))*(r02)*(x2343)))+(((sj5)*(x2344)))))-1) <= IKFAST_SINCOS_THRESH )
10376     continue;
10377 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x2343)*(x2344)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2342)))), ((((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2342)))+(((IkReal(-1.00000000000000))*(r02)*(x2343)))+(((sj5)*(x2344)))));
10378 sj3array[0]=IKsin(j3array[0]);
10379 cj3array[0]=IKcos(j3array[0]);
10380 if( j3array[0] > IKPI )
10381 {
10382     j3array[0]-=IK2PI;
10383 }
10384 else if( j3array[0] < -IKPI )
10385 {    j3array[0]+=IK2PI;
10386 }
10387 j3valid[0] = true;
10388 for(int ij3 = 0; ij3 < 1; ++ij3)
10389 {
10390 if( !j3valid[ij3] )
10391 {
10392     continue;
10393 }
10394 _ij3[0] = ij3; _ij3[1] = -1;
10395 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10396 {
10397 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10398 {
10399     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10400 }
10401 }
10402 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10403 {
10404 IkReal evalcond[6];
10405 IkReal x2345=IKsin(j3);
10406 IkReal x2346=IKcos(j3);
10407 IkReal x2347=((r11)*(sj6));
10408 IkReal x2348=((IkReal(1.00000000000000))*(sj5));
10409 IkReal x2349=((sj4)*(sj6));
10410 IkReal x2350=((cj4)*(sj5));
10411 IkReal x2351=((cj6)*(sj4));
10412 IkReal x2352=((cj4)*(cj5));
10413 IkReal x2353=((r01)*(sj6));
10414 IkReal x2354=((r21)*(sj6));
10415 IkReal x2355=((cj6)*(sj5));
10416 IkReal x2356=((IkReal(1.00000000000000))*(cj6)*(x2352));
10417 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2348)*(x2354)))+(((r20)*(x2355)))+(((sj2)*(x2345))));
10418 evalcond[1]=((((r00)*(x2355)))+(x2346)+(((IkReal(-1.00000000000000))*(x2348)*(x2353)))+(((cj5)*(r02))));
10419 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x2355)))+(((IkReal(-1.00000000000000))*(cj2)*(x2345)))+(((IkReal(-1.00000000000000))*(x2347)*(x2348))));
10420 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x2356)))+(((r22)*(x2350)))+(((x2352)*(x2354)))+(((IkReal(-1.00000000000000))*(sj2)*(x2346)))+(((r20)*(x2349)))+(((r21)*(x2351))));
10421 evalcond[4]=((((r01)*(x2351)))+(x2345)+(((IkReal(-1.00000000000000))*(r00)*(x2356)))+(((r00)*(x2349)))+(((x2352)*(x2353)))+(((r02)*(x2350))));
10422 evalcond[5]=((((r10)*(x2349)))+(((r12)*(x2350)))+(((cj2)*(x2346)))+(((r11)*(x2351)))+(((IkReal(-1.00000000000000))*(r10)*(x2356)))+(((x2347)*(x2352))));
10423 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
10424 {
10425 continue;
10426 }
10427 }
10428 
10429 {
10430 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10431 vinfos[0].jointtype = 1;
10432 vinfos[0].foffset = j0;
10433 vinfos[0].indices[0] = _ij0[0];
10434 vinfos[0].indices[1] = _ij0[1];
10435 vinfos[0].maxsolutions = _nj0;
10436 vinfos[1].jointtype = 1;
10437 vinfos[1].foffset = j1;
10438 vinfos[1].indices[0] = _ij1[0];
10439 vinfos[1].indices[1] = _ij1[1];
10440 vinfos[1].maxsolutions = _nj1;
10441 vinfos[2].jointtype = 1;
10442 vinfos[2].foffset = j2;
10443 vinfos[2].indices[0] = _ij2[0];
10444 vinfos[2].indices[1] = _ij2[1];
10445 vinfos[2].maxsolutions = _nj2;
10446 vinfos[3].jointtype = 1;
10447 vinfos[3].foffset = j3;
10448 vinfos[3].indices[0] = _ij3[0];
10449 vinfos[3].indices[1] = _ij3[1];
10450 vinfos[3].maxsolutions = _nj3;
10451 vinfos[4].jointtype = 1;
10452 vinfos[4].foffset = j4;
10453 vinfos[4].indices[0] = _ij4[0];
10454 vinfos[4].indices[1] = _ij4[1];
10455 vinfos[4].maxsolutions = _nj4;
10456 vinfos[5].jointtype = 1;
10457 vinfos[5].foffset = j5;
10458 vinfos[5].indices[0] = _ij5[0];
10459 vinfos[5].indices[1] = _ij5[1];
10460 vinfos[5].maxsolutions = _nj5;
10461 vinfos[6].jointtype = 1;
10462 vinfos[6].foffset = j6;
10463 vinfos[6].indices[0] = _ij6[0];
10464 vinfos[6].indices[1] = _ij6[1];
10465 vinfos[6].maxsolutions = _nj6;
10466 std::vector<int> vfree(0);
10467 solutions.AddSolution(vinfos,vfree);
10468 }
10469 }
10470 }
10471 
10472 } else
10473 {
10474 IkReal x2357=((sj4)*(sj5));
10475 IkReal x2358=((IkReal(0.390000000000000))*(sj5));
10476 IkReal x2359=((r21)*(sj6));
10477 IkReal x2360=((IkReal(0.390000000000000))*(cj5));
10478 IkReal x2361=((cj6)*(r00));
10479 IkReal x2362=((IkReal(0.000500000000000000))*(cj6));
10480 IkReal x2363=((r11)*(sj6));
10481 IkReal x2364=((cj5)*(sj4));
10482 IkReal x2365=((IkReal(1.00000000000000))*(r20));
10483 IkReal x2366=((cj4)*(sj6));
10484 IkReal x2367=((r01)*(sj6));
10485 IkReal x2368=((IkReal(0.000500000000000000))*(sj6));
10486 IkReal x2369=((IkReal(1.00000000000000))*(r10));
10487 IkReal x2370=((IkReal(1.00000000000000))*(cj4)*(cj6));
10488 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
10489 evalcond[1]=((((r20)*(x2368)))+(((IkReal(-1.00000000000000))*(x2358)*(x2359)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x2360)))+(((cj6)*(r20)*(x2358)))+(((IkReal(-0.00200000000000000))*(cj2)))+(((r21)*(x2362))));
10490 evalcond[2]=((IkReal(0.400000000000000))+(((r02)*(x2360)))+(((x2358)*(x2361)))+(((IkReal(-1.00000000000000))*(x2358)*(x2367)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x2362)))+(((r00)*(x2368))));
10491 evalcond[3]=((((r11)*(x2362)))+(((r12)*(x2360)))+(((cj6)*(r10)*(x2358)))+(((r10)*(x2368)))+(((IkReal(0.00200000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(x2358)*(x2363)))+(((IkReal(-1.00000000000000))*(py))));
10492 evalcond[4]=((((r22)*(x2357)))+(((IkReal(-1.00000000000000))*(cj6)*(x2364)*(x2365)))+(((IkReal(-1.00000000000000))*(r21)*(x2370)))+(((x2359)*(x2364)))+(((IkReal(-1.00000000000000))*(x2365)*(x2366)))+(((IkReal(-1.00000000000000))*(cj2))));
10493 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x2366)))+(((x2364)*(x2367)))+(((IkReal(-1.00000000000000))*(x2361)*(x2364)))+(((IkReal(-1.00000000000000))*(r01)*(x2370)))+(((r02)*(x2357))));
10494 evalcond[6]=((sj2)+(((IkReal(-1.00000000000000))*(cj6)*(x2364)*(x2369)))+(((r12)*(x2357)))+(((IkReal(-1.00000000000000))*(x2366)*(x2369)))+(((x2363)*(x2364)))+(((IkReal(-1.00000000000000))*(r11)*(x2370))));
10495 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
10496 {
10497 {
10498 IkReal j3array[1], cj3array[1], sj3array[1];
10499 bool j3valid[1]={false};
10500 _nj3 = 1;
10501 IkReal x2371=((cj4)*(cj5));
10502 IkReal x2372=((r01)*(sj6));
10503 IkReal x2373=((cj6)*(r00));
10504 if( IKabs(((((r00)*(sj4)*(sj6)))+(((x2371)*(x2372)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x2371)*(x2373))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj5)*(x2373)))+(((IkReal(-1.00000000000000))*(sj5)*(x2372)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r00)*(sj4)*(sj6)))+(((x2371)*(x2372)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x2371)*(x2373)))))+IKsqr(((((sj5)*(x2373)))+(((IkReal(-1.00000000000000))*(sj5)*(x2372)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
10505     continue;
10506 j3array[0]=IKatan2(((((r00)*(sj4)*(sj6)))+(((x2371)*(x2372)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x2371)*(x2373)))), ((((sj5)*(x2373)))+(((IkReal(-1.00000000000000))*(sj5)*(x2372)))+(((cj5)*(r02)))));
10507 sj3array[0]=IKsin(j3array[0]);
10508 cj3array[0]=IKcos(j3array[0]);
10509 if( j3array[0] > IKPI )
10510 {
10511     j3array[0]-=IK2PI;
10512 }
10513 else if( j3array[0] < -IKPI )
10514 {    j3array[0]+=IK2PI;
10515 }
10516 j3valid[0] = true;
10517 for(int ij3 = 0; ij3 < 1; ++ij3)
10518 {
10519 if( !j3valid[ij3] )
10520 {
10521     continue;
10522 }
10523 _ij3[0] = ij3; _ij3[1] = -1;
10524 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10525 {
10526 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10527 {
10528     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10529 }
10530 }
10531 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10532 {
10533 IkReal evalcond[6];
10534 IkReal x2374=IKsin(j3);
10535 IkReal x2375=IKcos(j3);
10536 IkReal x2376=((r11)*(sj6));
10537 IkReal x2377=((IkReal(1.00000000000000))*(sj5));
10538 IkReal x2378=((cj6)*(sj4));
10539 IkReal x2379=((sj4)*(sj6));
10540 IkReal x2380=((cj4)*(sj5));
10541 IkReal x2381=((cj4)*(cj5));
10542 IkReal x2382=((r01)*(sj6));
10543 IkReal x2383=((r21)*(sj6));
10544 IkReal x2384=((cj6)*(sj5));
10545 IkReal x2385=((IkReal(1.00000000000000))*(x2375));
10546 IkReal x2386=((IkReal(1.00000000000000))*(cj6)*(x2381));
10547 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x2384)))+(((sj2)*(x2374)))+(((IkReal(-1.00000000000000))*(x2377)*(x2383))));
10548 evalcond[1]=((((r00)*(x2384)))+(((IkReal(-1.00000000000000))*(x2385)))+(((IkReal(-1.00000000000000))*(x2377)*(x2382)))+(((cj5)*(r02))));
10549 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2376)*(x2377)))+(((r10)*(x2384)))+(((cj2)*(x2374))));
10550 evalcond[3]=((((r20)*(x2379)))+(((x2381)*(x2383)))+(((IkReal(-1.00000000000000))*(sj2)*(x2385)))+(((IkReal(-1.00000000000000))*(r20)*(x2386)))+(((r22)*(x2380)))+(((r21)*(x2378))));
10551 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x2386)))+(((x2381)*(x2382)))+(((IkReal(-1.00000000000000))*(x2374)))+(((r02)*(x2380)))+(((r00)*(x2379)))+(((r01)*(x2378))));
10552 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x2386)))+(((x2376)*(x2381)))+(((r10)*(x2379)))+(((IkReal(-1.00000000000000))*(cj2)*(x2385)))+(((r12)*(x2380)))+(((r11)*(x2378))));
10553 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
10554 {
10555 continue;
10556 }
10557 }
10558 
10559 {
10560 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10561 vinfos[0].jointtype = 1;
10562 vinfos[0].foffset = j0;
10563 vinfos[0].indices[0] = _ij0[0];
10564 vinfos[0].indices[1] = _ij0[1];
10565 vinfos[0].maxsolutions = _nj0;
10566 vinfos[1].jointtype = 1;
10567 vinfos[1].foffset = j1;
10568 vinfos[1].indices[0] = _ij1[0];
10569 vinfos[1].indices[1] = _ij1[1];
10570 vinfos[1].maxsolutions = _nj1;
10571 vinfos[2].jointtype = 1;
10572 vinfos[2].foffset = j2;
10573 vinfos[2].indices[0] = _ij2[0];
10574 vinfos[2].indices[1] = _ij2[1];
10575 vinfos[2].maxsolutions = _nj2;
10576 vinfos[3].jointtype = 1;
10577 vinfos[3].foffset = j3;
10578 vinfos[3].indices[0] = _ij3[0];
10579 vinfos[3].indices[1] = _ij3[1];
10580 vinfos[3].maxsolutions = _nj3;
10581 vinfos[4].jointtype = 1;
10582 vinfos[4].foffset = j4;
10583 vinfos[4].indices[0] = _ij4[0];
10584 vinfos[4].indices[1] = _ij4[1];
10585 vinfos[4].maxsolutions = _nj4;
10586 vinfos[5].jointtype = 1;
10587 vinfos[5].foffset = j5;
10588 vinfos[5].indices[0] = _ij5[0];
10589 vinfos[5].indices[1] = _ij5[1];
10590 vinfos[5].maxsolutions = _nj5;
10591 vinfos[6].jointtype = 1;
10592 vinfos[6].foffset = j6;
10593 vinfos[6].indices[0] = _ij6[0];
10594 vinfos[6].indices[1] = _ij6[1];
10595 vinfos[6].maxsolutions = _nj6;
10596 std::vector<int> vfree(0);
10597 solutions.AddSolution(vinfos,vfree);
10598 }
10599 }
10600 }
10601 
10602 } else
10603 {
10604 IkReal x2387=((sj4)*(sj5));
10605 IkReal x2388=((IkReal(0.390000000000000))*(sj5));
10606 IkReal x2389=((r21)*(sj6));
10607 IkReal x2390=((IkReal(0.390000000000000))*(cj5));
10608 IkReal x2391=((cj6)*(r00));
10609 IkReal x2392=((IkReal(0.000500000000000000))*(cj6));
10610 IkReal x2393=((r11)*(sj6));
10611 IkReal x2394=((cj5)*(sj4));
10612 IkReal x2395=((r01)*(sj6));
10613 IkReal x2396=((IkReal(1.00000000000000))*(r20));
10614 IkReal x2397=((cj4)*(sj6));
10615 IkReal x2398=((IkReal(0.000500000000000000))*(sj6));
10616 IkReal x2399=((IkReal(1.00000000000000))*(r10));
10617 IkReal x2400=((IkReal(1.00000000000000))*(cj4)*(cj6));
10618 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
10619 evalcond[1]=((((r20)*(x2398)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x2390)))+(((IkReal(-1.00000000000000))*(x2388)*(x2389)))+(((cj6)*(r20)*(x2388)))+(((r21)*(x2392))));
10620 evalcond[2]=((((r00)*(x2398)))+(((r01)*(x2392)))+(((r02)*(x2390)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(x2388)*(x2395)))+(((x2388)*(x2391)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1))));
10621 evalcond[3]=((((r12)*(x2390)))+(((cj6)*(r10)*(x2388)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj1)))+(((r10)*(x2398)))+(((IkReal(-1.00000000000000))*(x2388)*(x2393)))+(((r11)*(x2392))));
10622 evalcond[4]=((((IkReal(-1.00000000000000))*(x2396)*(x2397)))+(((IkReal(-1.00000000000000))*(cj6)*(x2394)*(x2396)))+(((r22)*(x2387)))+(((IkReal(-1.00000000000000))*(r21)*(x2400)))+(((x2389)*(x2394))));
10623 evalcond[5]=((((IkReal(-1.00000000000000))*(x2391)*(x2394)))+(((IkReal(-1.00000000000000))*(r01)*(x2400)))+(((r02)*(x2387)))+(((IkReal(-1.00000000000000))*(r00)*(x2397)))+(((x2394)*(x2395)))+(((IkReal(-1.00000000000000))*(cj1))));
10624 evalcond[6]=((((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj6)*(x2394)*(x2399)))+(((x2393)*(x2394)))+(((IkReal(-1.00000000000000))*(x2397)*(x2399)))+(((IkReal(-1.00000000000000))*(r11)*(x2400)))+(((r12)*(x2387))));
10625 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
10626 {
10627 {
10628 IkReal j3array[1], cj3array[1], sj3array[1];
10629 bool j3valid[1]={false};
10630 _nj3 = 1;
10631 IkReal x2401=((cj4)*(cj5));
10632 IkReal x2402=((r21)*(sj6));
10633 IkReal x2403=((IkReal(1.00000000000000))*(cj6)*(r20));
10634 if( IKabs(((((sj5)*(x2402)))+(((IkReal(-1.00000000000000))*(sj5)*(x2403)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x2401)*(x2403)))+(((cj4)*(r22)*(sj5)))+(((x2401)*(x2402)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x2402)))+(((IkReal(-1.00000000000000))*(sj5)*(x2403)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((IkReal(-1.00000000000000))*(x2401)*(x2403)))+(((cj4)*(r22)*(sj5)))+(((x2401)*(x2402)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
10635     continue;
10636 j3array[0]=IKatan2(((((sj5)*(x2402)))+(((IkReal(-1.00000000000000))*(sj5)*(x2403)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((IkReal(-1.00000000000000))*(x2401)*(x2403)))+(((cj4)*(r22)*(sj5)))+(((x2401)*(x2402)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))));
10637 sj3array[0]=IKsin(j3array[0]);
10638 cj3array[0]=IKcos(j3array[0]);
10639 if( j3array[0] > IKPI )
10640 {
10641     j3array[0]-=IK2PI;
10642 }
10643 else if( j3array[0] < -IKPI )
10644 {    j3array[0]+=IK2PI;
10645 }
10646 j3valid[0] = true;
10647 for(int ij3 = 0; ij3 < 1; ++ij3)
10648 {
10649 if( !j3valid[ij3] )
10650 {
10651     continue;
10652 }
10653 _ij3[0] = ij3; _ij3[1] = -1;
10654 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10655 {
10656 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10657 {
10658     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10659 }
10660 }
10661 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10662 {
10663 IkReal evalcond[6];
10664 IkReal x2404=IKcos(j3);
10665 IkReal x2405=IKsin(j3);
10666 IkReal x2406=((r11)*(sj6));
10667 IkReal x2407=((IkReal(1.00000000000000))*(sj5));
10668 IkReal x2408=((IkReal(1.00000000000000))*(cj1));
10669 IkReal x2409=((cj6)*(sj4));
10670 IkReal x2410=((sj4)*(sj6));
10671 IkReal x2411=((cj4)*(sj5));
10672 IkReal x2412=((cj4)*(cj5));
10673 IkReal x2413=((r01)*(sj6));
10674 IkReal x2414=((r21)*(sj6));
10675 IkReal x2415=((cj6)*(sj5));
10676 IkReal x2416=((IkReal(1.00000000000000))*(cj6)*(x2412));
10677 evalcond[0]=((((cj5)*(r22)))+(x2405)+(((IkReal(-1.00000000000000))*(x2407)*(x2414)))+(((r20)*(x2415))));
10678 evalcond[1]=((((IkReal(-1.00000000000000))*(x2407)*(x2413)))+(((r00)*(x2415)))+(((sj1)*(x2404)))+(((cj5)*(r02))));
10679 evalcond[2]=((((IkReal(-1.00000000000000))*(x2404)*(x2408)))+(((cj5)*(r12)))+(((r10)*(x2415)))+(((IkReal(-1.00000000000000))*(x2406)*(x2407))));
10680 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x2416)))+(((r22)*(x2411)))+(((IkReal(-1.00000000000000))*(x2404)))+(((r20)*(x2410)))+(((r21)*(x2409)))+(((x2412)*(x2414))));
10681 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x2416)))+(((r01)*(x2409)))+(((r00)*(x2410)))+(((r02)*(x2411)))+(((sj1)*(x2405)))+(((x2412)*(x2413))));
10682 evalcond[5]=((((IkReal(-1.00000000000000))*(x2405)*(x2408)))+(((r10)*(x2410)))+(((x2406)*(x2412)))+(((r11)*(x2409)))+(((IkReal(-1.00000000000000))*(r10)*(x2416)))+(((r12)*(x2411))));
10683 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
10684 {
10685 continue;
10686 }
10687 }
10688 
10689 {
10690 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10691 vinfos[0].jointtype = 1;
10692 vinfos[0].foffset = j0;
10693 vinfos[0].indices[0] = _ij0[0];
10694 vinfos[0].indices[1] = _ij0[1];
10695 vinfos[0].maxsolutions = _nj0;
10696 vinfos[1].jointtype = 1;
10697 vinfos[1].foffset = j1;
10698 vinfos[1].indices[0] = _ij1[0];
10699 vinfos[1].indices[1] = _ij1[1];
10700 vinfos[1].maxsolutions = _nj1;
10701 vinfos[2].jointtype = 1;
10702 vinfos[2].foffset = j2;
10703 vinfos[2].indices[0] = _ij2[0];
10704 vinfos[2].indices[1] = _ij2[1];
10705 vinfos[2].maxsolutions = _nj2;
10706 vinfos[3].jointtype = 1;
10707 vinfos[3].foffset = j3;
10708 vinfos[3].indices[0] = _ij3[0];
10709 vinfos[3].indices[1] = _ij3[1];
10710 vinfos[3].maxsolutions = _nj3;
10711 vinfos[4].jointtype = 1;
10712 vinfos[4].foffset = j4;
10713 vinfos[4].indices[0] = _ij4[0];
10714 vinfos[4].indices[1] = _ij4[1];
10715 vinfos[4].maxsolutions = _nj4;
10716 vinfos[5].jointtype = 1;
10717 vinfos[5].foffset = j5;
10718 vinfos[5].indices[0] = _ij5[0];
10719 vinfos[5].indices[1] = _ij5[1];
10720 vinfos[5].maxsolutions = _nj5;
10721 vinfos[6].jointtype = 1;
10722 vinfos[6].foffset = j6;
10723 vinfos[6].indices[0] = _ij6[0];
10724 vinfos[6].indices[1] = _ij6[1];
10725 vinfos[6].maxsolutions = _nj6;
10726 std::vector<int> vfree(0);
10727 solutions.AddSolution(vinfos,vfree);
10728 }
10729 }
10730 }
10731 
10732 } else
10733 {
10734 IkReal x2417=((sj4)*(sj5));
10735 IkReal x2418=((IkReal(0.390000000000000))*(sj5));
10736 IkReal x2419=((r21)*(sj6));
10737 IkReal x2420=((IkReal(0.390000000000000))*(cj5));
10738 IkReal x2421=((cj6)*(r00));
10739 IkReal x2422=((IkReal(0.000500000000000000))*(cj6));
10740 IkReal x2423=((r11)*(sj6));
10741 IkReal x2424=((cj5)*(sj4));
10742 IkReal x2425=((r01)*(sj6));
10743 IkReal x2426=((IkReal(1.00000000000000))*(r20));
10744 IkReal x2427=((cj4)*(sj6));
10745 IkReal x2428=((IkReal(0.000500000000000000))*(sj6));
10746 IkReal x2429=((IkReal(1.00000000000000))*(r10));
10747 IkReal x2430=((IkReal(1.00000000000000))*(cj4)*(cj6));
10748 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
10749 evalcond[1]=((((r22)*(x2420)))+(((r21)*(x2422)))+(((cj6)*(r20)*(x2418)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x2418)*(x2419)))+(((r20)*(x2428))));
10750 evalcond[2]=((((x2418)*(x2421)))+(((IkReal(0.00200000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x2428)))+(((r01)*(x2422)))+(((IkReal(-1.00000000000000))*(x2418)*(x2425)))+(((IkReal(-0.400000000000000))*(sj1)))+(((r02)*(x2420))));
10751 evalcond[3]=((((IkReal(0.400000000000000))*(cj1)))+(((r10)*(x2428)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x2422)))+(((IkReal(-1.00000000000000))*(x2418)*(x2423)))+(((cj6)*(r10)*(x2418)))+(((r12)*(x2420))));
10752 evalcond[4]=((((r22)*(x2417)))+(((IkReal(-1.00000000000000))*(cj6)*(x2424)*(x2426)))+(((x2419)*(x2424)))+(((IkReal(-1.00000000000000))*(r21)*(x2430)))+(((IkReal(-1.00000000000000))*(x2426)*(x2427))));
10753 evalcond[5]=((cj1)+(((IkReal(-1.00000000000000))*(x2421)*(x2424)))+(((IkReal(-1.00000000000000))*(r01)*(x2430)))+(((x2424)*(x2425)))+(((r02)*(x2417)))+(((IkReal(-1.00000000000000))*(r00)*(x2427))));
10754 evalcond[6]=((sj1)+(((x2423)*(x2424)))+(((IkReal(-1.00000000000000))*(cj6)*(x2424)*(x2429)))+(((IkReal(-1.00000000000000))*(r11)*(x2430)))+(((r12)*(x2417)))+(((IkReal(-1.00000000000000))*(x2427)*(x2429))));
10755 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
10756 {
10757 {
10758 IkReal j3array[1], cj3array[1], sj3array[1];
10759 bool j3valid[1]={false};
10760 _nj3 = 1;
10761 IkReal x2431=((cj4)*(cj5));
10762 IkReal x2432=((cj6)*(r20));
10763 IkReal x2433=((IkReal(1.00000000000000))*(sj4));
10764 IkReal x2434=((IkReal(1.00000000000000))*(r21)*(sj6));
10765 if( IKabs(((((cj5)*(r22)))+(((sj5)*(x2432)))+(((IkReal(-1.00000000000000))*(sj5)*(x2434))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2433)))+(((IkReal(-1.00000000000000))*(x2431)*(x2434)))+(((x2431)*(x2432)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2433))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((sj5)*(x2432)))+(((IkReal(-1.00000000000000))*(sj5)*(x2434)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2433)))+(((IkReal(-1.00000000000000))*(x2431)*(x2434)))+(((x2431)*(x2432)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2433)))))-1) <= IKFAST_SINCOS_THRESH )
10766     continue;
10767 j3array[0]=IKatan2(((((cj5)*(r22)))+(((sj5)*(x2432)))+(((IkReal(-1.00000000000000))*(sj5)*(x2434)))), ((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2433)))+(((IkReal(-1.00000000000000))*(x2431)*(x2434)))+(((x2431)*(x2432)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2433)))));
10768 sj3array[0]=IKsin(j3array[0]);
10769 cj3array[0]=IKcos(j3array[0]);
10770 if( j3array[0] > IKPI )
10771 {
10772     j3array[0]-=IK2PI;
10773 }
10774 else if( j3array[0] < -IKPI )
10775 {    j3array[0]+=IK2PI;
10776 }
10777 j3valid[0] = true;
10778 for(int ij3 = 0; ij3 < 1; ++ij3)
10779 {
10780 if( !j3valid[ij3] )
10781 {
10782     continue;
10783 }
10784 _ij3[0] = ij3; _ij3[1] = -1;
10785 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10786 {
10787 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10788 {
10789     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10790 }
10791 }
10792 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10793 {
10794 IkReal evalcond[6];
10795 IkReal x2435=IKcos(j3);
10796 IkReal x2436=IKsin(j3);
10797 IkReal x2437=((r11)*(sj6));
10798 IkReal x2438=((IkReal(1.00000000000000))*(sj5));
10799 IkReal x2439=((IkReal(1.00000000000000))*(cj1));
10800 IkReal x2440=((sj4)*(sj6));
10801 IkReal x2441=((cj4)*(sj5));
10802 IkReal x2442=((cj6)*(sj4));
10803 IkReal x2443=((cj4)*(cj5));
10804 IkReal x2444=((r01)*(sj6));
10805 IkReal x2445=((r21)*(sj6));
10806 IkReal x2446=((cj6)*(sj5));
10807 IkReal x2447=((IkReal(1.00000000000000))*(cj6)*(x2443));
10808 evalcond[0]=((((IkReal(-1.00000000000000))*(x2436)))+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2438)*(x2445)))+(((r20)*(x2446))));
10809 evalcond[1]=((((r00)*(x2446)))+(((IkReal(-1.00000000000000))*(x2438)*(x2444)))+(((sj1)*(x2435)))+(((cj5)*(r02))));
10810 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x2446)))+(((IkReal(-1.00000000000000))*(x2435)*(x2439)))+(((IkReal(-1.00000000000000))*(x2437)*(x2438))));
10811 evalcond[3]=((x2435)+(((IkReal(-1.00000000000000))*(r20)*(x2447)))+(((r22)*(x2441)))+(((x2443)*(x2445)))+(((r20)*(x2440)))+(((r21)*(x2442))));
10812 evalcond[4]=((((r02)*(x2441)))+(((x2443)*(x2444)))+(((r00)*(x2440)))+(((r01)*(x2442)))+(((sj1)*(x2436)))+(((IkReal(-1.00000000000000))*(r00)*(x2447))));
10813 evalcond[5]=((((r10)*(x2440)))+(((IkReal(-1.00000000000000))*(r10)*(x2447)))+(((IkReal(-1.00000000000000))*(x2436)*(x2439)))+(((r12)*(x2441)))+(((x2437)*(x2443)))+(((r11)*(x2442))));
10814 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
10815 {
10816 continue;
10817 }
10818 }
10819 
10820 {
10821 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10822 vinfos[0].jointtype = 1;
10823 vinfos[0].foffset = j0;
10824 vinfos[0].indices[0] = _ij0[0];
10825 vinfos[0].indices[1] = _ij0[1];
10826 vinfos[0].maxsolutions = _nj0;
10827 vinfos[1].jointtype = 1;
10828 vinfos[1].foffset = j1;
10829 vinfos[1].indices[0] = _ij1[0];
10830 vinfos[1].indices[1] = _ij1[1];
10831 vinfos[1].maxsolutions = _nj1;
10832 vinfos[2].jointtype = 1;
10833 vinfos[2].foffset = j2;
10834 vinfos[2].indices[0] = _ij2[0];
10835 vinfos[2].indices[1] = _ij2[1];
10836 vinfos[2].maxsolutions = _nj2;
10837 vinfos[3].jointtype = 1;
10838 vinfos[3].foffset = j3;
10839 vinfos[3].indices[0] = _ij3[0];
10840 vinfos[3].indices[1] = _ij3[1];
10841 vinfos[3].maxsolutions = _nj3;
10842 vinfos[4].jointtype = 1;
10843 vinfos[4].foffset = j4;
10844 vinfos[4].indices[0] = _ij4[0];
10845 vinfos[4].indices[1] = _ij4[1];
10846 vinfos[4].maxsolutions = _nj4;
10847 vinfos[5].jointtype = 1;
10848 vinfos[5].foffset = j5;
10849 vinfos[5].indices[0] = _ij5[0];
10850 vinfos[5].indices[1] = _ij5[1];
10851 vinfos[5].maxsolutions = _nj5;
10852 vinfos[6].jointtype = 1;
10853 vinfos[6].foffset = j6;
10854 vinfos[6].indices[0] = _ij6[0];
10855 vinfos[6].indices[1] = _ij6[1];
10856 vinfos[6].maxsolutions = _nj6;
10857 std::vector<int> vfree(0);
10858 solutions.AddSolution(vinfos,vfree);
10859 }
10860 }
10861 }
10862 
10863 } else
10864 {
10865 if( 1 )
10866 {
10867 continue;
10868 
10869 } else
10870 {
10871 }
10872 }
10873 }
10874 }
10875 }
10876 }
10877 }
10878 }
10879 
10880 } else
10881 {
10882 {
10883 IkReal j3array[1], cj3array[1], sj3array[1];
10884 bool j3valid[1]={false};
10885 _nj3 = 1;
10886 IkReal x2448=((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30);
10887 IkReal x2449=((IkReal(1.00000000000000))*(sj5));
10888 IkReal x2450=((cj2)*(sj1));
10889 IkReal x2451=((r21)*(sj6));
10890 IkReal x2452=((cj6)*(r20));
10891 IkReal x2453=((cj5)*(r22));
10892 if( IKabs(((x2448)*(((((IkReal(-1.00000000000000))*(x2453)))+(((IkReal(-1.00000000000000))*(x2449)*(x2452)))+(((sj5)*(x2451))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x2448)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2450)*(x2452)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2449)))+(((IkReal(-1.00000000000000))*(x2449)*(x2450)*(x2451)))+(((x2450)*(x2453)))+(((cj6)*(r10)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x2448)*(((((IkReal(-1.00000000000000))*(x2453)))+(((IkReal(-1.00000000000000))*(x2449)*(x2452)))+(((sj5)*(x2451)))))))+IKsqr(((x2448)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2450)*(x2452)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2449)))+(((IkReal(-1.00000000000000))*(x2449)*(x2450)*(x2451)))+(((x2450)*(x2453)))+(((cj6)*(r10)*(sj2)*(sj5)))))))-1) <= IKFAST_SINCOS_THRESH )
10893     continue;
10894 j3array[0]=IKatan2(((x2448)*(((((IkReal(-1.00000000000000))*(x2453)))+(((IkReal(-1.00000000000000))*(x2449)*(x2452)))+(((sj5)*(x2451)))))), ((x2448)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2450)*(x2452)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2449)))+(((IkReal(-1.00000000000000))*(x2449)*(x2450)*(x2451)))+(((x2450)*(x2453)))+(((cj6)*(r10)*(sj2)*(sj5)))))));
10895 sj3array[0]=IKsin(j3array[0]);
10896 cj3array[0]=IKcos(j3array[0]);
10897 if( j3array[0] > IKPI )
10898 {
10899     j3array[0]-=IK2PI;
10900 }
10901 else if( j3array[0] < -IKPI )
10902 {    j3array[0]+=IK2PI;
10903 }
10904 j3valid[0] = true;
10905 for(int ij3 = 0; ij3 < 1; ++ij3)
10906 {
10907 if( !j3valid[ij3] )
10908 {
10909     continue;
10910 }
10911 _ij3[0] = ij3; _ij3[1] = -1;
10912 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10913 {
10914 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10915 {
10916     j3valid[iij3]=false; _ij3[1] = iij3; break; 
10917 }
10918 }
10919 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10920 {
10921 IkReal evalcond[6];
10922 IkReal x2454=IKsin(j3);
10923 IkReal x2455=IKcos(j3);
10924 IkReal x2456=((r11)*(sj6));
10925 IkReal x2457=((IkReal(1.00000000000000))*(sj5));
10926 IkReal x2458=((sj4)*(sj6));
10927 IkReal x2459=((cj4)*(sj5));
10928 IkReal x2460=((cj6)*(sj4));
10929 IkReal x2461=((cj4)*(cj5));
10930 IkReal x2462=((r01)*(sj6));
10931 IkReal x2463=((r21)*(sj6));
10932 IkReal x2464=((cj6)*(sj5));
10933 IkReal x2465=((IkReal(1.00000000000000))*(x2455));
10934 IkReal x2466=((sj1)*(x2455));
10935 IkReal x2467=((IkReal(1.00000000000000))*(cj6)*(x2461));
10936 IkReal x2468=((IkReal(1.00000000000000))*(cj2)*(x2454));
10937 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x2454)))+(((IkReal(-1.00000000000000))*(x2457)*(x2463)))+(((r20)*(x2464))));
10938 evalcond[1]=((x2466)+(((IkReal(-1.00000000000000))*(x2457)*(x2462)))+(((IkReal(-1.00000000000000))*(cj1)*(x2468)))+(((cj5)*(r02)))+(((r00)*(x2464))));
10939 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2456)*(x2457)))+(((IkReal(-1.00000000000000))*(cj1)*(x2465)))+(((IkReal(-1.00000000000000))*(sj1)*(x2468)))+(((r10)*(x2464))));
10940 evalcond[3]=((((x2461)*(x2463)))+(((r22)*(x2459)))+(((r21)*(x2460)))+(((IkReal(-1.00000000000000))*(r20)*(x2467)))+(((IkReal(-1.00000000000000))*(sj2)*(x2465)))+(((r20)*(x2458))));
10941 evalcond[4]=((((r02)*(x2459)))+(((r01)*(x2460)))+(((IkReal(-1.00000000000000))*(r00)*(x2467)))+(((x2461)*(x2462)))+(((sj1)*(x2454)))+(((cj1)*(cj2)*(x2455)))+(((r00)*(x2458))));
10942 evalcond[5]=((((cj2)*(x2466)))+(((r10)*(x2458)))+(((r11)*(x2460)))+(((r12)*(x2459)))+(((x2456)*(x2461)))+(((IkReal(-1.00000000000000))*(r10)*(x2467)))+(((IkReal(-1.00000000000000))*(cj1)*(x2454))));
10943 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
10944 {
10945 continue;
10946 }
10947 }
10948 
10949 {
10950 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10951 vinfos[0].jointtype = 1;
10952 vinfos[0].foffset = j0;
10953 vinfos[0].indices[0] = _ij0[0];
10954 vinfos[0].indices[1] = _ij0[1];
10955 vinfos[0].maxsolutions = _nj0;
10956 vinfos[1].jointtype = 1;
10957 vinfos[1].foffset = j1;
10958 vinfos[1].indices[0] = _ij1[0];
10959 vinfos[1].indices[1] = _ij1[1];
10960 vinfos[1].maxsolutions = _nj1;
10961 vinfos[2].jointtype = 1;
10962 vinfos[2].foffset = j2;
10963 vinfos[2].indices[0] = _ij2[0];
10964 vinfos[2].indices[1] = _ij2[1];
10965 vinfos[2].maxsolutions = _nj2;
10966 vinfos[3].jointtype = 1;
10967 vinfos[3].foffset = j3;
10968 vinfos[3].indices[0] = _ij3[0];
10969 vinfos[3].indices[1] = _ij3[1];
10970 vinfos[3].maxsolutions = _nj3;
10971 vinfos[4].jointtype = 1;
10972 vinfos[4].foffset = j4;
10973 vinfos[4].indices[0] = _ij4[0];
10974 vinfos[4].indices[1] = _ij4[1];
10975 vinfos[4].maxsolutions = _nj4;
10976 vinfos[5].jointtype = 1;
10977 vinfos[5].foffset = j5;
10978 vinfos[5].indices[0] = _ij5[0];
10979 vinfos[5].indices[1] = _ij5[1];
10980 vinfos[5].maxsolutions = _nj5;
10981 vinfos[6].jointtype = 1;
10982 vinfos[6].foffset = j6;
10983 vinfos[6].indices[0] = _ij6[0];
10984 vinfos[6].indices[1] = _ij6[1];
10985 vinfos[6].maxsolutions = _nj6;
10986 std::vector<int> vfree(0);
10987 solutions.AddSolution(vinfos,vfree);
10988 }
10989 }
10990 }
10991 
10992 }
10993 
10994 }
10995 
10996 } else
10997 {
10998 {
10999 IkReal j3array[1], cj3array[1], sj3array[1];
11000 bool j3valid[1]={false};
11001 _nj3 = 1;
11002 IkReal x2469=(sj1)*(sj1);
11003 IkReal x2470=(cj1)*(cj1);
11004 IkReal x2471=((cj5)*(r12));
11005 IkReal x2472=((cj6)*(sj5));
11006 IkReal x2473=((IkReal(1.00000000000000))*(sj1));
11007 IkReal x2474=((cj5)*(r02));
11008 IkReal x2475=((IkReal(1.00000000000000))*(cj1));
11009 IkReal x2476=((r01)*(sj5)*(sj6));
11010 IkReal x2477=((r11)*(sj5)*(sj6));
11011 if( IKabs(((((IKabs(((((cj2)*(x2470)))+(((cj2)*(x2469))))) != 0)?((IkReal)1/(((((cj2)*(x2470)))+(((cj2)*(x2469)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x2472)))+(((IkReal(-1.00000000000000))*(x2473)*(x2477)))+(((sj1)*(x2471)))+(((cj1)*(r00)*(x2472)))+(((IkReal(-1.00000000000000))*(x2475)*(x2476)))+(((cj1)*(x2474))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x2469)+(x2470))) != 0)?((IkReal)1/(((x2469)+(x2470)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2473)*(x2474)))+(((sj1)*(x2476)))+(((IkReal(-1.00000000000000))*(x2475)*(x2477)))+(((cj1)*(r10)*(x2472)))+(((IkReal(-1.00000000000000))*(r00)*(x2472)*(x2473)))+(((cj1)*(x2471))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x2470)))+(((cj2)*(x2469))))) != 0)?((IkReal)1/(((((cj2)*(x2470)))+(((cj2)*(x2469)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x2472)))+(((IkReal(-1.00000000000000))*(x2473)*(x2477)))+(((sj1)*(x2471)))+(((cj1)*(r00)*(x2472)))+(((IkReal(-1.00000000000000))*(x2475)*(x2476)))+(((cj1)*(x2474)))))))+IKsqr(((((IKabs(((x2469)+(x2470))) != 0)?((IkReal)1/(((x2469)+(x2470)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2473)*(x2474)))+(((sj1)*(x2476)))+(((IkReal(-1.00000000000000))*(x2475)*(x2477)))+(((cj1)*(r10)*(x2472)))+(((IkReal(-1.00000000000000))*(r00)*(x2472)*(x2473)))+(((cj1)*(x2471)))))))-1) <= IKFAST_SINCOS_THRESH )
11012     continue;
11013 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x2470)))+(((cj2)*(x2469))))) != 0)?((IkReal)1/(((((cj2)*(x2470)))+(((cj2)*(x2469)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x2472)))+(((IkReal(-1.00000000000000))*(x2473)*(x2477)))+(((sj1)*(x2471)))+(((cj1)*(r00)*(x2472)))+(((IkReal(-1.00000000000000))*(x2475)*(x2476)))+(((cj1)*(x2474)))))), ((((IKabs(((x2469)+(x2470))) != 0)?((IkReal)1/(((x2469)+(x2470)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2473)*(x2474)))+(((sj1)*(x2476)))+(((IkReal(-1.00000000000000))*(x2475)*(x2477)))+(((cj1)*(r10)*(x2472)))+(((IkReal(-1.00000000000000))*(r00)*(x2472)*(x2473)))+(((cj1)*(x2471)))))));
11014 sj3array[0]=IKsin(j3array[0]);
11015 cj3array[0]=IKcos(j3array[0]);
11016 if( j3array[0] > IKPI )
11017 {
11018     j3array[0]-=IK2PI;
11019 }
11020 else if( j3array[0] < -IKPI )
11021 {    j3array[0]+=IK2PI;
11022 }
11023 j3valid[0] = true;
11024 for(int ij3 = 0; ij3 < 1; ++ij3)
11025 {
11026 if( !j3valid[ij3] )
11027 {
11028     continue;
11029 }
11030 _ij3[0] = ij3; _ij3[1] = -1;
11031 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11032 {
11033 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11034 {
11035     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11036 }
11037 }
11038 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11039 {
11040 IkReal evalcond[6];
11041 IkReal x2478=IKsin(j3);
11042 IkReal x2479=IKcos(j3);
11043 IkReal x2480=((r11)*(sj6));
11044 IkReal x2481=((IkReal(1.00000000000000))*(sj5));
11045 IkReal x2482=((sj4)*(sj6));
11046 IkReal x2483=((cj4)*(sj5));
11047 IkReal x2484=((cj6)*(sj4));
11048 IkReal x2485=((cj4)*(cj5));
11049 IkReal x2486=((r01)*(sj6));
11050 IkReal x2487=((r21)*(sj6));
11051 IkReal x2488=((cj6)*(sj5));
11052 IkReal x2489=((IkReal(1.00000000000000))*(x2479));
11053 IkReal x2490=((sj1)*(x2479));
11054 IkReal x2491=((IkReal(1.00000000000000))*(cj6)*(x2485));
11055 IkReal x2492=((IkReal(1.00000000000000))*(cj2)*(x2478));
11056 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x2478)))+(((IkReal(-1.00000000000000))*(x2481)*(x2487)))+(((r20)*(x2488))));
11057 evalcond[1]=((x2490)+(((IkReal(-1.00000000000000))*(cj1)*(x2492)))+(((IkReal(-1.00000000000000))*(x2481)*(x2486)))+(((cj5)*(r02)))+(((r00)*(x2488))));
11058 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x2492)))+(((IkReal(-1.00000000000000))*(cj1)*(x2489)))+(((IkReal(-1.00000000000000))*(x2480)*(x2481)))+(((r10)*(x2488))));
11059 evalcond[3]=((((x2485)*(x2487)))+(((r22)*(x2483)))+(((IkReal(-1.00000000000000))*(sj2)*(x2489)))+(((IkReal(-1.00000000000000))*(r20)*(x2491)))+(((r21)*(x2484)))+(((r20)*(x2482))));
11060 evalcond[4]=((((r01)*(x2484)))+(((x2485)*(x2486)))+(((r02)*(x2483)))+(((IkReal(-1.00000000000000))*(r00)*(x2491)))+(((sj1)*(x2478)))+(((cj1)*(cj2)*(x2479)))+(((r00)*(x2482))));
11061 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x2491)))+(((cj2)*(x2490)))+(((x2480)*(x2485)))+(((IkReal(-1.00000000000000))*(cj1)*(x2478)))+(((r11)*(x2484)))+(((r12)*(x2483)))+(((r10)*(x2482))));
11062 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
11063 {
11064 continue;
11065 }
11066 }
11067 
11068 {
11069 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11070 vinfos[0].jointtype = 1;
11071 vinfos[0].foffset = j0;
11072 vinfos[0].indices[0] = _ij0[0];
11073 vinfos[0].indices[1] = _ij0[1];
11074 vinfos[0].maxsolutions = _nj0;
11075 vinfos[1].jointtype = 1;
11076 vinfos[1].foffset = j1;
11077 vinfos[1].indices[0] = _ij1[0];
11078 vinfos[1].indices[1] = _ij1[1];
11079 vinfos[1].maxsolutions = _nj1;
11080 vinfos[2].jointtype = 1;
11081 vinfos[2].foffset = j2;
11082 vinfos[2].indices[0] = _ij2[0];
11083 vinfos[2].indices[1] = _ij2[1];
11084 vinfos[2].maxsolutions = _nj2;
11085 vinfos[3].jointtype = 1;
11086 vinfos[3].foffset = j3;
11087 vinfos[3].indices[0] = _ij3[0];
11088 vinfos[3].indices[1] = _ij3[1];
11089 vinfos[3].maxsolutions = _nj3;
11090 vinfos[4].jointtype = 1;
11091 vinfos[4].foffset = j4;
11092 vinfos[4].indices[0] = _ij4[0];
11093 vinfos[4].indices[1] = _ij4[1];
11094 vinfos[4].maxsolutions = _nj4;
11095 vinfos[5].jointtype = 1;
11096 vinfos[5].foffset = j5;
11097 vinfos[5].indices[0] = _ij5[0];
11098 vinfos[5].indices[1] = _ij5[1];
11099 vinfos[5].maxsolutions = _nj5;
11100 vinfos[6].jointtype = 1;
11101 vinfos[6].foffset = j6;
11102 vinfos[6].indices[0] = _ij6[0];
11103 vinfos[6].indices[1] = _ij6[1];
11104 vinfos[6].maxsolutions = _nj6;
11105 std::vector<int> vfree(0);
11106 solutions.AddSolution(vinfos,vfree);
11107 }
11108 }
11109 }
11110 
11111 }
11112 
11113 }
11114 
11115 } else
11116 {
11117 {
11118 IkReal j3array[1], cj3array[1], sj3array[1];
11119 bool j3valid[1]={false};
11120 _nj3 = 1;
11121 IkReal x2493=((cj4)*(cj5));
11122 IkReal x2494=((r21)*(sj6));
11123 IkReal x2495=((IkReal(1.00000000000000))*(cj6)*(r20));
11124 if( IKabs(((gconst4)*(((((sj5)*(x2494)))+(((IkReal(-1.00000000000000))*(sj5)*(x2495)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x2493)*(x2495)))+(((x2493)*(x2494)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
11125     continue;
11126 j3array[0]=IKatan2(((gconst4)*(((((sj5)*(x2494)))+(((IkReal(-1.00000000000000))*(sj5)*(x2495)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))), ((gconst4)*(((((IkReal(-1.00000000000000))*(x2493)*(x2495)))+(((x2493)*(x2494)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))))));
11127 sj3array[0]=IKsin(j3array[0]);
11128 cj3array[0]=IKcos(j3array[0]);
11129 if( j3array[0] > IKPI )
11130 {
11131     j3array[0]-=IK2PI;
11132 }
11133 else if( j3array[0] < -IKPI )
11134 {    j3array[0]+=IK2PI;
11135 }
11136 j3valid[0] = true;
11137 for(int ij3 = 0; ij3 < 1; ++ij3)
11138 {
11139 if( !j3valid[ij3] )
11140 {
11141     continue;
11142 }
11143 _ij3[0] = ij3; _ij3[1] = -1;
11144 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11145 {
11146 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11147 {
11148     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11149 }
11150 }
11151 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11152 {
11153 IkReal evalcond[6];
11154 IkReal x2496=IKsin(j3);
11155 IkReal x2497=IKcos(j3);
11156 IkReal x2498=((r11)*(sj6));
11157 IkReal x2499=((IkReal(1.00000000000000))*(sj5));
11158 IkReal x2500=((sj4)*(sj6));
11159 IkReal x2501=((cj4)*(sj5));
11160 IkReal x2502=((cj6)*(sj4));
11161 IkReal x2503=((cj4)*(cj5));
11162 IkReal x2504=((r01)*(sj6));
11163 IkReal x2505=((r21)*(sj6));
11164 IkReal x2506=((cj6)*(sj5));
11165 IkReal x2507=((IkReal(1.00000000000000))*(x2497));
11166 IkReal x2508=((sj1)*(x2497));
11167 IkReal x2509=((IkReal(1.00000000000000))*(cj6)*(x2503));
11168 IkReal x2510=((IkReal(1.00000000000000))*(cj2)*(x2496));
11169 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x2496)))+(((IkReal(-1.00000000000000))*(x2499)*(x2505)))+(((r20)*(x2506))));
11170 evalcond[1]=((x2508)+(((IkReal(-1.00000000000000))*(x2499)*(x2504)))+(((IkReal(-1.00000000000000))*(cj1)*(x2510)))+(((r00)*(x2506)))+(((cj5)*(r02))));
11171 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x2510)))+(((r10)*(x2506)))+(((IkReal(-1.00000000000000))*(x2498)*(x2499)))+(((IkReal(-1.00000000000000))*(cj1)*(x2507))));
11172 evalcond[3]=((((r22)*(x2501)))+(((r21)*(x2502)))+(((IkReal(-1.00000000000000))*(sj2)*(x2507)))+(((r20)*(x2500)))+(((x2503)*(x2505)))+(((IkReal(-1.00000000000000))*(r20)*(x2509))));
11173 evalcond[4]=((((cj1)*(cj2)*(x2497)))+(((sj1)*(x2496)))+(((r02)*(x2501)))+(((IkReal(-1.00000000000000))*(r00)*(x2509)))+(((r00)*(x2500)))+(((x2503)*(x2504)))+(((r01)*(x2502))));
11174 evalcond[5]=((((r12)*(x2501)))+(((r10)*(x2500)))+(((IkReal(-1.00000000000000))*(cj1)*(x2496)))+(((cj2)*(x2508)))+(((IkReal(-1.00000000000000))*(r10)*(x2509)))+(((r11)*(x2502)))+(((x2498)*(x2503))));
11175 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
11176 {
11177 continue;
11178 }
11179 }
11180 
11181 {
11182 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11183 vinfos[0].jointtype = 1;
11184 vinfos[0].foffset = j0;
11185 vinfos[0].indices[0] = _ij0[0];
11186 vinfos[0].indices[1] = _ij0[1];
11187 vinfos[0].maxsolutions = _nj0;
11188 vinfos[1].jointtype = 1;
11189 vinfos[1].foffset = j1;
11190 vinfos[1].indices[0] = _ij1[0];
11191 vinfos[1].indices[1] = _ij1[1];
11192 vinfos[1].maxsolutions = _nj1;
11193 vinfos[2].jointtype = 1;
11194 vinfos[2].foffset = j2;
11195 vinfos[2].indices[0] = _ij2[0];
11196 vinfos[2].indices[1] = _ij2[1];
11197 vinfos[2].maxsolutions = _nj2;
11198 vinfos[3].jointtype = 1;
11199 vinfos[3].foffset = j3;
11200 vinfos[3].indices[0] = _ij3[0];
11201 vinfos[3].indices[1] = _ij3[1];
11202 vinfos[3].maxsolutions = _nj3;
11203 vinfos[4].jointtype = 1;
11204 vinfos[4].foffset = j4;
11205 vinfos[4].indices[0] = _ij4[0];
11206 vinfos[4].indices[1] = _ij4[1];
11207 vinfos[4].maxsolutions = _nj4;
11208 vinfos[5].jointtype = 1;
11209 vinfos[5].foffset = j5;
11210 vinfos[5].indices[0] = _ij5[0];
11211 vinfos[5].indices[1] = _ij5[1];
11212 vinfos[5].maxsolutions = _nj5;
11213 vinfos[6].jointtype = 1;
11214 vinfos[6].foffset = j6;
11215 vinfos[6].indices[0] = _ij6[0];
11216 vinfos[6].indices[1] = _ij6[1];
11217 vinfos[6].maxsolutions = _nj6;
11218 std::vector<int> vfree(0);
11219 solutions.AddSolution(vinfos,vfree);
11220 }
11221 }
11222 }
11223 
11224 }
11225 
11226 }
11227 }
11228 }
11229 
11230 }
11231 
11232 }
11233 
11234 } else
11235 {
11236 {
11237 IkReal j4array[1], cj4array[1], sj4array[1];
11238 bool j4valid[1]={false};
11239 _nj4 = 1;
11240 IkReal x2511=((cj2)*(sj6));
11241 IkReal x2512=((cj2)*(cj6));
11242 IkReal x2513=((IkReal(1.00000000000000))*(cj1)*(sj2));
11243 if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2513)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2513)))+(((r01)*(x2512)))+(((r00)*(x2511))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(cj5)*(r00)*(x2512)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2513)))+(((cj5)*(r01)*(x2511)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2513)))+(((cj2)*(r02)*(sj5)))+(((cj1)*(cj5)*(cj6)*(r20)*(sj2))))))) < IKFAST_ATAN2_MAGTHRESH )
11244     continue;
11245 j4array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2513)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2513)))+(((r01)*(x2512)))+(((r00)*(x2511)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(cj5)*(r00)*(x2512)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2513)))+(((cj5)*(r01)*(x2511)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2513)))+(((cj2)*(r02)*(sj5)))+(((cj1)*(cj5)*(cj6)*(r20)*(sj2)))))));
11246 sj4array[0]=IKsin(j4array[0]);
11247 cj4array[0]=IKcos(j4array[0]);
11248 if( j4array[0] > IKPI )
11249 {
11250     j4array[0]-=IK2PI;
11251 }
11252 else if( j4array[0] < -IKPI )
11253 {    j4array[0]+=IK2PI;
11254 }
11255 j4valid[0] = true;
11256 for(int ij4 = 0; ij4 < 1; ++ij4)
11257 {
11258 if( !j4valid[ij4] )
11259 {
11260     continue;
11261 }
11262 _ij4[0] = ij4; _ij4[1] = -1;
11263 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11264 {
11265 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11266 {
11267     j4valid[iij4]=false; _ij4[1] = iij4; break; 
11268 }
11269 }
11270 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11271 {
11272 IkReal evalcond[3];
11273 IkReal x2514=IKsin(j4);
11274 IkReal x2515=IKcos(j4);
11275 IkReal x2516=((IkReal(1.00000000000000))*(sj2));
11276 IkReal x2517=((IkReal(1.00000000000000))*(cj6));
11277 IkReal x2518=((sj5)*(x2514));
11278 IkReal x2519=((cj5)*(x2514));
11279 IkReal x2520=((IkReal(1.00000000000000))*(sj6)*(x2515));
11280 evalcond[0]=((((r21)*(sj6)*(x2519)))+(((IkReal(-1.00000000000000))*(r21)*(x2515)*(x2517)))+(((r22)*(x2518)))+(((IkReal(-1.00000000000000))*(r20)*(x2520)))+(((IkReal(-1.00000000000000))*(r20)*(x2517)*(x2519)))+(((IkReal(-1.00000000000000))*(cj2))));
11281 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x2517)*(x2519)))+(((r01)*(sj6)*(x2519)))+(((r02)*(x2518)))+(((IkReal(-1.00000000000000))*(r00)*(x2520)))+(((IkReal(-1.00000000000000))*(cj1)*(x2516)))+(((IkReal(-1.00000000000000))*(r01)*(x2515)*(x2517))));
11282 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)*(x2516)))+(((IkReal(-1.00000000000000))*(r10)*(x2517)*(x2519)))+(((r11)*(sj6)*(x2519)))+(((IkReal(-1.00000000000000))*(r11)*(x2515)*(x2517)))+(((r12)*(x2518)))+(((IkReal(-1.00000000000000))*(r10)*(x2520))));
11283 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
11284 {
11285 continue;
11286 }
11287 }
11288 
11289 {
11290 IkReal dummyeval[1];
11291 IkReal gconst4;
11292 gconst4=IKsign(sj2);
11293 dummyeval[0]=sj2;
11294 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
11295 {
11296 {
11297 IkReal dummyeval[2];
11298 IkReal x2521=(sj1)*(sj1);
11299 IkReal x2522=(cj1)*(cj1);
11300 dummyeval[0]=((((cj2)*(x2522)))+(((cj2)*(x2521))));
11301 dummyeval[1]=((x2521)+(x2522));
11302 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
11303 {
11304 {
11305 IkReal dummyeval[2];
11306 dummyeval[0]=sj2;
11307 dummyeval[1]=cj1;
11308 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
11309 {
11310 {
11311 IkReal evalcond[9];
11312 IkReal x2523=((sj4)*(sj5));
11313 IkReal x2524=((cj6)*(r20));
11314 IkReal x2525=((IkReal(1.00000000000000))*(cj4));
11315 IkReal x2526=((IkReal(0.390000000000000))*(sj5));
11316 IkReal x2527=((r21)*(sj6));
11317 IkReal x2528=((IkReal(0.390000000000000))*(cj5));
11318 IkReal x2529=((cj6)*(r00));
11319 IkReal x2530=((IkReal(0.000500000000000000))*(cj6));
11320 IkReal x2531=((r11)*(sj6));
11321 IkReal x2532=((cj5)*(sj4));
11322 IkReal x2533=((r01)*(sj6));
11323 IkReal x2534=((r20)*(sj6));
11324 IkReal x2535=((r10)*(sj6));
11325 IkReal x2536=((cj6)*(r10));
11326 IkReal x2537=((cj6)*(r21));
11327 IkReal x2538=((r00)*(sj6));
11328 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
11329 evalcond[1]=((((cj5)*(r22)))+(((sj5)*(x2524)))+(((IkReal(-1.00000000000000))*(sj5)*(x2527))));
11330 evalcond[2]=((IkReal(-0.00200000000000000))+(((r22)*(x2528)))+(((r21)*(x2530)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(0.000500000000000000))*(x2534)))+(((IkReal(-1.00000000000000))*(x2526)*(x2527)))+(((x2524)*(x2526))));
11331 evalcond[3]=((((r02)*(x2528)))+(((r01)*(x2530)))+(((IkReal(-1.00000000000000))*(x2526)*(x2533)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(0.000500000000000000))*(x2538)))+(((x2526)*(x2529)))+(((IkReal(-0.400000000000000))*(sj1))));
11332 evalcond[4]=((((x2526)*(x2536)))+(((r12)*(x2528)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x2526)*(x2531)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.000500000000000000))*(x2535)))+(((r11)*(x2530))));
11333 evalcond[5]=((IkReal(-1.00000000000000))+(((r22)*(x2523)))+(((IkReal(-1.00000000000000))*(x2525)*(x2534)))+(((IkReal(-1.00000000000000))*(x2525)*(x2537)))+(((x2527)*(x2532)))+(((IkReal(-1.00000000000000))*(x2524)*(x2532))));
11334 evalcond[6]=((((cj4)*(cj5)*(x2527)))+(((IkReal(-1.00000000000000))*(cj5)*(x2524)*(x2525)))+(((cj4)*(r22)*(sj5)))+(((sj4)*(x2534)))+(((sj4)*(x2537))));
11335 evalcond[7]=((((r02)*(x2523)))+(((IkReal(-1.00000000000000))*(x2525)*(x2538)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2525)))+(((IkReal(-1.00000000000000))*(x2529)*(x2532)))+(((x2532)*(x2533))));
11336 evalcond[8]=((((x2531)*(x2532)))+(((IkReal(-1.00000000000000))*(x2525)*(x2535)))+(((IkReal(-1.00000000000000))*(x2532)*(x2536)))+(((r12)*(x2523)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x2525))));
11337 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
11338 {
11339 {
11340 IkReal dummyeval[1];
11341 IkReal gconst5;
11342 gconst5=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
11343 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
11344 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
11345 {
11346 {
11347 IkReal dummyeval[1];
11348 IkReal gconst6;
11349 gconst6=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
11350 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
11351 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
11352 {
11353 continue;
11354 
11355 } else
11356 {
11357 {
11358 IkReal j3array[1], cj3array[1], sj3array[1];
11359 bool j3valid[1]={false};
11360 _nj3 = 1;
11361 IkReal x2539=((IkReal(1.00000000000000))*(sj4));
11362 IkReal x2540=((r00)*(sj6));
11363 IkReal x2541=((cj1)*(cj6));
11364 IkReal x2542=((cj4)*(sj5));
11365 IkReal x2543=((cj4)*(cj5));
11366 IkReal x2544=((r01)*(sj1));
11367 IkReal x2545=((IkReal(1.00000000000000))*(sj6));
11368 IkReal x2546=((cj1)*(r02));
11369 IkReal x2547=((cj1)*(r01));
11370 IkReal x2548=((IkReal(1.00000000000000))*(r02)*(sj1));
11371 IkReal x2549=((cj6)*(r00)*(sj1));
11372 if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x2542)*(x2548)))+(((IkReal(-1.00000000000000))*(sj1)*(x2539)*(x2540)))+(((x2543)*(x2549)))+(((cj5)*(x2546)))+(((IkReal(-1.00000000000000))*(sj5)*(x2545)*(x2547)))+(((r00)*(sj5)*(x2541)))+(((IkReal(-1.00000000000000))*(x2543)*(x2544)*(x2545)))+(((IkReal(-1.00000000000000))*(cj6)*(x2539)*(x2544))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(r01)*(x2539)*(x2541)))+(((sj5)*(sj6)*(x2544)))+(((IkReal(-1.00000000000000))*(cj1)*(x2539)*(x2540)))+(((IkReal(-1.00000000000000))*(x2543)*(x2545)*(x2547)))+(((IkReal(-1.00000000000000))*(sj5)*(x2549)))+(((IkReal(-1.00000000000000))*(x2542)*(x2546)))+(((IkReal(-1.00000000000000))*(cj5)*(x2548)))+(((r00)*(x2541)*(x2543))))))) < IKFAST_ATAN2_MAGTHRESH )
11373     continue;
11374 j3array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(x2542)*(x2548)))+(((IkReal(-1.00000000000000))*(sj1)*(x2539)*(x2540)))+(((x2543)*(x2549)))+(((cj5)*(x2546)))+(((IkReal(-1.00000000000000))*(sj5)*(x2545)*(x2547)))+(((r00)*(sj5)*(x2541)))+(((IkReal(-1.00000000000000))*(x2543)*(x2544)*(x2545)))+(((IkReal(-1.00000000000000))*(cj6)*(x2539)*(x2544)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(r01)*(x2539)*(x2541)))+(((sj5)*(sj6)*(x2544)))+(((IkReal(-1.00000000000000))*(cj1)*(x2539)*(x2540)))+(((IkReal(-1.00000000000000))*(x2543)*(x2545)*(x2547)))+(((IkReal(-1.00000000000000))*(sj5)*(x2549)))+(((IkReal(-1.00000000000000))*(x2542)*(x2546)))+(((IkReal(-1.00000000000000))*(cj5)*(x2548)))+(((r00)*(x2541)*(x2543)))))));
11375 sj3array[0]=IKsin(j3array[0]);
11376 cj3array[0]=IKcos(j3array[0]);
11377 if( j3array[0] > IKPI )
11378 {
11379     j3array[0]-=IK2PI;
11380 }
11381 else if( j3array[0] < -IKPI )
11382 {    j3array[0]+=IK2PI;
11383 }
11384 j3valid[0] = true;
11385 for(int ij3 = 0; ij3 < 1; ++ij3)
11386 {
11387 if( !j3valid[ij3] )
11388 {
11389     continue;
11390 }
11391 _ij3[0] = ij3; _ij3[1] = -1;
11392 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11393 {
11394 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11395 {
11396     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11397 }
11398 }
11399 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11400 {
11401 IkReal evalcond[4];
11402 IkReal x2550=IKcos(j3);
11403 IkReal x2551=IKsin(j3);
11404 IkReal x2552=((r11)*(sj6));
11405 IkReal x2553=((IkReal(1.00000000000000))*(sj5));
11406 IkReal x2554=((sj4)*(sj6));
11407 IkReal x2555=((cj4)*(sj5));
11408 IkReal x2556=((cj6)*(sj4));
11409 IkReal x2557=((cj4)*(cj5));
11410 IkReal x2558=((r01)*(sj6));
11411 IkReal x2559=((cj6)*(sj5));
11412 IkReal x2560=((sj1)*(x2550));
11413 IkReal x2561=((cj1)*(x2550));
11414 IkReal x2562=((IkReal(1.00000000000000))*(x2551));
11415 IkReal x2563=((cj1)*(x2562));
11416 IkReal x2564=((IkReal(1.00000000000000))*(cj6)*(x2557));
11417 evalcond[0]=((x2560)+(((IkReal(-1.00000000000000))*(x2563)))+(((r00)*(x2559)))+(((IkReal(-1.00000000000000))*(x2553)*(x2558)))+(((cj5)*(r02))));
11418 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2561)))+(((IkReal(-1.00000000000000))*(x2552)*(x2553)))+(((IkReal(-1.00000000000000))*(sj1)*(x2562)))+(((r10)*(x2559))));
11419 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x2564)))+(x2561)+(((r01)*(x2556)))+(((r00)*(x2554)))+(((x2557)*(x2558)))+(((sj1)*(x2551)))+(((r02)*(x2555))));
11420 evalcond[3]=((x2560)+(((r12)*(x2555)))+(((r11)*(x2556)))+(((IkReal(-1.00000000000000))*(x2563)))+(((x2552)*(x2557)))+(((IkReal(-1.00000000000000))*(r10)*(x2564)))+(((r10)*(x2554))));
11421 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
11422 {
11423 continue;
11424 }
11425 }
11426 
11427 {
11428 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11429 vinfos[0].jointtype = 1;
11430 vinfos[0].foffset = j0;
11431 vinfos[0].indices[0] = _ij0[0];
11432 vinfos[0].indices[1] = _ij0[1];
11433 vinfos[0].maxsolutions = _nj0;
11434 vinfos[1].jointtype = 1;
11435 vinfos[1].foffset = j1;
11436 vinfos[1].indices[0] = _ij1[0];
11437 vinfos[1].indices[1] = _ij1[1];
11438 vinfos[1].maxsolutions = _nj1;
11439 vinfos[2].jointtype = 1;
11440 vinfos[2].foffset = j2;
11441 vinfos[2].indices[0] = _ij2[0];
11442 vinfos[2].indices[1] = _ij2[1];
11443 vinfos[2].maxsolutions = _nj2;
11444 vinfos[3].jointtype = 1;
11445 vinfos[3].foffset = j3;
11446 vinfos[3].indices[0] = _ij3[0];
11447 vinfos[3].indices[1] = _ij3[1];
11448 vinfos[3].maxsolutions = _nj3;
11449 vinfos[4].jointtype = 1;
11450 vinfos[4].foffset = j4;
11451 vinfos[4].indices[0] = _ij4[0];
11452 vinfos[4].indices[1] = _ij4[1];
11453 vinfos[4].maxsolutions = _nj4;
11454 vinfos[5].jointtype = 1;
11455 vinfos[5].foffset = j5;
11456 vinfos[5].indices[0] = _ij5[0];
11457 vinfos[5].indices[1] = _ij5[1];
11458 vinfos[5].maxsolutions = _nj5;
11459 vinfos[6].jointtype = 1;
11460 vinfos[6].foffset = j6;
11461 vinfos[6].indices[0] = _ij6[0];
11462 vinfos[6].indices[1] = _ij6[1];
11463 vinfos[6].maxsolutions = _nj6;
11464 std::vector<int> vfree(0);
11465 solutions.AddSolution(vinfos,vfree);
11466 }
11467 }
11468 }
11469 
11470 }
11471 
11472 }
11473 
11474 } else
11475 {
11476 {
11477 IkReal j3array[1], cj3array[1], sj3array[1];
11478 bool j3valid[1]={false};
11479 _nj3 = 1;
11480 IkReal x2565=((cj5)*(r12));
11481 IkReal x2566=((cj6)*(sj5));
11482 IkReal x2567=((IkReal(1.00000000000000))*(sj1));
11483 IkReal x2568=((cj5)*(r02));
11484 IkReal x2569=((IkReal(1.00000000000000))*(cj1));
11485 IkReal x2570=((r01)*(sj5)*(sj6));
11486 IkReal x2571=((r11)*(sj5)*(sj6));
11487 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x2569)*(x2570)))+(((cj1)*(x2568)))+(((cj1)*(r00)*(x2566)))+(((r10)*(sj1)*(x2566)))+(((IkReal(-1.00000000000000))*(x2567)*(x2571)))+(((sj1)*(x2565))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x2569)*(x2571)))+(((cj1)*(x2565)))+(((IkReal(-1.00000000000000))*(r00)*(x2566)*(x2567)))+(((cj1)*(r10)*(x2566)))+(((IkReal(-1.00000000000000))*(x2567)*(x2568)))+(((sj1)*(x2570))))))) < IKFAST_ATAN2_MAGTHRESH )
11488     continue;
11489 j3array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x2569)*(x2570)))+(((cj1)*(x2568)))+(((cj1)*(r00)*(x2566)))+(((r10)*(sj1)*(x2566)))+(((IkReal(-1.00000000000000))*(x2567)*(x2571)))+(((sj1)*(x2565)))))), ((gconst5)*(((((IkReal(-1.00000000000000))*(x2569)*(x2571)))+(((cj1)*(x2565)))+(((IkReal(-1.00000000000000))*(r00)*(x2566)*(x2567)))+(((cj1)*(r10)*(x2566)))+(((IkReal(-1.00000000000000))*(x2567)*(x2568)))+(((sj1)*(x2570)))))));
11490 sj3array[0]=IKsin(j3array[0]);
11491 cj3array[0]=IKcos(j3array[0]);
11492 if( j3array[0] > IKPI )
11493 {
11494     j3array[0]-=IK2PI;
11495 }
11496 else if( j3array[0] < -IKPI )
11497 {    j3array[0]+=IK2PI;
11498 }
11499 j3valid[0] = true;
11500 for(int ij3 = 0; ij3 < 1; ++ij3)
11501 {
11502 if( !j3valid[ij3] )
11503 {
11504     continue;
11505 }
11506 _ij3[0] = ij3; _ij3[1] = -1;
11507 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11508 {
11509 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11510 {
11511     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11512 }
11513 }
11514 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11515 {
11516 IkReal evalcond[4];
11517 IkReal x2572=IKcos(j3);
11518 IkReal x2573=IKsin(j3);
11519 IkReal x2574=((r11)*(sj6));
11520 IkReal x2575=((IkReal(1.00000000000000))*(sj5));
11521 IkReal x2576=((sj4)*(sj6));
11522 IkReal x2577=((cj4)*(sj5));
11523 IkReal x2578=((cj6)*(sj4));
11524 IkReal x2579=((cj4)*(cj5));
11525 IkReal x2580=((r01)*(sj6));
11526 IkReal x2581=((cj6)*(sj5));
11527 IkReal x2582=((sj1)*(x2572));
11528 IkReal x2583=((cj1)*(x2572));
11529 IkReal x2584=((IkReal(1.00000000000000))*(x2573));
11530 IkReal x2585=((cj1)*(x2584));
11531 IkReal x2586=((IkReal(1.00000000000000))*(cj6)*(x2579));
11532 evalcond[0]=((x2582)+(((r00)*(x2581)))+(((IkReal(-1.00000000000000))*(x2585)))+(((IkReal(-1.00000000000000))*(x2575)*(x2580)))+(((cj5)*(r02))));
11533 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2583)))+(((r10)*(x2581)))+(((IkReal(-1.00000000000000))*(sj1)*(x2584)))+(((IkReal(-1.00000000000000))*(x2574)*(x2575))));
11534 evalcond[2]=((x2583)+(((IkReal(-1.00000000000000))*(r00)*(x2586)))+(((r01)*(x2578)))+(((x2579)*(x2580)))+(((r00)*(x2576)))+(((r02)*(x2577)))+(((sj1)*(x2573))));
11535 evalcond[3]=((((r10)*(x2576)))+(x2582)+(((x2574)*(x2579)))+(((r11)*(x2578)))+(((r12)*(x2577)))+(((IkReal(-1.00000000000000))*(x2585)))+(((IkReal(-1.00000000000000))*(r10)*(x2586))));
11536 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
11537 {
11538 continue;
11539 }
11540 }
11541 
11542 {
11543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11544 vinfos[0].jointtype = 1;
11545 vinfos[0].foffset = j0;
11546 vinfos[0].indices[0] = _ij0[0];
11547 vinfos[0].indices[1] = _ij0[1];
11548 vinfos[0].maxsolutions = _nj0;
11549 vinfos[1].jointtype = 1;
11550 vinfos[1].foffset = j1;
11551 vinfos[1].indices[0] = _ij1[0];
11552 vinfos[1].indices[1] = _ij1[1];
11553 vinfos[1].maxsolutions = _nj1;
11554 vinfos[2].jointtype = 1;
11555 vinfos[2].foffset = j2;
11556 vinfos[2].indices[0] = _ij2[0];
11557 vinfos[2].indices[1] = _ij2[1];
11558 vinfos[2].maxsolutions = _nj2;
11559 vinfos[3].jointtype = 1;
11560 vinfos[3].foffset = j3;
11561 vinfos[3].indices[0] = _ij3[0];
11562 vinfos[3].indices[1] = _ij3[1];
11563 vinfos[3].maxsolutions = _nj3;
11564 vinfos[4].jointtype = 1;
11565 vinfos[4].foffset = j4;
11566 vinfos[4].indices[0] = _ij4[0];
11567 vinfos[4].indices[1] = _ij4[1];
11568 vinfos[4].maxsolutions = _nj4;
11569 vinfos[5].jointtype = 1;
11570 vinfos[5].foffset = j5;
11571 vinfos[5].indices[0] = _ij5[0];
11572 vinfos[5].indices[1] = _ij5[1];
11573 vinfos[5].maxsolutions = _nj5;
11574 vinfos[6].jointtype = 1;
11575 vinfos[6].foffset = j6;
11576 vinfos[6].indices[0] = _ij6[0];
11577 vinfos[6].indices[1] = _ij6[1];
11578 vinfos[6].maxsolutions = _nj6;
11579 std::vector<int> vfree(0);
11580 solutions.AddSolution(vinfos,vfree);
11581 }
11582 }
11583 }
11584 
11585 }
11586 
11587 }
11588 
11589 } else
11590 {
11591 IkReal x2587=((sj4)*(sj5));
11592 IkReal x2588=((cj6)*(r20));
11593 IkReal x2589=((IkReal(1.00000000000000))*(cj4));
11594 IkReal x2590=((IkReal(0.390000000000000))*(sj5));
11595 IkReal x2591=((r21)*(sj6));
11596 IkReal x2592=((IkReal(0.390000000000000))*(cj5));
11597 IkReal x2593=((cj6)*(r00));
11598 IkReal x2594=((IkReal(0.000500000000000000))*(cj6));
11599 IkReal x2595=((r11)*(sj6));
11600 IkReal x2596=((cj5)*(sj4));
11601 IkReal x2597=((r01)*(sj6));
11602 IkReal x2598=((r20)*(sj6));
11603 IkReal x2599=((r10)*(sj6));
11604 IkReal x2600=((cj6)*(r10));
11605 IkReal x2601=((cj6)*(r21));
11606 IkReal x2602=((r00)*(sj6));
11607 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
11608 evalcond[1]=((((sj5)*(x2588)))+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x2591))));
11609 evalcond[2]=((IkReal(0.00200000000000000))+(((r22)*(x2592)))+(((IkReal(-1.00000000000000))*(x2590)*(x2591)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(0.000500000000000000))*(x2598)))+(((x2588)*(x2590)))+(((r21)*(x2594))));
11610 evalcond[3]=((((IkReal(-1.00000000000000))*(x2590)*(x2597)))+(((r01)*(x2594)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(0.000500000000000000))*(x2602)))+(((r02)*(x2592)))+(((x2590)*(x2593))));
11611 evalcond[4]=((((IkReal(-1.00000000000000))*(x2590)*(x2595)))+(((IkReal(0.400000000000000))*(cj1)))+(((x2590)*(x2600)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.000500000000000000))*(x2599)))+(((r12)*(x2592)))+(((r11)*(x2594))));
11612 evalcond[5]=((IkReal(1.00000000000000))+(((r22)*(x2587)))+(((x2591)*(x2596)))+(((IkReal(-1.00000000000000))*(x2589)*(x2598)))+(((IkReal(-1.00000000000000))*(x2588)*(x2596)))+(((IkReal(-1.00000000000000))*(x2589)*(x2601))));
11613 evalcond[6]=((((IkReal(-1.00000000000000))*(cj5)*(x2588)*(x2589)))+(((cj4)*(r22)*(sj5)))+(((sj4)*(x2598)))+(((sj4)*(x2601)))+(((cj4)*(cj5)*(x2591))));
11614 evalcond[7]=((((IkReal(-1.00000000000000))*(x2593)*(x2596)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2589)))+(((IkReal(-1.00000000000000))*(x2589)*(x2602)))+(((x2596)*(x2597)))+(((r02)*(x2587))));
11615 evalcond[8]=((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x2589)))+(((IkReal(-1.00000000000000))*(x2596)*(x2600)))+(((IkReal(-1.00000000000000))*(x2589)*(x2599)))+(((r12)*(x2587)))+(((x2595)*(x2596))));
11616 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
11617 {
11618 {
11619 IkReal dummyeval[1];
11620 IkReal gconst7;
11621 gconst7=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
11622 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
11623 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
11624 {
11625 {
11626 IkReal dummyeval[1];
11627 IkReal gconst8;
11628 gconst8=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
11629 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
11630 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
11631 {
11632 continue;
11633 
11634 } else
11635 {
11636 {
11637 IkReal j3array[1], cj3array[1], sj3array[1];
11638 bool j3valid[1]={false};
11639 _nj3 = 1;
11640 IkReal x2603=((sj1)*(sj6));
11641 IkReal x2604=((r00)*(sj4));
11642 IkReal x2605=((IkReal(1.00000000000000))*(r01));
11643 IkReal x2606=((cj1)*(cj6));
11644 IkReal x2607=((cj1)*(sj6));
11645 IkReal x2608=((cj4)*(cj5));
11646 IkReal x2609=((r02)*(sj1));
11647 IkReal x2610=((cj4)*(sj5));
11648 IkReal x2611=((cj1)*(r02));
11649 IkReal x2612=((cj6)*(r00)*(sj1));
11650 if( IKabs(((gconst8)*(((((x2603)*(x2604)))+(((cj5)*(x2611)))+(((r00)*(sj5)*(x2606)))+(((r01)*(x2603)*(x2608)))+(((IkReal(-1.00000000000000))*(sj5)*(x2605)*(x2607)))+(((IkReal(-1.00000000000000))*(x2608)*(x2612)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((x2609)*(x2610))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((sj5)*(x2612)))+(((cj5)*(x2609)))+(((IkReal(-1.00000000000000))*(sj4)*(x2605)*(x2606)))+(((r00)*(x2606)*(x2608)))+(((IkReal(-1.00000000000000))*(x2604)*(x2607)))+(((IkReal(-1.00000000000000))*(x2605)*(x2607)*(x2608)))+(((IkReal(-1.00000000000000))*(sj5)*(x2603)*(x2605)))+(((IkReal(-1.00000000000000))*(x2610)*(x2611))))))) < IKFAST_ATAN2_MAGTHRESH )
11651     continue;
11652 j3array[0]=IKatan2(((gconst8)*(((((x2603)*(x2604)))+(((cj5)*(x2611)))+(((r00)*(sj5)*(x2606)))+(((r01)*(x2603)*(x2608)))+(((IkReal(-1.00000000000000))*(sj5)*(x2605)*(x2607)))+(((IkReal(-1.00000000000000))*(x2608)*(x2612)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((x2609)*(x2610)))))), ((gconst8)*(((((sj5)*(x2612)))+(((cj5)*(x2609)))+(((IkReal(-1.00000000000000))*(sj4)*(x2605)*(x2606)))+(((r00)*(x2606)*(x2608)))+(((IkReal(-1.00000000000000))*(x2604)*(x2607)))+(((IkReal(-1.00000000000000))*(x2605)*(x2607)*(x2608)))+(((IkReal(-1.00000000000000))*(sj5)*(x2603)*(x2605)))+(((IkReal(-1.00000000000000))*(x2610)*(x2611)))))));
11653 sj3array[0]=IKsin(j3array[0]);
11654 cj3array[0]=IKcos(j3array[0]);
11655 if( j3array[0] > IKPI )
11656 {
11657     j3array[0]-=IK2PI;
11658 }
11659 else if( j3array[0] < -IKPI )
11660 {    j3array[0]+=IK2PI;
11661 }
11662 j3valid[0] = true;
11663 for(int ij3 = 0; ij3 < 1; ++ij3)
11664 {
11665 if( !j3valid[ij3] )
11666 {
11667     continue;
11668 }
11669 _ij3[0] = ij3; _ij3[1] = -1;
11670 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11671 {
11672 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11673 {
11674     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11675 }
11676 }
11677 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11678 {
11679 IkReal evalcond[4];
11680 IkReal x2613=IKsin(j3);
11681 IkReal x2614=IKcos(j3);
11682 IkReal x2615=((r11)*(sj6));
11683 IkReal x2616=((IkReal(1.00000000000000))*(sj5));
11684 IkReal x2617=((sj4)*(sj6));
11685 IkReal x2618=((cj4)*(sj5));
11686 IkReal x2619=((cj6)*(sj4));
11687 IkReal x2620=((cj4)*(cj5));
11688 IkReal x2621=((r01)*(sj6));
11689 IkReal x2622=((cj6)*(sj5));
11690 IkReal x2623=((sj1)*(x2613));
11691 IkReal x2624=((cj1)*(x2613));
11692 IkReal x2625=((IkReal(1.00000000000000))*(x2614));
11693 IkReal x2626=((cj1)*(x2625));
11694 IkReal x2627=((IkReal(1.00000000000000))*(cj6)*(x2620));
11695 evalcond[0]=((((r00)*(x2622)))+(((sj1)*(x2614)))+(((IkReal(-1.00000000000000))*(x2616)*(x2621)))+(x2624)+(((cj5)*(r02))));
11696 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2626)))+(((r10)*(x2622)))+(((IkReal(-1.00000000000000))*(x2615)*(x2616)))+(x2623));
11697 evalcond[2]=((((r00)*(x2617)))+(((r01)*(x2619)))+(((IkReal(-1.00000000000000))*(r00)*(x2627)))+(((r02)*(x2618)))+(((x2620)*(x2621)))+(((IkReal(-1.00000000000000))*(x2626)))+(x2623));
11698 evalcond[3]=((((IkReal(-1.00000000000000))*(x2624)))+(((x2615)*(x2620)))+(((IkReal(-1.00000000000000))*(r10)*(x2627)))+(((r11)*(x2619)))+(((r10)*(x2617)))+(((r12)*(x2618)))+(((IkReal(-1.00000000000000))*(sj1)*(x2625))));
11699 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
11700 {
11701 continue;
11702 }
11703 }
11704 
11705 {
11706 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11707 vinfos[0].jointtype = 1;
11708 vinfos[0].foffset = j0;
11709 vinfos[0].indices[0] = _ij0[0];
11710 vinfos[0].indices[1] = _ij0[1];
11711 vinfos[0].maxsolutions = _nj0;
11712 vinfos[1].jointtype = 1;
11713 vinfos[1].foffset = j1;
11714 vinfos[1].indices[0] = _ij1[0];
11715 vinfos[1].indices[1] = _ij1[1];
11716 vinfos[1].maxsolutions = _nj1;
11717 vinfos[2].jointtype = 1;
11718 vinfos[2].foffset = j2;
11719 vinfos[2].indices[0] = _ij2[0];
11720 vinfos[2].indices[1] = _ij2[1];
11721 vinfos[2].maxsolutions = _nj2;
11722 vinfos[3].jointtype = 1;
11723 vinfos[3].foffset = j3;
11724 vinfos[3].indices[0] = _ij3[0];
11725 vinfos[3].indices[1] = _ij3[1];
11726 vinfos[3].maxsolutions = _nj3;
11727 vinfos[4].jointtype = 1;
11728 vinfos[4].foffset = j4;
11729 vinfos[4].indices[0] = _ij4[0];
11730 vinfos[4].indices[1] = _ij4[1];
11731 vinfos[4].maxsolutions = _nj4;
11732 vinfos[5].jointtype = 1;
11733 vinfos[5].foffset = j5;
11734 vinfos[5].indices[0] = _ij5[0];
11735 vinfos[5].indices[1] = _ij5[1];
11736 vinfos[5].maxsolutions = _nj5;
11737 vinfos[6].jointtype = 1;
11738 vinfos[6].foffset = j6;
11739 vinfos[6].indices[0] = _ij6[0];
11740 vinfos[6].indices[1] = _ij6[1];
11741 vinfos[6].maxsolutions = _nj6;
11742 std::vector<int> vfree(0);
11743 solutions.AddSolution(vinfos,vfree);
11744 }
11745 }
11746 }
11747 
11748 }
11749 
11750 }
11751 
11752 } else
11753 {
11754 {
11755 IkReal j3array[1], cj3array[1], sj3array[1];
11756 bool j3valid[1]={false};
11757 _nj3 = 1;
11758 IkReal x2628=((sj1)*(sj5));
11759 IkReal x2629=((cj6)*(r10));
11760 IkReal x2630=((cj5)*(sj1));
11761 IkReal x2631=((cj1)*(cj5));
11762 IkReal x2632=((cj1)*(sj5));
11763 IkReal x2633=((cj6)*(r00));
11764 IkReal x2634=((r11)*(sj6));
11765 IkReal x2635=((IkReal(1.00000000000000))*(r01)*(sj6));
11766 if( IKabs(((gconst7)*(((((x2628)*(x2629)))+(((r12)*(x2630)))+(((IkReal(-1.00000000000000))*(x2632)*(x2635)))+(((x2632)*(x2633)))+(((IkReal(-1.00000000000000))*(x2628)*(x2634)))+(((r02)*(x2631))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x2631)))+(((IkReal(-1.00000000000000))*(x2628)*(x2635)))+(((IkReal(-1.00000000000000))*(x2629)*(x2632)))+(((x2632)*(x2634)))+(((x2628)*(x2633)))+(((r02)*(x2630))))))) < IKFAST_ATAN2_MAGTHRESH )
11767     continue;
11768 j3array[0]=IKatan2(((gconst7)*(((((x2628)*(x2629)))+(((r12)*(x2630)))+(((IkReal(-1.00000000000000))*(x2632)*(x2635)))+(((x2632)*(x2633)))+(((IkReal(-1.00000000000000))*(x2628)*(x2634)))+(((r02)*(x2631)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x2631)))+(((IkReal(-1.00000000000000))*(x2628)*(x2635)))+(((IkReal(-1.00000000000000))*(x2629)*(x2632)))+(((x2632)*(x2634)))+(((x2628)*(x2633)))+(((r02)*(x2630)))))));
11769 sj3array[0]=IKsin(j3array[0]);
11770 cj3array[0]=IKcos(j3array[0]);
11771 if( j3array[0] > IKPI )
11772 {
11773     j3array[0]-=IK2PI;
11774 }
11775 else if( j3array[0] < -IKPI )
11776 {    j3array[0]+=IK2PI;
11777 }
11778 j3valid[0] = true;
11779 for(int ij3 = 0; ij3 < 1; ++ij3)
11780 {
11781 if( !j3valid[ij3] )
11782 {
11783     continue;
11784 }
11785 _ij3[0] = ij3; _ij3[1] = -1;
11786 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11787 {
11788 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11789 {
11790     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11791 }
11792 }
11793 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11794 {
11795 IkReal evalcond[4];
11796 IkReal x2636=IKsin(j3);
11797 IkReal x2637=IKcos(j3);
11798 IkReal x2638=((r11)*(sj6));
11799 IkReal x2639=((IkReal(1.00000000000000))*(sj5));
11800 IkReal x2640=((sj4)*(sj6));
11801 IkReal x2641=((cj4)*(sj5));
11802 IkReal x2642=((cj6)*(sj4));
11803 IkReal x2643=((cj4)*(cj5));
11804 IkReal x2644=((r01)*(sj6));
11805 IkReal x2645=((cj6)*(sj5));
11806 IkReal x2646=((sj1)*(x2636));
11807 IkReal x2647=((cj1)*(x2636));
11808 IkReal x2648=((IkReal(1.00000000000000))*(x2637));
11809 IkReal x2649=((cj1)*(x2648));
11810 IkReal x2650=((IkReal(1.00000000000000))*(cj6)*(x2643));
11811 evalcond[0]=((((sj1)*(x2637)))+(((r00)*(x2645)))+(((IkReal(-1.00000000000000))*(x2639)*(x2644)))+(x2647)+(((cj5)*(r02))));
11812 evalcond[1]=((((cj5)*(r12)))+(((r10)*(x2645)))+(((IkReal(-1.00000000000000))*(x2638)*(x2639)))+(((IkReal(-1.00000000000000))*(x2649)))+(x2646));
11813 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x2650)))+(((r02)*(x2641)))+(((r00)*(x2640)))+(((r01)*(x2642)))+(((IkReal(-1.00000000000000))*(x2649)))+(((x2643)*(x2644)))+(x2646));
11814 evalcond[3]=((((r12)*(x2641)))+(((r11)*(x2642)))+(((x2638)*(x2643)))+(((r10)*(x2640)))+(((IkReal(-1.00000000000000))*(r10)*(x2650)))+(((IkReal(-1.00000000000000))*(x2647)))+(((IkReal(-1.00000000000000))*(sj1)*(x2648))));
11815 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
11816 {
11817 continue;
11818 }
11819 }
11820 
11821 {
11822 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11823 vinfos[0].jointtype = 1;
11824 vinfos[0].foffset = j0;
11825 vinfos[0].indices[0] = _ij0[0];
11826 vinfos[0].indices[1] = _ij0[1];
11827 vinfos[0].maxsolutions = _nj0;
11828 vinfos[1].jointtype = 1;
11829 vinfos[1].foffset = j1;
11830 vinfos[1].indices[0] = _ij1[0];
11831 vinfos[1].indices[1] = _ij1[1];
11832 vinfos[1].maxsolutions = _nj1;
11833 vinfos[2].jointtype = 1;
11834 vinfos[2].foffset = j2;
11835 vinfos[2].indices[0] = _ij2[0];
11836 vinfos[2].indices[1] = _ij2[1];
11837 vinfos[2].maxsolutions = _nj2;
11838 vinfos[3].jointtype = 1;
11839 vinfos[3].foffset = j3;
11840 vinfos[3].indices[0] = _ij3[0];
11841 vinfos[3].indices[1] = _ij3[1];
11842 vinfos[3].maxsolutions = _nj3;
11843 vinfos[4].jointtype = 1;
11844 vinfos[4].foffset = j4;
11845 vinfos[4].indices[0] = _ij4[0];
11846 vinfos[4].indices[1] = _ij4[1];
11847 vinfos[4].maxsolutions = _nj4;
11848 vinfos[5].jointtype = 1;
11849 vinfos[5].foffset = j5;
11850 vinfos[5].indices[0] = _ij5[0];
11851 vinfos[5].indices[1] = _ij5[1];
11852 vinfos[5].maxsolutions = _nj5;
11853 vinfos[6].jointtype = 1;
11854 vinfos[6].foffset = j6;
11855 vinfos[6].indices[0] = _ij6[0];
11856 vinfos[6].indices[1] = _ij6[1];
11857 vinfos[6].maxsolutions = _nj6;
11858 std::vector<int> vfree(0);
11859 solutions.AddSolution(vinfos,vfree);
11860 }
11861 }
11862 }
11863 
11864 }
11865 
11866 }
11867 
11868 } else
11869 {
11870 IkReal x2651=((sj4)*(sj5));
11871 IkReal x2652=((IkReal(0.390000000000000))*(sj5));
11872 IkReal x2653=((r21)*(sj6));
11873 IkReal x2654=((IkReal(0.390000000000000))*(cj5));
11874 IkReal x2655=((cj6)*(r00));
11875 IkReal x2656=((IkReal(0.000500000000000000))*(cj6));
11876 IkReal x2657=((r11)*(sj6));
11877 IkReal x2658=((cj5)*(sj4));
11878 IkReal x2659=((IkReal(1.00000000000000))*(r20));
11879 IkReal x2660=((cj4)*(sj6));
11880 IkReal x2661=((r01)*(sj6));
11881 IkReal x2662=((IkReal(0.000500000000000000))*(sj6));
11882 IkReal x2663=((IkReal(1.00000000000000))*(r10));
11883 IkReal x2664=((IkReal(1.00000000000000))*(cj4)*(cj6));
11884 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
11885 evalcond[1]=((((IkReal(-1.00000000000000))*(x2652)*(x2653)))+(((r21)*(x2656)))+(((cj6)*(r20)*(x2652)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x2654)))+(((IkReal(-0.00200000000000000))*(cj2)))+(((r20)*(x2662))));
11886 evalcond[2]=((IkReal(-0.400000000000000))+(((IkReal(-1.00000000000000))*(x2652)*(x2661)))+(((r02)*(x2654)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x2656)))+(((x2652)*(x2655)))+(((r00)*(x2662))));
11887 evalcond[3]=((((IkReal(-1.00000000000000))*(x2652)*(x2657)))+(((r12)*(x2654)))+(((r11)*(x2656)))+(((r10)*(x2662)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj2)))+(((cj6)*(r10)*(x2652))));
11888 evalcond[4]=((((x2653)*(x2658)))+(((r22)*(x2651)))+(((IkReal(-1.00000000000000))*(x2659)*(x2660)))+(((IkReal(-1.00000000000000))*(r21)*(x2664)))+(((IkReal(-1.00000000000000))*(cj6)*(x2658)*(x2659)))+(((IkReal(-1.00000000000000))*(cj2))));
11889 evalcond[5]=((((IkReal(-1.00000000000000))*(x2655)*(x2658)))+(((IkReal(-1.00000000000000))*(r00)*(x2660)))+(((x2658)*(x2661)))+(((IkReal(-1.00000000000000))*(r01)*(x2664)))+(((r02)*(x2651))));
11890 evalcond[6]=((((r12)*(x2651)))+(((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(r11)*(x2664)))+(((IkReal(-1.00000000000000))*(x2660)*(x2663)))+(((x2657)*(x2658)))+(((IkReal(-1.00000000000000))*(cj6)*(x2658)*(x2663))));
11891 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
11892 {
11893 {
11894 IkReal j3array[1], cj3array[1], sj3array[1];
11895 bool j3valid[1]={false};
11896 _nj3 = 1;
11897 IkReal x2665=((IkReal(1.00000000000000))*(cj6));
11898 IkReal x2666=((IkReal(1.00000000000000))*(cj5));
11899 IkReal x2667=((r01)*(sj6));
11900 if( IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2665)))+(((IkReal(-1.00000000000000))*(cj4)*(x2666)*(x2667)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x2666)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2665)))+(((sj5)*(x2667))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2665)))+(((IkReal(-1.00000000000000))*(cj4)*(x2666)*(x2667)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x2666)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2665)))+(((sj5)*(x2667)))))-1) <= IKFAST_SINCOS_THRESH )
11901     continue;
11902 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x2665)))+(((IkReal(-1.00000000000000))*(cj4)*(x2666)*(x2667)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))), ((((IkReal(-1.00000000000000))*(r02)*(x2666)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x2665)))+(((sj5)*(x2667)))));
11903 sj3array[0]=IKsin(j3array[0]);
11904 cj3array[0]=IKcos(j3array[0]);
11905 if( j3array[0] > IKPI )
11906 {
11907     j3array[0]-=IK2PI;
11908 }
11909 else if( j3array[0] < -IKPI )
11910 {    j3array[0]+=IK2PI;
11911 }
11912 j3valid[0] = true;
11913 for(int ij3 = 0; ij3 < 1; ++ij3)
11914 {
11915 if( !j3valid[ij3] )
11916 {
11917     continue;
11918 }
11919 _ij3[0] = ij3; _ij3[1] = -1;
11920 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11921 {
11922 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11923 {
11924     j3valid[iij3]=false; _ij3[1] = iij3; break; 
11925 }
11926 }
11927 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11928 {
11929 IkReal evalcond[6];
11930 IkReal x2668=IKsin(j3);
11931 IkReal x2669=IKcos(j3);
11932 IkReal x2670=((r11)*(sj6));
11933 IkReal x2671=((IkReal(1.00000000000000))*(sj5));
11934 IkReal x2672=((sj4)*(sj6));
11935 IkReal x2673=((cj4)*(sj5));
11936 IkReal x2674=((cj6)*(sj4));
11937 IkReal x2675=((cj4)*(cj5));
11938 IkReal x2676=((r01)*(sj6));
11939 IkReal x2677=((r21)*(sj6));
11940 IkReal x2678=((cj6)*(sj5));
11941 IkReal x2679=((IkReal(1.00000000000000))*(cj6)*(x2675));
11942 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2671)*(x2677)))+(((sj2)*(x2668)))+(((r20)*(x2678))));
11943 evalcond[1]=((((IkReal(-1.00000000000000))*(x2671)*(x2676)))+(((r00)*(x2678)))+(x2669)+(((cj5)*(r02))));
11944 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2670)*(x2671)))+(((IkReal(-1.00000000000000))*(cj2)*(x2668)))+(((r10)*(x2678))));
11945 evalcond[3]=((((IkReal(-1.00000000000000))*(sj2)*(x2669)))+(((r21)*(x2674)))+(((r22)*(x2673)))+(((x2675)*(x2677)))+(((IkReal(-1.00000000000000))*(r20)*(x2679)))+(((r20)*(x2672))));
11946 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x2679)))+(((r02)*(x2673)))+(((x2675)*(x2676)))+(((r00)*(x2672)))+(((r01)*(x2674)))+(x2668));
11947 evalcond[5]=((((r12)*(x2673)))+(((r10)*(x2672)))+(((x2670)*(x2675)))+(((cj2)*(x2669)))+(((r11)*(x2674)))+(((IkReal(-1.00000000000000))*(r10)*(x2679))));
11948 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
11949 {
11950 continue;
11951 }
11952 }
11953 
11954 {
11955 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11956 vinfos[0].jointtype = 1;
11957 vinfos[0].foffset = j0;
11958 vinfos[0].indices[0] = _ij0[0];
11959 vinfos[0].indices[1] = _ij0[1];
11960 vinfos[0].maxsolutions = _nj0;
11961 vinfos[1].jointtype = 1;
11962 vinfos[1].foffset = j1;
11963 vinfos[1].indices[0] = _ij1[0];
11964 vinfos[1].indices[1] = _ij1[1];
11965 vinfos[1].maxsolutions = _nj1;
11966 vinfos[2].jointtype = 1;
11967 vinfos[2].foffset = j2;
11968 vinfos[2].indices[0] = _ij2[0];
11969 vinfos[2].indices[1] = _ij2[1];
11970 vinfos[2].maxsolutions = _nj2;
11971 vinfos[3].jointtype = 1;
11972 vinfos[3].foffset = j3;
11973 vinfos[3].indices[0] = _ij3[0];
11974 vinfos[3].indices[1] = _ij3[1];
11975 vinfos[3].maxsolutions = _nj3;
11976 vinfos[4].jointtype = 1;
11977 vinfos[4].foffset = j4;
11978 vinfos[4].indices[0] = _ij4[0];
11979 vinfos[4].indices[1] = _ij4[1];
11980 vinfos[4].maxsolutions = _nj4;
11981 vinfos[5].jointtype = 1;
11982 vinfos[5].foffset = j5;
11983 vinfos[5].indices[0] = _ij5[0];
11984 vinfos[5].indices[1] = _ij5[1];
11985 vinfos[5].maxsolutions = _nj5;
11986 vinfos[6].jointtype = 1;
11987 vinfos[6].foffset = j6;
11988 vinfos[6].indices[0] = _ij6[0];
11989 vinfos[6].indices[1] = _ij6[1];
11990 vinfos[6].maxsolutions = _nj6;
11991 std::vector<int> vfree(0);
11992 solutions.AddSolution(vinfos,vfree);
11993 }
11994 }
11995 }
11996 
11997 } else
11998 {
11999 IkReal x2680=((sj4)*(sj5));
12000 IkReal x2681=((IkReal(0.390000000000000))*(sj5));
12001 IkReal x2682=((r21)*(sj6));
12002 IkReal x2683=((IkReal(0.390000000000000))*(cj5));
12003 IkReal x2684=((cj6)*(r00));
12004 IkReal x2685=((IkReal(0.000500000000000000))*(cj6));
12005 IkReal x2686=((r11)*(sj6));
12006 IkReal x2687=((cj5)*(sj4));
12007 IkReal x2688=((IkReal(1.00000000000000))*(r20));
12008 IkReal x2689=((cj4)*(sj6));
12009 IkReal x2690=((r01)*(sj6));
12010 IkReal x2691=((IkReal(0.000500000000000000))*(sj6));
12011 IkReal x2692=((IkReal(1.00000000000000))*(r10));
12012 IkReal x2693=((IkReal(1.00000000000000))*(cj4)*(cj6));
12013 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
12014 evalcond[1]=((((r22)*(x2683)))+(((r21)*(x2685)))+(((IkReal(-1.00000000000000))*(pz)))+(((cj6)*(r20)*(x2681)))+(((IkReal(-1.00000000000000))*(x2681)*(x2682)))+(((r20)*(x2691)))+(((IkReal(-0.00200000000000000))*(cj2))));
12015 evalcond[2]=((IkReal(0.400000000000000))+(((r02)*(x2683)))+(((IkReal(-1.00000000000000))*(x2681)*(x2690)))+(((r01)*(x2685)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x2691)))+(((x2681)*(x2684))));
12016 evalcond[3]=((((r11)*(x2685)))+(((r10)*(x2691)))+(((IkReal(0.00200000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(x2681)*(x2686)))+(((r12)*(x2683)))+(((cj6)*(r10)*(x2681))));
12017 evalcond[4]=((((IkReal(-1.00000000000000))*(x2688)*(x2689)))+(((r22)*(x2680)))+(((IkReal(-1.00000000000000))*(cj6)*(x2687)*(x2688)))+(((x2682)*(x2687)))+(((IkReal(-1.00000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(r21)*(x2693))));
12018 evalcond[5]=((((r02)*(x2680)))+(((IkReal(-1.00000000000000))*(r00)*(x2689)))+(((IkReal(-1.00000000000000))*(x2684)*(x2687)))+(((x2687)*(x2690)))+(((IkReal(-1.00000000000000))*(r01)*(x2693))));
12019 evalcond[6]=((sj2)+(((IkReal(-1.00000000000000))*(x2689)*(x2692)))+(((IkReal(-1.00000000000000))*(cj6)*(x2687)*(x2692)))+(((x2686)*(x2687)))+(((IkReal(-1.00000000000000))*(r11)*(x2693)))+(((r12)*(x2680))));
12020 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
12021 {
12022 {
12023 IkReal j3array[1], cj3array[1], sj3array[1];
12024 bool j3valid[1]={false};
12025 _nj3 = 1;
12026 IkReal x2694=((cj4)*(cj5));
12027 IkReal x2695=((r01)*(sj6));
12028 IkReal x2696=((cj6)*(r00));
12029 if( IKabs(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x2694)*(x2696)))+(((cj4)*(r02)*(sj5)))+(((x2694)*(x2695))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x2695)))+(((sj5)*(x2696)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x2694)*(x2696)))+(((cj4)*(r02)*(sj5)))+(((x2694)*(x2695)))))+IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x2695)))+(((sj5)*(x2696)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
12030     continue;
12031 j3array[0]=IKatan2(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((IkReal(-1.00000000000000))*(x2694)*(x2696)))+(((cj4)*(r02)*(sj5)))+(((x2694)*(x2695)))), ((((IkReal(-1.00000000000000))*(sj5)*(x2695)))+(((sj5)*(x2696)))+(((cj5)*(r02)))));
12032 sj3array[0]=IKsin(j3array[0]);
12033 cj3array[0]=IKcos(j3array[0]);
12034 if( j3array[0] > IKPI )
12035 {
12036     j3array[0]-=IK2PI;
12037 }
12038 else if( j3array[0] < -IKPI )
12039 {    j3array[0]+=IK2PI;
12040 }
12041 j3valid[0] = true;
12042 for(int ij3 = 0; ij3 < 1; ++ij3)
12043 {
12044 if( !j3valid[ij3] )
12045 {
12046     continue;
12047 }
12048 _ij3[0] = ij3; _ij3[1] = -1;
12049 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12050 {
12051 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12052 {
12053     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12054 }
12055 }
12056 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12057 {
12058 IkReal evalcond[6];
12059 IkReal x2697=IKsin(j3);
12060 IkReal x2698=IKcos(j3);
12061 IkReal x2699=((r11)*(sj6));
12062 IkReal x2700=((IkReal(1.00000000000000))*(sj5));
12063 IkReal x2701=((cj6)*(sj4));
12064 IkReal x2702=((sj4)*(sj6));
12065 IkReal x2703=((cj4)*(sj5));
12066 IkReal x2704=((cj4)*(cj5));
12067 IkReal x2705=((r01)*(sj6));
12068 IkReal x2706=((r21)*(sj6));
12069 IkReal x2707=((cj6)*(sj5));
12070 IkReal x2708=((IkReal(1.00000000000000))*(x2698));
12071 IkReal x2709=((IkReal(1.00000000000000))*(cj6)*(x2704));
12072 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x2707)))+(((sj2)*(x2697)))+(((IkReal(-1.00000000000000))*(x2700)*(x2706))));
12073 evalcond[1]=((((r00)*(x2707)))+(((IkReal(-1.00000000000000))*(x2708)))+(((IkReal(-1.00000000000000))*(x2700)*(x2705)))+(((cj5)*(r02))));
12074 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x2707)))+(((IkReal(-1.00000000000000))*(x2699)*(x2700)))+(((cj2)*(x2697))));
12075 evalcond[3]=((((r22)*(x2703)))+(((x2704)*(x2706)))+(((IkReal(-1.00000000000000))*(r20)*(x2709)))+(((r20)*(x2702)))+(((r21)*(x2701)))+(((IkReal(-1.00000000000000))*(sj2)*(x2708))));
12076 evalcond[4]=((((r00)*(x2702)))+(((IkReal(-1.00000000000000))*(r00)*(x2709)))+(((x2704)*(x2705)))+(((r01)*(x2701)))+(((IkReal(-1.00000000000000))*(x2697)))+(((r02)*(x2703))));
12077 evalcond[5]=((((r12)*(x2703)))+(((IkReal(-1.00000000000000))*(r10)*(x2709)))+(((r11)*(x2701)))+(((x2699)*(x2704)))+(((r10)*(x2702)))+(((IkReal(-1.00000000000000))*(cj2)*(x2708))));
12078 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12079 {
12080 continue;
12081 }
12082 }
12083 
12084 {
12085 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12086 vinfos[0].jointtype = 1;
12087 vinfos[0].foffset = j0;
12088 vinfos[0].indices[0] = _ij0[0];
12089 vinfos[0].indices[1] = _ij0[1];
12090 vinfos[0].maxsolutions = _nj0;
12091 vinfos[1].jointtype = 1;
12092 vinfos[1].foffset = j1;
12093 vinfos[1].indices[0] = _ij1[0];
12094 vinfos[1].indices[1] = _ij1[1];
12095 vinfos[1].maxsolutions = _nj1;
12096 vinfos[2].jointtype = 1;
12097 vinfos[2].foffset = j2;
12098 vinfos[2].indices[0] = _ij2[0];
12099 vinfos[2].indices[1] = _ij2[1];
12100 vinfos[2].maxsolutions = _nj2;
12101 vinfos[3].jointtype = 1;
12102 vinfos[3].foffset = j3;
12103 vinfos[3].indices[0] = _ij3[0];
12104 vinfos[3].indices[1] = _ij3[1];
12105 vinfos[3].maxsolutions = _nj3;
12106 vinfos[4].jointtype = 1;
12107 vinfos[4].foffset = j4;
12108 vinfos[4].indices[0] = _ij4[0];
12109 vinfos[4].indices[1] = _ij4[1];
12110 vinfos[4].maxsolutions = _nj4;
12111 vinfos[5].jointtype = 1;
12112 vinfos[5].foffset = j5;
12113 vinfos[5].indices[0] = _ij5[0];
12114 vinfos[5].indices[1] = _ij5[1];
12115 vinfos[5].maxsolutions = _nj5;
12116 vinfos[6].jointtype = 1;
12117 vinfos[6].foffset = j6;
12118 vinfos[6].indices[0] = _ij6[0];
12119 vinfos[6].indices[1] = _ij6[1];
12120 vinfos[6].maxsolutions = _nj6;
12121 std::vector<int> vfree(0);
12122 solutions.AddSolution(vinfos,vfree);
12123 }
12124 }
12125 }
12126 
12127 } else
12128 {
12129 IkReal x2710=((sj4)*(sj5));
12130 IkReal x2711=((IkReal(0.390000000000000))*(sj5));
12131 IkReal x2712=((r21)*(sj6));
12132 IkReal x2713=((IkReal(0.390000000000000))*(cj5));
12133 IkReal x2714=((cj6)*(r00));
12134 IkReal x2715=((IkReal(0.000500000000000000))*(cj6));
12135 IkReal x2716=((r11)*(sj6));
12136 IkReal x2717=((cj5)*(sj4));
12137 IkReal x2718=((r01)*(sj6));
12138 IkReal x2719=((IkReal(1.00000000000000))*(r20));
12139 IkReal x2720=((cj4)*(sj6));
12140 IkReal x2721=((IkReal(0.000500000000000000))*(sj6));
12141 IkReal x2722=((IkReal(1.00000000000000))*(r10));
12142 IkReal x2723=((IkReal(1.00000000000000))*(cj4)*(cj6));
12143 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
12144 evalcond[1]=((((r22)*(x2713)))+(((cj6)*(r20)*(x2711)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x2721)))+(((r21)*(x2715)))+(((IkReal(-1.00000000000000))*(x2711)*(x2712))));
12145 evalcond[2]=((((r02)*(x2713)))+(((IkReal(-1.00000000000000))*(px)))+(((x2711)*(x2714)))+(((r00)*(x2721)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x2711)*(x2718)))+(((r01)*(x2715))));
12146 evalcond[3]=((((r12)*(x2713)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj1)))+(((cj6)*(r10)*(x2711)))+(((r10)*(x2721)))+(((r11)*(x2715)))+(((IkReal(-1.00000000000000))*(x2711)*(x2716))));
12147 evalcond[4]=((((IkReal(-1.00000000000000))*(r21)*(x2723)))+(((IkReal(-1.00000000000000))*(x2719)*(x2720)))+(((IkReal(-1.00000000000000))*(cj6)*(x2717)*(x2719)))+(((x2712)*(x2717)))+(((r22)*(x2710))));
12148 evalcond[5]=((((IkReal(-1.00000000000000))*(x2714)*(x2717)))+(((IkReal(-1.00000000000000))*(r01)*(x2723)))+(((r02)*(x2710)))+(((IkReal(-1.00000000000000))*(r00)*(x2720)))+(((IkReal(-1.00000000000000))*(cj1)))+(((x2717)*(x2718))));
12149 evalcond[6]=((((r12)*(x2710)))+(((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj6)*(x2717)*(x2722)))+(((x2716)*(x2717)))+(((IkReal(-1.00000000000000))*(x2720)*(x2722)))+(((IkReal(-1.00000000000000))*(r11)*(x2723))));
12150 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
12151 {
12152 {
12153 IkReal j3array[1], cj3array[1], sj3array[1];
12154 bool j3valid[1]={false};
12155 _nj3 = 1;
12156 IkReal x2724=((cj4)*(cj5));
12157 IkReal x2725=((r21)*(sj6));
12158 IkReal x2726=((IkReal(1.00000000000000))*(cj6)*(r20));
12159 if( IKabs(((((sj5)*(x2725)))+(((IkReal(-1.00000000000000))*(sj5)*(x2726)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x2724)*(x2726)))+(((x2724)*(x2725)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x2725)))+(((IkReal(-1.00000000000000))*(sj5)*(x2726)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((IkReal(-1.00000000000000))*(x2724)*(x2726)))+(((x2724)*(x2725)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
12160     continue;
12161 j3array[0]=IKatan2(((((sj5)*(x2725)))+(((IkReal(-1.00000000000000))*(sj5)*(x2726)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((IkReal(-1.00000000000000))*(x2724)*(x2726)))+(((x2724)*(x2725)))+(((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))));
12162 sj3array[0]=IKsin(j3array[0]);
12163 cj3array[0]=IKcos(j3array[0]);
12164 if( j3array[0] > IKPI )
12165 {
12166     j3array[0]-=IK2PI;
12167 }
12168 else if( j3array[0] < -IKPI )
12169 {    j3array[0]+=IK2PI;
12170 }
12171 j3valid[0] = true;
12172 for(int ij3 = 0; ij3 < 1; ++ij3)
12173 {
12174 if( !j3valid[ij3] )
12175 {
12176     continue;
12177 }
12178 _ij3[0] = ij3; _ij3[1] = -1;
12179 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12180 {
12181 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12182 {
12183     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12184 }
12185 }
12186 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12187 {
12188 IkReal evalcond[6];
12189 IkReal x2727=IKcos(j3);
12190 IkReal x2728=IKsin(j3);
12191 IkReal x2729=((r11)*(sj6));
12192 IkReal x2730=((IkReal(1.00000000000000))*(sj5));
12193 IkReal x2731=((IkReal(1.00000000000000))*(cj1));
12194 IkReal x2732=((cj6)*(sj4));
12195 IkReal x2733=((sj4)*(sj6));
12196 IkReal x2734=((cj4)*(sj5));
12197 IkReal x2735=((cj4)*(cj5));
12198 IkReal x2736=((r01)*(sj6));
12199 IkReal x2737=((r21)*(sj6));
12200 IkReal x2738=((cj6)*(sj5));
12201 IkReal x2739=((IkReal(1.00000000000000))*(cj6)*(x2735));
12202 evalcond[0]=((x2728)+(((IkReal(-1.00000000000000))*(x2730)*(x2737)))+(((cj5)*(r22)))+(((r20)*(x2738))));
12203 evalcond[1]=((((IkReal(-1.00000000000000))*(x2730)*(x2736)))+(((r00)*(x2738)))+(((sj1)*(x2727)))+(((cj5)*(r02))));
12204 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2727)*(x2731)))+(((r10)*(x2738)))+(((IkReal(-1.00000000000000))*(x2729)*(x2730))));
12205 evalcond[3]=((((r21)*(x2732)))+(((r22)*(x2734)))+(((x2735)*(x2737)))+(((IkReal(-1.00000000000000))*(x2727)))+(((r20)*(x2733)))+(((IkReal(-1.00000000000000))*(r20)*(x2739))));
12206 evalcond[4]=((((r02)*(x2734)))+(((r01)*(x2732)))+(((x2735)*(x2736)))+(((r00)*(x2733)))+(((sj1)*(x2728)))+(((IkReal(-1.00000000000000))*(r00)*(x2739))));
12207 evalcond[5]=((((IkReal(-1.00000000000000))*(x2728)*(x2731)))+(((r12)*(x2734)))+(((r10)*(x2733)))+(((IkReal(-1.00000000000000))*(r10)*(x2739)))+(((r11)*(x2732)))+(((x2729)*(x2735))));
12208 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12209 {
12210 continue;
12211 }
12212 }
12213 
12214 {
12215 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12216 vinfos[0].jointtype = 1;
12217 vinfos[0].foffset = j0;
12218 vinfos[0].indices[0] = _ij0[0];
12219 vinfos[0].indices[1] = _ij0[1];
12220 vinfos[0].maxsolutions = _nj0;
12221 vinfos[1].jointtype = 1;
12222 vinfos[1].foffset = j1;
12223 vinfos[1].indices[0] = _ij1[0];
12224 vinfos[1].indices[1] = _ij1[1];
12225 vinfos[1].maxsolutions = _nj1;
12226 vinfos[2].jointtype = 1;
12227 vinfos[2].foffset = j2;
12228 vinfos[2].indices[0] = _ij2[0];
12229 vinfos[2].indices[1] = _ij2[1];
12230 vinfos[2].maxsolutions = _nj2;
12231 vinfos[3].jointtype = 1;
12232 vinfos[3].foffset = j3;
12233 vinfos[3].indices[0] = _ij3[0];
12234 vinfos[3].indices[1] = _ij3[1];
12235 vinfos[3].maxsolutions = _nj3;
12236 vinfos[4].jointtype = 1;
12237 vinfos[4].foffset = j4;
12238 vinfos[4].indices[0] = _ij4[0];
12239 vinfos[4].indices[1] = _ij4[1];
12240 vinfos[4].maxsolutions = _nj4;
12241 vinfos[5].jointtype = 1;
12242 vinfos[5].foffset = j5;
12243 vinfos[5].indices[0] = _ij5[0];
12244 vinfos[5].indices[1] = _ij5[1];
12245 vinfos[5].maxsolutions = _nj5;
12246 vinfos[6].jointtype = 1;
12247 vinfos[6].foffset = j6;
12248 vinfos[6].indices[0] = _ij6[0];
12249 vinfos[6].indices[1] = _ij6[1];
12250 vinfos[6].maxsolutions = _nj6;
12251 std::vector<int> vfree(0);
12252 solutions.AddSolution(vinfos,vfree);
12253 }
12254 }
12255 }
12256 
12257 } else
12258 {
12259 IkReal x2740=((sj4)*(sj5));
12260 IkReal x2741=((IkReal(0.390000000000000))*(sj5));
12261 IkReal x2742=((r21)*(sj6));
12262 IkReal x2743=((IkReal(0.390000000000000))*(cj5));
12263 IkReal x2744=((cj6)*(r00));
12264 IkReal x2745=((IkReal(0.000500000000000000))*(cj6));
12265 IkReal x2746=((r11)*(sj6));
12266 IkReal x2747=((cj5)*(sj4));
12267 IkReal x2748=((r01)*(sj6));
12268 IkReal x2749=((IkReal(1.00000000000000))*(r20));
12269 IkReal x2750=((cj4)*(sj6));
12270 IkReal x2751=((IkReal(0.000500000000000000))*(sj6));
12271 IkReal x2752=((IkReal(1.00000000000000))*(r10));
12272 IkReal x2753=((IkReal(1.00000000000000))*(cj4)*(cj6));
12273 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
12274 evalcond[1]=((((r21)*(x2745)))+(((r22)*(x2743)))+(((cj6)*(r20)*(x2741)))+(((IkReal(-1.00000000000000))*(x2741)*(x2742)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x2751))));
12275 evalcond[2]=((((IkReal(0.00200000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x2741)*(x2748)))+(((IkReal(-1.00000000000000))*(px)))+(((r01)*(x2745)))+(((r00)*(x2751)))+(((x2741)*(x2744)))+(((IkReal(-0.400000000000000))*(sj1)))+(((r02)*(x2743))));
12276 evalcond[3]=((((r11)*(x2745)))+(((r12)*(x2743)))+(((r10)*(x2751)))+(((IkReal(0.400000000000000))*(cj1)))+(((cj6)*(r10)*(x2741)))+(((IkReal(-1.00000000000000))*(x2741)*(x2746)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py))));
12277 evalcond[4]=((((r22)*(x2740)))+(((x2742)*(x2747)))+(((IkReal(-1.00000000000000))*(cj6)*(x2747)*(x2749)))+(((IkReal(-1.00000000000000))*(x2749)*(x2750)))+(((IkReal(-1.00000000000000))*(r21)*(x2753))));
12278 evalcond[5]=((cj1)+(((IkReal(-1.00000000000000))*(r01)*(x2753)))+(((IkReal(-1.00000000000000))*(x2744)*(x2747)))+(((IkReal(-1.00000000000000))*(r00)*(x2750)))+(((x2747)*(x2748)))+(((r02)*(x2740))));
12279 evalcond[6]=((sj1)+(((x2746)*(x2747)))+(((IkReal(-1.00000000000000))*(cj6)*(x2747)*(x2752)))+(((r12)*(x2740)))+(((IkReal(-1.00000000000000))*(x2750)*(x2752)))+(((IkReal(-1.00000000000000))*(r11)*(x2753))));
12280 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
12281 {
12282 {
12283 IkReal j3array[1], cj3array[1], sj3array[1];
12284 bool j3valid[1]={false};
12285 _nj3 = 1;
12286 IkReal x2754=((cj4)*(cj5));
12287 IkReal x2755=((cj6)*(r20));
12288 IkReal x2756=((IkReal(1.00000000000000))*(sj4));
12289 IkReal x2757=((IkReal(1.00000000000000))*(r21)*(sj6));
12290 if( IKabs(((((cj5)*(r22)))+(((sj5)*(x2755)))+(((IkReal(-1.00000000000000))*(sj5)*(x2757))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((x2754)*(x2755)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x2754)*(x2757)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2756)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2756))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((sj5)*(x2755)))+(((IkReal(-1.00000000000000))*(sj5)*(x2757)))))+IKsqr(((((x2754)*(x2755)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x2754)*(x2757)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2756)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2756)))))-1) <= IKFAST_SINCOS_THRESH )
12291     continue;
12292 j3array[0]=IKatan2(((((cj5)*(r22)))+(((sj5)*(x2755)))+(((IkReal(-1.00000000000000))*(sj5)*(x2757)))), ((((x2754)*(x2755)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x2754)*(x2757)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2756)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2756)))));
12293 sj3array[0]=IKsin(j3array[0]);
12294 cj3array[0]=IKcos(j3array[0]);
12295 if( j3array[0] > IKPI )
12296 {
12297     j3array[0]-=IK2PI;
12298 }
12299 else if( j3array[0] < -IKPI )
12300 {    j3array[0]+=IK2PI;
12301 }
12302 j3valid[0] = true;
12303 for(int ij3 = 0; ij3 < 1; ++ij3)
12304 {
12305 if( !j3valid[ij3] )
12306 {
12307     continue;
12308 }
12309 _ij3[0] = ij3; _ij3[1] = -1;
12310 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12311 {
12312 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12313 {
12314     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12315 }
12316 }
12317 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12318 {
12319 IkReal evalcond[6];
12320 IkReal x2758=IKcos(j3);
12321 IkReal x2759=IKsin(j3);
12322 IkReal x2760=((r11)*(sj6));
12323 IkReal x2761=((IkReal(1.00000000000000))*(sj5));
12324 IkReal x2762=((IkReal(1.00000000000000))*(cj1));
12325 IkReal x2763=((sj4)*(sj6));
12326 IkReal x2764=((cj4)*(sj5));
12327 IkReal x2765=((cj6)*(sj4));
12328 IkReal x2766=((cj4)*(cj5));
12329 IkReal x2767=((r01)*(sj6));
12330 IkReal x2768=((r21)*(sj6));
12331 IkReal x2769=((cj6)*(sj5));
12332 IkReal x2770=((IkReal(1.00000000000000))*(cj6)*(x2766));
12333 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2761)*(x2768)))+(((r20)*(x2769)))+(((IkReal(-1.00000000000000))*(x2759))));
12334 evalcond[1]=((((IkReal(-1.00000000000000))*(x2761)*(x2767)))+(((r00)*(x2769)))+(((sj1)*(x2758)))+(((cj5)*(r02))));
12335 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x2760)*(x2761)))+(((IkReal(-1.00000000000000))*(x2758)*(x2762)))+(((r10)*(x2769))));
12336 evalcond[3]=((x2758)+(((r20)*(x2763)))+(((IkReal(-1.00000000000000))*(r20)*(x2770)))+(((r21)*(x2765)))+(((r22)*(x2764)))+(((x2766)*(x2768))));
12337 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x2770)))+(((r00)*(x2763)))+(((r01)*(x2765)))+(((sj1)*(x2759)))+(((r02)*(x2764)))+(((x2766)*(x2767))));
12338 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x2770)))+(((r12)*(x2764)))+(((IkReal(-1.00000000000000))*(x2759)*(x2762)))+(((r10)*(x2763)))+(((x2760)*(x2766)))+(((r11)*(x2765))));
12339 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12340 {
12341 continue;
12342 }
12343 }
12344 
12345 {
12346 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12347 vinfos[0].jointtype = 1;
12348 vinfos[0].foffset = j0;
12349 vinfos[0].indices[0] = _ij0[0];
12350 vinfos[0].indices[1] = _ij0[1];
12351 vinfos[0].maxsolutions = _nj0;
12352 vinfos[1].jointtype = 1;
12353 vinfos[1].foffset = j1;
12354 vinfos[1].indices[0] = _ij1[0];
12355 vinfos[1].indices[1] = _ij1[1];
12356 vinfos[1].maxsolutions = _nj1;
12357 vinfos[2].jointtype = 1;
12358 vinfos[2].foffset = j2;
12359 vinfos[2].indices[0] = _ij2[0];
12360 vinfos[2].indices[1] = _ij2[1];
12361 vinfos[2].maxsolutions = _nj2;
12362 vinfos[3].jointtype = 1;
12363 vinfos[3].foffset = j3;
12364 vinfos[3].indices[0] = _ij3[0];
12365 vinfos[3].indices[1] = _ij3[1];
12366 vinfos[3].maxsolutions = _nj3;
12367 vinfos[4].jointtype = 1;
12368 vinfos[4].foffset = j4;
12369 vinfos[4].indices[0] = _ij4[0];
12370 vinfos[4].indices[1] = _ij4[1];
12371 vinfos[4].maxsolutions = _nj4;
12372 vinfos[5].jointtype = 1;
12373 vinfos[5].foffset = j5;
12374 vinfos[5].indices[0] = _ij5[0];
12375 vinfos[5].indices[1] = _ij5[1];
12376 vinfos[5].maxsolutions = _nj5;
12377 vinfos[6].jointtype = 1;
12378 vinfos[6].foffset = j6;
12379 vinfos[6].indices[0] = _ij6[0];
12380 vinfos[6].indices[1] = _ij6[1];
12381 vinfos[6].maxsolutions = _nj6;
12382 std::vector<int> vfree(0);
12383 solutions.AddSolution(vinfos,vfree);
12384 }
12385 }
12386 }
12387 
12388 } else
12389 {
12390 if( 1 )
12391 {
12392 continue;
12393 
12394 } else
12395 {
12396 }
12397 }
12398 }
12399 }
12400 }
12401 }
12402 }
12403 }
12404 
12405 } else
12406 {
12407 {
12408 IkReal j3array[1], cj3array[1], sj3array[1];
12409 bool j3valid[1]={false};
12410 _nj3 = 1;
12411 IkReal x2771=((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30);
12412 IkReal x2772=((IkReal(1.00000000000000))*(sj5));
12413 IkReal x2773=((cj2)*(sj1));
12414 IkReal x2774=((r21)*(sj6));
12415 IkReal x2775=((cj6)*(r20));
12416 IkReal x2776=((cj5)*(r22));
12417 if( IKabs(((x2771)*(((((IkReal(-1.00000000000000))*(x2776)))+(((IkReal(-1.00000000000000))*(x2772)*(x2775)))+(((sj5)*(x2774))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x2771)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2773)*(x2775)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2772)))+(((IkReal(-1.00000000000000))*(x2772)*(x2773)*(x2774)))+(((x2773)*(x2776)))+(((cj6)*(r10)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x2771)*(((((IkReal(-1.00000000000000))*(x2776)))+(((IkReal(-1.00000000000000))*(x2772)*(x2775)))+(((sj5)*(x2774)))))))+IKsqr(((x2771)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2773)*(x2775)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2772)))+(((IkReal(-1.00000000000000))*(x2772)*(x2773)*(x2774)))+(((x2773)*(x2776)))+(((cj6)*(r10)*(sj2)*(sj5)))))))-1) <= IKFAST_SINCOS_THRESH )
12418     continue;
12419 j3array[0]=IKatan2(((x2771)*(((((IkReal(-1.00000000000000))*(x2776)))+(((IkReal(-1.00000000000000))*(x2772)*(x2775)))+(((sj5)*(x2774)))))), ((x2771)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((sj5)*(x2773)*(x2775)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x2772)))+(((IkReal(-1.00000000000000))*(x2772)*(x2773)*(x2774)))+(((x2773)*(x2776)))+(((cj6)*(r10)*(sj2)*(sj5)))))));
12420 sj3array[0]=IKsin(j3array[0]);
12421 cj3array[0]=IKcos(j3array[0]);
12422 if( j3array[0] > IKPI )
12423 {
12424     j3array[0]-=IK2PI;
12425 }
12426 else if( j3array[0] < -IKPI )
12427 {    j3array[0]+=IK2PI;
12428 }
12429 j3valid[0] = true;
12430 for(int ij3 = 0; ij3 < 1; ++ij3)
12431 {
12432 if( !j3valid[ij3] )
12433 {
12434     continue;
12435 }
12436 _ij3[0] = ij3; _ij3[1] = -1;
12437 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12438 {
12439 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12440 {
12441     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12442 }
12443 }
12444 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12445 {
12446 IkReal evalcond[6];
12447 IkReal x2777=IKsin(j3);
12448 IkReal x2778=IKcos(j3);
12449 IkReal x2779=((r11)*(sj6));
12450 IkReal x2780=((IkReal(1.00000000000000))*(sj5));
12451 IkReal x2781=((sj4)*(sj6));
12452 IkReal x2782=((cj4)*(sj5));
12453 IkReal x2783=((cj6)*(sj4));
12454 IkReal x2784=((cj4)*(cj5));
12455 IkReal x2785=((r01)*(sj6));
12456 IkReal x2786=((r21)*(sj6));
12457 IkReal x2787=((cj6)*(sj5));
12458 IkReal x2788=((IkReal(1.00000000000000))*(x2778));
12459 IkReal x2789=((sj1)*(x2778));
12460 IkReal x2790=((IkReal(1.00000000000000))*(cj6)*(x2784));
12461 IkReal x2791=((IkReal(1.00000000000000))*(cj2)*(x2777));
12462 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x2780)*(x2786)))+(((sj2)*(x2777)))+(((r20)*(x2787))));
12463 evalcond[1]=((x2789)+(((IkReal(-1.00000000000000))*(x2780)*(x2785)))+(((r00)*(x2787)))+(((IkReal(-1.00000000000000))*(cj1)*(x2791)))+(((cj5)*(r02))));
12464 evalcond[2]=((((IkReal(-1.00000000000000))*(cj1)*(x2788)))+(((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x2791)))+(((IkReal(-1.00000000000000))*(x2779)*(x2780)))+(((r10)*(x2787))));
12465 evalcond[3]=((((r21)*(x2783)))+(((IkReal(-1.00000000000000))*(r20)*(x2790)))+(((IkReal(-1.00000000000000))*(sj2)*(x2788)))+(((x2784)*(x2786)))+(((r22)*(x2782)))+(((r20)*(x2781))));
12466 evalcond[4]=((((sj1)*(x2777)))+(((x2784)*(x2785)))+(((r02)*(x2782)))+(((cj1)*(cj2)*(x2778)))+(((r01)*(x2783)))+(((r00)*(x2781)))+(((IkReal(-1.00000000000000))*(r00)*(x2790))));
12467 evalcond[5]=((((x2779)*(x2784)))+(((cj2)*(x2789)))+(((r12)*(x2782)))+(((IkReal(-1.00000000000000))*(r10)*(x2790)))+(((r11)*(x2783)))+(((IkReal(-1.00000000000000))*(cj1)*(x2777)))+(((r10)*(x2781))));
12468 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12469 {
12470 continue;
12471 }
12472 }
12473 
12474 {
12475 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12476 vinfos[0].jointtype = 1;
12477 vinfos[0].foffset = j0;
12478 vinfos[0].indices[0] = _ij0[0];
12479 vinfos[0].indices[1] = _ij0[1];
12480 vinfos[0].maxsolutions = _nj0;
12481 vinfos[1].jointtype = 1;
12482 vinfos[1].foffset = j1;
12483 vinfos[1].indices[0] = _ij1[0];
12484 vinfos[1].indices[1] = _ij1[1];
12485 vinfos[1].maxsolutions = _nj1;
12486 vinfos[2].jointtype = 1;
12487 vinfos[2].foffset = j2;
12488 vinfos[2].indices[0] = _ij2[0];
12489 vinfos[2].indices[1] = _ij2[1];
12490 vinfos[2].maxsolutions = _nj2;
12491 vinfos[3].jointtype = 1;
12492 vinfos[3].foffset = j3;
12493 vinfos[3].indices[0] = _ij3[0];
12494 vinfos[3].indices[1] = _ij3[1];
12495 vinfos[3].maxsolutions = _nj3;
12496 vinfos[4].jointtype = 1;
12497 vinfos[4].foffset = j4;
12498 vinfos[4].indices[0] = _ij4[0];
12499 vinfos[4].indices[1] = _ij4[1];
12500 vinfos[4].maxsolutions = _nj4;
12501 vinfos[5].jointtype = 1;
12502 vinfos[5].foffset = j5;
12503 vinfos[5].indices[0] = _ij5[0];
12504 vinfos[5].indices[1] = _ij5[1];
12505 vinfos[5].maxsolutions = _nj5;
12506 vinfos[6].jointtype = 1;
12507 vinfos[6].foffset = j6;
12508 vinfos[6].indices[0] = _ij6[0];
12509 vinfos[6].indices[1] = _ij6[1];
12510 vinfos[6].maxsolutions = _nj6;
12511 std::vector<int> vfree(0);
12512 solutions.AddSolution(vinfos,vfree);
12513 }
12514 }
12515 }
12516 
12517 }
12518 
12519 }
12520 
12521 } else
12522 {
12523 {
12524 IkReal j3array[1], cj3array[1], sj3array[1];
12525 bool j3valid[1]={false};
12526 _nj3 = 1;
12527 IkReal x2792=(sj1)*(sj1);
12528 IkReal x2793=(cj1)*(cj1);
12529 IkReal x2794=((cj5)*(r12));
12530 IkReal x2795=((cj6)*(sj5));
12531 IkReal x2796=((IkReal(1.00000000000000))*(sj1));
12532 IkReal x2797=((cj5)*(r02));
12533 IkReal x2798=((IkReal(1.00000000000000))*(cj1));
12534 IkReal x2799=((r01)*(sj5)*(sj6));
12535 IkReal x2800=((r11)*(sj5)*(sj6));
12536 if( IKabs(((((IKabs(((((cj2)*(x2792)))+(((cj2)*(x2793))))) != 0)?((IkReal)1/(((((cj2)*(x2792)))+(((cj2)*(x2793)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2798)*(x2799)))+(((r10)*(sj1)*(x2795)))+(((cj1)*(x2797)))+(((cj1)*(r00)*(x2795)))+(((sj1)*(x2794)))+(((IkReal(-1.00000000000000))*(x2796)*(x2800))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x2792)+(x2793))) != 0)?((IkReal)1/(((x2792)+(x2793)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2796)*(x2797)))+(((cj1)*(x2794)))+(((sj1)*(x2799)))+(((cj1)*(r10)*(x2795)))+(((IkReal(-1.00000000000000))*(x2798)*(x2800)))+(((IkReal(-1.00000000000000))*(r00)*(x2795)*(x2796))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x2792)))+(((cj2)*(x2793))))) != 0)?((IkReal)1/(((((cj2)*(x2792)))+(((cj2)*(x2793)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2798)*(x2799)))+(((r10)*(sj1)*(x2795)))+(((cj1)*(x2797)))+(((cj1)*(r00)*(x2795)))+(((sj1)*(x2794)))+(((IkReal(-1.00000000000000))*(x2796)*(x2800)))))))+IKsqr(((((IKabs(((x2792)+(x2793))) != 0)?((IkReal)1/(((x2792)+(x2793)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2796)*(x2797)))+(((cj1)*(x2794)))+(((sj1)*(x2799)))+(((cj1)*(r10)*(x2795)))+(((IkReal(-1.00000000000000))*(x2798)*(x2800)))+(((IkReal(-1.00000000000000))*(r00)*(x2795)*(x2796)))))))-1) <= IKFAST_SINCOS_THRESH )
12537     continue;
12538 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x2792)))+(((cj2)*(x2793))))) != 0)?((IkReal)1/(((((cj2)*(x2792)))+(((cj2)*(x2793)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2798)*(x2799)))+(((r10)*(sj1)*(x2795)))+(((cj1)*(x2797)))+(((cj1)*(r00)*(x2795)))+(((sj1)*(x2794)))+(((IkReal(-1.00000000000000))*(x2796)*(x2800)))))), ((((IKabs(((x2792)+(x2793))) != 0)?((IkReal)1/(((x2792)+(x2793)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x2796)*(x2797)))+(((cj1)*(x2794)))+(((sj1)*(x2799)))+(((cj1)*(r10)*(x2795)))+(((IkReal(-1.00000000000000))*(x2798)*(x2800)))+(((IkReal(-1.00000000000000))*(r00)*(x2795)*(x2796)))))));
12539 sj3array[0]=IKsin(j3array[0]);
12540 cj3array[0]=IKcos(j3array[0]);
12541 if( j3array[0] > IKPI )
12542 {
12543     j3array[0]-=IK2PI;
12544 }
12545 else if( j3array[0] < -IKPI )
12546 {    j3array[0]+=IK2PI;
12547 }
12548 j3valid[0] = true;
12549 for(int ij3 = 0; ij3 < 1; ++ij3)
12550 {
12551 if( !j3valid[ij3] )
12552 {
12553     continue;
12554 }
12555 _ij3[0] = ij3; _ij3[1] = -1;
12556 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12557 {
12558 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12559 {
12560     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12561 }
12562 }
12563 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12564 {
12565 IkReal evalcond[6];
12566 IkReal x2801=IKsin(j3);
12567 IkReal x2802=IKcos(j3);
12568 IkReal x2803=((r11)*(sj6));
12569 IkReal x2804=((IkReal(1.00000000000000))*(sj5));
12570 IkReal x2805=((sj4)*(sj6));
12571 IkReal x2806=((cj4)*(sj5));
12572 IkReal x2807=((cj6)*(sj4));
12573 IkReal x2808=((cj4)*(cj5));
12574 IkReal x2809=((r01)*(sj6));
12575 IkReal x2810=((r21)*(sj6));
12576 IkReal x2811=((cj6)*(sj5));
12577 IkReal x2812=((IkReal(1.00000000000000))*(x2802));
12578 IkReal x2813=((sj1)*(x2802));
12579 IkReal x2814=((IkReal(1.00000000000000))*(cj6)*(x2808));
12580 IkReal x2815=((IkReal(1.00000000000000))*(cj2)*(x2801));
12581 evalcond[0]=((((IkReal(-1.00000000000000))*(x2804)*(x2810)))+(((cj5)*(r22)))+(((sj2)*(x2801)))+(((r20)*(x2811))));
12582 evalcond[1]=((((r00)*(x2811)))+(((IkReal(-1.00000000000000))*(cj1)*(x2815)))+(((IkReal(-1.00000000000000))*(x2804)*(x2809)))+(x2813)+(((cj5)*(r02))));
12583 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x2811)))+(((IkReal(-1.00000000000000))*(cj1)*(x2812)))+(((IkReal(-1.00000000000000))*(x2803)*(x2804)))+(((IkReal(-1.00000000000000))*(sj1)*(x2815))));
12584 evalcond[3]=((((r20)*(x2805)))+(((IkReal(-1.00000000000000))*(sj2)*(x2812)))+(((IkReal(-1.00000000000000))*(r20)*(x2814)))+(((r21)*(x2807)))+(((r22)*(x2806)))+(((x2808)*(x2810))));
12585 evalcond[4]=((((sj1)*(x2801)))+(((x2808)*(x2809)))+(((r02)*(x2806)))+(((cj1)*(cj2)*(x2802)))+(((IkReal(-1.00000000000000))*(r00)*(x2814)))+(((r01)*(x2807)))+(((r00)*(x2805))));
12586 evalcond[5]=((((r12)*(x2806)))+(((r10)*(x2805)))+(((IkReal(-1.00000000000000))*(cj1)*(x2801)))+(((x2803)*(x2808)))+(((cj2)*(x2813)))+(((r11)*(x2807)))+(((IkReal(-1.00000000000000))*(r10)*(x2814))));
12587 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12588 {
12589 continue;
12590 }
12591 }
12592 
12593 {
12594 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12595 vinfos[0].jointtype = 1;
12596 vinfos[0].foffset = j0;
12597 vinfos[0].indices[0] = _ij0[0];
12598 vinfos[0].indices[1] = _ij0[1];
12599 vinfos[0].maxsolutions = _nj0;
12600 vinfos[1].jointtype = 1;
12601 vinfos[1].foffset = j1;
12602 vinfos[1].indices[0] = _ij1[0];
12603 vinfos[1].indices[1] = _ij1[1];
12604 vinfos[1].maxsolutions = _nj1;
12605 vinfos[2].jointtype = 1;
12606 vinfos[2].foffset = j2;
12607 vinfos[2].indices[0] = _ij2[0];
12608 vinfos[2].indices[1] = _ij2[1];
12609 vinfos[2].maxsolutions = _nj2;
12610 vinfos[3].jointtype = 1;
12611 vinfos[3].foffset = j3;
12612 vinfos[3].indices[0] = _ij3[0];
12613 vinfos[3].indices[1] = _ij3[1];
12614 vinfos[3].maxsolutions = _nj3;
12615 vinfos[4].jointtype = 1;
12616 vinfos[4].foffset = j4;
12617 vinfos[4].indices[0] = _ij4[0];
12618 vinfos[4].indices[1] = _ij4[1];
12619 vinfos[4].maxsolutions = _nj4;
12620 vinfos[5].jointtype = 1;
12621 vinfos[5].foffset = j5;
12622 vinfos[5].indices[0] = _ij5[0];
12623 vinfos[5].indices[1] = _ij5[1];
12624 vinfos[5].maxsolutions = _nj5;
12625 vinfos[6].jointtype = 1;
12626 vinfos[6].foffset = j6;
12627 vinfos[6].indices[0] = _ij6[0];
12628 vinfos[6].indices[1] = _ij6[1];
12629 vinfos[6].maxsolutions = _nj6;
12630 std::vector<int> vfree(0);
12631 solutions.AddSolution(vinfos,vfree);
12632 }
12633 }
12634 }
12635 
12636 }
12637 
12638 }
12639 
12640 } else
12641 {
12642 {
12643 IkReal j3array[1], cj3array[1], sj3array[1];
12644 bool j3valid[1]={false};
12645 _nj3 = 1;
12646 IkReal x2816=((cj4)*(cj5));
12647 IkReal x2817=((r21)*(sj6));
12648 IkReal x2818=((IkReal(1.00000000000000))*(cj6)*(r20));
12649 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(sj5)*(x2818)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((sj5)*(x2817))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((x2816)*(x2817)))+(((IkReal(-1.00000000000000))*(x2816)*(x2818))))))) < IKFAST_ATAN2_MAGTHRESH )
12650     continue;
12651 j3array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(sj5)*(x2818)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((sj5)*(x2817)))))), ((gconst4)*(((((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((x2816)*(x2817)))+(((IkReal(-1.00000000000000))*(x2816)*(x2818)))))));
12652 sj3array[0]=IKsin(j3array[0]);
12653 cj3array[0]=IKcos(j3array[0]);
12654 if( j3array[0] > IKPI )
12655 {
12656     j3array[0]-=IK2PI;
12657 }
12658 else if( j3array[0] < -IKPI )
12659 {    j3array[0]+=IK2PI;
12660 }
12661 j3valid[0] = true;
12662 for(int ij3 = 0; ij3 < 1; ++ij3)
12663 {
12664 if( !j3valid[ij3] )
12665 {
12666     continue;
12667 }
12668 _ij3[0] = ij3; _ij3[1] = -1;
12669 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12670 {
12671 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12672 {
12673     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12674 }
12675 }
12676 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12677 {
12678 IkReal evalcond[6];
12679 IkReal x2819=IKsin(j3);
12680 IkReal x2820=IKcos(j3);
12681 IkReal x2821=((r11)*(sj6));
12682 IkReal x2822=((IkReal(1.00000000000000))*(sj5));
12683 IkReal x2823=((sj4)*(sj6));
12684 IkReal x2824=((cj4)*(sj5));
12685 IkReal x2825=((cj6)*(sj4));
12686 IkReal x2826=((cj4)*(cj5));
12687 IkReal x2827=((r01)*(sj6));
12688 IkReal x2828=((r21)*(sj6));
12689 IkReal x2829=((cj6)*(sj5));
12690 IkReal x2830=((IkReal(1.00000000000000))*(x2820));
12691 IkReal x2831=((sj1)*(x2820));
12692 IkReal x2832=((IkReal(1.00000000000000))*(cj6)*(x2826));
12693 IkReal x2833=((IkReal(1.00000000000000))*(cj2)*(x2819));
12694 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x2819)))+(((r20)*(x2829)))+(((IkReal(-1.00000000000000))*(x2822)*(x2828))));
12695 evalcond[1]=((((IkReal(-1.00000000000000))*(cj1)*(x2833)))+(((IkReal(-1.00000000000000))*(x2822)*(x2827)))+(((r00)*(x2829)))+(x2831)+(((cj5)*(r02))));
12696 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj1)*(x2830)))+(((IkReal(-1.00000000000000))*(sj1)*(x2833)))+(((r10)*(x2829)))+(((IkReal(-1.00000000000000))*(x2821)*(x2822))));
12697 evalcond[3]=((((IkReal(-1.00000000000000))*(sj2)*(x2830)))+(((r20)*(x2823)))+(((IkReal(-1.00000000000000))*(r20)*(x2832)))+(((r21)*(x2825)))+(((r22)*(x2824)))+(((x2826)*(x2828))));
12698 evalcond[4]=((((r02)*(x2824)))+(((IkReal(-1.00000000000000))*(r00)*(x2832)))+(((cj1)*(cj2)*(x2820)))+(((r00)*(x2823)))+(((sj1)*(x2819)))+(((r01)*(x2825)))+(((x2826)*(x2827))));
12699 evalcond[5]=((((x2821)*(x2826)))+(((r10)*(x2823)))+(((IkReal(-1.00000000000000))*(r10)*(x2832)))+(((r11)*(x2825)))+(((IkReal(-1.00000000000000))*(cj1)*(x2819)))+(((cj2)*(x2831)))+(((r12)*(x2824))));
12700 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
12701 {
12702 continue;
12703 }
12704 }
12705 
12706 {
12707 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12708 vinfos[0].jointtype = 1;
12709 vinfos[0].foffset = j0;
12710 vinfos[0].indices[0] = _ij0[0];
12711 vinfos[0].indices[1] = _ij0[1];
12712 vinfos[0].maxsolutions = _nj0;
12713 vinfos[1].jointtype = 1;
12714 vinfos[1].foffset = j1;
12715 vinfos[1].indices[0] = _ij1[0];
12716 vinfos[1].indices[1] = _ij1[1];
12717 vinfos[1].maxsolutions = _nj1;
12718 vinfos[2].jointtype = 1;
12719 vinfos[2].foffset = j2;
12720 vinfos[2].indices[0] = _ij2[0];
12721 vinfos[2].indices[1] = _ij2[1];
12722 vinfos[2].maxsolutions = _nj2;
12723 vinfos[3].jointtype = 1;
12724 vinfos[3].foffset = j3;
12725 vinfos[3].indices[0] = _ij3[0];
12726 vinfos[3].indices[1] = _ij3[1];
12727 vinfos[3].maxsolutions = _nj3;
12728 vinfos[4].jointtype = 1;
12729 vinfos[4].foffset = j4;
12730 vinfos[4].indices[0] = _ij4[0];
12731 vinfos[4].indices[1] = _ij4[1];
12732 vinfos[4].maxsolutions = _nj4;
12733 vinfos[5].jointtype = 1;
12734 vinfos[5].foffset = j5;
12735 vinfos[5].indices[0] = _ij5[0];
12736 vinfos[5].indices[1] = _ij5[1];
12737 vinfos[5].maxsolutions = _nj5;
12738 vinfos[6].jointtype = 1;
12739 vinfos[6].foffset = j6;
12740 vinfos[6].indices[0] = _ij6[0];
12741 vinfos[6].indices[1] = _ij6[1];
12742 vinfos[6].maxsolutions = _nj6;
12743 std::vector<int> vfree(0);
12744 solutions.AddSolution(vinfos,vfree);
12745 }
12746 }
12747 }
12748 
12749 }
12750 
12751 }
12752 }
12753 }
12754 
12755 }
12756 
12757 }
12758 }
12759 }
12760 
12761 }
12762 
12763 }
12764 
12765 } else
12766 {
12767 {
12768 IkReal j2array[1], cj2array[1], sj2array[1];
12769 bool j2valid[1]={false};
12770 _nj2 = 1;
12771 IkReal x2834=((IkReal(0.250000000000000))*(cj6));
12772 IkReal x2835=((IkReal(0.250000000000000))*(sj6));
12773 IkReal x2836=((IkReal(195.000000000000))*(sj5));
12774 IkReal x2837=((IkReal(195.000000000000))*(cj5));
12775 if( IKabs(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x2836)))+(((r02)*(x2837)))+(((IkReal(-500.000000000000))*(px)))+(((IkReal(-200.000000000000))*(sj1)))+(((r00)*(x2835)))+(((r01)*(x2834)))+(((cj6)*(r00)*(x2836))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj6)*(r20)*(x2836)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x2836)))+(((r20)*(x2835)))+(((r22)*(x2837)))+(((r21)*(x2834))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x2836)))+(((r02)*(x2837)))+(((IkReal(-500.000000000000))*(px)))+(((IkReal(-200.000000000000))*(sj1)))+(((r00)*(x2835)))+(((r01)*(x2834)))+(((cj6)*(r00)*(x2836)))))))+IKsqr(((((cj6)*(r20)*(x2836)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x2836)))+(((r20)*(x2835)))+(((r22)*(x2837)))+(((r21)*(x2834)))))-1) <= IKFAST_SINCOS_THRESH )
12776     continue;
12777 j2array[0]=IKatan2(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x2836)))+(((r02)*(x2837)))+(((IkReal(-500.000000000000))*(px)))+(((IkReal(-200.000000000000))*(sj1)))+(((r00)*(x2835)))+(((r01)*(x2834)))+(((cj6)*(r00)*(x2836)))))), ((((cj6)*(r20)*(x2836)))+(((IkReal(-500.000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x2836)))+(((r20)*(x2835)))+(((r22)*(x2837)))+(((r21)*(x2834)))));
12778 sj2array[0]=IKsin(j2array[0]);
12779 cj2array[0]=IKcos(j2array[0]);
12780 if( j2array[0] > IKPI )
12781 {
12782     j2array[0]-=IK2PI;
12783 }
12784 else if( j2array[0] < -IKPI )
12785 {    j2array[0]+=IK2PI;
12786 }
12787 j2valid[0] = true;
12788 for(int ij2 = 0; ij2 < 1; ++ij2)
12789 {
12790 if( !j2valid[ij2] )
12791 {
12792     continue;
12793 }
12794 _ij2[0] = ij2; _ij2[1] = -1;
12795 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
12796 {
12797 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
12798 {
12799     j2valid[iij2]=false; _ij2[1] = iij2; break; 
12800 }
12801 }
12802 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
12803 {
12804 IkReal evalcond[3];
12805 IkReal x2838=IKsin(j2);
12806 IkReal x2839=((IkReal(0.390000000000000))*(sj5));
12807 IkReal x2840=((IkReal(0.390000000000000))*(cj5));
12808 IkReal x2841=((IkReal(0.000500000000000000))*(cj6));
12809 IkReal x2842=((IkReal(0.000500000000000000))*(sj6));
12810 IkReal x2843=((IkReal(0.00200000000000000))*(x2838));
12811 evalcond[0]=((((cj6)*(r20)*(x2839)))+(((r21)*(x2841)))+(((r20)*(x2842)))+(((IkReal(-0.00200000000000000))*(IKcos(j2))))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x2839)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x2840))));
12812 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x2839)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x2842)))+(((r01)*(x2841)))+(((IkReal(-0.400000000000000))*(sj1)))+(((cj6)*(r00)*(x2839)))+(((r02)*(x2840)))+(((IkReal(-1.00000000000000))*(cj1)*(x2843))));
12813 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)*(x2843)))+(((r11)*(x2841)))+(((IkReal(0.400000000000000))*(cj1)))+(((r12)*(x2840)))+(((cj6)*(r10)*(x2839)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x2842)))+(((IkReal(-1.00000000000000))*(r11)*(sj6)*(x2839))));
12814 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
12815 {
12816 continue;
12817 }
12818 }
12819 
12820 {
12821 IkReal dummyeval[1];
12822 IkReal gconst0;
12823 IkReal x2844=(sj6)*(sj6);
12824 IkReal x2845=(cj6)*(cj6);
12825 IkReal x2846=((IkReal(1.00000000000000))*(r20));
12826 IkReal x2847=((r00)*(r21));
12827 IkReal x2848=((r22)*(sj5));
12828 IkReal x2849=((r02)*(sj5));
12829 IkReal x2850=((cj5)*(x2844));
12830 IkReal x2851=((cj5)*(x2845));
12831 gconst0=IKsign(((((IkReal(-1.00000000000000))*(sj6)*(x2846)*(x2849)))+(((x2847)*(x2850)))+(((x2847)*(x2851)))+(((r00)*(sj6)*(x2848)))+(((IkReal(-1.00000000000000))*(r01)*(x2846)*(x2851)))+(((IkReal(-1.00000000000000))*(r01)*(x2846)*(x2850)))+(((cj6)*(r01)*(x2848)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2849)))));
12832 IkReal x2852=(sj6)*(sj6);
12833 IkReal x2853=(cj6)*(cj6);
12834 IkReal x2854=((IkReal(1.00000000000000))*(r20));
12835 IkReal x2855=((r00)*(r21));
12836 IkReal x2856=((r22)*(sj5));
12837 IkReal x2857=((r02)*(sj5));
12838 IkReal x2858=((cj5)*(x2852));
12839 IkReal x2859=((cj5)*(x2853));
12840 dummyeval[0]=((((x2855)*(x2859)))+(((x2855)*(x2858)))+(((IkReal(-1.00000000000000))*(sj6)*(x2854)*(x2857)))+(((IkReal(-1.00000000000000))*(r01)*(x2854)*(x2859)))+(((IkReal(-1.00000000000000))*(r01)*(x2854)*(x2858)))+(((r00)*(sj6)*(x2856)))+(((cj6)*(r01)*(x2856)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2857))));
12841 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
12842 {
12843 {
12844 IkReal dummyeval[1];
12845 IkReal gconst1;
12846 IkReal x2860=(sj6)*(sj6);
12847 IkReal x2861=(cj6)*(cj6);
12848 IkReal x2862=((cj6)*(sj5));
12849 IkReal x2863=((r10)*(r21));
12850 IkReal x2864=((IkReal(1.00000000000000))*(r12));
12851 IkReal x2865=((sj5)*(sj6));
12852 IkReal x2866=((cj5)*(x2861));
12853 IkReal x2867=((IkReal(1.00000000000000))*(r11)*(r20));
12854 IkReal x2868=((cj5)*(x2860));
12855 gconst1=IKsign(((((IkReal(-1.00000000000000))*(r21)*(x2862)*(x2864)))+(((r11)*(r22)*(x2862)))+(((x2863)*(x2866)))+(((x2863)*(x2868)))+(((r10)*(r22)*(x2865)))+(((IkReal(-1.00000000000000))*(x2867)*(x2868)))+(((IkReal(-1.00000000000000))*(r20)*(x2864)*(x2865)))+(((IkReal(-1.00000000000000))*(x2866)*(x2867)))));
12856 IkReal x2869=(sj6)*(sj6);
12857 IkReal x2870=(cj6)*(cj6);
12858 IkReal x2871=((cj6)*(sj5));
12859 IkReal x2872=((r10)*(r21));
12860 IkReal x2873=((IkReal(1.00000000000000))*(r12));
12861 IkReal x2874=((sj5)*(sj6));
12862 IkReal x2875=((cj5)*(x2870));
12863 IkReal x2876=((IkReal(1.00000000000000))*(r11)*(r20));
12864 IkReal x2877=((cj5)*(x2869));
12865 dummyeval[0]=((((IkReal(-1.00000000000000))*(x2876)*(x2877)))+(((IkReal(-1.00000000000000))*(r21)*(x2871)*(x2873)))+(((x2872)*(x2877)))+(((x2872)*(x2875)))+(((IkReal(-1.00000000000000))*(r20)*(x2873)*(x2874)))+(((r10)*(r22)*(x2874)))+(((IkReal(-1.00000000000000))*(x2875)*(x2876)))+(((r11)*(r22)*(x2871))));
12866 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
12867 {
12868 {
12869 IkReal dummyeval[2];
12870 IkReal x2878=(sj1)*(sj1);
12871 IkReal x2879=(cj1)*(cj1);
12872 dummyeval[0]=((((cj2)*(x2879)))+(((cj2)*(x2878))));
12873 dummyeval[1]=((x2879)+(x2878));
12874 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
12875 {
12876 {
12877 IkReal evalcond[4];
12878 IkReal x2880=((IkReal(0.390000000000000))*(sj5));
12879 IkReal x2881=((IkReal(0.390000000000000))*(cj5));
12880 IkReal x2882=((IkReal(0.000500000000000000))*(cj6));
12881 IkReal x2883=((IkReal(0.000500000000000000))*(sj6));
12882 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
12883 evalcond[1]=((((r20)*(x2883)))+(((r22)*(x2881)))+(((r21)*(x2882)))+(((cj6)*(r20)*(x2880)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x2880))));
12884 evalcond[2]=((((cj6)*(r00)*(x2880)))+(((r00)*(x2883)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x2880)))+(((r01)*(x2882)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x2881)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1))));
12885 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(sj6)*(x2880)))+(((IkReal(0.400000000000000))*(cj1)))+(((r11)*(x2882)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj1)))+(((r10)*(x2883)))+(((r12)*(x2881)))+(((cj6)*(r10)*(x2880))));
12886 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
12887 {
12888 {
12889 IkReal dummyeval[1];
12890 IkReal gconst9;
12891 IkReal x2884=(sj6)*(sj6);
12892 IkReal x2885=(cj6)*(cj6);
12893 IkReal x2886=((IkReal(1.00000000000000))*(r20));
12894 IkReal x2887=((r00)*(r21));
12895 IkReal x2888=((r22)*(sj5));
12896 IkReal x2889=((r02)*(sj5));
12897 IkReal x2890=((cj5)*(x2884));
12898 IkReal x2891=((cj5)*(x2885));
12899 gconst9=IKsign(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2889)))+(((cj6)*(r01)*(x2888)))+(((x2887)*(x2891)))+(((x2887)*(x2890)))+(((IkReal(-1.00000000000000))*(r01)*(x2886)*(x2891)))+(((IkReal(-1.00000000000000))*(r01)*(x2886)*(x2890)))+(((r00)*(sj6)*(x2888)))+(((IkReal(-1.00000000000000))*(sj6)*(x2886)*(x2889)))));
12900 IkReal x2892=(sj6)*(sj6);
12901 IkReal x2893=(cj6)*(cj6);
12902 IkReal x2894=((IkReal(1.00000000000000))*(r20));
12903 IkReal x2895=((r00)*(r21));
12904 IkReal x2896=((r22)*(sj5));
12905 IkReal x2897=((r02)*(sj5));
12906 IkReal x2898=((cj5)*(x2892));
12907 IkReal x2899=((cj5)*(x2893));
12908 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2897)))+(((IkReal(-1.00000000000000))*(sj6)*(x2894)*(x2897)))+(((x2895)*(x2898)))+(((x2895)*(x2899)))+(((cj6)*(r01)*(x2896)))+(((IkReal(-1.00000000000000))*(r01)*(x2894)*(x2898)))+(((IkReal(-1.00000000000000))*(r01)*(x2894)*(x2899)))+(((r00)*(sj6)*(x2896))));
12909 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
12910 {
12911 {
12912 IkReal dummyeval[1];
12913 IkReal gconst10;
12914 IkReal x2900=(sj6)*(sj6);
12915 IkReal x2901=(cj6)*(cj6);
12916 IkReal x2902=((cj6)*(sj5));
12917 IkReal x2903=((r10)*(r21));
12918 IkReal x2904=((IkReal(1.00000000000000))*(r12));
12919 IkReal x2905=((sj5)*(sj6));
12920 IkReal x2906=((cj5)*(x2901));
12921 IkReal x2907=((IkReal(1.00000000000000))*(r11)*(r20));
12922 IkReal x2908=((cj5)*(x2900));
12923 gconst10=IKsign(((((r10)*(r22)*(x2905)))+(((IkReal(-1.00000000000000))*(x2906)*(x2907)))+(((IkReal(-1.00000000000000))*(r21)*(x2902)*(x2904)))+(((x2903)*(x2906)))+(((x2903)*(x2908)))+(((IkReal(-1.00000000000000))*(x2907)*(x2908)))+(((IkReal(-1.00000000000000))*(r20)*(x2904)*(x2905)))+(((r11)*(r22)*(x2902)))));
12924 IkReal x2909=(sj6)*(sj6);
12925 IkReal x2910=(cj6)*(cj6);
12926 IkReal x2911=((cj6)*(sj5));
12927 IkReal x2912=((r10)*(r21));
12928 IkReal x2913=((IkReal(1.00000000000000))*(r12));
12929 IkReal x2914=((sj5)*(sj6));
12930 IkReal x2915=((cj5)*(x2910));
12931 IkReal x2916=((IkReal(1.00000000000000))*(r11)*(r20));
12932 IkReal x2917=((cj5)*(x2909));
12933 dummyeval[0]=((((IkReal(-1.00000000000000))*(x2915)*(x2916)))+(((r10)*(r22)*(x2914)))+(((r11)*(r22)*(x2911)))+(((IkReal(-1.00000000000000))*(r20)*(x2913)*(x2914)))+(((IkReal(-1.00000000000000))*(r21)*(x2911)*(x2913)))+(((x2912)*(x2917)))+(((x2912)*(x2915)))+(((IkReal(-1.00000000000000))*(x2916)*(x2917))));
12934 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
12935 {
12936 {
12937 IkReal dummyeval[1];
12938 dummyeval[0]=cj1;
12939 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
12940 {
12941 {
12942 IkReal evalcond[5];
12943 IkReal x2918=((IkReal(0.000500000000000000))*(cj6));
12944 IkReal x2919=((IkReal(0.000500000000000000))*(sj6));
12945 IkReal x2920=((sj5)*(sj6));
12946 IkReal x2921=((cj5)*(r12));
12947 IkReal x2922=((IkReal(0.390000000000000))*(cj5));
12948 IkReal x2923=((IkReal(0.390000000000000))*(cj6)*(sj5));
12949 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
12950 evalcond[1]=((((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x2920)))+(x2921));
12951 evalcond[2]=((((r22)*(x2922)))+(((r20)*(x2919)))+(((IkReal(-0.390000000000000))*(r21)*(x2920)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x2923)))+(((r21)*(x2918))));
12952 evalcond[3]=((IkReal(-0.400000000000000))+(((r01)*(x2918)))+(((r00)*(x2919)))+(((r00)*(x2923)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.390000000000000))*(r01)*(x2920)))+(((r02)*(x2922))));
12953 evalcond[4]=((IkReal(-0.00200000000000000))+(((r11)*(x2918)))+(((IkReal(0.390000000000000))*(x2921)))+(((IkReal(-0.390000000000000))*(r11)*(x2920)))+(((r10)*(x2923)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x2919))));
12954 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
12955 {
12956 {
12957 IkReal j3array[1], cj3array[1], sj3array[1];
12958 bool j3valid[1]={false};
12959 _nj3 = 1;
12960 IkReal x2924=((sj5)*(sj6));
12961 IkReal x2925=((IkReal(1.00000000000000))*(cj5));
12962 IkReal x2926=((IkReal(1.00000000000000))*(cj6)*(sj5));
12963 if( IKabs(((((IkReal(-1.00000000000000))*(r20)*(x2926)))+(((r21)*(x2924)))+(((IkReal(-1.00000000000000))*(r22)*(x2925))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x2925)))+(((r01)*(x2924)))+(((IkReal(-1.00000000000000))*(r00)*(x2926))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r20)*(x2926)))+(((r21)*(x2924)))+(((IkReal(-1.00000000000000))*(r22)*(x2925)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x2925)))+(((r01)*(x2924)))+(((IkReal(-1.00000000000000))*(r00)*(x2926)))))-1) <= IKFAST_SINCOS_THRESH )
12964     continue;
12965 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r20)*(x2926)))+(((r21)*(x2924)))+(((IkReal(-1.00000000000000))*(r22)*(x2925)))), ((((IkReal(-1.00000000000000))*(r02)*(x2925)))+(((r01)*(x2924)))+(((IkReal(-1.00000000000000))*(r00)*(x2926)))));
12966 sj3array[0]=IKsin(j3array[0]);
12967 cj3array[0]=IKcos(j3array[0]);
12968 if( j3array[0] > IKPI )
12969 {
12970     j3array[0]-=IK2PI;
12971 }
12972 else if( j3array[0] < -IKPI )
12973 {    j3array[0]+=IK2PI;
12974 }
12975 j3valid[0] = true;
12976 for(int ij3 = 0; ij3 < 1; ++ij3)
12977 {
12978 if( !j3valid[ij3] )
12979 {
12980     continue;
12981 }
12982 _ij3[0] = ij3; _ij3[1] = -1;
12983 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12984 {
12985 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12986 {
12987     j3valid[iij3]=false; _ij3[1] = iij3; break; 
12988 }
12989 }
12990 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12991 {
12992 IkReal evalcond[2];
12993 IkReal x2927=((cj6)*(sj5));
12994 IkReal x2928=((IkReal(1.00000000000000))*(sj5)*(sj6));
12995 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x2928)))+(IKsin(j3))+(((r20)*(x2927))));
12996 evalcond[1]=((((r00)*(x2927)))+(IKcos(j3))+(((IkReal(-1.00000000000000))*(r01)*(x2928)))+(((cj5)*(r02))));
12997 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
12998 {
12999 continue;
13000 }
13001 }
13002 
13003 {
13004 IkReal dummyeval[1];
13005 IkReal gconst15;
13006 IkReal x2929=(sj6)*(sj6);
13007 IkReal x2930=(cj6)*(cj6);
13008 IkReal x2931=((r01)*(r10));
13009 IkReal x2932=((IkReal(1.00000000000000))*(r00));
13010 IkReal x2933=((r02)*(sj5));
13011 IkReal x2934=((r12)*(sj5));
13012 IkReal x2935=((cj5)*(x2929));
13013 IkReal x2936=((cj5)*(x2930));
13014 gconst15=IKsign(((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2934)))+(((r10)*(sj6)*(x2933)))+(((IkReal(-1.00000000000000))*(sj6)*(x2932)*(x2934)))+(((cj6)*(r11)*(x2933)))+(((IkReal(-1.00000000000000))*(r11)*(x2932)*(x2935)))+(((IkReal(-1.00000000000000))*(r11)*(x2932)*(x2936)))+(((x2931)*(x2936)))+(((x2931)*(x2935)))));
13015 IkReal x2937=(sj6)*(sj6);
13016 IkReal x2938=(cj6)*(cj6);
13017 IkReal x2939=((r01)*(r10));
13018 IkReal x2940=((IkReal(1.00000000000000))*(r00));
13019 IkReal x2941=((r02)*(sj5));
13020 IkReal x2942=((r12)*(sj5));
13021 IkReal x2943=((cj5)*(x2937));
13022 IkReal x2944=((cj5)*(x2938));
13023 dummyeval[0]=((((x2939)*(x2943)))+(((x2939)*(x2944)))+(((r10)*(sj6)*(x2941)))+(((cj6)*(r11)*(x2941)))+(((IkReal(-1.00000000000000))*(r11)*(x2940)*(x2943)))+(((IkReal(-1.00000000000000))*(r11)*(x2940)*(x2944)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x2942)))+(((IkReal(-1.00000000000000))*(sj6)*(x2940)*(x2942))));
13024 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13025 {
13026 {
13027 IkReal dummyeval[1];
13028 IkReal gconst16;
13029 IkReal x2945=(cj5)*(cj5);
13030 IkReal x2946=(sj6)*(sj6);
13031 IkReal x2947=(cj6)*(cj6);
13032 IkReal x2948=((r00)*(r20));
13033 IkReal x2949=((cj5)*(sj5));
13034 IkReal x2950=((r01)*(r21));
13035 IkReal x2951=((r21)*(sj6));
13036 IkReal x2952=((sj6)*(x2949));
13037 IkReal x2953=((IkReal(1.00000000000000))*(cj6)*(r00));
13038 IkReal x2954=((cj6)*(r01)*(r20)*(sj6));
13039 gconst16=IKsign(((((IkReal(-1.00000000000000))*(x2945)*(x2951)*(x2953)))+(((x2946)*(x2948)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x2949)))+(((x2947)*(x2950)))+(((x2945)*(x2947)*(x2948)))+(((r02)*(x2949)*(x2951)))+(((IkReal(-1.00000000000000))*(x2945)*(x2954)))+(((r01)*(r22)*(x2952)))+(((IkReal(-1.00000000000000))*(r22)*(x2949)*(x2953)))+(((x2945)*(x2946)*(x2950)))+(((r02)*(r22)*((sj5)*(sj5))))+(((cj6)*(r00)*(x2951)))+(x2954)));
13040 IkReal x2955=(cj5)*(cj5);
13041 IkReal x2956=(sj6)*(sj6);
13042 IkReal x2957=(cj6)*(cj6);
13043 IkReal x2958=((r00)*(r20));
13044 IkReal x2959=((cj5)*(sj5));
13045 IkReal x2960=((r01)*(r21));
13046 IkReal x2961=((r21)*(sj6));
13047 IkReal x2962=((sj6)*(x2959));
13048 IkReal x2963=((IkReal(1.00000000000000))*(cj6)*(r00));
13049 IkReal x2964=x2954;
13050 dummyeval[0]=((((cj6)*(r00)*(x2961)))+(((x2955)*(x2957)*(x2958)))+(((r02)*(x2959)*(x2961)))+(((r01)*(r22)*(x2962)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x2959)))+(((x2957)*(x2960)))+(((r02)*(r22)*((sj5)*(sj5))))+(((x2955)*(x2956)*(x2960)))+(((IkReal(-1.00000000000000))*(r22)*(x2959)*(x2963)))+(((IkReal(-1.00000000000000))*(x2955)*(x2961)*(x2963)))+(x2964)+(((x2956)*(x2958)))+(((IkReal(-1.00000000000000))*(x2955)*(x2964))));
13051 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13052 {
13053 continue;
13054 
13055 } else
13056 {
13057 {
13058 IkReal j4array[1], cj4array[1], sj4array[1];
13059 bool j4valid[1]={false};
13060 _nj4 = 1;
13061 IkReal x2965=((IkReal(1.00000000000000))*(sj3));
13062 if( IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2965)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2965))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2965)))+(((cj5)*(cj6)*(r20)*(sj3)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2965))))))) < IKFAST_ATAN2_MAGTHRESH )
13063     continue;
13064 j4array[0]=IKatan2(((gconst16)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x2965)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x2965)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x2965)))+(((cj5)*(cj6)*(r20)*(sj3)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x2965)))))));
13065 sj4array[0]=IKsin(j4array[0]);
13066 cj4array[0]=IKcos(j4array[0]);
13067 if( j4array[0] > IKPI )
13068 {
13069     j4array[0]-=IK2PI;
13070 }
13071 else if( j4array[0] < -IKPI )
13072 {    j4array[0]+=IK2PI;
13073 }
13074 j4valid[0] = true;
13075 for(int ij4 = 0; ij4 < 1; ++ij4)
13076 {
13077 if( !j4valid[ij4] )
13078 {
13079     continue;
13080 }
13081 _ij4[0] = ij4; _ij4[1] = -1;
13082 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13083 {
13084 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13085 {
13086     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13087 }
13088 }
13089 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13090 {
13091 IkReal evalcond[6];
13092 IkReal x2966=IKsin(j4);
13093 IkReal x2967=IKcos(j4);
13094 IkReal x2968=((IkReal(1.00000000000000))*(r00));
13095 IkReal x2969=((cj5)*(r01));
13096 IkReal x2970=((cj5)*(r11));
13097 IkReal x2971=((IkReal(1.00000000000000))*(cj5));
13098 IkReal x2972=((cj5)*(r21));
13099 IkReal x2973=((sj5)*(x2966));
13100 IkReal x2974=((cj6)*(x2966));
13101 IkReal x2975=((sj6)*(x2967));
13102 IkReal x2976=((sj6)*(x2966));
13103 IkReal x2977=((sj5)*(x2967));
13104 IkReal x2978=((cj6)*(x2967));
13105 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x2978)))+(((IkReal(-1.00000000000000))*(r20)*(x2971)*(x2974)))+(((r22)*(x2973)))+(((IkReal(-1.00000000000000))*(r20)*(x2975)))+(((x2972)*(x2976))));
13106 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x2971)*(x2978)))+(((r22)*(x2977)))+(((r20)*(x2976)))+(((IkReal(-1.00000000000000))*(cj3)))+(((x2972)*(x2975)))+(((r21)*(x2974))));
13107 evalcond[2]=((((r02)*(x2973)))+(((IkReal(-1.00000000000000))*(x2968)*(x2975)))+(((x2969)*(x2976)))+(((IkReal(-1.00000000000000))*(r01)*(x2978)))+(((IkReal(-1.00000000000000))*(cj5)*(x2968)*(x2974))));
13108 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x2978)))+(((x2970)*(x2976)))+(((r12)*(x2973)))+(((IkReal(-1.00000000000000))*(r10)*(x2975)))+(((IkReal(-1.00000000000000))*(r10)*(x2971)*(x2974))));
13109 evalcond[4]=((sj3)+(((r02)*(x2977)))+(((r01)*(x2974)))+(((x2969)*(x2975)))+(((r00)*(x2976)))+(((IkReal(-1.00000000000000))*(cj5)*(x2968)*(x2978))));
13110 evalcond[5]=((((x2970)*(x2975)))+(((r10)*(x2976)))+(((r12)*(x2977)))+(((r11)*(x2974)))+(((IkReal(-1.00000000000000))*(r10)*(x2971)*(x2978))));
13111 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13112 {
13113 continue;
13114 }
13115 }
13116 
13117 {
13118 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13119 vinfos[0].jointtype = 1;
13120 vinfos[0].foffset = j0;
13121 vinfos[0].indices[0] = _ij0[0];
13122 vinfos[0].indices[1] = _ij0[1];
13123 vinfos[0].maxsolutions = _nj0;
13124 vinfos[1].jointtype = 1;
13125 vinfos[1].foffset = j1;
13126 vinfos[1].indices[0] = _ij1[0];
13127 vinfos[1].indices[1] = _ij1[1];
13128 vinfos[1].maxsolutions = _nj1;
13129 vinfos[2].jointtype = 1;
13130 vinfos[2].foffset = j2;
13131 vinfos[2].indices[0] = _ij2[0];
13132 vinfos[2].indices[1] = _ij2[1];
13133 vinfos[2].maxsolutions = _nj2;
13134 vinfos[3].jointtype = 1;
13135 vinfos[3].foffset = j3;
13136 vinfos[3].indices[0] = _ij3[0];
13137 vinfos[3].indices[1] = _ij3[1];
13138 vinfos[3].maxsolutions = _nj3;
13139 vinfos[4].jointtype = 1;
13140 vinfos[4].foffset = j4;
13141 vinfos[4].indices[0] = _ij4[0];
13142 vinfos[4].indices[1] = _ij4[1];
13143 vinfos[4].maxsolutions = _nj4;
13144 vinfos[5].jointtype = 1;
13145 vinfos[5].foffset = j5;
13146 vinfos[5].indices[0] = _ij5[0];
13147 vinfos[5].indices[1] = _ij5[1];
13148 vinfos[5].maxsolutions = _nj5;
13149 vinfos[6].jointtype = 1;
13150 vinfos[6].foffset = j6;
13151 vinfos[6].indices[0] = _ij6[0];
13152 vinfos[6].indices[1] = _ij6[1];
13153 vinfos[6].maxsolutions = _nj6;
13154 std::vector<int> vfree(0);
13155 solutions.AddSolution(vinfos,vfree);
13156 }
13157 }
13158 }
13159 
13160 }
13161 
13162 }
13163 
13164 } else
13165 {
13166 {
13167 IkReal j4array[1], cj4array[1], sj4array[1];
13168 bool j4valid[1]={false};
13169 _nj4 = 1;
13170 IkReal x2979=((cj5)*(sj3));
13171 IkReal x2980=((IkReal(1.00000000000000))*(r10));
13172 if( IKabs(((gconst15)*(((((r12)*(sj3)*(sj5)))+(((r11)*(sj6)*(x2979)))+(((IkReal(-1.00000000000000))*(cj6)*(x2979)*(x2980))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(sj6)*(x2980))))))) < IKFAST_ATAN2_MAGTHRESH )
13173     continue;
13174 j4array[0]=IKatan2(((gconst15)*(((((r12)*(sj3)*(sj5)))+(((r11)*(sj6)*(x2979)))+(((IkReal(-1.00000000000000))*(cj6)*(x2979)*(x2980)))))), ((gconst15)*(((((IkReal(-1.00000000000000))*(cj6)*(r11)*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(sj6)*(x2980)))))));
13175 sj4array[0]=IKsin(j4array[0]);
13176 cj4array[0]=IKcos(j4array[0]);
13177 if( j4array[0] > IKPI )
13178 {
13179     j4array[0]-=IK2PI;
13180 }
13181 else if( j4array[0] < -IKPI )
13182 {    j4array[0]+=IK2PI;
13183 }
13184 j4valid[0] = true;
13185 for(int ij4 = 0; ij4 < 1; ++ij4)
13186 {
13187 if( !j4valid[ij4] )
13188 {
13189     continue;
13190 }
13191 _ij4[0] = ij4; _ij4[1] = -1;
13192 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13193 {
13194 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13195 {
13196     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13197 }
13198 }
13199 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13200 {
13201 IkReal evalcond[6];
13202 IkReal x2981=IKsin(j4);
13203 IkReal x2982=IKcos(j4);
13204 IkReal x2983=((IkReal(1.00000000000000))*(r00));
13205 IkReal x2984=((cj5)*(r01));
13206 IkReal x2985=((cj5)*(r11));
13207 IkReal x2986=((IkReal(1.00000000000000))*(cj5));
13208 IkReal x2987=((cj5)*(r21));
13209 IkReal x2988=((sj5)*(x2981));
13210 IkReal x2989=((cj6)*(x2981));
13211 IkReal x2990=((sj6)*(x2982));
13212 IkReal x2991=((sj6)*(x2981));
13213 IkReal x2992=((sj5)*(x2982));
13214 IkReal x2993=((cj6)*(x2982));
13215 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x2986)*(x2989)))+(((x2987)*(x2991)))+(((IkReal(-1.00000000000000))*(r21)*(x2993)))+(((IkReal(-1.00000000000000))*(r20)*(x2990)))+(((r22)*(x2988))));
13216 evalcond[1]=((((x2987)*(x2990)))+(((r20)*(x2991)))+(((IkReal(-1.00000000000000))*(r20)*(x2986)*(x2993)))+(((r22)*(x2992)))+(((IkReal(-1.00000000000000))*(cj3)))+(((r21)*(x2989))));
13217 evalcond[2]=((((IkReal(-1.00000000000000))*(x2983)*(x2990)))+(((x2984)*(x2991)))+(((r02)*(x2988)))+(((IkReal(-1.00000000000000))*(cj5)*(x2983)*(x2989)))+(((IkReal(-1.00000000000000))*(r01)*(x2993))));
13218 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x2986)*(x2989)))+(((x2985)*(x2991)))+(((IkReal(-1.00000000000000))*(r10)*(x2990)))+(((r12)*(x2988)))+(((IkReal(-1.00000000000000))*(r11)*(x2993))));
13219 evalcond[4]=((sj3)+(((r00)*(x2991)))+(((r01)*(x2989)))+(((IkReal(-1.00000000000000))*(cj5)*(x2983)*(x2993)))+(((r02)*(x2992)))+(((x2984)*(x2990))));
13220 evalcond[5]=((((r11)*(x2989)))+(((r12)*(x2992)))+(((x2985)*(x2990)))+(((IkReal(-1.00000000000000))*(r10)*(x2986)*(x2993)))+(((r10)*(x2991))));
13221 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13222 {
13223 continue;
13224 }
13225 }
13226 
13227 {
13228 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13229 vinfos[0].jointtype = 1;
13230 vinfos[0].foffset = j0;
13231 vinfos[0].indices[0] = _ij0[0];
13232 vinfos[0].indices[1] = _ij0[1];
13233 vinfos[0].maxsolutions = _nj0;
13234 vinfos[1].jointtype = 1;
13235 vinfos[1].foffset = j1;
13236 vinfos[1].indices[0] = _ij1[0];
13237 vinfos[1].indices[1] = _ij1[1];
13238 vinfos[1].maxsolutions = _nj1;
13239 vinfos[2].jointtype = 1;
13240 vinfos[2].foffset = j2;
13241 vinfos[2].indices[0] = _ij2[0];
13242 vinfos[2].indices[1] = _ij2[1];
13243 vinfos[2].maxsolutions = _nj2;
13244 vinfos[3].jointtype = 1;
13245 vinfos[3].foffset = j3;
13246 vinfos[3].indices[0] = _ij3[0];
13247 vinfos[3].indices[1] = _ij3[1];
13248 vinfos[3].maxsolutions = _nj3;
13249 vinfos[4].jointtype = 1;
13250 vinfos[4].foffset = j4;
13251 vinfos[4].indices[0] = _ij4[0];
13252 vinfos[4].indices[1] = _ij4[1];
13253 vinfos[4].maxsolutions = _nj4;
13254 vinfos[5].jointtype = 1;
13255 vinfos[5].foffset = j5;
13256 vinfos[5].indices[0] = _ij5[0];
13257 vinfos[5].indices[1] = _ij5[1];
13258 vinfos[5].maxsolutions = _nj5;
13259 vinfos[6].jointtype = 1;
13260 vinfos[6].foffset = j6;
13261 vinfos[6].indices[0] = _ij6[0];
13262 vinfos[6].indices[1] = _ij6[1];
13263 vinfos[6].maxsolutions = _nj6;
13264 std::vector<int> vfree(0);
13265 solutions.AddSolution(vinfos,vfree);
13266 }
13267 }
13268 }
13269 
13270 }
13271 
13272 }
13273 }
13274 }
13275 
13276 } else
13277 {
13278 IkReal x2994=((IkReal(0.000500000000000000))*(cj6));
13279 IkReal x2995=((IkReal(0.000500000000000000))*(sj6));
13280 IkReal x2996=((sj5)*(sj6));
13281 IkReal x2997=((cj5)*(r12));
13282 IkReal x2998=((IkReal(0.390000000000000))*(cj5));
13283 IkReal x2999=((IkReal(0.390000000000000))*(cj6)*(sj5));
13284 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
13285 evalcond[1]=((((cj6)*(r10)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x2996)))+(x2997));
13286 evalcond[2]=((((IkReal(-0.390000000000000))*(r21)*(x2996)))+(((r20)*(x2999)))+(((r20)*(x2995)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x2994)))+(((r22)*(x2998))));
13287 evalcond[3]=((IkReal(0.400000000000000))+(((IkReal(-0.390000000000000))*(r01)*(x2996)))+(((r00)*(x2995)))+(((r00)*(x2999)))+(((r01)*(x2994)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x2998))));
13288 evalcond[4]=((IkReal(0.00200000000000000))+(((IkReal(0.390000000000000))*(x2997)))+(((IkReal(-0.390000000000000))*(r11)*(x2996)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x2995)))+(((r10)*(x2999)))+(((r11)*(x2994))));
13289 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
13290 {
13291 {
13292 IkReal j3array[1], cj3array[1], sj3array[1];
13293 bool j3valid[1]={false};
13294 _nj3 = 1;
13295 IkReal x3000=((IkReal(1.00000000000000))*(sj5));
13296 if( IKabs(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x3000))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x3000)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x3000)))))+IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x3000)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
13297     continue;
13298 j3array[0]=IKatan2(((((r21)*(sj5)*(sj6)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(cj6)*(r20)*(x3000)))), ((((IkReal(-1.00000000000000))*(r01)*(sj6)*(x3000)))+(((cj6)*(r00)*(sj5)))+(((cj5)*(r02)))));
13299 sj3array[0]=IKsin(j3array[0]);
13300 cj3array[0]=IKcos(j3array[0]);
13301 if( j3array[0] > IKPI )
13302 {
13303     j3array[0]-=IK2PI;
13304 }
13305 else if( j3array[0] < -IKPI )
13306 {    j3array[0]+=IK2PI;
13307 }
13308 j3valid[0] = true;
13309 for(int ij3 = 0; ij3 < 1; ++ij3)
13310 {
13311 if( !j3valid[ij3] )
13312 {
13313     continue;
13314 }
13315 _ij3[0] = ij3; _ij3[1] = -1;
13316 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13317 {
13318 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13319 {
13320     j3valid[iij3]=false; _ij3[1] = iij3; break; 
13321 }
13322 }
13323 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13324 {
13325 IkReal evalcond[2];
13326 IkReal x3001=((cj6)*(sj5));
13327 IkReal x3002=((IkReal(1.00000000000000))*(sj5)*(sj6));
13328 evalcond[0]=((((cj5)*(r22)))+(IKsin(j3))+(((r20)*(x3001)))+(((IkReal(-1.00000000000000))*(r21)*(x3002))));
13329 evalcond[1]=((((r00)*(x3001)))+(((IkReal(-1.00000000000000))*(r01)*(x3002)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((cj5)*(r02))));
13330 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
13331 {
13332 continue;
13333 }
13334 }
13335 
13336 {
13337 IkReal dummyeval[1];
13338 IkReal gconst19;
13339 IkReal x3003=(cj6)*(cj6);
13340 IkReal x3004=(sj6)*(sj6);
13341 IkReal x3005=((r11)*(r20));
13342 IkReal x3006=((r12)*(sj5));
13343 IkReal x3007=((IkReal(1.00000000000000))*(r10));
13344 IkReal x3008=((r22)*(sj5));
13345 IkReal x3009=((cj5)*(x3004));
13346 IkReal x3010=((cj5)*(x3003));
13347 gconst19=IKsign(((((IkReal(-1.00000000000000))*(r21)*(x3007)*(x3010)))+(((cj6)*(r21)*(x3006)))+(((IkReal(-1.00000000000000))*(r21)*(x3007)*(x3009)))+(((IkReal(-1.00000000000000))*(sj6)*(x3007)*(x3008)))+(((x3005)*(x3009)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3008)))+(((x3005)*(x3010)))+(((r20)*(sj6)*(x3006)))));
13348 IkReal x3011=(cj6)*(cj6);
13349 IkReal x3012=(sj6)*(sj6);
13350 IkReal x3013=((r11)*(r20));
13351 IkReal x3014=((r12)*(sj5));
13352 IkReal x3015=((IkReal(1.00000000000000))*(r10));
13353 IkReal x3016=((r22)*(sj5));
13354 IkReal x3017=((cj5)*(x3012));
13355 IkReal x3018=((cj5)*(x3011));
13356 dummyeval[0]=((((cj6)*(r21)*(x3014)))+(((IkReal(-1.00000000000000))*(sj6)*(x3015)*(x3016)))+(((x3013)*(x3017)))+(((x3013)*(x3018)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3016)))+(((IkReal(-1.00000000000000))*(r21)*(x3015)*(x3017)))+(((IkReal(-1.00000000000000))*(r21)*(x3015)*(x3018)))+(((r20)*(sj6)*(x3014))));
13357 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13358 {
13359 {
13360 IkReal dummyeval[1];
13361 IkReal gconst20;
13362 IkReal x3019=(cj5)*(cj5);
13363 IkReal x3020=(r11)*(r11);
13364 IkReal x3021=(sj6)*(sj6);
13365 IkReal x3022=(cj6)*(cj6);
13366 IkReal x3023=(r10)*(r10);
13367 IkReal x3024=((r11)*(sj6));
13368 IkReal x3025=((IkReal(1.00000000000000))*(x3021));
13369 IkReal x3026=((IkReal(1.00000000000000))*(x3022));
13370 IkReal x3027=((IkReal(2.00000000000000))*(cj6)*(r10));
13371 IkReal x3028=((cj5)*(r12)*(sj5));
13372 gconst20=IKsign(((((IkReal(-1.00000000000000))*(x3019)*(x3020)*(x3025)))+(((IkReal(-1.00000000000000))*(x3019)*(x3023)*(x3026)))+(((x3019)*(x3024)*(x3027)))+(((x3027)*(x3028)))+(((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3020)*(x3026)))+(((IkReal(-2.00000000000000))*(x3024)*(x3028)))+(((IkReal(-1.00000000000000))*(x3024)*(x3027)))+(((IkReal(-1.00000000000000))*(x3023)*(x3025)))));
13373 IkReal x3029=(cj5)*(cj5);
13374 IkReal x3030=(r11)*(r11);
13375 IkReal x3031=(sj6)*(sj6);
13376 IkReal x3032=(cj6)*(cj6);
13377 IkReal x3033=(r10)*(r10);
13378 IkReal x3034=((r11)*(sj6));
13379 IkReal x3035=((IkReal(1.00000000000000))*(x3031));
13380 IkReal x3036=((IkReal(1.00000000000000))*(x3032));
13381 IkReal x3037=((IkReal(2.00000000000000))*(cj6)*(r10));
13382 IkReal x3038=((cj5)*(r12)*(sj5));
13383 dummyeval[0]=((((IkReal(-1.00000000000000))*(x3030)*(x3036)))+(((IkReal(-1.00000000000000))*((r12)*(r12))*((sj5)*(sj5))))+(((x3037)*(x3038)))+(((IkReal(-1.00000000000000))*(x3029)*(x3030)*(x3035)))+(((x3029)*(x3034)*(x3037)))+(((IkReal(-1.00000000000000))*(x3033)*(x3035)))+(((IkReal(-1.00000000000000))*(x3034)*(x3037)))+(((IkReal(-2.00000000000000))*(x3034)*(x3038)))+(((IkReal(-1.00000000000000))*(x3029)*(x3033)*(x3036))));
13384 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13385 {
13386 continue;
13387 
13388 } else
13389 {
13390 {
13391 IkReal j4array[1], cj4array[1], sj4array[1];
13392 bool j4valid[1]={false};
13393 _nj4 = 1;
13394 IkReal x3039=((IkReal(1.00000000000000))*(r10));
13395 if( IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x3039)))+(((cj5)*(r11)*(sj6)))+(((r12)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(sj6)*(x3039)))+(((IkReal(-1.00000000000000))*(cj6)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
13396     continue;
13397 j4array[0]=IKatan2(((gconst20)*(((((IkReal(-1.00000000000000))*(cj5)*(cj6)*(x3039)))+(((cj5)*(r11)*(sj6)))+(((r12)*(sj5)))))), ((gconst20)*(((((IkReal(-1.00000000000000))*(sj6)*(x3039)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)))))));
13398 sj4array[0]=IKsin(j4array[0]);
13399 cj4array[0]=IKcos(j4array[0]);
13400 if( j4array[0] > IKPI )
13401 {
13402     j4array[0]-=IK2PI;
13403 }
13404 else if( j4array[0] < -IKPI )
13405 {    j4array[0]+=IK2PI;
13406 }
13407 j4valid[0] = true;
13408 for(int ij4 = 0; ij4 < 1; ++ij4)
13409 {
13410 if( !j4valid[ij4] )
13411 {
13412     continue;
13413 }
13414 _ij4[0] = ij4; _ij4[1] = -1;
13415 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13416 {
13417 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13418 {
13419     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13420 }
13421 }
13422 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13423 {
13424 IkReal evalcond[6];
13425 IkReal x3040=IKsin(j4);
13426 IkReal x3041=IKcos(j4);
13427 IkReal x3042=((IkReal(1.00000000000000))*(r00));
13428 IkReal x3043=((cj5)*(r01));
13429 IkReal x3044=((cj5)*(r11));
13430 IkReal x3045=((IkReal(1.00000000000000))*(cj5));
13431 IkReal x3046=((cj5)*(r21));
13432 IkReal x3047=((sj5)*(x3040));
13433 IkReal x3048=((cj6)*(x3040));
13434 IkReal x3049=((sj6)*(x3041));
13435 IkReal x3050=((sj6)*(x3040));
13436 IkReal x3051=((sj5)*(x3041));
13437 IkReal x3052=((cj6)*(x3041));
13438 evalcond[0]=((((r22)*(x3047)))+(((IkReal(-1.00000000000000))*(r20)*(x3045)*(x3048)))+(((IkReal(-1.00000000000000))*(r21)*(x3052)))+(((IkReal(-1.00000000000000))*(r20)*(x3049)))+(((x3046)*(x3050))));
13439 evalcond[1]=((((r21)*(x3048)))+(((x3046)*(x3049)))+(((r22)*(x3051)))+(((IkReal(-1.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(r20)*(x3045)*(x3052)))+(((r20)*(x3050))));
13440 evalcond[2]=((((IkReal(-1.00000000000000))*(x3042)*(x3049)))+(((IkReal(-1.00000000000000))*(r01)*(x3052)))+(((IkReal(-1.00000000000000))*(cj5)*(x3042)*(x3048)))+(((x3043)*(x3050)))+(((r02)*(x3047))));
13441 evalcond[3]=((IkReal(1.00000000000000))+(((x3044)*(x3050)))+(((IkReal(-1.00000000000000))*(r10)*(x3045)*(x3048)))+(((IkReal(-1.00000000000000))*(r11)*(x3052)))+(((IkReal(-1.00000000000000))*(r10)*(x3049)))+(((r12)*(x3047))));
13442 evalcond[4]=((((x3043)*(x3049)))+(((IkReal(-1.00000000000000))*(sj3)))+(((r02)*(x3051)))+(((r00)*(x3050)))+(((IkReal(-1.00000000000000))*(cj5)*(x3042)*(x3052)))+(((r01)*(x3048))));
13443 evalcond[5]=((((r12)*(x3051)))+(((r10)*(x3050)))+(((x3044)*(x3049)))+(((IkReal(-1.00000000000000))*(r10)*(x3045)*(x3052)))+(((r11)*(x3048))));
13444 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13445 {
13446 continue;
13447 }
13448 }
13449 
13450 {
13451 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13452 vinfos[0].jointtype = 1;
13453 vinfos[0].foffset = j0;
13454 vinfos[0].indices[0] = _ij0[0];
13455 vinfos[0].indices[1] = _ij0[1];
13456 vinfos[0].maxsolutions = _nj0;
13457 vinfos[1].jointtype = 1;
13458 vinfos[1].foffset = j1;
13459 vinfos[1].indices[0] = _ij1[0];
13460 vinfos[1].indices[1] = _ij1[1];
13461 vinfos[1].maxsolutions = _nj1;
13462 vinfos[2].jointtype = 1;
13463 vinfos[2].foffset = j2;
13464 vinfos[2].indices[0] = _ij2[0];
13465 vinfos[2].indices[1] = _ij2[1];
13466 vinfos[2].maxsolutions = _nj2;
13467 vinfos[3].jointtype = 1;
13468 vinfos[3].foffset = j3;
13469 vinfos[3].indices[0] = _ij3[0];
13470 vinfos[3].indices[1] = _ij3[1];
13471 vinfos[3].maxsolutions = _nj3;
13472 vinfos[4].jointtype = 1;
13473 vinfos[4].foffset = j4;
13474 vinfos[4].indices[0] = _ij4[0];
13475 vinfos[4].indices[1] = _ij4[1];
13476 vinfos[4].maxsolutions = _nj4;
13477 vinfos[5].jointtype = 1;
13478 vinfos[5].foffset = j5;
13479 vinfos[5].indices[0] = _ij5[0];
13480 vinfos[5].indices[1] = _ij5[1];
13481 vinfos[5].maxsolutions = _nj5;
13482 vinfos[6].jointtype = 1;
13483 vinfos[6].foffset = j6;
13484 vinfos[6].indices[0] = _ij6[0];
13485 vinfos[6].indices[1] = _ij6[1];
13486 vinfos[6].maxsolutions = _nj6;
13487 std::vector<int> vfree(0);
13488 solutions.AddSolution(vinfos,vfree);
13489 }
13490 }
13491 }
13492 
13493 }
13494 
13495 }
13496 
13497 } else
13498 {
13499 {
13500 IkReal j4array[1], cj4array[1], sj4array[1];
13501 bool j4valid[1]={false};
13502 _nj4 = 1;
13503 IkReal x3053=((cj3)*(sj6));
13504 IkReal x3054=((IkReal(1.00000000000000))*(r10));
13505 IkReal x3055=((cj3)*(cj6));
13506 if( IKabs(((gconst19)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3054)*(x3055)))+(((cj5)*(r11)*(x3053))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((IkReal(-1.00000000000000))*(x3053)*(x3054)))+(((IkReal(-1.00000000000000))*(r11)*(x3055))))))) < IKFAST_ATAN2_MAGTHRESH )
13507     continue;
13508 j4array[0]=IKatan2(((gconst19)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3054)*(x3055)))+(((cj5)*(r11)*(x3053)))))), ((gconst19)*(((((IkReal(-1.00000000000000))*(x3053)*(x3054)))+(((IkReal(-1.00000000000000))*(r11)*(x3055)))))));
13509 sj4array[0]=IKsin(j4array[0]);
13510 cj4array[0]=IKcos(j4array[0]);
13511 if( j4array[0] > IKPI )
13512 {
13513     j4array[0]-=IK2PI;
13514 }
13515 else if( j4array[0] < -IKPI )
13516 {    j4array[0]+=IK2PI;
13517 }
13518 j4valid[0] = true;
13519 for(int ij4 = 0; ij4 < 1; ++ij4)
13520 {
13521 if( !j4valid[ij4] )
13522 {
13523     continue;
13524 }
13525 _ij4[0] = ij4; _ij4[1] = -1;
13526 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13527 {
13528 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13529 {
13530     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13531 }
13532 }
13533 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13534 {
13535 IkReal evalcond[6];
13536 IkReal x3056=IKsin(j4);
13537 IkReal x3057=IKcos(j4);
13538 IkReal x3058=((IkReal(1.00000000000000))*(r00));
13539 IkReal x3059=((cj5)*(r01));
13540 IkReal x3060=((cj5)*(r11));
13541 IkReal x3061=((IkReal(1.00000000000000))*(cj5));
13542 IkReal x3062=((cj5)*(r21));
13543 IkReal x3063=((sj5)*(x3056));
13544 IkReal x3064=((cj6)*(x3056));
13545 IkReal x3065=((sj6)*(x3057));
13546 IkReal x3066=((sj6)*(x3056));
13547 IkReal x3067=((sj5)*(x3057));
13548 IkReal x3068=((cj6)*(x3057));
13549 evalcond[0]=((((r22)*(x3063)))+(((IkReal(-1.00000000000000))*(r21)*(x3068)))+(((IkReal(-1.00000000000000))*(r20)*(x3061)*(x3064)))+(((IkReal(-1.00000000000000))*(r20)*(x3065)))+(((x3062)*(x3066))));
13550 evalcond[1]=((((r22)*(x3067)))+(((IkReal(-1.00000000000000))*(r20)*(x3061)*(x3068)))+(((r20)*(x3066)))+(((x3062)*(x3065)))+(((IkReal(-1.00000000000000))*(cj3)))+(((r21)*(x3064))));
13551 evalcond[2]=((((IkReal(-1.00000000000000))*(x3058)*(x3065)))+(((IkReal(-1.00000000000000))*(cj5)*(x3058)*(x3064)))+(((x3059)*(x3066)))+(((IkReal(-1.00000000000000))*(r01)*(x3068)))+(((r02)*(x3063))));
13552 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x3061)*(x3064)))+(((r12)*(x3063)))+(((x3060)*(x3066)))+(((IkReal(-1.00000000000000))*(r10)*(x3065)))+(((IkReal(-1.00000000000000))*(r11)*(x3068))));
13553 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((r00)*(x3066)))+(((r01)*(x3064)))+(((IkReal(-1.00000000000000))*(cj5)*(x3058)*(x3068)))+(((x3059)*(x3065)))+(((r02)*(x3067))));
13554 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3061)*(x3068)))+(((r11)*(x3064)))+(((r10)*(x3066)))+(((r12)*(x3067)))+(((x3060)*(x3065))));
13555 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13556 {
13557 continue;
13558 }
13559 }
13560 
13561 {
13562 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13563 vinfos[0].jointtype = 1;
13564 vinfos[0].foffset = j0;
13565 vinfos[0].indices[0] = _ij0[0];
13566 vinfos[0].indices[1] = _ij0[1];
13567 vinfos[0].maxsolutions = _nj0;
13568 vinfos[1].jointtype = 1;
13569 vinfos[1].foffset = j1;
13570 vinfos[1].indices[0] = _ij1[0];
13571 vinfos[1].indices[1] = _ij1[1];
13572 vinfos[1].maxsolutions = _nj1;
13573 vinfos[2].jointtype = 1;
13574 vinfos[2].foffset = j2;
13575 vinfos[2].indices[0] = _ij2[0];
13576 vinfos[2].indices[1] = _ij2[1];
13577 vinfos[2].maxsolutions = _nj2;
13578 vinfos[3].jointtype = 1;
13579 vinfos[3].foffset = j3;
13580 vinfos[3].indices[0] = _ij3[0];
13581 vinfos[3].indices[1] = _ij3[1];
13582 vinfos[3].maxsolutions = _nj3;
13583 vinfos[4].jointtype = 1;
13584 vinfos[4].foffset = j4;
13585 vinfos[4].indices[0] = _ij4[0];
13586 vinfos[4].indices[1] = _ij4[1];
13587 vinfos[4].maxsolutions = _nj4;
13588 vinfos[5].jointtype = 1;
13589 vinfos[5].foffset = j5;
13590 vinfos[5].indices[0] = _ij5[0];
13591 vinfos[5].indices[1] = _ij5[1];
13592 vinfos[5].maxsolutions = _nj5;
13593 vinfos[6].jointtype = 1;
13594 vinfos[6].foffset = j6;
13595 vinfos[6].indices[0] = _ij6[0];
13596 vinfos[6].indices[1] = _ij6[1];
13597 vinfos[6].maxsolutions = _nj6;
13598 std::vector<int> vfree(0);
13599 solutions.AddSolution(vinfos,vfree);
13600 }
13601 }
13602 }
13603 
13604 }
13605 
13606 }
13607 }
13608 }
13609 
13610 } else
13611 {
13612 if( 1 )
13613 {
13614 continue;
13615 
13616 } else
13617 {
13618 }
13619 }
13620 }
13621 }
13622 
13623 } else
13624 {
13625 {
13626 IkReal j3array[1], cj3array[1], sj3array[1];
13627 bool j3valid[1]={false};
13628 _nj3 = 1;
13629 IkReal x3069=((cj6)*(sj5));
13630 IkReal x3070=((sj5)*(sj6));
13631 if( IKabs(((((IkReal(-1.00000000000000))*(r20)*(x3069)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((r21)*(x3070))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x3070)))+(((r10)*(x3069))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r20)*(x3069)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((r21)*(x3070)))))+IKsqr(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x3070)))+(((r10)*(x3069)))))))-1) <= IKFAST_SINCOS_THRESH )
13632     continue;
13633 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r20)*(x3069)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((r21)*(x3070)))), ((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x3070)))+(((r10)*(x3069)))))));
13634 sj3array[0]=IKsin(j3array[0]);
13635 cj3array[0]=IKcos(j3array[0]);
13636 if( j3array[0] > IKPI )
13637 {
13638     j3array[0]-=IK2PI;
13639 }
13640 else if( j3array[0] < -IKPI )
13641 {    j3array[0]+=IK2PI;
13642 }
13643 j3valid[0] = true;
13644 for(int ij3 = 0; ij3 < 1; ++ij3)
13645 {
13646 if( !j3valid[ij3] )
13647 {
13648     continue;
13649 }
13650 _ij3[0] = ij3; _ij3[1] = -1;
13651 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13652 {
13653 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13654 {
13655     j3valid[iij3]=false; _ij3[1] = iij3; break; 
13656 }
13657 }
13658 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13659 {
13660 IkReal evalcond[3];
13661 IkReal x3071=IKcos(j3);
13662 IkReal x3072=((cj6)*(sj5));
13663 IkReal x3073=((IkReal(1.00000000000000))*(sj5)*(sj6));
13664 evalcond[0]=((((cj5)*(r22)))+(IKsin(j3))+(((r20)*(x3072)))+(((IkReal(-1.00000000000000))*(r21)*(x3073))));
13665 evalcond[1]=((((r00)*(x3072)))+(((IkReal(-1.00000000000000))*(r01)*(x3073)))+(((sj1)*(x3071)))+(((cj5)*(r02))));
13666 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x3072)))+(((IkReal(-1.00000000000000))*(r11)*(x3073)))+(((IkReal(-1.00000000000000))*(cj1)*(x3071))));
13667 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
13668 {
13669 continue;
13670 }
13671 }
13672 
13673 {
13674 IkReal dummyeval[1];
13675 IkReal gconst11;
13676 IkReal x3074=(cj5)*(cj5);
13677 IkReal x3075=(r21)*(r21);
13678 IkReal x3076=(sj6)*(sj6);
13679 IkReal x3077=(cj6)*(cj6);
13680 IkReal x3078=(r20)*(r20);
13681 IkReal x3079=((cj6)*(r20));
13682 IkReal x3080=((IkReal(1.00000000000000))*(x3077));
13683 IkReal x3081=((IkReal(2.00000000000000))*(r21)*(sj6));
13684 IkReal x3082=((cj5)*(r22)*(sj5));
13685 IkReal x3083=((IkReal(1.00000000000000))*(x3076));
13686 gconst11=IKsign(((((IkReal(2.00000000000000))*(x3079)*(x3082)))+(((IkReal(-1.00000000000000))*(x3079)*(x3081)))+(((IkReal(-1.00000000000000))*(x3075)*(x3080)))+(((IkReal(-1.00000000000000))*(x3078)*(x3083)))+(((IkReal(-1.00000000000000))*(x3074)*(x3078)*(x3080)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3074)*(x3075)*(x3083)))+(((IkReal(-1.00000000000000))*(x3081)*(x3082)))+(((x3074)*(x3079)*(x3081)))));
13687 IkReal x3084=(cj5)*(cj5);
13688 IkReal x3085=(r21)*(r21);
13689 IkReal x3086=(sj6)*(sj6);
13690 IkReal x3087=(cj6)*(cj6);
13691 IkReal x3088=(r20)*(r20);
13692 IkReal x3089=((cj6)*(r20));
13693 IkReal x3090=((IkReal(1.00000000000000))*(x3087));
13694 IkReal x3091=((IkReal(2.00000000000000))*(r21)*(sj6));
13695 IkReal x3092=((cj5)*(r22)*(sj5));
13696 IkReal x3093=((IkReal(1.00000000000000))*(x3086));
13697 dummyeval[0]=((((IkReal(-1.00000000000000))*(x3085)*(x3090)))+(((IkReal(-1.00000000000000))*(x3089)*(x3091)))+(((x3084)*(x3089)*(x3091)))+(((IkReal(-1.00000000000000))*(x3091)*(x3092)))+(((IkReal(-1.00000000000000))*(x3084)*(x3088)*(x3090)))+(((IkReal(2.00000000000000))*(x3089)*(x3092)))+(((IkReal(-1.00000000000000))*(x3088)*(x3093)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3084)*(x3085)*(x3093))));
13698 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13699 {
13700 {
13701 IkReal dummyeval[1];
13702 IkReal gconst12;
13703 IkReal x3094=(cj5)*(cj5);
13704 IkReal x3095=(sj6)*(sj6);
13705 IkReal x3096=(cj6)*(cj6);
13706 IkReal x3097=((r00)*(r20));
13707 IkReal x3098=((cj5)*(sj5));
13708 IkReal x3099=((r01)*(r21));
13709 IkReal x3100=((r21)*(sj6));
13710 IkReal x3101=((sj6)*(x3098));
13711 IkReal x3102=((IkReal(1.00000000000000))*(cj6)*(r00));
13712 IkReal x3103=((cj6)*(r01)*(r20)*(sj6));
13713 gconst12=IKsign(((((r02)*(x3098)*(x3100)))+(x3103)+(((x3094)*(x3095)*(x3099)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3098)))+(((r01)*(r22)*(x3101)))+(((cj6)*(r00)*(x3100)))+(((x3094)*(x3096)*(x3097)))+(((IkReal(-1.00000000000000))*(x3094)*(x3103)))+(((x3096)*(x3099)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3094)*(x3100)*(x3102)))+(((IkReal(-1.00000000000000))*(r22)*(x3098)*(x3102)))+(((x3095)*(x3097)))));
13714 IkReal x3104=(cj5)*(cj5);
13715 IkReal x3105=(sj6)*(sj6);
13716 IkReal x3106=(cj6)*(cj6);
13717 IkReal x3107=((r00)*(r20));
13718 IkReal x3108=((cj5)*(sj5));
13719 IkReal x3109=((r01)*(r21));
13720 IkReal x3110=((r21)*(sj6));
13721 IkReal x3111=((sj6)*(x3108));
13722 IkReal x3112=((IkReal(1.00000000000000))*(cj6)*(r00));
13723 IkReal x3113=x3103;
13724 dummyeval[0]=((x3113)+(((IkReal(-1.00000000000000))*(x3104)*(x3110)*(x3112)))+(((IkReal(-1.00000000000000))*(r22)*(x3108)*(x3112)))+(((x3104)*(x3105)*(x3109)))+(((r01)*(r22)*(x3111)))+(((x3104)*(x3106)*(x3107)))+(((cj6)*(r00)*(x3110)))+(((r02)*(r22)*((sj5)*(sj5))))+(((r02)*(x3108)*(x3110)))+(((x3106)*(x3109)))+(((x3105)*(x3107)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3108)))+(((IkReal(-1.00000000000000))*(x3104)*(x3113))));
13725 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
13726 {
13727 continue;
13728 
13729 } else
13730 {
13731 {
13732 IkReal j4array[1], cj4array[1], sj4array[1];
13733 bool j4valid[1]={false};
13734 _nj4 = 1;
13735 IkReal x3114=((IkReal(1.00000000000000))*(sj1)*(sj3));
13736 if( IKabs(((gconst12)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3114)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3114))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3114)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3114))))))) < IKFAST_ATAN2_MAGTHRESH )
13737     continue;
13738 j4array[0]=IKatan2(((gconst12)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3114)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3114)))))), ((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3114)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3114)))))));
13739 sj4array[0]=IKsin(j4array[0]);
13740 cj4array[0]=IKcos(j4array[0]);
13741 if( j4array[0] > IKPI )
13742 {
13743     j4array[0]-=IK2PI;
13744 }
13745 else if( j4array[0] < -IKPI )
13746 {    j4array[0]+=IK2PI;
13747 }
13748 j4valid[0] = true;
13749 for(int ij4 = 0; ij4 < 1; ++ij4)
13750 {
13751 if( !j4valid[ij4] )
13752 {
13753     continue;
13754 }
13755 _ij4[0] = ij4; _ij4[1] = -1;
13756 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13757 {
13758 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13759 {
13760     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13761 }
13762 }
13763 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13764 {
13765 IkReal evalcond[6];
13766 IkReal x3115=IKsin(j4);
13767 IkReal x3116=IKcos(j4);
13768 IkReal x3117=((IkReal(1.00000000000000))*(r00));
13769 IkReal x3118=((cj5)*(r01));
13770 IkReal x3119=((cj5)*(r11));
13771 IkReal x3120=((IkReal(1.00000000000000))*(cj5));
13772 IkReal x3121=((IkReal(1.00000000000000))*(cj1));
13773 IkReal x3122=((cj5)*(r21));
13774 IkReal x3123=((sj5)*(x3115));
13775 IkReal x3124=((cj6)*(x3115));
13776 IkReal x3125=((sj6)*(x3116));
13777 IkReal x3126=((sj6)*(x3115));
13778 IkReal x3127=((sj5)*(x3116));
13779 IkReal x3128=((cj6)*(x3116));
13780 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3125)))+(((IkReal(-1.00000000000000))*(r20)*(x3120)*(x3124)))+(((r22)*(x3123)))+(((IkReal(-1.00000000000000))*(r21)*(x3128)))+(((x3122)*(x3126))));
13781 evalcond[1]=((((r20)*(x3126)))+(((r21)*(x3124)))+(((IkReal(-1.00000000000000))*(r20)*(x3120)*(x3128)))+(((r22)*(x3127)))+(((x3122)*(x3125)))+(((IkReal(-1.00000000000000))*(cj3))));
13782 evalcond[2]=((((IkReal(-1.00000000000000))*(x3117)*(x3125)))+(((IkReal(-1.00000000000000))*(cj5)*(x3117)*(x3124)))+(((x3118)*(x3126)))+(((r02)*(x3123)))+(((IkReal(-1.00000000000000))*(r01)*(x3128)))+(((IkReal(-1.00000000000000))*(x3121))));
13783 evalcond[3]=((((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r10)*(x3120)*(x3124)))+(((x3119)*(x3126)))+(((IkReal(-1.00000000000000))*(r11)*(x3128)))+(((IkReal(-1.00000000000000))*(r10)*(x3125)))+(((r12)*(x3123))));
13784 evalcond[4]=((((r00)*(x3126)))+(((r01)*(x3124)))+(((IkReal(-1.00000000000000))*(cj5)*(x3117)*(x3128)))+(((sj1)*(sj3)))+(((x3118)*(x3125)))+(((r02)*(x3127))));
13785 evalcond[5]=((((IkReal(-1.00000000000000))*(sj3)*(x3121)))+(((IkReal(-1.00000000000000))*(r10)*(x3120)*(x3128)))+(((x3119)*(x3125)))+(((r10)*(x3126)))+(((r12)*(x3127)))+(((r11)*(x3124))));
13786 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13787 {
13788 continue;
13789 }
13790 }
13791 
13792 {
13793 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13794 vinfos[0].jointtype = 1;
13795 vinfos[0].foffset = j0;
13796 vinfos[0].indices[0] = _ij0[0];
13797 vinfos[0].indices[1] = _ij0[1];
13798 vinfos[0].maxsolutions = _nj0;
13799 vinfos[1].jointtype = 1;
13800 vinfos[1].foffset = j1;
13801 vinfos[1].indices[0] = _ij1[0];
13802 vinfos[1].indices[1] = _ij1[1];
13803 vinfos[1].maxsolutions = _nj1;
13804 vinfos[2].jointtype = 1;
13805 vinfos[2].foffset = j2;
13806 vinfos[2].indices[0] = _ij2[0];
13807 vinfos[2].indices[1] = _ij2[1];
13808 vinfos[2].maxsolutions = _nj2;
13809 vinfos[3].jointtype = 1;
13810 vinfos[3].foffset = j3;
13811 vinfos[3].indices[0] = _ij3[0];
13812 vinfos[3].indices[1] = _ij3[1];
13813 vinfos[3].maxsolutions = _nj3;
13814 vinfos[4].jointtype = 1;
13815 vinfos[4].foffset = j4;
13816 vinfos[4].indices[0] = _ij4[0];
13817 vinfos[4].indices[1] = _ij4[1];
13818 vinfos[4].maxsolutions = _nj4;
13819 vinfos[5].jointtype = 1;
13820 vinfos[5].foffset = j5;
13821 vinfos[5].indices[0] = _ij5[0];
13822 vinfos[5].indices[1] = _ij5[1];
13823 vinfos[5].maxsolutions = _nj5;
13824 vinfos[6].jointtype = 1;
13825 vinfos[6].foffset = j6;
13826 vinfos[6].indices[0] = _ij6[0];
13827 vinfos[6].indices[1] = _ij6[1];
13828 vinfos[6].maxsolutions = _nj6;
13829 std::vector<int> vfree(0);
13830 solutions.AddSolution(vinfos,vfree);
13831 }
13832 }
13833 }
13834 
13835 }
13836 
13837 }
13838 
13839 } else
13840 {
13841 {
13842 IkReal j4array[1], cj4array[1], sj4array[1];
13843 bool j4valid[1]={false};
13844 _nj4 = 1;
13845 IkReal x3129=((cj3)*(cj6));
13846 IkReal x3130=((IkReal(1.00000000000000))*(r21));
13847 IkReal x3131=((cj3)*(sj6));
13848 if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(r20)*(x3131)))+(((IkReal(-1.00000000000000))*(x3129)*(x3130))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj5)*(r20)*(x3129)))+(((IkReal(-1.00000000000000))*(cj5)*(x3130)*(x3131)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
13849     continue;
13850 j4array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(r20)*(x3131)))+(((IkReal(-1.00000000000000))*(x3129)*(x3130)))))), ((gconst11)*(((((cj5)*(r20)*(x3129)))+(((IkReal(-1.00000000000000))*(cj5)*(x3130)*(x3131)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))))));
13851 sj4array[0]=IKsin(j4array[0]);
13852 cj4array[0]=IKcos(j4array[0]);
13853 if( j4array[0] > IKPI )
13854 {
13855     j4array[0]-=IK2PI;
13856 }
13857 else if( j4array[0] < -IKPI )
13858 {    j4array[0]+=IK2PI;
13859 }
13860 j4valid[0] = true;
13861 for(int ij4 = 0; ij4 < 1; ++ij4)
13862 {
13863 if( !j4valid[ij4] )
13864 {
13865     continue;
13866 }
13867 _ij4[0] = ij4; _ij4[1] = -1;
13868 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13869 {
13870 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13871 {
13872     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13873 }
13874 }
13875 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13876 {
13877 IkReal evalcond[6];
13878 IkReal x3132=IKsin(j4);
13879 IkReal x3133=IKcos(j4);
13880 IkReal x3134=((IkReal(1.00000000000000))*(r00));
13881 IkReal x3135=((cj5)*(r01));
13882 IkReal x3136=((cj5)*(r11));
13883 IkReal x3137=((IkReal(1.00000000000000))*(cj5));
13884 IkReal x3138=((IkReal(1.00000000000000))*(cj1));
13885 IkReal x3139=((cj5)*(r21));
13886 IkReal x3140=((sj5)*(x3132));
13887 IkReal x3141=((cj6)*(x3132));
13888 IkReal x3142=((sj6)*(x3133));
13889 IkReal x3143=((sj6)*(x3132));
13890 IkReal x3144=((sj5)*(x3133));
13891 IkReal x3145=((cj6)*(x3133));
13892 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3137)*(x3141)))+(((r22)*(x3140)))+(((x3139)*(x3143)))+(((IkReal(-1.00000000000000))*(r21)*(x3145)))+(((IkReal(-1.00000000000000))*(r20)*(x3142))));
13893 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x3137)*(x3145)))+(((r22)*(x3144)))+(((r20)*(x3143)))+(((x3139)*(x3142)))+(((r21)*(x3141)))+(((IkReal(-1.00000000000000))*(cj3))));
13894 evalcond[2]=((((r02)*(x3140)))+(((x3135)*(x3143)))+(((IkReal(-1.00000000000000))*(x3134)*(x3142)))+(((IkReal(-1.00000000000000))*(cj5)*(x3134)*(x3141)))+(((IkReal(-1.00000000000000))*(x3138)))+(((IkReal(-1.00000000000000))*(r01)*(x3145))));
13895 evalcond[3]=((((IkReal(-1.00000000000000))*(sj1)))+(((r12)*(x3140)))+(((IkReal(-1.00000000000000))*(r10)*(x3137)*(x3141)))+(((IkReal(-1.00000000000000))*(r10)*(x3142)))+(((IkReal(-1.00000000000000))*(r11)*(x3145)))+(((x3136)*(x3143))));
13896 evalcond[4]=((((r02)*(x3144)))+(((x3135)*(x3142)))+(((sj1)*(sj3)))+(((r01)*(x3141)))+(((r00)*(x3143)))+(((IkReal(-1.00000000000000))*(cj5)*(x3134)*(x3145))));
13897 evalcond[5]=((((r10)*(x3143)))+(((r12)*(x3144)))+(((IkReal(-1.00000000000000))*(sj3)*(x3138)))+(((IkReal(-1.00000000000000))*(r10)*(x3137)*(x3145)))+(((r11)*(x3141)))+(((x3136)*(x3142))));
13898 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
13899 {
13900 continue;
13901 }
13902 }
13903 
13904 {
13905 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13906 vinfos[0].jointtype = 1;
13907 vinfos[0].foffset = j0;
13908 vinfos[0].indices[0] = _ij0[0];
13909 vinfos[0].indices[1] = _ij0[1];
13910 vinfos[0].maxsolutions = _nj0;
13911 vinfos[1].jointtype = 1;
13912 vinfos[1].foffset = j1;
13913 vinfos[1].indices[0] = _ij1[0];
13914 vinfos[1].indices[1] = _ij1[1];
13915 vinfos[1].maxsolutions = _nj1;
13916 vinfos[2].jointtype = 1;
13917 vinfos[2].foffset = j2;
13918 vinfos[2].indices[0] = _ij2[0];
13919 vinfos[2].indices[1] = _ij2[1];
13920 vinfos[2].maxsolutions = _nj2;
13921 vinfos[3].jointtype = 1;
13922 vinfos[3].foffset = j3;
13923 vinfos[3].indices[0] = _ij3[0];
13924 vinfos[3].indices[1] = _ij3[1];
13925 vinfos[3].maxsolutions = _nj3;
13926 vinfos[4].jointtype = 1;
13927 vinfos[4].foffset = j4;
13928 vinfos[4].indices[0] = _ij4[0];
13929 vinfos[4].indices[1] = _ij4[1];
13930 vinfos[4].maxsolutions = _nj4;
13931 vinfos[5].jointtype = 1;
13932 vinfos[5].foffset = j5;
13933 vinfos[5].indices[0] = _ij5[0];
13934 vinfos[5].indices[1] = _ij5[1];
13935 vinfos[5].maxsolutions = _nj5;
13936 vinfos[6].jointtype = 1;
13937 vinfos[6].foffset = j6;
13938 vinfos[6].indices[0] = _ij6[0];
13939 vinfos[6].indices[1] = _ij6[1];
13940 vinfos[6].maxsolutions = _nj6;
13941 std::vector<int> vfree(0);
13942 solutions.AddSolution(vinfos,vfree);
13943 }
13944 }
13945 }
13946 
13947 }
13948 
13949 }
13950 }
13951 }
13952 
13953 }
13954 
13955 }
13956 
13957 } else
13958 {
13959 {
13960 IkReal j4array[1], cj4array[1], sj4array[1];
13961 bool j4valid[1]={false};
13962 _nj4 = 1;
13963 IkReal x3146=((cj6)*(sj1));
13964 IkReal x3147=((IkReal(1.00000000000000))*(sj1)*(sj6));
13965 if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(r20)*(x3147)))+(((IkReal(-1.00000000000000))*(r21)*(x3146))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x3147)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))+(((cj5)*(r20)*(x3146))))))) < IKFAST_ATAN2_MAGTHRESH )
13966     continue;
13967 j4array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(r20)*(x3147)))+(((IkReal(-1.00000000000000))*(r21)*(x3146)))))), ((gconst10)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x3147)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))+(((cj5)*(r20)*(x3146)))))));
13968 sj4array[0]=IKsin(j4array[0]);
13969 cj4array[0]=IKcos(j4array[0]);
13970 if( j4array[0] > IKPI )
13971 {
13972     j4array[0]-=IK2PI;
13973 }
13974 else if( j4array[0] < -IKPI )
13975 {    j4array[0]+=IK2PI;
13976 }
13977 j4valid[0] = true;
13978 for(int ij4 = 0; ij4 < 1; ++ij4)
13979 {
13980 if( !j4valid[ij4] )
13981 {
13982     continue;
13983 }
13984 _ij4[0] = ij4; _ij4[1] = -1;
13985 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13986 {
13987 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13988 {
13989     j4valid[iij4]=false; _ij4[1] = iij4; break; 
13990 }
13991 }
13992 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13993 {
13994 IkReal evalcond[3];
13995 IkReal x3148=IKsin(j4);
13996 IkReal x3149=IKcos(j4);
13997 IkReal x3150=((IkReal(1.00000000000000))*(cj6));
13998 IkReal x3151=((sj5)*(x3148));
13999 IkReal x3152=((cj5)*(x3148));
14000 IkReal x3153=((IkReal(1.00000000000000))*(sj6)*(x3149));
14001 evalcond[0]=((((r22)*(x3151)))+(((IkReal(-1.00000000000000))*(r20)*(x3150)*(x3152)))+(((IkReal(-1.00000000000000))*(r20)*(x3153)))+(((IkReal(-1.00000000000000))*(r21)*(x3149)*(x3150)))+(((r21)*(sj6)*(x3152))));
14002 evalcond[1]=((((r02)*(x3151)))+(((IkReal(-1.00000000000000))*(r01)*(x3149)*(x3150)))+(((r01)*(sj6)*(x3152)))+(((IkReal(-1.00000000000000))*(r00)*(x3150)*(x3152)))+(((IkReal(-1.00000000000000))*(r00)*(x3153)))+(((IkReal(-1.00000000000000))*(cj1))));
14003 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r11)*(x3149)*(x3150)))+(((IkReal(-1.00000000000000))*(r10)*(x3150)*(x3152)))+(((r12)*(x3151)))+(((r11)*(sj6)*(x3152)))+(((IkReal(-1.00000000000000))*(r10)*(x3153))));
14004 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
14005 {
14006 continue;
14007 }
14008 }
14009 
14010 {
14011 IkReal j3array[1], cj3array[1], sj3array[1];
14012 bool j3valid[1]={false};
14013 _nj3 = 1;
14014 IkReal x3154=((cj4)*(cj5));
14015 IkReal x3155=((r21)*(sj6));
14016 IkReal x3156=((IkReal(1.00000000000000))*(cj6)*(r20));
14017 if( IKabs(((((sj5)*(x3155)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x3156))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(r22)*(sj5)))+(((x3154)*(x3155)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3154)*(x3156)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x3155)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x3156)))))+IKsqr(((((cj4)*(r22)*(sj5)))+(((x3154)*(x3155)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3154)*(x3156)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
14018     continue;
14019 j3array[0]=IKatan2(((((sj5)*(x3155)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x3156)))), ((((cj4)*(r22)*(sj5)))+(((x3154)*(x3155)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3154)*(x3156)))+(((r20)*(sj4)*(sj6)))));
14020 sj3array[0]=IKsin(j3array[0]);
14021 cj3array[0]=IKcos(j3array[0]);
14022 if( j3array[0] > IKPI )
14023 {
14024     j3array[0]-=IK2PI;
14025 }
14026 else if( j3array[0] < -IKPI )
14027 {    j3array[0]+=IK2PI;
14028 }
14029 j3valid[0] = true;
14030 for(int ij3 = 0; ij3 < 1; ++ij3)
14031 {
14032 if( !j3valid[ij3] )
14033 {
14034     continue;
14035 }
14036 _ij3[0] = ij3; _ij3[1] = -1;
14037 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14038 {
14039 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14040 {
14041     j3valid[iij3]=false; _ij3[1] = iij3; break; 
14042 }
14043 }
14044 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14045 {
14046 IkReal evalcond[6];
14047 IkReal x3157=IKcos(j3);
14048 IkReal x3158=IKsin(j3);
14049 IkReal x3159=((r11)*(sj6));
14050 IkReal x3160=((IkReal(1.00000000000000))*(sj5));
14051 IkReal x3161=((IkReal(1.00000000000000))*(cj1));
14052 IkReal x3162=((cj6)*(sj4));
14053 IkReal x3163=((sj4)*(sj6));
14054 IkReal x3164=((cj4)*(sj5));
14055 IkReal x3165=((cj4)*(cj5));
14056 IkReal x3166=((r01)*(sj6));
14057 IkReal x3167=((r21)*(sj6));
14058 IkReal x3168=((cj6)*(sj5));
14059 IkReal x3169=((IkReal(1.00000000000000))*(cj6)*(x3165));
14060 evalcond[0]=((x3158)+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3160)*(x3167)))+(((r20)*(x3168))));
14061 evalcond[1]=((((IkReal(-1.00000000000000))*(x3160)*(x3166)))+(((sj1)*(x3157)))+(((r00)*(x3168)))+(((cj5)*(r02))));
14062 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3157)*(x3161)))+(((r10)*(x3168)))+(((IkReal(-1.00000000000000))*(x3159)*(x3160))));
14063 evalcond[3]=((((r22)*(x3164)))+(((IkReal(-1.00000000000000))*(x3157)))+(((r20)*(x3163)))+(((r21)*(x3162)))+(((IkReal(-1.00000000000000))*(r20)*(x3169)))+(((x3165)*(x3167))));
14064 evalcond[4]=((((r02)*(x3164)))+(((sj1)*(x3158)))+(((r01)*(x3162)))+(((r00)*(x3163)))+(((IkReal(-1.00000000000000))*(r00)*(x3169)))+(((x3165)*(x3166))));
14065 evalcond[5]=((((r11)*(x3162)))+(((r10)*(x3163)))+(((IkReal(-1.00000000000000))*(x3158)*(x3161)))+(((r12)*(x3164)))+(((x3159)*(x3165)))+(((IkReal(-1.00000000000000))*(r10)*(x3169))));
14066 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14067 {
14068 continue;
14069 }
14070 }
14071 
14072 {
14073 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14074 vinfos[0].jointtype = 1;
14075 vinfos[0].foffset = j0;
14076 vinfos[0].indices[0] = _ij0[0];
14077 vinfos[0].indices[1] = _ij0[1];
14078 vinfos[0].maxsolutions = _nj0;
14079 vinfos[1].jointtype = 1;
14080 vinfos[1].foffset = j1;
14081 vinfos[1].indices[0] = _ij1[0];
14082 vinfos[1].indices[1] = _ij1[1];
14083 vinfos[1].maxsolutions = _nj1;
14084 vinfos[2].jointtype = 1;
14085 vinfos[2].foffset = j2;
14086 vinfos[2].indices[0] = _ij2[0];
14087 vinfos[2].indices[1] = _ij2[1];
14088 vinfos[2].maxsolutions = _nj2;
14089 vinfos[3].jointtype = 1;
14090 vinfos[3].foffset = j3;
14091 vinfos[3].indices[0] = _ij3[0];
14092 vinfos[3].indices[1] = _ij3[1];
14093 vinfos[3].maxsolutions = _nj3;
14094 vinfos[4].jointtype = 1;
14095 vinfos[4].foffset = j4;
14096 vinfos[4].indices[0] = _ij4[0];
14097 vinfos[4].indices[1] = _ij4[1];
14098 vinfos[4].maxsolutions = _nj4;
14099 vinfos[5].jointtype = 1;
14100 vinfos[5].foffset = j5;
14101 vinfos[5].indices[0] = _ij5[0];
14102 vinfos[5].indices[1] = _ij5[1];
14103 vinfos[5].maxsolutions = _nj5;
14104 vinfos[6].jointtype = 1;
14105 vinfos[6].foffset = j6;
14106 vinfos[6].indices[0] = _ij6[0];
14107 vinfos[6].indices[1] = _ij6[1];
14108 vinfos[6].maxsolutions = _nj6;
14109 std::vector<int> vfree(0);
14110 solutions.AddSolution(vinfos,vfree);
14111 }
14112 }
14113 }
14114 }
14115 }
14116 
14117 }
14118 
14119 }
14120 
14121 } else
14122 {
14123 {
14124 IkReal j4array[1], cj4array[1], sj4array[1];
14125 bool j4valid[1]={false};
14126 _nj4 = 1;
14127 IkReal x3170=((cj1)*(cj6));
14128 IkReal x3171=((IkReal(1.00000000000000))*(cj1));
14129 if( IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3171)))+(((IkReal(-1.00000000000000))*(r21)*(x3170))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3171)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3171)))+(((cj5)*(r20)*(x3170))))))) < IKFAST_ATAN2_MAGTHRESH )
14130     continue;
14131 j4array[0]=IKatan2(((gconst9)*(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3171)))+(((IkReal(-1.00000000000000))*(r21)*(x3170)))))), ((gconst9)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3171)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3171)))+(((cj5)*(r20)*(x3170)))))));
14132 sj4array[0]=IKsin(j4array[0]);
14133 cj4array[0]=IKcos(j4array[0]);
14134 if( j4array[0] > IKPI )
14135 {
14136     j4array[0]-=IK2PI;
14137 }
14138 else if( j4array[0] < -IKPI )
14139 {    j4array[0]+=IK2PI;
14140 }
14141 j4valid[0] = true;
14142 for(int ij4 = 0; ij4 < 1; ++ij4)
14143 {
14144 if( !j4valid[ij4] )
14145 {
14146     continue;
14147 }
14148 _ij4[0] = ij4; _ij4[1] = -1;
14149 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14150 {
14151 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14152 {
14153     j4valid[iij4]=false; _ij4[1] = iij4; break; 
14154 }
14155 }
14156 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14157 {
14158 IkReal evalcond[3];
14159 IkReal x3172=IKsin(j4);
14160 IkReal x3173=IKcos(j4);
14161 IkReal x3174=((IkReal(1.00000000000000))*(cj6));
14162 IkReal x3175=((sj5)*(x3172));
14163 IkReal x3176=((cj5)*(x3172));
14164 IkReal x3177=((IkReal(1.00000000000000))*(sj6)*(x3173));
14165 evalcond[0]=((((r21)*(sj6)*(x3176)))+(((r22)*(x3175)))+(((IkReal(-1.00000000000000))*(r20)*(x3174)*(x3176)))+(((IkReal(-1.00000000000000))*(r21)*(x3173)*(x3174)))+(((IkReal(-1.00000000000000))*(r20)*(x3177))));
14166 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x3174)*(x3176)))+(((IkReal(-1.00000000000000))*(r01)*(x3173)*(x3174)))+(((IkReal(-1.00000000000000))*(r00)*(x3177)))+(((r01)*(sj6)*(x3176)))+(((r02)*(x3175)))+(((IkReal(-1.00000000000000))*(cj1))));
14167 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x3174)*(x3176)))+(((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(r10)*(x3177)))+(((IkReal(-1.00000000000000))*(r11)*(x3173)*(x3174)))+(((r11)*(sj6)*(x3176)))+(((r12)*(x3175))));
14168 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
14169 {
14170 continue;
14171 }
14172 }
14173 
14174 {
14175 IkReal j3array[1], cj3array[1], sj3array[1];
14176 bool j3valid[1]={false};
14177 _nj3 = 1;
14178 IkReal x3178=((cj4)*(cj5));
14179 IkReal x3179=((r21)*(sj6));
14180 IkReal x3180=((IkReal(1.00000000000000))*(cj6)*(r20));
14181 if( IKabs(((((sj5)*(x3179)))+(((IkReal(-1.00000000000000))*(sj5)*(x3180)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3178)*(x3180)))+(((x3178)*(x3179)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((sj5)*(x3179)))+(((IkReal(-1.00000000000000))*(sj5)*(x3180)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3178)*(x3180)))+(((x3178)*(x3179)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
14182     continue;
14183 j3array[0]=IKatan2(((((sj5)*(x3179)))+(((IkReal(-1.00000000000000))*(sj5)*(x3180)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((cj4)*(r22)*(sj5)))+(((cj6)*(r21)*(sj4)))+(((IkReal(-1.00000000000000))*(x3178)*(x3180)))+(((x3178)*(x3179)))+(((r20)*(sj4)*(sj6)))));
14184 sj3array[0]=IKsin(j3array[0]);
14185 cj3array[0]=IKcos(j3array[0]);
14186 if( j3array[0] > IKPI )
14187 {
14188     j3array[0]-=IK2PI;
14189 }
14190 else if( j3array[0] < -IKPI )
14191 {    j3array[0]+=IK2PI;
14192 }
14193 j3valid[0] = true;
14194 for(int ij3 = 0; ij3 < 1; ++ij3)
14195 {
14196 if( !j3valid[ij3] )
14197 {
14198     continue;
14199 }
14200 _ij3[0] = ij3; _ij3[1] = -1;
14201 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14202 {
14203 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14204 {
14205     j3valid[iij3]=false; _ij3[1] = iij3; break; 
14206 }
14207 }
14208 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14209 {
14210 IkReal evalcond[6];
14211 IkReal x3181=IKcos(j3);
14212 IkReal x3182=IKsin(j3);
14213 IkReal x3183=((r11)*(sj6));
14214 IkReal x3184=((IkReal(1.00000000000000))*(sj5));
14215 IkReal x3185=((IkReal(1.00000000000000))*(cj1));
14216 IkReal x3186=((cj6)*(sj4));
14217 IkReal x3187=((sj4)*(sj6));
14218 IkReal x3188=((cj4)*(sj5));
14219 IkReal x3189=((cj4)*(cj5));
14220 IkReal x3190=((r01)*(sj6));
14221 IkReal x3191=((r21)*(sj6));
14222 IkReal x3192=((cj6)*(sj5));
14223 IkReal x3193=((IkReal(1.00000000000000))*(cj6)*(x3189));
14224 evalcond[0]=((x3182)+(((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3184)*(x3191)))+(((r20)*(x3192))));
14225 evalcond[1]=((((IkReal(-1.00000000000000))*(x3184)*(x3190)))+(((r00)*(x3192)))+(((sj1)*(x3181)))+(((cj5)*(r02))));
14226 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3183)*(x3184)))+(((IkReal(-1.00000000000000))*(x3181)*(x3185)))+(((r10)*(x3192))));
14227 evalcond[3]=((((r22)*(x3188)))+(((r20)*(x3187)))+(((IkReal(-1.00000000000000))*(r20)*(x3193)))+(((IkReal(-1.00000000000000))*(x3181)))+(((r21)*(x3186)))+(((x3189)*(x3191))));
14228 evalcond[4]=((((r01)*(x3186)))+(((r00)*(x3187)))+(((sj1)*(x3182)))+(((IkReal(-1.00000000000000))*(r00)*(x3193)))+(((r02)*(x3188)))+(((x3189)*(x3190))));
14229 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3193)))+(((r12)*(x3188)))+(((IkReal(-1.00000000000000))*(x3182)*(x3185)))+(((x3183)*(x3189)))+(((r11)*(x3186)))+(((r10)*(x3187))));
14230 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14231 {
14232 continue;
14233 }
14234 }
14235 
14236 {
14237 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14238 vinfos[0].jointtype = 1;
14239 vinfos[0].foffset = j0;
14240 vinfos[0].indices[0] = _ij0[0];
14241 vinfos[0].indices[1] = _ij0[1];
14242 vinfos[0].maxsolutions = _nj0;
14243 vinfos[1].jointtype = 1;
14244 vinfos[1].foffset = j1;
14245 vinfos[1].indices[0] = _ij1[0];
14246 vinfos[1].indices[1] = _ij1[1];
14247 vinfos[1].maxsolutions = _nj1;
14248 vinfos[2].jointtype = 1;
14249 vinfos[2].foffset = j2;
14250 vinfos[2].indices[0] = _ij2[0];
14251 vinfos[2].indices[1] = _ij2[1];
14252 vinfos[2].maxsolutions = _nj2;
14253 vinfos[3].jointtype = 1;
14254 vinfos[3].foffset = j3;
14255 vinfos[3].indices[0] = _ij3[0];
14256 vinfos[3].indices[1] = _ij3[1];
14257 vinfos[3].maxsolutions = _nj3;
14258 vinfos[4].jointtype = 1;
14259 vinfos[4].foffset = j4;
14260 vinfos[4].indices[0] = _ij4[0];
14261 vinfos[4].indices[1] = _ij4[1];
14262 vinfos[4].maxsolutions = _nj4;
14263 vinfos[5].jointtype = 1;
14264 vinfos[5].foffset = j5;
14265 vinfos[5].indices[0] = _ij5[0];
14266 vinfos[5].indices[1] = _ij5[1];
14267 vinfos[5].maxsolutions = _nj5;
14268 vinfos[6].jointtype = 1;
14269 vinfos[6].foffset = j6;
14270 vinfos[6].indices[0] = _ij6[0];
14271 vinfos[6].indices[1] = _ij6[1];
14272 vinfos[6].maxsolutions = _nj6;
14273 std::vector<int> vfree(0);
14274 solutions.AddSolution(vinfos,vfree);
14275 }
14276 }
14277 }
14278 }
14279 }
14280 
14281 }
14282 
14283 }
14284 
14285 } else
14286 {
14287 IkReal x3194=((IkReal(0.390000000000000))*(sj5));
14288 IkReal x3195=((IkReal(0.390000000000000))*(cj5));
14289 IkReal x3196=((IkReal(0.000500000000000000))*(cj6));
14290 IkReal x3197=((IkReal(0.000500000000000000))*(sj6));
14291 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
14292 evalcond[1]=((((r21)*(x3196)))+(((IkReal(-1.00000000000000))*(r21)*(sj6)*(x3194)))+(((r20)*(x3197)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x3195)))+(((cj6)*(r20)*(x3194))));
14293 evalcond[2]=((((r00)*(x3197)))+(((IkReal(0.00200000000000000))*(cj1)))+(((r01)*(x3196)))+(((IkReal(-1.00000000000000))*(r01)*(sj6)*(x3194)))+(((IkReal(-1.00000000000000))*(px)))+(((cj6)*(r00)*(x3194)))+(((r02)*(x3195)))+(((IkReal(-0.400000000000000))*(sj1))));
14294 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(sj6)*(x3194)))+(((IkReal(0.400000000000000))*(cj1)))+(((r11)*(x3196)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x3195)))+(((cj6)*(r10)*(x3194)))+(((r10)*(x3197))));
14295 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
14296 {
14297 {
14298 IkReal dummyeval[1];
14299 IkReal gconst21;
14300 IkReal x3198=(cj6)*(cj6);
14301 IkReal x3199=(sj6)*(sj6);
14302 IkReal x3200=((r01)*(r20));
14303 IkReal x3201=((r02)*(sj5));
14304 IkReal x3202=((cj5)*(x3199));
14305 IkReal x3203=((IkReal(1.00000000000000))*(r22)*(sj5));
14306 IkReal x3204=((IkReal(1.00000000000000))*(r00)*(r21));
14307 IkReal x3205=((cj5)*(x3198));
14308 gconst21=IKsign(((((IkReal(-1.00000000000000))*(r00)*(sj6)*(x3203)))+(((IkReal(-1.00000000000000))*(x3204)*(x3205)))+(((r20)*(sj6)*(x3201)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3203)))+(((IkReal(-1.00000000000000))*(x3202)*(x3204)))+(((x3200)*(x3205)))+(((x3200)*(x3202)))+(((cj6)*(r21)*(x3201)))));
14309 IkReal x3206=(cj6)*(cj6);
14310 IkReal x3207=(sj6)*(sj6);
14311 IkReal x3208=((r01)*(r20));
14312 IkReal x3209=((r02)*(sj5));
14313 IkReal x3210=((cj5)*(x3207));
14314 IkReal x3211=((IkReal(1.00000000000000))*(r22)*(sj5));
14315 IkReal x3212=((IkReal(1.00000000000000))*(r00)*(r21));
14316 IkReal x3213=((cj5)*(x3206));
14317 dummyeval[0]=((((r20)*(sj6)*(x3209)))+(((x3208)*(x3210)))+(((x3208)*(x3213)))+(((IkReal(-1.00000000000000))*(x3210)*(x3212)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3211)))+(((IkReal(-1.00000000000000))*(x3212)*(x3213)))+(((IkReal(-1.00000000000000))*(r00)*(sj6)*(x3211)))+(((cj6)*(r21)*(x3209))));
14318 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14319 {
14320 {
14321 IkReal dummyeval[1];
14322 IkReal gconst22;
14323 IkReal x3214=(cj6)*(cj6);
14324 IkReal x3215=(sj6)*(sj6);
14325 IkReal x3216=((r11)*(r20));
14326 IkReal x3217=((r12)*(sj5));
14327 IkReal x3218=((IkReal(1.00000000000000))*(r10));
14328 IkReal x3219=((r22)*(sj5));
14329 IkReal x3220=((cj5)*(x3215));
14330 IkReal x3221=((cj5)*(x3214));
14331 gconst22=IKsign(((((x3216)*(x3220)))+(((x3216)*(x3221)))+(((cj6)*(r21)*(x3217)))+(((IkReal(-1.00000000000000))*(r21)*(x3218)*(x3221)))+(((IkReal(-1.00000000000000))*(r21)*(x3218)*(x3220)))+(((r20)*(sj6)*(x3217)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3219)))+(((IkReal(-1.00000000000000))*(sj6)*(x3218)*(x3219)))));
14332 IkReal x3222=(cj6)*(cj6);
14333 IkReal x3223=(sj6)*(sj6);
14334 IkReal x3224=((r11)*(r20));
14335 IkReal x3225=((r12)*(sj5));
14336 IkReal x3226=((IkReal(1.00000000000000))*(r10));
14337 IkReal x3227=((r22)*(sj5));
14338 IkReal x3228=((cj5)*(x3223));
14339 IkReal x3229=((cj5)*(x3222));
14340 dummyeval[0]=((((r20)*(sj6)*(x3225)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3227)))+(((IkReal(-1.00000000000000))*(r21)*(x3226)*(x3228)))+(((IkReal(-1.00000000000000))*(r21)*(x3226)*(x3229)))+(((x3224)*(x3229)))+(((x3224)*(x3228)))+(((IkReal(-1.00000000000000))*(sj6)*(x3226)*(x3227)))+(((cj6)*(r21)*(x3225))));
14341 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14342 {
14343 {
14344 IkReal dummyeval[1];
14345 dummyeval[0]=cj1;
14346 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14347 {
14348 {
14349 IkReal evalcond[5];
14350 IkReal x3230=((IkReal(0.000500000000000000))*(cj6));
14351 IkReal x3231=((IkReal(0.000500000000000000))*(sj6));
14352 IkReal x3232=((sj5)*(sj6));
14353 IkReal x3233=((cj5)*(r12));
14354 IkReal x3234=((IkReal(0.390000000000000))*(cj5));
14355 IkReal x3235=((IkReal(0.390000000000000))*(cj6)*(sj5));
14356 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
14357 evalcond[1]=((((cj6)*(r10)*(sj5)))+(x3233)+(((IkReal(-1.00000000000000))*(r11)*(x3232))));
14358 evalcond[2]=((((IkReal(-0.390000000000000))*(r21)*(x3232)))+(((r22)*(x3234)))+(((r20)*(x3231)))+(((r20)*(x3235)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x3230))));
14359 evalcond[3]=((IkReal(-0.400000000000000))+(((IkReal(-0.390000000000000))*(r01)*(x3232)))+(((r01)*(x3230)))+(((r00)*(x3231)))+(((r00)*(x3235)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x3234))));
14360 evalcond[4]=((IkReal(0.00200000000000000))+(((r11)*(x3230)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(0.390000000000000))*(x3233)))+(((IkReal(-0.390000000000000))*(r11)*(x3232)))+(((r10)*(x3231)))+(((r10)*(x3235))));
14361 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
14362 {
14363 {
14364 IkReal j3array[1], cj3array[1], sj3array[1];
14365 bool j3valid[1]={false};
14366 _nj3 = 1;
14367 IkReal x3236=((cj6)*(sj5));
14368 IkReal x3237=((sj5)*(sj6));
14369 if( IKabs(((((cj5)*(r22)))+(((r20)*(x3236)))+(((IkReal(-1.00000000000000))*(r21)*(x3237))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((r01)*(x3237)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))+(((IkReal(-1.00000000000000))*(r00)*(x3236))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((r20)*(x3236)))+(((IkReal(-1.00000000000000))*(r21)*(x3237)))))+IKsqr(((((r01)*(x3237)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))+(((IkReal(-1.00000000000000))*(r00)*(x3236)))))-1) <= IKFAST_SINCOS_THRESH )
14370     continue;
14371 j3array[0]=IKatan2(((((cj5)*(r22)))+(((r20)*(x3236)))+(((IkReal(-1.00000000000000))*(r21)*(x3237)))), ((((r01)*(x3237)))+(((IkReal(-1.00000000000000))*(cj5)*(r02)))+(((IkReal(-1.00000000000000))*(r00)*(x3236)))));
14372 sj3array[0]=IKsin(j3array[0]);
14373 cj3array[0]=IKcos(j3array[0]);
14374 if( j3array[0] > IKPI )
14375 {
14376     j3array[0]-=IK2PI;
14377 }
14378 else if( j3array[0] < -IKPI )
14379 {    j3array[0]+=IK2PI;
14380 }
14381 j3valid[0] = true;
14382 for(int ij3 = 0; ij3 < 1; ++ij3)
14383 {
14384 if( !j3valid[ij3] )
14385 {
14386     continue;
14387 }
14388 _ij3[0] = ij3; _ij3[1] = -1;
14389 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14390 {
14391 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14392 {
14393     j3valid[iij3]=false; _ij3[1] = iij3; break; 
14394 }
14395 }
14396 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14397 {
14398 IkReal evalcond[2];
14399 IkReal x3238=((cj6)*(sj5));
14400 IkReal x3239=((IkReal(1.00000000000000))*(sj5)*(sj6));
14401 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3238)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(r21)*(x3239))));
14402 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x3239)))+(((r00)*(x3238)))+(IKcos(j3))+(((cj5)*(r02))));
14403 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
14404 {
14405 continue;
14406 }
14407 }
14408 
14409 {
14410 IkReal dummyeval[1];
14411 IkReal gconst27;
14412 IkReal x3240=(sj6)*(sj6);
14413 IkReal x3241=(cj6)*(cj6);
14414 IkReal x3242=((cj6)*(sj5));
14415 IkReal x3243=((r10)*(r21));
14416 IkReal x3244=((IkReal(1.00000000000000))*(r12));
14417 IkReal x3245=((sj5)*(sj6));
14418 IkReal x3246=((cj5)*(x3241));
14419 IkReal x3247=((IkReal(1.00000000000000))*(r11)*(r20));
14420 IkReal x3248=((cj5)*(x3240));
14421 gconst27=IKsign(((((r10)*(r22)*(x3245)))+(((r11)*(r22)*(x3242)))+(((IkReal(-1.00000000000000))*(r21)*(x3242)*(x3244)))+(((IkReal(-1.00000000000000))*(r20)*(x3244)*(x3245)))+(((IkReal(-1.00000000000000))*(x3246)*(x3247)))+(((IkReal(-1.00000000000000))*(x3247)*(x3248)))+(((x3243)*(x3246)))+(((x3243)*(x3248)))));
14422 IkReal x3249=(sj6)*(sj6);
14423 IkReal x3250=(cj6)*(cj6);
14424 IkReal x3251=((cj6)*(sj5));
14425 IkReal x3252=((r10)*(r21));
14426 IkReal x3253=((IkReal(1.00000000000000))*(r12));
14427 IkReal x3254=((sj5)*(sj6));
14428 IkReal x3255=((cj5)*(x3250));
14429 IkReal x3256=((IkReal(1.00000000000000))*(r11)*(r20));
14430 IkReal x3257=((cj5)*(x3249));
14431 dummyeval[0]=((((IkReal(-1.00000000000000))*(r21)*(x3251)*(x3253)))+(((x3252)*(x3257)))+(((x3252)*(x3255)))+(((IkReal(-1.00000000000000))*(x3256)*(x3257)))+(((r10)*(r22)*(x3254)))+(((r11)*(r22)*(x3251)))+(((IkReal(-1.00000000000000))*(x3255)*(x3256)))+(((IkReal(-1.00000000000000))*(r20)*(x3253)*(x3254))));
14432 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14433 {
14434 {
14435 IkReal dummyeval[1];
14436 IkReal gconst28;
14437 IkReal x3258=(sj6)*(sj6);
14438 IkReal x3259=(cj6)*(cj6);
14439 IkReal x3260=((r01)*(r10));
14440 IkReal x3261=((IkReal(1.00000000000000))*(r00));
14441 IkReal x3262=((r02)*(sj5));
14442 IkReal x3263=((r12)*(sj5));
14443 IkReal x3264=((cj5)*(x3258));
14444 IkReal x3265=((cj5)*(x3259));
14445 gconst28=IKsign(((((r10)*(sj6)*(x3262)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3263)))+(((IkReal(-1.00000000000000))*(sj6)*(x3261)*(x3263)))+(((cj6)*(r11)*(x3262)))+(((IkReal(-1.00000000000000))*(r11)*(x3261)*(x3264)))+(((IkReal(-1.00000000000000))*(r11)*(x3261)*(x3265)))+(((x3260)*(x3264)))+(((x3260)*(x3265)))));
14446 IkReal x3266=(sj6)*(sj6);
14447 IkReal x3267=(cj6)*(cj6);
14448 IkReal x3268=((r01)*(r10));
14449 IkReal x3269=((IkReal(1.00000000000000))*(r00));
14450 IkReal x3270=((r02)*(sj5));
14451 IkReal x3271=((r12)*(sj5));
14452 IkReal x3272=((cj5)*(x3266));
14453 IkReal x3273=((cj5)*(x3267));
14454 dummyeval[0]=((((cj6)*(r11)*(x3270)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3271)))+(((IkReal(-1.00000000000000))*(r11)*(x3269)*(x3273)))+(((IkReal(-1.00000000000000))*(r11)*(x3269)*(x3272)))+(((x3268)*(x3272)))+(((x3268)*(x3273)))+(((IkReal(-1.00000000000000))*(sj6)*(x3269)*(x3271)))+(((r10)*(sj6)*(x3270))));
14455 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14456 {
14457 continue;
14458 
14459 } else
14460 {
14461 {
14462 IkReal j4array[1], cj4array[1], sj4array[1];
14463 bool j4valid[1]={false};
14464 _nj4 = 1;
14465 IkReal x3274=((sj3)*(sj6));
14466 IkReal x3275=((IkReal(1.00000000000000))*(r10));
14467 IkReal x3276=((cj6)*(sj3));
14468 if( IKabs(((gconst28)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3275)*(x3276)))+(((cj5)*(r11)*(x3274))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(r11)*(x3276)))+(((IkReal(-1.00000000000000))*(x3274)*(x3275))))))) < IKFAST_ATAN2_MAGTHRESH )
14469     continue;
14470 j4array[0]=IKatan2(((gconst28)*(((((r12)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3275)*(x3276)))+(((cj5)*(r11)*(x3274)))))), ((gconst28)*(((((IkReal(-1.00000000000000))*(r11)*(x3276)))+(((IkReal(-1.00000000000000))*(x3274)*(x3275)))))));
14471 sj4array[0]=IKsin(j4array[0]);
14472 cj4array[0]=IKcos(j4array[0]);
14473 if( j4array[0] > IKPI )
14474 {
14475     j4array[0]-=IK2PI;
14476 }
14477 else if( j4array[0] < -IKPI )
14478 {    j4array[0]+=IK2PI;
14479 }
14480 j4valid[0] = true;
14481 for(int ij4 = 0; ij4 < 1; ++ij4)
14482 {
14483 if( !j4valid[ij4] )
14484 {
14485     continue;
14486 }
14487 _ij4[0] = ij4; _ij4[1] = -1;
14488 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14489 {
14490 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14491 {
14492     j4valid[iij4]=false; _ij4[1] = iij4; break; 
14493 }
14494 }
14495 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14496 {
14497 IkReal evalcond[6];
14498 IkReal x3277=IKsin(j4);
14499 IkReal x3278=IKcos(j4);
14500 IkReal x3279=((IkReal(1.00000000000000))*(r00));
14501 IkReal x3280=((cj5)*(r01));
14502 IkReal x3281=((cj5)*(r11));
14503 IkReal x3282=((IkReal(1.00000000000000))*(cj5));
14504 IkReal x3283=((cj5)*(r21));
14505 IkReal x3284=((sj5)*(x3277));
14506 IkReal x3285=((cj6)*(x3277));
14507 IkReal x3286=((sj6)*(x3278));
14508 IkReal x3287=((sj6)*(x3277));
14509 IkReal x3288=((sj5)*(x3278));
14510 IkReal x3289=((cj6)*(x3278));
14511 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3286)))+(((x3283)*(x3287)))+(((IkReal(-1.00000000000000))*(r20)*(x3282)*(x3285)))+(((r22)*(x3284)))+(((IkReal(-1.00000000000000))*(r21)*(x3289))));
14512 evalcond[1]=((((r20)*(x3287)))+(cj3)+(((x3283)*(x3286)))+(((IkReal(-1.00000000000000))*(r20)*(x3282)*(x3289)))+(((r22)*(x3288)))+(((r21)*(x3285))));
14513 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x3279)*(x3285)))+(((r02)*(x3284)))+(((IkReal(-1.00000000000000))*(x3279)*(x3286)))+(((x3280)*(x3287)))+(((IkReal(-1.00000000000000))*(r01)*(x3289))));
14514 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x3286)))+(((IkReal(-1.00000000000000))*(r10)*(x3282)*(x3285)))+(((IkReal(-1.00000000000000))*(r11)*(x3289)))+(((x3281)*(x3287)))+(((r12)*(x3284))));
14515 evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x3279)*(x3289)))+(((r00)*(x3287)))+(sj3)+(((r02)*(x3288)))+(((x3280)*(x3286)))+(((r01)*(x3285))));
14516 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3282)*(x3289)))+(((r10)*(x3287)))+(((x3281)*(x3286)))+(((r11)*(x3285)))+(((r12)*(x3288))));
14517 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14518 {
14519 continue;
14520 }
14521 }
14522 
14523 {
14524 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14525 vinfos[0].jointtype = 1;
14526 vinfos[0].foffset = j0;
14527 vinfos[0].indices[0] = _ij0[0];
14528 vinfos[0].indices[1] = _ij0[1];
14529 vinfos[0].maxsolutions = _nj0;
14530 vinfos[1].jointtype = 1;
14531 vinfos[1].foffset = j1;
14532 vinfos[1].indices[0] = _ij1[0];
14533 vinfos[1].indices[1] = _ij1[1];
14534 vinfos[1].maxsolutions = _nj1;
14535 vinfos[2].jointtype = 1;
14536 vinfos[2].foffset = j2;
14537 vinfos[2].indices[0] = _ij2[0];
14538 vinfos[2].indices[1] = _ij2[1];
14539 vinfos[2].maxsolutions = _nj2;
14540 vinfos[3].jointtype = 1;
14541 vinfos[3].foffset = j3;
14542 vinfos[3].indices[0] = _ij3[0];
14543 vinfos[3].indices[1] = _ij3[1];
14544 vinfos[3].maxsolutions = _nj3;
14545 vinfos[4].jointtype = 1;
14546 vinfos[4].foffset = j4;
14547 vinfos[4].indices[0] = _ij4[0];
14548 vinfos[4].indices[1] = _ij4[1];
14549 vinfos[4].maxsolutions = _nj4;
14550 vinfos[5].jointtype = 1;
14551 vinfos[5].foffset = j5;
14552 vinfos[5].indices[0] = _ij5[0];
14553 vinfos[5].indices[1] = _ij5[1];
14554 vinfos[5].maxsolutions = _nj5;
14555 vinfos[6].jointtype = 1;
14556 vinfos[6].foffset = j6;
14557 vinfos[6].indices[0] = _ij6[0];
14558 vinfos[6].indices[1] = _ij6[1];
14559 vinfos[6].maxsolutions = _nj6;
14560 std::vector<int> vfree(0);
14561 solutions.AddSolution(vinfos,vfree);
14562 }
14563 }
14564 }
14565 
14566 }
14567 
14568 }
14569 
14570 } else
14571 {
14572 {
14573 IkReal j4array[1], cj4array[1], sj4array[1];
14574 bool j4valid[1]={false};
14575 _nj4 = 1;
14576 IkReal x3290=((cj3)*(cj5));
14577 IkReal x3291=((IkReal(1.00000000000000))*(cj6));
14578 if( IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(r10)*(x3290)*(x3291)))+(((cj3)*(r12)*(sj5)))+(((r11)*(sj6)*(x3290))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(cj3)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(x3291))))))) < IKFAST_ATAN2_MAGTHRESH )
14579     continue;
14580 j4array[0]=IKatan2(((gconst27)*(((((IkReal(-1.00000000000000))*(r10)*(x3290)*(x3291)))+(((cj3)*(r12)*(sj5)))+(((r11)*(sj6)*(x3290)))))), ((gconst27)*(((((IkReal(-1.00000000000000))*(cj3)*(r10)*(sj6)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(x3291)))))));
14581 sj4array[0]=IKsin(j4array[0]);
14582 cj4array[0]=IKcos(j4array[0]);
14583 if( j4array[0] > IKPI )
14584 {
14585     j4array[0]-=IK2PI;
14586 }
14587 else if( j4array[0] < -IKPI )
14588 {    j4array[0]+=IK2PI;
14589 }
14590 j4valid[0] = true;
14591 for(int ij4 = 0; ij4 < 1; ++ij4)
14592 {
14593 if( !j4valid[ij4] )
14594 {
14595     continue;
14596 }
14597 _ij4[0] = ij4; _ij4[1] = -1;
14598 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14599 {
14600 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14601 {
14602     j4valid[iij4]=false; _ij4[1] = iij4; break; 
14603 }
14604 }
14605 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14606 {
14607 IkReal evalcond[6];
14608 IkReal x3292=IKsin(j4);
14609 IkReal x3293=IKcos(j4);
14610 IkReal x3294=((IkReal(1.00000000000000))*(r00));
14611 IkReal x3295=((cj5)*(r01));
14612 IkReal x3296=((cj5)*(r11));
14613 IkReal x3297=((IkReal(1.00000000000000))*(cj5));
14614 IkReal x3298=((cj5)*(r21));
14615 IkReal x3299=((sj5)*(x3292));
14616 IkReal x3300=((cj6)*(x3292));
14617 IkReal x3301=((sj6)*(x3293));
14618 IkReal x3302=((sj6)*(x3292));
14619 IkReal x3303=((sj5)*(x3293));
14620 IkReal x3304=((cj6)*(x3293));
14621 evalcond[0]=((((x3298)*(x3302)))+(((IkReal(-1.00000000000000))*(r21)*(x3304)))+(((r22)*(x3299)))+(((IkReal(-1.00000000000000))*(r20)*(x3301)))+(((IkReal(-1.00000000000000))*(r20)*(x3297)*(x3300))));
14622 evalcond[1]=((cj3)+(((r21)*(x3300)))+(((r22)*(x3303)))+(((x3298)*(x3301)))+(((IkReal(-1.00000000000000))*(r20)*(x3297)*(x3304)))+(((r20)*(x3302))));
14623 evalcond[2]=((((r02)*(x3299)))+(((IkReal(-1.00000000000000))*(r01)*(x3304)))+(((IkReal(-1.00000000000000))*(x3294)*(x3301)))+(((x3295)*(x3302)))+(((IkReal(-1.00000000000000))*(cj5)*(x3294)*(x3300))));
14624 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x3297)*(x3300)))+(((IkReal(-1.00000000000000))*(r11)*(x3304)))+(((x3296)*(x3302)))+(((IkReal(-1.00000000000000))*(r10)*(x3301)))+(((r12)*(x3299))));
14625 evalcond[4]=((sj3)+(((r02)*(x3303)))+(((x3295)*(x3301)))+(((IkReal(-1.00000000000000))*(cj5)*(x3294)*(x3304)))+(((r00)*(x3302)))+(((r01)*(x3300))));
14626 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3297)*(x3304)))+(((r10)*(x3302)))+(((r11)*(x3300)))+(((x3296)*(x3301)))+(((r12)*(x3303))));
14627 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14628 {
14629 continue;
14630 }
14631 }
14632 
14633 {
14634 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14635 vinfos[0].jointtype = 1;
14636 vinfos[0].foffset = j0;
14637 vinfos[0].indices[0] = _ij0[0];
14638 vinfos[0].indices[1] = _ij0[1];
14639 vinfos[0].maxsolutions = _nj0;
14640 vinfos[1].jointtype = 1;
14641 vinfos[1].foffset = j1;
14642 vinfos[1].indices[0] = _ij1[0];
14643 vinfos[1].indices[1] = _ij1[1];
14644 vinfos[1].maxsolutions = _nj1;
14645 vinfos[2].jointtype = 1;
14646 vinfos[2].foffset = j2;
14647 vinfos[2].indices[0] = _ij2[0];
14648 vinfos[2].indices[1] = _ij2[1];
14649 vinfos[2].maxsolutions = _nj2;
14650 vinfos[3].jointtype = 1;
14651 vinfos[3].foffset = j3;
14652 vinfos[3].indices[0] = _ij3[0];
14653 vinfos[3].indices[1] = _ij3[1];
14654 vinfos[3].maxsolutions = _nj3;
14655 vinfos[4].jointtype = 1;
14656 vinfos[4].foffset = j4;
14657 vinfos[4].indices[0] = _ij4[0];
14658 vinfos[4].indices[1] = _ij4[1];
14659 vinfos[4].maxsolutions = _nj4;
14660 vinfos[5].jointtype = 1;
14661 vinfos[5].foffset = j5;
14662 vinfos[5].indices[0] = _ij5[0];
14663 vinfos[5].indices[1] = _ij5[1];
14664 vinfos[5].maxsolutions = _nj5;
14665 vinfos[6].jointtype = 1;
14666 vinfos[6].foffset = j6;
14667 vinfos[6].indices[0] = _ij6[0];
14668 vinfos[6].indices[1] = _ij6[1];
14669 vinfos[6].maxsolutions = _nj6;
14670 std::vector<int> vfree(0);
14671 solutions.AddSolution(vinfos,vfree);
14672 }
14673 }
14674 }
14675 
14676 }
14677 
14678 }
14679 }
14680 }
14681 
14682 } else
14683 {
14684 IkReal x3305=((IkReal(0.000500000000000000))*(cj6));
14685 IkReal x3306=((IkReal(0.000500000000000000))*(sj6));
14686 IkReal x3307=((sj5)*(sj6));
14687 IkReal x3308=((cj5)*(r12));
14688 IkReal x3309=((IkReal(0.390000000000000))*(cj5));
14689 IkReal x3310=((IkReal(0.390000000000000))*(cj6)*(sj5));
14690 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
14691 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x3307)))+(((cj6)*(r10)*(sj5)))+(x3308));
14692 evalcond[2]=((((r21)*(x3305)))+(((r22)*(x3309)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x3310)))+(((IkReal(-0.390000000000000))*(r21)*(x3307)))+(((r20)*(x3306))));
14693 evalcond[3]=((IkReal(0.400000000000000))+(((r02)*(x3309)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.390000000000000))*(r01)*(x3307)))+(((r00)*(x3306)))+(((r00)*(x3310)))+(((r01)*(x3305))));
14694 evalcond[4]=((IkReal(-0.00200000000000000))+(((r10)*(x3306)))+(((r11)*(x3305)))+(((r10)*(x3310)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.390000000000000))*(r11)*(x3307)))+(((IkReal(0.390000000000000))*(x3308))));
14695 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
14696 {
14697 {
14698 IkReal j3array[1], cj3array[1], sj3array[1];
14699 bool j3valid[1]={false};
14700 _nj3 = 1;
14701 IkReal x3311=((cj6)*(sj5));
14702 IkReal x3312=((IkReal(1.00000000000000))*(sj5)*(sj6));
14703 if( IKabs(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x3312)))+(((r20)*(x3311))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r01)*(x3312)))+(((r00)*(x3311)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x3312)))+(((r20)*(x3311)))))+IKsqr(((((IkReal(-1.00000000000000))*(r01)*(x3312)))+(((r00)*(x3311)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
14704     continue;
14705 j3array[0]=IKatan2(((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x3312)))+(((r20)*(x3311)))), ((((IkReal(-1.00000000000000))*(r01)*(x3312)))+(((r00)*(x3311)))+(((cj5)*(r02)))));
14706 sj3array[0]=IKsin(j3array[0]);
14707 cj3array[0]=IKcos(j3array[0]);
14708 if( j3array[0] > IKPI )
14709 {
14710     j3array[0]-=IK2PI;
14711 }
14712 else if( j3array[0] < -IKPI )
14713 {    j3array[0]+=IK2PI;
14714 }
14715 j3valid[0] = true;
14716 for(int ij3 = 0; ij3 < 1; ++ij3)
14717 {
14718 if( !j3valid[ij3] )
14719 {
14720     continue;
14721 }
14722 _ij3[0] = ij3; _ij3[1] = -1;
14723 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14724 {
14725 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14726 {
14727     j3valid[iij3]=false; _ij3[1] = iij3; break; 
14728 }
14729 }
14730 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14731 {
14732 IkReal evalcond[2];
14733 IkReal x3313=((cj6)*(sj5));
14734 IkReal x3314=((IkReal(1.00000000000000))*(sj5)*(sj6));
14735 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(r21)*(x3314)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((r20)*(x3313))));
14736 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x3314)))+(((IkReal(-1.00000000000000))*(IKcos(j3))))+(((r00)*(x3313)))+(((cj5)*(r02))));
14737 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
14738 {
14739 continue;
14740 }
14741 }
14742 
14743 {
14744 IkReal dummyeval[1];
14745 IkReal gconst31;
14746 IkReal x3315=(sj6)*(sj6);
14747 IkReal x3316=(cj6)*(cj6);
14748 IkReal x3317=((cj6)*(sj5));
14749 IkReal x3318=((r10)*(r21));
14750 IkReal x3319=((IkReal(1.00000000000000))*(r12));
14751 IkReal x3320=((sj5)*(sj6));
14752 IkReal x3321=((cj5)*(x3316));
14753 IkReal x3322=((IkReal(1.00000000000000))*(r11)*(r20));
14754 IkReal x3323=((cj5)*(x3315));
14755 gconst31=IKsign(((((IkReal(-1.00000000000000))*(x3322)*(x3323)))+(((IkReal(-1.00000000000000))*(x3321)*(x3322)))+(((IkReal(-1.00000000000000))*(r21)*(x3317)*(x3319)))+(((r11)*(r22)*(x3317)))+(((IkReal(-1.00000000000000))*(r20)*(x3319)*(x3320)))+(((r10)*(r22)*(x3320)))+(((x3318)*(x3321)))+(((x3318)*(x3323)))));
14756 IkReal x3324=(sj6)*(sj6);
14757 IkReal x3325=(cj6)*(cj6);
14758 IkReal x3326=((cj6)*(sj5));
14759 IkReal x3327=((r10)*(r21));
14760 IkReal x3328=((IkReal(1.00000000000000))*(r12));
14761 IkReal x3329=((sj5)*(sj6));
14762 IkReal x3330=((cj5)*(x3325));
14763 IkReal x3331=((IkReal(1.00000000000000))*(r11)*(r20));
14764 IkReal x3332=((cj5)*(x3324));
14765 dummyeval[0]=((((r11)*(r22)*(x3326)))+(((IkReal(-1.00000000000000))*(x3331)*(x3332)))+(((IkReal(-1.00000000000000))*(x3330)*(x3331)))+(((x3327)*(x3330)))+(((x3327)*(x3332)))+(((IkReal(-1.00000000000000))*(r20)*(x3328)*(x3329)))+(((IkReal(-1.00000000000000))*(r21)*(x3326)*(x3328)))+(((r10)*(r22)*(x3329))));
14766 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14767 {
14768 {
14769 IkReal dummyeval[1];
14770 IkReal gconst32;
14771 IkReal x3333=(r20)*(r20);
14772 IkReal x3334=(cj5)*(cj5);
14773 IkReal x3335=(sj6)*(sj6);
14774 IkReal x3336=(cj6)*(cj6);
14775 IkReal x3337=(r21)*(r21);
14776 IkReal x3338=((cj6)*(r20));
14777 IkReal x3339=((IkReal(2.00000000000000))*(r21)*(sj6));
14778 IkReal x3340=((cj5)*(r22)*(sj5));
14779 gconst32=IKsign(((((x3333)*(x3335)))+(((IkReal(-1.00000000000000))*(x3334)*(x3338)*(x3339)))+(((x3339)*(x3340)))+(((x3336)*(x3337)))+(((x3334)*(x3335)*(x3337)))+(((x3338)*(x3339)))+(((IkReal(-2.00000000000000))*(x3338)*(x3340)))+((((r22)*(r22))*((sj5)*(sj5))))+(((x3333)*(x3334)*(x3336)))));
14780 IkReal x3341=(r20)*(r20);
14781 IkReal x3342=(cj5)*(cj5);
14782 IkReal x3343=(sj6)*(sj6);
14783 IkReal x3344=(cj6)*(cj6);
14784 IkReal x3345=(r21)*(r21);
14785 IkReal x3346=((cj6)*(r20));
14786 IkReal x3347=((IkReal(2.00000000000000))*(r21)*(sj6));
14787 IkReal x3348=((cj5)*(r22)*(sj5));
14788 dummyeval[0]=((((x3344)*(x3345)))+(((x3347)*(x3348)))+(((x3341)*(x3342)*(x3344)))+(((x3346)*(x3347)))+(((x3341)*(x3343)))+(((IkReal(-2.00000000000000))*(x3346)*(x3348)))+(((x3342)*(x3343)*(x3345)))+((((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3342)*(x3346)*(x3347))));
14789 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
14790 {
14791 continue;
14792 
14793 } else
14794 {
14795 {
14796 IkReal j4array[1], cj4array[1], sj4array[1];
14797 bool j4valid[1]={false};
14798 _nj4 = 1;
14799 IkReal x3349=((cj3)*(cj6));
14800 IkReal x3350=((IkReal(1.00000000000000))*(r21));
14801 IkReal x3351=((cj3)*(sj6));
14802 if( IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(x3349)*(x3350)))+(((IkReal(-1.00000000000000))*(r20)*(x3351))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst32)*(((((cj5)*(r20)*(x3349)))+(((IkReal(-1.00000000000000))*(cj5)*(x3350)*(x3351)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
14803     continue;
14804 j4array[0]=IKatan2(((gconst32)*(((((IkReal(-1.00000000000000))*(x3349)*(x3350)))+(((IkReal(-1.00000000000000))*(r20)*(x3351)))))), ((gconst32)*(((((cj5)*(r20)*(x3349)))+(((IkReal(-1.00000000000000))*(cj5)*(x3350)*(x3351)))+(((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))))));
14805 sj4array[0]=IKsin(j4array[0]);
14806 cj4array[0]=IKcos(j4array[0]);
14807 if( j4array[0] > IKPI )
14808 {
14809     j4array[0]-=IK2PI;
14810 }
14811 else if( j4array[0] < -IKPI )
14812 {    j4array[0]+=IK2PI;
14813 }
14814 j4valid[0] = true;
14815 for(int ij4 = 0; ij4 < 1; ++ij4)
14816 {
14817 if( !j4valid[ij4] )
14818 {
14819     continue;
14820 }
14821 _ij4[0] = ij4; _ij4[1] = -1;
14822 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14823 {
14824 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14825 {
14826     j4valid[iij4]=false; _ij4[1] = iij4; break; 
14827 }
14828 }
14829 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14830 {
14831 IkReal evalcond[6];
14832 IkReal x3352=IKsin(j4);
14833 IkReal x3353=IKcos(j4);
14834 IkReal x3354=((IkReal(1.00000000000000))*(r00));
14835 IkReal x3355=((cj5)*(r01));
14836 IkReal x3356=((cj5)*(r11));
14837 IkReal x3357=((IkReal(1.00000000000000))*(cj5));
14838 IkReal x3358=((cj5)*(r21));
14839 IkReal x3359=((sj5)*(x3352));
14840 IkReal x3360=((cj6)*(x3352));
14841 IkReal x3361=((sj6)*(x3353));
14842 IkReal x3362=((sj6)*(x3352));
14843 IkReal x3363=((sj5)*(x3353));
14844 IkReal x3364=((cj6)*(x3353));
14845 evalcond[0]=((((x3358)*(x3362)))+(((IkReal(-1.00000000000000))*(r20)*(x3361)))+(((IkReal(-1.00000000000000))*(r21)*(x3364)))+(((IkReal(-1.00000000000000))*(r20)*(x3357)*(x3360)))+(((r22)*(x3359))));
14846 evalcond[1]=((cj3)+(((r22)*(x3363)))+(((r21)*(x3360)))+(((x3358)*(x3361)))+(((r20)*(x3362)))+(((IkReal(-1.00000000000000))*(r20)*(x3357)*(x3364))));
14847 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x3364)))+(((IkReal(-1.00000000000000))*(x3354)*(x3361)))+(((IkReal(-1.00000000000000))*(cj5)*(x3354)*(x3360)))+(((x3355)*(x3362)))+(((r02)*(x3359))));
14848 evalcond[3]=((IkReal(-1.00000000000000))+(((x3356)*(x3362)))+(((r12)*(x3359)))+(((IkReal(-1.00000000000000))*(r10)*(x3357)*(x3360)))+(((IkReal(-1.00000000000000))*(r10)*(x3361)))+(((IkReal(-1.00000000000000))*(r11)*(x3364))));
14849 evalcond[4]=((((IkReal(-1.00000000000000))*(sj3)))+(((r02)*(x3363)))+(((r01)*(x3360)))+(((r00)*(x3362)))+(((IkReal(-1.00000000000000))*(cj5)*(x3354)*(x3364)))+(((x3355)*(x3361))));
14850 evalcond[5]=((((x3356)*(x3361)))+(((IkReal(-1.00000000000000))*(r10)*(x3357)*(x3364)))+(((r10)*(x3362)))+(((r12)*(x3363)))+(((r11)*(x3360))));
14851 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14852 {
14853 continue;
14854 }
14855 }
14856 
14857 {
14858 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14859 vinfos[0].jointtype = 1;
14860 vinfos[0].foffset = j0;
14861 vinfos[0].indices[0] = _ij0[0];
14862 vinfos[0].indices[1] = _ij0[1];
14863 vinfos[0].maxsolutions = _nj0;
14864 vinfos[1].jointtype = 1;
14865 vinfos[1].foffset = j1;
14866 vinfos[1].indices[0] = _ij1[0];
14867 vinfos[1].indices[1] = _ij1[1];
14868 vinfos[1].maxsolutions = _nj1;
14869 vinfos[2].jointtype = 1;
14870 vinfos[2].foffset = j2;
14871 vinfos[2].indices[0] = _ij2[0];
14872 vinfos[2].indices[1] = _ij2[1];
14873 vinfos[2].maxsolutions = _nj2;
14874 vinfos[3].jointtype = 1;
14875 vinfos[3].foffset = j3;
14876 vinfos[3].indices[0] = _ij3[0];
14877 vinfos[3].indices[1] = _ij3[1];
14878 vinfos[3].maxsolutions = _nj3;
14879 vinfos[4].jointtype = 1;
14880 vinfos[4].foffset = j4;
14881 vinfos[4].indices[0] = _ij4[0];
14882 vinfos[4].indices[1] = _ij4[1];
14883 vinfos[4].maxsolutions = _nj4;
14884 vinfos[5].jointtype = 1;
14885 vinfos[5].foffset = j5;
14886 vinfos[5].indices[0] = _ij5[0];
14887 vinfos[5].indices[1] = _ij5[1];
14888 vinfos[5].maxsolutions = _nj5;
14889 vinfos[6].jointtype = 1;
14890 vinfos[6].foffset = j6;
14891 vinfos[6].indices[0] = _ij6[0];
14892 vinfos[6].indices[1] = _ij6[1];
14893 vinfos[6].maxsolutions = _nj6;
14894 std::vector<int> vfree(0);
14895 solutions.AddSolution(vinfos,vfree);
14896 }
14897 }
14898 }
14899 
14900 }
14901 
14902 }
14903 
14904 } else
14905 {
14906 {
14907 IkReal j4array[1], cj4array[1], sj4array[1];
14908 bool j4valid[1]={false};
14909 _nj4 = 1;
14910 IkReal x3365=((cj3)*(sj6));
14911 IkReal x3366=((IkReal(1.00000000000000))*(r10));
14912 IkReal x3367=((cj3)*(cj6));
14913 if( IKabs(((gconst31)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3366)*(x3367)))+(((cj5)*(r11)*(x3365))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(x3365)*(x3366)))+(((IkReal(-1.00000000000000))*(r11)*(x3367))))))) < IKFAST_ATAN2_MAGTHRESH )
14914     continue;
14915 j4array[0]=IKatan2(((gconst31)*(((((cj3)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3366)*(x3367)))+(((cj5)*(r11)*(x3365)))))), ((gconst31)*(((((IkReal(-1.00000000000000))*(x3365)*(x3366)))+(((IkReal(-1.00000000000000))*(r11)*(x3367)))))));
14916 sj4array[0]=IKsin(j4array[0]);
14917 cj4array[0]=IKcos(j4array[0]);
14918 if( j4array[0] > IKPI )
14919 {
14920     j4array[0]-=IK2PI;
14921 }
14922 else if( j4array[0] < -IKPI )
14923 {    j4array[0]+=IK2PI;
14924 }
14925 j4valid[0] = true;
14926 for(int ij4 = 0; ij4 < 1; ++ij4)
14927 {
14928 if( !j4valid[ij4] )
14929 {
14930     continue;
14931 }
14932 _ij4[0] = ij4; _ij4[1] = -1;
14933 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14934 {
14935 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14936 {
14937     j4valid[iij4]=false; _ij4[1] = iij4; break; 
14938 }
14939 }
14940 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14941 {
14942 IkReal evalcond[6];
14943 IkReal x3368=IKsin(j4);
14944 IkReal x3369=IKcos(j4);
14945 IkReal x3370=((IkReal(1.00000000000000))*(r00));
14946 IkReal x3371=((cj5)*(r01));
14947 IkReal x3372=((cj5)*(r11));
14948 IkReal x3373=((IkReal(1.00000000000000))*(cj5));
14949 IkReal x3374=((cj5)*(r21));
14950 IkReal x3375=((sj5)*(x3368));
14951 IkReal x3376=((cj6)*(x3368));
14952 IkReal x3377=((sj6)*(x3369));
14953 IkReal x3378=((sj6)*(x3368));
14954 IkReal x3379=((sj5)*(x3369));
14955 IkReal x3380=((cj6)*(x3369));
14956 evalcond[0]=((((r22)*(x3375)))+(((IkReal(-1.00000000000000))*(r21)*(x3380)))+(((IkReal(-1.00000000000000))*(r20)*(x3373)*(x3376)))+(((x3374)*(x3378)))+(((IkReal(-1.00000000000000))*(r20)*(x3377))));
14957 evalcond[1]=((cj3)+(((r22)*(x3379)))+(((x3374)*(x3377)))+(((IkReal(-1.00000000000000))*(r20)*(x3373)*(x3380)))+(((r21)*(x3376)))+(((r20)*(x3378))));
14958 evalcond[2]=((((r02)*(x3375)))+(((IkReal(-1.00000000000000))*(x3370)*(x3377)))+(((x3371)*(x3378)))+(((IkReal(-1.00000000000000))*(r01)*(x3380)))+(((IkReal(-1.00000000000000))*(cj5)*(x3370)*(x3376))));
14959 evalcond[3]=((IkReal(-1.00000000000000))+(((r12)*(x3375)))+(((IkReal(-1.00000000000000))*(r10)*(x3373)*(x3376)))+(((x3372)*(x3378)))+(((IkReal(-1.00000000000000))*(r10)*(x3377)))+(((IkReal(-1.00000000000000))*(r11)*(x3380))));
14960 evalcond[4]=((((r01)*(x3376)))+(((IkReal(-1.00000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(cj5)*(x3370)*(x3380)))+(((r02)*(x3379)))+(((x3371)*(x3377)))+(((r00)*(x3378))));
14961 evalcond[5]=((((r12)*(x3379)))+(((r10)*(x3378)))+(((x3372)*(x3377)))+(((r11)*(x3376)))+(((IkReal(-1.00000000000000))*(r10)*(x3373)*(x3380))));
14962 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
14963 {
14964 continue;
14965 }
14966 }
14967 
14968 {
14969 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14970 vinfos[0].jointtype = 1;
14971 vinfos[0].foffset = j0;
14972 vinfos[0].indices[0] = _ij0[0];
14973 vinfos[0].indices[1] = _ij0[1];
14974 vinfos[0].maxsolutions = _nj0;
14975 vinfos[1].jointtype = 1;
14976 vinfos[1].foffset = j1;
14977 vinfos[1].indices[0] = _ij1[0];
14978 vinfos[1].indices[1] = _ij1[1];
14979 vinfos[1].maxsolutions = _nj1;
14980 vinfos[2].jointtype = 1;
14981 vinfos[2].foffset = j2;
14982 vinfos[2].indices[0] = _ij2[0];
14983 vinfos[2].indices[1] = _ij2[1];
14984 vinfos[2].maxsolutions = _nj2;
14985 vinfos[3].jointtype = 1;
14986 vinfos[3].foffset = j3;
14987 vinfos[3].indices[0] = _ij3[0];
14988 vinfos[3].indices[1] = _ij3[1];
14989 vinfos[3].maxsolutions = _nj3;
14990 vinfos[4].jointtype = 1;
14991 vinfos[4].foffset = j4;
14992 vinfos[4].indices[0] = _ij4[0];
14993 vinfos[4].indices[1] = _ij4[1];
14994 vinfos[4].maxsolutions = _nj4;
14995 vinfos[5].jointtype = 1;
14996 vinfos[5].foffset = j5;
14997 vinfos[5].indices[0] = _ij5[0];
14998 vinfos[5].indices[1] = _ij5[1];
14999 vinfos[5].maxsolutions = _nj5;
15000 vinfos[6].jointtype = 1;
15001 vinfos[6].foffset = j6;
15002 vinfos[6].indices[0] = _ij6[0];
15003 vinfos[6].indices[1] = _ij6[1];
15004 vinfos[6].maxsolutions = _nj6;
15005 std::vector<int> vfree(0);
15006 solutions.AddSolution(vinfos,vfree);
15007 }
15008 }
15009 }
15010 
15011 }
15012 
15013 }
15014 }
15015 }
15016 
15017 } else
15018 {
15019 if( 1 )
15020 {
15021 continue;
15022 
15023 } else
15024 {
15025 }
15026 }
15027 }
15028 }
15029 
15030 } else
15031 {
15032 {
15033 IkReal j3array[1], cj3array[1], sj3array[1];
15034 bool j3valid[1]={false};
15035 _nj3 = 1;
15036 IkReal x3381=((cj6)*(sj5));
15037 IkReal x3382=((IkReal(1.00000000000000))*(sj5)*(sj6));
15038 if( IKabs(((((cj5)*(r22)))+(((r20)*(x3381)))+(((IkReal(-1.00000000000000))*(r21)*(x3382))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x3381)))+(((IkReal(-1.00000000000000))*(r11)*(x3382))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((r20)*(x3381)))+(((IkReal(-1.00000000000000))*(r21)*(x3382)))))+IKsqr(((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x3381)))+(((IkReal(-1.00000000000000))*(r11)*(x3382)))))))-1) <= IKFAST_SINCOS_THRESH )
15039     continue;
15040 j3array[0]=IKatan2(((((cj5)*(r22)))+(((r20)*(x3381)))+(((IkReal(-1.00000000000000))*(r21)*(x3382)))), ((((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)))+(((r10)*(x3381)))+(((IkReal(-1.00000000000000))*(r11)*(x3382)))))));
15041 sj3array[0]=IKsin(j3array[0]);
15042 cj3array[0]=IKcos(j3array[0]);
15043 if( j3array[0] > IKPI )
15044 {
15045     j3array[0]-=IK2PI;
15046 }
15047 else if( j3array[0] < -IKPI )
15048 {    j3array[0]+=IK2PI;
15049 }
15050 j3valid[0] = true;
15051 for(int ij3 = 0; ij3 < 1; ++ij3)
15052 {
15053 if( !j3valid[ij3] )
15054 {
15055     continue;
15056 }
15057 _ij3[0] = ij3; _ij3[1] = -1;
15058 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15059 {
15060 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15061 {
15062     j3valid[iij3]=false; _ij3[1] = iij3; break; 
15063 }
15064 }
15065 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15066 {
15067 IkReal evalcond[3];
15068 IkReal x3383=IKcos(j3);
15069 IkReal x3384=((cj6)*(sj5));
15070 IkReal x3385=((IkReal(1.00000000000000))*(sj5)*(sj6));
15071 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3384)))+(((IkReal(-1.00000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(r21)*(x3385))));
15072 evalcond[1]=((((sj1)*(x3383)))+(((IkReal(-1.00000000000000))*(r01)*(x3385)))+(((r00)*(x3384)))+(((cj5)*(r02))));
15073 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x3384)))+(((IkReal(-1.00000000000000))*(cj1)*(x3383)))+(((IkReal(-1.00000000000000))*(r11)*(x3385))));
15074 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
15075 {
15076 continue;
15077 }
15078 }
15079 
15080 {
15081 IkReal dummyeval[1];
15082 IkReal gconst23;
15083 IkReal x3386=(r20)*(r20);
15084 IkReal x3387=(cj5)*(cj5);
15085 IkReal x3388=(sj6)*(sj6);
15086 IkReal x3389=(cj6)*(cj6);
15087 IkReal x3390=(r21)*(r21);
15088 IkReal x3391=((cj6)*(r20));
15089 IkReal x3392=((IkReal(2.00000000000000))*(r21)*(sj6));
15090 IkReal x3393=((cj5)*(r22)*(sj5));
15091 gconst23=IKsign(((((x3387)*(x3388)*(x3390)))+(((x3391)*(x3392)))+(((x3389)*(x3390)))+(((IkReal(-2.00000000000000))*(x3391)*(x3393)))+(((x3386)*(x3387)*(x3389)))+(((x3392)*(x3393)))+(((IkReal(-1.00000000000000))*(x3387)*(x3391)*(x3392)))+((((r22)*(r22))*((sj5)*(sj5))))+(((x3386)*(x3388)))));
15092 IkReal x3394=(r20)*(r20);
15093 IkReal x3395=(cj5)*(cj5);
15094 IkReal x3396=(sj6)*(sj6);
15095 IkReal x3397=(cj6)*(cj6);
15096 IkReal x3398=(r21)*(r21);
15097 IkReal x3399=((cj6)*(r20));
15098 IkReal x3400=((IkReal(2.00000000000000))*(r21)*(sj6));
15099 IkReal x3401=((cj5)*(r22)*(sj5));
15100 dummyeval[0]=((((x3395)*(x3396)*(x3398)))+(((x3394)*(x3395)*(x3397)))+(((IkReal(-1.00000000000000))*(x3395)*(x3399)*(x3400)))+(((x3394)*(x3396)))+(((IkReal(-2.00000000000000))*(x3399)*(x3401)))+(((x3399)*(x3400)))+(((x3397)*(x3398)))+(((x3400)*(x3401)))+((((r22)*(r22))*((sj5)*(sj5)))));
15101 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
15102 {
15103 {
15104 IkReal dummyeval[1];
15105 IkReal gconst24;
15106 IkReal x3402=(cj5)*(cj5);
15107 IkReal x3403=(sj6)*(sj6);
15108 IkReal x3404=(cj6)*(cj6);
15109 IkReal x3405=((r00)*(r20));
15110 IkReal x3406=((cj5)*(sj5));
15111 IkReal x3407=((r01)*(r21));
15112 IkReal x3408=((r21)*(sj6));
15113 IkReal x3409=((sj6)*(x3406));
15114 IkReal x3410=((IkReal(1.00000000000000))*(cj6)*(r00));
15115 IkReal x3411=((cj6)*(r01)*(r20)*(sj6));
15116 gconst24=IKsign(((((IkReal(-1.00000000000000))*(x3402)*(x3411)))+(((r01)*(r22)*(x3409)))+(((x3402)*(x3404)*(x3405)))+(((cj6)*(r00)*(x3408)))+(x3411)+(((x3403)*(x3405)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(r22)*(x3406)*(x3410)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3406)))+(((x3404)*(x3407)))+(((x3402)*(x3403)*(x3407)))+(((IkReal(-1.00000000000000))*(x3402)*(x3408)*(x3410)))+(((r02)*(x3406)*(x3408)))));
15117 IkReal x3412=(cj5)*(cj5);
15118 IkReal x3413=(sj6)*(sj6);
15119 IkReal x3414=(cj6)*(cj6);
15120 IkReal x3415=((r00)*(r20));
15121 IkReal x3416=((cj5)*(sj5));
15122 IkReal x3417=((r01)*(r21));
15123 IkReal x3418=((r21)*(sj6));
15124 IkReal x3419=((sj6)*(x3416));
15125 IkReal x3420=((IkReal(1.00000000000000))*(cj6)*(r00));
15126 IkReal x3421=x3411;
15127 dummyeval[0]=((((IkReal(-1.00000000000000))*(x3412)*(x3421)))+(((IkReal(-1.00000000000000))*(x3412)*(x3418)*(x3420)))+(((r01)*(r22)*(x3419)))+(((IkReal(-1.00000000000000))*(r22)*(x3416)*(x3420)))+(((x3414)*(x3417)))+(((r02)*(x3416)*(x3418)))+(x3421)+(((cj6)*(r00)*(x3418)))+(((x3412)*(x3413)*(x3417)))+(((r02)*(r22)*((sj5)*(sj5))))+(((x3413)*(x3415)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3416)))+(((x3412)*(x3414)*(x3415))));
15128 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
15129 {
15130 continue;
15131 
15132 } else
15133 {
15134 {
15135 IkReal j4array[1], cj4array[1], sj4array[1];
15136 bool j4valid[1]={false};
15137 _nj4 = 1;
15138 IkReal x3422=((cj1)*(r20));
15139 IkReal x3423=((cj5)*(cj6));
15140 IkReal x3424=((IkReal(1.00000000000000))*(cj1));
15141 IkReal x3425=((IkReal(1.00000000000000))*(cj3));
15142 IkReal x3426=((sj6)*(x3425));
15143 if( IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3424)))+(((IkReal(-1.00000000000000))*(r00)*(x3426)))+(((x3422)*(x3423)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3424)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3425))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst24)*(((((sj6)*(x3422)))+(((cj1)*(cj6)*(r21)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x3425)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x3426)))+(((cj3)*(r00)*(x3423))))))) < IKFAST_ATAN2_MAGTHRESH )
15144     continue;
15145 j4array[0]=IKatan2(((gconst24)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3424)))+(((IkReal(-1.00000000000000))*(r00)*(x3426)))+(((x3422)*(x3423)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3424)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3425)))))), ((gconst24)*(((((sj6)*(x3422)))+(((cj1)*(cj6)*(r21)))+(((IkReal(-1.00000000000000))*(r02)*(sj5)*(x3425)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x3426)))+(((cj3)*(r00)*(x3423)))))));
15146 sj4array[0]=IKsin(j4array[0]);
15147 cj4array[0]=IKcos(j4array[0]);
15148 if( j4array[0] > IKPI )
15149 {
15150     j4array[0]-=IK2PI;
15151 }
15152 else if( j4array[0] < -IKPI )
15153 {    j4array[0]+=IK2PI;
15154 }
15155 j4valid[0] = true;
15156 for(int ij4 = 0; ij4 < 1; ++ij4)
15157 {
15158 if( !j4valid[ij4] )
15159 {
15160     continue;
15161 }
15162 _ij4[0] = ij4; _ij4[1] = -1;
15163 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15164 {
15165 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15166 {
15167     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15168 }
15169 }
15170 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15171 {
15172 IkReal evalcond[6];
15173 IkReal x3427=IKsin(j4);
15174 IkReal x3428=IKcos(j4);
15175 IkReal x3429=((IkReal(1.00000000000000))*(r00));
15176 IkReal x3430=((cj5)*(r01));
15177 IkReal x3431=((cj5)*(r11));
15178 IkReal x3432=((IkReal(1.00000000000000))*(cj5));
15179 IkReal x3433=((cj5)*(r21));
15180 IkReal x3434=((sj5)*(x3427));
15181 IkReal x3435=((cj6)*(x3427));
15182 IkReal x3436=((sj6)*(x3428));
15183 IkReal x3437=((sj6)*(x3427));
15184 IkReal x3438=((sj5)*(x3428));
15185 IkReal x3439=((cj6)*(x3428));
15186 evalcond[0]=((((x3433)*(x3437)))+(((IkReal(-1.00000000000000))*(r20)*(x3436)))+(((r22)*(x3434)))+(((IkReal(-1.00000000000000))*(r21)*(x3439)))+(((IkReal(-1.00000000000000))*(r20)*(x3432)*(x3435))));
15187 evalcond[1]=((cj3)+(((r20)*(x3437)))+(((x3433)*(x3436)))+(((r21)*(x3435)))+(((r22)*(x3438)))+(((IkReal(-1.00000000000000))*(r20)*(x3432)*(x3439))));
15188 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(x3429)*(x3435)))+(cj1)+(((x3430)*(x3437)))+(((IkReal(-1.00000000000000))*(r01)*(x3439)))+(((r02)*(x3434)))+(((IkReal(-1.00000000000000))*(x3429)*(x3436))));
15189 evalcond[3]=((sj1)+(((IkReal(-1.00000000000000))*(r10)*(x3432)*(x3435)))+(((IkReal(-1.00000000000000))*(r10)*(x3436)))+(((r12)*(x3434)))+(((x3431)*(x3437)))+(((IkReal(-1.00000000000000))*(r11)*(x3439))));
15190 evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x3429)*(x3439)))+(((r01)*(x3435)))+(((x3430)*(x3436)))+(((sj1)*(sj3)))+(((r02)*(x3438)))+(((r00)*(x3437))));
15191 evalcond[5]=((((r10)*(x3437)))+(((IkReal(-1.00000000000000))*(r10)*(x3432)*(x3439)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((r12)*(x3438)))+(((x3431)*(x3436)))+(((r11)*(x3435))));
15192 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
15193 {
15194 continue;
15195 }
15196 }
15197 
15198 {
15199 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15200 vinfos[0].jointtype = 1;
15201 vinfos[0].foffset = j0;
15202 vinfos[0].indices[0] = _ij0[0];
15203 vinfos[0].indices[1] = _ij0[1];
15204 vinfos[0].maxsolutions = _nj0;
15205 vinfos[1].jointtype = 1;
15206 vinfos[1].foffset = j1;
15207 vinfos[1].indices[0] = _ij1[0];
15208 vinfos[1].indices[1] = _ij1[1];
15209 vinfos[1].maxsolutions = _nj1;
15210 vinfos[2].jointtype = 1;
15211 vinfos[2].foffset = j2;
15212 vinfos[2].indices[0] = _ij2[0];
15213 vinfos[2].indices[1] = _ij2[1];
15214 vinfos[2].maxsolutions = _nj2;
15215 vinfos[3].jointtype = 1;
15216 vinfos[3].foffset = j3;
15217 vinfos[3].indices[0] = _ij3[0];
15218 vinfos[3].indices[1] = _ij3[1];
15219 vinfos[3].maxsolutions = _nj3;
15220 vinfos[4].jointtype = 1;
15221 vinfos[4].foffset = j4;
15222 vinfos[4].indices[0] = _ij4[0];
15223 vinfos[4].indices[1] = _ij4[1];
15224 vinfos[4].maxsolutions = _nj4;
15225 vinfos[5].jointtype = 1;
15226 vinfos[5].foffset = j5;
15227 vinfos[5].indices[0] = _ij5[0];
15228 vinfos[5].indices[1] = _ij5[1];
15229 vinfos[5].maxsolutions = _nj5;
15230 vinfos[6].jointtype = 1;
15231 vinfos[6].foffset = j6;
15232 vinfos[6].indices[0] = _ij6[0];
15233 vinfos[6].indices[1] = _ij6[1];
15234 vinfos[6].maxsolutions = _nj6;
15235 std::vector<int> vfree(0);
15236 solutions.AddSolution(vinfos,vfree);
15237 }
15238 }
15239 }
15240 
15241 }
15242 
15243 }
15244 
15245 } else
15246 {
15247 {
15248 IkReal j4array[1], cj4array[1], sj4array[1];
15249 bool j4valid[1]={false};
15250 _nj4 = 1;
15251 IkReal x3440=((cj3)*(cj6));
15252 IkReal x3441=((IkReal(1.00000000000000))*(r21));
15253 IkReal x3442=((cj3)*(sj6));
15254 if( IKabs(((gconst23)*(((((IkReal(-1.00000000000000))*(r20)*(x3442)))+(((IkReal(-1.00000000000000))*(x3440)*(x3441))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst23)*(((((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x3440)))+(((IkReal(-1.00000000000000))*(cj5)*(x3441)*(x3442))))))) < IKFAST_ATAN2_MAGTHRESH )
15255     continue;
15256 j4array[0]=IKatan2(((gconst23)*(((((IkReal(-1.00000000000000))*(r20)*(x3442)))+(((IkReal(-1.00000000000000))*(x3440)*(x3441)))))), ((gconst23)*(((((IkReal(-1.00000000000000))*(cj3)*(r22)*(sj5)))+(((cj5)*(r20)*(x3440)))+(((IkReal(-1.00000000000000))*(cj5)*(x3441)*(x3442)))))));
15257 sj4array[0]=IKsin(j4array[0]);
15258 cj4array[0]=IKcos(j4array[0]);
15259 if( j4array[0] > IKPI )
15260 {
15261     j4array[0]-=IK2PI;
15262 }
15263 else if( j4array[0] < -IKPI )
15264 {    j4array[0]+=IK2PI;
15265 }
15266 j4valid[0] = true;
15267 for(int ij4 = 0; ij4 < 1; ++ij4)
15268 {
15269 if( !j4valid[ij4] )
15270 {
15271     continue;
15272 }
15273 _ij4[0] = ij4; _ij4[1] = -1;
15274 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15275 {
15276 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15277 {
15278     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15279 }
15280 }
15281 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15282 {
15283 IkReal evalcond[6];
15284 IkReal x3443=IKsin(j4);
15285 IkReal x3444=IKcos(j4);
15286 IkReal x3445=((IkReal(1.00000000000000))*(r00));
15287 IkReal x3446=((cj5)*(r01));
15288 IkReal x3447=((cj5)*(r11));
15289 IkReal x3448=((IkReal(1.00000000000000))*(cj5));
15290 IkReal x3449=((cj5)*(r21));
15291 IkReal x3450=((sj5)*(x3443));
15292 IkReal x3451=((cj6)*(x3443));
15293 IkReal x3452=((sj6)*(x3444));
15294 IkReal x3453=((sj6)*(x3443));
15295 IkReal x3454=((sj5)*(x3444));
15296 IkReal x3455=((cj6)*(x3444));
15297 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3448)*(x3451)))+(((r22)*(x3450)))+(((IkReal(-1.00000000000000))*(r21)*(x3455)))+(((IkReal(-1.00000000000000))*(r20)*(x3452)))+(((x3449)*(x3453))));
15298 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x3448)*(x3455)))+(cj3)+(((r21)*(x3451)))+(((r22)*(x3454)))+(((r20)*(x3453)))+(((x3449)*(x3452))));
15299 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x3455)))+(cj1)+(((r02)*(x3450)))+(((x3446)*(x3453)))+(((IkReal(-1.00000000000000))*(x3445)*(x3452)))+(((IkReal(-1.00000000000000))*(cj5)*(x3445)*(x3451))));
15300 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x3455)))+(sj1)+(((x3447)*(x3453)))+(((r12)*(x3450)))+(((IkReal(-1.00000000000000))*(r10)*(x3448)*(x3451)))+(((IkReal(-1.00000000000000))*(r10)*(x3452))));
15301 evalcond[4]=((((sj1)*(sj3)))+(((r02)*(x3454)))+(((x3446)*(x3452)))+(((r00)*(x3453)))+(((IkReal(-1.00000000000000))*(cj5)*(x3445)*(x3455)))+(((r01)*(x3451))));
15302 evalcond[5]=((((x3447)*(x3452)))+(((r12)*(x3454)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((IkReal(-1.00000000000000))*(r10)*(x3448)*(x3455)))+(((r10)*(x3453)))+(((r11)*(x3451))));
15303 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
15304 {
15305 continue;
15306 }
15307 }
15308 
15309 {
15310 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15311 vinfos[0].jointtype = 1;
15312 vinfos[0].foffset = j0;
15313 vinfos[0].indices[0] = _ij0[0];
15314 vinfos[0].indices[1] = _ij0[1];
15315 vinfos[0].maxsolutions = _nj0;
15316 vinfos[1].jointtype = 1;
15317 vinfos[1].foffset = j1;
15318 vinfos[1].indices[0] = _ij1[0];
15319 vinfos[1].indices[1] = _ij1[1];
15320 vinfos[1].maxsolutions = _nj1;
15321 vinfos[2].jointtype = 1;
15322 vinfos[2].foffset = j2;
15323 vinfos[2].indices[0] = _ij2[0];
15324 vinfos[2].indices[1] = _ij2[1];
15325 vinfos[2].maxsolutions = _nj2;
15326 vinfos[3].jointtype = 1;
15327 vinfos[3].foffset = j3;
15328 vinfos[3].indices[0] = _ij3[0];
15329 vinfos[3].indices[1] = _ij3[1];
15330 vinfos[3].maxsolutions = _nj3;
15331 vinfos[4].jointtype = 1;
15332 vinfos[4].foffset = j4;
15333 vinfos[4].indices[0] = _ij4[0];
15334 vinfos[4].indices[1] = _ij4[1];
15335 vinfos[4].maxsolutions = _nj4;
15336 vinfos[5].jointtype = 1;
15337 vinfos[5].foffset = j5;
15338 vinfos[5].indices[0] = _ij5[0];
15339 vinfos[5].indices[1] = _ij5[1];
15340 vinfos[5].maxsolutions = _nj5;
15341 vinfos[6].jointtype = 1;
15342 vinfos[6].foffset = j6;
15343 vinfos[6].indices[0] = _ij6[0];
15344 vinfos[6].indices[1] = _ij6[1];
15345 vinfos[6].maxsolutions = _nj6;
15346 std::vector<int> vfree(0);
15347 solutions.AddSolution(vinfos,vfree);
15348 }
15349 }
15350 }
15351 
15352 }
15353 
15354 }
15355 }
15356 }
15357 
15358 }
15359 
15360 }
15361 
15362 } else
15363 {
15364 {
15365 IkReal j4array[1], cj4array[1], sj4array[1];
15366 bool j4valid[1]={false};
15367 _nj4 = 1;
15368 IkReal x3456=((cj6)*(sj1));
15369 IkReal x3457=((IkReal(1.00000000000000))*(sj1)*(sj6));
15370 if( IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(r21)*(x3456)))+(((IkReal(-1.00000000000000))*(r20)*(x3457))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x3457)))+(((cj5)*(r20)*(x3456)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH )
15371     continue;
15372 j4array[0]=IKatan2(((gconst22)*(((((IkReal(-1.00000000000000))*(r21)*(x3456)))+(((IkReal(-1.00000000000000))*(r20)*(x3457)))))), ((gconst22)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x3457)))+(((cj5)*(r20)*(x3456)))+(((IkReal(-1.00000000000000))*(r22)*(sj1)*(sj5)))))));
15373 sj4array[0]=IKsin(j4array[0]);
15374 cj4array[0]=IKcos(j4array[0]);
15375 if( j4array[0] > IKPI )
15376 {
15377     j4array[0]-=IK2PI;
15378 }
15379 else if( j4array[0] < -IKPI )
15380 {    j4array[0]+=IK2PI;
15381 }
15382 j4valid[0] = true;
15383 for(int ij4 = 0; ij4 < 1; ++ij4)
15384 {
15385 if( !j4valid[ij4] )
15386 {
15387     continue;
15388 }
15389 _ij4[0] = ij4; _ij4[1] = -1;
15390 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15391 {
15392 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15393 {
15394     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15395 }
15396 }
15397 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15398 {
15399 IkReal evalcond[3];
15400 IkReal x3458=IKsin(j4);
15401 IkReal x3459=IKcos(j4);
15402 IkReal x3460=((IkReal(1.00000000000000))*(cj6));
15403 IkReal x3461=((sj5)*(x3458));
15404 IkReal x3462=((cj5)*(x3458));
15405 IkReal x3463=((IkReal(1.00000000000000))*(sj6)*(x3459));
15406 evalcond[0]=((((IkReal(-1.00000000000000))*(r21)*(x3459)*(x3460)))+(((IkReal(-1.00000000000000))*(r20)*(x3463)))+(((r21)*(sj6)*(x3462)))+(((IkReal(-1.00000000000000))*(r20)*(x3460)*(x3462)))+(((r22)*(x3461))));
15407 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x3463)))+(((IkReal(-1.00000000000000))*(r00)*(x3460)*(x3462)))+(cj1)+(((r01)*(sj6)*(x3462)))+(((IkReal(-1.00000000000000))*(r01)*(x3459)*(x3460)))+(((r02)*(x3461))));
15408 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x3459)*(x3460)))+(sj1)+(((IkReal(-1.00000000000000))*(r10)*(x3463)))+(((IkReal(-1.00000000000000))*(r10)*(x3460)*(x3462)))+(((r12)*(x3461)))+(((r11)*(sj6)*(x3462))));
15409 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
15410 {
15411 continue;
15412 }
15413 }
15414 
15415 {
15416 IkReal j3array[1], cj3array[1], sj3array[1];
15417 bool j3valid[1]={false};
15418 _nj3 = 1;
15419 IkReal x3464=((cj4)*(cj5));
15420 IkReal x3465=((cj6)*(r20));
15421 IkReal x3466=((IkReal(1.00000000000000))*(sj4));
15422 IkReal x3467=((IkReal(1.00000000000000))*(r21)*(sj6));
15423 if( IKabs(((((cj5)*(r22)))+(((sj5)*(x3465)))+(((IkReal(-1.00000000000000))*(sj5)*(x3467))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((x3464)*(x3465)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x3464)*(x3467)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3466)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3466))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((sj5)*(x3465)))+(((IkReal(-1.00000000000000))*(sj5)*(x3467)))))+IKsqr(((((x3464)*(x3465)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x3464)*(x3467)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3466)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3466)))))-1) <= IKFAST_SINCOS_THRESH )
15424     continue;
15425 j3array[0]=IKatan2(((((cj5)*(r22)))+(((sj5)*(x3465)))+(((IkReal(-1.00000000000000))*(sj5)*(x3467)))), ((((x3464)*(x3465)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x3464)*(x3467)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3466)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3466)))));
15426 sj3array[0]=IKsin(j3array[0]);
15427 cj3array[0]=IKcos(j3array[0]);
15428 if( j3array[0] > IKPI )
15429 {
15430     j3array[0]-=IK2PI;
15431 }
15432 else if( j3array[0] < -IKPI )
15433 {    j3array[0]+=IK2PI;
15434 }
15435 j3valid[0] = true;
15436 for(int ij3 = 0; ij3 < 1; ++ij3)
15437 {
15438 if( !j3valid[ij3] )
15439 {
15440     continue;
15441 }
15442 _ij3[0] = ij3; _ij3[1] = -1;
15443 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15444 {
15445 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15446 {
15447     j3valid[iij3]=false; _ij3[1] = iij3; break; 
15448 }
15449 }
15450 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15451 {
15452 IkReal evalcond[6];
15453 IkReal x3468=IKcos(j3);
15454 IkReal x3469=IKsin(j3);
15455 IkReal x3470=((r11)*(sj6));
15456 IkReal x3471=((IkReal(1.00000000000000))*(sj5));
15457 IkReal x3472=((IkReal(1.00000000000000))*(cj1));
15458 IkReal x3473=((sj4)*(sj6));
15459 IkReal x3474=((cj4)*(sj5));
15460 IkReal x3475=((cj6)*(sj4));
15461 IkReal x3476=((cj4)*(cj5));
15462 IkReal x3477=((r01)*(sj6));
15463 IkReal x3478=((r21)*(sj6));
15464 IkReal x3479=((cj6)*(sj5));
15465 IkReal x3480=((IkReal(1.00000000000000))*(cj6)*(x3476));
15466 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3471)*(x3478)))+(((r20)*(x3479)))+(((IkReal(-1.00000000000000))*(x3469))));
15467 evalcond[1]=((((IkReal(-1.00000000000000))*(x3471)*(x3477)))+(((r00)*(x3479)))+(((sj1)*(x3468)))+(((cj5)*(r02))));
15468 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x3479)))+(((IkReal(-1.00000000000000))*(x3468)*(x3472)))+(((IkReal(-1.00000000000000))*(x3470)*(x3471))));
15469 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x3480)))+(((x3476)*(x3478)))+(x3468)+(((r20)*(x3473)))+(((r21)*(x3475)))+(((r22)*(x3474))));
15470 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x3480)))+(((x3476)*(x3477)))+(((r01)*(x3475)))+(((r00)*(x3473)))+(((sj1)*(x3469)))+(((r02)*(x3474))));
15471 evalcond[5]=((((x3470)*(x3476)))+(((IkReal(-1.00000000000000))*(r10)*(x3480)))+(((r12)*(x3474)))+(((IkReal(-1.00000000000000))*(x3469)*(x3472)))+(((r10)*(x3473)))+(((r11)*(x3475))));
15472 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
15473 {
15474 continue;
15475 }
15476 }
15477 
15478 {
15479 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15480 vinfos[0].jointtype = 1;
15481 vinfos[0].foffset = j0;
15482 vinfos[0].indices[0] = _ij0[0];
15483 vinfos[0].indices[1] = _ij0[1];
15484 vinfos[0].maxsolutions = _nj0;
15485 vinfos[1].jointtype = 1;
15486 vinfos[1].foffset = j1;
15487 vinfos[1].indices[0] = _ij1[0];
15488 vinfos[1].indices[1] = _ij1[1];
15489 vinfos[1].maxsolutions = _nj1;
15490 vinfos[2].jointtype = 1;
15491 vinfos[2].foffset = j2;
15492 vinfos[2].indices[0] = _ij2[0];
15493 vinfos[2].indices[1] = _ij2[1];
15494 vinfos[2].maxsolutions = _nj2;
15495 vinfos[3].jointtype = 1;
15496 vinfos[3].foffset = j3;
15497 vinfos[3].indices[0] = _ij3[0];
15498 vinfos[3].indices[1] = _ij3[1];
15499 vinfos[3].maxsolutions = _nj3;
15500 vinfos[4].jointtype = 1;
15501 vinfos[4].foffset = j4;
15502 vinfos[4].indices[0] = _ij4[0];
15503 vinfos[4].indices[1] = _ij4[1];
15504 vinfos[4].maxsolutions = _nj4;
15505 vinfos[5].jointtype = 1;
15506 vinfos[5].foffset = j5;
15507 vinfos[5].indices[0] = _ij5[0];
15508 vinfos[5].indices[1] = _ij5[1];
15509 vinfos[5].maxsolutions = _nj5;
15510 vinfos[6].jointtype = 1;
15511 vinfos[6].foffset = j6;
15512 vinfos[6].indices[0] = _ij6[0];
15513 vinfos[6].indices[1] = _ij6[1];
15514 vinfos[6].maxsolutions = _nj6;
15515 std::vector<int> vfree(0);
15516 solutions.AddSolution(vinfos,vfree);
15517 }
15518 }
15519 }
15520 }
15521 }
15522 
15523 }
15524 
15525 }
15526 
15527 } else
15528 {
15529 {
15530 IkReal j4array[1], cj4array[1], sj4array[1];
15531 bool j4valid[1]={false};
15532 _nj4 = 1;
15533 IkReal x3481=((cj1)*(cj6));
15534 IkReal x3482=((IkReal(1.00000000000000))*(cj1));
15535 if( IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x3481)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3482))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3482)))+(((cj5)*(r20)*(x3481)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3482))))))) < IKFAST_ATAN2_MAGTHRESH )
15536     continue;
15537 j4array[0]=IKatan2(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x3481)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3482)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3482)))+(((cj5)*(r20)*(x3481)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3482)))))));
15538 sj4array[0]=IKsin(j4array[0]);
15539 cj4array[0]=IKcos(j4array[0]);
15540 if( j4array[0] > IKPI )
15541 {
15542     j4array[0]-=IK2PI;
15543 }
15544 else if( j4array[0] < -IKPI )
15545 {    j4array[0]+=IK2PI;
15546 }
15547 j4valid[0] = true;
15548 for(int ij4 = 0; ij4 < 1; ++ij4)
15549 {
15550 if( !j4valid[ij4] )
15551 {
15552     continue;
15553 }
15554 _ij4[0] = ij4; _ij4[1] = -1;
15555 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15556 {
15557 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15558 {
15559     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15560 }
15561 }
15562 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15563 {
15564 IkReal evalcond[3];
15565 IkReal x3483=IKsin(j4);
15566 IkReal x3484=IKcos(j4);
15567 IkReal x3485=((IkReal(1.00000000000000))*(cj6));
15568 IkReal x3486=((sj5)*(x3483));
15569 IkReal x3487=((cj5)*(x3483));
15570 IkReal x3488=((IkReal(1.00000000000000))*(sj6)*(x3484));
15571 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3488)))+(((r21)*(sj6)*(x3487)))+(((r22)*(x3486)))+(((IkReal(-1.00000000000000))*(r21)*(x3484)*(x3485)))+(((IkReal(-1.00000000000000))*(r20)*(x3485)*(x3487))));
15572 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x3488)))+(cj1)+(((r01)*(sj6)*(x3487)))+(((IkReal(-1.00000000000000))*(r00)*(x3485)*(x3487)))+(((r02)*(x3486)))+(((IkReal(-1.00000000000000))*(r01)*(x3484)*(x3485))));
15573 evalcond[2]=((sj1)+(((IkReal(-1.00000000000000))*(r10)*(x3488)))+(((r11)*(sj6)*(x3487)))+(((IkReal(-1.00000000000000))*(r11)*(x3484)*(x3485)))+(((r12)*(x3486)))+(((IkReal(-1.00000000000000))*(r10)*(x3485)*(x3487))));
15574 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
15575 {
15576 continue;
15577 }
15578 }
15579 
15580 {
15581 IkReal j3array[1], cj3array[1], sj3array[1];
15582 bool j3valid[1]={false};
15583 _nj3 = 1;
15584 IkReal x3489=((cj4)*(cj5));
15585 IkReal x3490=((cj6)*(r20));
15586 IkReal x3491=((IkReal(1.00000000000000))*(sj4));
15587 IkReal x3492=((IkReal(1.00000000000000))*(r21)*(sj6));
15588 if( IKabs(((((cj5)*(r22)))+(((sj5)*(x3490)))+(((IkReal(-1.00000000000000))*(sj5)*(x3492))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3491)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3491)))+(((IkReal(-1.00000000000000))*(x3489)*(x3492)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x3489)*(x3490))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((sj5)*(x3490)))+(((IkReal(-1.00000000000000))*(sj5)*(x3492)))))+IKsqr(((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3491)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3491)))+(((IkReal(-1.00000000000000))*(x3489)*(x3492)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x3489)*(x3490)))))-1) <= IKFAST_SINCOS_THRESH )
15589     continue;
15590 j3array[0]=IKatan2(((((cj5)*(r22)))+(((sj5)*(x3490)))+(((IkReal(-1.00000000000000))*(sj5)*(x3492)))), ((((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3491)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3491)))+(((IkReal(-1.00000000000000))*(x3489)*(x3492)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x3489)*(x3490)))));
15591 sj3array[0]=IKsin(j3array[0]);
15592 cj3array[0]=IKcos(j3array[0]);
15593 if( j3array[0] > IKPI )
15594 {
15595     j3array[0]-=IK2PI;
15596 }
15597 else if( j3array[0] < -IKPI )
15598 {    j3array[0]+=IK2PI;
15599 }
15600 j3valid[0] = true;
15601 for(int ij3 = 0; ij3 < 1; ++ij3)
15602 {
15603 if( !j3valid[ij3] )
15604 {
15605     continue;
15606 }
15607 _ij3[0] = ij3; _ij3[1] = -1;
15608 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15609 {
15610 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15611 {
15612     j3valid[iij3]=false; _ij3[1] = iij3; break; 
15613 }
15614 }
15615 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15616 {
15617 IkReal evalcond[6];
15618 IkReal x3493=IKcos(j3);
15619 IkReal x3494=IKsin(j3);
15620 IkReal x3495=((r11)*(sj6));
15621 IkReal x3496=((IkReal(1.00000000000000))*(sj5));
15622 IkReal x3497=((IkReal(1.00000000000000))*(cj1));
15623 IkReal x3498=((sj4)*(sj6));
15624 IkReal x3499=((cj4)*(sj5));
15625 IkReal x3500=((cj6)*(sj4));
15626 IkReal x3501=((cj4)*(cj5));
15627 IkReal x3502=((r01)*(sj6));
15628 IkReal x3503=((r21)*(sj6));
15629 IkReal x3504=((cj6)*(sj5));
15630 IkReal x3505=((IkReal(1.00000000000000))*(cj6)*(x3501));
15631 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3504)))+(((IkReal(-1.00000000000000))*(x3496)*(x3503)))+(((IkReal(-1.00000000000000))*(x3494))));
15632 evalcond[1]=((((r00)*(x3504)))+(((IkReal(-1.00000000000000))*(x3496)*(x3502)))+(((sj1)*(x3493)))+(((cj5)*(r02))));
15633 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x3504)))+(((IkReal(-1.00000000000000))*(x3493)*(x3497)))+(((IkReal(-1.00000000000000))*(x3495)*(x3496))));
15634 evalcond[3]=((((r20)*(x3498)))+(((x3501)*(x3503)))+(x3493)+(((r22)*(x3499)))+(((IkReal(-1.00000000000000))*(r20)*(x3505)))+(((r21)*(x3500))));
15635 evalcond[4]=((((r01)*(x3500)))+(((x3501)*(x3502)))+(((r00)*(x3498)))+(((IkReal(-1.00000000000000))*(r00)*(x3505)))+(((r02)*(x3499)))+(((sj1)*(x3494))));
15636 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3505)))+(((x3495)*(x3501)))+(((IkReal(-1.00000000000000))*(x3494)*(x3497)))+(((r10)*(x3498)))+(((r11)*(x3500)))+(((r12)*(x3499))));
15637 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
15638 {
15639 continue;
15640 }
15641 }
15642 
15643 {
15644 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15645 vinfos[0].jointtype = 1;
15646 vinfos[0].foffset = j0;
15647 vinfos[0].indices[0] = _ij0[0];
15648 vinfos[0].indices[1] = _ij0[1];
15649 vinfos[0].maxsolutions = _nj0;
15650 vinfos[1].jointtype = 1;
15651 vinfos[1].foffset = j1;
15652 vinfos[1].indices[0] = _ij1[0];
15653 vinfos[1].indices[1] = _ij1[1];
15654 vinfos[1].maxsolutions = _nj1;
15655 vinfos[2].jointtype = 1;
15656 vinfos[2].foffset = j2;
15657 vinfos[2].indices[0] = _ij2[0];
15658 vinfos[2].indices[1] = _ij2[1];
15659 vinfos[2].maxsolutions = _nj2;
15660 vinfos[3].jointtype = 1;
15661 vinfos[3].foffset = j3;
15662 vinfos[3].indices[0] = _ij3[0];
15663 vinfos[3].indices[1] = _ij3[1];
15664 vinfos[3].maxsolutions = _nj3;
15665 vinfos[4].jointtype = 1;
15666 vinfos[4].foffset = j4;
15667 vinfos[4].indices[0] = _ij4[0];
15668 vinfos[4].indices[1] = _ij4[1];
15669 vinfos[4].maxsolutions = _nj4;
15670 vinfos[5].jointtype = 1;
15671 vinfos[5].foffset = j5;
15672 vinfos[5].indices[0] = _ij5[0];
15673 vinfos[5].indices[1] = _ij5[1];
15674 vinfos[5].maxsolutions = _nj5;
15675 vinfos[6].jointtype = 1;
15676 vinfos[6].foffset = j6;
15677 vinfos[6].indices[0] = _ij6[0];
15678 vinfos[6].indices[1] = _ij6[1];
15679 vinfos[6].maxsolutions = _nj6;
15680 std::vector<int> vfree(0);
15681 solutions.AddSolution(vinfos,vfree);
15682 }
15683 }
15684 }
15685 }
15686 }
15687 
15688 }
15689 
15690 }
15691 
15692 } else
15693 {
15694 if( 1 )
15695 {
15696 continue;
15697 
15698 } else
15699 {
15700 }
15701 }
15702 }
15703 }
15704 
15705 } else
15706 {
15707 {
15708 IkReal j3array[1], cj3array[1], sj3array[1];
15709 bool j3valid[1]={false};
15710 _nj3 = 1;
15711 IkReal x3506=(sj1)*(sj1);
15712 IkReal x3507=(cj1)*(cj1);
15713 IkReal x3508=((cj5)*(r12));
15714 IkReal x3509=((cj6)*(sj5));
15715 IkReal x3510=((IkReal(1.00000000000000))*(sj1));
15716 IkReal x3511=((cj5)*(r02));
15717 IkReal x3512=((IkReal(1.00000000000000))*(cj1));
15718 IkReal x3513=((r01)*(sj5)*(sj6));
15719 IkReal x3514=((r11)*(sj5)*(sj6));
15720 if( IKabs(((((IKabs(((((cj2)*(x3506)))+(((cj2)*(x3507))))) != 0)?((IkReal)1/(((((cj2)*(x3506)))+(((cj2)*(x3507)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3513)))+(((cj1)*(r00)*(x3509)))+(((r10)*(sj1)*(x3509)))+(((IkReal(-1.00000000000000))*(x3510)*(x3514)))+(((cj1)*(x3511)))+(((sj1)*(x3508))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x3506)+(x3507))) != 0)?((IkReal)1/(((x3506)+(x3507)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3514)))+(((IkReal(-1.00000000000000))*(x3510)*(x3511)))+(((cj1)*(x3508)))+(((sj1)*(x3513)))+(((IkReal(-1.00000000000000))*(r00)*(x3509)*(x3510)))+(((cj1)*(r10)*(x3509))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x3506)))+(((cj2)*(x3507))))) != 0)?((IkReal)1/(((((cj2)*(x3506)))+(((cj2)*(x3507)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3513)))+(((cj1)*(r00)*(x3509)))+(((r10)*(sj1)*(x3509)))+(((IkReal(-1.00000000000000))*(x3510)*(x3514)))+(((cj1)*(x3511)))+(((sj1)*(x3508)))))))+IKsqr(((((IKabs(((x3506)+(x3507))) != 0)?((IkReal)1/(((x3506)+(x3507)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3514)))+(((IkReal(-1.00000000000000))*(x3510)*(x3511)))+(((cj1)*(x3508)))+(((sj1)*(x3513)))+(((IkReal(-1.00000000000000))*(r00)*(x3509)*(x3510)))+(((cj1)*(r10)*(x3509)))))))-1) <= IKFAST_SINCOS_THRESH )
15721     continue;
15722 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x3506)))+(((cj2)*(x3507))))) != 0)?((IkReal)1/(((((cj2)*(x3506)))+(((cj2)*(x3507)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3513)))+(((cj1)*(r00)*(x3509)))+(((r10)*(sj1)*(x3509)))+(((IkReal(-1.00000000000000))*(x3510)*(x3514)))+(((cj1)*(x3511)))+(((sj1)*(x3508)))))), ((((IKabs(((x3506)+(x3507))) != 0)?((IkReal)1/(((x3506)+(x3507)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3512)*(x3514)))+(((IkReal(-1.00000000000000))*(x3510)*(x3511)))+(((cj1)*(x3508)))+(((sj1)*(x3513)))+(((IkReal(-1.00000000000000))*(r00)*(x3509)*(x3510)))+(((cj1)*(r10)*(x3509)))))));
15723 sj3array[0]=IKsin(j3array[0]);
15724 cj3array[0]=IKcos(j3array[0]);
15725 if( j3array[0] > IKPI )
15726 {
15727     j3array[0]-=IK2PI;
15728 }
15729 else if( j3array[0] < -IKPI )
15730 {    j3array[0]+=IK2PI;
15731 }
15732 j3valid[0] = true;
15733 for(int ij3 = 0; ij3 < 1; ++ij3)
15734 {
15735 if( !j3valid[ij3] )
15736 {
15737     continue;
15738 }
15739 _ij3[0] = ij3; _ij3[1] = -1;
15740 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15741 {
15742 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15743 {
15744     j3valid[iij3]=false; _ij3[1] = iij3; break; 
15745 }
15746 }
15747 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15748 {
15749 IkReal evalcond[3];
15750 IkReal x3515=IKsin(j3);
15751 IkReal x3516=IKcos(j3);
15752 IkReal x3517=((IkReal(1.00000000000000))*(cj1));
15753 IkReal x3518=((cj6)*(sj5));
15754 IkReal x3519=((IkReal(1.00000000000000))*(sj5)*(sj6));
15755 IkReal x3520=((cj2)*(x3515));
15756 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3518)))+(((sj2)*(x3515)))+(((IkReal(-1.00000000000000))*(r21)*(x3519))));
15757 evalcond[1]=((((r00)*(x3518)))+(((IkReal(-1.00000000000000))*(x3517)*(x3520)))+(((sj1)*(x3516)))+(((IkReal(-1.00000000000000))*(r01)*(x3519)))+(((cj5)*(r02))));
15758 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x3520)))+(((r10)*(x3518)))+(((IkReal(-1.00000000000000))*(r11)*(x3519)))+(((IkReal(-1.00000000000000))*(x3516)*(x3517))));
15759 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
15760 {
15761 continue;
15762 }
15763 }
15764 
15765 {
15766 IkReal dummyeval[1];
15767 IkReal gconst2;
15768 IkReal x3521=(cj5)*(cj5);
15769 IkReal x3522=(r21)*(r21);
15770 IkReal x3523=(sj6)*(sj6);
15771 IkReal x3524=(cj6)*(cj6);
15772 IkReal x3525=(r20)*(r20);
15773 IkReal x3526=((cj6)*(r20));
15774 IkReal x3527=((IkReal(1.00000000000000))*(x3524));
15775 IkReal x3528=((IkReal(2.00000000000000))*(r21)*(sj6));
15776 IkReal x3529=((cj5)*(r22)*(sj5));
15777 IkReal x3530=((IkReal(1.00000000000000))*(x3523));
15778 gconst2=IKsign(((((IkReal(-1.00000000000000))*(x3522)*(x3527)))+(((IkReal(-1.00000000000000))*(x3521)*(x3525)*(x3527)))+(((IkReal(-1.00000000000000))*(x3526)*(x3528)))+(((IkReal(-1.00000000000000))*(x3525)*(x3530)))+(((IkReal(2.00000000000000))*(x3526)*(x3529)))+(((x3521)*(x3526)*(x3528)))+(((IkReal(-1.00000000000000))*(x3521)*(x3522)*(x3530)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3528)*(x3529)))));
15779 IkReal x3531=(cj5)*(cj5);
15780 IkReal x3532=(r21)*(r21);
15781 IkReal x3533=(sj6)*(sj6);
15782 IkReal x3534=(cj6)*(cj6);
15783 IkReal x3535=(r20)*(r20);
15784 IkReal x3536=((cj6)*(r20));
15785 IkReal x3537=((IkReal(1.00000000000000))*(x3534));
15786 IkReal x3538=((IkReal(2.00000000000000))*(r21)*(sj6));
15787 IkReal x3539=((cj5)*(r22)*(sj5));
15788 IkReal x3540=((IkReal(1.00000000000000))*(x3533));
15789 dummyeval[0]=((((IkReal(-1.00000000000000))*(x3538)*(x3539)))+(((IkReal(2.00000000000000))*(x3536)*(x3539)))+(((IkReal(-1.00000000000000))*(x3531)*(x3532)*(x3540)))+(((IkReal(-1.00000000000000))*(x3535)*(x3540)))+(((x3531)*(x3536)*(x3538)))+(((IkReal(-1.00000000000000))*(x3532)*(x3537)))+(((IkReal(-1.00000000000000))*(x3531)*(x3535)*(x3537)))+(((IkReal(-1.00000000000000))*((r22)*(r22))*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3536)*(x3538))));
15790 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
15791 {
15792 {
15793 IkReal dummyeval[1];
15794 IkReal gconst3;
15795 IkReal x3541=(cj5)*(cj5);
15796 IkReal x3542=(sj6)*(sj6);
15797 IkReal x3543=(cj6)*(cj6);
15798 IkReal x3544=((r00)*(r20));
15799 IkReal x3545=((cj5)*(sj5));
15800 IkReal x3546=((r01)*(r21));
15801 IkReal x3547=((r21)*(sj6));
15802 IkReal x3548=((sj6)*(x3545));
15803 IkReal x3549=((IkReal(1.00000000000000))*(cj6)*(r00));
15804 IkReal x3550=((cj6)*(r01)*(r20)*(sj6));
15805 gconst3=IKsign(((((x3541)*(x3542)*(x3546)))+(((r01)*(r22)*(x3548)))+(((x3542)*(x3544)))+(((x3543)*(x3546)))+(((IkReal(-1.00000000000000))*(r22)*(x3545)*(x3549)))+(x3550)+(((cj6)*(r00)*(x3547)))+(((IkReal(-1.00000000000000))*(x3541)*(x3547)*(x3549)))+(((r02)*(x3545)*(x3547)))+(((r02)*(r22)*((sj5)*(sj5))))+(((IkReal(-1.00000000000000))*(x3541)*(x3550)))+(((x3541)*(x3543)*(x3544)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3545)))));
15806 IkReal x3551=(cj5)*(cj5);
15807 IkReal x3552=(sj6)*(sj6);
15808 IkReal x3553=(cj6)*(cj6);
15809 IkReal x3554=((r00)*(r20));
15810 IkReal x3555=((cj5)*(sj5));
15811 IkReal x3556=((r01)*(r21));
15812 IkReal x3557=((r21)*(sj6));
15813 IkReal x3558=((sj6)*(x3555));
15814 IkReal x3559=((IkReal(1.00000000000000))*(cj6)*(r00));
15815 IkReal x3560=x3550;
15816 dummyeval[0]=((((r01)*(r22)*(x3558)))+(((IkReal(-1.00000000000000))*(r22)*(x3555)*(x3559)))+(x3560)+(((x3551)*(x3552)*(x3556)))+(((cj6)*(r00)*(x3557)))+(((x3552)*(x3554)))+(((x3551)*(x3553)*(x3554)))+(((x3553)*(x3556)))+(((r02)*(r22)*((sj5)*(sj5))))+(((r02)*(x3555)*(x3557)))+(((IkReal(-1.00000000000000))*(cj6)*(r02)*(r20)*(x3555)))+(((IkReal(-1.00000000000000))*(x3551)*(x3560)))+(((IkReal(-1.00000000000000))*(x3551)*(x3557)*(x3559))));
15817 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
15818 {
15819 continue;
15820 
15821 } else
15822 {
15823 {
15824 IkReal j4array[1], cj4array[1], sj4array[1];
15825 bool j4valid[1]={false};
15826 _nj4 = 1;
15827 IkReal x3561=((sj2)*(sj5));
15828 IkReal x3562=((cj5)*(sj6));
15829 IkReal x3563=((sj2)*(sj6));
15830 IkReal x3564=((cj3)*(r00));
15831 IkReal x3565=((cj3)*(r01)*(sj2));
15832 IkReal x3566=((IkReal(1.00000000000000))*(cj1)*(cj6)*(sj2));
15833 if( IKabs(((gconst3)*(((((x3563)*(x3564)))+(((cj1)*(r21)*(sj2)*(x3562)))+(((cj1)*(r22)*(x3561)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x3566)))+(((cj6)*(x3565))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((x3562)*(x3565)))+(((IkReal(-1.00000000000000))*(r21)*(x3566)))+(((cj3)*(r02)*(x3561)))+(((IkReal(-1.00000000000000))*(cj1)*(r20)*(x3563)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(sj2)*(x3564))))))) < IKFAST_ATAN2_MAGTHRESH )
15834     continue;
15835 j4array[0]=IKatan2(((gconst3)*(((((x3563)*(x3564)))+(((cj1)*(r21)*(sj2)*(x3562)))+(((cj1)*(r22)*(x3561)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x3566)))+(((cj6)*(x3565)))))), ((gconst3)*(((((x3562)*(x3565)))+(((IkReal(-1.00000000000000))*(r21)*(x3566)))+(((cj3)*(r02)*(x3561)))+(((IkReal(-1.00000000000000))*(cj1)*(r20)*(x3563)))+(((IkReal(-1.00000000000000))*(cj5)*(cj6)*(sj2)*(x3564)))))));
15836 sj4array[0]=IKsin(j4array[0]);
15837 cj4array[0]=IKcos(j4array[0]);
15838 if( j4array[0] > IKPI )
15839 {
15840     j4array[0]-=IK2PI;
15841 }
15842 else if( j4array[0] < -IKPI )
15843 {    j4array[0]+=IK2PI;
15844 }
15845 j4valid[0] = true;
15846 for(int ij4 = 0; ij4 < 1; ++ij4)
15847 {
15848 if( !j4valid[ij4] )
15849 {
15850     continue;
15851 }
15852 _ij4[0] = ij4; _ij4[1] = -1;
15853 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15854 {
15855 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15856 {
15857     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15858 }
15859 }
15860 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15861 {
15862 IkReal evalcond[6];
15863 IkReal x3567=IKsin(j4);
15864 IkReal x3568=IKcos(j4);
15865 IkReal x3569=((IkReal(1.00000000000000))*(r00));
15866 IkReal x3570=((cj5)*(r01));
15867 IkReal x3571=((cj5)*(r11));
15868 IkReal x3572=((cj2)*(cj3));
15869 IkReal x3573=((IkReal(1.00000000000000))*(sj2));
15870 IkReal x3574=((IkReal(1.00000000000000))*(cj5));
15871 IkReal x3575=((cj5)*(r21));
15872 IkReal x3576=((sj5)*(x3567));
15873 IkReal x3577=((cj6)*(x3567));
15874 IkReal x3578=((sj6)*(x3568));
15875 IkReal x3579=((sj6)*(x3567));
15876 IkReal x3580=((sj5)*(x3568));
15877 IkReal x3581=((cj6)*(x3568));
15878 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3574)*(x3577)))+(((IkReal(-1.00000000000000))*(r20)*(x3578)))+(((x3575)*(x3579)))+(((IkReal(-1.00000000000000))*(r21)*(x3581)))+(((r22)*(x3576)))+(((IkReal(-1.00000000000000))*(cj2))));
15879 evalcond[1]=((((r21)*(x3577)))+(((IkReal(-1.00000000000000))*(cj3)*(x3573)))+(((IkReal(-1.00000000000000))*(r20)*(x3574)*(x3581)))+(((x3575)*(x3578)))+(((r20)*(x3579)))+(((r22)*(x3580))));
15880 evalcond[2]=((((x3570)*(x3579)))+(((IkReal(-1.00000000000000))*(r01)*(x3581)))+(((IkReal(-1.00000000000000))*(x3569)*(x3578)))+(((r02)*(x3576)))+(((IkReal(-1.00000000000000))*(cj5)*(x3569)*(x3577)))+(((IkReal(-1.00000000000000))*(cj1)*(x3573))));
15881 evalcond[3]=((((x3571)*(x3579)))+(((r12)*(x3576)))+(((IkReal(-1.00000000000000))*(sj1)*(x3573)))+(((IkReal(-1.00000000000000))*(r11)*(x3581)))+(((IkReal(-1.00000000000000))*(r10)*(x3578)))+(((IkReal(-1.00000000000000))*(r10)*(x3574)*(x3577))));
15882 evalcond[4]=((((x3570)*(x3578)))+(((r00)*(x3579)))+(((r01)*(x3577)))+(((r02)*(x3580)))+(((IkReal(-1.00000000000000))*(cj5)*(x3569)*(x3581)))+(((sj1)*(sj3)))+(((cj1)*(x3572))));
15883 evalcond[5]=((((r10)*(x3579)))+(((r12)*(x3580)))+(((x3571)*(x3578)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((IkReal(-1.00000000000000))*(r10)*(x3574)*(x3581)))+(((sj1)*(x3572)))+(((r11)*(x3577))));
15884 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
15885 {
15886 continue;
15887 }
15888 }
15889 
15890 {
15891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15892 vinfos[0].jointtype = 1;
15893 vinfos[0].foffset = j0;
15894 vinfos[0].indices[0] = _ij0[0];
15895 vinfos[0].indices[1] = _ij0[1];
15896 vinfos[0].maxsolutions = _nj0;
15897 vinfos[1].jointtype = 1;
15898 vinfos[1].foffset = j1;
15899 vinfos[1].indices[0] = _ij1[0];
15900 vinfos[1].indices[1] = _ij1[1];
15901 vinfos[1].maxsolutions = _nj1;
15902 vinfos[2].jointtype = 1;
15903 vinfos[2].foffset = j2;
15904 vinfos[2].indices[0] = _ij2[0];
15905 vinfos[2].indices[1] = _ij2[1];
15906 vinfos[2].maxsolutions = _nj2;
15907 vinfos[3].jointtype = 1;
15908 vinfos[3].foffset = j3;
15909 vinfos[3].indices[0] = _ij3[0];
15910 vinfos[3].indices[1] = _ij3[1];
15911 vinfos[3].maxsolutions = _nj3;
15912 vinfos[4].jointtype = 1;
15913 vinfos[4].foffset = j4;
15914 vinfos[4].indices[0] = _ij4[0];
15915 vinfos[4].indices[1] = _ij4[1];
15916 vinfos[4].maxsolutions = _nj4;
15917 vinfos[5].jointtype = 1;
15918 vinfos[5].foffset = j5;
15919 vinfos[5].indices[0] = _ij5[0];
15920 vinfos[5].indices[1] = _ij5[1];
15921 vinfos[5].maxsolutions = _nj5;
15922 vinfos[6].jointtype = 1;
15923 vinfos[6].foffset = j6;
15924 vinfos[6].indices[0] = _ij6[0];
15925 vinfos[6].indices[1] = _ij6[1];
15926 vinfos[6].maxsolutions = _nj6;
15927 std::vector<int> vfree(0);
15928 solutions.AddSolution(vinfos,vfree);
15929 }
15930 }
15931 }
15932 
15933 }
15934 
15935 }
15936 
15937 } else
15938 {
15939 {
15940 IkReal j4array[1], cj4array[1], sj4array[1];
15941 bool j4valid[1]={false};
15942 _nj4 = 1;
15943 IkReal x3582=((r22)*(sj5));
15944 IkReal x3583=((cj2)*(cj5));
15945 IkReal x3584=((r21)*(sj6));
15946 IkReal x3585=((cj6)*(r20));
15947 IkReal x3586=((cj6)*(r21));
15948 IkReal x3587=((r20)*(sj6));
15949 IkReal x3588=((IkReal(1.00000000000000))*(cj3)*(sj2));
15950 if( IKabs(((gconst2)*(((((x3583)*(x3585)))+(((IkReal(-1.00000000000000))*(x3587)*(x3588)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)))+(((IkReal(-1.00000000000000))*(cj2)*(x3582)))+(((IkReal(-1.00000000000000))*(x3586)*(x3588))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(cj5)*(x3584)*(x3588)))+(((IkReal(-1.00000000000000))*(x3582)*(x3588)))+(((cj3)*(cj5)*(sj2)*(x3585)))+(((cj2)*(x3587)))+(((cj2)*(x3586))))))) < IKFAST_ATAN2_MAGTHRESH )
15951     continue;
15952 j4array[0]=IKatan2(((gconst2)*(((((x3583)*(x3585)))+(((IkReal(-1.00000000000000))*(x3587)*(x3588)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)))+(((IkReal(-1.00000000000000))*(cj2)*(x3582)))+(((IkReal(-1.00000000000000))*(x3586)*(x3588)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(cj5)*(x3584)*(x3588)))+(((IkReal(-1.00000000000000))*(x3582)*(x3588)))+(((cj3)*(cj5)*(sj2)*(x3585)))+(((cj2)*(x3587)))+(((cj2)*(x3586)))))));
15953 sj4array[0]=IKsin(j4array[0]);
15954 cj4array[0]=IKcos(j4array[0]);
15955 if( j4array[0] > IKPI )
15956 {
15957     j4array[0]-=IK2PI;
15958 }
15959 else if( j4array[0] < -IKPI )
15960 {    j4array[0]+=IK2PI;
15961 }
15962 j4valid[0] = true;
15963 for(int ij4 = 0; ij4 < 1; ++ij4)
15964 {
15965 if( !j4valid[ij4] )
15966 {
15967     continue;
15968 }
15969 _ij4[0] = ij4; _ij4[1] = -1;
15970 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15971 {
15972 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15973 {
15974     j4valid[iij4]=false; _ij4[1] = iij4; break; 
15975 }
15976 }
15977 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15978 {
15979 IkReal evalcond[6];
15980 IkReal x3589=IKsin(j4);
15981 IkReal x3590=IKcos(j4);
15982 IkReal x3591=((IkReal(1.00000000000000))*(r00));
15983 IkReal x3592=((cj5)*(r01));
15984 IkReal x3593=((cj5)*(r11));
15985 IkReal x3594=((cj2)*(cj3));
15986 IkReal x3595=((IkReal(1.00000000000000))*(sj2));
15987 IkReal x3596=((IkReal(1.00000000000000))*(cj5));
15988 IkReal x3597=((cj5)*(r21));
15989 IkReal x3598=((sj5)*(x3589));
15990 IkReal x3599=((cj6)*(x3589));
15991 IkReal x3600=((sj6)*(x3590));
15992 IkReal x3601=((sj6)*(x3589));
15993 IkReal x3602=((sj5)*(x3590));
15994 IkReal x3603=((cj6)*(x3590));
15995 evalcond[0]=((((IkReal(-1.00000000000000))*(r20)*(x3596)*(x3599)))+(((IkReal(-1.00000000000000))*(r20)*(x3600)))+(((x3597)*(x3601)))+(((r22)*(x3598)))+(((IkReal(-1.00000000000000))*(r21)*(x3603)))+(((IkReal(-1.00000000000000))*(cj2))));
15996 evalcond[1]=((((r21)*(x3599)))+(((r22)*(x3602)))+(((x3597)*(x3600)))+(((IkReal(-1.00000000000000))*(r20)*(x3596)*(x3603)))+(((r20)*(x3601)))+(((IkReal(-1.00000000000000))*(cj3)*(x3595))));
15997 evalcond[2]=((((IkReal(-1.00000000000000))*(cj1)*(x3595)))+(((r02)*(x3598)))+(((x3592)*(x3601)))+(((IkReal(-1.00000000000000))*(cj5)*(x3591)*(x3599)))+(((IkReal(-1.00000000000000))*(x3591)*(x3600)))+(((IkReal(-1.00000000000000))*(r01)*(x3603))));
15998 evalcond[3]=((((x3593)*(x3601)))+(((r12)*(x3598)))+(((IkReal(-1.00000000000000))*(sj1)*(x3595)))+(((IkReal(-1.00000000000000))*(r11)*(x3603)))+(((IkReal(-1.00000000000000))*(r10)*(x3600)))+(((IkReal(-1.00000000000000))*(r10)*(x3596)*(x3599))));
15999 evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x3591)*(x3603)))+(((r02)*(x3602)))+(((sj1)*(sj3)))+(((x3592)*(x3600)))+(((cj1)*(x3594)))+(((r00)*(x3601)))+(((r01)*(x3599))));
16000 evalcond[5]=((((sj1)*(x3594)))+(((r11)*(x3599)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)))+(((x3593)*(x3600)))+(((r10)*(x3601)))+(((IkReal(-1.00000000000000))*(r10)*(x3596)*(x3603)))+(((r12)*(x3602))));
16001 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
16002 {
16003 continue;
16004 }
16005 }
16006 
16007 {
16008 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16009 vinfos[0].jointtype = 1;
16010 vinfos[0].foffset = j0;
16011 vinfos[0].indices[0] = _ij0[0];
16012 vinfos[0].indices[1] = _ij0[1];
16013 vinfos[0].maxsolutions = _nj0;
16014 vinfos[1].jointtype = 1;
16015 vinfos[1].foffset = j1;
16016 vinfos[1].indices[0] = _ij1[0];
16017 vinfos[1].indices[1] = _ij1[1];
16018 vinfos[1].maxsolutions = _nj1;
16019 vinfos[2].jointtype = 1;
16020 vinfos[2].foffset = j2;
16021 vinfos[2].indices[0] = _ij2[0];
16022 vinfos[2].indices[1] = _ij2[1];
16023 vinfos[2].maxsolutions = _nj2;
16024 vinfos[3].jointtype = 1;
16025 vinfos[3].foffset = j3;
16026 vinfos[3].indices[0] = _ij3[0];
16027 vinfos[3].indices[1] = _ij3[1];
16028 vinfos[3].maxsolutions = _nj3;
16029 vinfos[4].jointtype = 1;
16030 vinfos[4].foffset = j4;
16031 vinfos[4].indices[0] = _ij4[0];
16032 vinfos[4].indices[1] = _ij4[1];
16033 vinfos[4].maxsolutions = _nj4;
16034 vinfos[5].jointtype = 1;
16035 vinfos[5].foffset = j5;
16036 vinfos[5].indices[0] = _ij5[0];
16037 vinfos[5].indices[1] = _ij5[1];
16038 vinfos[5].maxsolutions = _nj5;
16039 vinfos[6].jointtype = 1;
16040 vinfos[6].foffset = j6;
16041 vinfos[6].indices[0] = _ij6[0];
16042 vinfos[6].indices[1] = _ij6[1];
16043 vinfos[6].maxsolutions = _nj6;
16044 std::vector<int> vfree(0);
16045 solutions.AddSolution(vinfos,vfree);
16046 }
16047 }
16048 }
16049 
16050 }
16051 
16052 }
16053 }
16054 }
16055 
16056 }
16057 
16058 }
16059 
16060 } else
16061 {
16062 {
16063 IkReal j4array[1], cj4array[1], sj4array[1];
16064 bool j4valid[1]={false};
16065 _nj4 = 1;
16066 IkReal x3604=((cj2)*(cj6));
16067 IkReal x3605=((cj5)*(sj6));
16068 IkReal x3606=((IkReal(1.00000000000000))*(sj1)*(sj2));
16069 IkReal x3607=((r21)*(x3606));
16070 if( IKabs(((gconst1)*(((((cj2)*(r10)*(sj6)))+(((r11)*(x3604)))+(((IkReal(-1.00000000000000))*(cj6)*(x3607)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3606))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((cj2)*(r11)*(x3605)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3606)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(cj5)*(r10)*(x3604)))+(((cj2)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(x3605)*(x3607))))))) < IKFAST_ATAN2_MAGTHRESH )
16071     continue;
16072 j4array[0]=IKatan2(((gconst1)*(((((cj2)*(r10)*(sj6)))+(((r11)*(x3604)))+(((IkReal(-1.00000000000000))*(cj6)*(x3607)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3606)))))), ((gconst1)*(((((cj2)*(r11)*(x3605)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3606)))+(((cj5)*(cj6)*(r20)*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(cj5)*(r10)*(x3604)))+(((cj2)*(r12)*(sj5)))+(((IkReal(-1.00000000000000))*(x3605)*(x3607)))))));
16073 sj4array[0]=IKsin(j4array[0]);
16074 cj4array[0]=IKcos(j4array[0]);
16075 if( j4array[0] > IKPI )
16076 {
16077     j4array[0]-=IK2PI;
16078 }
16079 else if( j4array[0] < -IKPI )
16080 {    j4array[0]+=IK2PI;
16081 }
16082 j4valid[0] = true;
16083 for(int ij4 = 0; ij4 < 1; ++ij4)
16084 {
16085 if( !j4valid[ij4] )
16086 {
16087     continue;
16088 }
16089 _ij4[0] = ij4; _ij4[1] = -1;
16090 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
16091 {
16092 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
16093 {
16094     j4valid[iij4]=false; _ij4[1] = iij4; break; 
16095 }
16096 }
16097 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
16098 {
16099 IkReal evalcond[3];
16100 IkReal x3608=IKsin(j4);
16101 IkReal x3609=IKcos(j4);
16102 IkReal x3610=((IkReal(1.00000000000000))*(sj2));
16103 IkReal x3611=((IkReal(1.00000000000000))*(cj6));
16104 IkReal x3612=((sj5)*(x3608));
16105 IkReal x3613=((cj5)*(x3608));
16106 IkReal x3614=((IkReal(1.00000000000000))*(sj6)*(x3609));
16107 evalcond[0]=((((r21)*(sj6)*(x3613)))+(((r22)*(x3612)))+(((IkReal(-1.00000000000000))*(r20)*(x3611)*(x3613)))+(((IkReal(-1.00000000000000))*(r21)*(x3609)*(x3611)))+(((IkReal(-1.00000000000000))*(r20)*(x3614)))+(((IkReal(-1.00000000000000))*(cj2))));
16108 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x3614)))+(((r01)*(sj6)*(x3613)))+(((r02)*(x3612)))+(((IkReal(-1.00000000000000))*(cj1)*(x3610)))+(((IkReal(-1.00000000000000))*(r00)*(x3611)*(x3613)))+(((IkReal(-1.00000000000000))*(r01)*(x3609)*(x3611))));
16109 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x3614)))+(((r11)*(sj6)*(x3613)))+(((IkReal(-1.00000000000000))*(r11)*(x3609)*(x3611)))+(((IkReal(-1.00000000000000))*(r10)*(x3611)*(x3613)))+(((IkReal(-1.00000000000000))*(sj1)*(x3610)))+(((r12)*(x3612))));
16110 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
16111 {
16112 continue;
16113 }
16114 }
16115 
16116 {
16117 IkReal dummyeval[1];
16118 IkReal gconst4;
16119 gconst4=IKsign(sj2);
16120 dummyeval[0]=sj2;
16121 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
16122 {
16123 {
16124 IkReal dummyeval[2];
16125 IkReal x3615=(sj1)*(sj1);
16126 IkReal x3616=(cj1)*(cj1);
16127 dummyeval[0]=((((cj2)*(x3615)))+(((cj2)*(x3616))));
16128 dummyeval[1]=((x3615)+(x3616));
16129 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
16130 {
16131 {
16132 IkReal dummyeval[2];
16133 dummyeval[0]=sj2;
16134 dummyeval[1]=cj1;
16135 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
16136 {
16137 {
16138 IkReal evalcond[9];
16139 IkReal x3617=((sj4)*(sj5));
16140 IkReal x3618=((cj6)*(r20));
16141 IkReal x3619=((IkReal(1.00000000000000))*(cj4));
16142 IkReal x3620=((IkReal(0.390000000000000))*(sj5));
16143 IkReal x3621=((r21)*(sj6));
16144 IkReal x3622=((IkReal(0.390000000000000))*(cj5));
16145 IkReal x3623=((cj6)*(r00));
16146 IkReal x3624=((IkReal(0.000500000000000000))*(cj6));
16147 IkReal x3625=((r11)*(sj6));
16148 IkReal x3626=((cj5)*(sj4));
16149 IkReal x3627=((r01)*(sj6));
16150 IkReal x3628=((r20)*(sj6));
16151 IkReal x3629=((r10)*(sj6));
16152 IkReal x3630=((cj6)*(r10));
16153 IkReal x3631=((cj6)*(r21));
16154 IkReal x3632=((r00)*(sj6));
16155 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
16156 evalcond[1]=((((cj5)*(r22)))+(((sj5)*(x3618)))+(((IkReal(-1.00000000000000))*(sj5)*(x3621))));
16157 evalcond[2]=((IkReal(-0.00200000000000000))+(((IkReal(0.000500000000000000))*(x3628)))+(((r21)*(x3624)))+(((x3618)*(x3620)))+(((IkReal(-1.00000000000000))*(pz)))+(((r22)*(x3622)))+(((IkReal(-1.00000000000000))*(x3620)*(x3621))));
16158 evalcond[3]=((((r01)*(x3624)))+(((r02)*(x3622)))+(((x3620)*(x3623)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(0.000500000000000000))*(x3632)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x3620)*(x3627))));
16159 evalcond[4]=((((x3620)*(x3630)))+(((IkReal(0.000500000000000000))*(x3629)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x3624)))+(((r12)*(x3622)))+(((IkReal(-1.00000000000000))*(x3620)*(x3625))));
16160 evalcond[5]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x3618)*(x3626)))+(((IkReal(-1.00000000000000))*(x3619)*(x3631)))+(((r22)*(x3617)))+(((x3621)*(x3626)))+(((IkReal(-1.00000000000000))*(x3619)*(x3628))));
16161 evalcond[6]=((((sj4)*(x3628)))+(((cj4)*(cj5)*(x3621)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3618)*(x3619)))+(((sj4)*(x3631))));
16162 evalcond[7]=((((x3626)*(x3627)))+(((IkReal(-1.00000000000000))*(x3619)*(x3632)))+(((r02)*(x3617)))+(((IkReal(-1.00000000000000))*(x3623)*(x3626)))+(((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3619))));
16163 evalcond[8]=((((IkReal(-1.00000000000000))*(x3626)*(x3630)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3619)))+(((x3625)*(x3626)))+(((IkReal(-1.00000000000000))*(x3619)*(x3629)))+(((r12)*(x3617))));
16164 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
16165 {
16166 {
16167 IkReal dummyeval[1];
16168 IkReal gconst5;
16169 gconst5=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
16170 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
16171 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
16172 {
16173 {
16174 IkReal dummyeval[1];
16175 IkReal gconst6;
16176 gconst6=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
16177 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
16178 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
16179 {
16180 continue;
16181 
16182 } else
16183 {
16184 {
16185 IkReal j3array[1], cj3array[1], sj3array[1];
16186 bool j3valid[1]={false};
16187 _nj3 = 1;
16188 IkReal x3633=((IkReal(1.00000000000000))*(sj4));
16189 IkReal x3634=((r00)*(sj6));
16190 IkReal x3635=((cj1)*(cj6));
16191 IkReal x3636=((cj4)*(sj5));
16192 IkReal x3637=((cj4)*(cj5));
16193 IkReal x3638=((r01)*(sj1));
16194 IkReal x3639=((IkReal(1.00000000000000))*(sj6));
16195 IkReal x3640=((cj1)*(r02));
16196 IkReal x3641=((cj1)*(r01));
16197 IkReal x3642=((IkReal(1.00000000000000))*(r02)*(sj1));
16198 IkReal x3643=((cj6)*(r00)*(sj1));
16199 if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(cj6)*(x3633)*(x3638)))+(((IkReal(-1.00000000000000))*(x3637)*(x3638)*(x3639)))+(((IkReal(-1.00000000000000))*(x3636)*(x3642)))+(((x3637)*(x3643)))+(((cj5)*(x3640)))+(((r00)*(sj5)*(x3635)))+(((IkReal(-1.00000000000000))*(sj5)*(x3639)*(x3641)))+(((IkReal(-1.00000000000000))*(sj1)*(x3633)*(x3634))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((sj5)*(sj6)*(x3638)))+(((IkReal(-1.00000000000000))*(r01)*(x3633)*(x3635)))+(((IkReal(-1.00000000000000))*(x3636)*(x3640)))+(((IkReal(-1.00000000000000))*(sj5)*(x3643)))+(((r00)*(x3635)*(x3637)))+(((IkReal(-1.00000000000000))*(x3637)*(x3639)*(x3641)))+(((IkReal(-1.00000000000000))*(cj5)*(x3642)))+(((IkReal(-1.00000000000000))*(cj1)*(x3633)*(x3634))))))) < IKFAST_ATAN2_MAGTHRESH )
16200     continue;
16201 j3array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(cj6)*(x3633)*(x3638)))+(((IkReal(-1.00000000000000))*(x3637)*(x3638)*(x3639)))+(((IkReal(-1.00000000000000))*(x3636)*(x3642)))+(((x3637)*(x3643)))+(((cj5)*(x3640)))+(((r00)*(sj5)*(x3635)))+(((IkReal(-1.00000000000000))*(sj5)*(x3639)*(x3641)))+(((IkReal(-1.00000000000000))*(sj1)*(x3633)*(x3634)))))), ((gconst6)*(((((sj5)*(sj6)*(x3638)))+(((IkReal(-1.00000000000000))*(r01)*(x3633)*(x3635)))+(((IkReal(-1.00000000000000))*(x3636)*(x3640)))+(((IkReal(-1.00000000000000))*(sj5)*(x3643)))+(((r00)*(x3635)*(x3637)))+(((IkReal(-1.00000000000000))*(x3637)*(x3639)*(x3641)))+(((IkReal(-1.00000000000000))*(cj5)*(x3642)))+(((IkReal(-1.00000000000000))*(cj1)*(x3633)*(x3634)))))));
16202 sj3array[0]=IKsin(j3array[0]);
16203 cj3array[0]=IKcos(j3array[0]);
16204 if( j3array[0] > IKPI )
16205 {
16206     j3array[0]-=IK2PI;
16207 }
16208 else if( j3array[0] < -IKPI )
16209 {    j3array[0]+=IK2PI;
16210 }
16211 j3valid[0] = true;
16212 for(int ij3 = 0; ij3 < 1; ++ij3)
16213 {
16214 if( !j3valid[ij3] )
16215 {
16216     continue;
16217 }
16218 _ij3[0] = ij3; _ij3[1] = -1;
16219 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16220 {
16221 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16222 {
16223     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16224 }
16225 }
16226 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16227 {
16228 IkReal evalcond[4];
16229 IkReal x3644=IKcos(j3);
16230 IkReal x3645=IKsin(j3);
16231 IkReal x3646=((r11)*(sj6));
16232 IkReal x3647=((IkReal(1.00000000000000))*(sj5));
16233 IkReal x3648=((sj4)*(sj6));
16234 IkReal x3649=((cj4)*(sj5));
16235 IkReal x3650=((cj6)*(sj4));
16236 IkReal x3651=((cj4)*(cj5));
16237 IkReal x3652=((r01)*(sj6));
16238 IkReal x3653=((cj6)*(sj5));
16239 IkReal x3654=((sj1)*(x3644));
16240 IkReal x3655=((cj1)*(x3644));
16241 IkReal x3656=((IkReal(1.00000000000000))*(x3645));
16242 IkReal x3657=((cj1)*(x3656));
16243 IkReal x3658=((IkReal(1.00000000000000))*(cj6)*(x3651));
16244 evalcond[0]=((((r00)*(x3653)))+(x3654)+(((IkReal(-1.00000000000000))*(x3647)*(x3652)))+(((cj5)*(r02)))+(((IkReal(-1.00000000000000))*(x3657))));
16245 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3646)*(x3647)))+(((IkReal(-1.00000000000000))*(x3655)))+(((r10)*(x3653)))+(((IkReal(-1.00000000000000))*(sj1)*(x3656))));
16246 evalcond[2]=((((r00)*(x3648)))+(((x3651)*(x3652)))+(x3655)+(((r02)*(x3649)))+(((IkReal(-1.00000000000000))*(r00)*(x3658)))+(((sj1)*(x3645)))+(((r01)*(x3650))));
16247 evalcond[3]=((((x3646)*(x3651)))+(((r12)*(x3649)))+(((r11)*(x3650)))+(((IkReal(-1.00000000000000))*(r10)*(x3658)))+(x3654)+(((r10)*(x3648)))+(((IkReal(-1.00000000000000))*(x3657))));
16248 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
16249 {
16250 continue;
16251 }
16252 }
16253 
16254 {
16255 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16256 vinfos[0].jointtype = 1;
16257 vinfos[0].foffset = j0;
16258 vinfos[0].indices[0] = _ij0[0];
16259 vinfos[0].indices[1] = _ij0[1];
16260 vinfos[0].maxsolutions = _nj0;
16261 vinfos[1].jointtype = 1;
16262 vinfos[1].foffset = j1;
16263 vinfos[1].indices[0] = _ij1[0];
16264 vinfos[1].indices[1] = _ij1[1];
16265 vinfos[1].maxsolutions = _nj1;
16266 vinfos[2].jointtype = 1;
16267 vinfos[2].foffset = j2;
16268 vinfos[2].indices[0] = _ij2[0];
16269 vinfos[2].indices[1] = _ij2[1];
16270 vinfos[2].maxsolutions = _nj2;
16271 vinfos[3].jointtype = 1;
16272 vinfos[3].foffset = j3;
16273 vinfos[3].indices[0] = _ij3[0];
16274 vinfos[3].indices[1] = _ij3[1];
16275 vinfos[3].maxsolutions = _nj3;
16276 vinfos[4].jointtype = 1;
16277 vinfos[4].foffset = j4;
16278 vinfos[4].indices[0] = _ij4[0];
16279 vinfos[4].indices[1] = _ij4[1];
16280 vinfos[4].maxsolutions = _nj4;
16281 vinfos[5].jointtype = 1;
16282 vinfos[5].foffset = j5;
16283 vinfos[5].indices[0] = _ij5[0];
16284 vinfos[5].indices[1] = _ij5[1];
16285 vinfos[5].maxsolutions = _nj5;
16286 vinfos[6].jointtype = 1;
16287 vinfos[6].foffset = j6;
16288 vinfos[6].indices[0] = _ij6[0];
16289 vinfos[6].indices[1] = _ij6[1];
16290 vinfos[6].maxsolutions = _nj6;
16291 std::vector<int> vfree(0);
16292 solutions.AddSolution(vinfos,vfree);
16293 }
16294 }
16295 }
16296 
16297 }
16298 
16299 }
16300 
16301 } else
16302 {
16303 {
16304 IkReal j3array[1], cj3array[1], sj3array[1];
16305 bool j3valid[1]={false};
16306 _nj3 = 1;
16307 IkReal x3659=((cj5)*(r12));
16308 IkReal x3660=((cj6)*(sj5));
16309 IkReal x3661=((IkReal(1.00000000000000))*(sj1));
16310 IkReal x3662=((cj5)*(r02));
16311 IkReal x3663=((IkReal(1.00000000000000))*(cj1));
16312 IkReal x3664=((r01)*(sj5)*(sj6));
16313 IkReal x3665=((r11)*(sj5)*(sj6));
16314 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x3663)*(x3664)))+(((cj1)*(x3662)))+(((IkReal(-1.00000000000000))*(x3661)*(x3665)))+(((r10)*(sj1)*(x3660)))+(((sj1)*(x3659)))+(((cj1)*(r00)*(x3660))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x3663)*(x3665)))+(((cj1)*(r10)*(x3660)))+(((IkReal(-1.00000000000000))*(x3661)*(x3662)))+(((IkReal(-1.00000000000000))*(r00)*(x3660)*(x3661)))+(((sj1)*(x3664)))+(((cj1)*(x3659))))))) < IKFAST_ATAN2_MAGTHRESH )
16315     continue;
16316 j3array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x3663)*(x3664)))+(((cj1)*(x3662)))+(((IkReal(-1.00000000000000))*(x3661)*(x3665)))+(((r10)*(sj1)*(x3660)))+(((sj1)*(x3659)))+(((cj1)*(r00)*(x3660)))))), ((gconst5)*(((((IkReal(-1.00000000000000))*(x3663)*(x3665)))+(((cj1)*(r10)*(x3660)))+(((IkReal(-1.00000000000000))*(x3661)*(x3662)))+(((IkReal(-1.00000000000000))*(r00)*(x3660)*(x3661)))+(((sj1)*(x3664)))+(((cj1)*(x3659)))))));
16317 sj3array[0]=IKsin(j3array[0]);
16318 cj3array[0]=IKcos(j3array[0]);
16319 if( j3array[0] > IKPI )
16320 {
16321     j3array[0]-=IK2PI;
16322 }
16323 else if( j3array[0] < -IKPI )
16324 {    j3array[0]+=IK2PI;
16325 }
16326 j3valid[0] = true;
16327 for(int ij3 = 0; ij3 < 1; ++ij3)
16328 {
16329 if( !j3valid[ij3] )
16330 {
16331     continue;
16332 }
16333 _ij3[0] = ij3; _ij3[1] = -1;
16334 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16335 {
16336 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16337 {
16338     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16339 }
16340 }
16341 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16342 {
16343 IkReal evalcond[4];
16344 IkReal x3666=IKcos(j3);
16345 IkReal x3667=IKsin(j3);
16346 IkReal x3668=((r11)*(sj6));
16347 IkReal x3669=((IkReal(1.00000000000000))*(sj5));
16348 IkReal x3670=((sj4)*(sj6));
16349 IkReal x3671=((cj4)*(sj5));
16350 IkReal x3672=((cj6)*(sj4));
16351 IkReal x3673=((cj4)*(cj5));
16352 IkReal x3674=((r01)*(sj6));
16353 IkReal x3675=((cj6)*(sj5));
16354 IkReal x3676=((sj1)*(x3666));
16355 IkReal x3677=((cj1)*(x3666));
16356 IkReal x3678=((IkReal(1.00000000000000))*(x3667));
16357 IkReal x3679=((cj1)*(x3678));
16358 IkReal x3680=((IkReal(1.00000000000000))*(cj6)*(x3673));
16359 evalcond[0]=((((r00)*(x3675)))+(x3676)+(((IkReal(-1.00000000000000))*(x3669)*(x3674)))+(((IkReal(-1.00000000000000))*(x3679)))+(((cj5)*(r02))));
16360 evalcond[1]=((((IkReal(-1.00000000000000))*(x3668)*(x3669)))+(((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(sj1)*(x3678)))+(((IkReal(-1.00000000000000))*(x3677)))+(((r10)*(x3675))));
16361 evalcond[2]=((((r01)*(x3672)))+(((r00)*(x3670)))+(((sj1)*(x3667)))+(((x3673)*(x3674)))+(x3677)+(((IkReal(-1.00000000000000))*(r00)*(x3680)))+(((r02)*(x3671))));
16362 evalcond[3]=((((r11)*(x3672)))+(((r12)*(x3671)))+(((x3668)*(x3673)))+(x3676)+(((r10)*(x3670)))+(((IkReal(-1.00000000000000))*(x3679)))+(((IkReal(-1.00000000000000))*(r10)*(x3680))));
16363 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
16364 {
16365 continue;
16366 }
16367 }
16368 
16369 {
16370 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16371 vinfos[0].jointtype = 1;
16372 vinfos[0].foffset = j0;
16373 vinfos[0].indices[0] = _ij0[0];
16374 vinfos[0].indices[1] = _ij0[1];
16375 vinfos[0].maxsolutions = _nj0;
16376 vinfos[1].jointtype = 1;
16377 vinfos[1].foffset = j1;
16378 vinfos[1].indices[0] = _ij1[0];
16379 vinfos[1].indices[1] = _ij1[1];
16380 vinfos[1].maxsolutions = _nj1;
16381 vinfos[2].jointtype = 1;
16382 vinfos[2].foffset = j2;
16383 vinfos[2].indices[0] = _ij2[0];
16384 vinfos[2].indices[1] = _ij2[1];
16385 vinfos[2].maxsolutions = _nj2;
16386 vinfos[3].jointtype = 1;
16387 vinfos[3].foffset = j3;
16388 vinfos[3].indices[0] = _ij3[0];
16389 vinfos[3].indices[1] = _ij3[1];
16390 vinfos[3].maxsolutions = _nj3;
16391 vinfos[4].jointtype = 1;
16392 vinfos[4].foffset = j4;
16393 vinfos[4].indices[0] = _ij4[0];
16394 vinfos[4].indices[1] = _ij4[1];
16395 vinfos[4].maxsolutions = _nj4;
16396 vinfos[5].jointtype = 1;
16397 vinfos[5].foffset = j5;
16398 vinfos[5].indices[0] = _ij5[0];
16399 vinfos[5].indices[1] = _ij5[1];
16400 vinfos[5].maxsolutions = _nj5;
16401 vinfos[6].jointtype = 1;
16402 vinfos[6].foffset = j6;
16403 vinfos[6].indices[0] = _ij6[0];
16404 vinfos[6].indices[1] = _ij6[1];
16405 vinfos[6].maxsolutions = _nj6;
16406 std::vector<int> vfree(0);
16407 solutions.AddSolution(vinfos,vfree);
16408 }
16409 }
16410 }
16411 
16412 }
16413 
16414 }
16415 
16416 } else
16417 {
16418 IkReal x3681=((sj4)*(sj5));
16419 IkReal x3682=((cj6)*(r20));
16420 IkReal x3683=((IkReal(1.00000000000000))*(cj4));
16421 IkReal x3684=((IkReal(0.390000000000000))*(sj5));
16422 IkReal x3685=((r21)*(sj6));
16423 IkReal x3686=((IkReal(0.390000000000000))*(cj5));
16424 IkReal x3687=((cj6)*(r00));
16425 IkReal x3688=((IkReal(0.000500000000000000))*(cj6));
16426 IkReal x3689=((r11)*(sj6));
16427 IkReal x3690=((cj5)*(sj4));
16428 IkReal x3691=((r01)*(sj6));
16429 IkReal x3692=((r20)*(sj6));
16430 IkReal x3693=((r10)*(sj6));
16431 IkReal x3694=((cj6)*(r10));
16432 IkReal x3695=((cj6)*(r21));
16433 IkReal x3696=((r00)*(sj6));
16434 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
16435 evalcond[1]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x3685)))+(((sj5)*(x3682))));
16436 evalcond[2]=((IkReal(0.00200000000000000))+(((r21)*(x3688)))+(((IkReal(0.000500000000000000))*(x3692)))+(((IkReal(-1.00000000000000))*(pz)))+(((x3682)*(x3684)))+(((IkReal(-1.00000000000000))*(x3684)*(x3685)))+(((r22)*(x3686))));
16437 evalcond[3]=((((r01)*(x3688)))+(((IkReal(0.000500000000000000))*(x3696)))+(((x3684)*(x3687)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x3686)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x3684)*(x3691))));
16438 evalcond[4]=((((IkReal(0.400000000000000))*(cj1)))+(((IkReal(0.000500000000000000))*(x3693)))+(((IkReal(-1.00000000000000))*(py)))+(((x3684)*(x3694)))+(((r12)*(x3686)))+(((IkReal(-1.00000000000000))*(x3684)*(x3689)))+(((r11)*(x3688))));
16439 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x3682)*(x3690)))+(((x3685)*(x3690)))+(((IkReal(-1.00000000000000))*(x3683)*(x3692)))+(((IkReal(-1.00000000000000))*(x3683)*(x3695)))+(((r22)*(x3681))));
16440 evalcond[6]=((((cj4)*(cj5)*(x3685)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3682)*(x3683)))+(((sj4)*(x3692)))+(((sj4)*(x3695))));
16441 evalcond[7]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3683)))+(((IkReal(-1.00000000000000))*(x3687)*(x3690)))+(((x3690)*(x3691)))+(((r02)*(x3681)))+(((IkReal(-1.00000000000000))*(x3683)*(x3696))));
16442 evalcond[8]=((((x3689)*(x3690)))+(((r12)*(x3681)))+(((IkReal(-1.00000000000000))*(x3683)*(x3693)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3683)))+(((IkReal(-1.00000000000000))*(x3690)*(x3694))));
16443 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
16444 {
16445 {
16446 IkReal dummyeval[1];
16447 IkReal gconst7;
16448 gconst7=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
16449 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
16450 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
16451 {
16452 {
16453 IkReal dummyeval[1];
16454 IkReal gconst8;
16455 gconst8=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
16456 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
16457 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
16458 {
16459 continue;
16460 
16461 } else
16462 {
16463 {
16464 IkReal j3array[1], cj3array[1], sj3array[1];
16465 bool j3valid[1]={false};
16466 _nj3 = 1;
16467 IkReal x3697=((sj1)*(sj6));
16468 IkReal x3698=((r00)*(sj4));
16469 IkReal x3699=((IkReal(1.00000000000000))*(r01));
16470 IkReal x3700=((cj1)*(cj6));
16471 IkReal x3701=((cj1)*(sj6));
16472 IkReal x3702=((cj4)*(cj5));
16473 IkReal x3703=((r02)*(sj1));
16474 IkReal x3704=((cj4)*(sj5));
16475 IkReal x3705=((cj1)*(r02));
16476 IkReal x3706=((cj6)*(r00)*(sj1));
16477 if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(x3702)*(x3706)))+(((x3697)*(x3698)))+(((cj5)*(x3705)))+(((r01)*(x3697)*(x3702)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((IkReal(-1.00000000000000))*(sj5)*(x3699)*(x3701)))+(((r00)*(sj5)*(x3700)))+(((x3703)*(x3704))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(x3698)*(x3701)))+(((IkReal(-1.00000000000000))*(sj4)*(x3699)*(x3700)))+(((cj5)*(x3703)))+(((r00)*(x3700)*(x3702)))+(((IkReal(-1.00000000000000))*(sj5)*(x3697)*(x3699)))+(((IkReal(-1.00000000000000))*(x3704)*(x3705)))+(((sj5)*(x3706)))+(((IkReal(-1.00000000000000))*(x3699)*(x3701)*(x3702))))))) < IKFAST_ATAN2_MAGTHRESH )
16478     continue;
16479 j3array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(x3702)*(x3706)))+(((x3697)*(x3698)))+(((cj5)*(x3705)))+(((r01)*(x3697)*(x3702)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((IkReal(-1.00000000000000))*(sj5)*(x3699)*(x3701)))+(((r00)*(sj5)*(x3700)))+(((x3703)*(x3704)))))), ((gconst8)*(((((IkReal(-1.00000000000000))*(x3698)*(x3701)))+(((IkReal(-1.00000000000000))*(sj4)*(x3699)*(x3700)))+(((cj5)*(x3703)))+(((r00)*(x3700)*(x3702)))+(((IkReal(-1.00000000000000))*(sj5)*(x3697)*(x3699)))+(((IkReal(-1.00000000000000))*(x3704)*(x3705)))+(((sj5)*(x3706)))+(((IkReal(-1.00000000000000))*(x3699)*(x3701)*(x3702)))))));
16480 sj3array[0]=IKsin(j3array[0]);
16481 cj3array[0]=IKcos(j3array[0]);
16482 if( j3array[0] > IKPI )
16483 {
16484     j3array[0]-=IK2PI;
16485 }
16486 else if( j3array[0] < -IKPI )
16487 {    j3array[0]+=IK2PI;
16488 }
16489 j3valid[0] = true;
16490 for(int ij3 = 0; ij3 < 1; ++ij3)
16491 {
16492 if( !j3valid[ij3] )
16493 {
16494     continue;
16495 }
16496 _ij3[0] = ij3; _ij3[1] = -1;
16497 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16498 {
16499 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16500 {
16501     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16502 }
16503 }
16504 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16505 {
16506 IkReal evalcond[4];
16507 IkReal x3707=IKsin(j3);
16508 IkReal x3708=IKcos(j3);
16509 IkReal x3709=((r11)*(sj6));
16510 IkReal x3710=((IkReal(1.00000000000000))*(sj5));
16511 IkReal x3711=((sj4)*(sj6));
16512 IkReal x3712=((cj4)*(sj5));
16513 IkReal x3713=((cj6)*(sj4));
16514 IkReal x3714=((cj4)*(cj5));
16515 IkReal x3715=((r01)*(sj6));
16516 IkReal x3716=((cj6)*(sj5));
16517 IkReal x3717=((sj1)*(x3707));
16518 IkReal x3718=((cj1)*(x3707));
16519 IkReal x3719=((IkReal(1.00000000000000))*(x3708));
16520 IkReal x3720=((cj1)*(x3719));
16521 IkReal x3721=((IkReal(1.00000000000000))*(cj6)*(x3714));
16522 evalcond[0]=((((sj1)*(x3708)))+(((IkReal(-1.00000000000000))*(x3710)*(x3715)))+(((r00)*(x3716)))+(x3718)+(((cj5)*(r02))));
16523 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3720)))+(x3717)+(((IkReal(-1.00000000000000))*(x3709)*(x3710)))+(((r10)*(x3716))));
16524 evalcond[2]=((((IkReal(-1.00000000000000))*(x3720)))+(((IkReal(-1.00000000000000))*(r00)*(x3721)))+(((r00)*(x3711)))+(((x3714)*(x3715)))+(((r01)*(x3713)))+(x3717)+(((r02)*(x3712))));
16525 evalcond[3]=((((r11)*(x3713)))+(((r12)*(x3712)))+(((x3709)*(x3714)))+(((IkReal(-1.00000000000000))*(sj1)*(x3719)))+(((IkReal(-1.00000000000000))*(r10)*(x3721)))+(((r10)*(x3711)))+(((IkReal(-1.00000000000000))*(x3718))));
16526 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
16527 {
16528 continue;
16529 }
16530 }
16531 
16532 {
16533 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16534 vinfos[0].jointtype = 1;
16535 vinfos[0].foffset = j0;
16536 vinfos[0].indices[0] = _ij0[0];
16537 vinfos[0].indices[1] = _ij0[1];
16538 vinfos[0].maxsolutions = _nj0;
16539 vinfos[1].jointtype = 1;
16540 vinfos[1].foffset = j1;
16541 vinfos[1].indices[0] = _ij1[0];
16542 vinfos[1].indices[1] = _ij1[1];
16543 vinfos[1].maxsolutions = _nj1;
16544 vinfos[2].jointtype = 1;
16545 vinfos[2].foffset = j2;
16546 vinfos[2].indices[0] = _ij2[0];
16547 vinfos[2].indices[1] = _ij2[1];
16548 vinfos[2].maxsolutions = _nj2;
16549 vinfos[3].jointtype = 1;
16550 vinfos[3].foffset = j3;
16551 vinfos[3].indices[0] = _ij3[0];
16552 vinfos[3].indices[1] = _ij3[1];
16553 vinfos[3].maxsolutions = _nj3;
16554 vinfos[4].jointtype = 1;
16555 vinfos[4].foffset = j4;
16556 vinfos[4].indices[0] = _ij4[0];
16557 vinfos[4].indices[1] = _ij4[1];
16558 vinfos[4].maxsolutions = _nj4;
16559 vinfos[5].jointtype = 1;
16560 vinfos[5].foffset = j5;
16561 vinfos[5].indices[0] = _ij5[0];
16562 vinfos[5].indices[1] = _ij5[1];
16563 vinfos[5].maxsolutions = _nj5;
16564 vinfos[6].jointtype = 1;
16565 vinfos[6].foffset = j6;
16566 vinfos[6].indices[0] = _ij6[0];
16567 vinfos[6].indices[1] = _ij6[1];
16568 vinfos[6].maxsolutions = _nj6;
16569 std::vector<int> vfree(0);
16570 solutions.AddSolution(vinfos,vfree);
16571 }
16572 }
16573 }
16574 
16575 }
16576 
16577 }
16578 
16579 } else
16580 {
16581 {
16582 IkReal j3array[1], cj3array[1], sj3array[1];
16583 bool j3valid[1]={false};
16584 _nj3 = 1;
16585 IkReal x3722=((sj1)*(sj5));
16586 IkReal x3723=((cj6)*(r10));
16587 IkReal x3724=((cj5)*(sj1));
16588 IkReal x3725=((cj1)*(cj5));
16589 IkReal x3726=((cj1)*(sj5));
16590 IkReal x3727=((cj6)*(r00));
16591 IkReal x3728=((r11)*(sj6));
16592 IkReal x3729=((IkReal(1.00000000000000))*(r01)*(sj6));
16593 if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x3726)*(x3729)))+(((x3726)*(x3727)))+(((r02)*(x3725)))+(((r12)*(x3724)))+(((x3722)*(x3723)))+(((IkReal(-1.00000000000000))*(x3722)*(x3728))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((x3726)*(x3728)))+(((r02)*(x3724)))+(((x3722)*(x3727)))+(((IkReal(-1.00000000000000))*(x3722)*(x3729)))+(((IkReal(-1.00000000000000))*(x3723)*(x3726)))+(((IkReal(-1.00000000000000))*(r12)*(x3725))))))) < IKFAST_ATAN2_MAGTHRESH )
16594     continue;
16595 j3array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x3726)*(x3729)))+(((x3726)*(x3727)))+(((r02)*(x3725)))+(((r12)*(x3724)))+(((x3722)*(x3723)))+(((IkReal(-1.00000000000000))*(x3722)*(x3728)))))), ((gconst7)*(((((x3726)*(x3728)))+(((r02)*(x3724)))+(((x3722)*(x3727)))+(((IkReal(-1.00000000000000))*(x3722)*(x3729)))+(((IkReal(-1.00000000000000))*(x3723)*(x3726)))+(((IkReal(-1.00000000000000))*(r12)*(x3725)))))));
16596 sj3array[0]=IKsin(j3array[0]);
16597 cj3array[0]=IKcos(j3array[0]);
16598 if( j3array[0] > IKPI )
16599 {
16600     j3array[0]-=IK2PI;
16601 }
16602 else if( j3array[0] < -IKPI )
16603 {    j3array[0]+=IK2PI;
16604 }
16605 j3valid[0] = true;
16606 for(int ij3 = 0; ij3 < 1; ++ij3)
16607 {
16608 if( !j3valid[ij3] )
16609 {
16610     continue;
16611 }
16612 _ij3[0] = ij3; _ij3[1] = -1;
16613 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16614 {
16615 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16616 {
16617     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16618 }
16619 }
16620 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16621 {
16622 IkReal evalcond[4];
16623 IkReal x3730=IKsin(j3);
16624 IkReal x3731=IKcos(j3);
16625 IkReal x3732=((r11)*(sj6));
16626 IkReal x3733=((IkReal(1.00000000000000))*(sj5));
16627 IkReal x3734=((sj4)*(sj6));
16628 IkReal x3735=((cj4)*(sj5));
16629 IkReal x3736=((cj6)*(sj4));
16630 IkReal x3737=((cj4)*(cj5));
16631 IkReal x3738=((r01)*(sj6));
16632 IkReal x3739=((cj6)*(sj5));
16633 IkReal x3740=((sj1)*(x3730));
16634 IkReal x3741=((cj1)*(x3730));
16635 IkReal x3742=((IkReal(1.00000000000000))*(x3731));
16636 IkReal x3743=((cj1)*(x3742));
16637 IkReal x3744=((IkReal(1.00000000000000))*(cj6)*(x3737));
16638 evalcond[0]=((((r00)*(x3739)))+(((sj1)*(x3731)))+(x3741)+(((IkReal(-1.00000000000000))*(x3733)*(x3738)))+(((cj5)*(r02))));
16639 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3743)))+(x3740)+(((IkReal(-1.00000000000000))*(x3732)*(x3733)))+(((r10)*(x3739))));
16640 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x3744)))+(((r01)*(x3736)))+(((r00)*(x3734)))+(((IkReal(-1.00000000000000))*(x3743)))+(((r02)*(x3735)))+(x3740)+(((x3737)*(x3738))));
16641 evalcond[3]=((((x3732)*(x3737)))+(((r11)*(x3736)))+(((r12)*(x3735)))+(((IkReal(-1.00000000000000))*(x3741)))+(((IkReal(-1.00000000000000))*(r10)*(x3744)))+(((IkReal(-1.00000000000000))*(sj1)*(x3742)))+(((r10)*(x3734))));
16642 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
16643 {
16644 continue;
16645 }
16646 }
16647 
16648 {
16649 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16650 vinfos[0].jointtype = 1;
16651 vinfos[0].foffset = j0;
16652 vinfos[0].indices[0] = _ij0[0];
16653 vinfos[0].indices[1] = _ij0[1];
16654 vinfos[0].maxsolutions = _nj0;
16655 vinfos[1].jointtype = 1;
16656 vinfos[1].foffset = j1;
16657 vinfos[1].indices[0] = _ij1[0];
16658 vinfos[1].indices[1] = _ij1[1];
16659 vinfos[1].maxsolutions = _nj1;
16660 vinfos[2].jointtype = 1;
16661 vinfos[2].foffset = j2;
16662 vinfos[2].indices[0] = _ij2[0];
16663 vinfos[2].indices[1] = _ij2[1];
16664 vinfos[2].maxsolutions = _nj2;
16665 vinfos[3].jointtype = 1;
16666 vinfos[3].foffset = j3;
16667 vinfos[3].indices[0] = _ij3[0];
16668 vinfos[3].indices[1] = _ij3[1];
16669 vinfos[3].maxsolutions = _nj3;
16670 vinfos[4].jointtype = 1;
16671 vinfos[4].foffset = j4;
16672 vinfos[4].indices[0] = _ij4[0];
16673 vinfos[4].indices[1] = _ij4[1];
16674 vinfos[4].maxsolutions = _nj4;
16675 vinfos[5].jointtype = 1;
16676 vinfos[5].foffset = j5;
16677 vinfos[5].indices[0] = _ij5[0];
16678 vinfos[5].indices[1] = _ij5[1];
16679 vinfos[5].maxsolutions = _nj5;
16680 vinfos[6].jointtype = 1;
16681 vinfos[6].foffset = j6;
16682 vinfos[6].indices[0] = _ij6[0];
16683 vinfos[6].indices[1] = _ij6[1];
16684 vinfos[6].maxsolutions = _nj6;
16685 std::vector<int> vfree(0);
16686 solutions.AddSolution(vinfos,vfree);
16687 }
16688 }
16689 }
16690 
16691 }
16692 
16693 }
16694 
16695 } else
16696 {
16697 IkReal x3745=((sj4)*(sj5));
16698 IkReal x3746=((IkReal(0.390000000000000))*(sj5));
16699 IkReal x3747=((r21)*(sj6));
16700 IkReal x3748=((IkReal(0.390000000000000))*(cj5));
16701 IkReal x3749=((cj6)*(r00));
16702 IkReal x3750=((IkReal(0.000500000000000000))*(cj6));
16703 IkReal x3751=((r11)*(sj6));
16704 IkReal x3752=((cj5)*(sj4));
16705 IkReal x3753=((IkReal(1.00000000000000))*(r20));
16706 IkReal x3754=((cj4)*(sj6));
16707 IkReal x3755=((r01)*(sj6));
16708 IkReal x3756=((IkReal(0.000500000000000000))*(sj6));
16709 IkReal x3757=((IkReal(1.00000000000000))*(r10));
16710 IkReal x3758=((IkReal(1.00000000000000))*(cj4)*(cj6));
16711 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
16712 evalcond[1]=((((r20)*(x3756)))+(((cj6)*(r20)*(x3746)))+(((IkReal(-1.00000000000000))*(x3746)*(x3747)))+(((r22)*(x3748)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x3750)))+(((IkReal(-0.00200000000000000))*(cj2))));
16713 evalcond[2]=((IkReal(-0.400000000000000))+(((r00)*(x3756)))+(((r01)*(x3750)))+(((IkReal(-1.00000000000000))*(x3746)*(x3755)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x3748)))+(((x3746)*(x3749))));
16714 evalcond[3]=((((r12)*(x3748)))+(((IkReal(-1.00000000000000))*(x3746)*(x3751)))+(((r11)*(x3750)))+(((r10)*(x3756)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-0.00200000000000000))*(sj2)))+(((cj6)*(r10)*(x3746))));
16715 evalcond[4]=((((IkReal(-1.00000000000000))*(r21)*(x3758)))+(((IkReal(-1.00000000000000))*(x3753)*(x3754)))+(((IkReal(-1.00000000000000))*(cj6)*(x3752)*(x3753)))+(((r22)*(x3745)))+(((x3747)*(x3752)))+(((IkReal(-1.00000000000000))*(cj2))));
16716 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x3758)))+(((x3752)*(x3755)))+(((r02)*(x3745)))+(((IkReal(-1.00000000000000))*(x3749)*(x3752)))+(((IkReal(-1.00000000000000))*(r00)*(x3754))));
16717 evalcond[6]=((((IkReal(-1.00000000000000))*(sj2)))+(((x3751)*(x3752)))+(((r12)*(x3745)))+(((IkReal(-1.00000000000000))*(r11)*(x3758)))+(((IkReal(-1.00000000000000))*(cj6)*(x3752)*(x3757)))+(((IkReal(-1.00000000000000))*(x3754)*(x3757))));
16718 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
16719 {
16720 {
16721 IkReal j3array[1], cj3array[1], sj3array[1];
16722 bool j3valid[1]={false};
16723 _nj3 = 1;
16724 IkReal x3759=((IkReal(1.00000000000000))*(cj6));
16725 IkReal x3760=((IkReal(1.00000000000000))*(cj5));
16726 IkReal x3761=((r01)*(sj6));
16727 if( IKabs(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x3760)*(x3761)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x3759)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj5)*(x3761)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x3759)))+(((IkReal(-1.00000000000000))*(r02)*(x3760))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x3760)*(x3761)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x3759)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))))+IKsqr(((((sj5)*(x3761)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x3759)))+(((IkReal(-1.00000000000000))*(r02)*(x3760)))))-1) <= IKFAST_SINCOS_THRESH )
16728     continue;
16729 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(x3760)*(x3761)))+(((IkReal(-1.00000000000000))*(r01)*(sj4)*(x3759)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))), ((((sj5)*(x3761)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x3759)))+(((IkReal(-1.00000000000000))*(r02)*(x3760)))));
16730 sj3array[0]=IKsin(j3array[0]);
16731 cj3array[0]=IKcos(j3array[0]);
16732 if( j3array[0] > IKPI )
16733 {
16734     j3array[0]-=IK2PI;
16735 }
16736 else if( j3array[0] < -IKPI )
16737 {    j3array[0]+=IK2PI;
16738 }
16739 j3valid[0] = true;
16740 for(int ij3 = 0; ij3 < 1; ++ij3)
16741 {
16742 if( !j3valid[ij3] )
16743 {
16744     continue;
16745 }
16746 _ij3[0] = ij3; _ij3[1] = -1;
16747 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16748 {
16749 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16750 {
16751     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16752 }
16753 }
16754 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16755 {
16756 IkReal evalcond[6];
16757 IkReal x3762=IKsin(j3);
16758 IkReal x3763=IKcos(j3);
16759 IkReal x3764=((r11)*(sj6));
16760 IkReal x3765=((IkReal(1.00000000000000))*(sj5));
16761 IkReal x3766=((sj4)*(sj6));
16762 IkReal x3767=((cj4)*(sj5));
16763 IkReal x3768=((cj6)*(sj4));
16764 IkReal x3769=((cj4)*(cj5));
16765 IkReal x3770=((r01)*(sj6));
16766 IkReal x3771=((r21)*(sj6));
16767 IkReal x3772=((cj6)*(sj5));
16768 IkReal x3773=((IkReal(1.00000000000000))*(cj6)*(x3769));
16769 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x3762)))+(((IkReal(-1.00000000000000))*(x3765)*(x3771)))+(((r20)*(x3772))));
16770 evalcond[1]=((((IkReal(-1.00000000000000))*(x3765)*(x3770)))+(x3763)+(((r00)*(x3772)))+(((cj5)*(r02))));
16771 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj2)*(x3762)))+(((IkReal(-1.00000000000000))*(x3764)*(x3765)))+(((r10)*(x3772))));
16772 evalcond[3]=((((r22)*(x3767)))+(((IkReal(-1.00000000000000))*(r20)*(x3773)))+(((r21)*(x3768)))+(((x3769)*(x3771)))+(((IkReal(-1.00000000000000))*(sj2)*(x3763)))+(((r20)*(x3766))));
16773 evalcond[4]=((((r02)*(x3767)))+(((x3769)*(x3770)))+(x3762)+(((IkReal(-1.00000000000000))*(r00)*(x3773)))+(((r00)*(x3766)))+(((r01)*(x3768))));
16774 evalcond[5]=((((r10)*(x3766)))+(((cj2)*(x3763)))+(((r11)*(x3768)))+(((r12)*(x3767)))+(((x3764)*(x3769)))+(((IkReal(-1.00000000000000))*(r10)*(x3773))));
16775 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
16776 {
16777 continue;
16778 }
16779 }
16780 
16781 {
16782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16783 vinfos[0].jointtype = 1;
16784 vinfos[0].foffset = j0;
16785 vinfos[0].indices[0] = _ij0[0];
16786 vinfos[0].indices[1] = _ij0[1];
16787 vinfos[0].maxsolutions = _nj0;
16788 vinfos[1].jointtype = 1;
16789 vinfos[1].foffset = j1;
16790 vinfos[1].indices[0] = _ij1[0];
16791 vinfos[1].indices[1] = _ij1[1];
16792 vinfos[1].maxsolutions = _nj1;
16793 vinfos[2].jointtype = 1;
16794 vinfos[2].foffset = j2;
16795 vinfos[2].indices[0] = _ij2[0];
16796 vinfos[2].indices[1] = _ij2[1];
16797 vinfos[2].maxsolutions = _nj2;
16798 vinfos[3].jointtype = 1;
16799 vinfos[3].foffset = j3;
16800 vinfos[3].indices[0] = _ij3[0];
16801 vinfos[3].indices[1] = _ij3[1];
16802 vinfos[3].maxsolutions = _nj3;
16803 vinfos[4].jointtype = 1;
16804 vinfos[4].foffset = j4;
16805 vinfos[4].indices[0] = _ij4[0];
16806 vinfos[4].indices[1] = _ij4[1];
16807 vinfos[4].maxsolutions = _nj4;
16808 vinfos[5].jointtype = 1;
16809 vinfos[5].foffset = j5;
16810 vinfos[5].indices[0] = _ij5[0];
16811 vinfos[5].indices[1] = _ij5[1];
16812 vinfos[5].maxsolutions = _nj5;
16813 vinfos[6].jointtype = 1;
16814 vinfos[6].foffset = j6;
16815 vinfos[6].indices[0] = _ij6[0];
16816 vinfos[6].indices[1] = _ij6[1];
16817 vinfos[6].maxsolutions = _nj6;
16818 std::vector<int> vfree(0);
16819 solutions.AddSolution(vinfos,vfree);
16820 }
16821 }
16822 }
16823 
16824 } else
16825 {
16826 IkReal x3774=((sj4)*(sj5));
16827 IkReal x3775=((IkReal(0.390000000000000))*(sj5));
16828 IkReal x3776=((r21)*(sj6));
16829 IkReal x3777=((IkReal(0.390000000000000))*(cj5));
16830 IkReal x3778=((cj6)*(r00));
16831 IkReal x3779=((IkReal(0.000500000000000000))*(cj6));
16832 IkReal x3780=((r11)*(sj6));
16833 IkReal x3781=((cj5)*(sj4));
16834 IkReal x3782=((IkReal(1.00000000000000))*(r20));
16835 IkReal x3783=((cj4)*(sj6));
16836 IkReal x3784=((r01)*(sj6));
16837 IkReal x3785=((IkReal(0.000500000000000000))*(sj6));
16838 IkReal x3786=((IkReal(1.00000000000000))*(r10));
16839 IkReal x3787=((IkReal(1.00000000000000))*(cj4)*(cj6));
16840 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
16841 evalcond[1]=((((r21)*(x3779)))+(((r22)*(x3777)))+(((cj6)*(r20)*(x3775)))+(((r20)*(x3785)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x3775)*(x3776)))+(((IkReal(-0.00200000000000000))*(cj2))));
16842 evalcond[2]=((IkReal(0.400000000000000))+(((r02)*(x3777)))+(((x3775)*(x3778)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x3785)))+(((IkReal(-1.00000000000000))*(x3775)*(x3784)))+(((r01)*(x3779))));
16843 evalcond[3]=((((cj6)*(r10)*(x3775)))+(((IkReal(0.00200000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(py)))+(((r11)*(x3779)))+(((r12)*(x3777)))+(((IkReal(-1.00000000000000))*(x3775)*(x3780)))+(((r10)*(x3785))));
16844 evalcond[4]=((((r22)*(x3774)))+(((IkReal(-1.00000000000000))*(x3782)*(x3783)))+(((IkReal(-1.00000000000000))*(cj6)*(x3781)*(x3782)))+(((x3776)*(x3781)))+(((IkReal(-1.00000000000000))*(r21)*(x3787)))+(((IkReal(-1.00000000000000))*(cj2))));
16845 evalcond[5]=((((r02)*(x3774)))+(((IkReal(-1.00000000000000))*(r01)*(x3787)))+(((IkReal(-1.00000000000000))*(r00)*(x3783)))+(((IkReal(-1.00000000000000))*(x3778)*(x3781)))+(((x3781)*(x3784))));
16846 evalcond[6]=((sj2)+(((IkReal(-1.00000000000000))*(cj6)*(x3781)*(x3786)))+(((IkReal(-1.00000000000000))*(r11)*(x3787)))+(((IkReal(-1.00000000000000))*(x3783)*(x3786)))+(((r12)*(x3774)))+(((x3780)*(x3781))));
16847 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
16848 {
16849 {
16850 IkReal j3array[1], cj3array[1], sj3array[1];
16851 bool j3valid[1]={false};
16852 _nj3 = 1;
16853 IkReal x3788=((cj4)*(cj5));
16854 IkReal x3789=((r01)*(sj6));
16855 IkReal x3790=((cj6)*(r00));
16856 if( IKabs(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x3788)*(x3789)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x3788)*(x3790))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj5)*(x3790)))+(((IkReal(-1.00000000000000))*(sj5)*(x3789)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x3788)*(x3789)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x3788)*(x3790)))))+IKsqr(((((sj5)*(x3790)))+(((IkReal(-1.00000000000000))*(sj5)*(x3789)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
16857     continue;
16858 j3array[0]=IKatan2(((((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((x3788)*(x3789)))+(((cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(x3788)*(x3790)))), ((((sj5)*(x3790)))+(((IkReal(-1.00000000000000))*(sj5)*(x3789)))+(((cj5)*(r02)))));
16859 sj3array[0]=IKsin(j3array[0]);
16860 cj3array[0]=IKcos(j3array[0]);
16861 if( j3array[0] > IKPI )
16862 {
16863     j3array[0]-=IK2PI;
16864 }
16865 else if( j3array[0] < -IKPI )
16866 {    j3array[0]+=IK2PI;
16867 }
16868 j3valid[0] = true;
16869 for(int ij3 = 0; ij3 < 1; ++ij3)
16870 {
16871 if( !j3valid[ij3] )
16872 {
16873     continue;
16874 }
16875 _ij3[0] = ij3; _ij3[1] = -1;
16876 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16877 {
16878 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16879 {
16880     j3valid[iij3]=false; _ij3[1] = iij3; break; 
16881 }
16882 }
16883 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16884 {
16885 IkReal evalcond[6];
16886 IkReal x3791=IKsin(j3);
16887 IkReal x3792=IKcos(j3);
16888 IkReal x3793=((r11)*(sj6));
16889 IkReal x3794=((IkReal(1.00000000000000))*(sj5));
16890 IkReal x3795=((cj6)*(sj4));
16891 IkReal x3796=((sj4)*(sj6));
16892 IkReal x3797=((cj4)*(sj5));
16893 IkReal x3798=((cj4)*(cj5));
16894 IkReal x3799=((r01)*(sj6));
16895 IkReal x3800=((r21)*(sj6));
16896 IkReal x3801=((cj6)*(sj5));
16897 IkReal x3802=((IkReal(1.00000000000000))*(x3792));
16898 IkReal x3803=((IkReal(1.00000000000000))*(cj6)*(x3798));
16899 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x3791)))+(((IkReal(-1.00000000000000))*(x3794)*(x3800)))+(((r20)*(x3801))));
16900 evalcond[1]=((((r00)*(x3801)))+(((IkReal(-1.00000000000000))*(x3802)))+(((IkReal(-1.00000000000000))*(x3794)*(x3799)))+(((cj5)*(r02))));
16901 evalcond[2]=((((cj5)*(r12)))+(((cj2)*(x3791)))+(((r10)*(x3801)))+(((IkReal(-1.00000000000000))*(x3793)*(x3794))));
16902 evalcond[3]=((((r21)*(x3795)))+(((IkReal(-1.00000000000000))*(sj2)*(x3802)))+(((r22)*(x3797)))+(((x3798)*(x3800)))+(((IkReal(-1.00000000000000))*(r20)*(x3803)))+(((r20)*(x3796))));
16903 evalcond[4]=((((IkReal(-1.00000000000000))*(x3791)))+(((IkReal(-1.00000000000000))*(r00)*(x3803)))+(((x3798)*(x3799)))+(((r02)*(x3797)))+(((r00)*(x3796)))+(((r01)*(x3795))));
16904 evalcond[5]=((((x3793)*(x3798)))+(((IkReal(-1.00000000000000))*(cj2)*(x3802)))+(((r10)*(x3796)))+(((r12)*(x3797)))+(((r11)*(x3795)))+(((IkReal(-1.00000000000000))*(r10)*(x3803))));
16905 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
16906 {
16907 continue;
16908 }
16909 }
16910 
16911 {
16912 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
16913 vinfos[0].jointtype = 1;
16914 vinfos[0].foffset = j0;
16915 vinfos[0].indices[0] = _ij0[0];
16916 vinfos[0].indices[1] = _ij0[1];
16917 vinfos[0].maxsolutions = _nj0;
16918 vinfos[1].jointtype = 1;
16919 vinfos[1].foffset = j1;
16920 vinfos[1].indices[0] = _ij1[0];
16921 vinfos[1].indices[1] = _ij1[1];
16922 vinfos[1].maxsolutions = _nj1;
16923 vinfos[2].jointtype = 1;
16924 vinfos[2].foffset = j2;
16925 vinfos[2].indices[0] = _ij2[0];
16926 vinfos[2].indices[1] = _ij2[1];
16927 vinfos[2].maxsolutions = _nj2;
16928 vinfos[3].jointtype = 1;
16929 vinfos[3].foffset = j3;
16930 vinfos[3].indices[0] = _ij3[0];
16931 vinfos[3].indices[1] = _ij3[1];
16932 vinfos[3].maxsolutions = _nj3;
16933 vinfos[4].jointtype = 1;
16934 vinfos[4].foffset = j4;
16935 vinfos[4].indices[0] = _ij4[0];
16936 vinfos[4].indices[1] = _ij4[1];
16937 vinfos[4].maxsolutions = _nj4;
16938 vinfos[5].jointtype = 1;
16939 vinfos[5].foffset = j5;
16940 vinfos[5].indices[0] = _ij5[0];
16941 vinfos[5].indices[1] = _ij5[1];
16942 vinfos[5].maxsolutions = _nj5;
16943 vinfos[6].jointtype = 1;
16944 vinfos[6].foffset = j6;
16945 vinfos[6].indices[0] = _ij6[0];
16946 vinfos[6].indices[1] = _ij6[1];
16947 vinfos[6].maxsolutions = _nj6;
16948 std::vector<int> vfree(0);
16949 solutions.AddSolution(vinfos,vfree);
16950 }
16951 }
16952 }
16953 
16954 } else
16955 {
16956 IkReal x3804=((sj4)*(sj5));
16957 IkReal x3805=((IkReal(0.390000000000000))*(sj5));
16958 IkReal x3806=((r21)*(sj6));
16959 IkReal x3807=((IkReal(0.390000000000000))*(cj5));
16960 IkReal x3808=((cj6)*(r00));
16961 IkReal x3809=((IkReal(0.000500000000000000))*(cj6));
16962 IkReal x3810=((r11)*(sj6));
16963 IkReal x3811=((cj5)*(sj4));
16964 IkReal x3812=((r01)*(sj6));
16965 IkReal x3813=((IkReal(1.00000000000000))*(r20));
16966 IkReal x3814=((cj4)*(sj6));
16967 IkReal x3815=((IkReal(0.000500000000000000))*(sj6));
16968 IkReal x3816=((IkReal(1.00000000000000))*(r10));
16969 IkReal x3817=((IkReal(1.00000000000000))*(cj4)*(cj6));
16970 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
16971 evalcond[1]=((((IkReal(-1.00000000000000))*(x3805)*(x3806)))+(((r21)*(x3809)))+(((r22)*(x3807)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x3815)))+(((cj6)*(r20)*(x3805))));
16972 evalcond[2]=((((r01)*(x3809)))+(((x3805)*(x3808)))+(((r02)*(x3807)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x3805)*(x3812)))+(((r00)*(x3815))));
16973 evalcond[3]=((((r11)*(x3809)))+(((r12)*(x3807)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r10)*(x3815)))+(((IkReal(-0.00200000000000000))*(sj1)))+(((cj6)*(r10)*(x3805)))+(((IkReal(-1.00000000000000))*(x3805)*(x3810))));
16974 evalcond[4]=((((r22)*(x3804)))+(((x3806)*(x3811)))+(((IkReal(-1.00000000000000))*(x3813)*(x3814)))+(((IkReal(-1.00000000000000))*(cj6)*(x3811)*(x3813)))+(((IkReal(-1.00000000000000))*(r21)*(x3817))));
16975 evalcond[5]=((((x3811)*(x3812)))+(((IkReal(-1.00000000000000))*(r01)*(x3817)))+(((IkReal(-1.00000000000000))*(r00)*(x3814)))+(((r02)*(x3804)))+(((IkReal(-1.00000000000000))*(x3808)*(x3811)))+(((IkReal(-1.00000000000000))*(cj1))));
16976 evalcond[6]=((((x3810)*(x3811)))+(((IkReal(-1.00000000000000))*(sj1)))+(((r12)*(x3804)))+(((IkReal(-1.00000000000000))*(x3814)*(x3816)))+(((IkReal(-1.00000000000000))*(cj6)*(x3811)*(x3816)))+(((IkReal(-1.00000000000000))*(r11)*(x3817))));
16977 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
16978 {
16979 {
16980 IkReal j3array[1], cj3array[1], sj3array[1];
16981 bool j3valid[1]={false};
16982 _nj3 = 1;
16983 IkReal x3818=((cj4)*(cj5));
16984 IkReal x3819=((r21)*(sj6));
16985 IkReal x3820=((IkReal(1.00000000000000))*(cj6)*(r20));
16986 if( IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x3820)))+(((sj5)*(x3819)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(r22)*(sj5)))+(((x3818)*(x3819)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x3818)*(x3820))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x3820)))+(((sj5)*(x3819)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((cj4)*(r22)*(sj5)))+(((x3818)*(x3819)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x3818)*(x3820)))))-1) <= IKFAST_SINCOS_THRESH )
16987     continue;
16988 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(sj5)*(x3820)))+(((sj5)*(x3819)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((cj4)*(r22)*(sj5)))+(((x3818)*(x3819)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))+(((IkReal(-1.00000000000000))*(x3818)*(x3820)))));
16989 sj3array[0]=IKsin(j3array[0]);
16990 cj3array[0]=IKcos(j3array[0]);
16991 if( j3array[0] > IKPI )
16992 {
16993     j3array[0]-=IK2PI;
16994 }
16995 else if( j3array[0] < -IKPI )
16996 {    j3array[0]+=IK2PI;
16997 }
16998 j3valid[0] = true;
16999 for(int ij3 = 0; ij3 < 1; ++ij3)
17000 {
17001 if( !j3valid[ij3] )
17002 {
17003     continue;
17004 }
17005 _ij3[0] = ij3; _ij3[1] = -1;
17006 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17007 {
17008 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17009 {
17010     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17011 }
17012 }
17013 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17014 {
17015 IkReal evalcond[6];
17016 IkReal x3821=IKcos(j3);
17017 IkReal x3822=IKsin(j3);
17018 IkReal x3823=((r11)*(sj6));
17019 IkReal x3824=((IkReal(1.00000000000000))*(sj5));
17020 IkReal x3825=((IkReal(1.00000000000000))*(cj1));
17021 IkReal x3826=((cj6)*(sj4));
17022 IkReal x3827=((sj4)*(sj6));
17023 IkReal x3828=((cj4)*(sj5));
17024 IkReal x3829=((cj4)*(cj5));
17025 IkReal x3830=((r01)*(sj6));
17026 IkReal x3831=((r21)*(sj6));
17027 IkReal x3832=((cj6)*(sj5));
17028 IkReal x3833=((IkReal(1.00000000000000))*(cj6)*(x3829));
17029 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3832)))+(x3822)+(((IkReal(-1.00000000000000))*(x3824)*(x3831))));
17030 evalcond[1]=((((r00)*(x3832)))+(((sj1)*(x3821)))+(((IkReal(-1.00000000000000))*(x3824)*(x3830)))+(((cj5)*(r02))));
17031 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3821)*(x3825)))+(((IkReal(-1.00000000000000))*(x3823)*(x3824)))+(((r10)*(x3832))));
17032 evalcond[3]=((((IkReal(-1.00000000000000))*(r20)*(x3833)))+(((x3829)*(x3831)))+(((IkReal(-1.00000000000000))*(x3821)))+(((r22)*(x3828)))+(((r20)*(x3827)))+(((r21)*(x3826))));
17033 evalcond[4]=((((x3829)*(x3830)))+(((r02)*(x3828)))+(((IkReal(-1.00000000000000))*(r00)*(x3833)))+(((sj1)*(x3822)))+(((r01)*(x3826)))+(((r00)*(x3827))));
17034 evalcond[5]=((((r12)*(x3828)))+(((IkReal(-1.00000000000000))*(r10)*(x3833)))+(((r11)*(x3826)))+(((IkReal(-1.00000000000000))*(x3822)*(x3825)))+(((r10)*(x3827)))+(((x3823)*(x3829))));
17035 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
17036 {
17037 continue;
17038 }
17039 }
17040 
17041 {
17042 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17043 vinfos[0].jointtype = 1;
17044 vinfos[0].foffset = j0;
17045 vinfos[0].indices[0] = _ij0[0];
17046 vinfos[0].indices[1] = _ij0[1];
17047 vinfos[0].maxsolutions = _nj0;
17048 vinfos[1].jointtype = 1;
17049 vinfos[1].foffset = j1;
17050 vinfos[1].indices[0] = _ij1[0];
17051 vinfos[1].indices[1] = _ij1[1];
17052 vinfos[1].maxsolutions = _nj1;
17053 vinfos[2].jointtype = 1;
17054 vinfos[2].foffset = j2;
17055 vinfos[2].indices[0] = _ij2[0];
17056 vinfos[2].indices[1] = _ij2[1];
17057 vinfos[2].maxsolutions = _nj2;
17058 vinfos[3].jointtype = 1;
17059 vinfos[3].foffset = j3;
17060 vinfos[3].indices[0] = _ij3[0];
17061 vinfos[3].indices[1] = _ij3[1];
17062 vinfos[3].maxsolutions = _nj3;
17063 vinfos[4].jointtype = 1;
17064 vinfos[4].foffset = j4;
17065 vinfos[4].indices[0] = _ij4[0];
17066 vinfos[4].indices[1] = _ij4[1];
17067 vinfos[4].maxsolutions = _nj4;
17068 vinfos[5].jointtype = 1;
17069 vinfos[5].foffset = j5;
17070 vinfos[5].indices[0] = _ij5[0];
17071 vinfos[5].indices[1] = _ij5[1];
17072 vinfos[5].maxsolutions = _nj5;
17073 vinfos[6].jointtype = 1;
17074 vinfos[6].foffset = j6;
17075 vinfos[6].indices[0] = _ij6[0];
17076 vinfos[6].indices[1] = _ij6[1];
17077 vinfos[6].maxsolutions = _nj6;
17078 std::vector<int> vfree(0);
17079 solutions.AddSolution(vinfos,vfree);
17080 }
17081 }
17082 }
17083 
17084 } else
17085 {
17086 IkReal x3834=((sj4)*(sj5));
17087 IkReal x3835=((IkReal(0.390000000000000))*(sj5));
17088 IkReal x3836=((r21)*(sj6));
17089 IkReal x3837=((IkReal(0.390000000000000))*(cj5));
17090 IkReal x3838=((cj6)*(r00));
17091 IkReal x3839=((IkReal(0.000500000000000000))*(cj6));
17092 IkReal x3840=((r11)*(sj6));
17093 IkReal x3841=((cj5)*(sj4));
17094 IkReal x3842=((r01)*(sj6));
17095 IkReal x3843=((IkReal(1.00000000000000))*(r20));
17096 IkReal x3844=((cj4)*(sj6));
17097 IkReal x3845=((IkReal(0.000500000000000000))*(sj6));
17098 IkReal x3846=((IkReal(1.00000000000000))*(r10));
17099 IkReal x3847=((IkReal(1.00000000000000))*(cj4)*(cj6));
17100 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
17101 evalcond[1]=((((cj6)*(r20)*(x3835)))+(((r20)*(x3845)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x3839)))+(((IkReal(-1.00000000000000))*(x3835)*(x3836)))+(((r22)*(x3837))));
17102 evalcond[2]=((((x3835)*(x3838)))+(((r02)*(x3837)))+(((r01)*(x3839)))+(((IkReal(0.00200000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x3835)*(x3842)))+(((IkReal(-1.00000000000000))*(px)))+(((r00)*(x3845)))+(((IkReal(-0.400000000000000))*(sj1))));
17103 evalcond[3]=((((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x3835)*(x3840)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x3837)))+(((cj6)*(r10)*(x3835)))+(((r11)*(x3839)))+(((r10)*(x3845))));
17104 evalcond[4]=((((IkReal(-1.00000000000000))*(r21)*(x3847)))+(((IkReal(-1.00000000000000))*(x3843)*(x3844)))+(((x3836)*(x3841)))+(((IkReal(-1.00000000000000))*(cj6)*(x3841)*(x3843)))+(((r22)*(x3834))));
17105 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x3844)))+(((IkReal(-1.00000000000000))*(x3838)*(x3841)))+(cj1)+(((r02)*(x3834)))+(((x3841)*(x3842)))+(((IkReal(-1.00000000000000))*(r01)*(x3847))));
17106 evalcond[6]=((sj1)+(((IkReal(-1.00000000000000))*(x3844)*(x3846)))+(((IkReal(-1.00000000000000))*(r11)*(x3847)))+(((r12)*(x3834)))+(((IkReal(-1.00000000000000))*(cj6)*(x3841)*(x3846)))+(((x3840)*(x3841))));
17107 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
17108 {
17109 {
17110 IkReal j3array[1], cj3array[1], sj3array[1];
17111 bool j3valid[1]={false};
17112 _nj3 = 1;
17113 IkReal x3848=((cj4)*(cj5));
17114 IkReal x3849=((cj6)*(r20));
17115 IkReal x3850=((IkReal(1.00000000000000))*(sj4));
17116 IkReal x3851=((IkReal(1.00000000000000))*(r21)*(sj6));
17117 if( IKabs(((((cj5)*(r22)))+(((sj5)*(x3849)))+(((IkReal(-1.00000000000000))*(sj5)*(x3851))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x3848)*(x3851)))+(((x3848)*(x3849)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3850)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3850))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj5)*(r22)))+(((sj5)*(x3849)))+(((IkReal(-1.00000000000000))*(sj5)*(x3851)))))+IKsqr(((((IkReal(-1.00000000000000))*(x3848)*(x3851)))+(((x3848)*(x3849)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3850)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3850)))))-1) <= IKFAST_SINCOS_THRESH )
17118     continue;
17119 j3array[0]=IKatan2(((((cj5)*(r22)))+(((sj5)*(x3849)))+(((IkReal(-1.00000000000000))*(sj5)*(x3851)))), ((((IkReal(-1.00000000000000))*(x3848)*(x3851)))+(((x3848)*(x3849)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3850)))+(((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3850)))));
17120 sj3array[0]=IKsin(j3array[0]);
17121 cj3array[0]=IKcos(j3array[0]);
17122 if( j3array[0] > IKPI )
17123 {
17124     j3array[0]-=IK2PI;
17125 }
17126 else if( j3array[0] < -IKPI )
17127 {    j3array[0]+=IK2PI;
17128 }
17129 j3valid[0] = true;
17130 for(int ij3 = 0; ij3 < 1; ++ij3)
17131 {
17132 if( !j3valid[ij3] )
17133 {
17134     continue;
17135 }
17136 _ij3[0] = ij3; _ij3[1] = -1;
17137 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17138 {
17139 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17140 {
17141     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17142 }
17143 }
17144 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17145 {
17146 IkReal evalcond[6];
17147 IkReal x3852=IKcos(j3);
17148 IkReal x3853=IKsin(j3);
17149 IkReal x3854=((r11)*(sj6));
17150 IkReal x3855=((IkReal(1.00000000000000))*(sj5));
17151 IkReal x3856=((IkReal(1.00000000000000))*(cj1));
17152 IkReal x3857=((sj4)*(sj6));
17153 IkReal x3858=((cj4)*(sj5));
17154 IkReal x3859=((cj6)*(sj4));
17155 IkReal x3860=((cj4)*(cj5));
17156 IkReal x3861=((r01)*(sj6));
17157 IkReal x3862=((r21)*(sj6));
17158 IkReal x3863=((cj6)*(sj5));
17159 IkReal x3864=((IkReal(1.00000000000000))*(cj6)*(x3860));
17160 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3853)))+(((r20)*(x3863)))+(((IkReal(-1.00000000000000))*(x3855)*(x3862))));
17161 evalcond[1]=((((r00)*(x3863)))+(((IkReal(-1.00000000000000))*(x3855)*(x3861)))+(((sj1)*(x3852)))+(((cj5)*(r02))));
17162 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3854)*(x3855)))+(((r10)*(x3863)))+(((IkReal(-1.00000000000000))*(x3852)*(x3856))));
17163 evalcond[3]=((((x3860)*(x3862)))+(((IkReal(-1.00000000000000))*(r20)*(x3864)))+(((r21)*(x3859)))+(((r20)*(x3857)))+(x3852)+(((r22)*(x3858))));
17164 evalcond[4]=((((r02)*(x3858)))+(((x3860)*(x3861)))+(((r01)*(x3859)))+(((r00)*(x3857)))+(((IkReal(-1.00000000000000))*(r00)*(x3864)))+(((sj1)*(x3853))));
17165 evalcond[5]=((((r11)*(x3859)))+(((r10)*(x3857)))+(((IkReal(-1.00000000000000))*(r10)*(x3864)))+(((IkReal(-1.00000000000000))*(x3853)*(x3856)))+(((r12)*(x3858)))+(((x3854)*(x3860))));
17166 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
17167 {
17168 continue;
17169 }
17170 }
17171 
17172 {
17173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17174 vinfos[0].jointtype = 1;
17175 vinfos[0].foffset = j0;
17176 vinfos[0].indices[0] = _ij0[0];
17177 vinfos[0].indices[1] = _ij0[1];
17178 vinfos[0].maxsolutions = _nj0;
17179 vinfos[1].jointtype = 1;
17180 vinfos[1].foffset = j1;
17181 vinfos[1].indices[0] = _ij1[0];
17182 vinfos[1].indices[1] = _ij1[1];
17183 vinfos[1].maxsolutions = _nj1;
17184 vinfos[2].jointtype = 1;
17185 vinfos[2].foffset = j2;
17186 vinfos[2].indices[0] = _ij2[0];
17187 vinfos[2].indices[1] = _ij2[1];
17188 vinfos[2].maxsolutions = _nj2;
17189 vinfos[3].jointtype = 1;
17190 vinfos[3].foffset = j3;
17191 vinfos[3].indices[0] = _ij3[0];
17192 vinfos[3].indices[1] = _ij3[1];
17193 vinfos[3].maxsolutions = _nj3;
17194 vinfos[4].jointtype = 1;
17195 vinfos[4].foffset = j4;
17196 vinfos[4].indices[0] = _ij4[0];
17197 vinfos[4].indices[1] = _ij4[1];
17198 vinfos[4].maxsolutions = _nj4;
17199 vinfos[5].jointtype = 1;
17200 vinfos[5].foffset = j5;
17201 vinfos[5].indices[0] = _ij5[0];
17202 vinfos[5].indices[1] = _ij5[1];
17203 vinfos[5].maxsolutions = _nj5;
17204 vinfos[6].jointtype = 1;
17205 vinfos[6].foffset = j6;
17206 vinfos[6].indices[0] = _ij6[0];
17207 vinfos[6].indices[1] = _ij6[1];
17208 vinfos[6].maxsolutions = _nj6;
17209 std::vector<int> vfree(0);
17210 solutions.AddSolution(vinfos,vfree);
17211 }
17212 }
17213 }
17214 
17215 } else
17216 {
17217 if( 1 )
17218 {
17219 continue;
17220 
17221 } else
17222 {
17223 }
17224 }
17225 }
17226 }
17227 }
17228 }
17229 }
17230 }
17231 
17232 } else
17233 {
17234 {
17235 IkReal j3array[1], cj3array[1], sj3array[1];
17236 bool j3valid[1]={false};
17237 _nj3 = 1;
17238 IkReal x3865=((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30);
17239 IkReal x3866=((IkReal(1.00000000000000))*(sj5));
17240 IkReal x3867=((cj2)*(sj1));
17241 IkReal x3868=((r21)*(sj6));
17242 IkReal x3869=((cj6)*(r20));
17243 IkReal x3870=((cj5)*(r22));
17244 if( IKabs(((x3865)*(((((IkReal(-1.00000000000000))*(x3866)*(x3869)))+(((sj5)*(x3868)))+(((IkReal(-1.00000000000000))*(x3870))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x3865)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x3867)*(x3870)))+(((sj5)*(x3867)*(x3869)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x3866)))+(((IkReal(-1.00000000000000))*(x3866)*(x3867)*(x3868)))+(((cj6)*(r10)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x3865)*(((((IkReal(-1.00000000000000))*(x3866)*(x3869)))+(((sj5)*(x3868)))+(((IkReal(-1.00000000000000))*(x3870)))))))+IKsqr(((x3865)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x3867)*(x3870)))+(((sj5)*(x3867)*(x3869)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x3866)))+(((IkReal(-1.00000000000000))*(x3866)*(x3867)*(x3868)))+(((cj6)*(r10)*(sj2)*(sj5)))))))-1) <= IKFAST_SINCOS_THRESH )
17245     continue;
17246 j3array[0]=IKatan2(((x3865)*(((((IkReal(-1.00000000000000))*(x3866)*(x3869)))+(((sj5)*(x3868)))+(((IkReal(-1.00000000000000))*(x3870)))))), ((x3865)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x3867)*(x3870)))+(((sj5)*(x3867)*(x3869)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x3866)))+(((IkReal(-1.00000000000000))*(x3866)*(x3867)*(x3868)))+(((cj6)*(r10)*(sj2)*(sj5)))))));
17247 sj3array[0]=IKsin(j3array[0]);
17248 cj3array[0]=IKcos(j3array[0]);
17249 if( j3array[0] > IKPI )
17250 {
17251     j3array[0]-=IK2PI;
17252 }
17253 else if( j3array[0] < -IKPI )
17254 {    j3array[0]+=IK2PI;
17255 }
17256 j3valid[0] = true;
17257 for(int ij3 = 0; ij3 < 1; ++ij3)
17258 {
17259 if( !j3valid[ij3] )
17260 {
17261     continue;
17262 }
17263 _ij3[0] = ij3; _ij3[1] = -1;
17264 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17265 {
17266 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17267 {
17268     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17269 }
17270 }
17271 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17272 {
17273 IkReal evalcond[6];
17274 IkReal x3871=IKsin(j3);
17275 IkReal x3872=IKcos(j3);
17276 IkReal x3873=((r11)*(sj6));
17277 IkReal x3874=((IkReal(1.00000000000000))*(sj5));
17278 IkReal x3875=((sj4)*(sj6));
17279 IkReal x3876=((cj4)*(sj5));
17280 IkReal x3877=((cj6)*(sj4));
17281 IkReal x3878=((cj4)*(cj5));
17282 IkReal x3879=((r01)*(sj6));
17283 IkReal x3880=((r21)*(sj6));
17284 IkReal x3881=((cj6)*(sj5));
17285 IkReal x3882=((IkReal(1.00000000000000))*(x3872));
17286 IkReal x3883=((sj1)*(x3872));
17287 IkReal x3884=((IkReal(1.00000000000000))*(cj6)*(x3878));
17288 IkReal x3885=((IkReal(1.00000000000000))*(cj2)*(x3871));
17289 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x3881)))+(((IkReal(-1.00000000000000))*(x3874)*(x3880)))+(((sj2)*(x3871))));
17290 evalcond[1]=((((IkReal(-1.00000000000000))*(cj1)*(x3885)))+(((IkReal(-1.00000000000000))*(x3874)*(x3879)))+(((r00)*(x3881)))+(x3883)+(((cj5)*(r02))));
17291 evalcond[2]=((((IkReal(-1.00000000000000))*(cj1)*(x3882)))+(((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3873)*(x3874)))+(((IkReal(-1.00000000000000))*(sj1)*(x3885)))+(((r10)*(x3881))));
17292 evalcond[3]=((((r20)*(x3875)))+(((x3878)*(x3880)))+(((IkReal(-1.00000000000000))*(r20)*(x3884)))+(((r22)*(x3876)))+(((r21)*(x3877)))+(((IkReal(-1.00000000000000))*(sj2)*(x3882))));
17293 evalcond[4]=((((r00)*(x3875)))+(((cj1)*(cj2)*(x3872)))+(((r01)*(x3877)))+(((IkReal(-1.00000000000000))*(r00)*(x3884)))+(((sj1)*(x3871)))+(((x3878)*(x3879)))+(((r02)*(x3876))));
17294 evalcond[5]=((((IkReal(-1.00000000000000))*(cj1)*(x3871)))+(((cj2)*(x3883)))+(((x3873)*(x3878)))+(((r10)*(x3875)))+(((IkReal(-1.00000000000000))*(r10)*(x3884)))+(((r12)*(x3876)))+(((r11)*(x3877))));
17295 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
17296 {
17297 continue;
17298 }
17299 }
17300 
17301 {
17302 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17303 vinfos[0].jointtype = 1;
17304 vinfos[0].foffset = j0;
17305 vinfos[0].indices[0] = _ij0[0];
17306 vinfos[0].indices[1] = _ij0[1];
17307 vinfos[0].maxsolutions = _nj0;
17308 vinfos[1].jointtype = 1;
17309 vinfos[1].foffset = j1;
17310 vinfos[1].indices[0] = _ij1[0];
17311 vinfos[1].indices[1] = _ij1[1];
17312 vinfos[1].maxsolutions = _nj1;
17313 vinfos[2].jointtype = 1;
17314 vinfos[2].foffset = j2;
17315 vinfos[2].indices[0] = _ij2[0];
17316 vinfos[2].indices[1] = _ij2[1];
17317 vinfos[2].maxsolutions = _nj2;
17318 vinfos[3].jointtype = 1;
17319 vinfos[3].foffset = j3;
17320 vinfos[3].indices[0] = _ij3[0];
17321 vinfos[3].indices[1] = _ij3[1];
17322 vinfos[3].maxsolutions = _nj3;
17323 vinfos[4].jointtype = 1;
17324 vinfos[4].foffset = j4;
17325 vinfos[4].indices[0] = _ij4[0];
17326 vinfos[4].indices[1] = _ij4[1];
17327 vinfos[4].maxsolutions = _nj4;
17328 vinfos[5].jointtype = 1;
17329 vinfos[5].foffset = j5;
17330 vinfos[5].indices[0] = _ij5[0];
17331 vinfos[5].indices[1] = _ij5[1];
17332 vinfos[5].maxsolutions = _nj5;
17333 vinfos[6].jointtype = 1;
17334 vinfos[6].foffset = j6;
17335 vinfos[6].indices[0] = _ij6[0];
17336 vinfos[6].indices[1] = _ij6[1];
17337 vinfos[6].maxsolutions = _nj6;
17338 std::vector<int> vfree(0);
17339 solutions.AddSolution(vinfos,vfree);
17340 }
17341 }
17342 }
17343 
17344 }
17345 
17346 }
17347 
17348 } else
17349 {
17350 {
17351 IkReal j3array[1], cj3array[1], sj3array[1];
17352 bool j3valid[1]={false};
17353 _nj3 = 1;
17354 IkReal x3886=(sj1)*(sj1);
17355 IkReal x3887=(cj1)*(cj1);
17356 IkReal x3888=((cj5)*(r12));
17357 IkReal x3889=((cj6)*(sj5));
17358 IkReal x3890=((IkReal(1.00000000000000))*(sj1));
17359 IkReal x3891=((cj5)*(r02));
17360 IkReal x3892=((IkReal(1.00000000000000))*(cj1));
17361 IkReal x3893=((r01)*(sj5)*(sj6));
17362 IkReal x3894=((r11)*(sj5)*(sj6));
17363 if( IKabs(((((IKabs(((((cj2)*(x3887)))+(((cj2)*(x3886))))) != 0)?((IkReal)1/(((((cj2)*(x3887)))+(((cj2)*(x3886)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3892)*(x3893)))+(((cj1)*(r00)*(x3889)))+(((cj1)*(x3891)))+(((r10)*(sj1)*(x3889)))+(((sj1)*(x3888)))+(((IkReal(-1.00000000000000))*(x3890)*(x3894))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x3887)+(x3886))) != 0)?((IkReal)1/(((x3887)+(x3886)))):(IkReal)1.0e30))*(((((cj1)*(x3888)))+(((IkReal(-1.00000000000000))*(r00)*(x3889)*(x3890)))+(((sj1)*(x3893)))+(((IkReal(-1.00000000000000))*(x3892)*(x3894)))+(((cj1)*(r10)*(x3889)))+(((IkReal(-1.00000000000000))*(x3890)*(x3891))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x3887)))+(((cj2)*(x3886))))) != 0)?((IkReal)1/(((((cj2)*(x3887)))+(((cj2)*(x3886)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3892)*(x3893)))+(((cj1)*(r00)*(x3889)))+(((cj1)*(x3891)))+(((r10)*(sj1)*(x3889)))+(((sj1)*(x3888)))+(((IkReal(-1.00000000000000))*(x3890)*(x3894)))))))+IKsqr(((((IKabs(((x3887)+(x3886))) != 0)?((IkReal)1/(((x3887)+(x3886)))):(IkReal)1.0e30))*(((((cj1)*(x3888)))+(((IkReal(-1.00000000000000))*(r00)*(x3889)*(x3890)))+(((sj1)*(x3893)))+(((IkReal(-1.00000000000000))*(x3892)*(x3894)))+(((cj1)*(r10)*(x3889)))+(((IkReal(-1.00000000000000))*(x3890)*(x3891)))))))-1) <= IKFAST_SINCOS_THRESH )
17364     continue;
17365 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x3887)))+(((cj2)*(x3886))))) != 0)?((IkReal)1/(((((cj2)*(x3887)))+(((cj2)*(x3886)))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x3892)*(x3893)))+(((cj1)*(r00)*(x3889)))+(((cj1)*(x3891)))+(((r10)*(sj1)*(x3889)))+(((sj1)*(x3888)))+(((IkReal(-1.00000000000000))*(x3890)*(x3894)))))), ((((IKabs(((x3887)+(x3886))) != 0)?((IkReal)1/(((x3887)+(x3886)))):(IkReal)1.0e30))*(((((cj1)*(x3888)))+(((IkReal(-1.00000000000000))*(r00)*(x3889)*(x3890)))+(((sj1)*(x3893)))+(((IkReal(-1.00000000000000))*(x3892)*(x3894)))+(((cj1)*(r10)*(x3889)))+(((IkReal(-1.00000000000000))*(x3890)*(x3891)))))));
17366 sj3array[0]=IKsin(j3array[0]);
17367 cj3array[0]=IKcos(j3array[0]);
17368 if( j3array[0] > IKPI )
17369 {
17370     j3array[0]-=IK2PI;
17371 }
17372 else if( j3array[0] < -IKPI )
17373 {    j3array[0]+=IK2PI;
17374 }
17375 j3valid[0] = true;
17376 for(int ij3 = 0; ij3 < 1; ++ij3)
17377 {
17378 if( !j3valid[ij3] )
17379 {
17380     continue;
17381 }
17382 _ij3[0] = ij3; _ij3[1] = -1;
17383 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17384 {
17385 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17386 {
17387     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17388 }
17389 }
17390 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17391 {
17392 IkReal evalcond[6];
17393 IkReal x3895=IKsin(j3);
17394 IkReal x3896=IKcos(j3);
17395 IkReal x3897=((r11)*(sj6));
17396 IkReal x3898=((IkReal(1.00000000000000))*(sj5));
17397 IkReal x3899=((sj4)*(sj6));
17398 IkReal x3900=((cj4)*(sj5));
17399 IkReal x3901=((cj6)*(sj4));
17400 IkReal x3902=((cj4)*(cj5));
17401 IkReal x3903=((r01)*(sj6));
17402 IkReal x3904=((r21)*(sj6));
17403 IkReal x3905=((cj6)*(sj5));
17404 IkReal x3906=((IkReal(1.00000000000000))*(x3896));
17405 IkReal x3907=((sj1)*(x3896));
17406 IkReal x3908=((IkReal(1.00000000000000))*(cj6)*(x3902));
17407 IkReal x3909=((IkReal(1.00000000000000))*(cj2)*(x3895));
17408 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3898)*(x3904)))+(((r20)*(x3905)))+(((sj2)*(x3895))));
17409 evalcond[1]=((((r00)*(x3905)))+(x3907)+(((IkReal(-1.00000000000000))*(x3898)*(x3903)))+(((IkReal(-1.00000000000000))*(cj1)*(x3909)))+(((cj5)*(r02))));
17410 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x3905)))+(((IkReal(-1.00000000000000))*(sj1)*(x3909)))+(((IkReal(-1.00000000000000))*(x3897)*(x3898)))+(((IkReal(-1.00000000000000))*(cj1)*(x3906))));
17411 evalcond[3]=((((r20)*(x3899)))+(((r22)*(x3900)))+(((x3902)*(x3904)))+(((IkReal(-1.00000000000000))*(sj2)*(x3906)))+(((r21)*(x3901)))+(((IkReal(-1.00000000000000))*(r20)*(x3908))));
17412 evalcond[4]=((((cj1)*(cj2)*(x3896)))+(((x3902)*(x3903)))+(((r01)*(x3901)))+(((sj1)*(x3895)))+(((r00)*(x3899)))+(((IkReal(-1.00000000000000))*(r00)*(x3908)))+(((r02)*(x3900))));
17413 evalcond[5]=((((r10)*(x3899)))+(((x3897)*(x3902)))+(((IkReal(-1.00000000000000))*(cj1)*(x3895)))+(((r12)*(x3900)))+(((r11)*(x3901)))+(((IkReal(-1.00000000000000))*(r10)*(x3908)))+(((cj2)*(x3907))));
17414 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
17415 {
17416 continue;
17417 }
17418 }
17419 
17420 {
17421 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17422 vinfos[0].jointtype = 1;
17423 vinfos[0].foffset = j0;
17424 vinfos[0].indices[0] = _ij0[0];
17425 vinfos[0].indices[1] = _ij0[1];
17426 vinfos[0].maxsolutions = _nj0;
17427 vinfos[1].jointtype = 1;
17428 vinfos[1].foffset = j1;
17429 vinfos[1].indices[0] = _ij1[0];
17430 vinfos[1].indices[1] = _ij1[1];
17431 vinfos[1].maxsolutions = _nj1;
17432 vinfos[2].jointtype = 1;
17433 vinfos[2].foffset = j2;
17434 vinfos[2].indices[0] = _ij2[0];
17435 vinfos[2].indices[1] = _ij2[1];
17436 vinfos[2].maxsolutions = _nj2;
17437 vinfos[3].jointtype = 1;
17438 vinfos[3].foffset = j3;
17439 vinfos[3].indices[0] = _ij3[0];
17440 vinfos[3].indices[1] = _ij3[1];
17441 vinfos[3].maxsolutions = _nj3;
17442 vinfos[4].jointtype = 1;
17443 vinfos[4].foffset = j4;
17444 vinfos[4].indices[0] = _ij4[0];
17445 vinfos[4].indices[1] = _ij4[1];
17446 vinfos[4].maxsolutions = _nj4;
17447 vinfos[5].jointtype = 1;
17448 vinfos[5].foffset = j5;
17449 vinfos[5].indices[0] = _ij5[0];
17450 vinfos[5].indices[1] = _ij5[1];
17451 vinfos[5].maxsolutions = _nj5;
17452 vinfos[6].jointtype = 1;
17453 vinfos[6].foffset = j6;
17454 vinfos[6].indices[0] = _ij6[0];
17455 vinfos[6].indices[1] = _ij6[1];
17456 vinfos[6].maxsolutions = _nj6;
17457 std::vector<int> vfree(0);
17458 solutions.AddSolution(vinfos,vfree);
17459 }
17460 }
17461 }
17462 
17463 }
17464 
17465 }
17466 
17467 } else
17468 {
17469 {
17470 IkReal j3array[1], cj3array[1], sj3array[1];
17471 bool j3valid[1]={false};
17472 _nj3 = 1;
17473 IkReal x3910=((cj4)*(cj5));
17474 IkReal x3911=((r21)*(sj6));
17475 IkReal x3912=((IkReal(1.00000000000000))*(cj6)*(r20));
17476 if( IKabs(((gconst4)*(((((sj5)*(x3911)))+(((IkReal(-1.00000000000000))*(sj5)*(x3912)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((x3910)*(x3911)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x3910)*(x3912)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
17477     continue;
17478 j3array[0]=IKatan2(((gconst4)*(((((sj5)*(x3911)))+(((IkReal(-1.00000000000000))*(sj5)*(x3912)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))), ((gconst4)*(((((x3910)*(x3911)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x3910)*(x3912)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))))));
17479 sj3array[0]=IKsin(j3array[0]);
17480 cj3array[0]=IKcos(j3array[0]);
17481 if( j3array[0] > IKPI )
17482 {
17483     j3array[0]-=IK2PI;
17484 }
17485 else if( j3array[0] < -IKPI )
17486 {    j3array[0]+=IK2PI;
17487 }
17488 j3valid[0] = true;
17489 for(int ij3 = 0; ij3 < 1; ++ij3)
17490 {
17491 if( !j3valid[ij3] )
17492 {
17493     continue;
17494 }
17495 _ij3[0] = ij3; _ij3[1] = -1;
17496 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17497 {
17498 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17499 {
17500     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17501 }
17502 }
17503 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17504 {
17505 IkReal evalcond[6];
17506 IkReal x3913=IKsin(j3);
17507 IkReal x3914=IKcos(j3);
17508 IkReal x3915=((r11)*(sj6));
17509 IkReal x3916=((IkReal(1.00000000000000))*(sj5));
17510 IkReal x3917=((sj4)*(sj6));
17511 IkReal x3918=((cj4)*(sj5));
17512 IkReal x3919=((cj6)*(sj4));
17513 IkReal x3920=((cj4)*(cj5));
17514 IkReal x3921=((r01)*(sj6));
17515 IkReal x3922=((r21)*(sj6));
17516 IkReal x3923=((cj6)*(sj5));
17517 IkReal x3924=((IkReal(1.00000000000000))*(x3914));
17518 IkReal x3925=((sj1)*(x3914));
17519 IkReal x3926=((IkReal(1.00000000000000))*(cj6)*(x3920));
17520 IkReal x3927=((IkReal(1.00000000000000))*(cj2)*(x3913));
17521 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x3916)*(x3922)))+(((sj2)*(x3913)))+(((r20)*(x3923))));
17522 evalcond[1]=((((IkReal(-1.00000000000000))*(cj1)*(x3927)))+(((IkReal(-1.00000000000000))*(x3916)*(x3921)))+(x3925)+(((cj5)*(r02)))+(((r00)*(x3923))));
17523 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3915)*(x3916)))+(((IkReal(-1.00000000000000))*(cj1)*(x3924)))+(((r10)*(x3923)))+(((IkReal(-1.00000000000000))*(sj1)*(x3927))));
17524 evalcond[3]=((((r21)*(x3919)))+(((x3920)*(x3922)))+(((IkReal(-1.00000000000000))*(r20)*(x3926)))+(((r20)*(x3917)))+(((IkReal(-1.00000000000000))*(sj2)*(x3924)))+(((r22)*(x3918))));
17525 evalcond[4]=((((cj1)*(cj2)*(x3914)))+(((x3920)*(x3921)))+(((sj1)*(x3913)))+(((r00)*(x3917)))+(((r01)*(x3919)))+(((r02)*(x3918)))+(((IkReal(-1.00000000000000))*(r00)*(x3926))));
17526 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x3926)))+(((cj2)*(x3925)))+(((IkReal(-1.00000000000000))*(cj1)*(x3913)))+(((r10)*(x3917)))+(((r11)*(x3919)))+(((x3915)*(x3920)))+(((r12)*(x3918))));
17527 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
17528 {
17529 continue;
17530 }
17531 }
17532 
17533 {
17534 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17535 vinfos[0].jointtype = 1;
17536 vinfos[0].foffset = j0;
17537 vinfos[0].indices[0] = _ij0[0];
17538 vinfos[0].indices[1] = _ij0[1];
17539 vinfos[0].maxsolutions = _nj0;
17540 vinfos[1].jointtype = 1;
17541 vinfos[1].foffset = j1;
17542 vinfos[1].indices[0] = _ij1[0];
17543 vinfos[1].indices[1] = _ij1[1];
17544 vinfos[1].maxsolutions = _nj1;
17545 vinfos[2].jointtype = 1;
17546 vinfos[2].foffset = j2;
17547 vinfos[2].indices[0] = _ij2[0];
17548 vinfos[2].indices[1] = _ij2[1];
17549 vinfos[2].maxsolutions = _nj2;
17550 vinfos[3].jointtype = 1;
17551 vinfos[3].foffset = j3;
17552 vinfos[3].indices[0] = _ij3[0];
17553 vinfos[3].indices[1] = _ij3[1];
17554 vinfos[3].maxsolutions = _nj3;
17555 vinfos[4].jointtype = 1;
17556 vinfos[4].foffset = j4;
17557 vinfos[4].indices[0] = _ij4[0];
17558 vinfos[4].indices[1] = _ij4[1];
17559 vinfos[4].maxsolutions = _nj4;
17560 vinfos[5].jointtype = 1;
17561 vinfos[5].foffset = j5;
17562 vinfos[5].indices[0] = _ij5[0];
17563 vinfos[5].indices[1] = _ij5[1];
17564 vinfos[5].maxsolutions = _nj5;
17565 vinfos[6].jointtype = 1;
17566 vinfos[6].foffset = j6;
17567 vinfos[6].indices[0] = _ij6[0];
17568 vinfos[6].indices[1] = _ij6[1];
17569 vinfos[6].maxsolutions = _nj6;
17570 std::vector<int> vfree(0);
17571 solutions.AddSolution(vinfos,vfree);
17572 }
17573 }
17574 }
17575 
17576 }
17577 
17578 }
17579 }
17580 }
17581 
17582 }
17583 
17584 }
17585 
17586 } else
17587 {
17588 {
17589 IkReal j4array[1], cj4array[1], sj4array[1];
17590 bool j4valid[1]={false};
17591 _nj4 = 1;
17592 IkReal x3928=((cj2)*(sj6));
17593 IkReal x3929=((cj2)*(cj6));
17594 IkReal x3930=((IkReal(1.00000000000000))*(cj1)*(sj2));
17595 if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3930)))+(((r01)*(x3929)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3930)))+(((r00)*(x3928))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3930)))+(((cj5)*(r01)*(x3928)))+(((cj2)*(r02)*(sj5)))+(((cj1)*(cj5)*(cj6)*(r20)*(sj2)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3930)))+(((IkReal(-1.00000000000000))*(cj5)*(r00)*(x3929))))))) < IKFAST_ATAN2_MAGTHRESH )
17596     continue;
17597 j4array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x3930)))+(((r01)*(x3929)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x3930)))+(((r00)*(x3928)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(sj6)*(x3930)))+(((cj5)*(r01)*(x3928)))+(((cj2)*(r02)*(sj5)))+(((cj1)*(cj5)*(cj6)*(r20)*(sj2)))+(((IkReal(-1.00000000000000))*(r22)*(sj5)*(x3930)))+(((IkReal(-1.00000000000000))*(cj5)*(r00)*(x3929)))))));
17598 sj4array[0]=IKsin(j4array[0]);
17599 cj4array[0]=IKcos(j4array[0]);
17600 if( j4array[0] > IKPI )
17601 {
17602     j4array[0]-=IK2PI;
17603 }
17604 else if( j4array[0] < -IKPI )
17605 {    j4array[0]+=IK2PI;
17606 }
17607 j4valid[0] = true;
17608 for(int ij4 = 0; ij4 < 1; ++ij4)
17609 {
17610 if( !j4valid[ij4] )
17611 {
17612     continue;
17613 }
17614 _ij4[0] = ij4; _ij4[1] = -1;
17615 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
17616 {
17617 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
17618 {
17619     j4valid[iij4]=false; _ij4[1] = iij4; break; 
17620 }
17621 }
17622 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
17623 {
17624 IkReal evalcond[3];
17625 IkReal x3931=IKsin(j4);
17626 IkReal x3932=IKcos(j4);
17627 IkReal x3933=((IkReal(1.00000000000000))*(sj2));
17628 IkReal x3934=((IkReal(1.00000000000000))*(cj6));
17629 IkReal x3935=((sj5)*(x3931));
17630 IkReal x3936=((cj5)*(x3931));
17631 IkReal x3937=((IkReal(1.00000000000000))*(sj6)*(x3932));
17632 evalcond[0]=((((r22)*(x3935)))+(((IkReal(-1.00000000000000))*(r21)*(x3932)*(x3934)))+(((IkReal(-1.00000000000000))*(r20)*(x3934)*(x3936)))+(((IkReal(-1.00000000000000))*(r20)*(x3937)))+(((r21)*(sj6)*(x3936)))+(((IkReal(-1.00000000000000))*(cj2))));
17633 evalcond[1]=((((r01)*(sj6)*(x3936)))+(((IkReal(-1.00000000000000))*(r01)*(x3932)*(x3934)))+(((IkReal(-1.00000000000000))*(cj1)*(x3933)))+(((IkReal(-1.00000000000000))*(r00)*(x3937)))+(((IkReal(-1.00000000000000))*(r00)*(x3934)*(x3936)))+(((r02)*(x3935))));
17634 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(x3934)*(x3936)))+(((IkReal(-1.00000000000000))*(r11)*(x3932)*(x3934)))+(((IkReal(-1.00000000000000))*(sj1)*(x3933)))+(((r11)*(sj6)*(x3936)))+(((IkReal(-1.00000000000000))*(r10)*(x3937)))+(((r12)*(x3935))));
17635 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
17636 {
17637 continue;
17638 }
17639 }
17640 
17641 {
17642 IkReal dummyeval[1];
17643 IkReal gconst4;
17644 gconst4=IKsign(sj2);
17645 dummyeval[0]=sj2;
17646 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
17647 {
17648 {
17649 IkReal dummyeval[2];
17650 IkReal x3938=(sj1)*(sj1);
17651 IkReal x3939=(cj1)*(cj1);
17652 dummyeval[0]=((((cj2)*(x3939)))+(((cj2)*(x3938))));
17653 dummyeval[1]=((x3939)+(x3938));
17654 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
17655 {
17656 {
17657 IkReal dummyeval[2];
17658 dummyeval[0]=sj2;
17659 dummyeval[1]=cj1;
17660 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
17661 {
17662 {
17663 IkReal evalcond[9];
17664 IkReal x3940=((sj4)*(sj5));
17665 IkReal x3941=((cj6)*(r20));
17666 IkReal x3942=((IkReal(1.00000000000000))*(cj4));
17667 IkReal x3943=((IkReal(0.390000000000000))*(sj5));
17668 IkReal x3944=((r21)*(sj6));
17669 IkReal x3945=((IkReal(0.390000000000000))*(cj5));
17670 IkReal x3946=((cj6)*(r00));
17671 IkReal x3947=((IkReal(0.000500000000000000))*(cj6));
17672 IkReal x3948=((r11)*(sj6));
17673 IkReal x3949=((cj5)*(sj4));
17674 IkReal x3950=((r01)*(sj6));
17675 IkReal x3951=((r20)*(sj6));
17676 IkReal x3952=((r10)*(sj6));
17677 IkReal x3953=((cj6)*(r10));
17678 IkReal x3954=((cj6)*(r21));
17679 IkReal x3955=((r00)*(sj6));
17680 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j2)), IkReal(6.28318530717959))));
17681 evalcond[1]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(sj5)*(x3944)))+(((sj5)*(x3941))));
17682 evalcond[2]=((IkReal(-0.00200000000000000))+(((r22)*(x3945)))+(((IkReal(0.000500000000000000))*(x3951)))+(((x3941)*(x3943)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x3943)*(x3944)))+(((r21)*(x3947))));
17683 evalcond[3]=((((r01)*(x3947)))+(((IkReal(0.000500000000000000))*(x3955)))+(((IkReal(-1.00000000000000))*(px)))+(((x3943)*(x3946)))+(((IkReal(-1.00000000000000))*(x3943)*(x3950)))+(((r02)*(x3945)))+(((IkReal(-0.400000000000000))*(sj1))));
17684 evalcond[4]=((((IkReal(0.000500000000000000))*(x3952)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x3945)))+(((x3943)*(x3953)))+(((IkReal(-1.00000000000000))*(x3943)*(x3948)))+(((r11)*(x3947))));
17685 evalcond[5]=((IkReal(-1.00000000000000))+(((r22)*(x3940)))+(((IkReal(-1.00000000000000))*(x3942)*(x3951)))+(((IkReal(-1.00000000000000))*(x3942)*(x3954)))+(((x3944)*(x3949)))+(((IkReal(-1.00000000000000))*(x3941)*(x3949))));
17686 evalcond[6]=((((sj4)*(x3954)))+(((sj4)*(x3951)))+(((cj4)*(cj5)*(x3944)))+(((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x3941)*(x3942))));
17687 evalcond[7]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x3942)))+(((x3949)*(x3950)))+(((IkReal(-1.00000000000000))*(x3942)*(x3955)))+(((IkReal(-1.00000000000000))*(x3946)*(x3949)))+(((r02)*(x3940))));
17688 evalcond[8]=((((IkReal(-1.00000000000000))*(cj6)*(r11)*(x3942)))+(((IkReal(-1.00000000000000))*(x3942)*(x3952)))+(((x3948)*(x3949)))+(((r12)*(x3940)))+(((IkReal(-1.00000000000000))*(x3949)*(x3953))));
17689 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
17690 {
17691 {
17692 IkReal dummyeval[1];
17693 IkReal gconst5;
17694 gconst5=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
17695 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
17696 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
17697 {
17698 {
17699 IkReal dummyeval[1];
17700 IkReal gconst6;
17701 gconst6=IKsign((((sj1)*(sj1))+((cj1)*(cj1))));
17702 dummyeval[0]=(((sj1)*(sj1))+((cj1)*(cj1)));
17703 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
17704 {
17705 continue;
17706 
17707 } else
17708 {
17709 {
17710 IkReal j3array[1], cj3array[1], sj3array[1];
17711 bool j3valid[1]={false};
17712 _nj3 = 1;
17713 IkReal x3956=((IkReal(1.00000000000000))*(sj4));
17714 IkReal x3957=((r00)*(sj6));
17715 IkReal x3958=((cj1)*(cj6));
17716 IkReal x3959=((cj4)*(sj5));
17717 IkReal x3960=((cj4)*(cj5));
17718 IkReal x3961=((r01)*(sj1));
17719 IkReal x3962=((IkReal(1.00000000000000))*(sj6));
17720 IkReal x3963=((cj1)*(r02));
17721 IkReal x3964=((cj1)*(r01));
17722 IkReal x3965=((IkReal(1.00000000000000))*(r02)*(sj1));
17723 IkReal x3966=((cj6)*(r00)*(sj1));
17724 if( IKabs(((gconst6)*(((((cj5)*(x3963)))+(((IkReal(-1.00000000000000))*(sj1)*(x3956)*(x3957)))+(((r00)*(sj5)*(x3958)))+(((x3960)*(x3966)))+(((IkReal(-1.00000000000000))*(x3960)*(x3961)*(x3962)))+(((IkReal(-1.00000000000000))*(x3959)*(x3965)))+(((IkReal(-1.00000000000000))*(sj5)*(x3962)*(x3964)))+(((IkReal(-1.00000000000000))*(cj6)*(x3956)*(x3961))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x3960)*(x3962)*(x3964)))+(((sj5)*(sj6)*(x3961)))+(((IkReal(-1.00000000000000))*(r01)*(x3956)*(x3958)))+(((r00)*(x3958)*(x3960)))+(((IkReal(-1.00000000000000))*(x3959)*(x3963)))+(((IkReal(-1.00000000000000))*(cj5)*(x3965)))+(((IkReal(-1.00000000000000))*(cj1)*(x3956)*(x3957)))+(((IkReal(-1.00000000000000))*(sj5)*(x3966))))))) < IKFAST_ATAN2_MAGTHRESH )
17725     continue;
17726 j3array[0]=IKatan2(((gconst6)*(((((cj5)*(x3963)))+(((IkReal(-1.00000000000000))*(sj1)*(x3956)*(x3957)))+(((r00)*(sj5)*(x3958)))+(((x3960)*(x3966)))+(((IkReal(-1.00000000000000))*(x3960)*(x3961)*(x3962)))+(((IkReal(-1.00000000000000))*(x3959)*(x3965)))+(((IkReal(-1.00000000000000))*(sj5)*(x3962)*(x3964)))+(((IkReal(-1.00000000000000))*(cj6)*(x3956)*(x3961)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(x3960)*(x3962)*(x3964)))+(((sj5)*(sj6)*(x3961)))+(((IkReal(-1.00000000000000))*(r01)*(x3956)*(x3958)))+(((r00)*(x3958)*(x3960)))+(((IkReal(-1.00000000000000))*(x3959)*(x3963)))+(((IkReal(-1.00000000000000))*(cj5)*(x3965)))+(((IkReal(-1.00000000000000))*(cj1)*(x3956)*(x3957)))+(((IkReal(-1.00000000000000))*(sj5)*(x3966)))))));
17727 sj3array[0]=IKsin(j3array[0]);
17728 cj3array[0]=IKcos(j3array[0]);
17729 if( j3array[0] > IKPI )
17730 {
17731     j3array[0]-=IK2PI;
17732 }
17733 else if( j3array[0] < -IKPI )
17734 {    j3array[0]+=IK2PI;
17735 }
17736 j3valid[0] = true;
17737 for(int ij3 = 0; ij3 < 1; ++ij3)
17738 {
17739 if( !j3valid[ij3] )
17740 {
17741     continue;
17742 }
17743 _ij3[0] = ij3; _ij3[1] = -1;
17744 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17745 {
17746 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17747 {
17748     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17749 }
17750 }
17751 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17752 {
17753 IkReal evalcond[4];
17754 IkReal x3967=IKcos(j3);
17755 IkReal x3968=IKsin(j3);
17756 IkReal x3969=((r11)*(sj6));
17757 IkReal x3970=((IkReal(1.00000000000000))*(sj5));
17758 IkReal x3971=((sj4)*(sj6));
17759 IkReal x3972=((cj4)*(sj5));
17760 IkReal x3973=((cj6)*(sj4));
17761 IkReal x3974=((cj4)*(cj5));
17762 IkReal x3975=((r01)*(sj6));
17763 IkReal x3976=((cj6)*(sj5));
17764 IkReal x3977=((sj1)*(x3967));
17765 IkReal x3978=((cj1)*(x3967));
17766 IkReal x3979=((IkReal(1.00000000000000))*(x3968));
17767 IkReal x3980=((cj1)*(x3979));
17768 IkReal x3981=((IkReal(1.00000000000000))*(cj6)*(x3974));
17769 evalcond[0]=((x3977)+(((IkReal(-1.00000000000000))*(x3970)*(x3975)))+(((r00)*(x3976)))+(((IkReal(-1.00000000000000))*(x3980)))+(((cj5)*(r02))));
17770 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x3969)*(x3970)))+(((r10)*(x3976)))+(((IkReal(-1.00000000000000))*(sj1)*(x3979)))+(((IkReal(-1.00000000000000))*(x3978))));
17771 evalcond[2]=((((r02)*(x3972)))+(((x3974)*(x3975)))+(x3978)+(((r01)*(x3973)))+(((IkReal(-1.00000000000000))*(r00)*(x3981)))+(((r00)*(x3971)))+(((sj1)*(x3968))));
17772 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x3981)))+(((r11)*(x3973)))+(((r10)*(x3971)))+(((r12)*(x3972)))+(x3977)+(((x3969)*(x3974)))+(((IkReal(-1.00000000000000))*(x3980))));
17773 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
17774 {
17775 continue;
17776 }
17777 }
17778 
17779 {
17780 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17781 vinfos[0].jointtype = 1;
17782 vinfos[0].foffset = j0;
17783 vinfos[0].indices[0] = _ij0[0];
17784 vinfos[0].indices[1] = _ij0[1];
17785 vinfos[0].maxsolutions = _nj0;
17786 vinfos[1].jointtype = 1;
17787 vinfos[1].foffset = j1;
17788 vinfos[1].indices[0] = _ij1[0];
17789 vinfos[1].indices[1] = _ij1[1];
17790 vinfos[1].maxsolutions = _nj1;
17791 vinfos[2].jointtype = 1;
17792 vinfos[2].foffset = j2;
17793 vinfos[2].indices[0] = _ij2[0];
17794 vinfos[2].indices[1] = _ij2[1];
17795 vinfos[2].maxsolutions = _nj2;
17796 vinfos[3].jointtype = 1;
17797 vinfos[3].foffset = j3;
17798 vinfos[3].indices[0] = _ij3[0];
17799 vinfos[3].indices[1] = _ij3[1];
17800 vinfos[3].maxsolutions = _nj3;
17801 vinfos[4].jointtype = 1;
17802 vinfos[4].foffset = j4;
17803 vinfos[4].indices[0] = _ij4[0];
17804 vinfos[4].indices[1] = _ij4[1];
17805 vinfos[4].maxsolutions = _nj4;
17806 vinfos[5].jointtype = 1;
17807 vinfos[5].foffset = j5;
17808 vinfos[5].indices[0] = _ij5[0];
17809 vinfos[5].indices[1] = _ij5[1];
17810 vinfos[5].maxsolutions = _nj5;
17811 vinfos[6].jointtype = 1;
17812 vinfos[6].foffset = j6;
17813 vinfos[6].indices[0] = _ij6[0];
17814 vinfos[6].indices[1] = _ij6[1];
17815 vinfos[6].maxsolutions = _nj6;
17816 std::vector<int> vfree(0);
17817 solutions.AddSolution(vinfos,vfree);
17818 }
17819 }
17820 }
17821 
17822 }
17823 
17824 }
17825 
17826 } else
17827 {
17828 {
17829 IkReal j3array[1], cj3array[1], sj3array[1];
17830 bool j3valid[1]={false};
17831 _nj3 = 1;
17832 IkReal x3982=((cj5)*(r12));
17833 IkReal x3983=((cj6)*(sj5));
17834 IkReal x3984=((IkReal(1.00000000000000))*(sj1));
17835 IkReal x3985=((cj5)*(r02));
17836 IkReal x3986=((IkReal(1.00000000000000))*(cj1));
17837 IkReal x3987=((r01)*(sj5)*(sj6));
17838 IkReal x3988=((r11)*(sj5)*(sj6));
17839 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x3984)*(x3988)))+(((sj1)*(x3982)))+(((IkReal(-1.00000000000000))*(x3986)*(x3987)))+(((cj1)*(r00)*(x3983)))+(((r10)*(sj1)*(x3983)))+(((cj1)*(x3985))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(r00)*(x3983)*(x3984)))+(((cj1)*(r10)*(x3983)))+(((IkReal(-1.00000000000000))*(x3984)*(x3985)))+(((sj1)*(x3987)))+(((IkReal(-1.00000000000000))*(x3986)*(x3988)))+(((cj1)*(x3982))))))) < IKFAST_ATAN2_MAGTHRESH )
17840     continue;
17841 j3array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x3984)*(x3988)))+(((sj1)*(x3982)))+(((IkReal(-1.00000000000000))*(x3986)*(x3987)))+(((cj1)*(r00)*(x3983)))+(((r10)*(sj1)*(x3983)))+(((cj1)*(x3985)))))), ((gconst5)*(((((IkReal(-1.00000000000000))*(r00)*(x3983)*(x3984)))+(((cj1)*(r10)*(x3983)))+(((IkReal(-1.00000000000000))*(x3984)*(x3985)))+(((sj1)*(x3987)))+(((IkReal(-1.00000000000000))*(x3986)*(x3988)))+(((cj1)*(x3982)))))));
17842 sj3array[0]=IKsin(j3array[0]);
17843 cj3array[0]=IKcos(j3array[0]);
17844 if( j3array[0] > IKPI )
17845 {
17846     j3array[0]-=IK2PI;
17847 }
17848 else if( j3array[0] < -IKPI )
17849 {    j3array[0]+=IK2PI;
17850 }
17851 j3valid[0] = true;
17852 for(int ij3 = 0; ij3 < 1; ++ij3)
17853 {
17854 if( !j3valid[ij3] )
17855 {
17856     continue;
17857 }
17858 _ij3[0] = ij3; _ij3[1] = -1;
17859 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17860 {
17861 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17862 {
17863     j3valid[iij3]=false; _ij3[1] = iij3; break; 
17864 }
17865 }
17866 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17867 {
17868 IkReal evalcond[4];
17869 IkReal x3989=IKcos(j3);
17870 IkReal x3990=IKsin(j3);
17871 IkReal x3991=((r11)*(sj6));
17872 IkReal x3992=((IkReal(1.00000000000000))*(sj5));
17873 IkReal x3993=((sj4)*(sj6));
17874 IkReal x3994=((cj4)*(sj5));
17875 IkReal x3995=((cj6)*(sj4));
17876 IkReal x3996=((cj4)*(cj5));
17877 IkReal x3997=((r01)*(sj6));
17878 IkReal x3998=((cj6)*(sj5));
17879 IkReal x3999=((sj1)*(x3989));
17880 IkReal x4000=((cj1)*(x3989));
17881 IkReal x4001=((IkReal(1.00000000000000))*(x3990));
17882 IkReal x4002=((cj1)*(x4001));
17883 IkReal x4003=((IkReal(1.00000000000000))*(cj6)*(x3996));
17884 evalcond[0]=((((IkReal(-1.00000000000000))*(x4002)))+(((IkReal(-1.00000000000000))*(x3992)*(x3997)))+(x3999)+(((r00)*(x3998)))+(((cj5)*(r02))));
17885 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x4000)))+(((r10)*(x3998)))+(((IkReal(-1.00000000000000))*(sj1)*(x4001)))+(((IkReal(-1.00000000000000))*(x3991)*(x3992))));
17886 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x4003)))+(((x3996)*(x3997)))+(((r02)*(x3994)))+(x4000)+(((r00)*(x3993)))+(((r01)*(x3995)))+(((sj1)*(x3990))));
17887 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x4003)))+(((IkReal(-1.00000000000000))*(x4002)))+(((r10)*(x3993)))+(((r12)*(x3994)))+(x3999)+(((r11)*(x3995)))+(((x3991)*(x3996))));
17888 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
17889 {
17890 continue;
17891 }
17892 }
17893 
17894 {
17895 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
17896 vinfos[0].jointtype = 1;
17897 vinfos[0].foffset = j0;
17898 vinfos[0].indices[0] = _ij0[0];
17899 vinfos[0].indices[1] = _ij0[1];
17900 vinfos[0].maxsolutions = _nj0;
17901 vinfos[1].jointtype = 1;
17902 vinfos[1].foffset = j1;
17903 vinfos[1].indices[0] = _ij1[0];
17904 vinfos[1].indices[1] = _ij1[1];
17905 vinfos[1].maxsolutions = _nj1;
17906 vinfos[2].jointtype = 1;
17907 vinfos[2].foffset = j2;
17908 vinfos[2].indices[0] = _ij2[0];
17909 vinfos[2].indices[1] = _ij2[1];
17910 vinfos[2].maxsolutions = _nj2;
17911 vinfos[3].jointtype = 1;
17912 vinfos[3].foffset = j3;
17913 vinfos[3].indices[0] = _ij3[0];
17914 vinfos[3].indices[1] = _ij3[1];
17915 vinfos[3].maxsolutions = _nj3;
17916 vinfos[4].jointtype = 1;
17917 vinfos[4].foffset = j4;
17918 vinfos[4].indices[0] = _ij4[0];
17919 vinfos[4].indices[1] = _ij4[1];
17920 vinfos[4].maxsolutions = _nj4;
17921 vinfos[5].jointtype = 1;
17922 vinfos[5].foffset = j5;
17923 vinfos[5].indices[0] = _ij5[0];
17924 vinfos[5].indices[1] = _ij5[1];
17925 vinfos[5].maxsolutions = _nj5;
17926 vinfos[6].jointtype = 1;
17927 vinfos[6].foffset = j6;
17928 vinfos[6].indices[0] = _ij6[0];
17929 vinfos[6].indices[1] = _ij6[1];
17930 vinfos[6].maxsolutions = _nj6;
17931 std::vector<int> vfree(0);
17932 solutions.AddSolution(vinfos,vfree);
17933 }
17934 }
17935 }
17936 
17937 }
17938 
17939 }
17940 
17941 } else
17942 {
17943 IkReal x4004=((sj4)*(sj5));
17944 IkReal x4005=((cj6)*(r20));
17945 IkReal x4006=((IkReal(1.00000000000000))*(cj4));
17946 IkReal x4007=((IkReal(0.390000000000000))*(sj5));
17947 IkReal x4008=((r21)*(sj6));
17948 IkReal x4009=((IkReal(0.390000000000000))*(cj5));
17949 IkReal x4010=((cj6)*(r00));
17950 IkReal x4011=((IkReal(0.000500000000000000))*(cj6));
17951 IkReal x4012=((r11)*(sj6));
17952 IkReal x4013=((cj5)*(sj4));
17953 IkReal x4014=((r01)*(sj6));
17954 IkReal x4015=((r20)*(sj6));
17955 IkReal x4016=((r10)*(sj6));
17956 IkReal x4017=((cj6)*(r10));
17957 IkReal x4018=((cj6)*(r21));
17958 IkReal x4019=((r00)*(sj6));
17959 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j2)), IkReal(6.28318530717959))));
17960 evalcond[1]=((((cj5)*(r22)))+(((sj5)*(x4005)))+(((IkReal(-1.00000000000000))*(sj5)*(x4008))));
17961 evalcond[2]=((IkReal(0.00200000000000000))+(((x4005)*(x4007)))+(((r22)*(x4009)))+(((IkReal(0.000500000000000000))*(x4015)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x4011)))+(((IkReal(-1.00000000000000))*(x4007)*(x4008))));
17962 evalcond[3]=((((r01)*(x4011)))+(((IkReal(0.000500000000000000))*(x4019)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x4009)))+(((IkReal(-1.00000000000000))*(x4007)*(x4014)))+(((x4007)*(x4010)))+(((IkReal(-0.400000000000000))*(sj1))));
17963 evalcond[4]=((((r12)*(x4009)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(0.000500000000000000))*(x4016)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(x4007)*(x4012)))+(((r11)*(x4011)))+(((x4007)*(x4017))));
17964 evalcond[5]=((IkReal(1.00000000000000))+(((r22)*(x4004)))+(((IkReal(-1.00000000000000))*(x4005)*(x4013)))+(((IkReal(-1.00000000000000))*(x4006)*(x4018)))+(((IkReal(-1.00000000000000))*(x4006)*(x4015)))+(((x4008)*(x4013))));
17965 evalcond[6]=((((cj4)*(cj5)*(x4008)))+(((cj4)*(r22)*(sj5)))+(((sj4)*(x4015)))+(((sj4)*(x4018)))+(((IkReal(-1.00000000000000))*(cj5)*(x4005)*(x4006))));
17966 evalcond[7]=((((IkReal(-1.00000000000000))*(cj6)*(r01)*(x4006)))+(((x4013)*(x4014)))+(((r02)*(x4004)))+(((IkReal(-1.00000000000000))*(x4006)*(x4019)))+(((IkReal(-1.00000000000000))*(x4010)*(x4013))));
17967 evalcond[8]=((((r12)*(x4004)))+(((IkReal(-1.00000000000000))*(cj6)*(r11)*(x4006)))+(((x4012)*(x4013)))+(((IkReal(-1.00000000000000))*(x4006)*(x4016)))+(((IkReal(-1.00000000000000))*(x4013)*(x4017))));
17968 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  )
17969 {
17970 {
17971 IkReal dummyeval[1];
17972 IkReal gconst7;
17973 gconst7=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
17974 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
17975 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
17976 {
17977 {
17978 IkReal dummyeval[1];
17979 IkReal gconst8;
17980 gconst8=IKsign(((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1))))));
17981 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj1)*(sj1))))+(((IkReal(-1.00000000000000))*((cj1)*(cj1)))));
17982 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
17983 {
17984 continue;
17985 
17986 } else
17987 {
17988 {
17989 IkReal j3array[1], cj3array[1], sj3array[1];
17990 bool j3valid[1]={false};
17991 _nj3 = 1;
17992 IkReal x4020=((sj1)*(sj6));
17993 IkReal x4021=((r00)*(sj4));
17994 IkReal x4022=((IkReal(1.00000000000000))*(r01));
17995 IkReal x4023=((cj1)*(cj6));
17996 IkReal x4024=((cj1)*(sj6));
17997 IkReal x4025=((cj4)*(cj5));
17998 IkReal x4026=((r02)*(sj1));
17999 IkReal x4027=((cj4)*(sj5));
18000 IkReal x4028=((cj1)*(r02));
18001 IkReal x4029=((cj6)*(r00)*(sj1));
18002 if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(sj5)*(x4022)*(x4024)))+(((r00)*(sj5)*(x4023)))+(((cj5)*(x4028)))+(((x4020)*(x4021)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((IkReal(-1.00000000000000))*(x4025)*(x4029)))+(((r01)*(x4020)*(x4025)))+(((x4026)*(x4027))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((cj5)*(x4026)))+(((IkReal(-1.00000000000000))*(x4027)*(x4028)))+(((IkReal(-1.00000000000000))*(x4022)*(x4024)*(x4025)))+(((sj5)*(x4029)))+(((r00)*(x4023)*(x4025)))+(((IkReal(-1.00000000000000))*(sj4)*(x4022)*(x4023)))+(((IkReal(-1.00000000000000))*(sj5)*(x4020)*(x4022)))+(((IkReal(-1.00000000000000))*(x4021)*(x4024))))))) < IKFAST_ATAN2_MAGTHRESH )
18003     continue;
18004 j3array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(sj5)*(x4022)*(x4024)))+(((r00)*(sj5)*(x4023)))+(((cj5)*(x4028)))+(((x4020)*(x4021)))+(((cj6)*(r01)*(sj1)*(sj4)))+(((IkReal(-1.00000000000000))*(x4025)*(x4029)))+(((r01)*(x4020)*(x4025)))+(((x4026)*(x4027)))))), ((gconst8)*(((((cj5)*(x4026)))+(((IkReal(-1.00000000000000))*(x4027)*(x4028)))+(((IkReal(-1.00000000000000))*(x4022)*(x4024)*(x4025)))+(((sj5)*(x4029)))+(((r00)*(x4023)*(x4025)))+(((IkReal(-1.00000000000000))*(sj4)*(x4022)*(x4023)))+(((IkReal(-1.00000000000000))*(sj5)*(x4020)*(x4022)))+(((IkReal(-1.00000000000000))*(x4021)*(x4024)))))));
18005 sj3array[0]=IKsin(j3array[0]);
18006 cj3array[0]=IKcos(j3array[0]);
18007 if( j3array[0] > IKPI )
18008 {
18009     j3array[0]-=IK2PI;
18010 }
18011 else if( j3array[0] < -IKPI )
18012 {    j3array[0]+=IK2PI;
18013 }
18014 j3valid[0] = true;
18015 for(int ij3 = 0; ij3 < 1; ++ij3)
18016 {
18017 if( !j3valid[ij3] )
18018 {
18019     continue;
18020 }
18021 _ij3[0] = ij3; _ij3[1] = -1;
18022 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18023 {
18024 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18025 {
18026     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18027 }
18028 }
18029 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18030 {
18031 IkReal evalcond[4];
18032 IkReal x4030=IKsin(j3);
18033 IkReal x4031=IKcos(j3);
18034 IkReal x4032=((r11)*(sj6));
18035 IkReal x4033=((IkReal(1.00000000000000))*(sj5));
18036 IkReal x4034=((sj4)*(sj6));
18037 IkReal x4035=((cj4)*(sj5));
18038 IkReal x4036=((cj6)*(sj4));
18039 IkReal x4037=((cj4)*(cj5));
18040 IkReal x4038=((r01)*(sj6));
18041 IkReal x4039=((cj6)*(sj5));
18042 IkReal x4040=((sj1)*(x4030));
18043 IkReal x4041=((cj1)*(x4030));
18044 IkReal x4042=((IkReal(1.00000000000000))*(x4031));
18045 IkReal x4043=((cj1)*(x4042));
18046 IkReal x4044=((IkReal(1.00000000000000))*(cj6)*(x4037));
18047 evalcond[0]=((x4041)+(((r00)*(x4039)))+(((sj1)*(x4031)))+(((IkReal(-1.00000000000000))*(x4033)*(x4038)))+(((cj5)*(r02))));
18048 evalcond[1]=((((cj5)*(r12)))+(x4040)+(((r10)*(x4039)))+(((IkReal(-1.00000000000000))*(x4043)))+(((IkReal(-1.00000000000000))*(x4032)*(x4033))));
18049 evalcond[2]=((((x4037)*(x4038)))+(((r02)*(x4035)))+(((IkReal(-1.00000000000000))*(r00)*(x4044)))+(x4040)+(((r00)*(x4034)))+(((r01)*(x4036)))+(((IkReal(-1.00000000000000))*(x4043))));
18050 evalcond[3]=((((x4032)*(x4037)))+(((IkReal(-1.00000000000000))*(sj1)*(x4042)))+(((IkReal(-1.00000000000000))*(r10)*(x4044)))+(((IkReal(-1.00000000000000))*(x4041)))+(((r10)*(x4034)))+(((r12)*(x4035)))+(((r11)*(x4036))));
18051 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
18052 {
18053 continue;
18054 }
18055 }
18056 
18057 {
18058 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18059 vinfos[0].jointtype = 1;
18060 vinfos[0].foffset = j0;
18061 vinfos[0].indices[0] = _ij0[0];
18062 vinfos[0].indices[1] = _ij0[1];
18063 vinfos[0].maxsolutions = _nj0;
18064 vinfos[1].jointtype = 1;
18065 vinfos[1].foffset = j1;
18066 vinfos[1].indices[0] = _ij1[0];
18067 vinfos[1].indices[1] = _ij1[1];
18068 vinfos[1].maxsolutions = _nj1;
18069 vinfos[2].jointtype = 1;
18070 vinfos[2].foffset = j2;
18071 vinfos[2].indices[0] = _ij2[0];
18072 vinfos[2].indices[1] = _ij2[1];
18073 vinfos[2].maxsolutions = _nj2;
18074 vinfos[3].jointtype = 1;
18075 vinfos[3].foffset = j3;
18076 vinfos[3].indices[0] = _ij3[0];
18077 vinfos[3].indices[1] = _ij3[1];
18078 vinfos[3].maxsolutions = _nj3;
18079 vinfos[4].jointtype = 1;
18080 vinfos[4].foffset = j4;
18081 vinfos[4].indices[0] = _ij4[0];
18082 vinfos[4].indices[1] = _ij4[1];
18083 vinfos[4].maxsolutions = _nj4;
18084 vinfos[5].jointtype = 1;
18085 vinfos[5].foffset = j5;
18086 vinfos[5].indices[0] = _ij5[0];
18087 vinfos[5].indices[1] = _ij5[1];
18088 vinfos[5].maxsolutions = _nj5;
18089 vinfos[6].jointtype = 1;
18090 vinfos[6].foffset = j6;
18091 vinfos[6].indices[0] = _ij6[0];
18092 vinfos[6].indices[1] = _ij6[1];
18093 vinfos[6].maxsolutions = _nj6;
18094 std::vector<int> vfree(0);
18095 solutions.AddSolution(vinfos,vfree);
18096 }
18097 }
18098 }
18099 
18100 }
18101 
18102 }
18103 
18104 } else
18105 {
18106 {
18107 IkReal j3array[1], cj3array[1], sj3array[1];
18108 bool j3valid[1]={false};
18109 _nj3 = 1;
18110 IkReal x4045=((sj1)*(sj5));
18111 IkReal x4046=((cj6)*(r10));
18112 IkReal x4047=((cj5)*(sj1));
18113 IkReal x4048=((cj1)*(cj5));
18114 IkReal x4049=((cj1)*(sj5));
18115 IkReal x4050=((cj6)*(r00));
18116 IkReal x4051=((r11)*(sj6));
18117 IkReal x4052=((IkReal(1.00000000000000))*(r01)*(sj6));
18118 if( IKabs(((gconst7)*(((((x4045)*(x4046)))+(((x4049)*(x4050)))+(((r02)*(x4048)))+(((r12)*(x4047)))+(((IkReal(-1.00000000000000))*(x4049)*(x4052)))+(((IkReal(-1.00000000000000))*(x4045)*(x4051))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x4048)))+(((x4049)*(x4051)))+(((x4045)*(x4050)))+(((r02)*(x4047)))+(((IkReal(-1.00000000000000))*(x4045)*(x4052)))+(((IkReal(-1.00000000000000))*(x4046)*(x4049))))))) < IKFAST_ATAN2_MAGTHRESH )
18119     continue;
18120 j3array[0]=IKatan2(((gconst7)*(((((x4045)*(x4046)))+(((x4049)*(x4050)))+(((r02)*(x4048)))+(((r12)*(x4047)))+(((IkReal(-1.00000000000000))*(x4049)*(x4052)))+(((IkReal(-1.00000000000000))*(x4045)*(x4051)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(r12)*(x4048)))+(((x4049)*(x4051)))+(((x4045)*(x4050)))+(((r02)*(x4047)))+(((IkReal(-1.00000000000000))*(x4045)*(x4052)))+(((IkReal(-1.00000000000000))*(x4046)*(x4049)))))));
18121 sj3array[0]=IKsin(j3array[0]);
18122 cj3array[0]=IKcos(j3array[0]);
18123 if( j3array[0] > IKPI )
18124 {
18125     j3array[0]-=IK2PI;
18126 }
18127 else if( j3array[0] < -IKPI )
18128 {    j3array[0]+=IK2PI;
18129 }
18130 j3valid[0] = true;
18131 for(int ij3 = 0; ij3 < 1; ++ij3)
18132 {
18133 if( !j3valid[ij3] )
18134 {
18135     continue;
18136 }
18137 _ij3[0] = ij3; _ij3[1] = -1;
18138 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18139 {
18140 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18141 {
18142     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18143 }
18144 }
18145 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18146 {
18147 IkReal evalcond[4];
18148 IkReal x4053=IKsin(j3);
18149 IkReal x4054=IKcos(j3);
18150 IkReal x4055=((r11)*(sj6));
18151 IkReal x4056=((IkReal(1.00000000000000))*(sj5));
18152 IkReal x4057=((sj4)*(sj6));
18153 IkReal x4058=((cj4)*(sj5));
18154 IkReal x4059=((cj6)*(sj4));
18155 IkReal x4060=((cj4)*(cj5));
18156 IkReal x4061=((r01)*(sj6));
18157 IkReal x4062=((cj6)*(sj5));
18158 IkReal x4063=((sj1)*(x4053));
18159 IkReal x4064=((cj1)*(x4053));
18160 IkReal x4065=((IkReal(1.00000000000000))*(x4054));
18161 IkReal x4066=((cj1)*(x4065));
18162 IkReal x4067=((IkReal(1.00000000000000))*(cj6)*(x4060));
18163 evalcond[0]=((x4064)+(((r00)*(x4062)))+(((sj1)*(x4054)))+(((IkReal(-1.00000000000000))*(x4056)*(x4061)))+(((cj5)*(r02))));
18164 evalcond[1]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x4055)*(x4056)))+(x4063)+(((IkReal(-1.00000000000000))*(x4066)))+(((r10)*(x4062))));
18165 evalcond[2]=((((x4060)*(x4061)))+(((r02)*(x4058)))+(x4063)+(((IkReal(-1.00000000000000))*(r00)*(x4067)))+(((IkReal(-1.00000000000000))*(x4066)))+(((r00)*(x4057)))+(((r01)*(x4059))));
18166 evalcond[3]=((((r11)*(x4059)))+(((IkReal(-1.00000000000000))*(r10)*(x4067)))+(((r12)*(x4058)))+(((IkReal(-1.00000000000000))*(x4064)))+(((x4055)*(x4060)))+(((r10)*(x4057)))+(((IkReal(-1.00000000000000))*(sj1)*(x4065))));
18167 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
18168 {
18169 continue;
18170 }
18171 }
18172 
18173 {
18174 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18175 vinfos[0].jointtype = 1;
18176 vinfos[0].foffset = j0;
18177 vinfos[0].indices[0] = _ij0[0];
18178 vinfos[0].indices[1] = _ij0[1];
18179 vinfos[0].maxsolutions = _nj0;
18180 vinfos[1].jointtype = 1;
18181 vinfos[1].foffset = j1;
18182 vinfos[1].indices[0] = _ij1[0];
18183 vinfos[1].indices[1] = _ij1[1];
18184 vinfos[1].maxsolutions = _nj1;
18185 vinfos[2].jointtype = 1;
18186 vinfos[2].foffset = j2;
18187 vinfos[2].indices[0] = _ij2[0];
18188 vinfos[2].indices[1] = _ij2[1];
18189 vinfos[2].maxsolutions = _nj2;
18190 vinfos[3].jointtype = 1;
18191 vinfos[3].foffset = j3;
18192 vinfos[3].indices[0] = _ij3[0];
18193 vinfos[3].indices[1] = _ij3[1];
18194 vinfos[3].maxsolutions = _nj3;
18195 vinfos[4].jointtype = 1;
18196 vinfos[4].foffset = j4;
18197 vinfos[4].indices[0] = _ij4[0];
18198 vinfos[4].indices[1] = _ij4[1];
18199 vinfos[4].maxsolutions = _nj4;
18200 vinfos[5].jointtype = 1;
18201 vinfos[5].foffset = j5;
18202 vinfos[5].indices[0] = _ij5[0];
18203 vinfos[5].indices[1] = _ij5[1];
18204 vinfos[5].maxsolutions = _nj5;
18205 vinfos[6].jointtype = 1;
18206 vinfos[6].foffset = j6;
18207 vinfos[6].indices[0] = _ij6[0];
18208 vinfos[6].indices[1] = _ij6[1];
18209 vinfos[6].maxsolutions = _nj6;
18210 std::vector<int> vfree(0);
18211 solutions.AddSolution(vinfos,vfree);
18212 }
18213 }
18214 }
18215 
18216 }
18217 
18218 }
18219 
18220 } else
18221 {
18222 IkReal x4068=((sj4)*(sj5));
18223 IkReal x4069=((IkReal(0.390000000000000))*(sj5));
18224 IkReal x4070=((r21)*(sj6));
18225 IkReal x4071=((IkReal(0.390000000000000))*(cj5));
18226 IkReal x4072=((cj6)*(r00));
18227 IkReal x4073=((IkReal(0.000500000000000000))*(cj6));
18228 IkReal x4074=((r11)*(sj6));
18229 IkReal x4075=((cj5)*(sj4));
18230 IkReal x4076=((IkReal(1.00000000000000))*(r20));
18231 IkReal x4077=((cj4)*(sj6));
18232 IkReal x4078=((r01)*(sj6));
18233 IkReal x4079=((IkReal(0.000500000000000000))*(sj6));
18234 IkReal x4080=((IkReal(1.00000000000000))*(r10));
18235 IkReal x4081=((IkReal(1.00000000000000))*(cj4)*(cj6));
18236 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959))));
18237 evalcond[1]=((((IkReal(-1.00000000000000))*(x4069)*(x4070)))+(((r20)*(x4079)))+(((cj6)*(r20)*(x4069)))+(((r22)*(x4071)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.00200000000000000))*(cj2)))+(((r21)*(x4073))));
18238 evalcond[2]=((IkReal(-0.400000000000000))+(((IkReal(-1.00000000000000))*(x4069)*(x4078)))+(((r00)*(x4079)))+(((r01)*(x4073)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x4071)))+(((x4069)*(x4072))));
18239 evalcond[3]=((((IkReal(-1.00000000000000))*(x4069)*(x4074)))+(((r12)*(x4071)))+(((r10)*(x4079)))+(((IkReal(-1.00000000000000))*(py)))+(((cj6)*(r10)*(x4069)))+(((IkReal(-0.00200000000000000))*(sj2)))+(((r11)*(x4073))));
18240 evalcond[4]=((((IkReal(-1.00000000000000))*(r21)*(x4081)))+(((x4070)*(x4075)))+(((r22)*(x4068)))+(((IkReal(-1.00000000000000))*(cj6)*(x4075)*(x4076)))+(((IkReal(-1.00000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x4076)*(x4077))));
18241 evalcond[5]=((((r02)*(x4068)))+(((x4075)*(x4078)))+(((IkReal(-1.00000000000000))*(r01)*(x4081)))+(((IkReal(-1.00000000000000))*(r00)*(x4077)))+(((IkReal(-1.00000000000000))*(x4072)*(x4075))));
18242 evalcond[6]=((((IkReal(-1.00000000000000))*(sj2)))+(((x4074)*(x4075)))+(((IkReal(-1.00000000000000))*(r11)*(x4081)))+(((IkReal(-1.00000000000000))*(cj6)*(x4075)*(x4080)))+(((r12)*(x4068)))+(((IkReal(-1.00000000000000))*(x4077)*(x4080))));
18243 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
18244 {
18245 {
18246 IkReal j3array[1], cj3array[1], sj3array[1];
18247 bool j3valid[1]={false};
18248 _nj3 = 1;
18249 IkReal x4082=((IkReal(1.00000000000000))*(cj6));
18250 IkReal x4083=((IkReal(1.00000000000000))*(cj5));
18251 IkReal x4084=((r01)*(sj6));
18252 if( IKabs(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x4082)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x4083)*(x4084))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(r02)*(x4083)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x4082)))+(((sj5)*(x4084))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x4082)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x4083)*(x4084)))))+IKsqr(((((IkReal(-1.00000000000000))*(r02)*(x4083)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x4082)))+(((sj5)*(x4084)))))-1) <= IKFAST_SINCOS_THRESH )
18253     continue;
18254 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(r01)*(sj4)*(x4082)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(sj5)))+(((IkReal(-1.00000000000000))*(r00)*(sj4)*(sj6)))+(((cj4)*(cj5)*(cj6)*(r00)))+(((IkReal(-1.00000000000000))*(cj4)*(x4083)*(x4084)))), ((((IkReal(-1.00000000000000))*(r02)*(x4083)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x4082)))+(((sj5)*(x4084)))));
18255 sj3array[0]=IKsin(j3array[0]);
18256 cj3array[0]=IKcos(j3array[0]);
18257 if( j3array[0] > IKPI )
18258 {
18259     j3array[0]-=IK2PI;
18260 }
18261 else if( j3array[0] < -IKPI )
18262 {    j3array[0]+=IK2PI;
18263 }
18264 j3valid[0] = true;
18265 for(int ij3 = 0; ij3 < 1; ++ij3)
18266 {
18267 if( !j3valid[ij3] )
18268 {
18269     continue;
18270 }
18271 _ij3[0] = ij3; _ij3[1] = -1;
18272 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18273 {
18274 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18275 {
18276     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18277 }
18278 }
18279 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18280 {
18281 IkReal evalcond[6];
18282 IkReal x4085=IKsin(j3);
18283 IkReal x4086=IKcos(j3);
18284 IkReal x4087=((r11)*(sj6));
18285 IkReal x4088=((IkReal(1.00000000000000))*(sj5));
18286 IkReal x4089=((sj4)*(sj6));
18287 IkReal x4090=((cj4)*(sj5));
18288 IkReal x4091=((cj6)*(sj4));
18289 IkReal x4092=((cj4)*(cj5));
18290 IkReal x4093=((r01)*(sj6));
18291 IkReal x4094=((r21)*(sj6));
18292 IkReal x4095=((cj6)*(sj5));
18293 IkReal x4096=((IkReal(1.00000000000000))*(cj6)*(x4092));
18294 evalcond[0]=((((r20)*(x4095)))+(((cj5)*(r22)))+(((sj2)*(x4085)))+(((IkReal(-1.00000000000000))*(x4088)*(x4094))));
18295 evalcond[1]=((((r00)*(x4095)))+(x4086)+(((IkReal(-1.00000000000000))*(x4088)*(x4093)))+(((cj5)*(r02))));
18296 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x4095)))+(((IkReal(-1.00000000000000))*(cj2)*(x4085)))+(((IkReal(-1.00000000000000))*(x4087)*(x4088))));
18297 evalcond[3]=((((r20)*(x4089)))+(((IkReal(-1.00000000000000))*(r20)*(x4096)))+(((IkReal(-1.00000000000000))*(sj2)*(x4086)))+(((r21)*(x4091)))+(((r22)*(x4090)))+(((x4092)*(x4094))));
18298 evalcond[4]=((((r00)*(x4089)))+(((r01)*(x4091)))+(x4085)+(((IkReal(-1.00000000000000))*(r00)*(x4096)))+(((r02)*(x4090)))+(((x4092)*(x4093))));
18299 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x4096)))+(((x4087)*(x4092)))+(((r10)*(x4089)))+(((r11)*(x4091)))+(((cj2)*(x4086)))+(((r12)*(x4090))));
18300 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18301 {
18302 continue;
18303 }
18304 }
18305 
18306 {
18307 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18308 vinfos[0].jointtype = 1;
18309 vinfos[0].foffset = j0;
18310 vinfos[0].indices[0] = _ij0[0];
18311 vinfos[0].indices[1] = _ij0[1];
18312 vinfos[0].maxsolutions = _nj0;
18313 vinfos[1].jointtype = 1;
18314 vinfos[1].foffset = j1;
18315 vinfos[1].indices[0] = _ij1[0];
18316 vinfos[1].indices[1] = _ij1[1];
18317 vinfos[1].maxsolutions = _nj1;
18318 vinfos[2].jointtype = 1;
18319 vinfos[2].foffset = j2;
18320 vinfos[2].indices[0] = _ij2[0];
18321 vinfos[2].indices[1] = _ij2[1];
18322 vinfos[2].maxsolutions = _nj2;
18323 vinfos[3].jointtype = 1;
18324 vinfos[3].foffset = j3;
18325 vinfos[3].indices[0] = _ij3[0];
18326 vinfos[3].indices[1] = _ij3[1];
18327 vinfos[3].maxsolutions = _nj3;
18328 vinfos[4].jointtype = 1;
18329 vinfos[4].foffset = j4;
18330 vinfos[4].indices[0] = _ij4[0];
18331 vinfos[4].indices[1] = _ij4[1];
18332 vinfos[4].maxsolutions = _nj4;
18333 vinfos[5].jointtype = 1;
18334 vinfos[5].foffset = j5;
18335 vinfos[5].indices[0] = _ij5[0];
18336 vinfos[5].indices[1] = _ij5[1];
18337 vinfos[5].maxsolutions = _nj5;
18338 vinfos[6].jointtype = 1;
18339 vinfos[6].foffset = j6;
18340 vinfos[6].indices[0] = _ij6[0];
18341 vinfos[6].indices[1] = _ij6[1];
18342 vinfos[6].maxsolutions = _nj6;
18343 std::vector<int> vfree(0);
18344 solutions.AddSolution(vinfos,vfree);
18345 }
18346 }
18347 }
18348 
18349 } else
18350 {
18351 IkReal x4097=((sj4)*(sj5));
18352 IkReal x4098=((IkReal(0.390000000000000))*(sj5));
18353 IkReal x4099=((r21)*(sj6));
18354 IkReal x4100=((IkReal(0.390000000000000))*(cj5));
18355 IkReal x4101=((cj6)*(r00));
18356 IkReal x4102=((IkReal(0.000500000000000000))*(cj6));
18357 IkReal x4103=((r11)*(sj6));
18358 IkReal x4104=((cj5)*(sj4));
18359 IkReal x4105=((IkReal(1.00000000000000))*(r20));
18360 IkReal x4106=((cj4)*(sj6));
18361 IkReal x4107=((r01)*(sj6));
18362 IkReal x4108=((IkReal(0.000500000000000000))*(sj6));
18363 IkReal x4109=((IkReal(1.00000000000000))*(r10));
18364 IkReal x4110=((IkReal(1.00000000000000))*(cj4)*(cj6));
18365 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959))));
18366 evalcond[1]=((((r22)*(x4100)))+(((IkReal(-1.00000000000000))*(x4098)*(x4099)))+(((r20)*(x4108)))+(((IkReal(-1.00000000000000))*(pz)))+(((cj6)*(r20)*(x4098)))+(((r21)*(x4102)))+(((IkReal(-0.00200000000000000))*(cj2))));
18367 evalcond[2]=((IkReal(0.400000000000000))+(((IkReal(-1.00000000000000))*(x4098)*(x4107)))+(((x4098)*(x4101)))+(((IkReal(-1.00000000000000))*(px)))+(((r02)*(x4100)))+(((r00)*(x4108)))+(((r01)*(x4102))));
18368 evalcond[3]=((((IkReal(-1.00000000000000))*(x4098)*(x4103)))+(((cj6)*(r10)*(x4098)))+(((IkReal(0.00200000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x4100)))+(((r11)*(x4102)))+(((r10)*(x4108))));
18369 evalcond[4]=((((x4099)*(x4104)))+(((IkReal(-1.00000000000000))*(x4105)*(x4106)))+(((IkReal(-1.00000000000000))*(cj6)*(x4104)*(x4105)))+(((r22)*(x4097)))+(((IkReal(-1.00000000000000))*(r21)*(x4110)))+(((IkReal(-1.00000000000000))*(cj2))));
18370 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x4110)))+(((IkReal(-1.00000000000000))*(x4101)*(x4104)))+(((r02)*(x4097)))+(((IkReal(-1.00000000000000))*(r00)*(x4106)))+(((x4104)*(x4107))));
18371 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x4110)))+(sj2)+(((IkReal(-1.00000000000000))*(cj6)*(x4104)*(x4109)))+(((x4103)*(x4104)))+(((IkReal(-1.00000000000000))*(x4106)*(x4109)))+(((r12)*(x4097))));
18372 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
18373 {
18374 {
18375 IkReal j3array[1], cj3array[1], sj3array[1];
18376 bool j3valid[1]={false};
18377 _nj3 = 1;
18378 IkReal x4111=((cj4)*(cj5));
18379 IkReal x4112=((r01)*(sj6));
18380 IkReal x4113=((cj6)*(r00));
18381 if( IKabs(((((IkReal(-1.00000000000000))*(x4111)*(x4113)))+(((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((x4111)*(x4112))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((sj5)*(x4113)))+(((IkReal(-1.00000000000000))*(sj5)*(x4112)))+(((cj5)*(r02))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(x4111)*(x4113)))+(((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((x4111)*(x4112)))))+IKsqr(((((sj5)*(x4113)))+(((IkReal(-1.00000000000000))*(sj5)*(x4112)))+(((cj5)*(r02)))))-1) <= IKFAST_SINCOS_THRESH )
18382     continue;
18383 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(x4111)*(x4113)))+(((r00)*(sj4)*(sj6)))+(((cj6)*(r01)*(sj4)))+(((cj4)*(r02)*(sj5)))+(((x4111)*(x4112)))), ((((sj5)*(x4113)))+(((IkReal(-1.00000000000000))*(sj5)*(x4112)))+(((cj5)*(r02)))));
18384 sj3array[0]=IKsin(j3array[0]);
18385 cj3array[0]=IKcos(j3array[0]);
18386 if( j3array[0] > IKPI )
18387 {
18388     j3array[0]-=IK2PI;
18389 }
18390 else if( j3array[0] < -IKPI )
18391 {    j3array[0]+=IK2PI;
18392 }
18393 j3valid[0] = true;
18394 for(int ij3 = 0; ij3 < 1; ++ij3)
18395 {
18396 if( !j3valid[ij3] )
18397 {
18398     continue;
18399 }
18400 _ij3[0] = ij3; _ij3[1] = -1;
18401 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18402 {
18403 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18404 {
18405     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18406 }
18407 }
18408 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18409 {
18410 IkReal evalcond[6];
18411 IkReal x4114=IKsin(j3);
18412 IkReal x4115=IKcos(j3);
18413 IkReal x4116=((r11)*(sj6));
18414 IkReal x4117=((IkReal(1.00000000000000))*(sj5));
18415 IkReal x4118=((cj6)*(sj4));
18416 IkReal x4119=((sj4)*(sj6));
18417 IkReal x4120=((cj4)*(sj5));
18418 IkReal x4121=((cj4)*(cj5));
18419 IkReal x4122=((r01)*(sj6));
18420 IkReal x4123=((r21)*(sj6));
18421 IkReal x4124=((cj6)*(sj5));
18422 IkReal x4125=((IkReal(1.00000000000000))*(x4115));
18423 IkReal x4126=((IkReal(1.00000000000000))*(cj6)*(x4121));
18424 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x4124)))+(((sj2)*(x4114)))+(((IkReal(-1.00000000000000))*(x4117)*(x4123))));
18425 evalcond[1]=((((IkReal(-1.00000000000000))*(x4125)))+(((r00)*(x4124)))+(((IkReal(-1.00000000000000))*(x4117)*(x4122)))+(((cj5)*(r02))));
18426 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x4116)*(x4117)))+(((cj2)*(x4114)))+(((r10)*(x4124))));
18427 evalcond[3]=((((r21)*(x4118)))+(((r20)*(x4119)))+(((IkReal(-1.00000000000000))*(sj2)*(x4125)))+(((IkReal(-1.00000000000000))*(r20)*(x4126)))+(((x4121)*(x4123)))+(((r22)*(x4120))));
18428 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x4126)))+(((r02)*(x4120)))+(((x4121)*(x4122)))+(((r00)*(x4119)))+(((IkReal(-1.00000000000000))*(x4114)))+(((r01)*(x4118))));
18429 evalcond[5]=((((r12)*(x4120)))+(((IkReal(-1.00000000000000))*(cj2)*(x4125)))+(((x4116)*(x4121)))+(((IkReal(-1.00000000000000))*(r10)*(x4126)))+(((r11)*(x4118)))+(((r10)*(x4119))));
18430 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18431 {
18432 continue;
18433 }
18434 }
18435 
18436 {
18437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18438 vinfos[0].jointtype = 1;
18439 vinfos[0].foffset = j0;
18440 vinfos[0].indices[0] = _ij0[0];
18441 vinfos[0].indices[1] = _ij0[1];
18442 vinfos[0].maxsolutions = _nj0;
18443 vinfos[1].jointtype = 1;
18444 vinfos[1].foffset = j1;
18445 vinfos[1].indices[0] = _ij1[0];
18446 vinfos[1].indices[1] = _ij1[1];
18447 vinfos[1].maxsolutions = _nj1;
18448 vinfos[2].jointtype = 1;
18449 vinfos[2].foffset = j2;
18450 vinfos[2].indices[0] = _ij2[0];
18451 vinfos[2].indices[1] = _ij2[1];
18452 vinfos[2].maxsolutions = _nj2;
18453 vinfos[3].jointtype = 1;
18454 vinfos[3].foffset = j3;
18455 vinfos[3].indices[0] = _ij3[0];
18456 vinfos[3].indices[1] = _ij3[1];
18457 vinfos[3].maxsolutions = _nj3;
18458 vinfos[4].jointtype = 1;
18459 vinfos[4].foffset = j4;
18460 vinfos[4].indices[0] = _ij4[0];
18461 vinfos[4].indices[1] = _ij4[1];
18462 vinfos[4].maxsolutions = _nj4;
18463 vinfos[5].jointtype = 1;
18464 vinfos[5].foffset = j5;
18465 vinfos[5].indices[0] = _ij5[0];
18466 vinfos[5].indices[1] = _ij5[1];
18467 vinfos[5].maxsolutions = _nj5;
18468 vinfos[6].jointtype = 1;
18469 vinfos[6].foffset = j6;
18470 vinfos[6].indices[0] = _ij6[0];
18471 vinfos[6].indices[1] = _ij6[1];
18472 vinfos[6].maxsolutions = _nj6;
18473 std::vector<int> vfree(0);
18474 solutions.AddSolution(vinfos,vfree);
18475 }
18476 }
18477 }
18478 
18479 } else
18480 {
18481 IkReal x4127=((sj4)*(sj5));
18482 IkReal x4128=((IkReal(0.390000000000000))*(sj5));
18483 IkReal x4129=((r21)*(sj6));
18484 IkReal x4130=((IkReal(0.390000000000000))*(cj5));
18485 IkReal x4131=((cj6)*(r00));
18486 IkReal x4132=((IkReal(0.000500000000000000))*(cj6));
18487 IkReal x4133=((r11)*(sj6));
18488 IkReal x4134=((cj5)*(sj4));
18489 IkReal x4135=((r01)*(sj6));
18490 IkReal x4136=((IkReal(1.00000000000000))*(r20));
18491 IkReal x4137=((cj4)*(sj6));
18492 IkReal x4138=((IkReal(0.000500000000000000))*(sj6));
18493 IkReal x4139=((IkReal(1.00000000000000))*(r10));
18494 IkReal x4140=((IkReal(1.00000000000000))*(cj4)*(cj6));
18495 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
18496 evalcond[1]=((((IkReal(-1.00000000000000))*(x4128)*(x4129)))+(((r22)*(x4130)))+(((cj6)*(r20)*(x4128)))+(((r21)*(x4132)))+(((IkReal(-1.00000000000000))*(pz)))+(((r20)*(x4138))));
18497 evalcond[2]=((((r00)*(x4138)))+(((x4128)*(x4131)))+(((r01)*(x4132)))+(((IkReal(-1.00000000000000))*(x4128)*(x4135)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-0.00200000000000000))*(cj1)))+(((IkReal(-0.400000000000000))*(sj1)))+(((r02)*(x4130))));
18498 evalcond[3]=((((cj6)*(r10)*(x4128)))+(((r11)*(x4132)))+(((IkReal(0.400000000000000))*(cj1)))+(((r10)*(x4138)))+(((IkReal(-1.00000000000000))*(x4128)*(x4133)))+(((IkReal(-1.00000000000000))*(py)))+(((r12)*(x4130)))+(((IkReal(-0.00200000000000000))*(sj1))));
18499 evalcond[4]=((((x4129)*(x4134)))+(((r22)*(x4127)))+(((IkReal(-1.00000000000000))*(x4136)*(x4137)))+(((IkReal(-1.00000000000000))*(cj6)*(x4134)*(x4136)))+(((IkReal(-1.00000000000000))*(r21)*(x4140))));
18500 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x4137)))+(((r02)*(x4127)))+(((IkReal(-1.00000000000000))*(r01)*(x4140)))+(((x4134)*(x4135)))+(((IkReal(-1.00000000000000))*(x4131)*(x4134)))+(((IkReal(-1.00000000000000))*(cj1))));
18501 evalcond[6]=((((r12)*(x4127)))+(((IkReal(-1.00000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x4137)*(x4139)))+(((IkReal(-1.00000000000000))*(r11)*(x4140)))+(((x4133)*(x4134)))+(((IkReal(-1.00000000000000))*(cj6)*(x4134)*(x4139))));
18502 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
18503 {
18504 {
18505 IkReal j3array[1], cj3array[1], sj3array[1];
18506 bool j3valid[1]={false};
18507 _nj3 = 1;
18508 IkReal x4141=((cj4)*(cj5));
18509 IkReal x4142=((r21)*(sj6));
18510 IkReal x4143=((IkReal(1.00000000000000))*(cj6)*(r20));
18511 if( IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x4143)))+(((sj5)*(x4142)))+(((IkReal(-1.00000000000000))*(cj5)*(r22))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(x4141)*(x4143)))+(((cj4)*(r22)*(sj5)))+(((x4141)*(x4142)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x4143)))+(((sj5)*(x4142)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))))+IKsqr(((((IkReal(-1.00000000000000))*(x4141)*(x4143)))+(((cj4)*(r22)*(sj5)))+(((x4141)*(x4142)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))))-1) <= IKFAST_SINCOS_THRESH )
18512     continue;
18513 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(sj5)*(x4143)))+(((sj5)*(x4142)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))), ((((IkReal(-1.00000000000000))*(x4141)*(x4143)))+(((cj4)*(r22)*(sj5)))+(((x4141)*(x4142)))+(((cj6)*(r21)*(sj4)))+(((r20)*(sj4)*(sj6)))));
18514 sj3array[0]=IKsin(j3array[0]);
18515 cj3array[0]=IKcos(j3array[0]);
18516 if( j3array[0] > IKPI )
18517 {
18518     j3array[0]-=IK2PI;
18519 }
18520 else if( j3array[0] < -IKPI )
18521 {    j3array[0]+=IK2PI;
18522 }
18523 j3valid[0] = true;
18524 for(int ij3 = 0; ij3 < 1; ++ij3)
18525 {
18526 if( !j3valid[ij3] )
18527 {
18528     continue;
18529 }
18530 _ij3[0] = ij3; _ij3[1] = -1;
18531 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18532 {
18533 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18534 {
18535     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18536 }
18537 }
18538 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18539 {
18540 IkReal evalcond[6];
18541 IkReal x4144=IKcos(j3);
18542 IkReal x4145=IKsin(j3);
18543 IkReal x4146=((r11)*(sj6));
18544 IkReal x4147=((IkReal(1.00000000000000))*(sj5));
18545 IkReal x4148=((IkReal(1.00000000000000))*(cj1));
18546 IkReal x4149=((cj6)*(sj4));
18547 IkReal x4150=((sj4)*(sj6));
18548 IkReal x4151=((cj4)*(sj5));
18549 IkReal x4152=((cj4)*(cj5));
18550 IkReal x4153=((r01)*(sj6));
18551 IkReal x4154=((r21)*(sj6));
18552 IkReal x4155=((cj6)*(sj5));
18553 IkReal x4156=((IkReal(1.00000000000000))*(cj6)*(x4152));
18554 evalcond[0]=((((cj5)*(r22)))+(((IkReal(-1.00000000000000))*(x4147)*(x4154)))+(x4145)+(((r20)*(x4155))));
18555 evalcond[1]=((((r00)*(x4155)))+(((IkReal(-1.00000000000000))*(x4147)*(x4153)))+(((sj1)*(x4144)))+(((cj5)*(r02))));
18556 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x4146)*(x4147)))+(((r10)*(x4155)))+(((IkReal(-1.00000000000000))*(x4144)*(x4148))));
18557 evalcond[3]=((((x4152)*(x4154)))+(((r21)*(x4149)))+(((IkReal(-1.00000000000000))*(x4144)))+(((r22)*(x4151)))+(((r20)*(x4150)))+(((IkReal(-1.00000000000000))*(r20)*(x4156))));
18558 evalcond[4]=((((r02)*(x4151)))+(((x4152)*(x4153)))+(((r00)*(x4150)))+(((r01)*(x4149)))+(((sj1)*(x4145)))+(((IkReal(-1.00000000000000))*(r00)*(x4156))));
18559 evalcond[5]=((((r10)*(x4150)))+(((r12)*(x4151)))+(((r11)*(x4149)))+(((IkReal(-1.00000000000000))*(x4145)*(x4148)))+(((IkReal(-1.00000000000000))*(r10)*(x4156)))+(((x4146)*(x4152))));
18560 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18561 {
18562 continue;
18563 }
18564 }
18565 
18566 {
18567 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18568 vinfos[0].jointtype = 1;
18569 vinfos[0].foffset = j0;
18570 vinfos[0].indices[0] = _ij0[0];
18571 vinfos[0].indices[1] = _ij0[1];
18572 vinfos[0].maxsolutions = _nj0;
18573 vinfos[1].jointtype = 1;
18574 vinfos[1].foffset = j1;
18575 vinfos[1].indices[0] = _ij1[0];
18576 vinfos[1].indices[1] = _ij1[1];
18577 vinfos[1].maxsolutions = _nj1;
18578 vinfos[2].jointtype = 1;
18579 vinfos[2].foffset = j2;
18580 vinfos[2].indices[0] = _ij2[0];
18581 vinfos[2].indices[1] = _ij2[1];
18582 vinfos[2].maxsolutions = _nj2;
18583 vinfos[3].jointtype = 1;
18584 vinfos[3].foffset = j3;
18585 vinfos[3].indices[0] = _ij3[0];
18586 vinfos[3].indices[1] = _ij3[1];
18587 vinfos[3].maxsolutions = _nj3;
18588 vinfos[4].jointtype = 1;
18589 vinfos[4].foffset = j4;
18590 vinfos[4].indices[0] = _ij4[0];
18591 vinfos[4].indices[1] = _ij4[1];
18592 vinfos[4].maxsolutions = _nj4;
18593 vinfos[5].jointtype = 1;
18594 vinfos[5].foffset = j5;
18595 vinfos[5].indices[0] = _ij5[0];
18596 vinfos[5].indices[1] = _ij5[1];
18597 vinfos[5].maxsolutions = _nj5;
18598 vinfos[6].jointtype = 1;
18599 vinfos[6].foffset = j6;
18600 vinfos[6].indices[0] = _ij6[0];
18601 vinfos[6].indices[1] = _ij6[1];
18602 vinfos[6].maxsolutions = _nj6;
18603 std::vector<int> vfree(0);
18604 solutions.AddSolution(vinfos,vfree);
18605 }
18606 }
18607 }
18608 
18609 } else
18610 {
18611 IkReal x4157=((sj4)*(sj5));
18612 IkReal x4158=((IkReal(0.390000000000000))*(sj5));
18613 IkReal x4159=((r21)*(sj6));
18614 IkReal x4160=((IkReal(0.390000000000000))*(cj5));
18615 IkReal x4161=((cj6)*(r00));
18616 IkReal x4162=((IkReal(0.000500000000000000))*(cj6));
18617 IkReal x4163=((r11)*(sj6));
18618 IkReal x4164=((cj5)*(sj4));
18619 IkReal x4165=((r01)*(sj6));
18620 IkReal x4166=((IkReal(1.00000000000000))*(r20));
18621 IkReal x4167=((cj4)*(sj6));
18622 IkReal x4168=((IkReal(0.000500000000000000))*(sj6));
18623 IkReal x4169=((IkReal(1.00000000000000))*(r10));
18624 IkReal x4170=((IkReal(1.00000000000000))*(cj4)*(cj6));
18625 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
18626 evalcond[1]=((((r22)*(x4160)))+(((r20)*(x4168)))+(((IkReal(-1.00000000000000))*(pz)))+(((r21)*(x4162)))+(((cj6)*(r20)*(x4158)))+(((IkReal(-1.00000000000000))*(x4158)*(x4159))));
18627 evalcond[2]=((((r01)*(x4162)))+(((x4158)*(x4161)))+(((r00)*(x4168)))+(((IkReal(0.00200000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(px)))+(((IkReal(-1.00000000000000))*(x4158)*(x4165)))+(((r02)*(x4160)))+(((IkReal(-0.400000000000000))*(sj1))));
18628 evalcond[3]=((((r10)*(x4168)))+(((IkReal(0.400000000000000))*(cj1)))+(((IkReal(0.00200000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(py)))+(((IkReal(-1.00000000000000))*(x4158)*(x4163)))+(((r11)*(x4162)))+(((r12)*(x4160)))+(((cj6)*(r10)*(x4158))));
18629 evalcond[4]=((((r22)*(x4157)))+(((IkReal(-1.00000000000000))*(r21)*(x4170)))+(((IkReal(-1.00000000000000))*(cj6)*(x4164)*(x4166)))+(((x4159)*(x4164)))+(((IkReal(-1.00000000000000))*(x4166)*(x4167))));
18630 evalcond[5]=((((r02)*(x4157)))+(cj1)+(((IkReal(-1.00000000000000))*(r01)*(x4170)))+(((IkReal(-1.00000000000000))*(x4161)*(x4164)))+(((IkReal(-1.00000000000000))*(r00)*(x4167)))+(((x4164)*(x4165))));
18631 evalcond[6]=((sj1)+(((r12)*(x4157)))+(((IkReal(-1.00000000000000))*(cj6)*(x4164)*(x4169)))+(((IkReal(-1.00000000000000))*(x4167)*(x4169)))+(((x4163)*(x4164)))+(((IkReal(-1.00000000000000))*(r11)*(x4170))));
18632 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
18633 {
18634 {
18635 IkReal j3array[1], cj3array[1], sj3array[1];
18636 bool j3valid[1]={false};
18637 _nj3 = 1;
18638 IkReal x4171=((cj4)*(cj5));
18639 IkReal x4172=((cj6)*(r20));
18640 IkReal x4173=((IkReal(1.00000000000000))*(sj4));
18641 IkReal x4174=((IkReal(1.00000000000000))*(r21)*(sj6));
18642 if( IKabs(((((IkReal(-1.00000000000000))*(sj5)*(x4174)))+(((cj5)*(r22)))+(((sj5)*(x4172))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x4173)))+(((IkReal(-1.00000000000000))*(x4171)*(x4174)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x4171)*(x4172)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x4173))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(sj5)*(x4174)))+(((cj5)*(r22)))+(((sj5)*(x4172)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x4173)))+(((IkReal(-1.00000000000000))*(x4171)*(x4174)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x4171)*(x4172)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x4173)))))-1) <= IKFAST_SINCOS_THRESH )
18643     continue;
18644 j3array[0]=IKatan2(((((IkReal(-1.00000000000000))*(sj5)*(x4174)))+(((cj5)*(r22)))+(((sj5)*(x4172)))), ((((IkReal(-1.00000000000000))*(cj6)*(r21)*(x4173)))+(((IkReal(-1.00000000000000))*(x4171)*(x4174)))+(((IkReal(-1.00000000000000))*(cj4)*(r22)*(sj5)))+(((x4171)*(x4172)))+(((IkReal(-1.00000000000000))*(r20)*(sj6)*(x4173)))));
18645 sj3array[0]=IKsin(j3array[0]);
18646 cj3array[0]=IKcos(j3array[0]);
18647 if( j3array[0] > IKPI )
18648 {
18649     j3array[0]-=IK2PI;
18650 }
18651 else if( j3array[0] < -IKPI )
18652 {    j3array[0]+=IK2PI;
18653 }
18654 j3valid[0] = true;
18655 for(int ij3 = 0; ij3 < 1; ++ij3)
18656 {
18657 if( !j3valid[ij3] )
18658 {
18659     continue;
18660 }
18661 _ij3[0] = ij3; _ij3[1] = -1;
18662 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18663 {
18664 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18665 {
18666     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18667 }
18668 }
18669 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18670 {
18671 IkReal evalcond[6];
18672 IkReal x4175=IKcos(j3);
18673 IkReal x4176=IKsin(j3);
18674 IkReal x4177=((r11)*(sj6));
18675 IkReal x4178=((IkReal(1.00000000000000))*(sj5));
18676 IkReal x4179=((IkReal(1.00000000000000))*(cj1));
18677 IkReal x4180=((sj4)*(sj6));
18678 IkReal x4181=((cj4)*(sj5));
18679 IkReal x4182=((cj6)*(sj4));
18680 IkReal x4183=((cj4)*(cj5));
18681 IkReal x4184=((r01)*(sj6));
18682 IkReal x4185=((r21)*(sj6));
18683 IkReal x4186=((cj6)*(sj5));
18684 IkReal x4187=((IkReal(1.00000000000000))*(cj6)*(x4183));
18685 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x4186)))+(((IkReal(-1.00000000000000))*(x4176)))+(((IkReal(-1.00000000000000))*(x4178)*(x4185))));
18686 evalcond[1]=((((sj1)*(x4175)))+(((r00)*(x4186)))+(((IkReal(-1.00000000000000))*(x4178)*(x4184)))+(((cj5)*(r02))));
18687 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(x4177)*(x4178)))+(((IkReal(-1.00000000000000))*(x4175)*(x4179)))+(((r10)*(x4186))));
18688 evalcond[3]=((((r20)*(x4180)))+(((r21)*(x4182)))+(((IkReal(-1.00000000000000))*(r20)*(x4187)))+(((r22)*(x4181)))+(((x4183)*(x4185)))+(x4175));
18689 evalcond[4]=((((r02)*(x4181)))+(((IkReal(-1.00000000000000))*(r00)*(x4187)))+(((sj1)*(x4176)))+(((x4183)*(x4184)))+(((r01)*(x4182)))+(((r00)*(x4180))));
18690 evalcond[5]=((((r12)*(x4181)))+(((r11)*(x4182)))+(((IkReal(-1.00000000000000))*(x4176)*(x4179)))+(((x4177)*(x4183)))+(((r10)*(x4180)))+(((IkReal(-1.00000000000000))*(r10)*(x4187))));
18691 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18692 {
18693 continue;
18694 }
18695 }
18696 
18697 {
18698 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18699 vinfos[0].jointtype = 1;
18700 vinfos[0].foffset = j0;
18701 vinfos[0].indices[0] = _ij0[0];
18702 vinfos[0].indices[1] = _ij0[1];
18703 vinfos[0].maxsolutions = _nj0;
18704 vinfos[1].jointtype = 1;
18705 vinfos[1].foffset = j1;
18706 vinfos[1].indices[0] = _ij1[0];
18707 vinfos[1].indices[1] = _ij1[1];
18708 vinfos[1].maxsolutions = _nj1;
18709 vinfos[2].jointtype = 1;
18710 vinfos[2].foffset = j2;
18711 vinfos[2].indices[0] = _ij2[0];
18712 vinfos[2].indices[1] = _ij2[1];
18713 vinfos[2].maxsolutions = _nj2;
18714 vinfos[3].jointtype = 1;
18715 vinfos[3].foffset = j3;
18716 vinfos[3].indices[0] = _ij3[0];
18717 vinfos[3].indices[1] = _ij3[1];
18718 vinfos[3].maxsolutions = _nj3;
18719 vinfos[4].jointtype = 1;
18720 vinfos[4].foffset = j4;
18721 vinfos[4].indices[0] = _ij4[0];
18722 vinfos[4].indices[1] = _ij4[1];
18723 vinfos[4].maxsolutions = _nj4;
18724 vinfos[5].jointtype = 1;
18725 vinfos[5].foffset = j5;
18726 vinfos[5].indices[0] = _ij5[0];
18727 vinfos[5].indices[1] = _ij5[1];
18728 vinfos[5].maxsolutions = _nj5;
18729 vinfos[6].jointtype = 1;
18730 vinfos[6].foffset = j6;
18731 vinfos[6].indices[0] = _ij6[0];
18732 vinfos[6].indices[1] = _ij6[1];
18733 vinfos[6].maxsolutions = _nj6;
18734 std::vector<int> vfree(0);
18735 solutions.AddSolution(vinfos,vfree);
18736 }
18737 }
18738 }
18739 
18740 } else
18741 {
18742 if( 1 )
18743 {
18744 continue;
18745 
18746 } else
18747 {
18748 }
18749 }
18750 }
18751 }
18752 }
18753 }
18754 }
18755 }
18756 
18757 } else
18758 {
18759 {
18760 IkReal j3array[1], cj3array[1], sj3array[1];
18761 bool j3valid[1]={false};
18762 _nj3 = 1;
18763 IkReal x4188=((IKabs(sj2) != 0)?((IkReal)1/(sj2)):(IkReal)1.0e30);
18764 IkReal x4189=((IkReal(1.00000000000000))*(sj5));
18765 IkReal x4190=((cj2)*(sj1));
18766 IkReal x4191=((r21)*(sj6));
18767 IkReal x4192=((cj6)*(r20));
18768 IkReal x4193=((cj5)*(r22));
18769 if( IKabs(((x4188)*(((((sj5)*(x4191)))+(((IkReal(-1.00000000000000))*(x4193)))+(((IkReal(-1.00000000000000))*(x4189)*(x4192))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x4188)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x4190)*(x4193)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x4189)))+(((IkReal(-1.00000000000000))*(x4189)*(x4190)*(x4191)))+(((sj5)*(x4190)*(x4192)))+(((cj6)*(r10)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x4188)*(((((sj5)*(x4191)))+(((IkReal(-1.00000000000000))*(x4193)))+(((IkReal(-1.00000000000000))*(x4189)*(x4192)))))))+IKsqr(((x4188)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x4190)*(x4193)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x4189)))+(((IkReal(-1.00000000000000))*(x4189)*(x4190)*(x4191)))+(((sj5)*(x4190)*(x4192)))+(((cj6)*(r10)*(sj2)*(sj5)))))))-1) <= IKFAST_SINCOS_THRESH )
18770     continue;
18771 j3array[0]=IKatan2(((x4188)*(((((sj5)*(x4191)))+(((IkReal(-1.00000000000000))*(x4193)))+(((IkReal(-1.00000000000000))*(x4189)*(x4192)))))), ((x4188)*(((IKabs(cj1) != 0)?((IkReal)1/(cj1)):(IkReal)1.0e30))*(((((cj5)*(r12)*(sj2)))+(((x4190)*(x4193)))+(((IkReal(-1.00000000000000))*(r11)*(sj2)*(sj6)*(x4189)))+(((IkReal(-1.00000000000000))*(x4189)*(x4190)*(x4191)))+(((sj5)*(x4190)*(x4192)))+(((cj6)*(r10)*(sj2)*(sj5)))))));
18772 sj3array[0]=IKsin(j3array[0]);
18773 cj3array[0]=IKcos(j3array[0]);
18774 if( j3array[0] > IKPI )
18775 {
18776     j3array[0]-=IK2PI;
18777 }
18778 else if( j3array[0] < -IKPI )
18779 {    j3array[0]+=IK2PI;
18780 }
18781 j3valid[0] = true;
18782 for(int ij3 = 0; ij3 < 1; ++ij3)
18783 {
18784 if( !j3valid[ij3] )
18785 {
18786     continue;
18787 }
18788 _ij3[0] = ij3; _ij3[1] = -1;
18789 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18790 {
18791 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18792 {
18793     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18794 }
18795 }
18796 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18797 {
18798 IkReal evalcond[6];
18799 IkReal x4194=IKsin(j3);
18800 IkReal x4195=IKcos(j3);
18801 IkReal x4196=((r11)*(sj6));
18802 IkReal x4197=((IkReal(1.00000000000000))*(sj5));
18803 IkReal x4198=((sj4)*(sj6));
18804 IkReal x4199=((cj4)*(sj5));
18805 IkReal x4200=((cj6)*(sj4));
18806 IkReal x4201=((cj4)*(cj5));
18807 IkReal x4202=((r01)*(sj6));
18808 IkReal x4203=((r21)*(sj6));
18809 IkReal x4204=((cj6)*(sj5));
18810 IkReal x4205=((IkReal(1.00000000000000))*(x4195));
18811 IkReal x4206=((sj1)*(x4195));
18812 IkReal x4207=((IkReal(1.00000000000000))*(cj6)*(x4201));
18813 IkReal x4208=((IkReal(1.00000000000000))*(cj2)*(x4194));
18814 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x4194)))+(((IkReal(-1.00000000000000))*(x4197)*(x4203)))+(((r20)*(x4204))));
18815 evalcond[1]=((((IkReal(-1.00000000000000))*(x4197)*(x4202)))+(((r00)*(x4204)))+(((IkReal(-1.00000000000000))*(cj1)*(x4208)))+(x4206)+(((cj5)*(r02))));
18816 evalcond[2]=((((cj5)*(r12)))+(((IkReal(-1.00000000000000))*(cj1)*(x4205)))+(((IkReal(-1.00000000000000))*(sj1)*(x4208)))+(((IkReal(-1.00000000000000))*(x4196)*(x4197)))+(((r10)*(x4204))));
18817 evalcond[3]=((((r20)*(x4198)))+(((IkReal(-1.00000000000000))*(sj2)*(x4205)))+(((r22)*(x4199)))+(((x4201)*(x4203)))+(((r21)*(x4200)))+(((IkReal(-1.00000000000000))*(r20)*(x4207))));
18818 evalcond[4]=((((r02)*(x4199)))+(((sj1)*(x4194)))+(((IkReal(-1.00000000000000))*(r00)*(x4207)))+(((cj1)*(cj2)*(x4195)))+(((r01)*(x4200)))+(((x4201)*(x4202)))+(((r00)*(x4198))));
18819 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x4207)))+(((r11)*(x4200)))+(((r12)*(x4199)))+(((x4196)*(x4201)))+(((cj2)*(x4206)))+(((r10)*(x4198)))+(((IkReal(-1.00000000000000))*(cj1)*(x4194))));
18820 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18821 {
18822 continue;
18823 }
18824 }
18825 
18826 {
18827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18828 vinfos[0].jointtype = 1;
18829 vinfos[0].foffset = j0;
18830 vinfos[0].indices[0] = _ij0[0];
18831 vinfos[0].indices[1] = _ij0[1];
18832 vinfos[0].maxsolutions = _nj0;
18833 vinfos[1].jointtype = 1;
18834 vinfos[1].foffset = j1;
18835 vinfos[1].indices[0] = _ij1[0];
18836 vinfos[1].indices[1] = _ij1[1];
18837 vinfos[1].maxsolutions = _nj1;
18838 vinfos[2].jointtype = 1;
18839 vinfos[2].foffset = j2;
18840 vinfos[2].indices[0] = _ij2[0];
18841 vinfos[2].indices[1] = _ij2[1];
18842 vinfos[2].maxsolutions = _nj2;
18843 vinfos[3].jointtype = 1;
18844 vinfos[3].foffset = j3;
18845 vinfos[3].indices[0] = _ij3[0];
18846 vinfos[3].indices[1] = _ij3[1];
18847 vinfos[3].maxsolutions = _nj3;
18848 vinfos[4].jointtype = 1;
18849 vinfos[4].foffset = j4;
18850 vinfos[4].indices[0] = _ij4[0];
18851 vinfos[4].indices[1] = _ij4[1];
18852 vinfos[4].maxsolutions = _nj4;
18853 vinfos[5].jointtype = 1;
18854 vinfos[5].foffset = j5;
18855 vinfos[5].indices[0] = _ij5[0];
18856 vinfos[5].indices[1] = _ij5[1];
18857 vinfos[5].maxsolutions = _nj5;
18858 vinfos[6].jointtype = 1;
18859 vinfos[6].foffset = j6;
18860 vinfos[6].indices[0] = _ij6[0];
18861 vinfos[6].indices[1] = _ij6[1];
18862 vinfos[6].maxsolutions = _nj6;
18863 std::vector<int> vfree(0);
18864 solutions.AddSolution(vinfos,vfree);
18865 }
18866 }
18867 }
18868 
18869 }
18870 
18871 }
18872 
18873 } else
18874 {
18875 {
18876 IkReal j3array[1], cj3array[1], sj3array[1];
18877 bool j3valid[1]={false};
18878 _nj3 = 1;
18879 IkReal x4209=(sj1)*(sj1);
18880 IkReal x4210=(cj1)*(cj1);
18881 IkReal x4211=((cj5)*(r12));
18882 IkReal x4212=((cj6)*(sj5));
18883 IkReal x4213=((IkReal(1.00000000000000))*(sj1));
18884 IkReal x4214=((cj5)*(r02));
18885 IkReal x4215=((IkReal(1.00000000000000))*(cj1));
18886 IkReal x4216=((r01)*(sj5)*(sj6));
18887 IkReal x4217=((r11)*(sj5)*(sj6));
18888 if( IKabs(((((IKabs(((((cj2)*(x4209)))+(((cj2)*(x4210))))) != 0)?((IkReal)1/(((((cj2)*(x4209)))+(((cj2)*(x4210)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x4212)))+(((IkReal(-1.00000000000000))*(x4213)*(x4217)))+(((cj1)*(r00)*(x4212)))+(((sj1)*(x4211)))+(((IkReal(-1.00000000000000))*(x4215)*(x4216)))+(((cj1)*(x4214))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((x4210)+(x4209))) != 0)?((IkReal)1/(((x4210)+(x4209)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x4213)*(x4214)))+(((sj1)*(x4216)))+(((IkReal(-1.00000000000000))*(x4215)*(x4217)))+(((IkReal(-1.00000000000000))*(r00)*(x4212)*(x4213)))+(((cj1)*(x4211)))+(((cj1)*(r10)*(x4212))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((cj2)*(x4209)))+(((cj2)*(x4210))))) != 0)?((IkReal)1/(((((cj2)*(x4209)))+(((cj2)*(x4210)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x4212)))+(((IkReal(-1.00000000000000))*(x4213)*(x4217)))+(((cj1)*(r00)*(x4212)))+(((sj1)*(x4211)))+(((IkReal(-1.00000000000000))*(x4215)*(x4216)))+(((cj1)*(x4214)))))))+IKsqr(((((IKabs(((x4210)+(x4209))) != 0)?((IkReal)1/(((x4210)+(x4209)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x4213)*(x4214)))+(((sj1)*(x4216)))+(((IkReal(-1.00000000000000))*(x4215)*(x4217)))+(((IkReal(-1.00000000000000))*(r00)*(x4212)*(x4213)))+(((cj1)*(x4211)))+(((cj1)*(r10)*(x4212)))))))-1) <= IKFAST_SINCOS_THRESH )
18889     continue;
18890 j3array[0]=IKatan2(((((IKabs(((((cj2)*(x4209)))+(((cj2)*(x4210))))) != 0)?((IkReal)1/(((((cj2)*(x4209)))+(((cj2)*(x4210)))))):(IkReal)1.0e30))*(((((r10)*(sj1)*(x4212)))+(((IkReal(-1.00000000000000))*(x4213)*(x4217)))+(((cj1)*(r00)*(x4212)))+(((sj1)*(x4211)))+(((IkReal(-1.00000000000000))*(x4215)*(x4216)))+(((cj1)*(x4214)))))), ((((IKabs(((x4210)+(x4209))) != 0)?((IkReal)1/(((x4210)+(x4209)))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(x4213)*(x4214)))+(((sj1)*(x4216)))+(((IkReal(-1.00000000000000))*(x4215)*(x4217)))+(((IkReal(-1.00000000000000))*(r00)*(x4212)*(x4213)))+(((cj1)*(x4211)))+(((cj1)*(r10)*(x4212)))))));
18891 sj3array[0]=IKsin(j3array[0]);
18892 cj3array[0]=IKcos(j3array[0]);
18893 if( j3array[0] > IKPI )
18894 {
18895     j3array[0]-=IK2PI;
18896 }
18897 else if( j3array[0] < -IKPI )
18898 {    j3array[0]+=IK2PI;
18899 }
18900 j3valid[0] = true;
18901 for(int ij3 = 0; ij3 < 1; ++ij3)
18902 {
18903 if( !j3valid[ij3] )
18904 {
18905     continue;
18906 }
18907 _ij3[0] = ij3; _ij3[1] = -1;
18908 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18909 {
18910 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18911 {
18912     j3valid[iij3]=false; _ij3[1] = iij3; break; 
18913 }
18914 }
18915 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18916 {
18917 IkReal evalcond[6];
18918 IkReal x4218=IKsin(j3);
18919 IkReal x4219=IKcos(j3);
18920 IkReal x4220=((r11)*(sj6));
18921 IkReal x4221=((IkReal(1.00000000000000))*(sj5));
18922 IkReal x4222=((sj4)*(sj6));
18923 IkReal x4223=((cj4)*(sj5));
18924 IkReal x4224=((cj6)*(sj4));
18925 IkReal x4225=((cj4)*(cj5));
18926 IkReal x4226=((r01)*(sj6));
18927 IkReal x4227=((r21)*(sj6));
18928 IkReal x4228=((cj6)*(sj5));
18929 IkReal x4229=((IkReal(1.00000000000000))*(x4219));
18930 IkReal x4230=((sj1)*(x4219));
18931 IkReal x4231=((IkReal(1.00000000000000))*(cj6)*(x4225));
18932 IkReal x4232=((IkReal(1.00000000000000))*(cj2)*(x4218));
18933 evalcond[0]=((((cj5)*(r22)))+(((sj2)*(x4218)))+(((IkReal(-1.00000000000000))*(x4221)*(x4227)))+(((r20)*(x4228))));
18934 evalcond[1]=((((r00)*(x4228)))+(((IkReal(-1.00000000000000))*(x4221)*(x4226)))+(x4230)+(((IkReal(-1.00000000000000))*(cj1)*(x4232)))+(((cj5)*(r02))));
18935 evalcond[2]=((((cj5)*(r12)))+(((r10)*(x4228)))+(((IkReal(-1.00000000000000))*(sj1)*(x4232)))+(((IkReal(-1.00000000000000))*(cj1)*(x4229)))+(((IkReal(-1.00000000000000))*(x4220)*(x4221))));
18936 evalcond[3]=((((IkReal(-1.00000000000000))*(sj2)*(x4229)))+(((r22)*(x4223)))+(((r21)*(x4224)))+(((x4225)*(x4227)))+(((r20)*(x4222)))+(((IkReal(-1.00000000000000))*(r20)*(x4231))));
18937 evalcond[4]=((((r01)*(x4224)))+(((r00)*(x4222)))+(((r02)*(x4223)))+(((IkReal(-1.00000000000000))*(r00)*(x4231)))+(((sj1)*(x4218)))+(((x4225)*(x4226)))+(((cj1)*(cj2)*(x4219))));
18938 evalcond[5]=((((r10)*(x4222)))+(((x4220)*(x4225)))+(((IkReal(-1.00000000000000))*(r10)*(x4231)))+(((cj2)*(x4230)))+(((IkReal(-1.00000000000000))*(cj1)*(x4218)))+(((r11)*(x4224)))+(((r12)*(x4223))));
18939 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
18940 {
18941 continue;
18942 }
18943 }
18944 
18945 {
18946 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18947 vinfos[0].jointtype = 1;
18948 vinfos[0].foffset = j0;
18949 vinfos[0].indices[0] = _ij0[0];
18950 vinfos[0].indices[1] = _ij0[1];
18951 vinfos[0].maxsolutions = _nj0;
18952 vinfos[1].jointtype = 1;
18953 vinfos[1].foffset = j1;
18954 vinfos[1].indices[0] = _ij1[0];
18955 vinfos[1].indices[1] = _ij1[1];
18956 vinfos[1].maxsolutions = _nj1;
18957 vinfos[2].jointtype = 1;
18958 vinfos[2].foffset = j2;
18959 vinfos[2].indices[0] = _ij2[0];
18960 vinfos[2].indices[1] = _ij2[1];
18961 vinfos[2].maxsolutions = _nj2;
18962 vinfos[3].jointtype = 1;
18963 vinfos[3].foffset = j3;
18964 vinfos[3].indices[0] = _ij3[0];
18965 vinfos[3].indices[1] = _ij3[1];
18966 vinfos[3].maxsolutions = _nj3;
18967 vinfos[4].jointtype = 1;
18968 vinfos[4].foffset = j4;
18969 vinfos[4].indices[0] = _ij4[0];
18970 vinfos[4].indices[1] = _ij4[1];
18971 vinfos[4].maxsolutions = _nj4;
18972 vinfos[5].jointtype = 1;
18973 vinfos[5].foffset = j5;
18974 vinfos[5].indices[0] = _ij5[0];
18975 vinfos[5].indices[1] = _ij5[1];
18976 vinfos[5].maxsolutions = _nj5;
18977 vinfos[6].jointtype = 1;
18978 vinfos[6].foffset = j6;
18979 vinfos[6].indices[0] = _ij6[0];
18980 vinfos[6].indices[1] = _ij6[1];
18981 vinfos[6].maxsolutions = _nj6;
18982 std::vector<int> vfree(0);
18983 solutions.AddSolution(vinfos,vfree);
18984 }
18985 }
18986 }
18987 
18988 }
18989 
18990 }
18991 
18992 } else
18993 {
18994 {
18995 IkReal j3array[1], cj3array[1], sj3array[1];
18996 bool j3valid[1]={false};
18997 _nj3 = 1;
18998 IkReal x4233=((cj4)*(cj5));
18999 IkReal x4234=((r21)*(sj6));
19000 IkReal x4235=((IkReal(1.00000000000000))*(cj6)*(r20));
19001 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(sj5)*(x4235)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((sj5)*(x4234))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x4233)*(x4235)))+(((cj6)*(r21)*(sj4)))+(((x4233)*(x4234)))+(((r20)*(sj4)*(sj6))))))) < IKFAST_ATAN2_MAGTHRESH )
19002     continue;
19003 j3array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(sj5)*(x4235)))+(((IkReal(-1.00000000000000))*(cj5)*(r22)))+(((sj5)*(x4234)))))), ((gconst4)*(((((cj4)*(r22)*(sj5)))+(((IkReal(-1.00000000000000))*(x4233)*(x4235)))+(((cj6)*(r21)*(sj4)))+(((x4233)*(x4234)))+(((r20)*(sj4)*(sj6)))))));
19004 sj3array[0]=IKsin(j3array[0]);
19005 cj3array[0]=IKcos(j3array[0]);
19006 if( j3array[0] > IKPI )
19007 {
19008     j3array[0]-=IK2PI;
19009 }
19010 else if( j3array[0] < -IKPI )
19011 {    j3array[0]+=IK2PI;
19012 }
19013 j3valid[0] = true;
19014 for(int ij3 = 0; ij3 < 1; ++ij3)
19015 {
19016 if( !j3valid[ij3] )
19017 {
19018     continue;
19019 }
19020 _ij3[0] = ij3; _ij3[1] = -1;
19021 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19022 {
19023 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19024 {
19025     j3valid[iij3]=false; _ij3[1] = iij3; break; 
19026 }
19027 }
19028 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19029 {
19030 IkReal evalcond[6];
19031 IkReal x4236=IKsin(j3);
19032 IkReal x4237=IKcos(j3);
19033 IkReal x4238=((r11)*(sj6));
19034 IkReal x4239=((IkReal(1.00000000000000))*(sj5));
19035 IkReal x4240=((sj4)*(sj6));
19036 IkReal x4241=((cj4)*(sj5));
19037 IkReal x4242=((cj6)*(sj4));
19038 IkReal x4243=((cj4)*(cj5));
19039 IkReal x4244=((r01)*(sj6));
19040 IkReal x4245=((r21)*(sj6));
19041 IkReal x4246=((cj6)*(sj5));
19042 IkReal x4247=((IkReal(1.00000000000000))*(x4237));
19043 IkReal x4248=((sj1)*(x4237));
19044 IkReal x4249=((IkReal(1.00000000000000))*(cj6)*(x4243));
19045 IkReal x4250=((IkReal(1.00000000000000))*(cj2)*(x4236));
19046 evalcond[0]=((((cj5)*(r22)))+(((r20)*(x4246)))+(((sj2)*(x4236)))+(((IkReal(-1.00000000000000))*(x4239)*(x4245))));
19047 evalcond[1]=((((r00)*(x4246)))+(((IkReal(-1.00000000000000))*(cj1)*(x4250)))+(x4248)+(((IkReal(-1.00000000000000))*(x4239)*(x4244)))+(((cj5)*(r02))));
19048 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)*(x4250)))+(((cj5)*(r12)))+(((r10)*(x4246)))+(((IkReal(-1.00000000000000))*(cj1)*(x4247)))+(((IkReal(-1.00000000000000))*(x4238)*(x4239))));
19049 evalcond[3]=((((r20)*(x4240)))+(((r22)*(x4241)))+(((IkReal(-1.00000000000000))*(sj2)*(x4247)))+(((x4243)*(x4245)))+(((IkReal(-1.00000000000000))*(r20)*(x4249)))+(((r21)*(x4242))));
19050 evalcond[4]=((((r00)*(x4240)))+(((r01)*(x4242)))+(((x4243)*(x4244)))+(((sj1)*(x4236)))+(((r02)*(x4241)))+(((cj1)*(cj2)*(x4237)))+(((IkReal(-1.00000000000000))*(r00)*(x4249))));
19051 evalcond[5]=((((r10)*(x4240)))+(((IkReal(-1.00000000000000))*(cj1)*(x4236)))+(((x4238)*(x4243)))+(((IkReal(-1.00000000000000))*(r10)*(x4249)))+(((cj2)*(x4248)))+(((r12)*(x4241)))+(((r11)*(x4242))));
19052 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
19053 {
19054 continue;
19055 }
19056 }
19057 
19058 {
19059 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19060 vinfos[0].jointtype = 1;
19061 vinfos[0].foffset = j0;
19062 vinfos[0].indices[0] = _ij0[0];
19063 vinfos[0].indices[1] = _ij0[1];
19064 vinfos[0].maxsolutions = _nj0;
19065 vinfos[1].jointtype = 1;
19066 vinfos[1].foffset = j1;
19067 vinfos[1].indices[0] = _ij1[0];
19068 vinfos[1].indices[1] = _ij1[1];
19069 vinfos[1].maxsolutions = _nj1;
19070 vinfos[2].jointtype = 1;
19071 vinfos[2].foffset = j2;
19072 vinfos[2].indices[0] = _ij2[0];
19073 vinfos[2].indices[1] = _ij2[1];
19074 vinfos[2].maxsolutions = _nj2;
19075 vinfos[3].jointtype = 1;
19076 vinfos[3].foffset = j3;
19077 vinfos[3].indices[0] = _ij3[0];
19078 vinfos[3].indices[1] = _ij3[1];
19079 vinfos[3].maxsolutions = _nj3;
19080 vinfos[4].jointtype = 1;
19081 vinfos[4].foffset = j4;
19082 vinfos[4].indices[0] = _ij4[0];
19083 vinfos[4].indices[1] = _ij4[1];
19084 vinfos[4].maxsolutions = _nj4;
19085 vinfos[5].jointtype = 1;
19086 vinfos[5].foffset = j5;
19087 vinfos[5].indices[0] = _ij5[0];
19088 vinfos[5].indices[1] = _ij5[1];
19089 vinfos[5].maxsolutions = _nj5;
19090 vinfos[6].jointtype = 1;
19091 vinfos[6].foffset = j6;
19092 vinfos[6].indices[0] = _ij6[0];
19093 vinfos[6].indices[1] = _ij6[1];
19094 vinfos[6].maxsolutions = _nj6;
19095 std::vector<int> vfree(0);
19096 solutions.AddSolution(vinfos,vfree);
19097 }
19098 }
19099 }
19100 
19101 }
19102 
19103 }
19104 }
19105 }
19106 
19107 }
19108 
19109 }
19110 }
19111 }
19112 
19113 }
19114 
19115 }
19116     }
19117 }
19118 return solutions.GetNumSolutions()>0;
19119 }
19120 
19121 static inline bool checkconsistency8(const IkReal* Breal)
19122 {
19123     IkReal norm = 0.1;
19124     for(int i = 0; i < 7; ++i) {
19125         norm += IKabs(Breal[i]);
19126     }
19127     IkReal tol = 1e-5*norm; // have to increase the threshold since many computations are involved
19128     return IKabs(Breal[0]*Breal[1]-Breal[2]) < tol && IKabs(Breal[1]*Breal[1]-Breal[3]) < tol && IKabs(Breal[0]*Breal[3]-Breal[4]) < tol && IKabs(Breal[1]*Breal[3]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol;
19129 }
19133 static inline void solvedialyticpoly8qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
19134 {
19135     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19136     IkReal IKFAST_ALIGNED16(M[16*16]) = {0};
19137     IkReal IKFAST_ALIGNED16(A[8*8]);
19138     IkReal IKFAST_ALIGNED16(work[16*16*15]);
19139     int ipiv[8];
19140     int info, coeffindex;
19141     const int worksize=16*16*15;
19142     const int matrixdim = 8;
19143     const int matrixdim2 = 16;
19144     numroots = 0;
19145     // first setup M = [0 I; -C -B] and A
19146     coeffindex = 0;
19147     for(int j = 0; j < 4; ++j) {
19148         for(int k = 0; k < 6; ++k) {
19149             M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -matcoeffs[coeffindex++];
19150         }
19151     }
19152     for(int j = 0; j < 4; ++j) {
19153         for(int k = 0; k < 6; ++k) {
19154             M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
19155         }
19156     }
19157     for(int j = 0; j < 4; ++j) {
19158         for(int k = 0; k < 6; ++k) {
19159             A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = matcoeffs[coeffindex++];
19160         }
19161         for(int k = 0; k < 2; ++k) {
19162             A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
19163         }
19164     }
19165     const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
19166     int lfindex = -1;
19167     bool bsingular = true;
19168     do {
19169         dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
19170         if( info == 0 ) {
19171             bsingular = false;
19172             for(int j = 0; j < matrixdim; ++j) {
19173                 if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
19174                     bsingular = true;
19175                     break;
19176                 }
19177             }
19178             if( !bsingular ) {
19179                 break;
19180             }
19181         }
19182         if( lfindex == 3 ) {
19183             break;
19184         }
19185         // transform by the linear functional
19186         lfindex++;
19187         const IkReal* lf = lfpossibilities[lfindex];
19188         // have to reinitialize A
19189         coeffindex = 0;
19190         for(int j = 0; j < 4; ++j) {
19191             for(int k = 0; k < 6; ++k) {
19192                 IkReal a = matcoeffs[coeffindex+48], b = matcoeffs[coeffindex+24], c = matcoeffs[coeffindex];
19193                 A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
19194                 M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
19195                 M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
19196                 coeffindex++;
19197             }
19198             for(int k = 0; k < 2; ++k) {
19199                 A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
19200             }
19201         }
19202     } while(lfindex<4);
19203 
19204     if( bsingular ) {
19205         return;
19206     }
19207     dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
19208     if( info != 0 ) {
19209         return;
19210     }
19211 
19212     // set identity in upper corner
19213     for(int j = 0; j < matrixdim; ++j) {
19214         M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
19215     }
19216     IkReal IKFAST_ALIGNED16(wr[16]);
19217     IkReal IKFAST_ALIGNED16(wi[16]);
19218     IkReal IKFAST_ALIGNED16(vr[16*16]);
19219     int one=1;
19220     dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
19221     if( info != 0 ) {
19222         return;
19223     }
19224     IkReal Breal[matrixdim-1];
19225     for(int i = 0; i < matrixdim2; ++i) {
19226         if( IKabs(wi[i]) < tol*100 ) {
19227             IkReal* ev = vr+matrixdim2*i;
19228             if( IKabs(wr[i]) > 1 ) {
19229                 ev += matrixdim;
19230             }
19231             // consistency has to be checked!!
19232             if( IKabs(ev[0]) < tol ) {
19233                 continue;
19234             }
19235             IkReal iconst = 1/ev[0];
19236             for(int j = 1; j < matrixdim; ++j) {
19237                 Breal[j-1] = ev[j]*iconst;
19238             }
19239             if( checkconsistency8(Breal) ) {
19240                 if( lfindex >= 0 ) {
19241                     const IkReal* lf = lfpossibilities[lfindex];
19242                     rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
19243                 }
19244                 else {
19245                     rawroots[numroots++] = wr[i];
19246                 }
19247                 bool bsmall0=IKabs(ev[0]) > IKabs(ev[2]);
19248                 bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
19249                 if( bsmall0 && bsmall1 ) {
19250                     rawroots[numroots++] = ev[2]/ev[0];
19251                     rawroots[numroots++] = ev[1]/ev[0];
19252                 }
19253                 else if( bsmall0 && !bsmall1 ) {
19254                     rawroots[numroots++] = ev[3]/ev[1];
19255                     rawroots[numroots++] = ev[1]/ev[0];
19256                 }
19257                 else if( !bsmall0 && bsmall1 ) {
19258                     rawroots[numroots++] = ev[6]/ev[4];
19259                     rawroots[numroots++] = ev[7]/ev[6];
19260                 }
19261                 else if( !bsmall0 && !bsmall1 ) {
19262                     rawroots[numroots++] = ev[7]/ev[5];
19263                     rawroots[numroots++] = ev[7]/ev[6];
19264                 }
19265             }
19266         }
19267     }
19268 }};
19269 
19270 
19273 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
19274 IKSolver solver;
19275 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
19276 }
19277 
19278 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - cob3_3 (e2dd794ce8555362ff4c38dd220c9234)>"; }
19279 
19280 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
19281 
19282 #ifdef IKFAST_NAMESPACE
19283 } // end namespace
19284 #endif
19285 
19286 #ifndef IKFAST_NO_MAIN
19287 #include <stdio.h>
19288 #include <stdlib.h>
19289 #ifdef IKFAST_NAMESPACE
19290 using namespace IKFAST_NAMESPACE;
19291 #endif
19292 int main(int argc, char** argv)
19293 {
19294     if( argc != 12+GetNumFreeParameters()+1 ) {
19295         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
19296                "Returns the ik solutions given the transformation of the end effector specified by\n"
19297                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
19298                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
19299         return 1;
19300     }
19301 
19302     IkSolutionList<IkReal> solutions;
19303     std::vector<IkReal> vfree(GetNumFreeParameters());
19304     IkReal eerot[9],eetrans[3];
19305     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
19306     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
19307     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
19308     for(std::size_t i = 0; i < vfree.size(); ++i)
19309         vfree[i] = atof(argv[13+i]);
19310     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
19311 
19312     if( !bSuccess ) {
19313         fprintf(stderr,"Failed to get ik solution\n");
19314         return -1;
19315     }
19316 
19317     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
19318     std::vector<IkReal> solvalues(GetNumJoints());
19319     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
19320         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
19321         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
19322         std::vector<IkReal> vsolfree(sol.GetFree().size());
19323         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
19324         for( std::size_t j = 0; j < solvalues.size(); ++j)
19325             printf("%.15f, ", solvalues[j]);
19326         printf("\n");
19327     }
19328     return 0;
19329 }
19330 
19331 #endif


cob_kinematics
Author(s): Mathias Luedtke
autogenerated on Wed Aug 26 2015 11:01:11