ikfast_lwa.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 #include "ros/ros.h"
00024 #define IKFAST_ASSERT ROS_ASSERT
00025 // check if the included ikfast version matches what this file was compiled with
00026 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00027 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00028 
00029 #include <cmath>
00030 #include <vector>
00031 #include <limits>
00032 #include <algorithm>
00033 #include <complex>
00034 
00035 #define IKFAST_STRINGIZE2(s) #s
00036 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
00037 
00038 #ifndef IKFAST_ASSERT
00039 #include <stdexcept>
00040 #include <sstream>
00041 #include <iostream>
00042 
00043 #ifdef _MSC_VER
00044 #ifndef __PRETTY_FUNCTION__
00045 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00046 #endif
00047 #endif
00048 
00049 #ifndef __PRETTY_FUNCTION__
00050 #define __PRETTY_FUNCTION__ __func__
00051 #endif
00052 
00053 #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()); } }
00054 
00055 #endif
00056 
00057 #if defined(_MSC_VER)
00058 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00059 #else
00060 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00061 #endif
00062 
00063 #define IK2PI  ((IkReal)6.28318530717959)
00064 #define IKPI  ((IkReal)3.14159265358979)
00065 #define IKPI_2  ((IkReal)1.57079632679490)
00066 
00067 #ifdef _MSC_VER
00068 #ifndef isnan
00069 #define isnan _isnan
00070 #endif
00071 #endif // _MSC_VER
00072 
00073 // lapack routines
00074 extern "C" {
00075   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00076   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00077   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00078   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00079   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);
00080   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);
00081 }
00082 
00083 using namespace std; // necessary to get std math routines
00084 
00085 #ifdef IKFAST_NAMESPACE
00086 namespace IKFAST_NAMESPACE {
00087 #endif
00088 
00089 inline float IKabs(float f) { return fabsf(f); }
00090 inline double IKabs(double f) { return fabs(f); }
00091 
00092 inline float IKsqr(float f) { return f*f; }
00093 inline double IKsqr(double f) { return f*f; }
00094 
00095 inline float IKlog(float f) { return logf(f); }
00096 inline double IKlog(double f) { return log(f); }
00097 
00098 // allows asin and acos to exceed 1
00099 #ifndef IKFAST_SINCOS_THRESH
00100 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00101 #endif
00102 
00103 // used to check input to atan2 for degenerate cases
00104 #ifndef IKFAST_ATAN2_MAGTHRESH
00105 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00106 #endif
00107 
00108 // minimum distance of separate solutions
00109 #ifndef IKFAST_SOLUTION_THRESH
00110 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00111 #endif
00112 
00113 inline float IKasin(float f)
00114 {
00115 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00116 if( f <= -1 ) return float(-IKPI_2);
00117 else if( f >= 1 ) return float(IKPI_2);
00118 return asinf(f);
00119 }
00120 inline double IKasin(double f)
00121 {
00122 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00123 if( f <= -1 ) return -IKPI_2;
00124 else if( f >= 1 ) return IKPI_2;
00125 return asin(f);
00126 }
00127 
00128 // return positive value in [0,y)
00129 inline float IKfmod(float x, float y)
00130 {
00131     while(x < 0) {
00132         x += y;
00133     }
00134     return fmodf(x,y);
00135 }
00136 
00137 // return positive value in [0,y)
00138 inline double IKfmod(double x, double y)
00139 {
00140     while(x < 0) {
00141         x += y;
00142     }
00143     return fmod(x,y);
00144 }
00145 
00146 inline float IKacos(float f)
00147 {
00148 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00149 if( f <= -1 ) return float(IKPI);
00150 else if( f >= 1 ) return float(0);
00151 return acosf(f);
00152 }
00153 inline double IKacos(double f)
00154 {
00155 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00156 if( f <= -1 ) return IKPI;
00157 else if( f >= 1 ) return 0;
00158 return acos(f);
00159 }
00160 inline float IKsin(float f) { return sinf(f); }
00161 inline double IKsin(double f) { return sin(f); }
00162 inline float IKcos(float f) { return cosf(f); }
00163 inline double IKcos(double f) { return cos(f); }
00164 inline float IKtan(float f) { return tanf(f); }
00165 inline double IKtan(double f) { return tan(f); }
00166 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00167 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00168 inline float IKatan2(float fy, float fx) {
00169     if( isnan(fy) ) {
00170         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00171         return float(IKPI_2);
00172     }
00173     else if( isnan(fx) ) {
00174         return 0;
00175     }
00176     return atan2f(fy,fx);
00177 }
00178 inline double IKatan2(double fy, double fx) {
00179     if( isnan(fy) ) {
00180         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00181         return IKPI_2;
00182     }
00183     else if( isnan(fx) ) {
00184         return 0;
00185     }
00186     return atan2(fy,fx);
00187 }
00188 
00189 inline float IKsign(float f) {
00190     if( f > 0 ) {
00191         return float(1);
00192     }
00193     else if( f < 0 ) {
00194         return float(-1);
00195     }
00196     return 0;
00197 }
00198 
00199 inline double IKsign(double f) {
00200     if( f > 0 ) {
00201         return 1.0;
00202     }
00203     else if( f < 0 ) {
00204         return -1.0;
00205     }
00206     return 0;
00207 }
00208 
00211 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00212 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,x66;
00213 x0=IKcos(j[0]);
00214 x1=IKsin(j[0]);
00215 x2=IKsin(j[2]);
00216 x3=IKcos(j[1]);
00217 x4=IKcos(j[2]);
00218 x5=IKcos(j[4]);
00219 x6=IKcos(j[3]);
00220 x7=IKsin(j[1]);
00221 x8=IKsin(j[3]);
00222 x9=IKsin(j[4]);
00223 x10=IKsin(j[6]);
00224 x11=IKcos(j[5]);
00225 x12=IKsin(j[5]);
00226 x13=IKcos(j[6]);
00227 x14=((IkReal(0.192700000000000))*(x7));
00228 x15=((IkReal(0.192700000000000))*(x8));
00229 x16=((IkReal(1.00000000000000))*(x7));
00230 x17=((IkReal(1.00000000000000))*(x1));
00231 x18=((IkReal(1.00000000000000))*(x3));
00232 x19=((IkReal(1.00000000000000))*(x11));
00233 x20=((IkReal(1.00000000000000))*(x12));
00234 x21=((IkReal(0.192700000000000))*(x5));
00235 x22=((IkReal(0.316500000000000))*(x0));
00236 x23=((IkReal(1.00000000000000))*(x0));
00237 x24=((IkReal(0.316500000000000))*(x6));
00238 x25=((IkReal(1.00000000000000))*(x5));
00239 x26=((x2)*(x9));
00240 x27=((x4)*(x8));
00241 x28=((x2)*(x3));
00242 x29=((x0)*(x7));
00243 x30=((x1)*(x7));
00244 x31=((x3)*(x4));
00245 x32=((x4)*(x6));
00246 x33=((x1)*(x2));
00247 x34=((x3)*(x6));
00248 x35=((x0)*(x18)*(x4));
00249 x36=((x16)*(x26));
00250 x37=((((IkReal(-1.00000000000000))*(x35)))+(x33));
00251 x38=((((x1)*(x4)))+(((x0)*(x28))));
00252 x39=((((x1)*(x28)))+(((IkReal(-1.00000000000000))*(x23)*(x4))));
00253 x40=((((IkReal(-1.00000000000000))*(x16)*(x27)))+(x34));
00254 x41=((((x32)*(x7)))+(((x3)*(x8))));
00255 x42=((((IkReal(-1.00000000000000))*(x17)*(x2)))+(x35));
00256 x43=((((x17)*(x31)))+(((x2)*(x23))));
00257 x44=((IkReal(-1.00000000000000))*(x43));
00258 x45=((x38)*(x9));
00259 x46=((x41)*(x5));
00260 x47=((x39)*(x9));
00261 x48=((x37)*(x6));
00262 x49=((x42)*(x8));
00263 x50=((x44)*(x6));
00264 x51=((((x29)*(x8)))+(x48));
00265 x52=((((IkReal(-1.00000000000000))*(x36)))+(x46));
00266 x53=((((x29)*(x6)))+(x49));
00267 x54=((((x30)*(x6)))+(((x43)*(x8))));
00268 x55=((((x30)*(x8)))+(x50));
00269 IkReal x67=((IkReal(1.00000000000000))*(x16));
00270 x56=((((x9)*(((((IkReal(-1.00000000000000))*(x18)*(x8)))+(((IkReal(-1.00000000000000))*(x32)*(x67)))))))+(((IkReal(-1.00000000000000))*(x2)*(x5)*(x67))));
00271 x57=((x11)*(x52));
00272 x58=((x5)*(x51));
00273 x59=((x12)*(x53));
00274 x60=((x12)*(x54));
00275 x61=((x5)*(x55));
00276 x62=((x45)+(x58));
00277 x63=((x47)+(x61));
00278 x64=((((x9)*(((((IkReal(-1.00000000000000))*(x48)))+(((IkReal(-1.00000000000000))*(x29)*(x8)))))))+(((x38)*(x5))));
00279 x65=((((x39)*(x5)))+(((x9)*(((((IkReal(-1.00000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x1)*(x16)*(x8))))))));
00280 x66=((x11)*(x63));
00281 eerot[0]=((((x13)*(((((x11)*(x62)))+(x59)))))+(((x10)*(x64))));
00282 eerot[1]=((((x13)*(x64)))+(((x10)*(((((IkReal(-1.00000000000000))*(x20)*(x53)))+(((IkReal(-1.00000000000000))*(x19)*(x62))))))));
00283 eerot[2]=((((x11)*(x53)))+(((x12)*(((((IkReal(-1.00000000000000))*(x45)))+(((IkReal(-1.00000000000000))*(x25)*(x51))))))));
00284 IkReal x68=((IkReal(1.00000000000000))*(x22));
00285 eetrans[0]=((((IkReal(-1.00000000000000))*(x6)*(x68)*(x7)))+(((x11)*(((((IkReal(-1.00000000000000))*(x15)*(x42)))+(((IkReal(-1.00000000000000))*(x0)*(x14)*(x6)))))))+(((IkReal(-0.408000000000000))*(x29)))+(((x12)*(((((IkReal(0.192700000000000))*(x45)))+(((x21)*(x51)))))))+(((x8)*(((((IkReal(-1.00000000000000))*(x31)*(x68)))+(((IkReal(0.316500000000000))*(x33))))))));
00286 eerot[3]=((((x10)*(x65)))+(((x13)*(((x60)+(x66))))));
00287 eerot[4]=((((x13)*(x65)))+(((x10)*(((((IkReal(-1.00000000000000))*(x66)))+(((IkReal(-1.00000000000000))*(x60))))))));
00288 eerot[5]=((((x12)*(((((IkReal(-1.00000000000000))*(x47)))+(((IkReal(-1.00000000000000))*(x25)*(x55)))))))+(((x11)*(x54))));
00289 eetrans[1]=((((x8)*(((((IkReal(-0.316500000000000))*(x1)*(x31)))+(((IkReal(-1.00000000000000))*(x2)*(x22)))))))+(((x11)*(((((IkReal(-1.00000000000000))*(x15)*(x43)))+(((IkReal(-1.00000000000000))*(x1)*(x14)*(x6)))))))+(((x12)*(((((IkReal(0.192700000000000))*(x47)))+(((x21)*(x55)))))))+(((IkReal(-0.408000000000000))*(x30)))+(((IkReal(-1.00000000000000))*(x24)*(x30))));
00290 eerot[6]=((((x10)*(x56)))+(((x13)*(((x57)+(((x12)*(x40))))))));
00291 eerot[7]=((((x13)*(x56)))+(((x10)*(((((IkReal(-1.00000000000000))*(x20)*(x40)))+(((IkReal(-1.00000000000000))*(x19)*(x52))))))));
00292 eerot[8]=((((x11)*(x40)))+(((x12)*(((((IkReal(-1.00000000000000))*(x25)*(x41)))+(x36))))));
00293 eetrans[2]=((IkReal(-0.250000000000000))+(((x11)*(((((x14)*(x27)))+(((IkReal(-0.192700000000000))*(x34)))))))+(((x12)*(((((x21)*(x41)))+(((IkReal(-1.00000000000000))*(x14)*(x26)))))))+(((IkReal(0.316500000000000))*(x27)*(x7)))+(((IkReal(-1.00000000000000))*(x24)*(x3)))+(((IkReal(-0.408000000000000))*(x3))));
00294 }
00295 
00296 IKFAST_API int GetNumFreeParameters() { return 1; }
00297 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {2}; return freeparams; }
00298 IKFAST_API int GetNumJoints() { return 7; }
00299 
00300 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00301 
00302 IKFAST_API int GetIkType() { return 0x67000001; }
00303 
00304 class IKSolver {
00305 public:
00306 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j2,cj2,sj2,htj2,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;
00307 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij2[2], _nj2;
00308 
00309 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00310 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; 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;  _ij2[0] = -1; _ij2[1] = -1; _nj2 = 0; 
00311 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00312     solutions.Clear();
00313 j2=pfree[0]; cj2=cos(pfree[0]); sj2=sin(pfree[0]);
00314 r00 = eerot[0*3+0];
00315 r01 = eerot[0*3+1];
00316 r02 = eerot[0*3+2];
00317 r10 = eerot[1*3+0];
00318 r11 = eerot[1*3+1];
00319 r12 = eerot[1*3+2];
00320 r20 = eerot[2*3+0];
00321 r21 = eerot[2*3+1];
00322 r22 = eerot[2*3+2];
00323 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00324 
00325 new_r00=r00;
00326 new_r01=r01;
00327 new_r02=r02;
00328 new_px=((px)+(((IkReal(0.192700000000000))*(r02))));
00329 new_r10=r10;
00330 new_r11=r11;
00331 new_r12=r12;
00332 new_py=((py)+(((IkReal(0.192700000000000))*(r12))));
00333 new_r20=r20;
00334 new_r21=r21;
00335 new_r22=r22;
00336 new_pz=((IkReal(0.250000000000000))+(((IkReal(0.192700000000000))*(r22)))+(pz));
00337 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;
00338 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00339 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00340 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00341 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00342 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00343 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00344 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00345 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00346 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00347 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00348 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00349 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00350 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00351 {
00352 IkReal j3array[2], cj3array[2], sj3array[2];
00353 bool j3valid[2]={false};
00354 _nj3 = 2;
00355 cj3array[0]=((IkReal(-1.03241741009200))+(((IkReal(3.87200693863643))*(pp))));
00356 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
00357 {
00358     j3valid[0] = j3valid[1] = true;
00359     j3array[0] = IKacos(cj3array[0]);
00360     sj3array[0] = IKsin(j3array[0]);
00361     cj3array[1] = cj3array[0];
00362     j3array[1] = -j3array[0];
00363     sj3array[1] = -sj3array[0];
00364 }
00365 else if( isnan(cj3array[0]) )
00366 {
00367     // probably any value will work
00368     j3valid[0] = true;
00369     cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
00370 }
00371 for(int ij3 = 0; ij3 < 2; ++ij3)
00372 {
00373 if( !j3valid[ij3] )
00374 {
00375     continue;
00376 }
00377 _ij3[0] = ij3; _ij3[1] = -1;
00378 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00379 {
00380 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00381 {
00382     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00383 }
00384 }
00385 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00386 
00387 {
00388 IkReal dummyeval[1];
00389 dummyeval[0]=(((px)*(px))+((py)*(py)));
00390 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00391 {
00392 {
00393 IkReal dummyeval[1];
00394 dummyeval[0]=(((px)*(px))+((py)*(py)));
00395 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00396 {
00397 {
00398 IkReal dummyeval[1];
00399 dummyeval[0]=((IkReal(1.66177758810449))+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((IkReal(2.57819905213270))*(cj3))));
00400 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00401 {
00402 continue;
00403 
00404 } else
00405 {
00406 {
00407 IkReal j1array[2], cj1array[2], sj1array[2];
00408 bool j1valid[2]={false};
00409 _nj1 = 2;
00410 IkReal x69=((IkReal(-0.408000000000000))+(((IkReal(-0.316500000000000))*(cj3))));
00411 if( IKabs(x69) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(0.316500000000000))*(cj2)*(sj3))) < IKFAST_ATAN2_MAGTHRESH )
00412     continue;
00413 IkReal x70=((IkReal(1.00000000000000))*(IKatan2(x69, ((IkReal(0.316500000000000))*(cj2)*(sj3)))));
00414 if( ((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3)))))) < (IkReal)-0.00001 )
00415     continue;
00416 if( (((pz)*(((IKabs(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((pz)*(((IKabs(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
00417     continue;
00418 IkReal x71=IKasin(((pz)*(((IKabs(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x69)*(x69))+(((IkReal(0.100172250000000))*((cj2)*(cj2))*((sj3)*(sj3))))))))):(IkReal)1.0e30))));
00419 j1array[0]=((((IkReal(-1.00000000000000))*(x70)))+(x71));
00420 sj1array[0]=IKsin(j1array[0]);
00421 cj1array[0]=IKcos(j1array[0]);
00422 j1array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x70)))+(((IkReal(-1.00000000000000))*(x71))));
00423 sj1array[1]=IKsin(j1array[1]);
00424 cj1array[1]=IKcos(j1array[1]);
00425 if( j1array[0] > IKPI )
00426 {
00427     j1array[0]-=IK2PI;
00428 }
00429 else if( j1array[0] < -IKPI )
00430 {    j1array[0]+=IK2PI;
00431 }
00432 j1valid[0] = true;
00433 if( j1array[1] > IKPI )
00434 {
00435     j1array[1]-=IK2PI;
00436 }
00437 else if( j1array[1] < -IKPI )
00438 {    j1array[1]+=IK2PI;
00439 }
00440 j1valid[1] = true;
00441 for(int ij1 = 0; ij1 < 2; ++ij1)
00442 {
00443 if( !j1valid[ij1] )
00444 {
00445     continue;
00446 }
00447 _ij1[0] = ij1; _ij1[1] = -1;
00448 for(int iij1 = ij1+1; iij1 < 2; ++iij1)
00449 {
00450 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00451 {
00452     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00453 }
00454 }
00455 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00456 
00457 {
00458 IkReal dummyeval[1];
00459 IkReal gconst0;
00460 IkReal x72=((IkReal(102000.000000000))*(sj1));
00461 gconst0=IKsign(((((x72)*((px)*(px))))+(((x72)*((py)*(py))))));
00462 dummyeval[0]=((((sj1)*((py)*(py))))+(((sj1)*((px)*(px)))));
00463 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00464 {
00465 {
00466 IkReal dummyeval[1];
00467 IkReal gconst1;
00468 IkReal x73=((IkReal(2000.00000000000))*(sj1));
00469 gconst1=IKsign(((((x73)*((px)*(px))))+(((x73)*((py)*(py))))));
00470 dummyeval[0]=((((sj1)*((py)*(py))))+(((sj1)*((px)*(px)))));
00471 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00472 {
00473 {
00474 IkReal evalcond[5];
00475 IkReal x74=((IkReal(1.00000000000000))*(pp));
00476 IkReal x75=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.316500000000000))*(cj3))));
00477 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959))));
00478 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(-1.00000000000000))*(x74)))+(((IkReal(0.258264000000000))*(cj3))));
00479 evalcond[2]=x75;
00480 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x74)))+(((IkReal(-0.816000000000000))*(pz))));
00481 evalcond[4]=x75;
00482 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  )
00483 {
00484 {
00485 IkReal dummyeval[1];
00486 IkReal gconst2;
00487 gconst2=IKsign(((((IkReal(2000.00000000000))*((py)*(py))))+(((IkReal(2000.00000000000))*((px)*(px))))));
00488 dummyeval[0]=(((px)*(px))+((py)*(py)));
00489 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00490 {
00491 {
00492 IkReal dummyeval[1];
00493 IkReal gconst3;
00494 IkReal x76=((IkReal(2000.00000000000))*(cj2));
00495 gconst3=IKsign(((((x76)*((px)*(px))))+(((x76)*((py)*(py))))));
00496 dummyeval[0]=((((cj2)*((py)*(py))))+(((cj2)*((px)*(px)))));
00497 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00498 {
00499 {
00500 IkReal evalcond[5];
00501 IkReal x77=((IkReal(1.00000000000000))*(pp));
00502 IkReal x78=x75;
00503 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
00504 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(-1.00000000000000))*(x77)))+(((IkReal(0.258264000000000))*(cj3))));
00505 evalcond[2]=x78;
00506 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x77)))+(((IkReal(-0.816000000000000))*(pz))));
00507 evalcond[4]=x78;
00508 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  )
00509 {
00510 {
00511 IkReal dummyeval[1];
00512 IkReal gconst4;
00513 gconst4=IKsign(((((IkReal(2000.00000000000))*((py)*(py))))+(((IkReal(2000.00000000000))*((px)*(px))))));
00514 dummyeval[0]=(((px)*(px))+((py)*(py)));
00515 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00516 {
00517 continue;
00518 
00519 } else
00520 {
00521 {
00522 IkReal j0array[1], cj0array[1], sj0array[1];
00523 bool j0valid[1]={false};
00524 _nj0 = 1;
00525 IkReal x79=((gconst4)*(sj3));
00526 if( IKabs(((IkReal(633.000000000000))*(px)*(x79))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-633.000000000000))*(py)*(x79))) < IKFAST_ATAN2_MAGTHRESH )
00527     continue;
00528 j0array[0]=IKatan2(((IkReal(633.000000000000))*(px)*(x79)), ((IkReal(-633.000000000000))*(py)*(x79)));
00529 sj0array[0]=IKsin(j0array[0]);
00530 cj0array[0]=IKcos(j0array[0]);
00531 if( j0array[0] > IKPI )
00532 {
00533     j0array[0]-=IK2PI;
00534 }
00535 else if( j0array[0] < -IKPI )
00536 {    j0array[0]+=IK2PI;
00537 }
00538 j0valid[0] = true;
00539 for(int ij0 = 0; ij0 < 1; ++ij0)
00540 {
00541 if( !j0valid[ij0] )
00542 {
00543     continue;
00544 }
00545 _ij0[0] = ij0; _ij0[1] = -1;
00546 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00547 {
00548 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00549 {
00550     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00551 }
00552 }
00553 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00554 {
00555 IkReal evalcond[2];
00556 IkReal x80=IKsin(j0);
00557 IkReal x81=IKcos(j0);
00558 evalcond[0]=((((IkReal(-0.316500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(py)*(x81)))+(((px)*(x80))));
00559 evalcond[1]=((((py)*(x80)))+(((px)*(x81))));
00560 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00561 {
00562 continue;
00563 }
00564 }
00565 
00566 rotationfunction0(solutions);
00567 }
00568 }
00569 
00570 }
00571 
00572 }
00573 
00574 } else
00575 {
00576 IkReal x230=((IkReal(1.00000000000000))*(pp));
00577 IkReal x231=x75;
00578 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
00579 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(-1.00000000000000))*(x230)))+(((IkReal(0.258264000000000))*(cj3))));
00580 evalcond[2]=x231;
00581 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x230)))+(((IkReal(-0.816000000000000))*(pz))));
00582 evalcond[4]=x231;
00583 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  )
00584 {
00585 {
00586 IkReal dummyeval[1];
00587 IkReal gconst5;
00588 gconst5=IKsign(((((IkReal(-2000.00000000000))*((py)*(py))))+(((IkReal(-2000.00000000000))*((px)*(px))))));
00589 dummyeval[0]=((((IkReal(-1.00000000000000))*((px)*(px))))+(((IkReal(-1.00000000000000))*((py)*(py)))));
00590 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00591 {
00592 continue;
00593 
00594 } else
00595 {
00596 {
00597 IkReal j0array[1], cj0array[1], sj0array[1];
00598 bool j0valid[1]={false};
00599 _nj0 = 1;
00600 IkReal x232=((gconst5)*(sj3));
00601 if( IKabs(((IkReal(633.000000000000))*(px)*(x232))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-633.000000000000))*(py)*(x232))) < IKFAST_ATAN2_MAGTHRESH )
00602     continue;
00603 j0array[0]=IKatan2(((IkReal(633.000000000000))*(px)*(x232)), ((IkReal(-633.000000000000))*(py)*(x232)));
00604 sj0array[0]=IKsin(j0array[0]);
00605 cj0array[0]=IKcos(j0array[0]);
00606 if( j0array[0] > IKPI )
00607 {
00608     j0array[0]-=IK2PI;
00609 }
00610 else if( j0array[0] < -IKPI )
00611 {    j0array[0]+=IK2PI;
00612 }
00613 j0valid[0] = true;
00614 for(int ij0 = 0; ij0 < 1; ++ij0)
00615 {
00616 if( !j0valid[ij0] )
00617 {
00618     continue;
00619 }
00620 _ij0[0] = ij0; _ij0[1] = -1;
00621 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00622 {
00623 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00624 {
00625     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00626 }
00627 }
00628 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00629 {
00630 IkReal evalcond[2];
00631 IkReal x233=IKsin(j0);
00632 IkReal x234=IKcos(j0);
00633 evalcond[0]=((((IkReal(-1.00000000000000))*(py)*(x234)))+(((IkReal(0.316500000000000))*(sj3)))+(((px)*(x233))));
00634 evalcond[1]=((((px)*(x234)))+(((py)*(x233))));
00635 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00636 {
00637 continue;
00638 }
00639 }
00640 
00641 rotationfunction0(solutions);
00642 }
00643 }
00644 
00645 }
00646 
00647 }
00648 
00649 } else
00650 {
00651 if( 1 )
00652 {
00653 continue;
00654 
00655 } else
00656 {
00657 }
00658 }
00659 }
00660 }
00661 
00662 } else
00663 {
00664 {
00665 IkReal j0array[1], cj0array[1], sj0array[1];
00666 bool j0valid[1]={false};
00667 _nj0 = 1;
00668 IkReal x235=(cj2)*(cj2);
00669 IkReal x236=((cj2)*(sj2));
00670 IkReal x237=((IkReal(633.000000000000))*(py)*(sj3));
00671 IkReal x238=((IkReal(633.000000000000))*(px)*(sj3));
00672 if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x235)*(x237)))+(((x236)*(x238))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x235)*(x238)))+(((IkReal(-1.00000000000000))*(x236)*(x237))))))) < IKFAST_ATAN2_MAGTHRESH )
00673     continue;
00674 j0array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x235)*(x237)))+(((x236)*(x238)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(x235)*(x238)))+(((IkReal(-1.00000000000000))*(x236)*(x237)))))));
00675 sj0array[0]=IKsin(j0array[0]);
00676 cj0array[0]=IKcos(j0array[0]);
00677 if( j0array[0] > IKPI )
00678 {
00679     j0array[0]-=IK2PI;
00680 }
00681 else if( j0array[0] < -IKPI )
00682 {    j0array[0]+=IK2PI;
00683 }
00684 j0valid[0] = true;
00685 for(int ij0 = 0; ij0 < 1; ++ij0)
00686 {
00687 if( !j0valid[ij0] )
00688 {
00689     continue;
00690 }
00691 _ij0[0] = ij0; _ij0[1] = -1;
00692 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00693 {
00694 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00695 {
00696     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00697 }
00698 }
00699 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00700 {
00701 IkReal evalcond[4];
00702 IkReal x239=IKsin(j0);
00703 IkReal x240=IKcos(j0);
00704 IkReal x241=((IkReal(0.316500000000000))*(sj3));
00705 IkReal x242=((px)*(x239));
00706 IkReal x243=((px)*(x240));
00707 IkReal x244=((py)*(x239));
00708 IkReal x245=((IkReal(1.00000000000000))*(py)*(x240));
00709 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x241)))+(x242)+(((IkReal(-1.00000000000000))*(x245))));
00710 evalcond[1]=((x243)+(x244)+(((cj2)*(x241))));
00711 evalcond[2]=((((sj2)*(x243)))+(((sj2)*(x244)))+(((IkReal(-1.00000000000000))*(cj2)*(x245)))+(((cj2)*(x242))));
00712 evalcond[3]=((((IkReal(-1.00000000000000))*(sj2)*(x242)))+(x241)+(((py)*(sj2)*(x240)))+(((cj2)*(x244)))+(((cj2)*(x243))));
00713 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00714 {
00715 continue;
00716 }
00717 }
00718 
00719 rotationfunction0(solutions);
00720 }
00721 }
00722 
00723 }
00724 
00725 }
00726 
00727 } else
00728 {
00729 {
00730 IkReal j0array[1], cj0array[1], sj0array[1];
00731 bool j0valid[1]={false};
00732 _nj0 = 1;
00733 IkReal x246=((IkReal(633.000000000000))*(sj3));
00734 if( IKabs(((gconst2)*(((((px)*(sj2)*(x246)))+(((IkReal(-1.00000000000000))*(cj2)*(py)*(x246))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(py)*(sj2)*(x246)))+(((IkReal(-1.00000000000000))*(cj2)*(px)*(x246))))))) < IKFAST_ATAN2_MAGTHRESH )
00735     continue;
00736 j0array[0]=IKatan2(((gconst2)*(((((px)*(sj2)*(x246)))+(((IkReal(-1.00000000000000))*(cj2)*(py)*(x246)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(py)*(sj2)*(x246)))+(((IkReal(-1.00000000000000))*(cj2)*(px)*(x246)))))));
00737 sj0array[0]=IKsin(j0array[0]);
00738 cj0array[0]=IKcos(j0array[0]);
00739 if( j0array[0] > IKPI )
00740 {
00741     j0array[0]-=IK2PI;
00742 }
00743 else if( j0array[0] < -IKPI )
00744 {    j0array[0]+=IK2PI;
00745 }
00746 j0valid[0] = true;
00747 for(int ij0 = 0; ij0 < 1; ++ij0)
00748 {
00749 if( !j0valid[ij0] )
00750 {
00751     continue;
00752 }
00753 _ij0[0] = ij0; _ij0[1] = -1;
00754 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00755 {
00756 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00757 {
00758     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00759 }
00760 }
00761 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00762 {
00763 IkReal evalcond[4];
00764 IkReal x247=IKsin(j0);
00765 IkReal x248=IKcos(j0);
00766 IkReal x249=((IkReal(0.316500000000000))*(sj3));
00767 IkReal x250=((px)*(x247));
00768 IkReal x251=((px)*(x248));
00769 IkReal x252=((py)*(x247));
00770 IkReal x253=((IkReal(1.00000000000000))*(py)*(x248));
00771 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x249)))+(x250)+(((IkReal(-1.00000000000000))*(x253))));
00772 evalcond[1]=((x251)+(x252)+(((cj2)*(x249))));
00773 evalcond[2]=((((cj2)*(x250)))+(((sj2)*(x251)))+(((sj2)*(x252)))+(((IkReal(-1.00000000000000))*(cj2)*(x253))));
00774 evalcond[3]=((((cj2)*(x251)))+(((cj2)*(x252)))+(x249)+(((py)*(sj2)*(x248)))+(((IkReal(-1.00000000000000))*(sj2)*(x250))));
00775 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00776 {
00777 continue;
00778 }
00779 }
00780 
00781 rotationfunction0(solutions);
00782 }
00783 }
00784 
00785 }
00786 
00787 }
00788 
00789 } else
00790 {
00791 IkReal x254=((IkReal(1.00000000000000))*(pp));
00792 IkReal x255=((IkReal(0.316500000000000))*(cj3));
00793 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959))));
00794 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(0.258264000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x254))));
00795 evalcond[2]=((IkReal(0.408000000000000))+(((IkReal(-1.00000000000000))*(pz)))+(x255));
00796 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(0.816000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x254))));
00797 evalcond[4]=((IkReal(-0.408000000000000))+(pz)+(((IkReal(-1.00000000000000))*(x255))));
00798 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  )
00799 {
00800 {
00801 IkReal dummyeval[1];
00802 IkReal gconst6;
00803 gconst6=IKsign(((((IkReal(2000.00000000000))*((py)*(py))))+(((IkReal(2000.00000000000))*((px)*(px))))));
00804 dummyeval[0]=(((px)*(px))+((py)*(py)));
00805 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00806 {
00807 {
00808 IkReal dummyeval[1];
00809 IkReal gconst7;
00810 IkReal x256=((IkReal(2000.00000000000))*(cj2));
00811 gconst7=IKsign(((((IkReal(-1.00000000000000))*(x256)*((px)*(px))))+(((IkReal(-1.00000000000000))*(x256)*((py)*(py))))));
00812 IkReal x257=((IkReal(1.00000000000000))*(cj2));
00813 dummyeval[0]=((((IkReal(-1.00000000000000))*(x257)*((py)*(py))))+(((IkReal(-1.00000000000000))*(x257)*((px)*(px)))));
00814 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00815 {
00816 {
00817 IkReal evalcond[5];
00818 IkReal x258=((IkReal(1.00000000000000))*(pp));
00819 IkReal x259=((IkReal(0.316500000000000))*(cj3));
00820 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j2)), IkReal(6.28318530717959))));
00821 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(0.258264000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x258))));
00822 evalcond[2]=((IkReal(0.408000000000000))+(((IkReal(-1.00000000000000))*(pz)))+(x259));
00823 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(0.816000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x258))));
00824 evalcond[4]=((IkReal(-0.408000000000000))+(pz)+(((IkReal(-1.00000000000000))*(x259))));
00825 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  )
00826 {
00827 {
00828 IkReal dummyeval[1];
00829 IkReal gconst8;
00830 gconst8=IKsign(((((IkReal(2000.00000000000))*((py)*(py))))+(((IkReal(2000.00000000000))*((px)*(px))))));
00831 dummyeval[0]=(((px)*(px))+((py)*(py)));
00832 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00833 {
00834 continue;
00835 
00836 } else
00837 {
00838 {
00839 IkReal j0array[1], cj0array[1], sj0array[1];
00840 bool j0valid[1]={false};
00841 _nj0 = 1;
00842 IkReal x260=((gconst8)*(sj3));
00843 if( IKabs(((IkReal(633.000000000000))*(px)*(x260))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-633.000000000000))*(py)*(x260))) < IKFAST_ATAN2_MAGTHRESH )
00844     continue;
00845 j0array[0]=IKatan2(((IkReal(633.000000000000))*(px)*(x260)), ((IkReal(-633.000000000000))*(py)*(x260)));
00846 sj0array[0]=IKsin(j0array[0]);
00847 cj0array[0]=IKcos(j0array[0]);
00848 if( j0array[0] > IKPI )
00849 {
00850     j0array[0]-=IK2PI;
00851 }
00852 else if( j0array[0] < -IKPI )
00853 {    j0array[0]+=IK2PI;
00854 }
00855 j0valid[0] = true;
00856 for(int ij0 = 0; ij0 < 1; ++ij0)
00857 {
00858 if( !j0valid[ij0] )
00859 {
00860     continue;
00861 }
00862 _ij0[0] = ij0; _ij0[1] = -1;
00863 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00864 {
00865 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00866 {
00867     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00868 }
00869 }
00870 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00871 {
00872 IkReal evalcond[2];
00873 IkReal x261=IKsin(j0);
00874 IkReal x262=IKcos(j0);
00875 evalcond[0]=((((IkReal(-0.316500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(py)*(x262)))+(((px)*(x261))));
00876 evalcond[1]=((((py)*(x261)))+(((px)*(x262))));
00877 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00878 {
00879 continue;
00880 }
00881 }
00882 
00883 rotationfunction0(solutions);
00884 }
00885 }
00886 
00887 }
00888 
00889 }
00890 
00891 } else
00892 {
00893 IkReal x263=((IkReal(1.00000000000000))*(pp));
00894 IkReal x264=((IkReal(0.316500000000000))*(cj3));
00895 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j2)), IkReal(6.28318530717959))));
00896 evalcond[1]=((IkReal(0.266636250000000))+(((IkReal(-1.00000000000000))*(x263)))+(((IkReal(0.258264000000000))*(cj3))));
00897 evalcond[2]=((IkReal(0.408000000000000))+(((IkReal(-1.00000000000000))*(pz)))+(x264));
00898 evalcond[3]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x263)))+(((IkReal(0.816000000000000))*(pz))));
00899 evalcond[4]=((IkReal(-0.408000000000000))+(pz)+(((IkReal(-1.00000000000000))*(x264))));
00900 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  )
00901 {
00902 {
00903 IkReal dummyeval[1];
00904 IkReal gconst9;
00905 gconst9=IKsign(((((IkReal(-2000.00000000000))*((py)*(py))))+(((IkReal(-2000.00000000000))*((px)*(px))))));
00906 dummyeval[0]=((((IkReal(-1.00000000000000))*((px)*(px))))+(((IkReal(-1.00000000000000))*((py)*(py)))));
00907 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00908 {
00909 continue;
00910 
00911 } else
00912 {
00913 {
00914 IkReal j0array[1], cj0array[1], sj0array[1];
00915 bool j0valid[1]={false};
00916 _nj0 = 1;
00917 IkReal x265=((gconst9)*(sj3));
00918 if( IKabs(((IkReal(633.000000000000))*(px)*(x265))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-633.000000000000))*(py)*(x265))) < IKFAST_ATAN2_MAGTHRESH )
00919     continue;
00920 j0array[0]=IKatan2(((IkReal(633.000000000000))*(px)*(x265)), ((IkReal(-633.000000000000))*(py)*(x265)));
00921 sj0array[0]=IKsin(j0array[0]);
00922 cj0array[0]=IKcos(j0array[0]);
00923 if( j0array[0] > IKPI )
00924 {
00925     j0array[0]-=IK2PI;
00926 }
00927 else if( j0array[0] < -IKPI )
00928 {    j0array[0]+=IK2PI;
00929 }
00930 j0valid[0] = true;
00931 for(int ij0 = 0; ij0 < 1; ++ij0)
00932 {
00933 if( !j0valid[ij0] )
00934 {
00935     continue;
00936 }
00937 _ij0[0] = ij0; _ij0[1] = -1;
00938 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
00939 {
00940 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00941 {
00942     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00943 }
00944 }
00945 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00946 {
00947 IkReal evalcond[2];
00948 IkReal x266=IKsin(j0);
00949 IkReal x267=IKcos(j0);
00950 evalcond[0]=((((IkReal(0.316500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(py)*(x267)))+(((px)*(x266))));
00951 evalcond[1]=((((py)*(x266)))+(((px)*(x267))));
00952 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00953 {
00954 continue;
00955 }
00956 }
00957 
00958 rotationfunction0(solutions);
00959 }
00960 }
00961 
00962 }
00963 
00964 }
00965 
00966 } else
00967 {
00968 if( 1 )
00969 {
00970 continue;
00971 
00972 } else
00973 {
00974 }
00975 }
00976 }
00977 }
00978 
00979 } else
00980 {
00981 {
00982 IkReal j0array[1], cj0array[1], sj0array[1];
00983 bool j0valid[1]={false};
00984 _nj0 = 1;
00985 IkReal x268=(cj2)*(cj2);
00986 IkReal x269=((IkReal(633.000000000000))*(sj3));
00987 IkReal x270=((cj2)*(sj2));
00988 if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(px)*(x269)*(x270)))+(((IkReal(-1.00000000000000))*(py)*(x268)*(x269))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(px)*(x268)*(x269)))+(((py)*(x269)*(x270))))))) < IKFAST_ATAN2_MAGTHRESH )
00989     continue;
00990 j0array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(px)*(x269)*(x270)))+(((IkReal(-1.00000000000000))*(py)*(x268)*(x269)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(px)*(x268)*(x269)))+(((py)*(x269)*(x270)))))));
00991 sj0array[0]=IKsin(j0array[0]);
00992 cj0array[0]=IKcos(j0array[0]);
00993 if( j0array[0] > IKPI )
00994 {
00995     j0array[0]-=IK2PI;
00996 }
00997 else if( j0array[0] < -IKPI )
00998 {    j0array[0]+=IK2PI;
00999 }
01000 j0valid[0] = true;
01001 for(int ij0 = 0; ij0 < 1; ++ij0)
01002 {
01003 if( !j0valid[ij0] )
01004 {
01005     continue;
01006 }
01007 _ij0[0] = ij0; _ij0[1] = -1;
01008 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
01009 {
01010 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01011 {
01012     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01013 }
01014 }
01015 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01016 {
01017 IkReal evalcond[4];
01018 IkReal x271=IKsin(j0);
01019 IkReal x272=IKcos(j0);
01020 IkReal x273=((IkReal(1.00000000000000))*(sj2));
01021 IkReal x274=((IkReal(0.316500000000000))*(sj3));
01022 IkReal x275=((IkReal(1.00000000000000))*(cj2));
01023 IkReal x276=((px)*(x271));
01024 IkReal x277=((px)*(x272));
01025 IkReal x278=((py)*(x271));
01026 IkReal x279=((IkReal(1.00000000000000))*(py)*(x272));
01027 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x274)))+(x276)+(((IkReal(-1.00000000000000))*(x279))));
01028 evalcond[1]=((((IkReal(-1.00000000000000))*(cj2)*(x274)))+(x277)+(x278));
01029 evalcond[2]=((((IkReal(-1.00000000000000))*(x273)*(x277)))+(((IkReal(-1.00000000000000))*(x273)*(x278)))+(((IkReal(-1.00000000000000))*(py)*(x272)*(x275)))+(((cj2)*(x276))));
01030 evalcond[3]=((((IkReal(-1.00000000000000))*(x273)*(x276)))+(x274)+(((py)*(sj2)*(x272)))+(((IkReal(-1.00000000000000))*(x275)*(x277)))+(((IkReal(-1.00000000000000))*(x275)*(x278))));
01031 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01032 {
01033 continue;
01034 }
01035 }
01036 
01037 rotationfunction0(solutions);
01038 }
01039 }
01040 
01041 }
01042 
01043 }
01044 
01045 } else
01046 {
01047 {
01048 IkReal j0array[1], cj0array[1], sj0array[1];
01049 bool j0valid[1]={false};
01050 _nj0 = 1;
01051 IkReal x280=((IkReal(633.000000000000))*(sj3));
01052 if( IKabs(((gconst6)*(((((px)*(sj2)*(x280)))+(((cj2)*(py)*(x280))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(py)*(sj2)*(x280)))+(((cj2)*(px)*(x280))))))) < IKFAST_ATAN2_MAGTHRESH )
01053     continue;
01054 j0array[0]=IKatan2(((gconst6)*(((((px)*(sj2)*(x280)))+(((cj2)*(py)*(x280)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(py)*(sj2)*(x280)))+(((cj2)*(px)*(x280)))))));
01055 sj0array[0]=IKsin(j0array[0]);
01056 cj0array[0]=IKcos(j0array[0]);
01057 if( j0array[0] > IKPI )
01058 {
01059     j0array[0]-=IK2PI;
01060 }
01061 else if( j0array[0] < -IKPI )
01062 {    j0array[0]+=IK2PI;
01063 }
01064 j0valid[0] = true;
01065 for(int ij0 = 0; ij0 < 1; ++ij0)
01066 {
01067 if( !j0valid[ij0] )
01068 {
01069     continue;
01070 }
01071 _ij0[0] = ij0; _ij0[1] = -1;
01072 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
01073 {
01074 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01075 {
01076     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01077 }
01078 }
01079 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01080 {
01081 IkReal evalcond[4];
01082 IkReal x281=IKsin(j0);
01083 IkReal x282=IKcos(j0);
01084 IkReal x283=((IkReal(1.00000000000000))*(sj2));
01085 IkReal x284=((IkReal(0.316500000000000))*(sj3));
01086 IkReal x285=((IkReal(1.00000000000000))*(cj2));
01087 IkReal x286=((px)*(x281));
01088 IkReal x287=((px)*(x282));
01089 IkReal x288=((py)*(x281));
01090 IkReal x289=((IkReal(1.00000000000000))*(py)*(x282));
01091 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x284)))+(x286)+(((IkReal(-1.00000000000000))*(x289))));
01092 evalcond[1]=((((IkReal(-1.00000000000000))*(cj2)*(x284)))+(x288)+(x287));
01093 evalcond[2]=((((cj2)*(x286)))+(((IkReal(-1.00000000000000))*(py)*(x282)*(x285)))+(((IkReal(-1.00000000000000))*(x283)*(x287)))+(((IkReal(-1.00000000000000))*(x283)*(x288))));
01094 evalcond[3]=((((py)*(sj2)*(x282)))+(x284)+(((IkReal(-1.00000000000000))*(x285)*(x287)))+(((IkReal(-1.00000000000000))*(x285)*(x288)))+(((IkReal(-1.00000000000000))*(x283)*(x286))));
01095 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01096 {
01097 continue;
01098 }
01099 }
01100 
01101 rotationfunction0(solutions);
01102 }
01103 }
01104 
01105 }
01106 
01107 }
01108 
01109 } else
01110 {
01111 if( 1 )
01112 {
01113 continue;
01114 
01115 } else
01116 {
01117 }
01118 }
01119 }
01120 }
01121 
01122 } else
01123 {
01124 {
01125 IkReal j0array[1], cj0array[1], sj0array[1];
01126 bool j0valid[1]={false};
01127 _nj0 = 1;
01128 IkReal x290=((IkReal(633.000000000000))*(px));
01129 IkReal x291=((IkReal(633.000000000000))*(py));
01130 IkReal x292=((IkReal(2000.00000000000))*(cj1)*(pz));
01131 IkReal x293=((sj1)*(sj2)*(sj3));
01132 if( IKabs(((gconst1)*(((((x290)*(x293)))+(((IkReal(-1.00000000000000))*(py)*(x292)))+(((IkReal(-816.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(cj3)*(x291))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x291)*(x293)))+(((IkReal(-816.000000000000))*(px)))+(((IkReal(-1.00000000000000))*(cj3)*(x290)))+(((IkReal(-1.00000000000000))*(px)*(x292))))))) < IKFAST_ATAN2_MAGTHRESH )
01133     continue;
01134 j0array[0]=IKatan2(((gconst1)*(((((x290)*(x293)))+(((IkReal(-1.00000000000000))*(py)*(x292)))+(((IkReal(-816.000000000000))*(py)))+(((IkReal(-1.00000000000000))*(cj3)*(x291)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x291)*(x293)))+(((IkReal(-816.000000000000))*(px)))+(((IkReal(-1.00000000000000))*(cj3)*(x290)))+(((IkReal(-1.00000000000000))*(px)*(x292)))))));
01135 sj0array[0]=IKsin(j0array[0]);
01136 cj0array[0]=IKcos(j0array[0]);
01137 if( j0array[0] > IKPI )
01138 {
01139     j0array[0]-=IK2PI;
01140 }
01141 else if( j0array[0] < -IKPI )
01142 {    j0array[0]+=IK2PI;
01143 }
01144 j0valid[0] = true;
01145 for(int ij0 = 0; ij0 < 1; ++ij0)
01146 {
01147 if( !j0valid[ij0] )
01148 {
01149     continue;
01150 }
01151 _ij0[0] = ij0; _ij0[1] = -1;
01152 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
01153 {
01154 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01155 {
01156     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01157 }
01158 }
01159 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01160 {
01161 IkReal evalcond[6];
01162 IkReal x294=IKcos(j0);
01163 IkReal x295=IKsin(j0);
01164 IkReal x296=((cj1)*(sj2));
01165 IkReal x297=((cj1)*(cj2));
01166 IkReal x298=((cj1)*(pz));
01167 IkReal x299=((IkReal(0.316500000000000))*(cj3));
01168 IkReal x300=((IkReal(1.00000000000000))*(sj2));
01169 IkReal x301=((IkReal(0.816000000000000))*(sj1));
01170 IkReal x302=((IkReal(1.00000000000000))*(cj2));
01171 IkReal x303=((pz)*(sj1));
01172 IkReal x304=((IkReal(0.316500000000000))*(sj3));
01173 IkReal x305=((IkReal(1.00000000000000))*(sj1));
01174 IkReal x306=((px)*(x295));
01175 IkReal x307=((px)*(x294));
01176 IkReal x308=((py)*(x295));
01177 IkReal x309=((py)*(x294));
01178 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x304)))+(((IkReal(-1.00000000000000))*(x309)))+(x306));
01179 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x301)*(x308)))+(((IkReal(-1.00000000000000))*(x301)*(x307)))+(((IkReal(-0.816000000000000))*(x298)))+(((IkReal(-1.00000000000000))*(pp))));
01180 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x298)))+(((IkReal(-1.00000000000000))*(x299)))+(((IkReal(-1.00000000000000))*(x305)*(x307)))+(((IkReal(-1.00000000000000))*(x305)*(x308))));
01181 evalcond[3]=((((sj1)*(x299)))+(x308)+(x307)+(((x297)*(x304)))+(((IkReal(0.408000000000000))*(sj1))));
01182 evalcond[4]=((((IkReal(-1.00000000000000))*(x302)*(x309)))+(((cj2)*(x306)))+(((x296)*(x307)))+(((x296)*(x308)))+(((IkReal(-1.00000000000000))*(x300)*(x303))));
01183 evalcond[5]=((((sj2)*(x309)))+(((IkReal(-1.00000000000000))*(x302)*(x303)))+(x304)+(((x297)*(x307)))+(((x297)*(x308)))+(((IkReal(-1.00000000000000))*(x300)*(x306))));
01184 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  )
01185 {
01186 continue;
01187 }
01188 }
01189 
01190 rotationfunction0(solutions);
01191 }
01192 }
01193 
01194 }
01195 
01196 }
01197 
01198 } else
01199 {
01200 {
01201 IkReal j0array[1], cj0array[1], sj0array[1];
01202 bool j0valid[1]={false};
01203 _nj0 = 1;
01204 IkReal x310=((IkReal(125000.000000000))*(pp));
01205 IkReal x311=((IkReal(102000.000000000))*(cj1)*(pz));
01206 IkReal x312=((IkReal(32283.0000000000))*(sj1)*(sj2)*(sj3));
01207 if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(x311)))+(((IkReal(-1.00000000000000))*(py)*(x310)))+(((px)*(x312)))+(((IkReal(-8286.46875000000))*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(x312)))+(((IkReal(-1.00000000000000))*(px)*(x310)))+(((IkReal(-1.00000000000000))*(px)*(x311)))+(((IkReal(-8286.46875000000))*(px))))))) < IKFAST_ATAN2_MAGTHRESH )
01208     continue;
01209 j0array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(x311)))+(((IkReal(-1.00000000000000))*(py)*(x310)))+(((px)*(x312)))+(((IkReal(-8286.46875000000))*(py)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(x312)))+(((IkReal(-1.00000000000000))*(px)*(x310)))+(((IkReal(-1.00000000000000))*(px)*(x311)))+(((IkReal(-8286.46875000000))*(px)))))));
01210 sj0array[0]=IKsin(j0array[0]);
01211 cj0array[0]=IKcos(j0array[0]);
01212 if( j0array[0] > IKPI )
01213 {
01214     j0array[0]-=IK2PI;
01215 }
01216 else if( j0array[0] < -IKPI )
01217 {    j0array[0]+=IK2PI;
01218 }
01219 j0valid[0] = true;
01220 for(int ij0 = 0; ij0 < 1; ++ij0)
01221 {
01222 if( !j0valid[ij0] )
01223 {
01224     continue;
01225 }
01226 _ij0[0] = ij0; _ij0[1] = -1;
01227 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
01228 {
01229 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01230 {
01231     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01232 }
01233 }
01234 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01235 {
01236 IkReal evalcond[6];
01237 IkReal x313=IKcos(j0);
01238 IkReal x314=IKsin(j0);
01239 IkReal x315=((cj1)*(sj2));
01240 IkReal x316=((cj1)*(cj2));
01241 IkReal x317=((cj1)*(pz));
01242 IkReal x318=((IkReal(0.316500000000000))*(cj3));
01243 IkReal x319=((IkReal(1.00000000000000))*(sj2));
01244 IkReal x320=((IkReal(0.816000000000000))*(sj1));
01245 IkReal x321=((IkReal(1.00000000000000))*(cj2));
01246 IkReal x322=((pz)*(sj1));
01247 IkReal x323=((IkReal(0.316500000000000))*(sj3));
01248 IkReal x324=((IkReal(1.00000000000000))*(sj1));
01249 IkReal x325=((px)*(x314));
01250 IkReal x326=((px)*(x313));
01251 IkReal x327=((py)*(x314));
01252 IkReal x328=((py)*(x313));
01253 evalcond[0]=((((IkReal(-1.00000000000000))*(x328)))+(x325)+(((IkReal(-1.00000000000000))*(sj2)*(x323))));
01254 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x320)*(x327)))+(((IkReal(-1.00000000000000))*(x320)*(x326)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.816000000000000))*(x317))));
01255 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x318)))+(((IkReal(-1.00000000000000))*(x317)))+(((IkReal(-1.00000000000000))*(x324)*(x326)))+(((IkReal(-1.00000000000000))*(x324)*(x327))));
01256 evalcond[3]=((((sj1)*(x318)))+(x326)+(x327)+(((IkReal(0.408000000000000))*(sj1)))+(((x316)*(x323))));
01257 evalcond[4]=((((cj2)*(x325)))+(((IkReal(-1.00000000000000))*(x321)*(x328)))+(((IkReal(-1.00000000000000))*(x319)*(x322)))+(((x315)*(x326)))+(((x315)*(x327))));
01258 evalcond[5]=((((IkReal(-1.00000000000000))*(x321)*(x322)))+(x323)+(((sj2)*(x328)))+(((IkReal(-1.00000000000000))*(x319)*(x325)))+(((x316)*(x327)))+(((x316)*(x326))));
01259 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  )
01260 {
01261 continue;
01262 }
01263 }
01264 
01265 rotationfunction0(solutions);
01266 }
01267 }
01268 
01269 }
01270 
01271 }
01272 }
01273 }
01274 
01275 }
01276 
01277 }
01278 
01279 } else
01280 {
01281 {
01282 IkReal j0array[2], cj0array[2], sj0array[2];
01283 bool j0valid[2]={false};
01284 _nj0 = 2;
01285 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
01286     continue;
01287 IkReal x329=((IkReal(1.00000000000000))*(IKatan2(((IkReal(-1.00000000000000))*(py)), px)));
01288 if( ((((px)*(px))+((py)*(py)))) < (IkReal)-0.00001 )
01289     continue;
01290 if( (((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
01291     continue;
01292 IkReal x330=IKasin(((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30))));
01293 j0array[0]=((((IkReal(-1.00000000000000))*(x329)))+(x330));
01294 sj0array[0]=IKsin(j0array[0]);
01295 cj0array[0]=IKcos(j0array[0]);
01296 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x329)))+(((IkReal(-1.00000000000000))*(x330))));
01297 sj0array[1]=IKsin(j0array[1]);
01298 cj0array[1]=IKcos(j0array[1]);
01299 if( j0array[0] > IKPI )
01300 {
01301     j0array[0]-=IK2PI;
01302 }
01303 else if( j0array[0] < -IKPI )
01304 {    j0array[0]+=IK2PI;
01305 }
01306 j0valid[0] = true;
01307 if( j0array[1] > IKPI )
01308 {
01309     j0array[1]-=IK2PI;
01310 }
01311 else if( j0array[1] < -IKPI )
01312 {    j0array[1]+=IK2PI;
01313 }
01314 j0valid[1] = true;
01315 for(int ij0 = 0; ij0 < 2; ++ij0)
01316 {
01317 if( !j0valid[ij0] )
01318 {
01319     continue;
01320 }
01321 _ij0[0] = ij0; _ij0[1] = -1;
01322 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
01323 {
01324 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01325 {
01326     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01327 }
01328 }
01329 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01330 {
01331 IkReal evalcond[2];
01332 IkReal x331=(sj2)*(sj2);
01333 IkReal x332=(cj2)*(cj2);
01334 IkReal x333=(py)*(py);
01335 IkReal x334=(px)*(px);
01336 IkReal x335=IKsin(j0);
01337 IkReal x336=IKcos(j0);
01338 IkReal x337=((IkReal(0.316500000000000))*(sj2)*(sj3));
01339 IkReal x338=((IkReal(1.00000000000000))*(x333));
01340 IkReal x339=((px)*(py)*(x331));
01341 IkReal x340=((px)*(py)*(x332));
01342 evalcond[0]=((((IkReal(-1.00000000000000))*(py)*(x337)))+(((x335)*(((x339)+(x340)))))+(((x336)*(((((IkReal(-1.00000000000000))*(x331)*(x338)))+(((IkReal(-1.00000000000000))*(x332)*(x338))))))));
01343 evalcond[1]=((((IkReal(-1.00000000000000))*(px)*(x337)))+(((x336)*(((((IkReal(-1.00000000000000))*(x339)))+(((IkReal(-1.00000000000000))*(x340)))))))+(((x335)*(((((x331)*(x334)))+(((x332)*(x334))))))));
01344 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
01345 {
01346 continue;
01347 }
01348 }
01349 
01350 {
01351 IkReal dummyeval[1];
01352 IkReal gconst11;
01353 IkReal x341=((py)*(sj0));
01354 IkReal x342=((IkReal(633.000000000000))*(cj3));
01355 IkReal x343=((cj0)*(px));
01356 gconst11=IKsign(((((IkReal(633.000000000000))*(cj2)*(pz)*(sj3)))+(((x341)*(x342)))+(((IkReal(816.000000000000))*(x341)))+(((IkReal(816.000000000000))*(x343)))+(((x342)*(x343)))));
01357 IkReal x344=((cj0)*(px));
01358 IkReal x345=((py)*(sj0));
01359 dummyeval[0]=((((cj2)*(pz)*(sj3)))+(((IkReal(1.28909952606635))*(x345)))+(((IkReal(1.28909952606635))*(x344)))+(((cj3)*(x344)))+(((cj3)*(x345))));
01360 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01361 {
01362 {
01363 IkReal dummyeval[1];
01364 IkReal gconst10;
01365 gconst10=IKsign(((IkReal(665856.000000000))+(((IkReal(1033056.00000000))*(cj3)))+(((IkReal(400689.000000000))*((cj3)*(cj3))))+(((IkReal(400689.000000000))*((cj2)*(cj2))*((sj3)*(sj3))))));
01366 dummyeval[0]=((IkReal(1.66177758810449))+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((IkReal(2.57819905213270))*(cj3))));
01367 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01368 {
01369 {
01370 IkReal dummyeval[1];
01371 IkReal x346=((cj0)*(px));
01372 IkReal x347=((py)*(sj0));
01373 dummyeval[0]=((((cj2)*(pz)*(sj3)))+(((IkReal(1.28909952606635))*(x347)))+(((IkReal(1.28909952606635))*(x346)))+(((cj3)*(x346)))+(((cj3)*(x347))));
01374 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01375 {
01376 continue;
01377 
01378 } else
01379 {
01380 {
01381 IkReal j1array[1], cj1array[1], sj1array[1];
01382 bool j1valid[1]={false};
01383 _nj1 = 1;
01384 IkReal x348=((py)*(sj0));
01385 IkReal x349=((IkReal(32283.0000000000))*(cj3));
01386 IkReal x350=((cj2)*(sj3));
01387 IkReal x351=((IkReal(64566.0000000000))*(cj3));
01388 IkReal x352=((cj0)*(px));
01389 IkReal x353=((IkReal(204000.000000000))*(pz));
01390 if( IKabs(((((IKabs(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x350)))+(((IkReal(-1.00000000000000))*(x348)*(x353)))+(((IkReal(-79125.0000000000))*(pp)*(x350)))+(((IkReal(-1.00000000000000))*(x352)*(x353))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz))))))))+IKsqr(((((IKabs(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x350)))+(((IkReal(-1.00000000000000))*(x348)*(x353)))+(((IkReal(-79125.0000000000))*(pp)*(x350)))+(((IkReal(-1.00000000000000))*(x352)*(x353)))))))-1) <= IKFAST_SINCOS_THRESH )
01391     continue;
01392 j1array[0]=IKatan2(((((IKabs(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((x348)*(x349)))+(((IkReal(41616.0000000000))*(x348)))+(((IkReal(41616.0000000000))*(x352)))+(((x349)*(x352)))+(((IkReal(32283.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz))))))), ((((IKabs(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350))))) != 0)?((IkReal)1/(((((IkReal(83232.0000000000))*(x348)))+(((IkReal(83232.0000000000))*(x352)))+(((x351)*(x352)))+(((x348)*(x351)))+(((IkReal(64566.0000000000))*(pz)*(x350)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x350)))+(((IkReal(-1.00000000000000))*(x348)*(x353)))+(((IkReal(-79125.0000000000))*(pp)*(x350)))+(((IkReal(-1.00000000000000))*(x352)*(x353)))))));
01393 sj1array[0]=IKsin(j1array[0]);
01394 cj1array[0]=IKcos(j1array[0]);
01395 if( j1array[0] > IKPI )
01396 {
01397     j1array[0]-=IK2PI;
01398 }
01399 else if( j1array[0] < -IKPI )
01400 {    j1array[0]+=IK2PI;
01401 }
01402 j1valid[0] = true;
01403 for(int ij1 = 0; ij1 < 1; ++ij1)
01404 {
01405 if( !j1valid[ij1] )
01406 {
01407     continue;
01408 }
01409 _ij1[0] = ij1; _ij1[1] = -1;
01410 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01411 {
01412 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01413 {
01414     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01415 }
01416 }
01417 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01418 {
01419 IkReal evalcond[6];
01420 IkReal x354=IKcos(j1);
01421 IkReal x355=IKsin(j1);
01422 IkReal x356=((py)*(sj0));
01423 IkReal x357=((cj0)*(px));
01424 IkReal x358=((IkReal(0.316500000000000))*(cj3));
01425 IkReal x359=((cj0)*(py));
01426 IkReal x360=((IkReal(0.316500000000000))*(sj3));
01427 IkReal x361=((px)*(sj0));
01428 IkReal x362=((IkReal(0.816000000000000))*(x355));
01429 IkReal x363=((sj2)*(x354));
01430 IkReal x364=((IkReal(1.00000000000000))*(x355));
01431 IkReal x365=((pz)*(x354));
01432 IkReal x366=((cj2)*(x354));
01433 evalcond[0]=((((IkReal(-0.408000000000000))*(x354)))+(((cj2)*(x355)*(x360)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x354)*(x358))));
01434 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-0.816000000000000))*(x365)))+(((IkReal(-1.00000000000000))*(x356)*(x362)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(x357)*(x362))));
01435 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x365)))+(((IkReal(-1.00000000000000))*(x358)))+(((IkReal(-1.00000000000000))*(x356)*(x364)))+(((IkReal(-1.00000000000000))*(x357)*(x364))));
01436 evalcond[3]=((((IkReal(0.408000000000000))*(x355)))+(((x355)*(x358)))+(x357)+(x356)+(((x360)*(x366))));
01437 evalcond[4]=((((IkReal(-1.00000000000000))*(cj2)*(x359)))+(((IkReal(-1.00000000000000))*(pz)*(sj2)*(x364)))+(((x356)*(x363)))+(((cj2)*(x361)))+(((x357)*(x363))));
01438 evalcond[5]=((((IkReal(-1.00000000000000))*(sj2)*(x361)))+(((x356)*(x366)))+(((sj2)*(x359)))+(x360)+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x364)))+(((x357)*(x366))));
01439 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  )
01440 {
01441 continue;
01442 }
01443 }
01444 
01445 rotationfunction0(solutions);
01446 }
01447 }
01448 
01449 }
01450 
01451 }
01452 
01453 } else
01454 {
01455 {
01456 IkReal j1array[1], cj1array[1], sj1array[1];
01457 bool j1valid[1]={false};
01458 _nj1 = 1;
01459 IkReal x367=((IkReal(1266000.00000000))*(cj3));
01460 IkReal x368=((cj0)*(px));
01461 IkReal x369=((py)*(sj0));
01462 IkReal x370=((IkReal(1266000.00000000))*(cj2)*(sj3));
01463 if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(x367)*(x369)))+(((IkReal(-1.00000000000000))*(x367)*(x368)))+(((pz)*(x370)))+(((IkReal(-1632000.00000000))*(x369)))+(((IkReal(-1632000.00000000))*(x368))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(x368)*(x370)))+(((IkReal(-1632000.00000000))*(pz)))+(((IkReal(-1.00000000000000))*(x369)*(x370)))+(((IkReal(-1.00000000000000))*(pz)*(x367))))))) < IKFAST_ATAN2_MAGTHRESH )
01464     continue;
01465 j1array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(x367)*(x369)))+(((IkReal(-1.00000000000000))*(x367)*(x368)))+(((pz)*(x370)))+(((IkReal(-1632000.00000000))*(x369)))+(((IkReal(-1632000.00000000))*(x368)))))), ((gconst10)*(((((IkReal(-1.00000000000000))*(x368)*(x370)))+(((IkReal(-1632000.00000000))*(pz)))+(((IkReal(-1.00000000000000))*(x369)*(x370)))+(((IkReal(-1.00000000000000))*(pz)*(x367)))))));
01466 sj1array[0]=IKsin(j1array[0]);
01467 cj1array[0]=IKcos(j1array[0]);
01468 if( j1array[0] > IKPI )
01469 {
01470     j1array[0]-=IK2PI;
01471 }
01472 else if( j1array[0] < -IKPI )
01473 {    j1array[0]+=IK2PI;
01474 }
01475 j1valid[0] = true;
01476 for(int ij1 = 0; ij1 < 1; ++ij1)
01477 {
01478 if( !j1valid[ij1] )
01479 {
01480     continue;
01481 }
01482 _ij1[0] = ij1; _ij1[1] = -1;
01483 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01484 {
01485 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01486 {
01487     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01488 }
01489 }
01490 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01491 {
01492 IkReal evalcond[6];
01493 IkReal x371=IKcos(j1);
01494 IkReal x372=IKsin(j1);
01495 IkReal x373=((py)*(sj0));
01496 IkReal x374=((cj0)*(px));
01497 IkReal x375=((IkReal(0.316500000000000))*(cj3));
01498 IkReal x376=((cj0)*(py));
01499 IkReal x377=((IkReal(0.316500000000000))*(sj3));
01500 IkReal x378=((px)*(sj0));
01501 IkReal x379=((IkReal(0.816000000000000))*(x372));
01502 IkReal x380=((sj2)*(x371));
01503 IkReal x381=((IkReal(1.00000000000000))*(x372));
01504 IkReal x382=((pz)*(x371));
01505 IkReal x383=((cj2)*(x371));
01506 evalcond[0]=((((IkReal(-0.408000000000000))*(x371)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x371)*(x375)))+(((cj2)*(x372)*(x377))));
01507 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x374)*(x379)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(x373)*(x379)))+(((IkReal(-0.816000000000000))*(x382))));
01508 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x373)*(x381)))+(((IkReal(-1.00000000000000))*(x374)*(x381)))+(((IkReal(-1.00000000000000))*(x375)))+(((IkReal(-1.00000000000000))*(x382))));
01509 evalcond[3]=((((x377)*(x383)))+(((x372)*(x375)))+(x374)+(x373)+(((IkReal(0.408000000000000))*(x372))));
01510 evalcond[4]=((((x373)*(x380)))+(((IkReal(-1.00000000000000))*(pz)*(sj2)*(x381)))+(((x374)*(x380)))+(((IkReal(-1.00000000000000))*(cj2)*(x376)))+(((cj2)*(x378))));
01511 evalcond[5]=((((x373)*(x383)))+(((IkReal(-1.00000000000000))*(sj2)*(x378)))+(((x374)*(x383)))+(x377)+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x381)))+(((sj2)*(x376))));
01512 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  )
01513 {
01514 continue;
01515 }
01516 }
01517 
01518 rotationfunction0(solutions);
01519 }
01520 }
01521 
01522 }
01523 
01524 }
01525 
01526 } else
01527 {
01528 {
01529 IkReal j1array[1], cj1array[1], sj1array[1];
01530 bool j1valid[1]={false};
01531 _nj1 = 1;
01532 IkReal x384=((IkReal(2000.00000000000))*(pz));
01533 IkReal x385=((cj2)*(sj3));
01534 if( IKabs(((gconst11)*(((IkReal(-332.928000000000))+(((IkReal(-516.528000000000))*(cj3)))+(((IkReal(-200.344500000000))*((cj3)*(cj3))))+(((pz)*(x384))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((IkReal(-200.344500000000))*(cj3)*(x385)))+(((IkReal(-258.264000000000))*(x385)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x384)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x384))))))) < IKFAST_ATAN2_MAGTHRESH )
01535     continue;
01536 j1array[0]=IKatan2(((gconst11)*(((IkReal(-332.928000000000))+(((IkReal(-516.528000000000))*(cj3)))+(((IkReal(-200.344500000000))*((cj3)*(cj3))))+(((pz)*(x384)))))), ((gconst11)*(((((IkReal(-200.344500000000))*(cj3)*(x385)))+(((IkReal(-258.264000000000))*(x385)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x384)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x384)))))));
01537 sj1array[0]=IKsin(j1array[0]);
01538 cj1array[0]=IKcos(j1array[0]);
01539 if( j1array[0] > IKPI )
01540 {
01541     j1array[0]-=IK2PI;
01542 }
01543 else if( j1array[0] < -IKPI )
01544 {    j1array[0]+=IK2PI;
01545 }
01546 j1valid[0] = true;
01547 for(int ij1 = 0; ij1 < 1; ++ij1)
01548 {
01549 if( !j1valid[ij1] )
01550 {
01551     continue;
01552 }
01553 _ij1[0] = ij1; _ij1[1] = -1;
01554 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01555 {
01556 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01557 {
01558     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01559 }
01560 }
01561 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01562 {
01563 IkReal evalcond[6];
01564 IkReal x386=IKcos(j1);
01565 IkReal x387=IKsin(j1);
01566 IkReal x388=((py)*(sj0));
01567 IkReal x389=((cj0)*(px));
01568 IkReal x390=((IkReal(0.316500000000000))*(cj3));
01569 IkReal x391=((cj0)*(py));
01570 IkReal x392=((IkReal(0.316500000000000))*(sj3));
01571 IkReal x393=((px)*(sj0));
01572 IkReal x394=((IkReal(0.816000000000000))*(x387));
01573 IkReal x395=((sj2)*(x386));
01574 IkReal x396=((IkReal(1.00000000000000))*(x387));
01575 IkReal x397=((pz)*(x386));
01576 IkReal x398=((cj2)*(x386));
01577 evalcond[0]=((((IkReal(-0.408000000000000))*(x386)))+(((cj2)*(x387)*(x392)))+(((IkReal(-1.00000000000000))*(x386)*(x390)))+(((IkReal(-1.00000000000000))*(pz))));
01578 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x389)*(x394)))+(((IkReal(-0.816000000000000))*(x397)))+(((IkReal(-1.00000000000000))*(x388)*(x394)))+(((IkReal(-1.00000000000000))*(pp))));
01579 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x390)))+(((IkReal(-1.00000000000000))*(x397)))+(((IkReal(-1.00000000000000))*(x389)*(x396)))+(((IkReal(-1.00000000000000))*(x388)*(x396))));
01580 evalcond[3]=((((IkReal(0.408000000000000))*(x387)))+(x388)+(x389)+(((x387)*(x390)))+(((x392)*(x398))));
01581 evalcond[4]=((((IkReal(-1.00000000000000))*(cj2)*(x391)))+(((IkReal(-1.00000000000000))*(pz)*(sj2)*(x396)))+(((cj2)*(x393)))+(((x389)*(x395)))+(((x388)*(x395))));
01582 evalcond[5]=((((IkReal(-1.00000000000000))*(sj2)*(x393)))+(x392)+(((sj2)*(x391)))+(((x389)*(x398)))+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x396)))+(((x388)*(x398))));
01583 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  )
01584 {
01585 continue;
01586 }
01587 }
01588 
01589 rotationfunction0(solutions);
01590 }
01591 }
01592 
01593 }
01594 
01595 }
01596 }
01597 }
01598 
01599 }
01600 
01601 }
01602 
01603 } else
01604 {
01605 {
01606 IkReal j0array[2], cj0array[2], sj0array[2];
01607 bool j0valid[2]={false};
01608 _nj0 = 2;
01609 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
01610     continue;
01611 IkReal x399=((IkReal(1.00000000000000))*(IKatan2(((IkReal(-1.00000000000000))*(py)), px)));
01612 if( ((((px)*(px))+((py)*(py)))) < (IkReal)-0.00001 )
01613     continue;
01614 if( (((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
01615     continue;
01616 IkReal x400=IKasin(((IkReal(0.316500000000000))*(sj2)*(sj3)*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30))));
01617 j0array[0]=((((IkReal(-1.00000000000000))*(x399)))+(x400));
01618 sj0array[0]=IKsin(j0array[0]);
01619 cj0array[0]=IKcos(j0array[0]);
01620 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x399)))+(((IkReal(-1.00000000000000))*(x400))));
01621 sj0array[1]=IKsin(j0array[1]);
01622 cj0array[1]=IKcos(j0array[1]);
01623 if( j0array[0] > IKPI )
01624 {
01625     j0array[0]-=IK2PI;
01626 }
01627 else if( j0array[0] < -IKPI )
01628 {    j0array[0]+=IK2PI;
01629 }
01630 j0valid[0] = true;
01631 if( j0array[1] > IKPI )
01632 {
01633     j0array[1]-=IK2PI;
01634 }
01635 else if( j0array[1] < -IKPI )
01636 {    j0array[1]+=IK2PI;
01637 }
01638 j0valid[1] = true;
01639 for(int ij0 = 0; ij0 < 2; ++ij0)
01640 {
01641 if( !j0valid[ij0] )
01642 {
01643     continue;
01644 }
01645 _ij0[0] = ij0; _ij0[1] = -1;
01646 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
01647 {
01648 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01649 {
01650     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01651 }
01652 }
01653 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01654 
01655 {
01656 IkReal dummyeval[1];
01657 IkReal gconst11;
01658 IkReal x401=((py)*(sj0));
01659 IkReal x402=((IkReal(633.000000000000))*(cj3));
01660 IkReal x403=((cj0)*(px));
01661 gconst11=IKsign(((((IkReal(633.000000000000))*(cj2)*(pz)*(sj3)))+(((IkReal(816.000000000000))*(x401)))+(((IkReal(816.000000000000))*(x403)))+(((x402)*(x403)))+(((x401)*(x402)))));
01662 IkReal x404=((cj0)*(px));
01663 IkReal x405=((py)*(sj0));
01664 dummyeval[0]=((((IkReal(1.28909952606635))*(x404)))+(((IkReal(1.28909952606635))*(x405)))+(((cj2)*(pz)*(sj3)))+(((cj3)*(x404)))+(((cj3)*(x405))));
01665 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01666 {
01667 {
01668 IkReal dummyeval[1];
01669 IkReal gconst10;
01670 gconst10=IKsign(((IkReal(665856.000000000))+(((IkReal(1033056.00000000))*(cj3)))+(((IkReal(400689.000000000))*((cj3)*(cj3))))+(((IkReal(400689.000000000))*((cj2)*(cj2))*((sj3)*(sj3))))));
01671 dummyeval[0]=((IkReal(1.66177758810449))+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((IkReal(2.57819905213270))*(cj3))));
01672 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01673 {
01674 {
01675 IkReal dummyeval[1];
01676 IkReal x406=((cj0)*(px));
01677 IkReal x407=((py)*(sj0));
01678 dummyeval[0]=((((IkReal(1.28909952606635))*(x406)))+(((IkReal(1.28909952606635))*(x407)))+(((cj2)*(pz)*(sj3)))+(((cj3)*(x407)))+(((cj3)*(x406))));
01679 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01680 {
01681 continue;
01682 
01683 } else
01684 {
01685 {
01686 IkReal j1array[1], cj1array[1], sj1array[1];
01687 bool j1valid[1]={false};
01688 _nj1 = 1;
01689 IkReal x408=((py)*(sj0));
01690 IkReal x409=((IkReal(32283.0000000000))*(cj3));
01691 IkReal x410=((cj2)*(sj3));
01692 IkReal x411=((IkReal(64566.0000000000))*(cj3));
01693 IkReal x412=((cj0)*(px));
01694 IkReal x413=((IkReal(204000.000000000))*(pz));
01695 if( IKabs(((((IKabs(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x410)))+(((IkReal(-1.00000000000000))*(x412)*(x413)))+(((IkReal(-79125.0000000000))*(pp)*(x410)))+(((IkReal(-1.00000000000000))*(x408)*(x413))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz))))))))+IKsqr(((((IKabs(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x410)))+(((IkReal(-1.00000000000000))*(x412)*(x413)))+(((IkReal(-79125.0000000000))*(pp)*(x410)))+(((IkReal(-1.00000000000000))*(x408)*(x413)))))))-1) <= IKFAST_SINCOS_THRESH )
01696     continue;
01697 j1array[0]=IKatan2(((((IKabs(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((IkReal(41616.0000000000))*(x412)))+(((x409)*(x412)))+(((x408)*(x409)))+(((IkReal(41616.0000000000))*(x408)))+(((IkReal(32283.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((IkReal(-3380.87925000000))+(((IkReal(-39562.5000000000))*(cj3)*(pp)))+(((IkReal(-2622.66735937500))*(cj3)))+(((IkReal(-51000.0000000000))*(pp)))+(((IkReal(102000.000000000))*((pz)*(pz))))))), ((((IKabs(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410))))) != 0)?((IkReal)1/(((((x411)*(x412)))+(((IkReal(83232.0000000000))*(x412)))+(((x408)*(x411)))+(((IkReal(83232.0000000000))*(x408)))+(((IkReal(64566.0000000000))*(pz)*(x410)))))):(IkReal)1.0e30))*(((((IkReal(-5245.33471875000))*(x410)))+(((IkReal(-1.00000000000000))*(x412)*(x413)))+(((IkReal(-79125.0000000000))*(pp)*(x410)))+(((IkReal(-1.00000000000000))*(x408)*(x413)))))));
01698 sj1array[0]=IKsin(j1array[0]);
01699 cj1array[0]=IKcos(j1array[0]);
01700 if( j1array[0] > IKPI )
01701 {
01702     j1array[0]-=IK2PI;
01703 }
01704 else if( j1array[0] < -IKPI )
01705 {    j1array[0]+=IK2PI;
01706 }
01707 j1valid[0] = true;
01708 for(int ij1 = 0; ij1 < 1; ++ij1)
01709 {
01710 if( !j1valid[ij1] )
01711 {
01712     continue;
01713 }
01714 _ij1[0] = ij1; _ij1[1] = -1;
01715 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01716 {
01717 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01718 {
01719     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01720 }
01721 }
01722 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01723 {
01724 IkReal evalcond[6];
01725 IkReal x414=IKcos(j1);
01726 IkReal x415=IKsin(j1);
01727 IkReal x416=((py)*(sj0));
01728 IkReal x417=((cj0)*(px));
01729 IkReal x418=((IkReal(0.316500000000000))*(cj3));
01730 IkReal x419=((cj0)*(py));
01731 IkReal x420=((IkReal(0.316500000000000))*(sj3));
01732 IkReal x421=((px)*(sj0));
01733 IkReal x422=((IkReal(0.816000000000000))*(x415));
01734 IkReal x423=((sj2)*(x414));
01735 IkReal x424=((IkReal(1.00000000000000))*(x415));
01736 IkReal x425=((pz)*(x414));
01737 IkReal x426=((cj2)*(x414));
01738 evalcond[0]=((((cj2)*(x415)*(x420)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.408000000000000))*(x414)))+(((IkReal(-1.00000000000000))*(x414)*(x418))));
01739 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(x416)*(x422)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(x417)*(x422)))+(((IkReal(-0.816000000000000))*(x425))));
01740 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x425)))+(((IkReal(-1.00000000000000))*(x416)*(x424)))+(((IkReal(-1.00000000000000))*(x417)*(x424)))+(((IkReal(-1.00000000000000))*(x418))));
01741 evalcond[3]=((((x420)*(x426)))+(((IkReal(0.408000000000000))*(x415)))+(((x415)*(x418)))+(x416)+(x417));
01742 evalcond[4]=((((x416)*(x423)))+(((IkReal(-1.00000000000000))*(cj2)*(x419)))+(((cj2)*(x421)))+(((IkReal(-1.00000000000000))*(pz)*(sj2)*(x424)))+(((x417)*(x423))));
01743 evalcond[5]=((((sj2)*(x419)))+(((x416)*(x426)))+(x420)+(((IkReal(-1.00000000000000))*(sj2)*(x421)))+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x424)))+(((x417)*(x426))));
01744 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  )
01745 {
01746 continue;
01747 }
01748 }
01749 
01750 rotationfunction0(solutions);
01751 }
01752 }
01753 
01754 }
01755 
01756 }
01757 
01758 } else
01759 {
01760 {
01761 IkReal j1array[1], cj1array[1], sj1array[1];
01762 bool j1valid[1]={false};
01763 _nj1 = 1;
01764 IkReal x427=((IkReal(1266000.00000000))*(cj3));
01765 IkReal x428=((cj0)*(px));
01766 IkReal x429=((py)*(sj0));
01767 IkReal x430=((IkReal(1266000.00000000))*(cj2)*(sj3));
01768 if( IKabs(((gconst10)*(((((IkReal(-1632000.00000000))*(x429)))+(((IkReal(-1632000.00000000))*(x428)))+(((IkReal(-1.00000000000000))*(x427)*(x428)))+(((IkReal(-1.00000000000000))*(x427)*(x429)))+(((pz)*(x430))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-1632000.00000000))*(pz)))+(((IkReal(-1.00000000000000))*(x429)*(x430)))+(((IkReal(-1.00000000000000))*(x428)*(x430)))+(((IkReal(-1.00000000000000))*(pz)*(x427))))))) < IKFAST_ATAN2_MAGTHRESH )
01769     continue;
01770 j1array[0]=IKatan2(((gconst10)*(((((IkReal(-1632000.00000000))*(x429)))+(((IkReal(-1632000.00000000))*(x428)))+(((IkReal(-1.00000000000000))*(x427)*(x428)))+(((IkReal(-1.00000000000000))*(x427)*(x429)))+(((pz)*(x430)))))), ((gconst10)*(((((IkReal(-1632000.00000000))*(pz)))+(((IkReal(-1.00000000000000))*(x429)*(x430)))+(((IkReal(-1.00000000000000))*(x428)*(x430)))+(((IkReal(-1.00000000000000))*(pz)*(x427)))))));
01771 sj1array[0]=IKsin(j1array[0]);
01772 cj1array[0]=IKcos(j1array[0]);
01773 if( j1array[0] > IKPI )
01774 {
01775     j1array[0]-=IK2PI;
01776 }
01777 else if( j1array[0] < -IKPI )
01778 {    j1array[0]+=IK2PI;
01779 }
01780 j1valid[0] = true;
01781 for(int ij1 = 0; ij1 < 1; ++ij1)
01782 {
01783 if( !j1valid[ij1] )
01784 {
01785     continue;
01786 }
01787 _ij1[0] = ij1; _ij1[1] = -1;
01788 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01789 {
01790 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01791 {
01792     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01793 }
01794 }
01795 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01796 {
01797 IkReal evalcond[6];
01798 IkReal x431=IKcos(j1);
01799 IkReal x432=IKsin(j1);
01800 IkReal x433=((py)*(sj0));
01801 IkReal x434=((cj0)*(px));
01802 IkReal x435=((IkReal(0.316500000000000))*(cj3));
01803 IkReal x436=((cj0)*(py));
01804 IkReal x437=((IkReal(0.316500000000000))*(sj3));
01805 IkReal x438=((px)*(sj0));
01806 IkReal x439=((IkReal(0.816000000000000))*(x432));
01807 IkReal x440=((sj2)*(x431));
01808 IkReal x441=((IkReal(1.00000000000000))*(x432));
01809 IkReal x442=((pz)*(x431));
01810 IkReal x443=((cj2)*(x431));
01811 evalcond[0]=((((IkReal(-1.00000000000000))*(x431)*(x435)))+(((cj2)*(x432)*(x437)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.408000000000000))*(x431))));
01812 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-0.816000000000000))*(x442)))+(((IkReal(-1.00000000000000))*(x433)*(x439)))+(((IkReal(-1.00000000000000))*(x434)*(x439)))+(((IkReal(-1.00000000000000))*(pp))));
01813 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x435)))+(((IkReal(-1.00000000000000))*(x433)*(x441)))+(((IkReal(-1.00000000000000))*(x442)))+(((IkReal(-1.00000000000000))*(x434)*(x441))));
01814 evalcond[3]=((x433)+(x434)+(((IkReal(0.408000000000000))*(x432)))+(((x437)*(x443)))+(((x432)*(x435))));
01815 evalcond[4]=((((x434)*(x440)))+(((cj2)*(x438)))+(((IkReal(-1.00000000000000))*(cj2)*(x436)))+(((x433)*(x440)))+(((IkReal(-1.00000000000000))*(pz)*(sj2)*(x441))));
01816 evalcond[5]=((((x434)*(x443)))+(x437)+(((sj2)*(x436)))+(((IkReal(-1.00000000000000))*(sj2)*(x438)))+(((x433)*(x443)))+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x441))));
01817 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  )
01818 {
01819 continue;
01820 }
01821 }
01822 
01823 rotationfunction0(solutions);
01824 }
01825 }
01826 
01827 }
01828 
01829 }
01830 
01831 } else
01832 {
01833 {
01834 IkReal j1array[1], cj1array[1], sj1array[1];
01835 bool j1valid[1]={false};
01836 _nj1 = 1;
01837 IkReal x444=((IkReal(2000.00000000000))*(pz));
01838 IkReal x445=((cj2)*(sj3));
01839 if( IKabs(((gconst11)*(((IkReal(-332.928000000000))+(((IkReal(-516.528000000000))*(cj3)))+(((IkReal(-200.344500000000))*((cj3)*(cj3))))+(((pz)*(x444))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(cj0)*(px)*(x444)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x444)))+(((IkReal(-258.264000000000))*(x445)))+(((IkReal(-200.344500000000))*(cj3)*(x445))))))) < IKFAST_ATAN2_MAGTHRESH )
01840     continue;
01841 j1array[0]=IKatan2(((gconst11)*(((IkReal(-332.928000000000))+(((IkReal(-516.528000000000))*(cj3)))+(((IkReal(-200.344500000000))*((cj3)*(cj3))))+(((pz)*(x444)))))), ((gconst11)*(((((IkReal(-1.00000000000000))*(cj0)*(px)*(x444)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x444)))+(((IkReal(-258.264000000000))*(x445)))+(((IkReal(-200.344500000000))*(cj3)*(x445)))))));
01842 sj1array[0]=IKsin(j1array[0]);
01843 cj1array[0]=IKcos(j1array[0]);
01844 if( j1array[0] > IKPI )
01845 {
01846     j1array[0]-=IK2PI;
01847 }
01848 else if( j1array[0] < -IKPI )
01849 {    j1array[0]+=IK2PI;
01850 }
01851 j1valid[0] = true;
01852 for(int ij1 = 0; ij1 < 1; ++ij1)
01853 {
01854 if( !j1valid[ij1] )
01855 {
01856     continue;
01857 }
01858 _ij1[0] = ij1; _ij1[1] = -1;
01859 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
01860 {
01861 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
01862 {
01863     j1valid[iij1]=false; _ij1[1] = iij1; break; 
01864 }
01865 }
01866 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01867 {
01868 IkReal evalcond[6];
01869 IkReal x446=IKcos(j1);
01870 IkReal x447=IKsin(j1);
01871 IkReal x448=((py)*(sj0));
01872 IkReal x449=((cj0)*(px));
01873 IkReal x450=((IkReal(0.316500000000000))*(cj3));
01874 IkReal x451=((cj0)*(py));
01875 IkReal x452=((IkReal(0.316500000000000))*(sj3));
01876 IkReal x453=((px)*(sj0));
01877 IkReal x454=((IkReal(0.816000000000000))*(x447));
01878 IkReal x455=((sj2)*(x446));
01879 IkReal x456=((IkReal(1.00000000000000))*(x447));
01880 IkReal x457=((pz)*(x446));
01881 IkReal x458=((cj2)*(x446));
01882 evalcond[0]=((((IkReal(-1.00000000000000))*(x446)*(x450)))+(((cj2)*(x447)*(x452)))+(((IkReal(-1.00000000000000))*(pz)))+(((IkReal(-0.408000000000000))*(x446))));
01883 evalcond[1]=((IkReal(-0.0662917500000000))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.816000000000000))*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x454)))+(((IkReal(-1.00000000000000))*(x448)*(x454))));
01884 evalcond[2]=((IkReal(-0.408000000000000))+(((IkReal(-1.00000000000000))*(x457)))+(((IkReal(-1.00000000000000))*(x450)))+(((IkReal(-1.00000000000000))*(x449)*(x456)))+(((IkReal(-1.00000000000000))*(x448)*(x456))));
01885 evalcond[3]=((((x452)*(x458)))+(((x447)*(x450)))+(x449)+(x448)+(((IkReal(0.408000000000000))*(x447))));
01886 evalcond[4]=((((IkReal(-1.00000000000000))*(pz)*(sj2)*(x456)))+(((IkReal(-1.00000000000000))*(cj2)*(x451)))+(((cj2)*(x453)))+(((x449)*(x455)))+(((x448)*(x455))));
01887 evalcond[5]=((((sj2)*(x451)))+(((IkReal(-1.00000000000000))*(cj2)*(pz)*(x456)))+(((IkReal(-1.00000000000000))*(sj2)*(x453)))+(x452)+(((x449)*(x458)))+(((x448)*(x458))));
01888 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  )
01889 {
01890 continue;
01891 }
01892 }
01893 
01894 rotationfunction0(solutions);
01895 }
01896 }
01897 
01898 }
01899 
01900 }
01901 }
01902 }
01903 
01904 }
01905 
01906 }
01907 }
01908 }
01909 }
01910 return solutions.GetNumSolutions()>0;
01911 }
01912 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
01913 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
01914 IkReal x82=((cj3)*(sj1));
01915 IkReal x83=((sj0)*(sj2));
01916 IkReal x84=((IkReal(1.00000000000000))*(sj3));
01917 IkReal x85=((IkReal(1.00000000000000))*(cj2));
01918 IkReal x86=((cj1)*(cj3));
01919 IkReal x87=((cj0)*(sj2));
01920 IkReal x88=((cj1)*(sj3));
01921 IkReal x89=((IkReal(1.00000000000000))*(sj1)*(sj2));
01922 IkReal x90=((((sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(x85)*(x86))));
01923 IkReal x91=((((cj2)*(x82)))+(x88));
01924 IkReal x92=((((IkReal(-1.00000000000000))*(cj0)*(x85)))+(((cj1)*(x83))));
01925 IkReal x93=((((cj2)*(sj0)))+(((cj1)*(x87))));
01926 IkReal x94=((((cj2)*(x88)))+(x82));
01927 IkReal x95=((x86)+(((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x84))));
01928 IkReal x96=((sj0)*(x90));
01929 IkReal x97=((((cj3)*(x83)))+(((cj0)*(x90))));
01930 IkReal x98=((x96)+(((IkReal(-1.00000000000000))*(cj3)*(x87))));
01931 IkReal x99=((((IkReal(-1.00000000000000))*(x83)*(x84)))+(((cj0)*(x94))));
01932 IkReal x100=((((sj0)*(x94)))+(((sj3)*(x87))));
01933 new_r00=((((r10)*(x98)))+(((r00)*(x97)))+(((r20)*(x91))));
01934 new_r01=((((r21)*(x91)))+(((r11)*(((x96)+(((IkReal(-1.00000000000000))*(cj3)*(x87)))))))+(((r01)*(x97))));
01935 new_r02=((((r12)*(x98)))+(((r02)*(x97)))+(((r22)*(x91))));
01936 new_r10=((((r10)*(x92)))+(((r00)*(x93)))+(((IkReal(-1.00000000000000))*(r20)*(x89))));
01937 new_r11=((((r11)*(x92)))+(((r01)*(x93)))+(((IkReal(-1.00000000000000))*(r21)*(x89))));
01938 new_r12=((((r12)*(x92)))+(((IkReal(-1.00000000000000))*(r22)*(x89)))+(((r02)*(x93))));
01939 new_r20=((((r10)*(x100)))+(((r00)*(x99)))+(((r20)*(x95))));
01940 new_r21=((((r21)*(x95)))+(((r01)*(x99)))+(((r11)*(x100))));
01941 new_r22=((((r12)*(x100)))+(((r02)*(x99)))+(((r22)*(x95))));
01942 {
01943 IkReal j5array[2], cj5array[2], sj5array[2];
01944 bool j5valid[2]={false};
01945 _nj5 = 2;
01946 cj5array[0]=new_r22;
01947 if( cj5array[0] >= -1-IKFAST_SINCOS_THRESH && cj5array[0] <= 1+IKFAST_SINCOS_THRESH )
01948 {
01949     j5valid[0] = j5valid[1] = true;
01950     j5array[0] = IKacos(cj5array[0]);
01951     sj5array[0] = IKsin(j5array[0]);
01952     cj5array[1] = cj5array[0];
01953     j5array[1] = -j5array[0];
01954     sj5array[1] = -sj5array[0];
01955 }
01956 else if( isnan(cj5array[0]) )
01957 {
01958     // probably any value will work
01959     j5valid[0] = true;
01960     cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
01961 }
01962 for(int ij5 = 0; ij5 < 2; ++ij5)
01963 {
01964 if( !j5valid[ij5] )
01965 {
01966     continue;
01967 }
01968 _ij5[0] = ij5; _ij5[1] = -1;
01969 for(int iij5 = ij5+1; iij5 < 2; ++iij5)
01970 {
01971 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01972 {
01973     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01974 }
01975 }
01976 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01977 
01978 {
01979 IkReal dummyeval[1];
01980 IkReal gconst14;
01981 gconst14=IKsign(sj5);
01982 dummyeval[0]=sj5;
01983 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01984 {
01985 {
01986 IkReal dummyeval[1];
01987 IkReal gconst12;
01988 gconst12=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))));
01989 dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))));
01990 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01991 {
01992 {
01993 IkReal dummyeval[1];
01994 IkReal gconst13;
01995 gconst13=IKsign(((((cj5)*((new_r02)*(new_r02))))+(((cj5)*((new_r12)*(new_r12))))));
01996 dummyeval[0]=((((cj5)*((new_r02)*(new_r02))))+(((cj5)*((new_r12)*(new_r12)))));
01997 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01998 {
01999 {
02000 IkReal evalcond[7];
02001 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j5)), IkReal(6.28318530717959))));
02002 evalcond[1]=new_r22;
02003 evalcond[2]=new_r22;
02004 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
02005 {
02006 {
02007 IkReal j6array[1], cj6array[1], sj6array[1];
02008 bool j6valid[1]={false};
02009 _nj6 = 1;
02010 if( IKabs(((IkReal(-1.00000000000000))*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
02011     continue;
02012 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)), new_r20);
02013 sj6array[0]=IKsin(j6array[0]);
02014 cj6array[0]=IKcos(j6array[0]);
02015 if( j6array[0] > IKPI )
02016 {
02017     j6array[0]-=IK2PI;
02018 }
02019 else if( j6array[0] < -IKPI )
02020 {    j6array[0]+=IK2PI;
02021 }
02022 j6valid[0] = true;
02023 for(int ij6 = 0; ij6 < 1; ++ij6)
02024 {
02025 if( !j6valid[ij6] )
02026 {
02027     continue;
02028 }
02029 _ij6[0] = ij6; _ij6[1] = -1;
02030 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
02031 {
02032 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
02033 {
02034     j6valid[iij6]=false; _ij6[1] = iij6; break; 
02035 }
02036 }
02037 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
02038 {
02039 IkReal evalcond[2];
02040 evalcond[0]=((IKsin(j6))+(new_r21));
02041 evalcond[1]=((((IkReal(-1.00000000000000))*(IKcos(j6))))+(new_r20));
02042 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02043 {
02044 continue;
02045 }
02046 }
02047 
02048 {
02049 IkReal dummyeval[1];
02050 IkReal gconst20;
02051 gconst20=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11)))));
02052 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11))));
02053 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02054 {
02055 {
02056 IkReal dummyeval[1];
02057 IkReal gconst21;
02058 gconst21=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))));
02059 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))));
02060 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02061 {
02062 continue;
02063 
02064 } else
02065 {
02066 {
02067 IkReal j4array[1], cj4array[1], sj4array[1];
02068 bool j4valid[1]={false};
02069 _nj4 = 1;
02070 if( IKabs(((gconst21)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst21)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
02071     continue;
02072 j4array[0]=IKatan2(((gconst21)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst21)*(new_r10)));
02073 sj4array[0]=IKsin(j4array[0]);
02074 cj4array[0]=IKcos(j4array[0]);
02075 if( j4array[0] > IKPI )
02076 {
02077     j4array[0]-=IK2PI;
02078 }
02079 else if( j4array[0] < -IKPI )
02080 {    j4array[0]+=IK2PI;
02081 }
02082 j4valid[0] = true;
02083 for(int ij4 = 0; ij4 < 1; ++ij4)
02084 {
02085 if( !j4valid[ij4] )
02086 {
02087     continue;
02088 }
02089 _ij4[0] = ij4; _ij4[1] = -1;
02090 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02091 {
02092 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02093 {
02094     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02095 }
02096 }
02097 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02098 {
02099 IkReal evalcond[6];
02100 IkReal x101=IKcos(j4);
02101 IkReal x102=IKsin(j4);
02102 IkReal x103=((IkReal(1.00000000000000))*(x101));
02103 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x102)))+(((new_r12)*(x101))));
02104 evalcond[1]=((IkReal(1.00000000000000))+(((new_r02)*(x101)))+(((new_r12)*(x102))));
02105 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x103)))+(((new_r00)*(x102))));
02106 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x103)))+(cj6)+(((new_r01)*(x102))));
02107 evalcond[4]=((((new_r01)*(x101)))+(((new_r11)*(x102))));
02108 evalcond[5]=((((new_r10)*(x102)))+(((new_r00)*(x101))));
02109 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  )
02110 {
02111 continue;
02112 }
02113 }
02114 
02115 {
02116 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02117 vinfos[0].jointtype = 1;
02118 vinfos[0].foffset = j0;
02119 vinfos[0].indices[0] = _ij0[0];
02120 vinfos[0].indices[1] = _ij0[1];
02121 vinfos[0].maxsolutions = _nj0;
02122 vinfos[1].jointtype = 1;
02123 vinfos[1].foffset = j1;
02124 vinfos[1].indices[0] = _ij1[0];
02125 vinfos[1].indices[1] = _ij1[1];
02126 vinfos[1].maxsolutions = _nj1;
02127 vinfos[2].jointtype = 1;
02128 vinfos[2].foffset = j2;
02129 vinfos[2].indices[0] = _ij2[0];
02130 vinfos[2].indices[1] = _ij2[1];
02131 vinfos[2].maxsolutions = _nj2;
02132 vinfos[3].jointtype = 1;
02133 vinfos[3].foffset = j3;
02134 vinfos[3].indices[0] = _ij3[0];
02135 vinfos[3].indices[1] = _ij3[1];
02136 vinfos[3].maxsolutions = _nj3;
02137 vinfos[4].jointtype = 1;
02138 vinfos[4].foffset = j4;
02139 vinfos[4].indices[0] = _ij4[0];
02140 vinfos[4].indices[1] = _ij4[1];
02141 vinfos[4].maxsolutions = _nj4;
02142 vinfos[5].jointtype = 1;
02143 vinfos[5].foffset = j5;
02144 vinfos[5].indices[0] = _ij5[0];
02145 vinfos[5].indices[1] = _ij5[1];
02146 vinfos[5].maxsolutions = _nj5;
02147 vinfos[6].jointtype = 1;
02148 vinfos[6].foffset = j6;
02149 vinfos[6].indices[0] = _ij6[0];
02150 vinfos[6].indices[1] = _ij6[1];
02151 vinfos[6].maxsolutions = _nj6;
02152 std::vector<int> vfree(0);
02153 solutions.AddSolution(vinfos,vfree);
02154 }
02155 }
02156 }
02157 
02158 }
02159 
02160 }
02161 
02162 } else
02163 {
02164 {
02165 IkReal j4array[1], cj4array[1], sj4array[1];
02166 bool j4valid[1]={false};
02167 _nj4 = 1;
02168 if( IKabs(((gconst20)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst20)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
02169     continue;
02170 j4array[0]=IKatan2(((gconst20)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst20)*(new_r11)));
02171 sj4array[0]=IKsin(j4array[0]);
02172 cj4array[0]=IKcos(j4array[0]);
02173 if( j4array[0] > IKPI )
02174 {
02175     j4array[0]-=IK2PI;
02176 }
02177 else if( j4array[0] < -IKPI )
02178 {    j4array[0]+=IK2PI;
02179 }
02180 j4valid[0] = true;
02181 for(int ij4 = 0; ij4 < 1; ++ij4)
02182 {
02183 if( !j4valid[ij4] )
02184 {
02185     continue;
02186 }
02187 _ij4[0] = ij4; _ij4[1] = -1;
02188 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02189 {
02190 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02191 {
02192     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02193 }
02194 }
02195 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02196 {
02197 IkReal evalcond[6];
02198 IkReal x104=IKcos(j4);
02199 IkReal x105=IKsin(j4);
02200 IkReal x106=((IkReal(1.00000000000000))*(x104));
02201 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x105)))+(((new_r12)*(x104))));
02202 evalcond[1]=((IkReal(1.00000000000000))+(((new_r02)*(x104)))+(((new_r12)*(x105))));
02203 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x106)))+(((new_r00)*(x105))));
02204 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x106)))+(cj6)+(((new_r01)*(x105))));
02205 evalcond[4]=((((new_r01)*(x104)))+(((new_r11)*(x105))));
02206 evalcond[5]=((((new_r10)*(x105)))+(((new_r00)*(x104))));
02207 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  )
02208 {
02209 continue;
02210 }
02211 }
02212 
02213 {
02214 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02215 vinfos[0].jointtype = 1;
02216 vinfos[0].foffset = j0;
02217 vinfos[0].indices[0] = _ij0[0];
02218 vinfos[0].indices[1] = _ij0[1];
02219 vinfos[0].maxsolutions = _nj0;
02220 vinfos[1].jointtype = 1;
02221 vinfos[1].foffset = j1;
02222 vinfos[1].indices[0] = _ij1[0];
02223 vinfos[1].indices[1] = _ij1[1];
02224 vinfos[1].maxsolutions = _nj1;
02225 vinfos[2].jointtype = 1;
02226 vinfos[2].foffset = j2;
02227 vinfos[2].indices[0] = _ij2[0];
02228 vinfos[2].indices[1] = _ij2[1];
02229 vinfos[2].maxsolutions = _nj2;
02230 vinfos[3].jointtype = 1;
02231 vinfos[3].foffset = j3;
02232 vinfos[3].indices[0] = _ij3[0];
02233 vinfos[3].indices[1] = _ij3[1];
02234 vinfos[3].maxsolutions = _nj3;
02235 vinfos[4].jointtype = 1;
02236 vinfos[4].foffset = j4;
02237 vinfos[4].indices[0] = _ij4[0];
02238 vinfos[4].indices[1] = _ij4[1];
02239 vinfos[4].maxsolutions = _nj4;
02240 vinfos[5].jointtype = 1;
02241 vinfos[5].foffset = j5;
02242 vinfos[5].indices[0] = _ij5[0];
02243 vinfos[5].indices[1] = _ij5[1];
02244 vinfos[5].maxsolutions = _nj5;
02245 vinfos[6].jointtype = 1;
02246 vinfos[6].foffset = j6;
02247 vinfos[6].indices[0] = _ij6[0];
02248 vinfos[6].indices[1] = _ij6[1];
02249 vinfos[6].maxsolutions = _nj6;
02250 std::vector<int> vfree(0);
02251 solutions.AddSolution(vinfos,vfree);
02252 }
02253 }
02254 }
02255 
02256 }
02257 
02258 }
02259 }
02260 }
02261 
02262 } else
02263 {
02264 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j5)), IkReal(6.28318530717959))));
02265 evalcond[1]=new_r22;
02266 evalcond[2]=((IkReal(-1.00000000000000))*(new_r22));
02267 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
02268 {
02269 {
02270 IkReal j6array[1], cj6array[1], sj6array[1];
02271 bool j6valid[1]={false};
02272 _nj6 = 1;
02273 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((IkReal(-1.00000000000000))*(new_r20)))-1) <= IKFAST_SINCOS_THRESH )
02274     continue;
02275 j6array[0]=IKatan2(new_r21, ((IkReal(-1.00000000000000))*(new_r20)));
02276 sj6array[0]=IKsin(j6array[0]);
02277 cj6array[0]=IKcos(j6array[0]);
02278 if( j6array[0] > IKPI )
02279 {
02280     j6array[0]-=IK2PI;
02281 }
02282 else if( j6array[0] < -IKPI )
02283 {    j6array[0]+=IK2PI;
02284 }
02285 j6valid[0] = true;
02286 for(int ij6 = 0; ij6 < 1; ++ij6)
02287 {
02288 if( !j6valid[ij6] )
02289 {
02290     continue;
02291 }
02292 _ij6[0] = ij6; _ij6[1] = -1;
02293 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
02294 {
02295 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
02296 {
02297     j6valid[iij6]=false; _ij6[1] = iij6; break; 
02298 }
02299 }
02300 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
02301 {
02302 IkReal evalcond[2];
02303 evalcond[0]=((new_r21)+(((IkReal(-1.00000000000000))*(IKsin(j6)))));
02304 evalcond[1]=((IKcos(j6))+(new_r20));
02305 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02306 {
02307 continue;
02308 }
02309 }
02310 
02311 {
02312 IkReal dummyeval[1];
02313 IkReal gconst24;
02314 gconst24=IKsign(((((new_r01)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r11)))));
02315 dummyeval[0]=((((new_r01)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r11))));
02316 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02317 {
02318 {
02319 IkReal dummyeval[1];
02320 IkReal gconst25;
02321 gconst25=IKsign(((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10)))));
02322 dummyeval[0]=((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10))));
02323 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02324 {
02325 continue;
02326 
02327 } else
02328 {
02329 {
02330 IkReal j4array[1], cj4array[1], sj4array[1];
02331 bool j4valid[1]={false};
02332 _nj4 = 1;
02333 if( IKabs(((gconst25)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst25)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
02334     continue;
02335 j4array[0]=IKatan2(((gconst25)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst25)*(new_r10)));
02336 sj4array[0]=IKsin(j4array[0]);
02337 cj4array[0]=IKcos(j4array[0]);
02338 if( j4array[0] > IKPI )
02339 {
02340     j4array[0]-=IK2PI;
02341 }
02342 else if( j4array[0] < -IKPI )
02343 {    j4array[0]+=IK2PI;
02344 }
02345 j4valid[0] = true;
02346 for(int ij4 = 0; ij4 < 1; ++ij4)
02347 {
02348 if( !j4valid[ij4] )
02349 {
02350     continue;
02351 }
02352 _ij4[0] = ij4; _ij4[1] = -1;
02353 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02354 {
02355 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02356 {
02357     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02358 }
02359 }
02360 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02361 {
02362 IkReal evalcond[6];
02363 IkReal x107=IKcos(j4);
02364 IkReal x108=IKsin(j4);
02365 IkReal x109=((IkReal(1.00000000000000))*(x107));
02366 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x108)))+(((new_r12)*(x107))));
02367 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r02)*(x107)))+(((new_r12)*(x108))));
02368 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x109)))+(((new_r00)*(x108))));
02369 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x109)))+(cj6)+(((new_r01)*(x108))));
02370 evalcond[4]=((((new_r01)*(x107)))+(((new_r11)*(x108))));
02371 evalcond[5]=((((new_r10)*(x108)))+(((new_r00)*(x107))));
02372 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  )
02373 {
02374 continue;
02375 }
02376 }
02377 
02378 {
02379 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02380 vinfos[0].jointtype = 1;
02381 vinfos[0].foffset = j0;
02382 vinfos[0].indices[0] = _ij0[0];
02383 vinfos[0].indices[1] = _ij0[1];
02384 vinfos[0].maxsolutions = _nj0;
02385 vinfos[1].jointtype = 1;
02386 vinfos[1].foffset = j1;
02387 vinfos[1].indices[0] = _ij1[0];
02388 vinfos[1].indices[1] = _ij1[1];
02389 vinfos[1].maxsolutions = _nj1;
02390 vinfos[2].jointtype = 1;
02391 vinfos[2].foffset = j2;
02392 vinfos[2].indices[0] = _ij2[0];
02393 vinfos[2].indices[1] = _ij2[1];
02394 vinfos[2].maxsolutions = _nj2;
02395 vinfos[3].jointtype = 1;
02396 vinfos[3].foffset = j3;
02397 vinfos[3].indices[0] = _ij3[0];
02398 vinfos[3].indices[1] = _ij3[1];
02399 vinfos[3].maxsolutions = _nj3;
02400 vinfos[4].jointtype = 1;
02401 vinfos[4].foffset = j4;
02402 vinfos[4].indices[0] = _ij4[0];
02403 vinfos[4].indices[1] = _ij4[1];
02404 vinfos[4].maxsolutions = _nj4;
02405 vinfos[5].jointtype = 1;
02406 vinfos[5].foffset = j5;
02407 vinfos[5].indices[0] = _ij5[0];
02408 vinfos[5].indices[1] = _ij5[1];
02409 vinfos[5].maxsolutions = _nj5;
02410 vinfos[6].jointtype = 1;
02411 vinfos[6].foffset = j6;
02412 vinfos[6].indices[0] = _ij6[0];
02413 vinfos[6].indices[1] = _ij6[1];
02414 vinfos[6].maxsolutions = _nj6;
02415 std::vector<int> vfree(0);
02416 solutions.AddSolution(vinfos,vfree);
02417 }
02418 }
02419 }
02420 
02421 }
02422 
02423 }
02424 
02425 } else
02426 {
02427 {
02428 IkReal j4array[1], cj4array[1], sj4array[1];
02429 bool j4valid[1]={false};
02430 _nj4 = 1;
02431 if( IKabs(((gconst24)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst24)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
02432     continue;
02433 j4array[0]=IKatan2(((gconst24)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst24)*(new_r11)));
02434 sj4array[0]=IKsin(j4array[0]);
02435 cj4array[0]=IKcos(j4array[0]);
02436 if( j4array[0] > IKPI )
02437 {
02438     j4array[0]-=IK2PI;
02439 }
02440 else if( j4array[0] < -IKPI )
02441 {    j4array[0]+=IK2PI;
02442 }
02443 j4valid[0] = true;
02444 for(int ij4 = 0; ij4 < 1; ++ij4)
02445 {
02446 if( !j4valid[ij4] )
02447 {
02448     continue;
02449 }
02450 _ij4[0] = ij4; _ij4[1] = -1;
02451 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02452 {
02453 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02454 {
02455     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02456 }
02457 }
02458 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02459 {
02460 IkReal evalcond[6];
02461 IkReal x110=IKcos(j4);
02462 IkReal x111=IKsin(j4);
02463 IkReal x112=((IkReal(1.00000000000000))*(x110));
02464 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x111)))+(((new_r12)*(x110))));
02465 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r02)*(x110)))+(((new_r12)*(x111))));
02466 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x112)))+(((new_r00)*(x111))));
02467 evalcond[3]=((cj6)+(((IkReal(-1.00000000000000))*(new_r11)*(x112)))+(((new_r01)*(x111))));
02468 evalcond[4]=((((new_r11)*(x111)))+(((new_r01)*(x110))));
02469 evalcond[5]=((((new_r10)*(x111)))+(((new_r00)*(x110))));
02470 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  )
02471 {
02472 continue;
02473 }
02474 }
02475 
02476 {
02477 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02478 vinfos[0].jointtype = 1;
02479 vinfos[0].foffset = j0;
02480 vinfos[0].indices[0] = _ij0[0];
02481 vinfos[0].indices[1] = _ij0[1];
02482 vinfos[0].maxsolutions = _nj0;
02483 vinfos[1].jointtype = 1;
02484 vinfos[1].foffset = j1;
02485 vinfos[1].indices[0] = _ij1[0];
02486 vinfos[1].indices[1] = _ij1[1];
02487 vinfos[1].maxsolutions = _nj1;
02488 vinfos[2].jointtype = 1;
02489 vinfos[2].foffset = j2;
02490 vinfos[2].indices[0] = _ij2[0];
02491 vinfos[2].indices[1] = _ij2[1];
02492 vinfos[2].maxsolutions = _nj2;
02493 vinfos[3].jointtype = 1;
02494 vinfos[3].foffset = j3;
02495 vinfos[3].indices[0] = _ij3[0];
02496 vinfos[3].indices[1] = _ij3[1];
02497 vinfos[3].maxsolutions = _nj3;
02498 vinfos[4].jointtype = 1;
02499 vinfos[4].foffset = j4;
02500 vinfos[4].indices[0] = _ij4[0];
02501 vinfos[4].indices[1] = _ij4[1];
02502 vinfos[4].maxsolutions = _nj4;
02503 vinfos[5].jointtype = 1;
02504 vinfos[5].foffset = j5;
02505 vinfos[5].indices[0] = _ij5[0];
02506 vinfos[5].indices[1] = _ij5[1];
02507 vinfos[5].maxsolutions = _nj5;
02508 vinfos[6].jointtype = 1;
02509 vinfos[6].foffset = j6;
02510 vinfos[6].indices[0] = _ij6[0];
02511 vinfos[6].indices[1] = _ij6[1];
02512 vinfos[6].maxsolutions = _nj6;
02513 std::vector<int> vfree(0);
02514 solutions.AddSolution(vinfos,vfree);
02515 }
02516 }
02517 }
02518 
02519 }
02520 
02521 }
02522 }
02523 }
02524 
02525 } else
02526 {
02527 IkReal x113=((IkReal(-1.00000000000000))+(new_r22));
02528 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j5)), IkReal(6.28318530717959))));
02529 evalcond[1]=x113;
02530 evalcond[2]=new_r21;
02531 evalcond[3]=new_r20;
02532 evalcond[4]=new_r20;
02533 evalcond[5]=new_r21;
02534 evalcond[6]=x113;
02535 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  )
02536 {
02537 {
02538 IkReal j4array[2], cj4array[2], sj4array[2];
02539 bool j4valid[2]={false};
02540 _nj4 = 2;
02541 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
02542     continue;
02543 IkReal x114=IKatan2(new_r02, new_r12);
02544 j4array[0]=((IkReal(-1.00000000000000))*(x114));
02545 sj4array[0]=IKsin(j4array[0]);
02546 cj4array[0]=IKcos(j4array[0]);
02547 j4array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x114))));
02548 sj4array[1]=IKsin(j4array[1]);
02549 cj4array[1]=IKcos(j4array[1]);
02550 if( j4array[0] > IKPI )
02551 {
02552     j4array[0]-=IK2PI;
02553 }
02554 else if( j4array[0] < -IKPI )
02555 {    j4array[0]+=IK2PI;
02556 }
02557 j4valid[0] = true;
02558 if( j4array[1] > IKPI )
02559 {
02560     j4array[1]-=IK2PI;
02561 }
02562 else if( j4array[1] < -IKPI )
02563 {    j4array[1]+=IK2PI;
02564 }
02565 j4valid[1] = true;
02566 for(int ij4 = 0; ij4 < 2; ++ij4)
02567 {
02568 if( !j4valid[ij4] )
02569 {
02570     continue;
02571 }
02572 _ij4[0] = ij4; _ij4[1] = -1;
02573 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
02574 {
02575 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02576 {
02577     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02578 }
02579 }
02580 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02581 {
02582 IkReal evalcond[1];
02583 evalcond[0]=((((new_r12)*(IKcos(j4))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j4)))));
02584 if( IKabs(evalcond[0]) > 0.000001  )
02585 {
02586 continue;
02587 }
02588 }
02589 
02590 {
02591 IkReal j6array[1], cj6array[1], sj6array[1];
02592 bool j6valid[1]={false};
02593 _nj6 = 1;
02594 IkReal x115=((IkReal(1.00000000000000))*(sj4));
02595 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(x115)))+(((cj4)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x115)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(x115)))+(((cj4)*(new_r10)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x115)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
02596     continue;
02597 j6array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(x115)))+(((cj4)*(new_r10)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x115)))+(((cj4)*(new_r11)))));
02598 sj6array[0]=IKsin(j6array[0]);
02599 cj6array[0]=IKcos(j6array[0]);
02600 if( j6array[0] > IKPI )
02601 {
02602     j6array[0]-=IK2PI;
02603 }
02604 else if( j6array[0] < -IKPI )
02605 {    j6array[0]+=IK2PI;
02606 }
02607 j6valid[0] = true;
02608 for(int ij6 = 0; ij6 < 1; ++ij6)
02609 {
02610 if( !j6valid[ij6] )
02611 {
02612     continue;
02613 }
02614 _ij6[0] = ij6; _ij6[1] = -1;
02615 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
02616 {
02617 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
02618 {
02619     j6valid[iij6]=false; _ij6[1] = iij6; break; 
02620 }
02621 }
02622 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
02623 {
02624 IkReal evalcond[4];
02625 IkReal x116=IKsin(j6);
02626 IkReal x117=IKcos(j6);
02627 IkReal x118=((IkReal(1.00000000000000))*(cj4));
02628 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x118)))+(((new_r00)*(sj4)))+(x116));
02629 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x118)))+(((new_r01)*(sj4)))+(x117));
02630 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(x116));
02631 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(((IkReal(-1.00000000000000))*(x117))));
02632 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02633 {
02634 continue;
02635 }
02636 }
02637 
02638 {
02639 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02640 vinfos[0].jointtype = 1;
02641 vinfos[0].foffset = j0;
02642 vinfos[0].indices[0] = _ij0[0];
02643 vinfos[0].indices[1] = _ij0[1];
02644 vinfos[0].maxsolutions = _nj0;
02645 vinfos[1].jointtype = 1;
02646 vinfos[1].foffset = j1;
02647 vinfos[1].indices[0] = _ij1[0];
02648 vinfos[1].indices[1] = _ij1[1];
02649 vinfos[1].maxsolutions = _nj1;
02650 vinfos[2].jointtype = 1;
02651 vinfos[2].foffset = j2;
02652 vinfos[2].indices[0] = _ij2[0];
02653 vinfos[2].indices[1] = _ij2[1];
02654 vinfos[2].maxsolutions = _nj2;
02655 vinfos[3].jointtype = 1;
02656 vinfos[3].foffset = j3;
02657 vinfos[3].indices[0] = _ij3[0];
02658 vinfos[3].indices[1] = _ij3[1];
02659 vinfos[3].maxsolutions = _nj3;
02660 vinfos[4].jointtype = 1;
02661 vinfos[4].foffset = j4;
02662 vinfos[4].indices[0] = _ij4[0];
02663 vinfos[4].indices[1] = _ij4[1];
02664 vinfos[4].maxsolutions = _nj4;
02665 vinfos[5].jointtype = 1;
02666 vinfos[5].foffset = j5;
02667 vinfos[5].indices[0] = _ij5[0];
02668 vinfos[5].indices[1] = _ij5[1];
02669 vinfos[5].maxsolutions = _nj5;
02670 vinfos[6].jointtype = 1;
02671 vinfos[6].foffset = j6;
02672 vinfos[6].indices[0] = _ij6[0];
02673 vinfos[6].indices[1] = _ij6[1];
02674 vinfos[6].maxsolutions = _nj6;
02675 std::vector<int> vfree(0);
02676 solutions.AddSolution(vinfos,vfree);
02677 }
02678 }
02679 }
02680 }
02681 }
02682 
02683 } else
02684 {
02685 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j5)), IkReal(6.28318530717959))));
02686 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
02687 evalcond[2]=new_r21;
02688 evalcond[3]=new_r20;
02689 evalcond[4]=((IkReal(-1.00000000000000))*(new_r20));
02690 evalcond[5]=((IkReal(-1.00000000000000))*(new_r21));
02691 evalcond[6]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
02692 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  )
02693 {
02694 {
02695 IkReal j4array[2], cj4array[2], sj4array[2];
02696 bool j4valid[2]={false};
02697 _nj4 = 2;
02698 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
02699     continue;
02700 IkReal x119=IKatan2(new_r02, new_r12);
02701 j4array[0]=((IkReal(-1.00000000000000))*(x119));
02702 sj4array[0]=IKsin(j4array[0]);
02703 cj4array[0]=IKcos(j4array[0]);
02704 j4array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x119))));
02705 sj4array[1]=IKsin(j4array[1]);
02706 cj4array[1]=IKcos(j4array[1]);
02707 if( j4array[0] > IKPI )
02708 {
02709     j4array[0]-=IK2PI;
02710 }
02711 else if( j4array[0] < -IKPI )
02712 {    j4array[0]+=IK2PI;
02713 }
02714 j4valid[0] = true;
02715 if( j4array[1] > IKPI )
02716 {
02717     j4array[1]-=IK2PI;
02718 }
02719 else if( j4array[1] < -IKPI )
02720 {    j4array[1]+=IK2PI;
02721 }
02722 j4valid[1] = true;
02723 for(int ij4 = 0; ij4 < 2; ++ij4)
02724 {
02725 if( !j4valid[ij4] )
02726 {
02727     continue;
02728 }
02729 _ij4[0] = ij4; _ij4[1] = -1;
02730 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
02731 {
02732 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02733 {
02734     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02735 }
02736 }
02737 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02738 {
02739 IkReal evalcond[1];
02740 evalcond[0]=((((new_r12)*(IKcos(j4))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j4)))));
02741 if( IKabs(evalcond[0]) > 0.000001  )
02742 {
02743 continue;
02744 }
02745 }
02746 
02747 {
02748 IkReal j6array[1], cj6array[1], sj6array[1];
02749 bool j6valid[1]={false};
02750 _nj6 = 1;
02751 IkReal x120=((IkReal(1.00000000000000))*(sj4));
02752 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x120))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x120)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x120)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x120)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
02753     continue;
02754 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x120)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x120)))+(((cj4)*(new_r11)))));
02755 sj6array[0]=IKsin(j6array[0]);
02756 cj6array[0]=IKcos(j6array[0]);
02757 if( j6array[0] > IKPI )
02758 {
02759     j6array[0]-=IK2PI;
02760 }
02761 else if( j6array[0] < -IKPI )
02762 {    j6array[0]+=IK2PI;
02763 }
02764 j6valid[0] = true;
02765 for(int ij6 = 0; ij6 < 1; ++ij6)
02766 {
02767 if( !j6valid[ij6] )
02768 {
02769     continue;
02770 }
02771 _ij6[0] = ij6; _ij6[1] = -1;
02772 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
02773 {
02774 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
02775 {
02776     j6valid[iij6]=false; _ij6[1] = iij6; break; 
02777 }
02778 }
02779 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
02780 {
02781 IkReal evalcond[4];
02782 IkReal x121=IKsin(j6);
02783 IkReal x122=IKcos(j6);
02784 IkReal x123=((IkReal(1.00000000000000))*(cj4));
02785 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x123)))+(((new_r00)*(sj4)))+(x121));
02786 evalcond[1]=((((new_r01)*(sj4)))+(x122)+(((IkReal(-1.00000000000000))*(new_r11)*(x123))));
02787 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x121))));
02788 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(x122));
02789 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02790 {
02791 continue;
02792 }
02793 }
02794 
02795 {
02796 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02797 vinfos[0].jointtype = 1;
02798 vinfos[0].foffset = j0;
02799 vinfos[0].indices[0] = _ij0[0];
02800 vinfos[0].indices[1] = _ij0[1];
02801 vinfos[0].maxsolutions = _nj0;
02802 vinfos[1].jointtype = 1;
02803 vinfos[1].foffset = j1;
02804 vinfos[1].indices[0] = _ij1[0];
02805 vinfos[1].indices[1] = _ij1[1];
02806 vinfos[1].maxsolutions = _nj1;
02807 vinfos[2].jointtype = 1;
02808 vinfos[2].foffset = j2;
02809 vinfos[2].indices[0] = _ij2[0];
02810 vinfos[2].indices[1] = _ij2[1];
02811 vinfos[2].maxsolutions = _nj2;
02812 vinfos[3].jointtype = 1;
02813 vinfos[3].foffset = j3;
02814 vinfos[3].indices[0] = _ij3[0];
02815 vinfos[3].indices[1] = _ij3[1];
02816 vinfos[3].maxsolutions = _nj3;
02817 vinfos[4].jointtype = 1;
02818 vinfos[4].foffset = j4;
02819 vinfos[4].indices[0] = _ij4[0];
02820 vinfos[4].indices[1] = _ij4[1];
02821 vinfos[4].maxsolutions = _nj4;
02822 vinfos[5].jointtype = 1;
02823 vinfos[5].foffset = j5;
02824 vinfos[5].indices[0] = _ij5[0];
02825 vinfos[5].indices[1] = _ij5[1];
02826 vinfos[5].maxsolutions = _nj5;
02827 vinfos[6].jointtype = 1;
02828 vinfos[6].foffset = j6;
02829 vinfos[6].indices[0] = _ij6[0];
02830 vinfos[6].indices[1] = _ij6[1];
02831 vinfos[6].maxsolutions = _nj6;
02832 std::vector<int> vfree(0);
02833 solutions.AddSolution(vinfos,vfree);
02834 }
02835 }
02836 }
02837 }
02838 }
02839 
02840 } else
02841 {
02842 if( 1 )
02843 {
02844 continue;
02845 
02846 } else
02847 {
02848 }
02849 }
02850 }
02851 }
02852 }
02853 }
02854 
02855 } else
02856 {
02857 {
02858 IkReal j4array[1], cj4array[1], sj4array[1];
02859 bool j4valid[1]={false};
02860 _nj4 = 1;
02861 IkReal x124=((IkReal(-1.00000000000000))*(gconst13)*(new_r22)*(sj5));
02862 if( IKabs(((new_r12)*(x124))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x124))) < IKFAST_ATAN2_MAGTHRESH )
02863     continue;
02864 j4array[0]=IKatan2(((new_r12)*(x124)), ((new_r02)*(x124)));
02865 sj4array[0]=IKsin(j4array[0]);
02866 cj4array[0]=IKcos(j4array[0]);
02867 if( j4array[0] > IKPI )
02868 {
02869     j4array[0]-=IK2PI;
02870 }
02871 else if( j4array[0] < -IKPI )
02872 {    j4array[0]+=IK2PI;
02873 }
02874 j4valid[0] = true;
02875 for(int ij4 = 0; ij4 < 1; ++ij4)
02876 {
02877 if( !j4valid[ij4] )
02878 {
02879     continue;
02880 }
02881 _ij4[0] = ij4; _ij4[1] = -1;
02882 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02883 {
02884 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02885 {
02886     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02887 }
02888 }
02889 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02890 {
02891 IkReal evalcond[6];
02892 IkReal x125=IKcos(j4);
02893 IkReal x126=IKsin(j4);
02894 IkReal x127=((IkReal(1.00000000000000))*(sj5));
02895 IkReal x128=((new_r12)*(x126));
02896 IkReal x129=((new_r02)*(x125));
02897 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x126)))+(((new_r12)*(x125))));
02898 evalcond[1]=((sj5)+(x128)+(x129));
02899 evalcond[2]=((((new_r22)*(sj5)))+(((cj5)*(x128)))+(((cj5)*(x129))));
02900 evalcond[3]=((((cj5)*(new_r20)))+(((IkReal(-1.00000000000000))*(new_r00)*(x125)*(x127)))+(((IkReal(-1.00000000000000))*(new_r10)*(x126)*(x127))));
02901 evalcond[4]=((((cj5)*(new_r21)))+(((IkReal(-1.00000000000000))*(new_r01)*(x125)*(x127)))+(((IkReal(-1.00000000000000))*(new_r11)*(x126)*(x127))));
02902 evalcond[5]=((IkReal(-1.00000000000000))+(((cj5)*(new_r22)))+(((IkReal(-1.00000000000000))*(x127)*(x129)))+(((IkReal(-1.00000000000000))*(x127)*(x128))));
02903 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  )
02904 {
02905 continue;
02906 }
02907 }
02908 
02909 {
02910 IkReal dummyeval[1];
02911 IkReal gconst15;
02912 gconst15=IKsign(sj5);
02913 dummyeval[0]=sj5;
02914 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02915 {
02916 {
02917 IkReal dummyeval[1];
02918 dummyeval[0]=sj5;
02919 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02920 {
02921 {
02922 IkReal dummyeval[1];
02923 dummyeval[0]=sj5;
02924 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02925 {
02926 {
02927 IkReal evalcond[11];
02928 IkReal x130=((IkReal(-1.00000000000000))+(new_r22));
02929 IkReal x131=((new_r02)*(sj4));
02930 IkReal x132=((cj4)*(new_r12));
02931 IkReal x133=((((new_r12)*(sj4)))+(((cj4)*(new_r02))));
02932 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j5)), IkReal(6.28318530717959))));
02933 evalcond[1]=x130;
02934 evalcond[2]=new_r21;
02935 evalcond[3]=new_r20;
02936 evalcond[4]=((x132)+(((IkReal(-1.00000000000000))*(x131))));
02937 evalcond[5]=((x131)+(((IkReal(-1.00000000000000))*(x132))));
02938 evalcond[6]=x133;
02939 evalcond[7]=x133;
02940 evalcond[8]=new_r20;
02941 evalcond[9]=new_r21;
02942 evalcond[10]=x130;
02943 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
02944 {
02945 {
02946 IkReal j6array[1], cj6array[1], sj6array[1];
02947 bool j6valid[1]={false};
02948 _nj6 = 1;
02949 IkReal x134=((IkReal(1.00000000000000))*(sj4));
02950 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x134))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x134)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x134)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x134)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
02951     continue;
02952 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x134)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x134)))+(((cj4)*(new_r11)))));
02953 sj6array[0]=IKsin(j6array[0]);
02954 cj6array[0]=IKcos(j6array[0]);
02955 if( j6array[0] > IKPI )
02956 {
02957     j6array[0]-=IK2PI;
02958 }
02959 else if( j6array[0] < -IKPI )
02960 {    j6array[0]+=IK2PI;
02961 }
02962 j6valid[0] = true;
02963 for(int ij6 = 0; ij6 < 1; ++ij6)
02964 {
02965 if( !j6valid[ij6] )
02966 {
02967     continue;
02968 }
02969 _ij6[0] = ij6; _ij6[1] = -1;
02970 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
02971 {
02972 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
02973 {
02974     j6valid[iij6]=false; _ij6[1] = iij6; break; 
02975 }
02976 }
02977 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
02978 {
02979 IkReal evalcond[4];
02980 IkReal x135=IKsin(j6);
02981 IkReal x136=IKcos(j6);
02982 IkReal x137=((IkReal(1.00000000000000))*(cj4));
02983 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x137)))+(((new_r00)*(sj4)))+(x135));
02984 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x137)))+(((new_r01)*(sj4)))+(x136));
02985 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(x135));
02986 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(((IkReal(-1.00000000000000))*(x136))));
02987 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02988 {
02989 continue;
02990 }
02991 }
02992 
02993 {
02994 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
02995 vinfos[0].jointtype = 1;
02996 vinfos[0].foffset = j0;
02997 vinfos[0].indices[0] = _ij0[0];
02998 vinfos[0].indices[1] = _ij0[1];
02999 vinfos[0].maxsolutions = _nj0;
03000 vinfos[1].jointtype = 1;
03001 vinfos[1].foffset = j1;
03002 vinfos[1].indices[0] = _ij1[0];
03003 vinfos[1].indices[1] = _ij1[1];
03004 vinfos[1].maxsolutions = _nj1;
03005 vinfos[2].jointtype = 1;
03006 vinfos[2].foffset = j2;
03007 vinfos[2].indices[0] = _ij2[0];
03008 vinfos[2].indices[1] = _ij2[1];
03009 vinfos[2].maxsolutions = _nj2;
03010 vinfos[3].jointtype = 1;
03011 vinfos[3].foffset = j3;
03012 vinfos[3].indices[0] = _ij3[0];
03013 vinfos[3].indices[1] = _ij3[1];
03014 vinfos[3].maxsolutions = _nj3;
03015 vinfos[4].jointtype = 1;
03016 vinfos[4].foffset = j4;
03017 vinfos[4].indices[0] = _ij4[0];
03018 vinfos[4].indices[1] = _ij4[1];
03019 vinfos[4].maxsolutions = _nj4;
03020 vinfos[5].jointtype = 1;
03021 vinfos[5].foffset = j5;
03022 vinfos[5].indices[0] = _ij5[0];
03023 vinfos[5].indices[1] = _ij5[1];
03024 vinfos[5].maxsolutions = _nj5;
03025 vinfos[6].jointtype = 1;
03026 vinfos[6].foffset = j6;
03027 vinfos[6].indices[0] = _ij6[0];
03028 vinfos[6].indices[1] = _ij6[1];
03029 vinfos[6].maxsolutions = _nj6;
03030 std::vector<int> vfree(0);
03031 solutions.AddSolution(vinfos,vfree);
03032 }
03033 }
03034 }
03035 
03036 } else
03037 {
03038 IkReal x138=((cj4)*(new_r02));
03039 IkReal x139=((IkReal(1.00000000000000))*(new_r12));
03040 IkReal x140=((new_r02)*(sj4));
03041 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j5)), IkReal(6.28318530717959))));
03042 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
03043 evalcond[2]=new_r21;
03044 evalcond[3]=new_r20;
03045 evalcond[4]=((((cj4)*(new_r12)))+(((IkReal(-1.00000000000000))*(x140))));
03046 evalcond[5]=((x140)+(((IkReal(-1.00000000000000))*(cj4)*(x139))));
03047 evalcond[6]=((((new_r12)*(sj4)))+(x138));
03048 evalcond[7]=((((IkReal(-1.00000000000000))*(sj4)*(x139)))+(((IkReal(-1.00000000000000))*(x138))));
03049 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
03050 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
03051 evalcond[10]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
03052 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
03053 {
03054 {
03055 IkReal j6array[1], cj6array[1], sj6array[1];
03056 bool j6valid[1]={false};
03057 _nj6 = 1;
03058 IkReal x141=((IkReal(1.00000000000000))*(sj4));
03059 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x141))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x141)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x141)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x141)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
03060     continue;
03061 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x141)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x141)))+(((cj4)*(new_r11)))));
03062 sj6array[0]=IKsin(j6array[0]);
03063 cj6array[0]=IKcos(j6array[0]);
03064 if( j6array[0] > IKPI )
03065 {
03066     j6array[0]-=IK2PI;
03067 }
03068 else if( j6array[0] < -IKPI )
03069 {    j6array[0]+=IK2PI;
03070 }
03071 j6valid[0] = true;
03072 for(int ij6 = 0; ij6 < 1; ++ij6)
03073 {
03074 if( !j6valid[ij6] )
03075 {
03076     continue;
03077 }
03078 _ij6[0] = ij6; _ij6[1] = -1;
03079 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03080 {
03081 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03082 {
03083     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03084 }
03085 }
03086 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03087 {
03088 IkReal evalcond[4];
03089 IkReal x142=IKsin(j6);
03090 IkReal x143=IKcos(j6);
03091 IkReal x144=((IkReal(1.00000000000000))*(cj4));
03092 evalcond[0]=((((new_r00)*(sj4)))+(x142)+(((IkReal(-1.00000000000000))*(new_r10)*(x144))));
03093 evalcond[1]=((((new_r01)*(sj4)))+(((IkReal(-1.00000000000000))*(new_r11)*(x144)))+(x143));
03094 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x142))));
03095 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(x143));
03096 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03097 {
03098 continue;
03099 }
03100 }
03101 
03102 {
03103 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03104 vinfos[0].jointtype = 1;
03105 vinfos[0].foffset = j0;
03106 vinfos[0].indices[0] = _ij0[0];
03107 vinfos[0].indices[1] = _ij0[1];
03108 vinfos[0].maxsolutions = _nj0;
03109 vinfos[1].jointtype = 1;
03110 vinfos[1].foffset = j1;
03111 vinfos[1].indices[0] = _ij1[0];
03112 vinfos[1].indices[1] = _ij1[1];
03113 vinfos[1].maxsolutions = _nj1;
03114 vinfos[2].jointtype = 1;
03115 vinfos[2].foffset = j2;
03116 vinfos[2].indices[0] = _ij2[0];
03117 vinfos[2].indices[1] = _ij2[1];
03118 vinfos[2].maxsolutions = _nj2;
03119 vinfos[3].jointtype = 1;
03120 vinfos[3].foffset = j3;
03121 vinfos[3].indices[0] = _ij3[0];
03122 vinfos[3].indices[1] = _ij3[1];
03123 vinfos[3].maxsolutions = _nj3;
03124 vinfos[4].jointtype = 1;
03125 vinfos[4].foffset = j4;
03126 vinfos[4].indices[0] = _ij4[0];
03127 vinfos[4].indices[1] = _ij4[1];
03128 vinfos[4].maxsolutions = _nj4;
03129 vinfos[5].jointtype = 1;
03130 vinfos[5].foffset = j5;
03131 vinfos[5].indices[0] = _ij5[0];
03132 vinfos[5].indices[1] = _ij5[1];
03133 vinfos[5].maxsolutions = _nj5;
03134 vinfos[6].jointtype = 1;
03135 vinfos[6].foffset = j6;
03136 vinfos[6].indices[0] = _ij6[0];
03137 vinfos[6].indices[1] = _ij6[1];
03138 vinfos[6].maxsolutions = _nj6;
03139 std::vector<int> vfree(0);
03140 solutions.AddSolution(vinfos,vfree);
03141 }
03142 }
03143 }
03144 
03145 } else
03146 {
03147 if( 1 )
03148 {
03149 continue;
03150 
03151 } else
03152 {
03153 }
03154 }
03155 }
03156 }
03157 
03158 } else
03159 {
03160 {
03161 IkReal j6array[1], cj6array[1], sj6array[1];
03162 bool j6valid[1]={false};
03163 _nj6 = 1;
03164 if( IKabs(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
03165     continue;
03166 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11)))));
03167 sj6array[0]=IKsin(j6array[0]);
03168 cj6array[0]=IKcos(j6array[0]);
03169 if( j6array[0] > IKPI )
03170 {
03171     j6array[0]-=IK2PI;
03172 }
03173 else if( j6array[0] < -IKPI )
03174 {    j6array[0]+=IK2PI;
03175 }
03176 j6valid[0] = true;
03177 for(int ij6 = 0; ij6 < 1; ++ij6)
03178 {
03179 if( !j6valid[ij6] )
03180 {
03181     continue;
03182 }
03183 _ij6[0] = ij6; _ij6[1] = -1;
03184 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03185 {
03186 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03187 {
03188     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03189 }
03190 }
03191 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03192 {
03193 IkReal evalcond[8];
03194 IkReal x145=IKsin(j6);
03195 IkReal x146=IKcos(j6);
03196 IkReal x147=((cj4)*(new_r00));
03197 IkReal x148=((cj4)*(new_r01));
03198 IkReal x149=((cj5)*(sj4));
03199 IkReal x150=((IkReal(1.00000000000000))*(cj4));
03200 IkReal x151=((IkReal(1.00000000000000))*(x146));
03201 evalcond[0]=((new_r21)+(((sj5)*(x145))));
03202 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj5)*(x151))));
03203 evalcond[2]=((((new_r00)*(sj4)))+(x145)+(((IkReal(-1.00000000000000))*(new_r10)*(x150))));
03204 evalcond[3]=((((new_r01)*(sj4)))+(x146)+(((IkReal(-1.00000000000000))*(new_r11)*(x150))));
03205 evalcond[4]=((((cj5)*(x145)))+(((new_r11)*(sj4)))+(x148));
03206 evalcond[5]=((((new_r10)*(sj4)))+(x147)+(((IkReal(-1.00000000000000))*(cj5)*(x151))));
03207 evalcond[6]=((((cj5)*(x148)))+(((new_r11)*(x149)))+(x145)+(((new_r21)*(sj5))));
03208 evalcond[7]=((((cj5)*(x147)))+(((IkReal(-1.00000000000000))*(x151)))+(((new_r20)*(sj5)))+(((new_r10)*(x149))));
03209 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
03210 {
03211 continue;
03212 }
03213 }
03214 
03215 {
03216 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03217 vinfos[0].jointtype = 1;
03218 vinfos[0].foffset = j0;
03219 vinfos[0].indices[0] = _ij0[0];
03220 vinfos[0].indices[1] = _ij0[1];
03221 vinfos[0].maxsolutions = _nj0;
03222 vinfos[1].jointtype = 1;
03223 vinfos[1].foffset = j1;
03224 vinfos[1].indices[0] = _ij1[0];
03225 vinfos[1].indices[1] = _ij1[1];
03226 vinfos[1].maxsolutions = _nj1;
03227 vinfos[2].jointtype = 1;
03228 vinfos[2].foffset = j2;
03229 vinfos[2].indices[0] = _ij2[0];
03230 vinfos[2].indices[1] = _ij2[1];
03231 vinfos[2].maxsolutions = _nj2;
03232 vinfos[3].jointtype = 1;
03233 vinfos[3].foffset = j3;
03234 vinfos[3].indices[0] = _ij3[0];
03235 vinfos[3].indices[1] = _ij3[1];
03236 vinfos[3].maxsolutions = _nj3;
03237 vinfos[4].jointtype = 1;
03238 vinfos[4].foffset = j4;
03239 vinfos[4].indices[0] = _ij4[0];
03240 vinfos[4].indices[1] = _ij4[1];
03241 vinfos[4].maxsolutions = _nj4;
03242 vinfos[5].jointtype = 1;
03243 vinfos[5].foffset = j5;
03244 vinfos[5].indices[0] = _ij5[0];
03245 vinfos[5].indices[1] = _ij5[1];
03246 vinfos[5].maxsolutions = _nj5;
03247 vinfos[6].jointtype = 1;
03248 vinfos[6].foffset = j6;
03249 vinfos[6].indices[0] = _ij6[0];
03250 vinfos[6].indices[1] = _ij6[1];
03251 vinfos[6].maxsolutions = _nj6;
03252 std::vector<int> vfree(0);
03253 solutions.AddSolution(vinfos,vfree);
03254 }
03255 }
03256 }
03257 
03258 }
03259 
03260 }
03261 
03262 } else
03263 {
03264 {
03265 IkReal j6array[1], cj6array[1], sj6array[1];
03266 bool j6valid[1]={false};
03267 _nj6 = 1;
03268 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4)))))+IKsqr(((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
03269     continue;
03270 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4)))), ((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))));
03271 sj6array[0]=IKsin(j6array[0]);
03272 cj6array[0]=IKcos(j6array[0]);
03273 if( j6array[0] > IKPI )
03274 {
03275     j6array[0]-=IK2PI;
03276 }
03277 else if( j6array[0] < -IKPI )
03278 {    j6array[0]+=IK2PI;
03279 }
03280 j6valid[0] = true;
03281 for(int ij6 = 0; ij6 < 1; ++ij6)
03282 {
03283 if( !j6valid[ij6] )
03284 {
03285     continue;
03286 }
03287 _ij6[0] = ij6; _ij6[1] = -1;
03288 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03289 {
03290 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03291 {
03292     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03293 }
03294 }
03295 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03296 {
03297 IkReal evalcond[8];
03298 IkReal x152=IKsin(j6);
03299 IkReal x153=IKcos(j6);
03300 IkReal x154=((cj4)*(new_r00));
03301 IkReal x155=((cj4)*(new_r01));
03302 IkReal x156=((cj5)*(sj4));
03303 IkReal x157=((IkReal(1.00000000000000))*(cj4));
03304 IkReal x158=((IkReal(1.00000000000000))*(x153));
03305 evalcond[0]=((((sj5)*(x152)))+(new_r21));
03306 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj5)*(x158))));
03307 evalcond[2]=((((new_r00)*(sj4)))+(x152)+(((IkReal(-1.00000000000000))*(new_r10)*(x157))));
03308 evalcond[3]=((((new_r01)*(sj4)))+(x153)+(((IkReal(-1.00000000000000))*(new_r11)*(x157))));
03309 evalcond[4]=((((new_r11)*(sj4)))+(x155)+(((cj5)*(x152))));
03310 evalcond[5]=((((new_r10)*(sj4)))+(x154)+(((IkReal(-1.00000000000000))*(cj5)*(x158))));
03311 evalcond[6]=((((new_r11)*(x156)))+(x152)+(((new_r21)*(sj5)))+(((cj5)*(x155))));
03312 evalcond[7]=((((IkReal(-1.00000000000000))*(x158)))+(((new_r20)*(sj5)))+(((new_r10)*(x156)))+(((cj5)*(x154))));
03313 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
03314 {
03315 continue;
03316 }
03317 }
03318 
03319 {
03320 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03321 vinfos[0].jointtype = 1;
03322 vinfos[0].foffset = j0;
03323 vinfos[0].indices[0] = _ij0[0];
03324 vinfos[0].indices[1] = _ij0[1];
03325 vinfos[0].maxsolutions = _nj0;
03326 vinfos[1].jointtype = 1;
03327 vinfos[1].foffset = j1;
03328 vinfos[1].indices[0] = _ij1[0];
03329 vinfos[1].indices[1] = _ij1[1];
03330 vinfos[1].maxsolutions = _nj1;
03331 vinfos[2].jointtype = 1;
03332 vinfos[2].foffset = j2;
03333 vinfos[2].indices[0] = _ij2[0];
03334 vinfos[2].indices[1] = _ij2[1];
03335 vinfos[2].maxsolutions = _nj2;
03336 vinfos[3].jointtype = 1;
03337 vinfos[3].foffset = j3;
03338 vinfos[3].indices[0] = _ij3[0];
03339 vinfos[3].indices[1] = _ij3[1];
03340 vinfos[3].maxsolutions = _nj3;
03341 vinfos[4].jointtype = 1;
03342 vinfos[4].foffset = j4;
03343 vinfos[4].indices[0] = _ij4[0];
03344 vinfos[4].indices[1] = _ij4[1];
03345 vinfos[4].maxsolutions = _nj4;
03346 vinfos[5].jointtype = 1;
03347 vinfos[5].foffset = j5;
03348 vinfos[5].indices[0] = _ij5[0];
03349 vinfos[5].indices[1] = _ij5[1];
03350 vinfos[5].maxsolutions = _nj5;
03351 vinfos[6].jointtype = 1;
03352 vinfos[6].foffset = j6;
03353 vinfos[6].indices[0] = _ij6[0];
03354 vinfos[6].indices[1] = _ij6[1];
03355 vinfos[6].maxsolutions = _nj6;
03356 std::vector<int> vfree(0);
03357 solutions.AddSolution(vinfos,vfree);
03358 }
03359 }
03360 }
03361 
03362 }
03363 
03364 }
03365 
03366 } else
03367 {
03368 {
03369 IkReal j6array[1], cj6array[1], sj6array[1];
03370 bool j6valid[1]={false};
03371 _nj6 = 1;
03372 if( IKabs(((IkReal(-1.00000000000000))*(gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
03373     continue;
03374 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst15)*(new_r21)), ((gconst15)*(new_r20)));
03375 sj6array[0]=IKsin(j6array[0]);
03376 cj6array[0]=IKcos(j6array[0]);
03377 if( j6array[0] > IKPI )
03378 {
03379     j6array[0]-=IK2PI;
03380 }
03381 else if( j6array[0] < -IKPI )
03382 {    j6array[0]+=IK2PI;
03383 }
03384 j6valid[0] = true;
03385 for(int ij6 = 0; ij6 < 1; ++ij6)
03386 {
03387 if( !j6valid[ij6] )
03388 {
03389     continue;
03390 }
03391 _ij6[0] = ij6; _ij6[1] = -1;
03392 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03393 {
03394 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03395 {
03396     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03397 }
03398 }
03399 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03400 {
03401 IkReal evalcond[8];
03402 IkReal x159=IKsin(j6);
03403 IkReal x160=IKcos(j6);
03404 IkReal x161=((cj4)*(new_r00));
03405 IkReal x162=((cj4)*(new_r01));
03406 IkReal x163=((cj5)*(sj4));
03407 IkReal x164=((IkReal(1.00000000000000))*(cj4));
03408 IkReal x165=((IkReal(1.00000000000000))*(x160));
03409 evalcond[0]=((((sj5)*(x159)))+(new_r21));
03410 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x165)))+(new_r20));
03411 evalcond[2]=((((new_r00)*(sj4)))+(((IkReal(-1.00000000000000))*(new_r10)*(x164)))+(x159));
03412 evalcond[3]=((((new_r01)*(sj4)))+(((IkReal(-1.00000000000000))*(new_r11)*(x164)))+(x160));
03413 evalcond[4]=((((new_r11)*(sj4)))+(x162)+(((cj5)*(x159))));
03414 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x165)))+(((new_r10)*(sj4)))+(x161));
03415 evalcond[6]=((((new_r11)*(x163)))+(((cj5)*(x162)))+(x159)+(((new_r21)*(sj5))));
03416 evalcond[7]=((((new_r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x165)))+(((new_r10)*(x163)))+(((cj5)*(x161))));
03417 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
03418 {
03419 continue;
03420 }
03421 }
03422 
03423 {
03424 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03425 vinfos[0].jointtype = 1;
03426 vinfos[0].foffset = j0;
03427 vinfos[0].indices[0] = _ij0[0];
03428 vinfos[0].indices[1] = _ij0[1];
03429 vinfos[0].maxsolutions = _nj0;
03430 vinfos[1].jointtype = 1;
03431 vinfos[1].foffset = j1;
03432 vinfos[1].indices[0] = _ij1[0];
03433 vinfos[1].indices[1] = _ij1[1];
03434 vinfos[1].maxsolutions = _nj1;
03435 vinfos[2].jointtype = 1;
03436 vinfos[2].foffset = j2;
03437 vinfos[2].indices[0] = _ij2[0];
03438 vinfos[2].indices[1] = _ij2[1];
03439 vinfos[2].maxsolutions = _nj2;
03440 vinfos[3].jointtype = 1;
03441 vinfos[3].foffset = j3;
03442 vinfos[3].indices[0] = _ij3[0];
03443 vinfos[3].indices[1] = _ij3[1];
03444 vinfos[3].maxsolutions = _nj3;
03445 vinfos[4].jointtype = 1;
03446 vinfos[4].foffset = j4;
03447 vinfos[4].indices[0] = _ij4[0];
03448 vinfos[4].indices[1] = _ij4[1];
03449 vinfos[4].maxsolutions = _nj4;
03450 vinfos[5].jointtype = 1;
03451 vinfos[5].foffset = j5;
03452 vinfos[5].indices[0] = _ij5[0];
03453 vinfos[5].indices[1] = _ij5[1];
03454 vinfos[5].maxsolutions = _nj5;
03455 vinfos[6].jointtype = 1;
03456 vinfos[6].foffset = j6;
03457 vinfos[6].indices[0] = _ij6[0];
03458 vinfos[6].indices[1] = _ij6[1];
03459 vinfos[6].maxsolutions = _nj6;
03460 std::vector<int> vfree(0);
03461 solutions.AddSolution(vinfos,vfree);
03462 }
03463 }
03464 }
03465 
03466 }
03467 
03468 }
03469 }
03470 }
03471 
03472 }
03473 
03474 }
03475 
03476 } else
03477 {
03478 {
03479 IkReal j4array[1], cj4array[1], sj4array[1];
03480 bool j4valid[1]={false};
03481 _nj4 = 1;
03482 IkReal x166=((gconst12)*(sj5));
03483 if( IKabs(((new_r12)*(x166))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x166))) < IKFAST_ATAN2_MAGTHRESH )
03484     continue;
03485 j4array[0]=IKatan2(((new_r12)*(x166)), ((new_r02)*(x166)));
03486 sj4array[0]=IKsin(j4array[0]);
03487 cj4array[0]=IKcos(j4array[0]);
03488 if( j4array[0] > IKPI )
03489 {
03490     j4array[0]-=IK2PI;
03491 }
03492 else if( j4array[0] < -IKPI )
03493 {    j4array[0]+=IK2PI;
03494 }
03495 j4valid[0] = true;
03496 for(int ij4 = 0; ij4 < 1; ++ij4)
03497 {
03498 if( !j4valid[ij4] )
03499 {
03500     continue;
03501 }
03502 _ij4[0] = ij4; _ij4[1] = -1;
03503 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03504 {
03505 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03506 {
03507     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03508 }
03509 }
03510 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03511 {
03512 IkReal evalcond[6];
03513 IkReal x167=IKcos(j4);
03514 IkReal x168=IKsin(j4);
03515 IkReal x169=((IkReal(1.00000000000000))*(sj5));
03516 IkReal x170=((new_r12)*(x168));
03517 IkReal x171=((new_r02)*(x167));
03518 evalcond[0]=((((new_r12)*(x167)))+(((IkReal(-1.00000000000000))*(new_r02)*(x168))));
03519 evalcond[1]=((sj5)+(x171)+(x170));
03520 evalcond[2]=((((cj5)*(x170)))+(((cj5)*(x171)))+(((new_r22)*(sj5))));
03521 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r00)*(x167)*(x169)))+(((cj5)*(new_r20)))+(((IkReal(-1.00000000000000))*(new_r10)*(x168)*(x169))));
03522 evalcond[4]=((((IkReal(-1.00000000000000))*(new_r01)*(x167)*(x169)))+(((IkReal(-1.00000000000000))*(new_r11)*(x168)*(x169)))+(((cj5)*(new_r21))));
03523 evalcond[5]=((IkReal(-1.00000000000000))+(((cj5)*(new_r22)))+(((IkReal(-1.00000000000000))*(x169)*(x171)))+(((IkReal(-1.00000000000000))*(x169)*(x170))));
03524 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  )
03525 {
03526 continue;
03527 }
03528 }
03529 
03530 {
03531 IkReal dummyeval[1];
03532 IkReal gconst15;
03533 gconst15=IKsign(sj5);
03534 dummyeval[0]=sj5;
03535 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03536 {
03537 {
03538 IkReal dummyeval[1];
03539 dummyeval[0]=sj5;
03540 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03541 {
03542 {
03543 IkReal dummyeval[1];
03544 dummyeval[0]=sj5;
03545 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03546 {
03547 {
03548 IkReal evalcond[11];
03549 IkReal x172=((IkReal(-1.00000000000000))+(new_r22));
03550 IkReal x173=((new_r02)*(sj4));
03551 IkReal x174=((cj4)*(new_r12));
03552 IkReal x175=((((new_r12)*(sj4)))+(((cj4)*(new_r02))));
03553 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j5)), IkReal(6.28318530717959))));
03554 evalcond[1]=x172;
03555 evalcond[2]=new_r21;
03556 evalcond[3]=new_r20;
03557 evalcond[4]=((((IkReal(-1.00000000000000))*(x173)))+(x174));
03558 evalcond[5]=((((IkReal(-1.00000000000000))*(x174)))+(x173));
03559 evalcond[6]=x175;
03560 evalcond[7]=x175;
03561 evalcond[8]=new_r20;
03562 evalcond[9]=new_r21;
03563 evalcond[10]=x172;
03564 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
03565 {
03566 {
03567 IkReal j6array[1], cj6array[1], sj6array[1];
03568 bool j6valid[1]={false};
03569 _nj6 = 1;
03570 IkReal x176=((IkReal(1.00000000000000))*(sj4));
03571 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x176))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x176))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x176)))))+IKsqr(((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x176)))))-1) <= IKFAST_SINCOS_THRESH )
03572     continue;
03573 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x176)))), ((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x176)))));
03574 sj6array[0]=IKsin(j6array[0]);
03575 cj6array[0]=IKcos(j6array[0]);
03576 if( j6array[0] > IKPI )
03577 {
03578     j6array[0]-=IK2PI;
03579 }
03580 else if( j6array[0] < -IKPI )
03581 {    j6array[0]+=IK2PI;
03582 }
03583 j6valid[0] = true;
03584 for(int ij6 = 0; ij6 < 1; ++ij6)
03585 {
03586 if( !j6valid[ij6] )
03587 {
03588     continue;
03589 }
03590 _ij6[0] = ij6; _ij6[1] = -1;
03591 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03592 {
03593 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03594 {
03595     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03596 }
03597 }
03598 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03599 {
03600 IkReal evalcond[4];
03601 IkReal x177=IKsin(j6);
03602 IkReal x178=IKcos(j6);
03603 IkReal x179=((IkReal(1.00000000000000))*(cj4));
03604 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x179)))+(((new_r00)*(sj4)))+(x177));
03605 evalcond[1]=((((new_r01)*(sj4)))+(((IkReal(-1.00000000000000))*(new_r11)*(x179)))+(x178));
03606 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(x177));
03607 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(((IkReal(-1.00000000000000))*(x178))));
03608 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03609 {
03610 continue;
03611 }
03612 }
03613 
03614 {
03615 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03616 vinfos[0].jointtype = 1;
03617 vinfos[0].foffset = j0;
03618 vinfos[0].indices[0] = _ij0[0];
03619 vinfos[0].indices[1] = _ij0[1];
03620 vinfos[0].maxsolutions = _nj0;
03621 vinfos[1].jointtype = 1;
03622 vinfos[1].foffset = j1;
03623 vinfos[1].indices[0] = _ij1[0];
03624 vinfos[1].indices[1] = _ij1[1];
03625 vinfos[1].maxsolutions = _nj1;
03626 vinfos[2].jointtype = 1;
03627 vinfos[2].foffset = j2;
03628 vinfos[2].indices[0] = _ij2[0];
03629 vinfos[2].indices[1] = _ij2[1];
03630 vinfos[2].maxsolutions = _nj2;
03631 vinfos[3].jointtype = 1;
03632 vinfos[3].foffset = j3;
03633 vinfos[3].indices[0] = _ij3[0];
03634 vinfos[3].indices[1] = _ij3[1];
03635 vinfos[3].maxsolutions = _nj3;
03636 vinfos[4].jointtype = 1;
03637 vinfos[4].foffset = j4;
03638 vinfos[4].indices[0] = _ij4[0];
03639 vinfos[4].indices[1] = _ij4[1];
03640 vinfos[4].maxsolutions = _nj4;
03641 vinfos[5].jointtype = 1;
03642 vinfos[5].foffset = j5;
03643 vinfos[5].indices[0] = _ij5[0];
03644 vinfos[5].indices[1] = _ij5[1];
03645 vinfos[5].maxsolutions = _nj5;
03646 vinfos[6].jointtype = 1;
03647 vinfos[6].foffset = j6;
03648 vinfos[6].indices[0] = _ij6[0];
03649 vinfos[6].indices[1] = _ij6[1];
03650 vinfos[6].maxsolutions = _nj6;
03651 std::vector<int> vfree(0);
03652 solutions.AddSolution(vinfos,vfree);
03653 }
03654 }
03655 }
03656 
03657 } else
03658 {
03659 IkReal x180=((cj4)*(new_r02));
03660 IkReal x181=((IkReal(1.00000000000000))*(new_r12));
03661 IkReal x182=((new_r02)*(sj4));
03662 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j5)), IkReal(6.28318530717959))));
03663 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
03664 evalcond[2]=new_r21;
03665 evalcond[3]=new_r20;
03666 evalcond[4]=((((cj4)*(new_r12)))+(((IkReal(-1.00000000000000))*(x182))));
03667 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x181)))+(x182));
03668 evalcond[6]=((((new_r12)*(sj4)))+(x180));
03669 evalcond[7]=((((IkReal(-1.00000000000000))*(x180)))+(((IkReal(-1.00000000000000))*(sj4)*(x181))));
03670 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
03671 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
03672 evalcond[10]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
03673 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
03674 {
03675 {
03676 IkReal j6array[1], cj6array[1], sj6array[1];
03677 bool j6valid[1]={false};
03678 _nj6 = 1;
03679 IkReal x183=((IkReal(1.00000000000000))*(sj4));
03680 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x183))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x183))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x183)))))+IKsqr(((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x183)))))-1) <= IKFAST_SINCOS_THRESH )
03681     continue;
03682 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x183)))), ((((cj4)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x183)))));
03683 sj6array[0]=IKsin(j6array[0]);
03684 cj6array[0]=IKcos(j6array[0]);
03685 if( j6array[0] > IKPI )
03686 {
03687     j6array[0]-=IK2PI;
03688 }
03689 else if( j6array[0] < -IKPI )
03690 {    j6array[0]+=IK2PI;
03691 }
03692 j6valid[0] = true;
03693 for(int ij6 = 0; ij6 < 1; ++ij6)
03694 {
03695 if( !j6valid[ij6] )
03696 {
03697     continue;
03698 }
03699 _ij6[0] = ij6; _ij6[1] = -1;
03700 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03701 {
03702 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03703 {
03704     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03705 }
03706 }
03707 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03708 {
03709 IkReal evalcond[4];
03710 IkReal x184=IKsin(j6);
03711 IkReal x185=IKcos(j6);
03712 IkReal x186=((IkReal(1.00000000000000))*(cj4));
03713 evalcond[0]=((((new_r00)*(sj4)))+(x184)+(((IkReal(-1.00000000000000))*(new_r10)*(x186))));
03714 evalcond[1]=((((new_r01)*(sj4)))+(x185)+(((IkReal(-1.00000000000000))*(new_r11)*(x186))));
03715 evalcond[2]=((((cj4)*(new_r01)))+(((new_r11)*(sj4)))+(((IkReal(-1.00000000000000))*(x184))));
03716 evalcond[3]=((((cj4)*(new_r00)))+(((new_r10)*(sj4)))+(x185));
03717 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03718 {
03719 continue;
03720 }
03721 }
03722 
03723 {
03724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03725 vinfos[0].jointtype = 1;
03726 vinfos[0].foffset = j0;
03727 vinfos[0].indices[0] = _ij0[0];
03728 vinfos[0].indices[1] = _ij0[1];
03729 vinfos[0].maxsolutions = _nj0;
03730 vinfos[1].jointtype = 1;
03731 vinfos[1].foffset = j1;
03732 vinfos[1].indices[0] = _ij1[0];
03733 vinfos[1].indices[1] = _ij1[1];
03734 vinfos[1].maxsolutions = _nj1;
03735 vinfos[2].jointtype = 1;
03736 vinfos[2].foffset = j2;
03737 vinfos[2].indices[0] = _ij2[0];
03738 vinfos[2].indices[1] = _ij2[1];
03739 vinfos[2].maxsolutions = _nj2;
03740 vinfos[3].jointtype = 1;
03741 vinfos[3].foffset = j3;
03742 vinfos[3].indices[0] = _ij3[0];
03743 vinfos[3].indices[1] = _ij3[1];
03744 vinfos[3].maxsolutions = _nj3;
03745 vinfos[4].jointtype = 1;
03746 vinfos[4].foffset = j4;
03747 vinfos[4].indices[0] = _ij4[0];
03748 vinfos[4].indices[1] = _ij4[1];
03749 vinfos[4].maxsolutions = _nj4;
03750 vinfos[5].jointtype = 1;
03751 vinfos[5].foffset = j5;
03752 vinfos[5].indices[0] = _ij5[0];
03753 vinfos[5].indices[1] = _ij5[1];
03754 vinfos[5].maxsolutions = _nj5;
03755 vinfos[6].jointtype = 1;
03756 vinfos[6].foffset = j6;
03757 vinfos[6].indices[0] = _ij6[0];
03758 vinfos[6].indices[1] = _ij6[1];
03759 vinfos[6].maxsolutions = _nj6;
03760 std::vector<int> vfree(0);
03761 solutions.AddSolution(vinfos,vfree);
03762 }
03763 }
03764 }
03765 
03766 } else
03767 {
03768 if( 1 )
03769 {
03770 continue;
03771 
03772 } else
03773 {
03774 }
03775 }
03776 }
03777 }
03778 
03779 } else
03780 {
03781 {
03782 IkReal j6array[1], cj6array[1], sj6array[1];
03783 bool j6valid[1]={false};
03784 _nj6 = 1;
03785 if( IKabs(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
03786     continue;
03787 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj4)))+(((cj4)*(new_r11)))));
03788 sj6array[0]=IKsin(j6array[0]);
03789 cj6array[0]=IKcos(j6array[0]);
03790 if( j6array[0] > IKPI )
03791 {
03792     j6array[0]-=IK2PI;
03793 }
03794 else if( j6array[0] < -IKPI )
03795 {    j6array[0]+=IK2PI;
03796 }
03797 j6valid[0] = true;
03798 for(int ij6 = 0; ij6 < 1; ++ij6)
03799 {
03800 if( !j6valid[ij6] )
03801 {
03802     continue;
03803 }
03804 _ij6[0] = ij6; _ij6[1] = -1;
03805 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03806 {
03807 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03808 {
03809     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03810 }
03811 }
03812 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03813 {
03814 IkReal evalcond[8];
03815 IkReal x187=IKsin(j6);
03816 IkReal x188=IKcos(j6);
03817 IkReal x189=((cj4)*(new_r00));
03818 IkReal x190=((cj4)*(new_r01));
03819 IkReal x191=((cj5)*(sj4));
03820 IkReal x192=((IkReal(1.00000000000000))*(cj4));
03821 IkReal x193=((IkReal(1.00000000000000))*(x188));
03822 evalcond[0]=((new_r21)+(((sj5)*(x187))));
03823 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x193)))+(new_r20));
03824 evalcond[2]=((((new_r00)*(sj4)))+(x187)+(((IkReal(-1.00000000000000))*(new_r10)*(x192))));
03825 evalcond[3]=((((new_r01)*(sj4)))+(x188)+(((IkReal(-1.00000000000000))*(new_r11)*(x192))));
03826 evalcond[4]=((((cj5)*(x187)))+(((new_r11)*(sj4)))+(x190));
03827 evalcond[5]=((((new_r10)*(sj4)))+(((IkReal(-1.00000000000000))*(cj5)*(x193)))+(x189));
03828 evalcond[6]=((((cj5)*(x190)))+(((new_r11)*(x191)))+(x187)+(((new_r21)*(sj5))));
03829 evalcond[7]=((((new_r10)*(x191)))+(((cj5)*(x189)))+(((new_r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x193))));
03830 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
03831 {
03832 continue;
03833 }
03834 }
03835 
03836 {
03837 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03838 vinfos[0].jointtype = 1;
03839 vinfos[0].foffset = j0;
03840 vinfos[0].indices[0] = _ij0[0];
03841 vinfos[0].indices[1] = _ij0[1];
03842 vinfos[0].maxsolutions = _nj0;
03843 vinfos[1].jointtype = 1;
03844 vinfos[1].foffset = j1;
03845 vinfos[1].indices[0] = _ij1[0];
03846 vinfos[1].indices[1] = _ij1[1];
03847 vinfos[1].maxsolutions = _nj1;
03848 vinfos[2].jointtype = 1;
03849 vinfos[2].foffset = j2;
03850 vinfos[2].indices[0] = _ij2[0];
03851 vinfos[2].indices[1] = _ij2[1];
03852 vinfos[2].maxsolutions = _nj2;
03853 vinfos[3].jointtype = 1;
03854 vinfos[3].foffset = j3;
03855 vinfos[3].indices[0] = _ij3[0];
03856 vinfos[3].indices[1] = _ij3[1];
03857 vinfos[3].maxsolutions = _nj3;
03858 vinfos[4].jointtype = 1;
03859 vinfos[4].foffset = j4;
03860 vinfos[4].indices[0] = _ij4[0];
03861 vinfos[4].indices[1] = _ij4[1];
03862 vinfos[4].maxsolutions = _nj4;
03863 vinfos[5].jointtype = 1;
03864 vinfos[5].foffset = j5;
03865 vinfos[5].indices[0] = _ij5[0];
03866 vinfos[5].indices[1] = _ij5[1];
03867 vinfos[5].maxsolutions = _nj5;
03868 vinfos[6].jointtype = 1;
03869 vinfos[6].foffset = j6;
03870 vinfos[6].indices[0] = _ij6[0];
03871 vinfos[6].indices[1] = _ij6[1];
03872 vinfos[6].maxsolutions = _nj6;
03873 std::vector<int> vfree(0);
03874 solutions.AddSolution(vinfos,vfree);
03875 }
03876 }
03877 }
03878 
03879 }
03880 
03881 }
03882 
03883 } else
03884 {
03885 {
03886 IkReal j6array[1], cj6array[1], sj6array[1];
03887 bool j6valid[1]={false};
03888 _nj6 = 1;
03889 if( IKabs(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4)))))+IKsqr(((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
03890     continue;
03891 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj4)))), ((new_r20)*(((IKabs(sj5) != 0)?((IkReal)1/(sj5)):(IkReal)1.0e30))));
03892 sj6array[0]=IKsin(j6array[0]);
03893 cj6array[0]=IKcos(j6array[0]);
03894 if( j6array[0] > IKPI )
03895 {
03896     j6array[0]-=IK2PI;
03897 }
03898 else if( j6array[0] < -IKPI )
03899 {    j6array[0]+=IK2PI;
03900 }
03901 j6valid[0] = true;
03902 for(int ij6 = 0; ij6 < 1; ++ij6)
03903 {
03904 if( !j6valid[ij6] )
03905 {
03906     continue;
03907 }
03908 _ij6[0] = ij6; _ij6[1] = -1;
03909 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
03910 {
03911 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
03912 {
03913     j6valid[iij6]=false; _ij6[1] = iij6; break; 
03914 }
03915 }
03916 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
03917 {
03918 IkReal evalcond[8];
03919 IkReal x194=IKsin(j6);
03920 IkReal x195=IKcos(j6);
03921 IkReal x196=((cj4)*(new_r00));
03922 IkReal x197=((cj4)*(new_r01));
03923 IkReal x198=((cj5)*(sj4));
03924 IkReal x199=((IkReal(1.00000000000000))*(cj4));
03925 IkReal x200=((IkReal(1.00000000000000))*(x195));
03926 evalcond[0]=((new_r21)+(((sj5)*(x194))));
03927 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x200)))+(new_r20));
03928 evalcond[2]=((((new_r00)*(sj4)))+(x194)+(((IkReal(-1.00000000000000))*(new_r10)*(x199))));
03929 evalcond[3]=((((new_r01)*(sj4)))+(x195)+(((IkReal(-1.00000000000000))*(new_r11)*(x199))));
03930 evalcond[4]=((((cj5)*(x194)))+(((new_r11)*(sj4)))+(x197));
03931 evalcond[5]=((((new_r10)*(sj4)))+(x196)+(((IkReal(-1.00000000000000))*(cj5)*(x200))));
03932 evalcond[6]=((((cj5)*(x197)))+(((new_r11)*(x198)))+(x194)+(((new_r21)*(sj5))));
03933 evalcond[7]=((((new_r10)*(x198)))+(((cj5)*(x196)))+(((new_r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x200))));
03934 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
03935 {
03936 continue;
03937 }
03938 }
03939 
03940 {
03941 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
03942 vinfos[0].jointtype = 1;
03943 vinfos[0].foffset = j0;
03944 vinfos[0].indices[0] = _ij0[0];
03945 vinfos[0].indices[1] = _ij0[1];
03946 vinfos[0].maxsolutions = _nj0;
03947 vinfos[1].jointtype = 1;
03948 vinfos[1].foffset = j1;
03949 vinfos[1].indices[0] = _ij1[0];
03950 vinfos[1].indices[1] = _ij1[1];
03951 vinfos[1].maxsolutions = _nj1;
03952 vinfos[2].jointtype = 1;
03953 vinfos[2].foffset = j2;
03954 vinfos[2].indices[0] = _ij2[0];
03955 vinfos[2].indices[1] = _ij2[1];
03956 vinfos[2].maxsolutions = _nj2;
03957 vinfos[3].jointtype = 1;
03958 vinfos[3].foffset = j3;
03959 vinfos[3].indices[0] = _ij3[0];
03960 vinfos[3].indices[1] = _ij3[1];
03961 vinfos[3].maxsolutions = _nj3;
03962 vinfos[4].jointtype = 1;
03963 vinfos[4].foffset = j4;
03964 vinfos[4].indices[0] = _ij4[0];
03965 vinfos[4].indices[1] = _ij4[1];
03966 vinfos[4].maxsolutions = _nj4;
03967 vinfos[5].jointtype = 1;
03968 vinfos[5].foffset = j5;
03969 vinfos[5].indices[0] = _ij5[0];
03970 vinfos[5].indices[1] = _ij5[1];
03971 vinfos[5].maxsolutions = _nj5;
03972 vinfos[6].jointtype = 1;
03973 vinfos[6].foffset = j6;
03974 vinfos[6].indices[0] = _ij6[0];
03975 vinfos[6].indices[1] = _ij6[1];
03976 vinfos[6].maxsolutions = _nj6;
03977 std::vector<int> vfree(0);
03978 solutions.AddSolution(vinfos,vfree);
03979 }
03980 }
03981 }
03982 
03983 }
03984 
03985 }
03986 
03987 } else
03988 {
03989 {
03990 IkReal j6array[1], cj6array[1], sj6array[1];
03991 bool j6valid[1]={false};
03992 _nj6 = 1;
03993 if( IKabs(((IkReal(-1.00000000000000))*(gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
03994     continue;
03995 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst15)*(new_r21)), ((gconst15)*(new_r20)));
03996 sj6array[0]=IKsin(j6array[0]);
03997 cj6array[0]=IKcos(j6array[0]);
03998 if( j6array[0] > IKPI )
03999 {
04000     j6array[0]-=IK2PI;
04001 }
04002 else if( j6array[0] < -IKPI )
04003 {    j6array[0]+=IK2PI;
04004 }
04005 j6valid[0] = true;
04006 for(int ij6 = 0; ij6 < 1; ++ij6)
04007 {
04008 if( !j6valid[ij6] )
04009 {
04010     continue;
04011 }
04012 _ij6[0] = ij6; _ij6[1] = -1;
04013 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
04014 {
04015 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
04016 {
04017     j6valid[iij6]=false; _ij6[1] = iij6; break; 
04018 }
04019 }
04020 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
04021 {
04022 IkReal evalcond[8];
04023 IkReal x201=IKsin(j6);
04024 IkReal x202=IKcos(j6);
04025 IkReal x203=((cj4)*(new_r00));
04026 IkReal x204=((cj4)*(new_r01));
04027 IkReal x205=((cj5)*(sj4));
04028 IkReal x206=((IkReal(1.00000000000000))*(cj4));
04029 IkReal x207=((IkReal(1.00000000000000))*(x202));
04030 evalcond[0]=((new_r21)+(((sj5)*(x201))));
04031 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x207)))+(new_r20));
04032 evalcond[2]=((((new_r00)*(sj4)))+(x201)+(((IkReal(-1.00000000000000))*(new_r10)*(x206))));
04033 evalcond[3]=((((new_r01)*(sj4)))+(x202)+(((IkReal(-1.00000000000000))*(new_r11)*(x206))));
04034 evalcond[4]=((((new_r11)*(sj4)))+(x204)+(((cj5)*(x201))));
04035 evalcond[5]=((((new_r10)*(sj4)))+(((IkReal(-1.00000000000000))*(cj5)*(x207)))+(x203));
04036 evalcond[6]=((((new_r11)*(x205)))+(x201)+(((cj5)*(x204)))+(((new_r21)*(sj5))));
04037 evalcond[7]=((((new_r20)*(sj5)))+(((new_r10)*(x205)))+(((IkReal(-1.00000000000000))*(x207)))+(((cj5)*(x203))));
04038 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
04039 {
04040 continue;
04041 }
04042 }
04043 
04044 {
04045 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04046 vinfos[0].jointtype = 1;
04047 vinfos[0].foffset = j0;
04048 vinfos[0].indices[0] = _ij0[0];
04049 vinfos[0].indices[1] = _ij0[1];
04050 vinfos[0].maxsolutions = _nj0;
04051 vinfos[1].jointtype = 1;
04052 vinfos[1].foffset = j1;
04053 vinfos[1].indices[0] = _ij1[0];
04054 vinfos[1].indices[1] = _ij1[1];
04055 vinfos[1].maxsolutions = _nj1;
04056 vinfos[2].jointtype = 1;
04057 vinfos[2].foffset = j2;
04058 vinfos[2].indices[0] = _ij2[0];
04059 vinfos[2].indices[1] = _ij2[1];
04060 vinfos[2].maxsolutions = _nj2;
04061 vinfos[3].jointtype = 1;
04062 vinfos[3].foffset = j3;
04063 vinfos[3].indices[0] = _ij3[0];
04064 vinfos[3].indices[1] = _ij3[1];
04065 vinfos[3].maxsolutions = _nj3;
04066 vinfos[4].jointtype = 1;
04067 vinfos[4].foffset = j4;
04068 vinfos[4].indices[0] = _ij4[0];
04069 vinfos[4].indices[1] = _ij4[1];
04070 vinfos[4].maxsolutions = _nj4;
04071 vinfos[5].jointtype = 1;
04072 vinfos[5].foffset = j5;
04073 vinfos[5].indices[0] = _ij5[0];
04074 vinfos[5].indices[1] = _ij5[1];
04075 vinfos[5].maxsolutions = _nj5;
04076 vinfos[6].jointtype = 1;
04077 vinfos[6].foffset = j6;
04078 vinfos[6].indices[0] = _ij6[0];
04079 vinfos[6].indices[1] = _ij6[1];
04080 vinfos[6].maxsolutions = _nj6;
04081 std::vector<int> vfree(0);
04082 solutions.AddSolution(vinfos,vfree);
04083 }
04084 }
04085 }
04086 
04087 }
04088 
04089 }
04090 }
04091 }
04092 
04093 }
04094 
04095 }
04096 
04097 } else
04098 {
04099 {
04100 IkReal j6array[1], cj6array[1], sj6array[1];
04101 bool j6valid[1]={false};
04102 _nj6 = 1;
04103 if( IKabs(((IkReal(-1.00000000000000))*(gconst14)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst14)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
04104     continue;
04105 j6array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst14)*(new_r21)), ((gconst14)*(new_r20)));
04106 sj6array[0]=IKsin(j6array[0]);
04107 cj6array[0]=IKcos(j6array[0]);
04108 if( j6array[0] > IKPI )
04109 {
04110     j6array[0]-=IK2PI;
04111 }
04112 else if( j6array[0] < -IKPI )
04113 {    j6array[0]+=IK2PI;
04114 }
04115 j6valid[0] = true;
04116 for(int ij6 = 0; ij6 < 1; ++ij6)
04117 {
04118 if( !j6valid[ij6] )
04119 {
04120     continue;
04121 }
04122 _ij6[0] = ij6; _ij6[1] = -1;
04123 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
04124 {
04125 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
04126 {
04127     j6valid[iij6]=false; _ij6[1] = iij6; break; 
04128 }
04129 }
04130 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
04131 {
04132 IkReal evalcond[2];
04133 evalcond[0]=((((sj5)*(IKsin(j6))))+(new_r21));
04134 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(IKcos(j6))))+(new_r20));
04135 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
04136 {
04137 continue;
04138 }
04139 }
04140 
04141 {
04142 IkReal dummyeval[1];
04143 IkReal gconst17;
04144 gconst17=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))));
04145 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))));
04146 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04147 {
04148 {
04149 IkReal dummyeval[1];
04150 IkReal gconst16;
04151 gconst16=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))));
04152 dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))));
04153 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04154 {
04155 continue;
04156 
04157 } else
04158 {
04159 {
04160 IkReal j4array[1], cj4array[1], sj4array[1];
04161 bool j4valid[1]={false};
04162 _nj4 = 1;
04163 IkReal x208=((gconst16)*(sj5));
04164 if( IKabs(((new_r12)*(x208))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x208))) < IKFAST_ATAN2_MAGTHRESH )
04165     continue;
04166 j4array[0]=IKatan2(((new_r12)*(x208)), ((new_r02)*(x208)));
04167 sj4array[0]=IKsin(j4array[0]);
04168 cj4array[0]=IKcos(j4array[0]);
04169 if( j4array[0] > IKPI )
04170 {
04171     j4array[0]-=IK2PI;
04172 }
04173 else if( j4array[0] < -IKPI )
04174 {    j4array[0]+=IK2PI;
04175 }
04176 j4valid[0] = true;
04177 for(int ij4 = 0; ij4 < 1; ++ij4)
04178 {
04179 if( !j4valid[ij4] )
04180 {
04181     continue;
04182 }
04183 _ij4[0] = ij4; _ij4[1] = -1;
04184 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04185 {
04186 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04187 {
04188     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04189 }
04190 }
04191 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04192 {
04193 IkReal evalcond[12];
04194 IkReal x209=IKcos(j4);
04195 IkReal x210=IKsin(j4);
04196 IkReal x211=((IkReal(1.00000000000000))*(cj6));
04197 IkReal x212=((IkReal(1.00000000000000))*(sj5));
04198 IkReal x213=((IkReal(1.00000000000000))*(x209));
04199 IkReal x214=((cj5)*(x210));
04200 IkReal x215=((new_r11)*(x210));
04201 IkReal x216=((cj5)*(x209));
04202 IkReal x217=((new_r12)*(x210));
04203 IkReal x218=((new_r10)*(x210));
04204 evalcond[0]=((((new_r12)*(x209)))+(((IkReal(-1.00000000000000))*(new_r02)*(x210))));
04205 evalcond[1]=((sj5)+(((new_r02)*(x209)))+(x217));
04206 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x213)))+(((new_r00)*(x210))));
04207 evalcond[3]=((cj6)+(((IkReal(-1.00000000000000))*(new_r11)*(x213)))+(((new_r01)*(x210))));
04208 evalcond[4]=((((new_r01)*(x209)))+(((cj5)*(sj6)))+(x215));
04209 evalcond[5]=((((new_r00)*(x209)))+(x218)+(((IkReal(-1.00000000000000))*(cj5)*(x211))));
04210 evalcond[6]=((((new_r22)*(sj5)))+(((new_r02)*(x216)))+(((new_r12)*(x214))));
04211 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r00)*(x209)*(x212)))+(((cj5)*(new_r20)))+(((IkReal(-1.00000000000000))*(x212)*(x218))));
04212 evalcond[8]=((((cj5)*(new_r21)))+(((IkReal(-1.00000000000000))*(new_r01)*(x209)*(x212)))+(((IkReal(-1.00000000000000))*(x212)*(x215))));
04213 evalcond[9]=((sj6)+(((new_r01)*(x216)))+(((new_r21)*(sj5)))+(((new_r11)*(x214))));
04214 evalcond[10]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r02)*(x209)*(x212)))+(((cj5)*(new_r22)))+(((IkReal(-1.00000000000000))*(x212)*(x217))));
04215 evalcond[11]=((((new_r20)*(sj5)))+(((new_r00)*(x216)))+(((IkReal(-1.00000000000000))*(x211)))+(((new_r10)*(x214))));
04216 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  || IKabs(evalcond[8]) > 0.000001  || IKabs(evalcond[9]) > 0.000001  || IKabs(evalcond[10]) > 0.000001  || IKabs(evalcond[11]) > 0.000001  )
04217 {
04218 continue;
04219 }
04220 }
04221 
04222 {
04223 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04224 vinfos[0].jointtype = 1;
04225 vinfos[0].foffset = j0;
04226 vinfos[0].indices[0] = _ij0[0];
04227 vinfos[0].indices[1] = _ij0[1];
04228 vinfos[0].maxsolutions = _nj0;
04229 vinfos[1].jointtype = 1;
04230 vinfos[1].foffset = j1;
04231 vinfos[1].indices[0] = _ij1[0];
04232 vinfos[1].indices[1] = _ij1[1];
04233 vinfos[1].maxsolutions = _nj1;
04234 vinfos[2].jointtype = 1;
04235 vinfos[2].foffset = j2;
04236 vinfos[2].indices[0] = _ij2[0];
04237 vinfos[2].indices[1] = _ij2[1];
04238 vinfos[2].maxsolutions = _nj2;
04239 vinfos[3].jointtype = 1;
04240 vinfos[3].foffset = j3;
04241 vinfos[3].indices[0] = _ij3[0];
04242 vinfos[3].indices[1] = _ij3[1];
04243 vinfos[3].maxsolutions = _nj3;
04244 vinfos[4].jointtype = 1;
04245 vinfos[4].foffset = j4;
04246 vinfos[4].indices[0] = _ij4[0];
04247 vinfos[4].indices[1] = _ij4[1];
04248 vinfos[4].maxsolutions = _nj4;
04249 vinfos[5].jointtype = 1;
04250 vinfos[5].foffset = j5;
04251 vinfos[5].indices[0] = _ij5[0];
04252 vinfos[5].indices[1] = _ij5[1];
04253 vinfos[5].maxsolutions = _nj5;
04254 vinfos[6].jointtype = 1;
04255 vinfos[6].foffset = j6;
04256 vinfos[6].indices[0] = _ij6[0];
04257 vinfos[6].indices[1] = _ij6[1];
04258 vinfos[6].maxsolutions = _nj6;
04259 std::vector<int> vfree(0);
04260 solutions.AddSolution(vinfos,vfree);
04261 }
04262 }
04263 }
04264 
04265 }
04266 
04267 }
04268 
04269 } else
04270 {
04271 {
04272 IkReal j4array[1], cj4array[1], sj4array[1];
04273 bool j4valid[1]={false};
04274 _nj4 = 1;
04275 IkReal x219=((gconst17)*(sj6));
04276 if( IKabs(((new_r12)*(x219))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x219))) < IKFAST_ATAN2_MAGTHRESH )
04277     continue;
04278 j4array[0]=IKatan2(((new_r12)*(x219)), ((new_r02)*(x219)));
04279 sj4array[0]=IKsin(j4array[0]);
04280 cj4array[0]=IKcos(j4array[0]);
04281 if( j4array[0] > IKPI )
04282 {
04283     j4array[0]-=IK2PI;
04284 }
04285 else if( j4array[0] < -IKPI )
04286 {    j4array[0]+=IK2PI;
04287 }
04288 j4valid[0] = true;
04289 for(int ij4 = 0; ij4 < 1; ++ij4)
04290 {
04291 if( !j4valid[ij4] )
04292 {
04293     continue;
04294 }
04295 _ij4[0] = ij4; _ij4[1] = -1;
04296 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04297 {
04298 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04299 {
04300     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04301 }
04302 }
04303 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04304 {
04305 IkReal evalcond[12];
04306 IkReal x220=IKcos(j4);
04307 IkReal x221=IKsin(j4);
04308 IkReal x222=((IkReal(1.00000000000000))*(cj6));
04309 IkReal x223=((IkReal(1.00000000000000))*(sj5));
04310 IkReal x224=((IkReal(1.00000000000000))*(x220));
04311 IkReal x225=((cj5)*(x221));
04312 IkReal x226=((new_r11)*(x221));
04313 IkReal x227=((cj5)*(x220));
04314 IkReal x228=((new_r12)*(x221));
04315 IkReal x229=((new_r10)*(x221));
04316 evalcond[0]=((((new_r12)*(x220)))+(((IkReal(-1.00000000000000))*(new_r02)*(x221))));
04317 evalcond[1]=((sj5)+(((new_r02)*(x220)))+(x228));
04318 evalcond[2]=((((new_r00)*(x221)))+(sj6)+(((IkReal(-1.00000000000000))*(new_r10)*(x224))));
04319 evalcond[3]=((((new_r01)*(x221)))+(cj6)+(((IkReal(-1.00000000000000))*(new_r11)*(x224))));
04320 evalcond[4]=((((new_r01)*(x220)))+(((cj5)*(sj6)))+(x226));
04321 evalcond[5]=((((new_r00)*(x220)))+(((IkReal(-1.00000000000000))*(cj5)*(x222)))+(x229));
04322 evalcond[6]=((((new_r02)*(x227)))+(((new_r22)*(sj5)))+(((new_r12)*(x225))));
04323 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r00)*(x220)*(x223)))+(((cj5)*(new_r20)))+(((IkReal(-1.00000000000000))*(x223)*(x229))));
04324 evalcond[8]=((((IkReal(-1.00000000000000))*(new_r01)*(x220)*(x223)))+(((cj5)*(new_r21)))+(((IkReal(-1.00000000000000))*(x223)*(x226))));
04325 evalcond[9]=((sj6)+(((new_r01)*(x227)))+(((new_r11)*(x225)))+(((new_r21)*(sj5))));
04326 evalcond[10]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r02)*(x220)*(x223)))+(((cj5)*(new_r22)))+(((IkReal(-1.00000000000000))*(x223)*(x228))));
04327 evalcond[11]=((((new_r00)*(x227)))+(((new_r20)*(sj5)))+(((new_r10)*(x225)))+(((IkReal(-1.00000000000000))*(x222))));
04328 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  || IKabs(evalcond[8]) > 0.000001  || IKabs(evalcond[9]) > 0.000001  || IKabs(evalcond[10]) > 0.000001  || IKabs(evalcond[11]) > 0.000001  )
04329 {
04330 continue;
04331 }
04332 }
04333 
04334 {
04335 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
04336 vinfos[0].jointtype = 1;
04337 vinfos[0].foffset = j0;
04338 vinfos[0].indices[0] = _ij0[0];
04339 vinfos[0].indices[1] = _ij0[1];
04340 vinfos[0].maxsolutions = _nj0;
04341 vinfos[1].jointtype = 1;
04342 vinfos[1].foffset = j1;
04343 vinfos[1].indices[0] = _ij1[0];
04344 vinfos[1].indices[1] = _ij1[1];
04345 vinfos[1].maxsolutions = _nj1;
04346 vinfos[2].jointtype = 1;
04347 vinfos[2].foffset = j2;
04348 vinfos[2].indices[0] = _ij2[0];
04349 vinfos[2].indices[1] = _ij2[1];
04350 vinfos[2].maxsolutions = _nj2;
04351 vinfos[3].jointtype = 1;
04352 vinfos[3].foffset = j3;
04353 vinfos[3].indices[0] = _ij3[0];
04354 vinfos[3].indices[1] = _ij3[1];
04355 vinfos[3].maxsolutions = _nj3;
04356 vinfos[4].jointtype = 1;
04357 vinfos[4].foffset = j4;
04358 vinfos[4].indices[0] = _ij4[0];
04359 vinfos[4].indices[1] = _ij4[1];
04360 vinfos[4].maxsolutions = _nj4;
04361 vinfos[5].jointtype = 1;
04362 vinfos[5].foffset = j5;
04363 vinfos[5].indices[0] = _ij5[0];
04364 vinfos[5].indices[1] = _ij5[1];
04365 vinfos[5].maxsolutions = _nj5;
04366 vinfos[6].jointtype = 1;
04367 vinfos[6].foffset = j6;
04368 vinfos[6].indices[0] = _ij6[0];
04369 vinfos[6].indices[1] = _ij6[1];
04370 vinfos[6].maxsolutions = _nj6;
04371 std::vector<int> vfree(0);
04372 solutions.AddSolution(vinfos,vfree);
04373 }
04374 }
04375 }
04376 
04377 }
04378 
04379 }
04380 }
04381 }
04382 
04383 }
04384 
04385 }
04386 }
04387 }
04388 }
04389 }};
04390 
04391 
04394 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
04395 IKSolver solver;
04396 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
04397 }
04398 
04399 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - cob3_2 (f8ac16d287e31152f9a0008d98e80d9e)>"; }
04400 
04401 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
04402 
04403 #ifdef IKFAST_NAMESPACE
04404 } // end namespace
04405 #endif
04406 
04407 #ifndef IKFAST_NO_MAIN
04408 #include <stdio.h>
04409 #include <stdlib.h>
04410 #ifdef IKFAST_NAMESPACE
04411 using namespace IKFAST_NAMESPACE;
04412 #endif
04413 int main(int argc, char** argv)
04414 {
04415     if( argc != 12+GetNumFreeParameters()+1 ) {
04416         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
04417                "Returns the ik solutions given the transformation of the end effector specified by\n"
04418                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
04419                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
04420         return 1;
04421     }
04422 
04423     IkSolutionList<IkReal> solutions;
04424     std::vector<IkReal> vfree(GetNumFreeParameters());
04425     IkReal eerot[9],eetrans[3];
04426     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
04427     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
04428     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
04429     for(std::size_t i = 0; i < vfree.size(); ++i)
04430         vfree[i] = atof(argv[13+i]);
04431     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
04432 
04433     if( !bSuccess ) {
04434         fprintf(stderr,"Failed to get ik solution\n");
04435         return -1;
04436     }
04437 
04438     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
04439     std::vector<IkReal> solvalues(GetNumJoints());
04440     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
04441         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
04442         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
04443         std::vector<IkReal> vsolfree(sol.GetFree().size());
04444         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
04445         for( std::size_t j = 0; j < solvalues.size(); ++j)
04446             printf("%.15f, ", solvalues[j]);
04447         printf("\n");
04448     }
04449     return 0;
04450 }
04451 
04452 #endif


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