ik_pr2_rightarm.cpp
Go to the documentation of this file.
00001 //#include "plugindefs.h"
00002 
00022 #include <cmath>
00023 #include <vector>
00024 #include <limits>
00025 #include <algorithm>
00026 #include <complex>
00027 
00028 #include <coverage_3d_arm_navigation/openrave_ik.h>
00029 #ifdef IKFAST_HEADER
00030 #include IKFAST_HEADER
00031 #endif
00032 
00033 #ifndef IKFAST_ASSERT
00034 #include <stdexcept>
00035 #include <sstream>
00036 #include <iostream>
00037 
00038 #ifdef _MSC_VER
00039 #ifndef __PRETTY_FUNCTION__
00040 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00041 #endif
00042 #endif
00043 
00044 #ifndef __PRETTY_FUNCTION__
00045 #define __PRETTY_FUNCTION__ __func__
00046 #endif
00047 
00048 #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()); } }
00049 
00050 #endif
00051 
00052 #if defined(_MSC_VER)
00053 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00054 #else
00055 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00056 #endif
00057 
00058 #define IK2PI  ((IKReal)6.28318530717959)
00059 #define IKPI  ((IKReal)3.14159265358979)
00060 #define IKPI_2  ((IKReal)1.57079632679490)
00061 
00062 #ifdef _MSC_VER
00063 #ifndef isnan
00064 #define isnan _isnan
00065 #endif
00066 #endif // _MSC_VER
00067 
00068 // defined when creating a shared object/dll
00069 #ifdef IKFAST_CLIBRARY
00070 #ifdef _MSC_VER
00071 #define IKFAST_API extern "C" __declspec(dllexport)
00072 #else
00073 #define IKFAST_API extern "C"
00074 #endif
00075 #else
00076 #define IKFAST_API
00077 #endif
00078 
00079 // lapack routines
00080 extern "C" {
00081   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00082   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00083   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00084   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00085   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);
00086   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);
00087 }
00088 
00089 using namespace std; // necessary to get std math routines
00090 
00091 #ifdef IKFAST_NAMESPACE
00092 namespace IKFAST_NAMESPACE {
00093 #endif
00094 
00095 #ifdef IKFAST_REAL
00096 typedef IKFAST_REAL IKReal;
00097 #else
00098 typedef double IKReal;
00099 #endif
00100 
00101 
00104     void IKSolution::GetSolution(IKReal* psolution, const IKReal* pfree) const {
00105         for(std::size_t i = 0; i < basesol.size(); ++i) {
00106             if( basesol[i].freeind < 0 )
00107                 psolution[i] = basesol[i].foffset;
00108             else {
00109                 IKFAST_ASSERT(pfree != NULL);
00110                 psolution[i] = pfree[basesol[i].freeind]*basesol[i].fmul + basesol[i].foffset;
00111                 if( psolution[i] > IKPI ) {
00112                     psolution[i] -= IK2PI;
00113                 }
00114                 else if( psolution[i] < -IKPI ) {
00115                     psolution[i] += IK2PI;
00116                 }
00117             }
00118         }
00119     }
00120 
00123     const std::vector<int>& IKSolution::GetFree() const { return vfree; }
00124 
00125 
00126     bool IKSolution::Validate() const {
00127         for(size_t i = 0; i < basesol.size(); ++i) {
00128             if( basesol[i].maxsolutions == (unsigned char)-1) {
00129                 return false;
00130             }
00131             if( basesol[i].maxsolutions > 0 ) {
00132                 if( basesol[i].indices[0] >= basesol[i].maxsolutions ) {
00133                     return false;
00134                 }
00135                 if( basesol[i].indices[1] != (unsigned char)-1 && basesol[i].indices[1] >= basesol[i].maxsolutions ) {
00136                     return false;
00137                 }
00138             }
00139         }
00140         return true;
00141     }
00142 
00143     void IKSolution::GetSolutionIndices(std::vector<unsigned int>& v) const {
00144         v.resize(0);
00145         v.push_back(0);
00146         for(int i = (int)basesol.size()-1; i >= 0; --i) {
00147             if( basesol[i].maxsolutions != (unsigned char)-1 && basesol[i].maxsolutions > 1 ) {
00148                 for(size_t j = 0; j < v.size(); ++j) {
00149                     v[j] *= basesol[i].maxsolutions;
00150                 }
00151                 size_t orgsize=v.size();
00152                 if( basesol[i].indices[1] != (unsigned char)-1 ) {
00153                     for(size_t j = 0; j < orgsize; ++j) {
00154                         v.push_back(v[j]+basesol[i].indices[1]);
00155                     }
00156                 }
00157                 if( basesol[i].indices[0] != (unsigned char)-1 ) {
00158                     for(size_t j = 0; j < orgsize; ++j) {
00159                         v[j] += basesol[i].indices[0];
00160                     }
00161                 }
00162             }
00163         }
00164     }
00165 
00166 
00167 inline float IKabs(float f) { return fabsf(f); }
00168 inline double IKabs(double f) { return fabs(f); }
00169 
00170 inline float IKsqr(float f) { return f*f; }
00171 inline double IKsqr(double f) { return f*f; }
00172 
00173 inline float IKlog(float f) { return logf(f); }
00174 inline double IKlog(double f) { return log(f); }
00175 
00176 // allows asin and acos to exceed 1
00177 #ifndef IKFAST_SINCOS_THRESH
00178 #define IKFAST_SINCOS_THRESH ((IKReal)0.000001)
00179 #endif
00180 
00181 // used to check input to atan2 for degenerate cases
00182 #ifndef IKFAST_ATAN2_MAGTHRESH
00183 #define IKFAST_ATAN2_MAGTHRESH ((IKReal)2e-6)
00184 #endif
00185 
00186 // minimum distance of separate solutions
00187 #ifndef IKFAST_SOLUTION_THRESH
00188 #define IKFAST_SOLUTION_THRESH ((IKReal)1e-6)
00189 #endif
00190 
00191 inline float IKasin(float f)
00192 {
00193 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00194 if( f <= -1 ) return -IKPI_2;
00195 else if( f >= 1 ) return IKPI_2;
00196 return asinf(f);
00197 }
00198 inline double IKasin(double f)
00199 {
00200 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00201 if( f <= -1 ) return -IKPI_2;
00202 else if( f >= 1 ) return IKPI_2;
00203 return asin(f);
00204 }
00205 
00206 // return positive value in [0,y)
00207 inline float IKfmod(float x, float y)
00208 {
00209     while(x < 0) {
00210         x += y;
00211     }
00212     return fmodf(x,y);
00213 }
00214 
00215 // return positive value in [0,y)
00216 inline float IKfmod(double x, double y)
00217 {
00218     while(x < 0) {
00219         x += y;
00220     }
00221     return fmod(x,y);
00222 }
00223 
00224 inline float IKacos(float f)
00225 {
00226 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00227 if( f <= -1 ) return IKPI;
00228 else if( f >= 1 ) return 0;
00229 return acosf(f);
00230 }
00231 inline double IKacos(double f)
00232 {
00233 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00234 if( f <= -1 ) return IKPI;
00235 else if( f >= 1 ) return 0;
00236 return acos(f);
00237 }
00238 inline float IKsin(float f) { return sinf(f); }
00239 inline double IKsin(double f) { return sin(f); }
00240 inline float IKcos(float f) { return cosf(f); }
00241 inline double IKcos(double f) { return cos(f); }
00242 inline float IKtan(float f) { return tanf(f); }
00243 inline double IKtan(double f) { return tan(f); }
00244 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00245 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00246 inline float IKatan2(float fy, float fx) {
00247     if( isnan(fy) ) {
00248         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00249         return IKPI_2;
00250     }
00251     else if( isnan(fx) ) {
00252         return 0;
00253     }
00254     return atan2f(fy,fx);
00255 }
00256 inline double IKatan2(double fy, double fx) {
00257     if( isnan(fy) ) {
00258         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00259         return IKPI_2;
00260     }
00261     else if( isnan(fx) ) {
00262         return 0;
00263     }
00264     return atan2(fy,fx);
00265 }
00266 
00267 inline float IKsign(float f) {
00268     if( f > 0 ) {
00269         return 1.0f;
00270     }
00271     else if( f < 0 ) {
00272         return -1.0f;
00273     }
00274     return 0;
00275 }
00276 
00277 inline double IKsign(double f) {
00278     if( f > 0 ) {
00279         return 1.0;
00280     }
00281     else if( f < 0 ) {
00282         return -1.0;
00283     }
00284     return 0;
00285 }
00286 
00289 IKFAST_API void fk(const IKReal* j, IKReal* eetrans, IKReal* eerot) {
00290 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,x67,x68;
00291 x0=IKcos(j[0]);
00292 x1=IKsin(j[1]);
00293 x2=IKsin(j[2]);
00294 x3=IKcos(j[2]);
00295 x4=IKsin(j[0]);
00296 x5=IKsin(j[4]);
00297 x6=((x0)*(x1)*(x2));
00298 x7=((x3)*(x4));
00299 x8=((x6)+(((-1.00000000000000)*(x7))));
00300 x9=IKcos(j[4]);
00301 x10=IKcos(j[3]);
00302 x11=((x2)*(x4));
00303 x12=((x0)*(x1)*(x3));
00304 x13=((x11)+(x12));
00305 x14=((-1.00000000000000)*(x13));
00306 x15=IKcos(j[1]);
00307 x16=IKsin(j[3]);
00308 x17=((x0)*(x15)*(x16));
00309 x18=IKsin(j[6]);
00310 x19=IKcos(j[5]);
00311 x20=((x5)*(x8));
00312 x21=((x10)*(x14));
00313 x22=IKsin(j[5]);
00314 x23=((-1.00000000000000)*(x14)*(x16));
00315 x24=((-1.00000000000000)*(x0)*(x10)*(x15));
00316 x25=((x24)+(x23));
00317 x26=IKcos(j[6]);
00318 x27=((x8)*(x9));
00319 x28=((-1.00000000000000)*(x17));
00320 x29=((x21)+(x28));
00321 x30=((x29)*(x9));
00322 x31=((x30)+(x20));
00323 x32=((((-1.00000000000000)*(x17)))+(x21));
00324 x33=((x0)*(x2));
00325 x34=((x1)*(x7));
00326 x35=((((-1.00000000000000)*(x34)))+(x33));
00327 x36=((x0)*(x3));
00328 x37=((x1)*(x11));
00329 x38=((x37)+(x36));
00330 x39=((x10)*(x35));
00331 x40=((x38)*(x9));
00332 x41=((x15)*(x16)*(x4));
00333 x42=((((-1.00000000000000)*(x39)))+(x41));
00334 x43=((x42)*(x5));
00335 x44=((x43)+(x40));
00336 x45=((-1.00000000000000)*(x41));
00337 x46=((x39)+(x45));
00338 x47=((x46)*(x9));
00339 x48=((x38)*(x5));
00340 x49=((x48)+(x47));
00341 x50=((-1.00000000000000)*(x10)*(x15)*(x4));
00342 x51=((-1.00000000000000)*(x16)*(x35));
00343 x52=((x51)+(x50));
00344 x53=((x1)*(x16));
00345 x54=((x10)*(x15)*(x3));
00346 x55=((x15)*(x2)*(x9));
00347 x56=((x54)+(((-1.00000000000000)*(x53))));
00348 x57=((x5)*(x56));
00349 x58=((x55)+(x57));
00350 x59=((x15)*(x16)*(x3));
00351 x60=((x1)*(x10));
00352 x61=((x59)+(x60));
00353 x62=((x22)*(x61));
00354 x63=((x53)+(((-1.00000000000000)*(x54))));
00355 x64=((x63)*(x9));
00356 x65=((x15)*(x2)*(x5));
00357 x66=((x64)+(x65));
00358 x67=((x19)*(x66));
00359 x68=((x62)+(x67));
00360 eerot[0]=((((x26)*(((((x19)*(((((x32)*(x9)))+(x20)))))+(((x22)*(x25)))))))+(((x18)*(((x27)+(((x5)*(((((-1.00000000000000)*(x21)))+(x17))))))))));
00361 eerot[1]=((((x18)*(((((-1.00000000000000)*(x22)*(x25)))+(((-1.00000000000000)*(x19)*(x31)))))))+(((x26)*(((x27)+(((x5)*(((((-1.00000000000000)*(x21)))+(x17))))))))));
00362 eerot[2]=((((x22)*(x31)))+(((x19)*(((((x0)*(x10)*(x15)))+(((x14)*(x16))))))));
00363 eetrans[0]=((((x16)*(((((-0.321000000000000)*(x12)))+(((-0.321000000000000)*(x11)))))))+(((0.321000000000000)*(x0)*(x10)*(x15)))+(((0.400000000000000)*(x0)*(x15)))+(((x22)*(((((0.180000000000000)*(x20)))+(((0.180000000000000)*(x32)*(x9)))))))+(((x19)*(((((0.180000000000000)*(x0)*(x10)*(x15)))+(((0.180000000000000)*(x14)*(x16)))))))+(((0.100000000000000)*(x0))));
00364 eerot[3]=((((x18)*(x44)))+(((x26)*(((((x19)*(x49)))+(((x22)*(x52))))))));
00365 eerot[4]=((((x26)*(x44)))+(((x18)*(((((-1.00000000000000)*(x22)*(x52)))+(((-1.00000000000000)*(x19)*(x49))))))));
00366 eerot[5]=((((x22)*(x49)))+(((x19)*(((((x16)*(x35)))+(((x10)*(x15)*(x4))))))));
00367 eetrans[1]=((-0.188000000000000)+(((x16)*(((((-0.321000000000000)*(x34)))+(((0.321000000000000)*(x33)))))))+(((x19)*(((((0.180000000000000)*(x10)*(x15)*(x4)))+(((0.180000000000000)*(x16)*(x35)))))))+(((0.400000000000000)*(x15)*(x4)))+(((0.100000000000000)*(x4)))+(((x22)*(((((0.180000000000000)*(x47)))+(((0.180000000000000)*(x48)))))))+(((0.321000000000000)*(x10)*(x15)*(x4))));
00368 eerot[6]=((((x26)*(x68)))+(((x18)*(x58))));
00369 eerot[7]=((((x26)*(x58)))+(((-1.00000000000000)*(x18)*(x68))));
00370 eerot[8]=((((-1.00000000000000)*(x19)*(x61)))+(((x22)*(x66))));
00371 eetrans[2]=((((x22)*(((((0.180000000000000)*(x65)))+(((0.180000000000000)*(x64)))))))+(((x19)*(((((-0.180000000000000)*(x60)))+(((-0.180000000000000)*(x59)))))))+(((-0.321000000000000)*(x59)))+(((-0.321000000000000)*(x60)))+(((-0.400000000000000)*(x1))));
00372 }
00373 
00374 IKFAST_API int getNumFreeParameters() { return 1; }
00375 IKFAST_API int* getFreeParameters() { static int freeparams[] = {2}; return freeparams; }
00376 IKFAST_API int getNumJoints() { return 7; }
00377 
00378 IKFAST_API int getIKRealSize() { return sizeof(IKReal); }
00379 
00380 IKFAST_API int getIKType() { return 0x67000001; }
00381 
00382 class IKSolver {
00383 public:
00384 IKReal j27,cj27,sj27,htj27,j28,cj28,sj28,htj28,j30,cj30,sj30,htj30,j31,cj31,sj31,htj31,j32,cj32,sj32,htj32,j33,cj33,sj33,htj33,j29,cj29,sj29,htj29,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;
00385 unsigned char _ij27[2], _nj27,_ij28[2], _nj28,_ij30[2], _nj30,_ij31[2], _nj31,_ij32[2], _nj32,_ij33[2], _nj33,_ij29[2], _nj29;
00386 
00387 bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
00388 j27=numeric_limits<IKReal>::quiet_NaN(); _ij27[0] = -1; _ij27[1] = -1; _nj27 = -1; j28=numeric_limits<IKReal>::quiet_NaN(); _ij28[0] = -1; _ij28[1] = -1; _nj28 = -1; j30=numeric_limits<IKReal>::quiet_NaN(); _ij30[0] = -1; _ij30[1] = -1; _nj30 = -1; j31=numeric_limits<IKReal>::quiet_NaN(); _ij31[0] = -1; _ij31[1] = -1; _nj31 = -1; j32=numeric_limits<IKReal>::quiet_NaN(); _ij32[0] = -1; _ij32[1] = -1; _nj32 = -1; j33=numeric_limits<IKReal>::quiet_NaN(); _ij33[0] = -1; _ij33[1] = -1; _nj33 = -1;  _ij29[0] = -1; _ij29[1] = -1; _nj29 = 0; 
00389 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00390     vsolutions.resize(0); vsolutions.reserve(8);
00391 j29=pfree[0]; cj29=cos(pfree[0]); sj29=sin(pfree[0]);
00392 r00 = eerot[0*3+0];
00393 r01 = eerot[0*3+1];
00394 r02 = eerot[0*3+2];
00395 r10 = eerot[1*3+0];
00396 r11 = eerot[1*3+1];
00397 r12 = eerot[1*3+2];
00398 r20 = eerot[2*3+0];
00399 r21 = eerot[2*3+1];
00400 r22 = eerot[2*3+2];
00401 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00402 
00403 new_r00=r00;
00404 new_r01=r01;
00405 new_r02=r02;
00406 new_px=((((-0.180000000000000)*(r02)))+(px));
00407 new_r10=r10;
00408 new_r11=r11;
00409 new_r12=r12;
00410 new_py=((0.188000000000000)+(py)+(((-0.180000000000000)*(r12))));
00411 new_r20=r20;
00412 new_r21=r21;
00413 new_r22=r22;
00414 new_pz=((((-0.180000000000000)*(r22)))+(pz));
00415 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;
00416 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00417 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00418 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00419 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00420 rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
00421 rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
00422 rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
00423 rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
00424 rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
00425 rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
00426 rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
00427 rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
00428 rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
00429 {
00430 IKReal dummyeval[1];
00431 IKReal gconst0;
00432 gconst0=((-1.00000000000000)*(py));
00433 IKReal gconst1;
00434 gconst1=((0.642000000000000)*(sj29));
00435 IKReal gconst2;
00436 gconst2=((-1.00000000000000)*(py));
00437 IKReal gconst3;
00438 gconst3=((0.509841000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00439 IKReal gconst4;
00440 gconst4=((-0.00375900000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00441 IKReal gconst5;
00442 gconst5=((-1.00000000000000)*(py));
00443 IKReal gconst6;
00444 gconst6=((0.642000000000000)*(sj29));
00445 IKReal gconst7;
00446 gconst7=((-1.00000000000000)*(py));
00447 IKReal gconst8;
00448 gconst8=((0.509841000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00449 IKReal gconst9;
00450 gconst9=((-0.00375900000000000)+(((0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00451 IKReal gconst10;
00452 gconst10=((2.00000000000000)*(px));
00453 IKReal gconst11;
00454 gconst11=((2.00000000000000)*(px));
00455 IKReal gconst12;
00456 gconst12=((0.400000000000000)*(py));
00457 IKReal gconst13;
00458 gconst13=((0.400000000000000)*(py));
00459 IKReal gconst14;
00460 gconst14=((2.00000000000000)*(px));
00461 IKReal gconst15;
00462 gconst15=((2.00000000000000)*(px));
00463 IKReal gconst16;
00464 gconst16=((0.400000000000000)*(py));
00465 IKReal gconst17;
00466 gconst17=((0.400000000000000)*(py));
00467 IKReal gconst18;
00468 gconst18=py;
00469 IKReal gconst19;
00470 gconst19=((0.642000000000000)*(sj29));
00471 IKReal gconst20;
00472 gconst20=py;
00473 IKReal gconst21;
00474 gconst21=((0.509841000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00475 IKReal gconst22;
00476 gconst22=((-0.00375900000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00477 IKReal gconst23;
00478 gconst23=py;
00479 IKReal gconst24;
00480 gconst24=((0.642000000000000)*(sj29));
00481 IKReal gconst25;
00482 gconst25=py;
00483 IKReal gconst26;
00484 gconst26=((0.509841000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00485 IKReal gconst27;
00486 gconst27=((-0.00375900000000000)+(((-0.200000000000000)*(px)))+(((-1.00000000000000)*(pp))));
00487 dummyeval[0]=((((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst26)))+(((gconst20)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst27)))+(((gconst18)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst26))));
00488 if( IKabs(dummyeval[0]) < 0.0000001000000000  )
00489 {
00490 continue;
00491 
00492 } else
00493 {
00494 IKReal op[8+1], zeror[8];
00495 int numroots;
00496 op[0]=((((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst26)))+(((gconst20)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst27)))+(((gconst18)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst26))));
00497 op[1]=((((-1.00000000000000)*(gconst13)*(gconst18)*(gconst23)*(gconst27)))+(((gconst10)*(gconst22)*(gconst25)*(gconst26)))+(((gconst12)*(gconst20)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst26)))+(((gconst16)*(gconst18)*(gconst22)*(gconst25)))+(((gconst15)*(gconst18)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst25)*(gconst26)))+(((gconst11)*(gconst21)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst22)*(gconst24)))+(((gconst13)*(gconst18)*(gconst25)*(gconst26)))+(((gconst14)*(gconst20)*(gconst21)*(gconst27)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst24)*(gconst26)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst25)*(gconst26)))+(((gconst17)*(gconst20)*(gconst21)*(gconst23))));
00498 op[2]=((((gconst2)*(gconst21)*(gconst23)*(gconst27)))+(((gconst20)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst24)*(gconst8)))+(((gconst20)*(gconst21)*(gconst23)*(gconst9)))+(((gconst20)*(gconst23)*(gconst27)*(gconst3)))+(((gconst11)*(gconst17)*(gconst21)*(gconst23)))+(((-1.00000000000000)*(gconst14)*(gconst17)*(gconst18)*(gconst22)))+(((gconst13)*(gconst16)*(gconst18)*(gconst25)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst23)*(gconst27)))+(((gconst11)*(gconst12)*(gconst23)*(gconst27)))+(((gconst10)*(gconst15)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst23)*(gconst9)))+(((gconst14)*(gconst17)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst19)*(gconst24)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst18)*(gconst27)))+(((gconst18)*(gconst22)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst20)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst23)*(gconst27)))+(((gconst12)*(gconst17)*(gconst20)*(gconst23)))+(((gconst18)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst20)*(gconst25)))+(((-1.00000000000000)*(gconst13)*(gconst16)*(gconst19)*(gconst24)))+(((gconst0)*(gconst22)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst27)*(gconst5)))+(((gconst10)*(gconst13)*(gconst25)*(gconst26)))+(((gconst11)*(gconst14)*(gconst21)*(gconst27)))+(((gconst12)*(gconst14)*(gconst20)*(gconst27)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst20)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst21)*(gconst26)))+(((gconst10)*(gconst16)*(gconst22)*(gconst25)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst25)*(gconst8)))+(((gconst15)*(gconst16)*(gconst18)*(gconst22)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst22)*(gconst27)))+(((gconst13)*(gconst15)*(gconst18)*(gconst26)))+(((gconst18)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst24)*(gconst26))));
00499 op[3]=((((-1.00000000000000)*(gconst11)*(gconst12)*(gconst16)*(gconst25)))+(((gconst16)*(gconst18)*(gconst25)*(gconst4)))+(((gconst15)*(gconst18)*(gconst22)*(gconst8)))+(((gconst11)*(gconst12)*(gconst14)*(gconst27)))+(((gconst0)*(gconst16)*(gconst22)*(gconst25)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst22)*(gconst27)))+(((gconst0)*(gconst15)*(gconst22)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst21)*(gconst7)))+(((gconst14)*(gconst2)*(gconst21)*(gconst27)))+(((gconst10)*(gconst13)*(gconst15)*(gconst26)))+(((-1.00000000000000)*(gconst11)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst16)*(gconst20)))+(((gconst11)*(gconst23)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst22)*(gconst23)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst16)*(gconst21)))+(((gconst11)*(gconst14)*(gconst17)*(gconst21)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst25)*(gconst26)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst24)*(gconst4)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst21)*(gconst25)))+(((gconst12)*(gconst14)*(gconst17)*(gconst20)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst17)*(gconst18)))+(((gconst11)*(gconst21)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst17)*(gconst22)))+(((gconst14)*(gconst20)*(gconst21)*(gconst9)))+(((gconst16)*(gconst18)*(gconst22)*(gconst7)))+(((gconst12)*(gconst20)*(gconst23)*(gconst9)))+(((gconst13)*(gconst18)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst25)*(gconst8)))+(((gconst10)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst10)*(gconst23)*(gconst27)*(gconst4)))+(((gconst14)*(gconst20)*(gconst27)*(gconst3)))+(((gconst15)*(gconst18)*(gconst26)*(gconst4)))+(((gconst13)*(gconst15)*(gconst16)*(gconst18)))+(((gconst10)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst22)*(gconst24)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst24)*(gconst26)))+(((gconst11)*(gconst12)*(gconst17)*(gconst23)))+(((gconst12)*(gconst2)*(gconst23)*(gconst27)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst26)))+(((gconst17)*(gconst20)*(gconst23)*(gconst3)))+(((gconst17)*(gconst20)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst8)))+(((gconst10)*(gconst15)*(gconst16)*(gconst22)))+(((gconst11)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst9)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst23)*(gconst4)))+(((gconst13)*(gconst18)*(gconst26)*(gconst7)))+(((gconst17)*(gconst2)*(gconst21)*(gconst23)))+(((gconst0)*(gconst13)*(gconst25)*(gconst26)))+(((gconst10)*(gconst22)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst26)*(gconst7)))+(((gconst12)*(gconst20)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst26)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst17)*(gconst23)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst24)*(gconst8)))+(((gconst10)*(gconst13)*(gconst16)*(gconst25)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst25)*(gconst3))));
00500 op[4]=((((gconst12)*(gconst14)*(gconst2)*(gconst27)))+(((gconst10)*(gconst15)*(gconst22)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst27)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst27)*(gconst5)))+(((gconst2)*(gconst23)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst2)*(gconst21)))+(((gconst18)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst20)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst2)*(gconst26)))+(((gconst14)*(gconst17)*(gconst2)*(gconst21)))+(((gconst20)*(gconst27)*(gconst3)*(gconst5)))+(((gconst11)*(gconst12)*(gconst14)*(gconst17)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst19)*(gconst24)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst16)*(gconst19)*(gconst6)))+(((gconst10)*(gconst16)*(gconst22)*(gconst7)))+(((gconst13)*(gconst15)*(gconst18)*(gconst8)))+(((gconst12)*(gconst17)*(gconst2)*(gconst23)))+(((gconst10)*(gconst13)*(gconst15)*(gconst16)))+(((gconst11)*(gconst14)*(gconst27)*(gconst3)))+(((gconst18)*(gconst25)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst24)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst20)*(gconst25)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst25)*(gconst3)))+(((gconst0)*(gconst13)*(gconst16)*(gconst25)))+(((gconst13)*(gconst16)*(gconst18)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst17)*(gconst23)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst16)*(gconst24)))+(((gconst11)*(gconst12)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst27)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst24)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst14)*(gconst18)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst27)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst5)*(gconst9)))+(((gconst10)*(gconst13)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst20)*(gconst3)))+(((-1.00000000000000)*(gconst19)*(gconst22)*(gconst6)*(gconst8)))+(((gconst10)*(gconst13)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst26)*(gconst3)))+(((gconst0)*(gconst13)*(gconst15)*(gconst26)))+(((gconst2)*(gconst21)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst2)*(gconst25)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst17)))+(((gconst2)*(gconst21)*(gconst23)*(gconst9)))+(((gconst12)*(gconst14)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst17)*(gconst18)*(gconst4)))+(((gconst18)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst19)*(gconst26)*(gconst4)*(gconst6)))+(((gconst20)*(gconst21)*(gconst5)*(gconst9)))+(((gconst11)*(gconst12)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst22)*(gconst9)))+(((gconst10)*(gconst15)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst2)*(gconst25)))+(((gconst11)*(gconst17)*(gconst23)*(gconst3)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst17)*(gconst22)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst16)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst21)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst25)*(gconst8)))+(((gconst0)*(gconst15)*(gconst16)*(gconst22)))+(((gconst0)*(gconst25)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst23)*(gconst4)))+(((gconst0)*(gconst22)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst26)*(gconst7)))+(((gconst11)*(gconst14)*(gconst21)*(gconst9)))+(((gconst14)*(gconst17)*(gconst20)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst27)*(gconst4)))+(((gconst12)*(gconst17)*(gconst20)*(gconst5)))+(((gconst11)*(gconst17)*(gconst21)*(gconst5)))+(((gconst15)*(gconst16)*(gconst18)*(gconst4)))+(((gconst0)*(gconst22)*(gconst26)*(gconst7)))+(((gconst20)*(gconst23)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst20)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst26)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst23)*(gconst9)))+(((gconst10)*(gconst16)*(gconst25)*(gconst4))));
00501 op[5]=((((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst17)))+(((-1.00000000000000)*(gconst10)*(gconst22)*(gconst5)*(gconst9)))+(((gconst0)*(gconst13)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst26)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst6)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst16)*(gconst2)))+(((gconst17)*(gconst20)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst16)*(gconst7)))+(((gconst10)*(gconst15)*(gconst16)*(gconst4)))+(((-1.00000000000000)*(gconst16)*(gconst20)*(gconst3)*(gconst7)))+(((gconst12)*(gconst20)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst4)*(gconst9)))+(((gconst11)*(gconst14)*(gconst17)*(gconst3)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst25)*(gconst3)))+(((gconst0)*(gconst15)*(gconst22)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst19)*(gconst4)*(gconst6)))+(((gconst10)*(gconst25)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst20)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst27)*(gconst4)))+(((gconst17)*(gconst2)*(gconst23)*(gconst3)))+(((gconst0)*(gconst16)*(gconst25)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst24)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst27)*(gconst4)*(gconst5)))+(((gconst10)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst25)*(gconst3)*(gconst8)))+(((gconst14)*(gconst2)*(gconst21)*(gconst9)))+(((gconst17)*(gconst2)*(gconst21)*(gconst5)))+(((gconst12)*(gconst14)*(gconst17)*(gconst2)))+(((gconst11)*(gconst23)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst22)*(gconst9)))+(((gconst10)*(gconst13)*(gconst16)*(gconst7)))+(((gconst11)*(gconst12)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst26)*(gconst6)))+(((gconst12)*(gconst2)*(gconst27)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst3)*(gconst8)))+(((gconst11)*(gconst21)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst26)*(gconst7)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst26)*(gconst3)))+(((gconst0)*(gconst13)*(gconst15)*(gconst16)))+(((gconst14)*(gconst20)*(gconst3)*(gconst9)))+(((gconst16)*(gconst18)*(gconst4)*(gconst7)))+(((gconst12)*(gconst2)*(gconst23)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst21)*(gconst7)*(gconst8)))+(((gconst10)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst17)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst18)*(gconst5)*(gconst9)))+(((gconst11)*(gconst12)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst23)*(gconst4)*(gconst9)))+(((gconst13)*(gconst18)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst23)*(gconst9)))+(((gconst10)*(gconst13)*(gconst15)*(gconst8)))+(((gconst14)*(gconst2)*(gconst27)*(gconst3)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst16)*(gconst3)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst24)*(gconst4)))+(((gconst0)*(gconst15)*(gconst26)*(gconst4)))+(((gconst15)*(gconst18)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst27)*(gconst5)))+(((gconst0)*(gconst13)*(gconst25)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst15)*(gconst8)))+(((gconst11)*(gconst27)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst8)))+(((gconst0)*(gconst16)*(gconst22)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst22)*(gconst6))));
00502 op[6]=((((-1.00000000000000)*(gconst1)*(gconst13)*(gconst16)*(gconst6)))+(((gconst0)*(gconst13)*(gconst15)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst27)*(gconst4)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst17)*(gconst4)*(gconst5)))+(((gconst14)*(gconst17)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst2)*(gconst26)*(gconst3)*(gconst7)))+(((gconst2)*(gconst23)*(gconst3)*(gconst9)))+(((gconst10)*(gconst13)*(gconst7)*(gconst8)))+(((gconst11)*(gconst12)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst24)*(gconst4)*(gconst8)))+(((-1.00000000000000)*(gconst20)*(gconst3)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst26)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst15)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst22)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst17)*(gconst4)))+(((gconst20)*(gconst3)*(gconst5)*(gconst9)))+(((gconst2)*(gconst27)*(gconst3)*(gconst5)))+(((gconst10)*(gconst16)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst19)*(gconst4)*(gconst6)*(gconst8)))+(((gconst0)*(gconst22)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst17)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst13)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst25)*(gconst3)*(gconst8)))+(((gconst11)*(gconst17)*(gconst3)*(gconst5)))+(((gconst0)*(gconst13)*(gconst16)*(gconst7)))+(((-1.00000000000000)*(gconst15)*(gconst16)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst4)*(gconst9)))+(((gconst0)*(gconst15)*(gconst16)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst16)*(gconst2)*(gconst7)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst7)*(gconst8)))+(((gconst0)*(gconst25)*(gconst4)*(gconst8)))+(((gconst12)*(gconst14)*(gconst2)*(gconst9)))+(((gconst18)*(gconst4)*(gconst7)*(gconst8)))+(((gconst11)*(gconst14)*(gconst3)*(gconst9)))+(((gconst10)*(gconst15)*(gconst4)*(gconst8)))+(((gconst2)*(gconst21)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst22)*(gconst6)*(gconst8)))+(((-1.00000000000000)*(gconst11)*(gconst16)*(gconst3)*(gconst7)))+(((gconst0)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst14)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst15)*(gconst2)*(gconst8)))+(((gconst12)*(gconst17)*(gconst2)*(gconst5))));
00503 op[7]=((((gconst0)*(gconst13)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst12)*(gconst2)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst3)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst13)*(gconst6)*(gconst8)))+(((gconst0)*(gconst15)*(gconst4)*(gconst8)))+(((gconst12)*(gconst2)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst3)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst13)*(gconst5)*(gconst9)))+(((gconst17)*(gconst2)*(gconst3)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst16)*(gconst4)*(gconst6)))+(((gconst14)*(gconst2)*(gconst3)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst14)*(gconst4)*(gconst9)))+(((gconst10)*(gconst4)*(gconst7)*(gconst8)))+(((gconst11)*(gconst3)*(gconst5)*(gconst9)))+(((gconst0)*(gconst16)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst4)*(gconst5))));
00504 op[8]=((((gconst0)*(gconst4)*(gconst7)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst4)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst4)*(gconst6)*(gconst8)))+(((gconst2)*(gconst3)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst7)*(gconst8))));
00505 polyroots8(op,zeror,numroots);
00506 IKReal j27array[8], cj27array[8], sj27array[8], tempj27array[1];
00507 int numsolutions = 0;
00508 for(int ij27 = 0; ij27 < numroots; ++ij27)
00509 {
00510 IKReal htj27 = zeror[ij27];
00511 tempj27array[0]=((2.00000000000000)*(atan(htj27)));
00512 for(int kj27 = 0; kj27 < 1; ++kj27)
00513 {
00514 j27array[numsolutions] = tempj27array[kj27];
00515 if( j27array[numsolutions] > IKPI )
00516 {
00517     j27array[numsolutions]-=IK2PI;
00518 }
00519 else if( j27array[numsolutions] < -IKPI )
00520 {
00521     j27array[numsolutions]+=IK2PI;
00522 }
00523 sj27array[numsolutions] = IKsin(j27array[numsolutions]);
00524 cj27array[numsolutions] = IKcos(j27array[numsolutions]);
00525 numsolutions++;
00526 }
00527 }
00528 bool j27valid[8]={true,true,true,true,true,true,true,true};
00529 _nj27 = 8;
00530 for(int ij27 = 0; ij27 < numsolutions; ++ij27)
00531     {
00532 if( !j27valid[ij27] )
00533 {
00534     continue;
00535 }
00536     j27 = j27array[ij27]; cj27 = cj27array[ij27]; sj27 = sj27array[ij27];
00537 htj27 = IKtan(j27/2);
00538 
00539 _ij27[0] = ij27; _ij27[1] = -1;
00540 for(int iij27 = ij27+1; iij27 < numsolutions; ++iij27)
00541 {
00542 if( j27valid[iij27] && IKabs(cj27array[ij27]-cj27array[iij27]) < IKFAST_SOLUTION_THRESH && IKabs(sj27array[ij27]-sj27array[iij27]) < IKFAST_SOLUTION_THRESH )
00543 {
00544     j27valid[iij27]=false; _ij27[1] = iij27; break; 
00545 }
00546 }
00547 {
00548 IKReal dummyeval[1];
00549 IKReal gconst44;
00550 gconst44=IKsign(((((-4.00000000000000)*(sj29)*((pz)*(pz))))+(((0.800000000000000)*(py)*(sj27)*(sj29)))+(((-4.00000000000000)*(sj29)*((cj27)*(cj27))*((px)*(px))))+(((0.800000000000000)*(cj27)*(px)*(sj29)))+(((-8.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.0400000000000000)*(sj29)))+(((-4.00000000000000)*(sj29)*((py)*(py))*((sj27)*(sj27))))));
00551 dummyeval[0]=((((20.0000000000000)*(cj27)*(px)*(sj29)))+(((-100.000000000000)*(sj29)*((py)*(py))*((sj27)*(sj27))))+(((20.0000000000000)*(py)*(sj27)*(sj29)))+(((-200.000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-1.00000000000000)*(sj29)))+(((-100.000000000000)*(sj29)*((cj27)*(cj27))*((px)*(px))))+(((-100.000000000000)*(sj29)*((pz)*(pz)))));
00552 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00553 {
00554 {
00555 IKReal dummyeval[1];
00556 dummyeval[0]=sj29;
00557 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00558 {
00559 {
00560 IKReal evalcond[3];
00561 IKReal x69=((px)*(sj27));
00562 IKReal x70=((cj27)*(py));
00563 IKReal x71=((((-1.00000000000000)*(x70)))+(x69));
00564 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
00565 evalcond[1]=x71;
00566 evalcond[2]=x71;
00567 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
00568 {
00569 {
00570 IKReal j30array[2], cj30array[2], sj30array[2];
00571 bool j30valid[2]={false};
00572 _nj30 = 2;
00573 cj30array[0]=((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))));
00574 if( cj30array[0] >= -1-IKFAST_SINCOS_THRESH && cj30array[0] <= 1+IKFAST_SINCOS_THRESH )
00575 {
00576     j30valid[0] = j30valid[1] = true;
00577     j30array[0] = IKacos(cj30array[0]);
00578     sj30array[0] = IKsin(j30array[0]);
00579     cj30array[1] = cj30array[0];
00580     j30array[1] = -j30array[0];
00581     sj30array[1] = -sj30array[0];
00582 }
00583 else if( isnan(cj30array[0]) )
00584 {
00585     // probably any value will work
00586     j30valid[0] = true;
00587     cj30array[0] = 1; sj30array[0] = 0; j30array[0] = 0;
00588 }
00589 for(int ij30 = 0; ij30 < 2; ++ij30)
00590 {
00591 if( !j30valid[ij30] )
00592 {
00593     continue;
00594 }
00595 _ij30[0] = ij30; _ij30[1] = -1;
00596 for(int iij30 = ij30+1; iij30 < 2; ++iij30)
00597 {
00598 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
00599 {
00600     j30valid[iij30]=false; _ij30[1] = iij30; break; 
00601 }
00602 }
00603 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
00604 
00605 {
00606 IKReal dummyeval[1];
00607 IKReal gconst47;
00608 gconst47=IKsign(((40.0000000000000)+(((-321.000000000000)*(cj27)*(cj30)*(px)))+(((-400.000000000000)*(cj27)*(px)))+(((-400.000000000000)*(py)*(sj27)))+(((-321.000000000000)*(cj30)*(py)*(sj27)))+(((-321.000000000000)*(pz)*(sj30)))+(((32.1000000000000)*(cj30)))));
00609 dummyeval[0]=((1.24610591900312)+(((-10.0000000000000)*(pz)*(sj30)))+(((-10.0000000000000)*(cj30)*(py)*(sj27)))+(cj30)+(((-12.4610591900312)*(py)*(sj27)))+(((-12.4610591900312)*(cj27)*(px)))+(((-10.0000000000000)*(cj27)*(cj30)*(px))));
00610 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00611 {
00612 {
00613 IKReal dummyeval[1];
00614 dummyeval[0]=((((-10.0000000000000)*(cj27)*(px)*(sj30)))+(((12.4610591900312)*(pz)))+(((-10.0000000000000)*(py)*(sj27)*(sj30)))+(((10.0000000000000)*(cj30)*(pz)))+(sj30));
00615 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00616 {
00617 continue;
00618 
00619 } else
00620 {
00621 {
00622 IKReal j28array[1], cj28array[1], sj28array[1];
00623 bool j28valid[1]={false};
00624 _nj28 = 1;
00625 if( IKabs(((((IKabs(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((103041.000000000)*((sj30)*(sj30))))+(((-1000000.00000000)*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((103041.000000000)*((sj30)*(sj30))))+(((-1000000.00000000)*((pz)*(pz))))))))+IKsqr(((((IKabs(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30)))))))-1) <= IKFAST_SINCOS_THRESH )
00626     continue;
00627 j28array[0]=IKatan2(((((IKabs(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((321000.000000000)*(cj30)*(pz)))+(((400000.000000000)*(pz)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((103041.000000000)*((sj30)*(sj30))))+(((-1000000.00000000)*((pz)*(pz))))))), ((((IKabs(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(px)*(pz)))+(((-128.400000000000)*(sj30)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((-103.041000000000)*(cj30)*(sj30)))))));
00628 sj28array[0]=IKsin(j28array[0]);
00629 cj28array[0]=IKcos(j28array[0]);
00630 if( j28array[0] > IKPI )
00631 {
00632     j28array[0]-=IK2PI;
00633 }
00634 else if( j28array[0] < -IKPI )
00635 {    j28array[0]+=IK2PI;
00636 }
00637 j28valid[0] = true;
00638 for(int ij28 = 0; ij28 < 1; ++ij28)
00639 {
00640 if( !j28valid[ij28] )
00641 {
00642     continue;
00643 }
00644 _ij28[0] = ij28; _ij28[1] = -1;
00645 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
00646 {
00647 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
00648 {
00649     j28valid[iij28]=false; _ij28[1] = iij28; break; 
00650 }
00651 }
00652 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
00653 {
00654 IKReal evalcond[5];
00655 IKReal x72=IKsin(j28);
00656 IKReal x73=IKcos(j28);
00657 evalcond[0]=((((0.400000000000000)*(x72)))+(pz)+(((0.321000000000000)*(sj30)*(x73)))+(((0.321000000000000)*(cj30)*(x72))));
00658 evalcond[1]=((0.400000000000000)+(((pz)*(x72)))+(((0.100000000000000)*(x73)))+(((0.321000000000000)*(cj30)))+(((-1.00000000000000)*(py)*(sj27)*(x73)))+(((-1.00000000000000)*(cj27)*(px)*(x73))));
00659 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(x73)))+(((-0.321000000000000)*(sj30)*(x72)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj30)*(x73))));
00660 evalcond[3]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(x72)))+(((0.800000000000000)*(py)*(sj27)*(x73)))+(((-0.0800000000000000)*(x73)))+(((0.800000000000000)*(cj27)*(px)*(x73)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
00661 evalcond[4]=((((pz)*(x73)))+(((py)*(sj27)*(x72)))+(((cj27)*(px)*(x72)))+(((-0.100000000000000)*(x72)))+(((0.321000000000000)*(sj30))));
00662 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  )
00663 {
00664 continue;
00665 }
00666 }
00667 
00668 rotationfunction0(vsolutions);
00669 }
00670 }
00671 
00672 }
00673 
00674 }
00675 
00676 } else
00677 {
00678 {
00679 IKReal j28array[1], cj28array[1], sj28array[1];
00680 bool j28valid[1]={false};
00681 _nj28 = 1;
00682 if( IKabs(((gconst47)*(((((128.400000000000)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30)))+(((-100.000000000000)*(pz))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst47)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
00683     continue;
00684 j28array[0]=IKatan2(((gconst47)*(((((128.400000000000)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30)))+(((-100.000000000000)*(pz)))))), ((-1.00000000000000)*(gconst47)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
00685 sj28array[0]=IKsin(j28array[0]);
00686 cj28array[0]=IKcos(j28array[0]);
00687 if( j28array[0] > IKPI )
00688 {
00689     j28array[0]-=IK2PI;
00690 }
00691 else if( j28array[0] < -IKPI )
00692 {    j28array[0]+=IK2PI;
00693 }
00694 j28valid[0] = true;
00695 for(int ij28 = 0; ij28 < 1; ++ij28)
00696 {
00697 if( !j28valid[ij28] )
00698 {
00699     continue;
00700 }
00701 _ij28[0] = ij28; _ij28[1] = -1;
00702 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
00703 {
00704 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
00705 {
00706     j28valid[iij28]=false; _ij28[1] = iij28; break; 
00707 }
00708 }
00709 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
00710 {
00711 IKReal evalcond[5];
00712 IKReal x136=IKsin(j28);
00713 IKReal x137=IKcos(j28);
00714 evalcond[0]=((((0.321000000000000)*(sj30)*(x137)))+(((0.321000000000000)*(cj30)*(x136)))+(((0.400000000000000)*(x136)))+(pz));
00715 evalcond[1]=((0.400000000000000)+(((-1.00000000000000)*(cj27)*(px)*(x137)))+(((pz)*(x136)))+(((-1.00000000000000)*(py)*(sj27)*(x137)))+(((0.321000000000000)*(cj30)))+(((0.100000000000000)*(x137))));
00716 evalcond[2]=((0.100000000000000)+(((-0.321000000000000)*(sj30)*(x136)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.321000000000000)*(cj30)*(x137)))+(((0.400000000000000)*(x137)))+(((-1.00000000000000)*(py)*(sj27))));
00717 evalcond[3]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(py)*(sj27)*(x137)))+(((-0.0800000000000000)*(x137)))+(((-0.800000000000000)*(pz)*(x136)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27)))+(((0.800000000000000)*(cj27)*(px)*(x137))));
00718 evalcond[4]=((((py)*(sj27)*(x136)))+(((-0.100000000000000)*(x136)))+(((pz)*(x137)))+(((0.321000000000000)*(sj30)))+(((cj27)*(px)*(x136))));
00719 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  )
00720 {
00721 continue;
00722 }
00723 }
00724 
00725 rotationfunction0(vsolutions);
00726 }
00727 }
00728 
00729 }
00730 
00731 }
00732 }
00733 }
00734 
00735 } else
00736 {
00737 IKReal x138=((cj27)*(py));
00738 IKReal x139=((px)*(sj27));
00739 evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
00740 evalcond[1]=((x139)+(((-1.00000000000000)*(x138))));
00741 evalcond[2]=((x138)+(((-1.00000000000000)*(x139))));
00742 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
00743 {
00744 {
00745 IKReal j30array[2], cj30array[2], sj30array[2];
00746 bool j30valid[2]={false};
00747 _nj30 = 2;
00748 cj30array[0]=((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))));
00749 if( cj30array[0] >= -1-IKFAST_SINCOS_THRESH && cj30array[0] <= 1+IKFAST_SINCOS_THRESH )
00750 {
00751     j30valid[0] = j30valid[1] = true;
00752     j30array[0] = IKacos(cj30array[0]);
00753     sj30array[0] = IKsin(j30array[0]);
00754     cj30array[1] = cj30array[0];
00755     j30array[1] = -j30array[0];
00756     sj30array[1] = -sj30array[0];
00757 }
00758 else if( isnan(cj30array[0]) )
00759 {
00760     // probably any value will work
00761     j30valid[0] = true;
00762     cj30array[0] = 1; sj30array[0] = 0; j30array[0] = 0;
00763 }
00764 for(int ij30 = 0; ij30 < 2; ++ij30)
00765 {
00766 if( !j30valid[ij30] )
00767 {
00768     continue;
00769 }
00770 _ij30[0] = ij30; _ij30[1] = -1;
00771 for(int iij30 = ij30+1; iij30 < 2; ++iij30)
00772 {
00773 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
00774 {
00775     j30valid[iij30]=false; _ij30[1] = iij30; break; 
00776 }
00777 }
00778 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
00779 
00780 {
00781 IKReal dummyeval[1];
00782 IKReal gconst48;
00783 gconst48=IKsign(((-40.0000000000000)+(((-32.1000000000000)*(cj30)))+(((321.000000000000)*(cj30)*(py)*(sj27)))+(((400.000000000000)*(py)*(sj27)))+(((-321.000000000000)*(pz)*(sj30)))+(((321.000000000000)*(cj27)*(cj30)*(px)))+(((400.000000000000)*(cj27)*(px)))));
00784 dummyeval[0]=((-1.24610591900312)+(((-10.0000000000000)*(pz)*(sj30)))+(((10.0000000000000)*(cj30)*(py)*(sj27)))+(((12.4610591900312)*(py)*(sj27)))+(((10.0000000000000)*(cj27)*(cj30)*(px)))+(((12.4610591900312)*(cj27)*(px)))+(((-1.00000000000000)*(cj30))));
00785 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00786 {
00787 {
00788 IKReal dummyeval[1];
00789 dummyeval[0]=((((-10.0000000000000)*(cj27)*(px)*(sj30)))+(((-10.0000000000000)*(cj30)*(pz)))+(((-10.0000000000000)*(py)*(sj27)*(sj30)))+(sj30)+(((-12.4610591900312)*(pz))));
00790 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00791 {
00792 continue;
00793 
00794 } else
00795 {
00796 {
00797 IKReal j28array[1], cj28array[1], sj28array[1];
00798 bool j28valid[1]={false};
00799 _nj28 = 1;
00800 if( IKabs(((((IKabs(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((-103041.000000000)*((sj30)*(sj30))))+(((1000000.00000000)*((pz)*(pz)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IKabs(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((-128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((-103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((-103041.000000000)*((sj30)*(sj30))))+(((1000000.00000000)*((pz)*(pz))))))))+IKsqr(((((IKabs(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((-128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((-103.041000000000)*(cj30)*(sj30)))))))-1) <= IKFAST_SINCOS_THRESH )
00801     continue;
00802 j28array[0]=IKatan2(((((IKabs(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30))))) != 0)?((IKReal)1/(((((-400000.000000000)*(pz)))+(((-321000.000000000)*(cj27)*(px)*(sj30)))+(((-321000.000000000)*(cj30)*(pz)))+(((-321000.000000000)*(py)*(sj27)*(sj30)))+(((32100.0000000000)*(sj30)))))):(IKReal)1.0e30))*(((((-103041.000000000)*((sj30)*(sj30))))+(((1000000.00000000)*((pz)*(pz))))))), ((((IKabs(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz))))) != 0)?((IKReal)1/(((((-321.000000000000)*(cj30)*(pz)))+(((-321.000000000000)*(cj27)*(px)*(sj30)))+(((-321.000000000000)*(py)*(sj27)*(sj30)))+(((32.1000000000000)*(sj30)))+(((-400.000000000000)*(pz)))))):(IKReal)1.0e30))*(((((-128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((-103.041000000000)*(cj30)*(sj30)))))));
00803 sj28array[0]=IKsin(j28array[0]);
00804 cj28array[0]=IKcos(j28array[0]);
00805 if( j28array[0] > IKPI )
00806 {
00807     j28array[0]-=IK2PI;
00808 }
00809 else if( j28array[0] < -IKPI )
00810 {    j28array[0]+=IK2PI;
00811 }
00812 j28valid[0] = true;
00813 for(int ij28 = 0; ij28 < 1; ++ij28)
00814 {
00815 if( !j28valid[ij28] )
00816 {
00817     continue;
00818 }
00819 _ij28[0] = ij28; _ij28[1] = -1;
00820 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
00821 {
00822 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
00823 {
00824     j28valid[iij28]=false; _ij28[1] = iij28; break; 
00825 }
00826 }
00827 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
00828 {
00829 IKReal evalcond[5];
00830 IKReal x140=IKsin(j28);
00831 IKReal x141=IKcos(j28);
00832 evalcond[0]=((((0.400000000000000)*(x140)))+(((-0.321000000000000)*(sj30)*(x141)))+(pz)+(((0.321000000000000)*(cj30)*(x140))));
00833 evalcond[1]=((0.400000000000000)+(((pz)*(x140)))+(((0.321000000000000)*(cj30)))+(((-1.00000000000000)*(py)*(sj27)*(x141)))+(((0.100000000000000)*(x141)))+(((-1.00000000000000)*(cj27)*(px)*(x141))));
00834 evalcond[2]=((0.100000000000000)+(((0.321000000000000)*(sj30)*(x140)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(x141)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj30)*(x141))));
00835 evalcond[3]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.0800000000000000)*(x141)))+(((-0.800000000000000)*(pz)*(x140)))+(((0.800000000000000)*(cj27)*(px)*(x141)))+(((0.800000000000000)*(py)*(sj27)*(x141)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
00836 evalcond[4]=((((-1.00000000000000)*(pz)*(x141)))+(((-1.00000000000000)*(py)*(sj27)*(x140)))+(((0.100000000000000)*(x140)))+(((0.321000000000000)*(sj30)))+(((-1.00000000000000)*(cj27)*(px)*(x140))));
00837 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  )
00838 {
00839 continue;
00840 }
00841 }
00842 
00843 rotationfunction0(vsolutions);
00844 }
00845 }
00846 
00847 }
00848 
00849 }
00850 
00851 } else
00852 {
00853 {
00854 IKReal j28array[1], cj28array[1], sj28array[1];
00855 bool j28valid[1]={false};
00856 _nj28 = 1;
00857 if( IKabs(((gconst48)*(((((128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
00858     continue;
00859 j28array[0]=IKatan2(((gconst48)*(((((128.400000000000)*(sj30)))+(((-1000.00000000000)*(cj27)*(px)*(pz)))+(((100.000000000000)*(pz)))+(((-1000.00000000000)*(py)*(pz)*(sj27)))+(((103.041000000000)*(cj30)*(sj30)))))), ((gconst48)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
00860 sj28array[0]=IKsin(j28array[0]);
00861 cj28array[0]=IKcos(j28array[0]);
00862 if( j28array[0] > IKPI )
00863 {
00864     j28array[0]-=IK2PI;
00865 }
00866 else if( j28array[0] < -IKPI )
00867 {    j28array[0]+=IK2PI;
00868 }
00869 j28valid[0] = true;
00870 for(int ij28 = 0; ij28 < 1; ++ij28)
00871 {
00872 if( !j28valid[ij28] )
00873 {
00874     continue;
00875 }
00876 _ij28[0] = ij28; _ij28[1] = -1;
00877 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
00878 {
00879 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
00880 {
00881     j28valid[iij28]=false; _ij28[1] = iij28; break; 
00882 }
00883 }
00884 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
00885 {
00886 IKReal evalcond[5];
00887 IKReal x142=IKsin(j28);
00888 IKReal x143=IKcos(j28);
00889 evalcond[0]=((((0.400000000000000)*(x142)))+(((-0.321000000000000)*(sj30)*(x143)))+(pz)+(((0.321000000000000)*(cj30)*(x142))));
00890 evalcond[1]=((0.400000000000000)+(((pz)*(x142)))+(((0.321000000000000)*(cj30)))+(((-1.00000000000000)*(py)*(sj27)*(x143)))+(((0.100000000000000)*(x143)))+(((-1.00000000000000)*(cj27)*(px)*(x143))));
00891 evalcond[2]=((0.100000000000000)+(((0.321000000000000)*(sj30)*(x142)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(x143)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj30)*(x143))));
00892 evalcond[3]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.0800000000000000)*(x143)))+(((-0.800000000000000)*(pz)*(x142)))+(((0.800000000000000)*(cj27)*(px)*(x143)))+(((0.800000000000000)*(py)*(sj27)*(x143)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
00893 evalcond[4]=((((-1.00000000000000)*(pz)*(x143)))+(((-1.00000000000000)*(py)*(sj27)*(x142)))+(((0.100000000000000)*(x142)))+(((0.321000000000000)*(sj30)))+(((-1.00000000000000)*(cj27)*(px)*(x142))));
00894 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  )
00895 {
00896 continue;
00897 }
00898 }
00899 
00900 rotationfunction0(vsolutions);
00901 }
00902 }
00903 
00904 }
00905 
00906 }
00907 }
00908 }
00909 
00910 } else
00911 {
00912 if( 1 )
00913 {
00914 continue;
00915 
00916 } else
00917 {
00918 }
00919 }
00920 }
00921 }
00922 
00923 } else
00924 {
00925 {
00926 IKReal j30array[1], cj30array[1], sj30array[1];
00927 bool j30valid[1]={false};
00928 _nj30 = 1;
00929 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
00930     continue;
00931 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
00932 sj30array[0]=IKsin(j30array[0]);
00933 cj30array[0]=IKcos(j30array[0]);
00934 if( j30array[0] > IKPI )
00935 {
00936     j30array[0]-=IK2PI;
00937 }
00938 else if( j30array[0] < -IKPI )
00939 {    j30array[0]+=IK2PI;
00940 }
00941 j30valid[0] = true;
00942 for(int ij30 = 0; ij30 < 1; ++ij30)
00943 {
00944 if( !j30valid[ij30] )
00945 {
00946     continue;
00947 }
00948 _ij30[0] = ij30; _ij30[1] = -1;
00949 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
00950 {
00951 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
00952 {
00953     j30valid[iij30]=false; _ij30[1] = iij30; break; 
00954 }
00955 }
00956 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
00957 {
00958 IKReal evalcond[2];
00959 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(IKsin(j30))))+(((px)*(sj27))));
00960 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(IKcos(j30))))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
00961 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
00962 {
00963 continue;
00964 }
00965 }
00966 
00967 {
00968 IKReal dummyeval[1];
00969 IKReal gconst45;
00970 gconst45=IKsign(((40.0000000000000)+(((-321.000000000000)*(cj27)*(cj30)*(px)))+(((-400.000000000000)*(cj27)*(px)))+(((-400.000000000000)*(py)*(sj27)))+(((-321.000000000000)*(cj30)*(py)*(sj27)))+(((-321.000000000000)*(cj29)*(pz)*(sj30)))+(((32.1000000000000)*(cj30)))));
00971 dummyeval[0]=((1.24610591900312)+(((-10.0000000000000)*(cj30)*(py)*(sj27)))+(cj30)+(((-12.4610591900312)*(py)*(sj27)))+(((-12.4610591900312)*(cj27)*(px)))+(((-10.0000000000000)*(cj29)*(pz)*(sj30)))+(((-10.0000000000000)*(cj27)*(cj30)*(px))));
00972 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00973 {
00974 continue;
00975 
00976 } else
00977 {
00978 {
00979 IKReal j28array[1], cj28array[1], sj28array[1];
00980 bool j28valid[1]={false};
00981 _nj28 = 1;
00982 if( IKabs(((gconst45)*(((((128.400000000000)*(cj29)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((103.041000000000)*(cj29)*(cj30)*(sj30))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst45)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30))))))) < IKFAST_ATAN2_MAGTHRESH )
00983     continue;
00984 j28array[0]=IKatan2(((gconst45)*(((((128.400000000000)*(cj29)*(sj30)))+(((1000.00000000000)*(cj27)*(px)*(pz)))+(((1000.00000000000)*(py)*(pz)*(sj27)))+(((-100.000000000000)*(pz)))+(((103.041000000000)*(cj29)*(cj30)*(sj30)))))), ((-1.00000000000000)*(gconst45)*(((160.000000000000)+(((-1000.00000000000)*((pz)*(pz))))+(((103.041000000000)*((cj30)*(cj30))))+(((256.800000000000)*(cj30)))))));
00985 sj28array[0]=IKsin(j28array[0]);
00986 cj28array[0]=IKcos(j28array[0]);
00987 if( j28array[0] > IKPI )
00988 {
00989     j28array[0]-=IK2PI;
00990 }
00991 else if( j28array[0] < -IKPI )
00992 {    j28array[0]+=IK2PI;
00993 }
00994 j28valid[0] = true;
00995 for(int ij28 = 0; ij28 < 1; ++ij28)
00996 {
00997 if( !j28valid[ij28] )
00998 {
00999     continue;
01000 }
01001 _ij28[0] = ij28; _ij28[1] = -1;
01002 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
01003 {
01004 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
01005 {
01006     j28valid[iij28]=false; _ij28[1] = iij28; break; 
01007 }
01008 }
01009 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
01010 {
01011 IKReal evalcond[6];
01012 IKReal x144=IKsin(j28);
01013 IKReal x145=IKcos(j28);
01014 evalcond[0]=((((0.321000000000000)*(cj29)*(sj30)*(x145)))+(((0.400000000000000)*(x144)))+(pz)+(((0.321000000000000)*(cj30)*(x144))));
01015 evalcond[1]=((0.400000000000000)+(((pz)*(x144)))+(((0.321000000000000)*(cj30)))+(((-1.00000000000000)*(py)*(sj27)*(x145)))+(((0.100000000000000)*(x145)))+(((-1.00000000000000)*(cj27)*(px)*(x145))));
01016 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(x145)))+(((-1.00000000000000)*(py)*(sj27)))+(((-0.321000000000000)*(cj29)*(sj30)*(x144)))+(((0.321000000000000)*(cj30)*(x145))));
01017 evalcond[3]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.0800000000000000)*(x145)))+(((-0.800000000000000)*(pz)*(x144)))+(((0.800000000000000)*(cj27)*(px)*(x145)))+(((0.800000000000000)*(py)*(sj27)*(x145)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01018 evalcond[4]=((((-1.00000000000000)*(py)*(sj27)*(sj29)*(x144)))+(((cj29)*(px)*(sj27)))+(((-1.00000000000000)*(cj27)*(px)*(sj29)*(x144)))+(((0.100000000000000)*(sj29)*(x144)))+(((-1.00000000000000)*(cj27)*(cj29)*(py)))+(((-1.00000000000000)*(pz)*(sj29)*(x145))));
01019 evalcond[5]=((((-0.100000000000000)*(cj29)*(x144)))+(((cj29)*(pz)*(x145)))+(((0.321000000000000)*(sj30)))+(((cj29)*(py)*(sj27)*(x144)))+(((cj27)*(cj29)*(px)*(x144)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
01020 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  )
01021 {
01022 continue;
01023 }
01024 }
01025 
01026 rotationfunction0(vsolutions);
01027 }
01028 }
01029 
01030 }
01031 
01032 }
01033 }
01034 }
01035 
01036 }
01037 
01038 }
01039 
01040 } else
01041 {
01042 {
01043 IKReal j28array[1], cj28array[1], sj28array[1];
01044 bool j28valid[1]={false};
01045 _nj28 = 1;
01046 IKReal x146=(py)*(py);
01047 IKReal x147=(sj27)*(sj27);
01048 IKReal x148=(cj27)*(cj27);
01049 IKReal x149=(px)*(px);
01050 if( IKabs(((gconst44)*(((((-4.00000000000000)*(cj27)*(cj29)*(sj27)*(x149)))+(((-4.00000000000000)*(cj29)*(px)*(py)*(x147)))+(((-1.00000000000000)*(cj27)*(px)*(pz)*(sj29)))+(((0.400000000000000)*(cj29)*(px)*(sj27)))+(((5.00000000000000)*(pp)*(pz)*(sj29)))+(((-1.00000000000000)*(py)*(pz)*(sj27)*(sj29)))+(((0.334795000000000)*(pz)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(sj27)*(x146)))+(((4.00000000000000)*(cj29)*(px)*(py)*(x148)))+(((-0.400000000000000)*(cj27)*(cj29)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((sj29)*(x148)*(x149)))+(((0.0334795000000000)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(py)*(pz)))+(((-5.00000000000000)*(pp)*(py)*(sj27)*(sj29)))+(((0.500000000000000)*(pp)*(sj29)))+(((-5.00000000000000)*(cj27)*(pp)*(px)*(sj29)))+(((-4.00000000000000)*(cj29)*(px)*(pz)*(sj27)))+(((2.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.434795000000000)*(cj27)*(px)*(sj29)))+(((-0.434795000000000)*(py)*(sj27)*(sj29)))+(((sj29)*(x146)*(x147))))))) < IKFAST_ATAN2_MAGTHRESH )
01051     continue;
01052 j28array[0]=IKatan2(((gconst44)*(((((-4.00000000000000)*(cj27)*(cj29)*(sj27)*(x149)))+(((-4.00000000000000)*(cj29)*(px)*(py)*(x147)))+(((-1.00000000000000)*(cj27)*(px)*(pz)*(sj29)))+(((0.400000000000000)*(cj29)*(px)*(sj27)))+(((5.00000000000000)*(pp)*(pz)*(sj29)))+(((-1.00000000000000)*(py)*(pz)*(sj27)*(sj29)))+(((0.334795000000000)*(pz)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(sj27)*(x146)))+(((4.00000000000000)*(cj29)*(px)*(py)*(x148)))+(((-0.400000000000000)*(cj27)*(cj29)*(py)))))), ((gconst44)*(((((sj29)*(x148)*(x149)))+(((0.0334795000000000)*(sj29)))+(((4.00000000000000)*(cj27)*(cj29)*(py)*(pz)))+(((-5.00000000000000)*(pp)*(py)*(sj27)*(sj29)))+(((0.500000000000000)*(pp)*(sj29)))+(((-5.00000000000000)*(cj27)*(pp)*(px)*(sj29)))+(((-4.00000000000000)*(cj29)*(px)*(pz)*(sj27)))+(((2.00000000000000)*(cj27)*(px)*(py)*(sj27)*(sj29)))+(((-0.434795000000000)*(cj27)*(px)*(sj29)))+(((-0.434795000000000)*(py)*(sj27)*(sj29)))+(((sj29)*(x146)*(x147)))))));
01053 sj28array[0]=IKsin(j28array[0]);
01054 cj28array[0]=IKcos(j28array[0]);
01055 if( j28array[0] > IKPI )
01056 {
01057     j28array[0]-=IK2PI;
01058 }
01059 else if( j28array[0] < -IKPI )
01060 {    j28array[0]+=IK2PI;
01061 }
01062 j28valid[0] = true;
01063 for(int ij28 = 0; ij28 < 1; ++ij28)
01064 {
01065 if( !j28valid[ij28] )
01066 {
01067     continue;
01068 }
01069 _ij28[0] = ij28; _ij28[1] = -1;
01070 for(int iij28 = ij28+1; iij28 < 1; ++iij28)
01071 {
01072 if( j28valid[iij28] && IKabs(cj28array[ij28]-cj28array[iij28]) < IKFAST_SOLUTION_THRESH && IKabs(sj28array[ij28]-sj28array[iij28]) < IKFAST_SOLUTION_THRESH )
01073 {
01074     j28valid[iij28]=false; _ij28[1] = iij28; break; 
01075 }
01076 }
01077 j28 = j28array[ij28]; cj28 = cj28array[ij28]; sj28 = sj28array[ij28];
01078 {
01079 IKReal evalcond[2];
01080 IKReal x150=IKcos(j28);
01081 IKReal x151=IKsin(j28);
01082 evalcond[0]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.0800000000000000)*(x150)))+(((-0.800000000000000)*(pz)*(x151)))+(((0.800000000000000)*(cj27)*(px)*(x150)))+(((0.800000000000000)*(py)*(sj27)*(x150)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01083 evalcond[1]=((((-1.00000000000000)*(pz)*(sj29)*(x150)))+(((cj29)*(px)*(sj27)))+(((-1.00000000000000)*(py)*(sj27)*(sj29)*(x151)))+(((-1.00000000000000)*(cj27)*(px)*(sj29)*(x151)))+(((0.100000000000000)*(sj29)*(x151)))+(((-1.00000000000000)*(cj27)*(cj29)*(py))));
01084 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
01085 {
01086 continue;
01087 }
01088 }
01089 
01090 {
01091 IKReal dummyeval[1];
01092 dummyeval[0]=sj29;
01093 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01094 {
01095 {
01096 IKReal dummyeval[1];
01097 dummyeval[0]=sj29;
01098 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01099 {
01100 {
01101 IKReal dummyeval[2];
01102 dummyeval[0]=cj28;
01103 dummyeval[1]=cj29;
01104 if( IKabs(dummyeval[0]) < 0.0000010000000000  || IKabs(dummyeval[1]) < 0.0000010000000000  )
01105 {
01106 {
01107 IKReal evalcond[4];
01108 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j28)), 6.28318530717959)));
01109 evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01110 evalcond[2]=((((cj29)*(px)*(sj27)))+(((0.100000000000000)*(sj29)))+(((-1.00000000000000)*(py)*(sj27)*(sj29)))+(((-1.00000000000000)*(cj27)*(px)*(sj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(py))));
01111 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
01112 {
01113 {
01114 IKReal dummyeval[1];
01115 dummyeval[0]=sj29;
01116 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01117 {
01118 {
01119 IKReal dummyeval[1];
01120 dummyeval[0]=cj29;
01121 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01122 {
01123 {
01124 IKReal dummyeval[1];
01125 dummyeval[0]=sj29;
01126 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01127 {
01128 {
01129 IKReal evalcond[4];
01130 IKReal x152=((px)*(sj27));
01131 IKReal x153=((cj27)*(py));
01132 IKReal x154=((((-1.00000000000000)*(x153)))+(x152));
01133 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
01134 evalcond[1]=x154;
01135 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01136 evalcond[3]=x154;
01137 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01138 {
01139 {
01140 IKReal j30array[1], cj30array[1], sj30array[1];
01141 bool j30valid[1]={false};
01142 _nj30 = 1;
01143 if( IKabs(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01144     continue;
01145 j30array[0]=IKatan2(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01146 sj30array[0]=IKsin(j30array[0]);
01147 cj30array[0]=IKcos(j30array[0]);
01148 if( j30array[0] > IKPI )
01149 {
01150     j30array[0]-=IK2PI;
01151 }
01152 else if( j30array[0] < -IKPI )
01153 {    j30array[0]+=IK2PI;
01154 }
01155 j30valid[0] = true;
01156 for(int ij30 = 0; ij30 < 1; ++ij30)
01157 {
01158 if( !j30valid[ij30] )
01159 {
01160     continue;
01161 }
01162 _ij30[0] = ij30; _ij30[1] = -1;
01163 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01164 {
01165 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01166 {
01167     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01168 }
01169 }
01170 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01171 {
01172 IKReal evalcond[3];
01173 IKReal x155=IKcos(j30);
01174 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x155)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01175 evalcond[1]=((0.400000000000000)+(((0.321000000000000)*(x155)))+(pz));
01176 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((-0.321000000000000)*(IKsin(j30)))));
01177 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01178 {
01179 continue;
01180 }
01181 }
01182 
01183 rotationfunction0(vsolutions);
01184 }
01185 }
01186 
01187 } else
01188 {
01189 IKReal x156=((cj27)*(py));
01190 IKReal x157=((px)*(sj27));
01191 evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
01192 evalcond[1]=((((-1.00000000000000)*(x156)))+(x157));
01193 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01194 evalcond[3]=((((-1.00000000000000)*(x157)))+(x156));
01195 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01196 {
01197 {
01198 IKReal j30array[1], cj30array[1], sj30array[1];
01199 bool j30valid[1]={false};
01200 _nj30 = 1;
01201 if( IKabs(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01202     continue;
01203 j30array[0]=IKatan2(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01204 sj30array[0]=IKsin(j30array[0]);
01205 cj30array[0]=IKcos(j30array[0]);
01206 if( j30array[0] > IKPI )
01207 {
01208     j30array[0]-=IK2PI;
01209 }
01210 else if( j30array[0] < -IKPI )
01211 {    j30array[0]+=IK2PI;
01212 }
01213 j30valid[0] = true;
01214 for(int ij30 = 0; ij30 < 1; ++ij30)
01215 {
01216 if( !j30valid[ij30] )
01217 {
01218     continue;
01219 }
01220 _ij30[0] = ij30; _ij30[1] = -1;
01221 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01222 {
01223 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01224 {
01225     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01226 }
01227 }
01228 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01229 {
01230 IKReal evalcond[3];
01231 IKReal x158=IKcos(j30);
01232 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x158)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01233 evalcond[1]=((0.400000000000000)+(((0.321000000000000)*(x158)))+(pz));
01234 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(IKsin(j30)))));
01235 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01236 {
01237 continue;
01238 }
01239 }
01240 
01241 rotationfunction0(vsolutions);
01242 }
01243 }
01244 
01245 } else
01246 {
01247 IKReal x159=((cj27)*(px));
01248 IKReal x160=((py)*(sj27));
01249 IKReal x161=((x160)+(x159));
01250 IKReal x162=((0.100000000000000)+(((-1.00000000000000)*(x161))));
01251 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
01252 evalcond[1]=x162;
01253 evalcond[2]=((-0.0669590000000000)+(((-0.800000000000000)*(pz)))+(((0.200000000000000)*(x160)))+(((0.200000000000000)*(x159)))+(((-1.00000000000000)*(pp))));
01254 evalcond[3]=x162;
01255 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01256 {
01257 {
01258 IKReal j30array[1], cj30array[1], sj30array[1];
01259 bool j30valid[1]={false};
01260 _nj30 = 1;
01261 if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01262     continue;
01263 j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01264 sj30array[0]=IKsin(j30array[0]);
01265 cj30array[0]=IKcos(j30array[0]);
01266 if( j30array[0] > IKPI )
01267 {
01268     j30array[0]-=IK2PI;
01269 }
01270 else if( j30array[0] < -IKPI )
01271 {    j30array[0]+=IK2PI;
01272 }
01273 j30valid[0] = true;
01274 for(int ij30 = 0; ij30 < 1; ++ij30)
01275 {
01276 if( !j30valid[ij30] )
01277 {
01278     continue;
01279 }
01280 _ij30[0] = ij30; _ij30[1] = -1;
01281 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01282 {
01283 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01284 {
01285     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01286 }
01287 }
01288 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01289 {
01290 IKReal evalcond[3];
01291 IKReal x163=IKcos(j30);
01292 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((0.321000000000000)*(IKsin(j30)))));
01293 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x163)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01294 evalcond[2]=((0.400000000000000)+(pz)+(((0.321000000000000)*(x163))));
01295 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01296 {
01297 continue;
01298 }
01299 }
01300 
01301 rotationfunction0(vsolutions);
01302 }
01303 }
01304 
01305 } else
01306 {
01307 IKReal x164=((cj27)*(px));
01308 IKReal x165=((py)*(sj27));
01309 evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
01310 evalcond[1]=((0.100000000000000)+(((-1.00000000000000)*(x165)))+(((-1.00000000000000)*(x164))));
01311 evalcond[2]=((-0.0669590000000000)+(((-0.800000000000000)*(pz)))+(((0.200000000000000)*(x165)))+(((0.200000000000000)*(x164)))+(((-1.00000000000000)*(pp))));
01312 evalcond[3]=((-0.100000000000000)+(x164)+(x165));
01313 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01314 {
01315 {
01316 IKReal j30array[1], cj30array[1], sj30array[1];
01317 bool j30valid[1]={false};
01318 _nj30 = 1;
01319 if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01320     continue;
01321 j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01322 sj30array[0]=IKsin(j30array[0]);
01323 cj30array[0]=IKcos(j30array[0]);
01324 if( j30array[0] > IKPI )
01325 {
01326     j30array[0]-=IK2PI;
01327 }
01328 else if( j30array[0] < -IKPI )
01329 {    j30array[0]+=IK2PI;
01330 }
01331 j30valid[0] = true;
01332 for(int ij30 = 0; ij30 < 1; ++ij30)
01333 {
01334 if( !j30valid[ij30] )
01335 {
01336     continue;
01337 }
01338 _ij30[0] = ij30; _ij30[1] = -1;
01339 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01340 {
01341 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01342 {
01343     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01344 }
01345 }
01346 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01347 {
01348 IKReal evalcond[3];
01349 IKReal x166=IKcos(j30);
01350 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((-0.321000000000000)*(IKsin(j30)))));
01351 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x166)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01352 evalcond[2]=((0.400000000000000)+(pz)+(((0.321000000000000)*(x166))));
01353 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01354 {
01355 continue;
01356 }
01357 }
01358 
01359 rotationfunction0(vsolutions);
01360 }
01361 }
01362 
01363 } else
01364 {
01365 if( 1 )
01366 {
01367 continue;
01368 
01369 } else
01370 {
01371 }
01372 }
01373 }
01374 }
01375 }
01376 }
01377 
01378 } else
01379 {
01380 {
01381 IKReal j30array[1], cj30array[1], sj30array[1];
01382 bool j30valid[1]={false};
01383 _nj30 = 1;
01384 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
01385     continue;
01386 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
01387 sj30array[0]=IKsin(j30array[0]);
01388 cj30array[0]=IKcos(j30array[0]);
01389 if( j30array[0] > IKPI )
01390 {
01391     j30array[0]-=IK2PI;
01392 }
01393 else if( j30array[0] < -IKPI )
01394 {    j30array[0]+=IK2PI;
01395 }
01396 j30valid[0] = true;
01397 for(int ij30 = 0; ij30 < 1; ++ij30)
01398 {
01399 if( !j30valid[ij30] )
01400 {
01401     continue;
01402 }
01403 _ij30[0] = ij30; _ij30[1] = -1;
01404 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01405 {
01406 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01407 {
01408     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01409 }
01410 }
01411 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01412 {
01413 IKReal evalcond[5];
01414 IKReal x167=IKcos(j30);
01415 IKReal x168=IKsin(j30);
01416 evalcond[0]=((((0.321000000000000)*(sj29)*(x168)))+(((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27))));
01417 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x167)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01418 evalcond[2]=((0.400000000000000)+(pz)+(((0.321000000000000)*(x167))));
01419 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-0.321000000000000)*(cj29)*(x168)))+(((-1.00000000000000)*(py)*(sj27))));
01420 evalcond[4]=((((cj29)*(py)*(sj27)))+(((cj27)*(cj29)*(px)))+(((-0.100000000000000)*(cj29)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29)))+(((0.321000000000000)*(x168))));
01421 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  )
01422 {
01423 continue;
01424 }
01425 }
01426 
01427 rotationfunction0(vsolutions);
01428 }
01429 }
01430 
01431 }
01432 
01433 }
01434 
01435 } else
01436 {
01437 {
01438 IKReal j30array[1], cj30array[1], sj30array[1];
01439 bool j30valid[1]={false};
01440 _nj30 = 1;
01441 if( IKabs(((0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01442     continue;
01443 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01444 sj30array[0]=IKsin(j30array[0]);
01445 cj30array[0]=IKcos(j30array[0]);
01446 if( j30array[0] > IKPI )
01447 {
01448     j30array[0]-=IK2PI;
01449 }
01450 else if( j30array[0] < -IKPI )
01451 {    j30array[0]+=IK2PI;
01452 }
01453 j30valid[0] = true;
01454 for(int ij30 = 0; ij30 < 1; ++ij30)
01455 {
01456 if( !j30valid[ij30] )
01457 {
01458     continue;
01459 }
01460 _ij30[0] = ij30; _ij30[1] = -1;
01461 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01462 {
01463 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01464 {
01465     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01466 }
01467 }
01468 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01469 {
01470 IKReal evalcond[5];
01471 IKReal x169=IKcos(j30);
01472 IKReal x170=IKsin(j30);
01473 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x170)))+(((px)*(sj27))));
01474 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x169)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01475 evalcond[2]=((0.400000000000000)+(pz)+(((0.321000000000000)*(x169))));
01476 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-0.321000000000000)*(cj29)*(x170)))+(((-1.00000000000000)*(py)*(sj27))));
01477 evalcond[4]=((((cj29)*(py)*(sj27)))+(((cj27)*(cj29)*(px)))+(((-0.100000000000000)*(cj29)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29)))+(((0.321000000000000)*(x170))));
01478 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  )
01479 {
01480 continue;
01481 }
01482 }
01483 
01484 rotationfunction0(vsolutions);
01485 }
01486 }
01487 
01488 }
01489 
01490 }
01491 
01492 } else
01493 {
01494 {
01495 IKReal j30array[1], cj30array[1], sj30array[1];
01496 bool j30valid[1]={false};
01497 _nj30 = 1;
01498 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((-3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-1.24610591900312)+(((-3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01499     continue;
01500 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((-3.11526479750779)*(pz)))));
01501 sj30array[0]=IKsin(j30array[0]);
01502 cj30array[0]=IKcos(j30array[0]);
01503 if( j30array[0] > IKPI )
01504 {
01505     j30array[0]-=IK2PI;
01506 }
01507 else if( j30array[0] < -IKPI )
01508 {    j30array[0]+=IK2PI;
01509 }
01510 j30valid[0] = true;
01511 for(int ij30 = 0; ij30 < 1; ++ij30)
01512 {
01513 if( !j30valid[ij30] )
01514 {
01515     continue;
01516 }
01517 _ij30[0] = ij30; _ij30[1] = -1;
01518 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01519 {
01520 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01521 {
01522     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01523 }
01524 }
01525 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01526 {
01527 IKReal evalcond[5];
01528 IKReal x171=IKcos(j30);
01529 IKReal x172=IKsin(j30);
01530 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x172)))+(((px)*(sj27))));
01531 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x171)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01532 evalcond[2]=((0.400000000000000)+(pz)+(((0.321000000000000)*(x171))));
01533 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-0.321000000000000)*(cj29)*(x172)))+(((-1.00000000000000)*(py)*(sj27))));
01534 evalcond[4]=((((cj29)*(py)*(sj27)))+(((cj27)*(cj29)*(px)))+(((-0.100000000000000)*(cj29)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29)))+(((0.321000000000000)*(x172))));
01535 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  )
01536 {
01537 continue;
01538 }
01539 }
01540 
01541 rotationfunction0(vsolutions);
01542 }
01543 }
01544 
01545 }
01546 
01547 }
01548 
01549 } else
01550 {
01551 evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j28)), 6.28318530717959)));
01552 evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01553 evalcond[2]=((((cj29)*(px)*(sj27)))+(((cj27)*(px)*(sj29)))+(((py)*(sj27)*(sj29)))+(((-0.100000000000000)*(sj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(py))));
01554 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
01555 {
01556 {
01557 IKReal dummyeval[1];
01558 dummyeval[0]=sj29;
01559 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01560 {
01561 {
01562 IKReal dummyeval[1];
01563 dummyeval[0]=cj29;
01564 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01565 {
01566 {
01567 IKReal dummyeval[1];
01568 dummyeval[0]=sj29;
01569 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01570 {
01571 {
01572 IKReal evalcond[4];
01573 IKReal x173=((px)*(sj27));
01574 IKReal x174=((cj27)*(py));
01575 IKReal x175=((((-1.00000000000000)*(x174)))+(x173));
01576 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
01577 evalcond[1]=x175;
01578 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01579 evalcond[3]=x175;
01580 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01581 {
01582 {
01583 IKReal j30array[1], cj30array[1], sj30array[1];
01584 bool j30valid[1]={false};
01585 _nj30 = 1;
01586 if( IKabs(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01587     continue;
01588 j30array[0]=IKatan2(((-0.311526479750779)+(((3.11526479750779)*(py)*(sj27)))+(((3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01589 sj30array[0]=IKsin(j30array[0]);
01590 cj30array[0]=IKcos(j30array[0]);
01591 if( j30array[0] > IKPI )
01592 {
01593     j30array[0]-=IK2PI;
01594 }
01595 else if( j30array[0] < -IKPI )
01596 {    j30array[0]+=IK2PI;
01597 }
01598 j30valid[0] = true;
01599 for(int ij30 = 0; ij30 < 1; ++ij30)
01600 {
01601 if( !j30valid[ij30] )
01602 {
01603     continue;
01604 }
01605 _ij30[0] = ij30; _ij30[1] = -1;
01606 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01607 {
01608 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01609 {
01610     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01611 }
01612 }
01613 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01614 {
01615 IKReal evalcond[3];
01616 IKReal x176=IKcos(j30);
01617 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x176)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01618 evalcond[1]=((-0.400000000000000)+(((-0.321000000000000)*(x176)))+(pz));
01619 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(IKsin(j30)))));
01620 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01621 {
01622 continue;
01623 }
01624 }
01625 
01626 rotationfunction0(vsolutions);
01627 }
01628 }
01629 
01630 } else
01631 {
01632 IKReal x177=((cj27)*(py));
01633 IKReal x178=((px)*(sj27));
01634 evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
01635 evalcond[1]=((((-1.00000000000000)*(x177)))+(x178));
01636 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01637 evalcond[3]=((((-1.00000000000000)*(x178)))+(x177));
01638 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01639 {
01640 {
01641 IKReal j30array[1], cj30array[1], sj30array[1];
01642 bool j30valid[1]={false};
01643 _nj30 = 1;
01644 if( IKabs(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01645     continue;
01646 j30array[0]=IKatan2(((0.311526479750779)+(((-3.11526479750779)*(py)*(sj27)))+(((-3.11526479750779)*(cj27)*(px)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01647 sj30array[0]=IKsin(j30array[0]);
01648 cj30array[0]=IKcos(j30array[0]);
01649 if( j30array[0] > IKPI )
01650 {
01651     j30array[0]-=IK2PI;
01652 }
01653 else if( j30array[0] < -IKPI )
01654 {    j30array[0]+=IK2PI;
01655 }
01656 j30valid[0] = true;
01657 for(int ij30 = 0; ij30 < 1; ++ij30)
01658 {
01659 if( !j30valid[ij30] )
01660 {
01661     continue;
01662 }
01663 _ij30[0] = ij30; _ij30[1] = -1;
01664 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01665 {
01666 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01667 {
01668     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01669 }
01670 }
01671 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01672 {
01673 IKReal evalcond[3];
01674 IKReal x179=IKcos(j30);
01675 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x179)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01676 evalcond[1]=((-0.400000000000000)+(((-0.321000000000000)*(x179)))+(pz));
01677 evalcond[2]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((-0.321000000000000)*(IKsin(j30)))));
01678 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01679 {
01680 continue;
01681 }
01682 }
01683 
01684 rotationfunction0(vsolutions);
01685 }
01686 }
01687 
01688 } else
01689 {
01690 IKReal x180=((cj27)*(px));
01691 IKReal x181=((py)*(sj27));
01692 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
01693 evalcond[1]=((0.100000000000000)+(((-1.00000000000000)*(x180)))+(((-1.00000000000000)*(x181))));
01694 evalcond[2]=((-0.0669590000000000)+(((0.800000000000000)*(pz)))+(((0.200000000000000)*(x181)))+(((0.200000000000000)*(x180)))+(((-1.00000000000000)*(pp))));
01695 evalcond[3]=((-0.100000000000000)+(x180)+(x181));
01696 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01697 {
01698 {
01699 IKReal j30array[1], cj30array[1], sj30array[1];
01700 bool j30valid[1]={false};
01701 _nj30 = 1;
01702 if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01703     continue;
01704 j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01705 sj30array[0]=IKsin(j30array[0]);
01706 cj30array[0]=IKcos(j30array[0]);
01707 if( j30array[0] > IKPI )
01708 {
01709     j30array[0]-=IK2PI;
01710 }
01711 else if( j30array[0] < -IKPI )
01712 {    j30array[0]+=IK2PI;
01713 }
01714 j30valid[0] = true;
01715 for(int ij30 = 0; ij30 < 1; ++ij30)
01716 {
01717 if( !j30valid[ij30] )
01718 {
01719     continue;
01720 }
01721 _ij30[0] = ij30; _ij30[1] = -1;
01722 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01723 {
01724 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01725 {
01726     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01727 }
01728 }
01729 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01730 {
01731 IKReal evalcond[3];
01732 IKReal x182=IKcos(j30);
01733 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((0.321000000000000)*(IKsin(j30)))));
01734 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x182)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01735 evalcond[2]=((-0.400000000000000)+(((-0.321000000000000)*(x182)))+(pz));
01736 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01737 {
01738 continue;
01739 }
01740 }
01741 
01742 rotationfunction0(vsolutions);
01743 }
01744 }
01745 
01746 } else
01747 {
01748 IKReal x183=((cj27)*(px));
01749 IKReal x184=((py)*(sj27));
01750 IKReal x185=((x184)+(x183));
01751 IKReal x186=((0.100000000000000)+(((-1.00000000000000)*(x185))));
01752 evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
01753 evalcond[1]=x186;
01754 evalcond[2]=((-0.0669590000000000)+(((0.800000000000000)*(pz)))+(((0.200000000000000)*(x183)))+(((0.200000000000000)*(x184)))+(((-1.00000000000000)*(pp))));
01755 evalcond[3]=x186;
01756 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
01757 {
01758 {
01759 IKReal j30array[1], cj30array[1], sj30array[1];
01760 bool j30valid[1]={false};
01761 _nj30 = 1;
01762 if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01763     continue;
01764 j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01765 sj30array[0]=IKsin(j30array[0]);
01766 cj30array[0]=IKcos(j30array[0]);
01767 if( j30array[0] > IKPI )
01768 {
01769     j30array[0]-=IK2PI;
01770 }
01771 else if( j30array[0] < -IKPI )
01772 {    j30array[0]+=IK2PI;
01773 }
01774 j30valid[0] = true;
01775 for(int ij30 = 0; ij30 < 1; ++ij30)
01776 {
01777 if( !j30valid[ij30] )
01778 {
01779     continue;
01780 }
01781 _ij30[0] = ij30; _ij30[1] = -1;
01782 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01783 {
01784 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01785 {
01786     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01787 }
01788 }
01789 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01790 {
01791 IKReal evalcond[3];
01792 IKReal x187=IKcos(j30);
01793 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((-0.321000000000000)*(IKsin(j30)))));
01794 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x187)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01795 evalcond[2]=((-0.400000000000000)+(((-0.321000000000000)*(x187)))+(pz));
01796 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  )
01797 {
01798 continue;
01799 }
01800 }
01801 
01802 rotationfunction0(vsolutions);
01803 }
01804 }
01805 
01806 } else
01807 {
01808 if( 1 )
01809 {
01810 continue;
01811 
01812 } else
01813 {
01814 }
01815 }
01816 }
01817 }
01818 }
01819 }
01820 
01821 } else
01822 {
01823 {
01824 IKReal j30array[1], cj30array[1], sj30array[1];
01825 bool j30valid[1]={false};
01826 _nj30 = 1;
01827 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
01828     continue;
01829 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
01830 sj30array[0]=IKsin(j30array[0]);
01831 cj30array[0]=IKcos(j30array[0]);
01832 if( j30array[0] > IKPI )
01833 {
01834     j30array[0]-=IK2PI;
01835 }
01836 else if( j30array[0] < -IKPI )
01837 {    j30array[0]+=IK2PI;
01838 }
01839 j30valid[0] = true;
01840 for(int ij30 = 0; ij30 < 1; ++ij30)
01841 {
01842 if( !j30valid[ij30] )
01843 {
01844     continue;
01845 }
01846 _ij30[0] = ij30; _ij30[1] = -1;
01847 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01848 {
01849 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01850 {
01851     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01852 }
01853 }
01854 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01855 {
01856 IKReal evalcond[5];
01857 IKReal x188=IKcos(j30);
01858 IKReal x189=IKsin(j30);
01859 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x189)))+(((px)*(sj27))));
01860 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x188)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01861 evalcond[2]=((-0.400000000000000)+(((-0.321000000000000)*(x188)))+(pz));
01862 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj29)*(x189))));
01863 evalcond[4]=((((-1.00000000000000)*(cj29)*(py)*(sj27)))+(((0.100000000000000)*(cj29)))+(((0.321000000000000)*(x189)))+(((-1.00000000000000)*(cj27)*(cj29)*(px)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
01864 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  )
01865 {
01866 continue;
01867 }
01868 }
01869 
01870 rotationfunction0(vsolutions);
01871 }
01872 }
01873 
01874 }
01875 
01876 }
01877 
01878 } else
01879 {
01880 {
01881 IKReal j30array[1], cj30array[1], sj30array[1];
01882 bool j30valid[1]={false};
01883 _nj30 = 1;
01884 if( IKabs(((-0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01885     continue;
01886 j30array[0]=IKatan2(((-0.00311526479750779)*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((100.000000000000)+(((-1000.00000000000)*(cj27)*(px)))+(((-1000.00000000000)*(py)*(sj27)))))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01887 sj30array[0]=IKsin(j30array[0]);
01888 cj30array[0]=IKcos(j30array[0]);
01889 if( j30array[0] > IKPI )
01890 {
01891     j30array[0]-=IK2PI;
01892 }
01893 else if( j30array[0] < -IKPI )
01894 {    j30array[0]+=IK2PI;
01895 }
01896 j30valid[0] = true;
01897 for(int ij30 = 0; ij30 < 1; ++ij30)
01898 {
01899 if( !j30valid[ij30] )
01900 {
01901     continue;
01902 }
01903 _ij30[0] = ij30; _ij30[1] = -1;
01904 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01905 {
01906 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01907 {
01908     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01909 }
01910 }
01911 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01912 {
01913 IKReal evalcond[5];
01914 IKReal x190=IKcos(j30);
01915 IKReal x191=IKsin(j30);
01916 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x191)))+(((px)*(sj27))));
01917 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x190)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01918 evalcond[2]=((-0.400000000000000)+(((-0.321000000000000)*(x190)))+(pz));
01919 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj29)*(x191))));
01920 evalcond[4]=((((-1.00000000000000)*(cj29)*(py)*(sj27)))+(((0.100000000000000)*(cj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(px)))+(((0.321000000000000)*(x191)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
01921 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  )
01922 {
01923 continue;
01924 }
01925 }
01926 
01927 rotationfunction0(vsolutions);
01928 }
01929 }
01930 
01931 }
01932 
01933 }
01934 
01935 } else
01936 {
01937 {
01938 IKReal j30array[1], cj30array[1], sj30array[1];
01939 bool j30valid[1]={false};
01940 _nj30 = 1;
01941 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(pz))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(pz)))))-1) <= IKFAST_SINCOS_THRESH )
01942     continue;
01943 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((3.11526479750779)*(pz)))));
01944 sj30array[0]=IKsin(j30array[0]);
01945 cj30array[0]=IKcos(j30array[0]);
01946 if( j30array[0] > IKPI )
01947 {
01948     j30array[0]-=IK2PI;
01949 }
01950 else if( j30array[0] < -IKPI )
01951 {    j30array[0]+=IK2PI;
01952 }
01953 j30valid[0] = true;
01954 for(int ij30 = 0; ij30 < 1; ++ij30)
01955 {
01956 if( !j30valid[ij30] )
01957 {
01958     continue;
01959 }
01960 _ij30[0] = ij30; _ij30[1] = -1;
01961 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
01962 {
01963 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
01964 {
01965     j30valid[iij30]=false; _ij30[1] = iij30; break; 
01966 }
01967 }
01968 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
01969 {
01970 IKReal evalcond[5];
01971 IKReal x192=IKcos(j30);
01972 IKReal x193=IKsin(j30);
01973 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x193)))+(((px)*(sj27))));
01974 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x192)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01975 evalcond[2]=((-0.400000000000000)+(((-0.321000000000000)*(x192)))+(pz));
01976 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj29)*(x193))));
01977 evalcond[4]=((((-1.00000000000000)*(cj29)*(py)*(sj27)))+(((0.100000000000000)*(cj29)))+(((-1.00000000000000)*(cj27)*(cj29)*(px)))+(((0.321000000000000)*(x193)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
01978 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  )
01979 {
01980 continue;
01981 }
01982 }
01983 
01984 rotationfunction0(vsolutions);
01985 }
01986 }
01987 
01988 }
01989 
01990 }
01991 
01992 } else
01993 {
01994 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.57079632679490)+(j29)), 6.28318530717959)));
01995 evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
01996 evalcond[2]=((((-1.00000000000000)*(cj28)*(pz)))+(((0.100000000000000)*(sj28)))+(((-1.00000000000000)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(px)*(sj28))));
01997 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
01998 {
01999 {
02000 IKReal j30array[1], cj30array[1], sj30array[1];
02001 bool j30valid[1]={false};
02002 _nj30 = 1;
02003 if( IKabs(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02004     continue;
02005 j30array[0]=IKatan2(((((-3.11526479750779)*(px)*(sj27)))+(((3.11526479750779)*(cj27)*(py)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02006 sj30array[0]=IKsin(j30array[0]);
02007 cj30array[0]=IKcos(j30array[0]);
02008 if( j30array[0] > IKPI )
02009 {
02010     j30array[0]-=IK2PI;
02011 }
02012 else if( j30array[0] < -IKPI )
02013 {    j30array[0]+=IK2PI;
02014 }
02015 j30valid[0] = true;
02016 for(int ij30 = 0; ij30 < 1; ++ij30)
02017 {
02018 if( !j30valid[ij30] )
02019 {
02020     continue;
02021 }
02022 _ij30[0] = ij30; _ij30[1] = -1;
02023 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02024 {
02025 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02026 {
02027     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02028 }
02029 }
02030 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02031 {
02032 IKReal evalcond[5];
02033 IKReal x194=IKcos(j30);
02034 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((0.321000000000000)*(IKsin(j30)))));
02035 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x194)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02036 evalcond[2]=((((0.321000000000000)*(sj28)*(x194)))+(pz)+(((0.400000000000000)*(sj28))));
02037 evalcond[3]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x194)))+(((pz)*(sj28))));
02038 evalcond[4]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x194))));
02039 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  )
02040 {
02041 continue;
02042 }
02043 }
02044 
02045 rotationfunction0(vsolutions);
02046 }
02047 }
02048 
02049 } else
02050 {
02051 evalcond[0]=((-3.14159265358979)+(IKfmod(((4.71238898038469)+(j29)), 6.28318530717959)));
02052 evalcond[1]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02053 evalcond[2]=((((cj28)*(pz)))+(((cj27)*(px)*(sj28)))+(((py)*(sj27)*(sj28)))+(((-0.100000000000000)*(sj28))));
02054 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  )
02055 {
02056 {
02057 IKReal j30array[1], cj30array[1], sj30array[1];
02058 bool j30valid[1]={false};
02059 _nj30 = 1;
02060 if( IKabs(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02061     continue;
02062 j30array[0]=IKatan2(((((-3.11526479750779)*(cj27)*(py)))+(((3.11526479750779)*(px)*(sj27)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02063 sj30array[0]=IKsin(j30array[0]);
02064 cj30array[0]=IKcos(j30array[0]);
02065 if( j30array[0] > IKPI )
02066 {
02067     j30array[0]-=IK2PI;
02068 }
02069 else if( j30array[0] < -IKPI )
02070 {    j30array[0]+=IK2PI;
02071 }
02072 j30valid[0] = true;
02073 for(int ij30 = 0; ij30 < 1; ++ij30)
02074 {
02075 if( !j30valid[ij30] )
02076 {
02077     continue;
02078 }
02079 _ij30[0] = ij30; _ij30[1] = -1;
02080 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02081 {
02082 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02083 {
02084     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02085 }
02086 }
02087 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02088 {
02089 IKReal evalcond[5];
02090 IKReal x195=IKcos(j30);
02091 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((px)*(sj27)))+(((-0.321000000000000)*(IKsin(j30)))));
02092 evalcond[1]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x195)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02093 evalcond[2]=((((0.321000000000000)*(sj28)*(x195)))+(pz)+(((0.400000000000000)*(sj28))));
02094 evalcond[3]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x195)))+(((pz)*(sj28))));
02095 evalcond[4]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x195))));
02096 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  )
02097 {
02098 continue;
02099 }
02100 }
02101 
02102 rotationfunction0(vsolutions);
02103 }
02104 }
02105 
02106 } else
02107 {
02108 IKReal x196=((px)*(sj27));
02109 IKReal x197=((cj27)*(py));
02110 IKReal x198=((((-1.00000000000000)*(x197)))+(x196));
02111 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j29)), 6.28318530717959)));
02112 evalcond[1]=x198;
02113 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02114 evalcond[3]=x198;
02115 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
02116 {
02117 {
02118 IKReal j30array[1], cj30array[1], sj30array[1];
02119 bool j30valid[1]={false};
02120 _nj30 = 1;
02121 if( IKabs(((((-3.11526479750779)*(cj28)*(pz)))+(((-3.11526479750779)*(py)*(sj27)*(sj28)))+(((0.311526479750779)*(sj28)))+(((-3.11526479750779)*(cj27)*(px)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-3.11526479750779)*(cj28)*(pz)))+(((-3.11526479750779)*(py)*(sj27)*(sj28)))+(((0.311526479750779)*(sj28)))+(((-3.11526479750779)*(cj27)*(px)*(sj28)))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02122     continue;
02123 j30array[0]=IKatan2(((((-3.11526479750779)*(cj28)*(pz)))+(((-3.11526479750779)*(py)*(sj27)*(sj28)))+(((0.311526479750779)*(sj28)))+(((-3.11526479750779)*(cj27)*(px)*(sj28)))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02124 sj30array[0]=IKsin(j30array[0]);
02125 cj30array[0]=IKcos(j30array[0]);
02126 if( j30array[0] > IKPI )
02127 {
02128     j30array[0]-=IK2PI;
02129 }
02130 else if( j30array[0] < -IKPI )
02131 {    j30array[0]+=IK2PI;
02132 }
02133 j30valid[0] = true;
02134 for(int ij30 = 0; ij30 < 1; ++ij30)
02135 {
02136 if( !j30valid[ij30] )
02137 {
02138     continue;
02139 }
02140 _ij30[0] = ij30; _ij30[1] = -1;
02141 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02142 {
02143 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02144 {
02145     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02146 }
02147 }
02148 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02149 {
02150 IKReal evalcond[5];
02151 IKReal x199=IKcos(j30);
02152 IKReal x200=IKsin(j30);
02153 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x199)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02154 evalcond[1]=((((0.321000000000000)*(cj28)*(x200)))+(((0.321000000000000)*(sj28)*(x199)))+(pz)+(((0.400000000000000)*(sj28))));
02155 evalcond[2]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x199)))+(((pz)*(sj28))));
02156 evalcond[3]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-0.321000000000000)*(sj28)*(x200)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x199))));
02157 evalcond[4]=((((cj28)*(pz)))+(((cj27)*(px)*(sj28)))+(((py)*(sj27)*(sj28)))+(((-0.100000000000000)*(sj28)))+(((0.321000000000000)*(x200))));
02158 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  )
02159 {
02160 continue;
02161 }
02162 }
02163 
02164 rotationfunction0(vsolutions);
02165 }
02166 }
02167 
02168 } else
02169 {
02170 IKReal x201=((cj27)*(py));
02171 IKReal x202=((px)*(sj27));
02172 evalcond[0]=((-3.14159265358979)+(IKfmod(j29, 6.28318530717959)));
02173 evalcond[1]=((((-1.00000000000000)*(x201)))+(x202));
02174 evalcond[2]=((-0.0669590000000000)+(((0.200000000000000)*(cj27)*(px)))+(((-0.800000000000000)*(pz)*(sj28)))+(((0.800000000000000)*(cj27)*(cj28)*(px)))+(((-0.0800000000000000)*(cj28)))+(((0.800000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02175 evalcond[3]=((((-1.00000000000000)*(x202)))+(x201));
02176 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  )
02177 {
02178 {
02179 IKReal dummyeval[1];
02180 IKReal gconst46;
02181 gconst46=IKsign(((((-321.000000000000)*((sj28)*(sj28))))+(((-321.000000000000)*((cj28)*(cj28))))));
02182 dummyeval[0]=((((-1.00000000000000)*((cj28)*(cj28))))+(((-1.00000000000000)*((sj28)*(sj28)))));
02183 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02184 {
02185 {
02186 IKReal dummyeval[1];
02187 dummyeval[0]=cj28;
02188 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02189 {
02190 {
02191 IKReal dummyeval[1];
02192 dummyeval[0]=cj28;
02193 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02194 {
02195 continue;
02196 
02197 } else
02198 {
02199 {
02200 IKReal j30array[1], cj30array[1], sj30array[1];
02201 bool j30valid[1]={false};
02202 _nj30 = 1;
02203 if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(cj28)*(px)*(sj28)))+(((-1000.00000000000)*(pz)*((sj28)*(sj28))))+(((1000.00000000000)*(cj28)*(py)*(sj27)*(sj28)))+(((1000.00000000000)*(pz)))+(((-100.000000000000)*(cj28)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(cj28)*(px)*(sj28)))+(((-1000.00000000000)*(pz)*((sj28)*(sj28))))+(((1000.00000000000)*(cj28)*(py)*(sj27)*(sj28)))+(((1000.00000000000)*(pz)))+(((-100.000000000000)*(cj28)*(sj28)))))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))))-1) <= IKFAST_SINCOS_THRESH )
02204     continue;
02205 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((1000.00000000000)*(cj27)*(cj28)*(px)*(sj28)))+(((-1000.00000000000)*(pz)*((sj28)*(sj28))))+(((1000.00000000000)*(cj28)*(py)*(sj27)*(sj28)))+(((1000.00000000000)*(pz)))+(((-100.000000000000)*(cj28)*(sj28)))))), ((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))));
02206 sj30array[0]=IKsin(j30array[0]);
02207 cj30array[0]=IKcos(j30array[0]);
02208 if( j30array[0] > IKPI )
02209 {
02210     j30array[0]-=IK2PI;
02211 }
02212 else if( j30array[0] < -IKPI )
02213 {    j30array[0]+=IK2PI;
02214 }
02215 j30valid[0] = true;
02216 for(int ij30 = 0; ij30 < 1; ++ij30)
02217 {
02218 if( !j30valid[ij30] )
02219 {
02220     continue;
02221 }
02222 _ij30[0] = ij30; _ij30[1] = -1;
02223 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02224 {
02225 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02226 {
02227     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02228 }
02229 }
02230 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02231 {
02232 IKReal evalcond[5];
02233 IKReal x203=IKcos(j30);
02234 IKReal x204=IKsin(j30);
02235 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x203)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02236 evalcond[1]=((((0.321000000000000)*(sj28)*(x203)))+(((-0.321000000000000)*(cj28)*(x204)))+(pz)+(((0.400000000000000)*(sj28))));
02237 evalcond[2]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x203)))+(((pz)*(sj28))));
02238 evalcond[3]=((0.100000000000000)+(((0.321000000000000)*(sj28)*(x204)))+(((0.321000000000000)*(cj28)*(x203)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27))));
02239 evalcond[4]=((((-1.00000000000000)*(cj28)*(pz)))+(((0.100000000000000)*(sj28)))+(((-1.00000000000000)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(px)*(sj28)))+(((0.321000000000000)*(x204))));
02240 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  )
02241 {
02242 continue;
02243 }
02244 }
02245 
02246 rotationfunction0(vsolutions);
02247 }
02248 }
02249 
02250 }
02251 
02252 }
02253 
02254 } else
02255 {
02256 {
02257 IKReal j30array[1], cj30array[1], sj30array[1];
02258 bool j30valid[1]={false};
02259 _nj30 = 1;
02260 if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((83.6987500000000)*(sj28)))+(((1250.00000000000)*(pp)*(sj28)))+(((1000.00000000000)*(pz)))+(((-250.000000000000)*(cj27)*(px)*(sj28)))+(((-250.000000000000)*(py)*(sj27)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((83.6987500000000)*(sj28)))+(((1250.00000000000)*(pp)*(sj28)))+(((1000.00000000000)*(pz)))+(((-250.000000000000)*(cj27)*(px)*(sj28)))+(((-250.000000000000)*(py)*(sj27)*(sj28)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02261     continue;
02262 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((((83.6987500000000)*(sj28)))+(((1250.00000000000)*(pp)*(sj28)))+(((1000.00000000000)*(pz)))+(((-250.000000000000)*(cj27)*(px)*(sj28)))+(((-250.000000000000)*(py)*(sj27)*(sj28)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02263 sj30array[0]=IKsin(j30array[0]);
02264 cj30array[0]=IKcos(j30array[0]);
02265 if( j30array[0] > IKPI )
02266 {
02267     j30array[0]-=IK2PI;
02268 }
02269 else if( j30array[0] < -IKPI )
02270 {    j30array[0]+=IK2PI;
02271 }
02272 j30valid[0] = true;
02273 for(int ij30 = 0; ij30 < 1; ++ij30)
02274 {
02275 if( !j30valid[ij30] )
02276 {
02277     continue;
02278 }
02279 _ij30[0] = ij30; _ij30[1] = -1;
02280 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02281 {
02282 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02283 {
02284     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02285 }
02286 }
02287 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02288 {
02289 IKReal evalcond[5];
02290 IKReal x205=IKcos(j30);
02291 IKReal x206=IKsin(j30);
02292 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x205)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02293 evalcond[1]=((((0.321000000000000)*(sj28)*(x205)))+(((-0.321000000000000)*(cj28)*(x206)))+(pz)+(((0.400000000000000)*(sj28))));
02294 evalcond[2]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x205)))+(((pz)*(sj28))));
02295 evalcond[3]=((0.100000000000000)+(((0.321000000000000)*(sj28)*(x206)))+(((0.321000000000000)*(cj28)*(x205)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27))));
02296 evalcond[4]=((((-1.00000000000000)*(cj28)*(pz)))+(((0.100000000000000)*(sj28)))+(((-1.00000000000000)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(px)*(sj28)))+(((0.321000000000000)*(x206))));
02297 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  )
02298 {
02299 continue;
02300 }
02301 }
02302 
02303 rotationfunction0(vsolutions);
02304 }
02305 }
02306 
02307 }
02308 
02309 }
02310 
02311 } else
02312 {
02313 {
02314 IKReal j30array[1], cj30array[1], sj30array[1];
02315 bool j30valid[1]={false};
02316 _nj30 = 1;
02317 if( IKabs(((gconst46)*(((((100.000000000000)*(sj28)))+(((-1000.00000000000)*(cj28)*(pz)))+(((-1000.00000000000)*(cj27)*(px)*(sj28)))+(((-1000.00000000000)*(py)*(sj27)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((1000.00000000000)*(pz)*(sj28)))+(((-1000.00000000000)*(cj27)*(cj28)*(px)))+(((100.000000000000)*(cj28)))+(((400.000000000000)*((sj28)*(sj28))))+(((-1000.00000000000)*(cj28)*(py)*(sj27)))+(((400.000000000000)*((cj28)*(cj28)))))))) < IKFAST_ATAN2_MAGTHRESH )
02318     continue;
02319 j30array[0]=IKatan2(((gconst46)*(((((100.000000000000)*(sj28)))+(((-1000.00000000000)*(cj28)*(pz)))+(((-1000.00000000000)*(cj27)*(px)*(sj28)))+(((-1000.00000000000)*(py)*(sj27)*(sj28)))))), ((gconst46)*(((((1000.00000000000)*(pz)*(sj28)))+(((-1000.00000000000)*(cj27)*(cj28)*(px)))+(((100.000000000000)*(cj28)))+(((400.000000000000)*((sj28)*(sj28))))+(((-1000.00000000000)*(cj28)*(py)*(sj27)))+(((400.000000000000)*((cj28)*(cj28))))))));
02320 sj30array[0]=IKsin(j30array[0]);
02321 cj30array[0]=IKcos(j30array[0]);
02322 if( j30array[0] > IKPI )
02323 {
02324     j30array[0]-=IK2PI;
02325 }
02326 else if( j30array[0] < -IKPI )
02327 {    j30array[0]+=IK2PI;
02328 }
02329 j30valid[0] = true;
02330 for(int ij30 = 0; ij30 < 1; ++ij30)
02331 {
02332 if( !j30valid[ij30] )
02333 {
02334     continue;
02335 }
02336 _ij30[0] = ij30; _ij30[1] = -1;
02337 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02338 {
02339 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02340 {
02341     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02342 }
02343 }
02344 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02345 {
02346 IKReal evalcond[5];
02347 IKReal x207=IKcos(j30);
02348 IKReal x208=IKsin(j30);
02349 evalcond[0]=((0.253041000000000)+(((0.200000000000000)*(cj27)*(px)))+(((0.256800000000000)*(x207)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02350 evalcond[1]=((((0.321000000000000)*(sj28)*(x207)))+(((-0.321000000000000)*(cj28)*(x208)))+(pz)+(((0.400000000000000)*(sj28))));
02351 evalcond[2]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((0.321000000000000)*(x207)))+(((pz)*(sj28))));
02352 evalcond[3]=((0.100000000000000)+(((0.321000000000000)*(sj28)*(x208)))+(((0.321000000000000)*(cj28)*(x207)))+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27))));
02353 evalcond[4]=((((-1.00000000000000)*(cj28)*(pz)))+(((0.100000000000000)*(sj28)))+(((-1.00000000000000)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(px)*(sj28)))+(((0.321000000000000)*(x208))));
02354 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  )
02355 {
02356 continue;
02357 }
02358 }
02359 
02360 rotationfunction0(vsolutions);
02361 }
02362 }
02363 
02364 }
02365 
02366 }
02367 
02368 } else
02369 {
02370 if( 1 )
02371 {
02372 continue;
02373 
02374 } else
02375 {
02376 }
02377 }
02378 }
02379 }
02380 }
02381 }
02382 }
02383 }
02384 
02385 } else
02386 {
02387 {
02388 IKReal j30array[1], cj30array[1], sj30array[1];
02389 bool j30valid[1]={false};
02390 _nj30 = 1;
02391 if( IKabs(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((((250.000000000000)*(py)*(sj27)*(sj28)))+(((-1250.00000000000)*(pp)*(sj28)))+(((-83.6987500000000)*(sj28)))+(((-1000.00000000000)*(pz)))+(((250.000000000000)*(cj27)*(px)*(sj28))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((((250.000000000000)*(py)*(sj27)*(sj28)))+(((-1250.00000000000)*(pp)*(sj28)))+(((-83.6987500000000)*(sj28)))+(((-1000.00000000000)*(pz)))+(((250.000000000000)*(cj27)*(px)*(sj28)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02392     continue;
02393 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(cj28) != 0)?((IKReal)1/(cj28)):(IKReal)1.0e30))*(((IKabs(cj29) != 0)?((IKReal)1/(cj29)):(IKReal)1.0e30))*(((((250.000000000000)*(py)*(sj27)*(sj28)))+(((-1250.00000000000)*(pp)*(sj28)))+(((-83.6987500000000)*(sj28)))+(((-1000.00000000000)*(pz)))+(((250.000000000000)*(cj27)*(px)*(sj28)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02394 sj30array[0]=IKsin(j30array[0]);
02395 cj30array[0]=IKcos(j30array[0]);
02396 if( j30array[0] > IKPI )
02397 {
02398     j30array[0]-=IK2PI;
02399 }
02400 else if( j30array[0] < -IKPI )
02401 {    j30array[0]+=IK2PI;
02402 }
02403 j30valid[0] = true;
02404 for(int ij30 = 0; ij30 < 1; ++ij30)
02405 {
02406 if( !j30valid[ij30] )
02407 {
02408     continue;
02409 }
02410 _ij30[0] = ij30; _ij30[1] = -1;
02411 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02412 {
02413 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02414 {
02415     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02416 }
02417 }
02418 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02419 {
02420 IKReal evalcond[6];
02421 IKReal x209=IKsin(j30);
02422 IKReal x210=IKcos(j30);
02423 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x209)))+(((px)*(sj27))));
02424 evalcond[1]=((0.253041000000000)+(((0.256800000000000)*(x210)))+(((0.200000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02425 evalcond[2]=((((0.321000000000000)*(cj28)*(cj29)*(x209)))+(pz)+(((0.321000000000000)*(sj28)*(x210)))+(((0.400000000000000)*(sj28))));
02426 evalcond[3]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((0.321000000000000)*(x210)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((pz)*(sj28))));
02427 evalcond[4]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((-0.321000000000000)*(cj29)*(sj28)*(x209)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x210))));
02428 evalcond[5]=((((cj28)*(cj29)*(pz)))+(((-0.100000000000000)*(cj29)*(sj28)))+(((cj27)*(cj29)*(px)*(sj28)))+(((cj29)*(py)*(sj27)*(sj28)))+(((0.321000000000000)*(x209)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
02429 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  )
02430 {
02431 continue;
02432 }
02433 }
02434 
02435 rotationfunction0(vsolutions);
02436 }
02437 }
02438 
02439 }
02440 
02441 }
02442 
02443 } else
02444 {
02445 {
02446 IKReal j30array[1], cj30array[1], sj30array[1];
02447 bool j30valid[1]={false};
02448 _nj30 = 1;
02449 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))))-1) <= IKFAST_SINCOS_THRESH )
02450     continue;
02451 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-1.24610591900312)+(((3.11526479750779)*(cj27)*(cj28)*(px)))+(((-0.311526479750779)*(cj28)))+(((3.11526479750779)*(cj28)*(py)*(sj27)))+(((-3.11526479750779)*(pz)*(sj28)))));
02452 sj30array[0]=IKsin(j30array[0]);
02453 cj30array[0]=IKcos(j30array[0]);
02454 if( j30array[0] > IKPI )
02455 {
02456     j30array[0]-=IK2PI;
02457 }
02458 else if( j30array[0] < -IKPI )
02459 {    j30array[0]+=IK2PI;
02460 }
02461 j30valid[0] = true;
02462 for(int ij30 = 0; ij30 < 1; ++ij30)
02463 {
02464 if( !j30valid[ij30] )
02465 {
02466     continue;
02467 }
02468 _ij30[0] = ij30; _ij30[1] = -1;
02469 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02470 {
02471 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02472 {
02473     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02474 }
02475 }
02476 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02477 {
02478 IKReal evalcond[6];
02479 IKReal x211=IKsin(j30);
02480 IKReal x212=IKcos(j30);
02481 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x211)))+(((px)*(sj27))));
02482 evalcond[1]=((0.253041000000000)+(((0.256800000000000)*(x212)))+(((0.200000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02483 evalcond[2]=((((0.321000000000000)*(cj28)*(cj29)*(x211)))+(pz)+(((0.321000000000000)*(sj28)*(x212)))+(((0.400000000000000)*(sj28))));
02484 evalcond[3]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((0.321000000000000)*(x212)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((pz)*(sj28))));
02485 evalcond[4]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x212)))+(((-0.321000000000000)*(cj29)*(sj28)*(x211))));
02486 evalcond[5]=((((cj28)*(cj29)*(pz)))+(((-0.100000000000000)*(cj29)*(sj28)))+(((0.321000000000000)*(x211)))+(((cj27)*(cj29)*(px)*(sj28)))+(((cj29)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
02487 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  )
02488 {
02489 continue;
02490 }
02491 }
02492 
02493 rotationfunction0(vsolutions);
02494 }
02495 }
02496 
02497 }
02498 
02499 }
02500 
02501 } else
02502 {
02503 {
02504 IKReal j30array[1], cj30array[1], sj30array[1];
02505 bool j30valid[1]={false};
02506 _nj30 = 1;
02507 if( IKabs(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))))+IKsqr(((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))))-1) <= IKFAST_SINCOS_THRESH )
02508     continue;
02509 j30array[0]=IKatan2(((0.00311526479750779)*(((IKabs(sj29) != 0)?((IKReal)1/(sj29)):(IKReal)1.0e30))*(((((-1000.00000000000)*(px)*(sj27)))+(((1000.00000000000)*(cj27)*(py)))))), ((-0.985362149532710)+(((-0.778816199376947)*(py)*(sj27)))+(((-0.778816199376947)*(cj27)*(px)))+(((3.89408099688474)*(pp)))));
02510 sj30array[0]=IKsin(j30array[0]);
02511 cj30array[0]=IKcos(j30array[0]);
02512 if( j30array[0] > IKPI )
02513 {
02514     j30array[0]-=IK2PI;
02515 }
02516 else if( j30array[0] < -IKPI )
02517 {    j30array[0]+=IK2PI;
02518 }
02519 j30valid[0] = true;
02520 for(int ij30 = 0; ij30 < 1; ++ij30)
02521 {
02522 if( !j30valid[ij30] )
02523 {
02524     continue;
02525 }
02526 _ij30[0] = ij30; _ij30[1] = -1;
02527 for(int iij30 = ij30+1; iij30 < 1; ++iij30)
02528 {
02529 if( j30valid[iij30] && IKabs(cj30array[ij30]-cj30array[iij30]) < IKFAST_SOLUTION_THRESH && IKabs(sj30array[ij30]-sj30array[iij30]) < IKFAST_SOLUTION_THRESH )
02530 {
02531     j30valid[iij30]=false; _ij30[1] = iij30; break; 
02532 }
02533 }
02534 j30 = j30array[ij30]; cj30 = cj30array[ij30]; sj30 = sj30array[ij30];
02535 {
02536 IKReal evalcond[6];
02537 IKReal x213=IKsin(j30);
02538 IKReal x214=IKcos(j30);
02539 evalcond[0]=((((-1.00000000000000)*(cj27)*(py)))+(((0.321000000000000)*(sj29)*(x213)))+(((px)*(sj27))));
02540 evalcond[1]=((0.253041000000000)+(((0.256800000000000)*(x214)))+(((0.200000000000000)*(cj27)*(px)))+(((-1.00000000000000)*(pp)))+(((0.200000000000000)*(py)*(sj27))));
02541 evalcond[2]=((((0.321000000000000)*(cj28)*(cj29)*(x213)))+(pz)+(((0.321000000000000)*(sj28)*(x214)))+(((0.400000000000000)*(sj28))));
02542 evalcond[3]=((0.400000000000000)+(((-1.00000000000000)*(cj28)*(py)*(sj27)))+(((0.321000000000000)*(x214)))+(((-1.00000000000000)*(cj27)*(cj28)*(px)))+(((0.100000000000000)*(cj28)))+(((pz)*(sj28))));
02543 evalcond[4]=((0.100000000000000)+(((-1.00000000000000)*(cj27)*(px)))+(((0.400000000000000)*(cj28)))+(((-1.00000000000000)*(py)*(sj27)))+(((0.321000000000000)*(cj28)*(x214)))+(((-0.321000000000000)*(cj29)*(sj28)*(x213))));
02544 evalcond[5]=((((cj28)*(cj29)*(pz)))+(((-0.100000000000000)*(cj29)*(sj28)))+(((0.321000000000000)*(x213)))+(((cj27)*(cj29)*(px)*(sj28)))+(((cj29)*(py)*(sj27)*(sj28)))+(((-1.00000000000000)*(cj27)*(py)*(sj29)))+(((px)*(sj27)*(sj29))));
02545 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
02546 {
02547 continue;
02548 }
02549 }
02550 
02551 rotationfunction0(vsolutions);
02552 }
02553 }
02554 
02555 }
02556 
02557 }
02558 }
02559 }
02560 
02561 }
02562 
02563 }
02564     }
02565 
02566 }
02567 
02568 }
02569 }
02570 return vsolutions.size()>0;
02571 }
02572 inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
02573 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
02574 IKReal x74=((cj29)*(cj30)*(sj28));
02575 IKReal x75=((cj28)*(sj30));
02576 IKReal x76=((x75)+(x74));
02577 IKReal x77=((-1.00000000000000)*(x76));
02578 IKReal x78=((cj27)*(x77));
02579 IKReal x79=((-1.00000000000000)*(cj30)*(sj27)*(sj29));
02580 IKReal x80=((x79)+(x78));
02581 IKReal x81=((sj28)*(sj30));
02582 IKReal x82=((cj28)*(cj29)*(cj30));
02583 IKReal x83=((x81)+(((-1.00000000000000)*(x82))));
02584 IKReal x84=((cj27)*(cj30)*(sj29));
02585 IKReal x85=((sj27)*(x77));
02586 IKReal x86=((x84)+(x85));
02587 IKReal x87=((cj27)*(sj28)*(sj29));
02588 IKReal x88=((cj29)*(sj27));
02589 IKReal x89=((x87)+(((-1.00000000000000)*(x88))));
02590 IKReal x90=((sj27)*(sj28)*(sj29));
02591 IKReal x91=((cj27)*(cj29));
02592 IKReal x92=((x91)+(x90));
02593 IKReal x93=((cj28)*(cj30));
02594 IKReal x94=((cj29)*(x81));
02595 IKReal x95=((x93)+(((-1.00000000000000)*(x94))));
02596 IKReal x96=((cj30)*(sj28));
02597 IKReal x97=((cj29)*(x75));
02598 IKReal x98=((x97)+(x96));
02599 IKReal x99=((-1.00000000000000)*(x98));
02600 IKReal x100=((cj27)*(sj29)*(sj30));
02601 IKReal x101=((sj27)*(x95));
02602 IKReal x102=((x100)+(x101));
02603 IKReal x103=((cj27)*(x95));
02604 IKReal x104=((sj27)*(sj29)*(sj30));
02605 IKReal x105=((((-1.00000000000000)*(x104)))+(x103));
02606 new_r00=((((r10)*(x86)))+(((r00)*(x80)))+(((r20)*(x83))));
02607 new_r01=((((r21)*(x83)))+(((r11)*(x86)))+(((r01)*(x80))));
02608 new_r02=((((r12)*(x86)))+(((r22)*(x83)))+(((r02)*(x80))));
02609 new_r10=((((r10)*(x92)))+(((r00)*(x89)))+(((cj28)*(r20)*(sj29))));
02610 new_r11=((((r11)*(x92)))+(((cj28)*(r21)*(sj29)))+(((r01)*(x89))));
02611 new_r12=((((r12)*(x92)))+(((cj28)*(r22)*(sj29)))+(((r02)*(x89))));
02612 new_r20=((((r10)*(x102)))+(((r20)*(x99)))+(((r00)*(x105))));
02613 new_r21=((((r21)*(x99)))+(((r01)*(x105)))+(((r11)*(x102))));
02614 new_r22=((((r12)*(x102)))+(((r22)*(x99)))+(((r02)*(((x103)+(((-1.00000000000000)*(x104))))))));
02615 {
02616 IKReal j32array[2], cj32array[2], sj32array[2];
02617 bool j32valid[2]={false};
02618 _nj32 = 2;
02619 cj32array[0]=new_r22;
02620 if( cj32array[0] >= -1-IKFAST_SINCOS_THRESH && cj32array[0] <= 1+IKFAST_SINCOS_THRESH )
02621 {
02622     j32valid[0] = j32valid[1] = true;
02623     j32array[0] = IKacos(cj32array[0]);
02624     sj32array[0] = IKsin(j32array[0]);
02625     cj32array[1] = cj32array[0];
02626     j32array[1] = -j32array[0];
02627     sj32array[1] = -sj32array[0];
02628 }
02629 else if( isnan(cj32array[0]) )
02630 {
02631     // probably any value will work
02632     j32valid[0] = true;
02633     cj32array[0] = 1; sj32array[0] = 0; j32array[0] = 0;
02634 }
02635 for(int ij32 = 0; ij32 < 2; ++ij32)
02636 {
02637 if( !j32valid[ij32] )
02638 {
02639     continue;
02640 }
02641 _ij32[0] = ij32; _ij32[1] = -1;
02642 for(int iij32 = ij32+1; iij32 < 2; ++iij32)
02643 {
02644 if( j32valid[iij32] && IKabs(cj32array[ij32]-cj32array[iij32]) < IKFAST_SOLUTION_THRESH && IKabs(sj32array[ij32]-sj32array[iij32]) < IKFAST_SOLUTION_THRESH )
02645 {
02646     j32valid[iij32]=false; _ij32[1] = iij32; break; 
02647 }
02648 }
02649 j32 = j32array[ij32]; cj32 = cj32array[ij32]; sj32 = sj32array[ij32];
02650 
02651 {
02652 IKReal dummyeval[1];
02653 IKReal gconst50;
02654 gconst50=IKsign(sj32);
02655 dummyeval[0]=sj32;
02656 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02657 {
02658 {
02659 IKReal dummyeval[1];
02660 IKReal gconst49;
02661 gconst49=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
02662 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
02663 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02664 {
02665 {
02666 IKReal evalcond[7];
02667 IKReal x106=((-1.00000000000000)+(new_r22));
02668 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j32)), 6.28318530717959)));
02669 evalcond[1]=x106;
02670 evalcond[2]=new_r20;
02671 evalcond[3]=new_r21;
02672 evalcond[4]=new_r20;
02673 evalcond[5]=new_r21;
02674 evalcond[6]=x106;
02675 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  )
02676 {
02677 {
02678 IKReal j31array[2], cj31array[2], sj31array[2];
02679 bool j31valid[2]={false};
02680 _nj31 = 2;
02681 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
02682     continue;
02683 IKReal x107=IKatan2(new_r02, new_r12);
02684 j31array[0]=((-1.00000000000000)*(x107));
02685 sj31array[0]=IKsin(j31array[0]);
02686 cj31array[0]=IKcos(j31array[0]);
02687 j31array[1]=((3.14159265358979)+(((-1.00000000000000)*(x107))));
02688 sj31array[1]=IKsin(j31array[1]);
02689 cj31array[1]=IKcos(j31array[1]);
02690 if( j31array[0] > IKPI )
02691 {
02692     j31array[0]-=IK2PI;
02693 }
02694 else if( j31array[0] < -IKPI )
02695 {    j31array[0]+=IK2PI;
02696 }
02697 j31valid[0] = true;
02698 if( j31array[1] > IKPI )
02699 {
02700     j31array[1]-=IK2PI;
02701 }
02702 else if( j31array[1] < -IKPI )
02703 {    j31array[1]+=IK2PI;
02704 }
02705 j31valid[1] = true;
02706 for(int ij31 = 0; ij31 < 2; ++ij31)
02707 {
02708 if( !j31valid[ij31] )
02709 {
02710     continue;
02711 }
02712 _ij31[0] = ij31; _ij31[1] = -1;
02713 for(int iij31 = ij31+1; iij31 < 2; ++iij31)
02714 {
02715 if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
02716 {
02717     j31valid[iij31]=false; _ij31[1] = iij31; break; 
02718 }
02719 }
02720 j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];
02721 {
02722 IKReal evalcond[1];
02723 evalcond[0]=((((new_r12)*(IKcos(j31))))+(((-1.00000000000000)*(new_r02)*(IKsin(j31)))));
02724 if( IKabs(evalcond[0]) > 0.000001  )
02725 {
02726 continue;
02727 }
02728 }
02729 
02730 {
02731 IKReal j33array[1], cj33array[1], sj33array[1];
02732 bool j33valid[1]={false};
02733 _nj33 = 1;
02734 if( IKabs(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj31)*(new_r00)))+(((new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))))+IKsqr(((((cj31)*(new_r00)))+(((new_r10)*(sj31)))))-1) <= IKFAST_SINCOS_THRESH )
02735     continue;
02736 j33array[0]=IKatan2(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))), ((((cj31)*(new_r00)))+(((new_r10)*(sj31)))));
02737 sj33array[0]=IKsin(j33array[0]);
02738 cj33array[0]=IKcos(j33array[0]);
02739 if( j33array[0] > IKPI )
02740 {
02741     j33array[0]-=IK2PI;
02742 }
02743 else if( j33array[0] < -IKPI )
02744 {    j33array[0]+=IK2PI;
02745 }
02746 j33valid[0] = true;
02747 for(int ij33 = 0; ij33 < 1; ++ij33)
02748 {
02749 if( !j33valid[ij33] )
02750 {
02751     continue;
02752 }
02753 _ij33[0] = ij33; _ij33[1] = -1;
02754 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
02755 {
02756 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
02757 {
02758     j33valid[iij33]=false; _ij33[1] = iij33; break; 
02759 }
02760 }
02761 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
02762 {
02763 IKReal evalcond[4];
02764 IKReal x108=IKsin(j33);
02765 IKReal x109=IKcos(j33);
02766 evalcond[0]=((((cj31)*(new_r10)))+(((-1.00000000000000)*(x108)))+(((-1.00000000000000)*(new_r00)*(sj31))));
02767 evalcond[1]=((((cj31)*(new_r11)))+(((-1.00000000000000)*(x109)))+(((-1.00000000000000)*(new_r01)*(sj31))));
02768 evalcond[2]=((((cj31)*(new_r01)))+(((new_r11)*(sj31)))+(x108));
02769 evalcond[3]=((((cj31)*(new_r00)))+(((-1.00000000000000)*(x109)))+(((new_r10)*(sj31))));
02770 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02771 {
02772 continue;
02773 }
02774 }
02775 
02776 {
02777 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
02778 solution.basesol.resize(7);
02779 solution.basesol[0].foffset = j27;
02780 solution.basesol[0].indices[0] = _ij27[0];
02781 solution.basesol[0].indices[1] = _ij27[1];
02782 solution.basesol[0].maxsolutions = _nj27;
02783 solution.basesol[1].foffset = j28;
02784 solution.basesol[1].indices[0] = _ij28[0];
02785 solution.basesol[1].indices[1] = _ij28[1];
02786 solution.basesol[1].maxsolutions = _nj28;
02787 solution.basesol[2].foffset = j29;
02788 solution.basesol[2].indices[0] = _ij29[0];
02789 solution.basesol[2].indices[1] = _ij29[1];
02790 solution.basesol[2].maxsolutions = _nj29;
02791 solution.basesol[3].foffset = j30;
02792 solution.basesol[3].indices[0] = _ij30[0];
02793 solution.basesol[3].indices[1] = _ij30[1];
02794 solution.basesol[3].maxsolutions = _nj30;
02795 solution.basesol[4].foffset = j31;
02796 solution.basesol[4].indices[0] = _ij31[0];
02797 solution.basesol[4].indices[1] = _ij31[1];
02798 solution.basesol[4].maxsolutions = _nj31;
02799 solution.basesol[5].foffset = j32;
02800 solution.basesol[5].indices[0] = _ij32[0];
02801 solution.basesol[5].indices[1] = _ij32[1];
02802 solution.basesol[5].maxsolutions = _nj32;
02803 solution.basesol[6].foffset = j33;
02804 solution.basesol[6].indices[0] = _ij33[0];
02805 solution.basesol[6].indices[1] = _ij33[1];
02806 solution.basesol[6].maxsolutions = _nj33;
02807 solution.vfree.resize(0);
02808 }
02809 }
02810 }
02811 }
02812 }
02813 
02814 } else
02815 {
02816 evalcond[0]=((-3.14159265358979)+(IKfmod(j32, 6.28318530717959)));
02817 evalcond[1]=((1.00000000000000)+(new_r22));
02818 evalcond[2]=new_r20;
02819 evalcond[3]=new_r21;
02820 evalcond[4]=((-1.00000000000000)*(new_r20));
02821 evalcond[5]=((-1.00000000000000)*(new_r21));
02822 evalcond[6]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
02823 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  )
02824 {
02825 {
02826 IKReal j31array[2], cj31array[2], sj31array[2];
02827 bool j31valid[2]={false};
02828 _nj31 = 2;
02829 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
02830     continue;
02831 IKReal x110=IKatan2(new_r02, new_r12);
02832 j31array[0]=((-1.00000000000000)*(x110));
02833 sj31array[0]=IKsin(j31array[0]);
02834 cj31array[0]=IKcos(j31array[0]);
02835 j31array[1]=((3.14159265358979)+(((-1.00000000000000)*(x110))));
02836 sj31array[1]=IKsin(j31array[1]);
02837 cj31array[1]=IKcos(j31array[1]);
02838 if( j31array[0] > IKPI )
02839 {
02840     j31array[0]-=IK2PI;
02841 }
02842 else if( j31array[0] < -IKPI )
02843 {    j31array[0]+=IK2PI;
02844 }
02845 j31valid[0] = true;
02846 if( j31array[1] > IKPI )
02847 {
02848     j31array[1]-=IK2PI;
02849 }
02850 else if( j31array[1] < -IKPI )
02851 {    j31array[1]+=IK2PI;
02852 }
02853 j31valid[1] = true;
02854 for(int ij31 = 0; ij31 < 2; ++ij31)
02855 {
02856 if( !j31valid[ij31] )
02857 {
02858     continue;
02859 }
02860 _ij31[0] = ij31; _ij31[1] = -1;
02861 for(int iij31 = ij31+1; iij31 < 2; ++iij31)
02862 {
02863 if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
02864 {
02865     j31valid[iij31]=false; _ij31[1] = iij31; break; 
02866 }
02867 }
02868 j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];
02869 {
02870 IKReal evalcond[1];
02871 evalcond[0]=((((new_r12)*(IKcos(j31))))+(((-1.00000000000000)*(new_r02)*(IKsin(j31)))));
02872 if( IKabs(evalcond[0]) > 0.000001  )
02873 {
02874 continue;
02875 }
02876 }
02877 
02878 {
02879 IKReal j33array[1], cj33array[1], sj33array[1];
02880 bool j33valid[1]={false};
02881 _nj33 = 1;
02882 if( IKabs(((((cj31)*(new_r01)))+(((new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))))+IKsqr(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))))-1) <= IKFAST_SINCOS_THRESH )
02883     continue;
02884 j33array[0]=IKatan2(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))), ((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))));
02885 sj33array[0]=IKsin(j33array[0]);
02886 cj33array[0]=IKcos(j33array[0]);
02887 if( j33array[0] > IKPI )
02888 {
02889     j33array[0]-=IK2PI;
02890 }
02891 else if( j33array[0] < -IKPI )
02892 {    j33array[0]+=IK2PI;
02893 }
02894 j33valid[0] = true;
02895 for(int ij33 = 0; ij33 < 1; ++ij33)
02896 {
02897 if( !j33valid[ij33] )
02898 {
02899     continue;
02900 }
02901 _ij33[0] = ij33; _ij33[1] = -1;
02902 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
02903 {
02904 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
02905 {
02906     j33valid[iij33]=false; _ij33[1] = iij33; break; 
02907 }
02908 }
02909 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
02910 {
02911 IKReal evalcond[4];
02912 IKReal x111=IKsin(j33);
02913 IKReal x112=IKcos(j33);
02914 evalcond[0]=((((cj31)*(new_r10)))+(((-1.00000000000000)*(x111)))+(((-1.00000000000000)*(new_r00)*(sj31))));
02915 evalcond[1]=((((cj31)*(new_r11)))+(((-1.00000000000000)*(x112)))+(((-1.00000000000000)*(new_r01)*(sj31))));
02916 evalcond[2]=((((cj31)*(new_r01)))+(((-1.00000000000000)*(x111)))+(((new_r11)*(sj31))));
02917 evalcond[3]=((((cj31)*(new_r00)))+(x112)+(((new_r10)*(sj31))));
02918 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02919 {
02920 continue;
02921 }
02922 }
02923 
02924 {
02925 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
02926 solution.basesol.resize(7);
02927 solution.basesol[0].foffset = j27;
02928 solution.basesol[0].indices[0] = _ij27[0];
02929 solution.basesol[0].indices[1] = _ij27[1];
02930 solution.basesol[0].maxsolutions = _nj27;
02931 solution.basesol[1].foffset = j28;
02932 solution.basesol[1].indices[0] = _ij28[0];
02933 solution.basesol[1].indices[1] = _ij28[1];
02934 solution.basesol[1].maxsolutions = _nj28;
02935 solution.basesol[2].foffset = j29;
02936 solution.basesol[2].indices[0] = _ij29[0];
02937 solution.basesol[2].indices[1] = _ij29[1];
02938 solution.basesol[2].maxsolutions = _nj29;
02939 solution.basesol[3].foffset = j30;
02940 solution.basesol[3].indices[0] = _ij30[0];
02941 solution.basesol[3].indices[1] = _ij30[1];
02942 solution.basesol[3].maxsolutions = _nj30;
02943 solution.basesol[4].foffset = j31;
02944 solution.basesol[4].indices[0] = _ij31[0];
02945 solution.basesol[4].indices[1] = _ij31[1];
02946 solution.basesol[4].maxsolutions = _nj31;
02947 solution.basesol[5].foffset = j32;
02948 solution.basesol[5].indices[0] = _ij32[0];
02949 solution.basesol[5].indices[1] = _ij32[1];
02950 solution.basesol[5].maxsolutions = _nj32;
02951 solution.basesol[6].foffset = j33;
02952 solution.basesol[6].indices[0] = _ij33[0];
02953 solution.basesol[6].indices[1] = _ij33[1];
02954 solution.basesol[6].maxsolutions = _nj33;
02955 solution.vfree.resize(0);
02956 }
02957 }
02958 }
02959 }
02960 }
02961 
02962 } else
02963 {
02964 if( 1 )
02965 {
02966 continue;
02967 
02968 } else
02969 {
02970 }
02971 }
02972 }
02973 }
02974 
02975 } else
02976 {
02977 {
02978 IKReal j31array[1], cj31array[1], sj31array[1];
02979 bool j31valid[1]={false};
02980 _nj31 = 1;
02981 if( IKabs(((gconst49)*(new_r12)*(sj32))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(new_r02)*(sj32))) < IKFAST_ATAN2_MAGTHRESH )
02982     continue;
02983 j31array[0]=IKatan2(((gconst49)*(new_r12)*(sj32)), ((gconst49)*(new_r02)*(sj32)));
02984 sj31array[0]=IKsin(j31array[0]);
02985 cj31array[0]=IKcos(j31array[0]);
02986 if( j31array[0] > IKPI )
02987 {
02988     j31array[0]-=IK2PI;
02989 }
02990 else if( j31array[0] < -IKPI )
02991 {    j31array[0]+=IK2PI;
02992 }
02993 j31valid[0] = true;
02994 for(int ij31 = 0; ij31 < 1; ++ij31)
02995 {
02996 if( !j31valid[ij31] )
02997 {
02998     continue;
02999 }
03000 _ij31[0] = ij31; _ij31[1] = -1;
03001 for(int iij31 = ij31+1; iij31 < 1; ++iij31)
03002 {
03003 if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
03004 {
03005     j31valid[iij31]=false; _ij31[1] = iij31; break; 
03006 }
03007 }
03008 j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];
03009 {
03010 IKReal evalcond[6];
03011 IKReal x113=IKsin(j31);
03012 IKReal x114=IKcos(j31);
03013 evalcond[0]=((((-1.00000000000000)*(new_r02)*(x113)))+(((new_r12)*(x114))));
03014 evalcond[1]=((((-1.00000000000000)*(sj32)))+(((new_r02)*(x114)))+(((new_r12)*(x113))));
03015 evalcond[2]=((((new_r10)*(sj32)*(x113)))+(((cj32)*(new_r20)))+(((new_r00)*(sj32)*(x114))));
03016 evalcond[3]=((((new_r11)*(sj32)*(x113)))+(((new_r01)*(sj32)*(x114)))+(((cj32)*(new_r21))));
03017 evalcond[4]=((-1.00000000000000)+(((new_r02)*(sj32)*(x114)))+(((cj32)*(new_r22)))+(((new_r12)*(sj32)*(x113))));
03018 evalcond[5]=((((cj32)*(new_r02)*(x114)))+(((cj32)*(new_r12)*(x113)))+(((-1.00000000000000)*(new_r22)*(sj32))));
03019 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  )
03020 {
03021 continue;
03022 }
03023 }
03024 
03025 {
03026 IKReal dummyeval[1];
03027 IKReal gconst51;
03028 gconst51=IKsign(sj32);
03029 dummyeval[0]=sj32;
03030 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03031 {
03032 {
03033 IKReal evalcond[11];
03034 IKReal x115=((cj31)*(new_r12));
03035 IKReal x116=((new_r02)*(sj31));
03036 IKReal x117=((((-1.00000000000000)*(x116)))+(x115));
03037 IKReal x118=((-1.00000000000000)+(new_r22));
03038 IKReal x119=((cj31)*(new_r02));
03039 IKReal x120=((new_r12)*(sj31));
03040 IKReal x121=((x120)+(x119));
03041 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j32)), 6.28318530717959)));
03042 evalcond[1]=x118;
03043 evalcond[2]=new_r20;
03044 evalcond[3]=new_r21;
03045 evalcond[4]=x117;
03046 evalcond[5]=x117;
03047 evalcond[6]=x121;
03048 evalcond[7]=new_r20;
03049 evalcond[8]=new_r21;
03050 evalcond[9]=x118;
03051 evalcond[10]=x121;
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 j33array[1], cj33array[1], sj33array[1];
03056 bool j33valid[1]={false};
03057 _nj33 = 1;
03058 if( IKabs(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj31)*(new_r00)))+(((new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))))+IKsqr(((((cj31)*(new_r00)))+(((new_r10)*(sj31)))))-1) <= IKFAST_SINCOS_THRESH )
03059     continue;
03060 j33array[0]=IKatan2(((((-1.00000000000000)*(cj31)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj31)))), ((((cj31)*(new_r00)))+(((new_r10)*(sj31)))));
03061 sj33array[0]=IKsin(j33array[0]);
03062 cj33array[0]=IKcos(j33array[0]);
03063 if( j33array[0] > IKPI )
03064 {
03065     j33array[0]-=IK2PI;
03066 }
03067 else if( j33array[0] < -IKPI )
03068 {    j33array[0]+=IK2PI;
03069 }
03070 j33valid[0] = true;
03071 for(int ij33 = 0; ij33 < 1; ++ij33)
03072 {
03073 if( !j33valid[ij33] )
03074 {
03075     continue;
03076 }
03077 _ij33[0] = ij33; _ij33[1] = -1;
03078 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
03079 {
03080 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
03081 {
03082     j33valid[iij33]=false; _ij33[1] = iij33; break; 
03083 }
03084 }
03085 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
03086 {
03087 IKReal evalcond[4];
03088 IKReal x122=IKsin(j33);
03089 IKReal x123=IKcos(j33);
03090 evalcond[0]=((((cj31)*(new_r10)))+(((-1.00000000000000)*(x122)))+(((-1.00000000000000)*(new_r00)*(sj31))));
03091 evalcond[1]=((((cj31)*(new_r11)))+(((-1.00000000000000)*(x123)))+(((-1.00000000000000)*(new_r01)*(sj31))));
03092 evalcond[2]=((((cj31)*(new_r01)))+(((new_r11)*(sj31)))+(x122));
03093 evalcond[3]=((((cj31)*(new_r00)))+(((-1.00000000000000)*(x123)))+(((new_r10)*(sj31))));
03094 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03095 {
03096 continue;
03097 }
03098 }
03099 
03100 {
03101 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
03102 solution.basesol.resize(7);
03103 solution.basesol[0].foffset = j27;
03104 solution.basesol[0].indices[0] = _ij27[0];
03105 solution.basesol[0].indices[1] = _ij27[1];
03106 solution.basesol[0].maxsolutions = _nj27;
03107 solution.basesol[1].foffset = j28;
03108 solution.basesol[1].indices[0] = _ij28[0];
03109 solution.basesol[1].indices[1] = _ij28[1];
03110 solution.basesol[1].maxsolutions = _nj28;
03111 solution.basesol[2].foffset = j29;
03112 solution.basesol[2].indices[0] = _ij29[0];
03113 solution.basesol[2].indices[1] = _ij29[1];
03114 solution.basesol[2].maxsolutions = _nj29;
03115 solution.basesol[3].foffset = j30;
03116 solution.basesol[3].indices[0] = _ij30[0];
03117 solution.basesol[3].indices[1] = _ij30[1];
03118 solution.basesol[3].maxsolutions = _nj30;
03119 solution.basesol[4].foffset = j31;
03120 solution.basesol[4].indices[0] = _ij31[0];
03121 solution.basesol[4].indices[1] = _ij31[1];
03122 solution.basesol[4].maxsolutions = _nj31;
03123 solution.basesol[5].foffset = j32;
03124 solution.basesol[5].indices[0] = _ij32[0];
03125 solution.basesol[5].indices[1] = _ij32[1];
03126 solution.basesol[5].maxsolutions = _nj32;
03127 solution.basesol[6].foffset = j33;
03128 solution.basesol[6].indices[0] = _ij33[0];
03129 solution.basesol[6].indices[1] = _ij33[1];
03130 solution.basesol[6].maxsolutions = _nj33;
03131 solution.vfree.resize(0);
03132 }
03133 }
03134 }
03135 
03136 } else
03137 {
03138 IKReal x124=((cj31)*(new_r12));
03139 IKReal x125=((new_r02)*(sj31));
03140 IKReal x126=((((-1.00000000000000)*(x125)))+(x124));
03141 IKReal x127=((cj31)*(new_r02));
03142 IKReal x128=((new_r12)*(sj31));
03143 IKReal x129=((x127)+(x128));
03144 evalcond[0]=((-3.14159265358979)+(IKfmod(j32, 6.28318530717959)));
03145 evalcond[1]=((1.00000000000000)+(new_r22));
03146 evalcond[2]=new_r20;
03147 evalcond[3]=new_r21;
03148 evalcond[4]=x126;
03149 evalcond[5]=x126;
03150 evalcond[6]=x129;
03151 evalcond[7]=((-1.00000000000000)*(new_r20));
03152 evalcond[8]=((-1.00000000000000)*(new_r21));
03153 evalcond[9]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
03154 evalcond[10]=((-1.00000000000000)*(x129));
03155 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  )
03156 {
03157 {
03158 IKReal j33array[1], cj33array[1], sj33array[1];
03159 bool j33valid[1]={false};
03160 _nj33 = 1;
03161 if( IKabs(((((cj31)*(new_r01)))+(((new_r11)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))))+IKsqr(((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))))-1) <= IKFAST_SINCOS_THRESH )
03162     continue;
03163 j33array[0]=IKatan2(((((cj31)*(new_r01)))+(((new_r11)*(sj31)))), ((((-1.00000000000000)*(cj31)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj31)))));
03164 sj33array[0]=IKsin(j33array[0]);
03165 cj33array[0]=IKcos(j33array[0]);
03166 if( j33array[0] > IKPI )
03167 {
03168     j33array[0]-=IK2PI;
03169 }
03170 else if( j33array[0] < -IKPI )
03171 {    j33array[0]+=IK2PI;
03172 }
03173 j33valid[0] = true;
03174 for(int ij33 = 0; ij33 < 1; ++ij33)
03175 {
03176 if( !j33valid[ij33] )
03177 {
03178     continue;
03179 }
03180 _ij33[0] = ij33; _ij33[1] = -1;
03181 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
03182 {
03183 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
03184 {
03185     j33valid[iij33]=false; _ij33[1] = iij33; break; 
03186 }
03187 }
03188 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
03189 {
03190 IKReal evalcond[4];
03191 IKReal x130=IKsin(j33);
03192 IKReal x131=IKcos(j33);
03193 evalcond[0]=((((cj31)*(new_r10)))+(((-1.00000000000000)*(x130)))+(((-1.00000000000000)*(new_r00)*(sj31))));
03194 evalcond[1]=((((cj31)*(new_r11)))+(((-1.00000000000000)*(x131)))+(((-1.00000000000000)*(new_r01)*(sj31))));
03195 evalcond[2]=((((cj31)*(new_r01)))+(((new_r11)*(sj31)))+(((-1.00000000000000)*(x130))));
03196 evalcond[3]=((((cj31)*(new_r00)))+(x131)+(((new_r10)*(sj31))));
03197 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03198 {
03199 continue;
03200 }
03201 }
03202 
03203 {
03204 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
03205 solution.basesol.resize(7);
03206 solution.basesol[0].foffset = j27;
03207 solution.basesol[0].indices[0] = _ij27[0];
03208 solution.basesol[0].indices[1] = _ij27[1];
03209 solution.basesol[0].maxsolutions = _nj27;
03210 solution.basesol[1].foffset = j28;
03211 solution.basesol[1].indices[0] = _ij28[0];
03212 solution.basesol[1].indices[1] = _ij28[1];
03213 solution.basesol[1].maxsolutions = _nj28;
03214 solution.basesol[2].foffset = j29;
03215 solution.basesol[2].indices[0] = _ij29[0];
03216 solution.basesol[2].indices[1] = _ij29[1];
03217 solution.basesol[2].maxsolutions = _nj29;
03218 solution.basesol[3].foffset = j30;
03219 solution.basesol[3].indices[0] = _ij30[0];
03220 solution.basesol[3].indices[1] = _ij30[1];
03221 solution.basesol[3].maxsolutions = _nj30;
03222 solution.basesol[4].foffset = j31;
03223 solution.basesol[4].indices[0] = _ij31[0];
03224 solution.basesol[4].indices[1] = _ij31[1];
03225 solution.basesol[4].maxsolutions = _nj31;
03226 solution.basesol[5].foffset = j32;
03227 solution.basesol[5].indices[0] = _ij32[0];
03228 solution.basesol[5].indices[1] = _ij32[1];
03229 solution.basesol[5].maxsolutions = _nj32;
03230 solution.basesol[6].foffset = j33;
03231 solution.basesol[6].indices[0] = _ij33[0];
03232 solution.basesol[6].indices[1] = _ij33[1];
03233 solution.basesol[6].maxsolutions = _nj33;
03234 solution.vfree.resize(0);
03235 }
03236 }
03237 }
03238 
03239 } else
03240 {
03241 if( 1 )
03242 {
03243 continue;
03244 
03245 } else
03246 {
03247 }
03248 }
03249 }
03250 }
03251 
03252 } else
03253 {
03254 {
03255 IKReal j33array[1], cj33array[1], sj33array[1];
03256 bool j33valid[1]={false};
03257 _nj33 = 1;
03258 if( IKabs(((gconst51)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst51)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
03259     continue;
03260 j33array[0]=IKatan2(((gconst51)*(new_r21)), ((-1.00000000000000)*(gconst51)*(new_r20)));
03261 sj33array[0]=IKsin(j33array[0]);
03262 cj33array[0]=IKcos(j33array[0]);
03263 if( j33array[0] > IKPI )
03264 {
03265     j33array[0]-=IK2PI;
03266 }
03267 else if( j33array[0] < -IKPI )
03268 {    j33array[0]+=IK2PI;
03269 }
03270 j33valid[0] = true;
03271 for(int ij33 = 0; ij33 < 1; ++ij33)
03272 {
03273 if( !j33valid[ij33] )
03274 {
03275     continue;
03276 }
03277 _ij33[0] = ij33; _ij33[1] = -1;
03278 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
03279 {
03280 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
03281 {
03282     j33valid[iij33]=false; _ij33[1] = iij33; break; 
03283 }
03284 }
03285 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
03286 {
03287 IKReal evalcond[8];
03288 IKReal x132=IKsin(j33);
03289 IKReal x133=IKcos(j33);
03290 evalcond[0]=((((sj32)*(x133)))+(new_r20));
03291 evalcond[1]=((((-1.00000000000000)*(sj32)*(x132)))+(new_r21));
03292 evalcond[2]=((((cj31)*(new_r10)))+(((-1.00000000000000)*(x132)))+(((-1.00000000000000)*(new_r00)*(sj31))));
03293 evalcond[3]=((((cj31)*(new_r11)))+(((-1.00000000000000)*(x133)))+(((-1.00000000000000)*(new_r01)*(sj31))));
03294 evalcond[4]=((((cj31)*(new_r01)))+(((new_r11)*(sj31)))+(((cj32)*(x132))));
03295 evalcond[5]=((((-1.00000000000000)*(cj32)*(x133)))+(((cj31)*(new_r00)))+(((new_r10)*(sj31))));
03296 evalcond[6]=((((cj32)*(new_r11)*(sj31)))+(x132)+(((-1.00000000000000)*(new_r21)*(sj32)))+(((cj31)*(cj32)*(new_r01))));
03297 evalcond[7]=((((-1.00000000000000)*(new_r20)*(sj32)))+(((cj32)*(new_r10)*(sj31)))+(((-1.00000000000000)*(x133)))+(((cj31)*(cj32)*(new_r00))));
03298 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  )
03299 {
03300 continue;
03301 }
03302 }
03303 
03304 {
03305 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
03306 solution.basesol.resize(7);
03307 solution.basesol[0].foffset = j27;
03308 solution.basesol[0].indices[0] = _ij27[0];
03309 solution.basesol[0].indices[1] = _ij27[1];
03310 solution.basesol[0].maxsolutions = _nj27;
03311 solution.basesol[1].foffset = j28;
03312 solution.basesol[1].indices[0] = _ij28[0];
03313 solution.basesol[1].indices[1] = _ij28[1];
03314 solution.basesol[1].maxsolutions = _nj28;
03315 solution.basesol[2].foffset = j29;
03316 solution.basesol[2].indices[0] = _ij29[0];
03317 solution.basesol[2].indices[1] = _ij29[1];
03318 solution.basesol[2].maxsolutions = _nj29;
03319 solution.basesol[3].foffset = j30;
03320 solution.basesol[3].indices[0] = _ij30[0];
03321 solution.basesol[3].indices[1] = _ij30[1];
03322 solution.basesol[3].maxsolutions = _nj30;
03323 solution.basesol[4].foffset = j31;
03324 solution.basesol[4].indices[0] = _ij31[0];
03325 solution.basesol[4].indices[1] = _ij31[1];
03326 solution.basesol[4].maxsolutions = _nj31;
03327 solution.basesol[5].foffset = j32;
03328 solution.basesol[5].indices[0] = _ij32[0];
03329 solution.basesol[5].indices[1] = _ij32[1];
03330 solution.basesol[5].maxsolutions = _nj32;
03331 solution.basesol[6].foffset = j33;
03332 solution.basesol[6].indices[0] = _ij33[0];
03333 solution.basesol[6].indices[1] = _ij33[1];
03334 solution.basesol[6].maxsolutions = _nj33;
03335 solution.vfree.resize(0);
03336 }
03337 }
03338 }
03339 
03340 }
03341 
03342 }
03343 }
03344 }
03345 
03346 }
03347 
03348 }
03349 
03350 } else
03351 {
03352 {
03353 IKReal j33array[1], cj33array[1], sj33array[1];
03354 bool j33valid[1]={false};
03355 _nj33 = 1;
03356 if( IKabs(((gconst50)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.00000000000000)*(gconst50)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
03357     continue;
03358 j33array[0]=IKatan2(((gconst50)*(new_r21)), ((-1.00000000000000)*(gconst50)*(new_r20)));
03359 sj33array[0]=IKsin(j33array[0]);
03360 cj33array[0]=IKcos(j33array[0]);
03361 if( j33array[0] > IKPI )
03362 {
03363     j33array[0]-=IK2PI;
03364 }
03365 else if( j33array[0] < -IKPI )
03366 {    j33array[0]+=IK2PI;
03367 }
03368 j33valid[0] = true;
03369 for(int ij33 = 0; ij33 < 1; ++ij33)
03370 {
03371 if( !j33valid[ij33] )
03372 {
03373     continue;
03374 }
03375 _ij33[0] = ij33; _ij33[1] = -1;
03376 for(int iij33 = ij33+1; iij33 < 1; ++iij33)
03377 {
03378 if( j33valid[iij33] && IKabs(cj33array[ij33]-cj33array[iij33]) < IKFAST_SOLUTION_THRESH && IKabs(sj33array[ij33]-sj33array[iij33]) < IKFAST_SOLUTION_THRESH )
03379 {
03380     j33valid[iij33]=false; _ij33[1] = iij33; break; 
03381 }
03382 }
03383 j33 = j33array[ij33]; cj33 = cj33array[ij33]; sj33 = sj33array[ij33];
03384 {
03385 IKReal evalcond[2];
03386 evalcond[0]=((((sj32)*(IKcos(j33))))+(new_r20));
03387 evalcond[1]=((((-1.00000000000000)*(sj32)*(IKsin(j33))))+(new_r21));
03388 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
03389 {
03390 continue;
03391 }
03392 }
03393 
03394 {
03395 IKReal dummyeval[1];
03396 IKReal gconst52;
03397 gconst52=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
03398 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
03399 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03400 {
03401 continue;
03402 
03403 } else
03404 {
03405 {
03406 IKReal j31array[1], cj31array[1], sj31array[1];
03407 bool j31valid[1]={false};
03408 _nj31 = 1;
03409 if( IKabs(((gconst52)*(new_r12)*(sj32))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(new_r02)*(sj32))) < IKFAST_ATAN2_MAGTHRESH )
03410     continue;
03411 j31array[0]=IKatan2(((gconst52)*(new_r12)*(sj32)), ((gconst52)*(new_r02)*(sj32)));
03412 sj31array[0]=IKsin(j31array[0]);
03413 cj31array[0]=IKcos(j31array[0]);
03414 if( j31array[0] > IKPI )
03415 {
03416     j31array[0]-=IK2PI;
03417 }
03418 else if( j31array[0] < -IKPI )
03419 {    j31array[0]+=IK2PI;
03420 }
03421 j31valid[0] = true;
03422 for(int ij31 = 0; ij31 < 1; ++ij31)
03423 {
03424 if( !j31valid[ij31] )
03425 {
03426     continue;
03427 }
03428 _ij31[0] = ij31; _ij31[1] = -1;
03429 for(int iij31 = ij31+1; iij31 < 1; ++iij31)
03430 {
03431 if( j31valid[iij31] && IKabs(cj31array[ij31]-cj31array[iij31]) < IKFAST_SOLUTION_THRESH && IKabs(sj31array[ij31]-sj31array[iij31]) < IKFAST_SOLUTION_THRESH )
03432 {
03433     j31valid[iij31]=false; _ij31[1] = iij31; break; 
03434 }
03435 }
03436 j31 = j31array[ij31]; cj31 = cj31array[ij31]; sj31 = sj31array[ij31];
03437 {
03438 IKReal evalcond[12];
03439 IKReal x134=IKsin(j31);
03440 IKReal x135=IKcos(j31);
03441 evalcond[0]=((((new_r12)*(x135)))+(((-1.00000000000000)*(new_r02)*(x134))));
03442 evalcond[1]=((((new_r12)*(x134)))+(((-1.00000000000000)*(sj32)))+(((new_r02)*(x135))));
03443 evalcond[2]=((((-1.00000000000000)*(new_r00)*(x134)))+(((-1.00000000000000)*(sj33)))+(((new_r10)*(x135))));
03444 evalcond[3]=((((-1.00000000000000)*(new_r01)*(x134)))+(((new_r11)*(x135)))+(((-1.00000000000000)*(cj33))));
03445 evalcond[4]=((((cj32)*(sj33)))+(((new_r01)*(x135)))+(((new_r11)*(x134))));
03446 evalcond[5]=((((new_r00)*(x135)))+(((-1.00000000000000)*(cj32)*(cj33)))+(((new_r10)*(x134))));
03447 evalcond[6]=((((new_r00)*(sj32)*(x135)))+(((new_r10)*(sj32)*(x134)))+(((cj32)*(new_r20))));
03448 evalcond[7]=((((new_r11)*(sj32)*(x134)))+(((new_r01)*(sj32)*(x135)))+(((cj32)*(new_r21))));
03449 evalcond[8]=((-1.00000000000000)+(((new_r02)*(sj32)*(x135)))+(((cj32)*(new_r22)))+(((new_r12)*(sj32)*(x134))));
03450 evalcond[9]=((((cj32)*(new_r12)*(x134)))+(((cj32)*(new_r02)*(x135)))+(((-1.00000000000000)*(new_r22)*(sj32))));
03451 evalcond[10]=((((cj32)*(new_r11)*(x134)))+(((cj32)*(new_r01)*(x135)))+(sj33)+(((-1.00000000000000)*(new_r21)*(sj32))));
03452 evalcond[11]=((((-1.00000000000000)*(new_r20)*(sj32)))+(((cj32)*(new_r10)*(x134)))+(((cj32)*(new_r00)*(x135)))+(((-1.00000000000000)*(cj33))));
03453 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  )
03454 {
03455 continue;
03456 }
03457 }
03458 
03459 {
03460 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
03461 solution.basesol.resize(7);
03462 solution.basesol[0].foffset = j27;
03463 solution.basesol[0].indices[0] = _ij27[0];
03464 solution.basesol[0].indices[1] = _ij27[1];
03465 solution.basesol[0].maxsolutions = _nj27;
03466 solution.basesol[1].foffset = j28;
03467 solution.basesol[1].indices[0] = _ij28[0];
03468 solution.basesol[1].indices[1] = _ij28[1];
03469 solution.basesol[1].maxsolutions = _nj28;
03470 solution.basesol[2].foffset = j29;
03471 solution.basesol[2].indices[0] = _ij29[0];
03472 solution.basesol[2].indices[1] = _ij29[1];
03473 solution.basesol[2].maxsolutions = _nj29;
03474 solution.basesol[3].foffset = j30;
03475 solution.basesol[3].indices[0] = _ij30[0];
03476 solution.basesol[3].indices[1] = _ij30[1];
03477 solution.basesol[3].maxsolutions = _nj30;
03478 solution.basesol[4].foffset = j31;
03479 solution.basesol[4].indices[0] = _ij31[0];
03480 solution.basesol[4].indices[1] = _ij31[1];
03481 solution.basesol[4].maxsolutions = _nj31;
03482 solution.basesol[5].foffset = j32;
03483 solution.basesol[5].indices[0] = _ij32[0];
03484 solution.basesol[5].indices[1] = _ij32[1];
03485 solution.basesol[5].maxsolutions = _nj32;
03486 solution.basesol[6].foffset = j33;
03487 solution.basesol[6].indices[0] = _ij33[0];
03488 solution.basesol[6].indices[1] = _ij33[1];
03489 solution.basesol[6].maxsolutions = _nj33;
03490 solution.vfree.resize(0);
03491 }
03492 }
03493 }
03494 
03495 }
03496 
03497 }
03498 }
03499 }
03500 
03501 }
03502 
03503 }
03504 }
03505 }
03506 }
03507 }static inline void polyroots8(IKReal rawcoeffs[8+1], IKReal rawroots[8], int& numroots)
03508 {
03509     using std::complex;
03510     IKFAST_ASSERT(rawcoeffs[0] != 0);
03511     const IKReal tol = 128.0*std::numeric_limits<IKReal>::epsilon();
03512     const IKReal tolsqrt = 8*sqrt(std::numeric_limits<IKReal>::epsilon());
03513     complex<IKReal> coeffs[8];
03514     const int maxsteps = 110;
03515     for(int i = 0; i < 8; ++i) {
03516         coeffs[i] = complex<IKReal>(rawcoeffs[i+1]/rawcoeffs[0]);
03517     }
03518     complex<IKReal> roots[8];
03519     IKReal err[8];
03520     roots[0] = complex<IKReal>(1,0);
03521     roots[1] = complex<IKReal>(0.4,0.9); // any complex number not a root of unity works
03522     err[0] = 1.0;
03523     err[1] = 1.0;
03524     for(int i = 2; i < 8; ++i) {
03525         roots[i] = roots[i-1]*roots[1];
03526         err[i] = 1.0;
03527     }
03528     for(int step = 0; step < maxsteps; ++step) {
03529         bool changed = false;
03530         for(int i = 0; i < 8; ++i) {
03531             if ( err[i] >= tol ) {
03532                 changed = true;
03533                 // evaluate
03534                 complex<IKReal> x = roots[i] + coeffs[0];
03535                 for(int j = 1; j < 8; ++j) {
03536                     x = roots[i] * x + coeffs[j];
03537                 }
03538                 for(int j = 0; j < 8; ++j) {
03539                     if( i != j ) {
03540                         if( roots[i] != roots[j] ) {
03541                             x /= (roots[i] - roots[j]);
03542                         }
03543                     }
03544                 }
03545                 roots[i] -= x;
03546                 err[i] = abs(x);
03547             }
03548         }
03549         if( !changed ) {
03550             break;
03551         }
03552     }
03553 
03554     numroots = 0;
03555     bool visited[8] = {false};
03556     for(int i = 0; i < 8; ++i) {
03557         if( !visited[i] ) {
03558             // might be a multiple root, in which case it will have more error than the other roots
03559             // find any neighboring roots, and take the average
03560             complex<IKReal> newroot=roots[i];
03561             int n = 1;
03562             for(int j = i+1; j < 8; ++j) {
03563                 if( abs(roots[i]-roots[j]) < tolsqrt ) {
03564                     newroot += roots[j];
03565                     n += 1;
03566                     visited[j] = true;
03567                 }
03568             }
03569             if( n > 1 ) {
03570                 newroot /= n;
03571             }
03572             // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
03573             if( IKabs(imag(newroot)) < sqrt(std::numeric_limits<IKReal>::epsilon()) ) {
03574                 rawroots[numroots++] = real(newroot);
03575             }
03576         }
03577     }
03578 }
03579 };
03580 
03581 
03584 IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
03585 IKSolver solver;
03586 return solver.ik(eetrans,eerot,pfree,vsolutions);
03587 }
03588 
03589 IKFAST_API const char* getKinematicsHash() { return "ad4f1590d2a9b2079648c64e9db33191"; }
03590 
03591 IKFAST_API const char* getIKFastVersion() { return "55"; }
03592 
03593 #ifdef IKFAST_NAMESPACE
03594 } // end namespace
03595 #endif
03596 
03597 #ifndef IKFAST_NO_MAIN
03598 #include <stdio.h>
03599 #include <stdlib.h>
03600 #ifdef IKFAST_NAMESPACE
03601 using namespace IKFAST_NAMESPACE;
03602 #endif
03603 int main(int argc, char** argv)
03604 {
03605     if( argc != 12+getNumFreeParameters()+1 ) {
03606         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
03607                "Returns the ik solutions given the transformation of the end effector specified by\n"
03608                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
03609                "There are %d free parameters that have to be specified.\n\n",getNumFreeParameters());
03610         return 1;
03611     }
03612 
03613     std::vector<IKSolution> vsolutions;
03614     std::vector<IKReal> vfree(getNumFreeParameters());
03615     IKReal eerot[9],eetrans[3];
03616     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
03617     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
03618     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
03619     for(std::size_t i = 0; i < vfree.size(); ++i)
03620         vfree[i] = atof(argv[13+i]);
03621     bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
03622 
03623     if( !bSuccess ) {
03624         fprintf(stderr,"Failed to get ik solution\n");
03625         return -1;
03626     }
03627 
03628     printf("Found %d ik solutions:\n", (int)vsolutions.size());
03629     std::vector<IKReal> sol(getNumJoints());
03630     for(std::size_t i = 0; i < vsolutions.size(); ++i) {
03631         printf("sol%d (free=%d): ", (int)i, (int)vsolutions[i].GetFree().size());
03632         std::vector<IKReal> vsolfree(vsolutions[i].GetFree().size());
03633         vsolutions[i].GetSolution(&sol[0],vsolfree.size()>0?&vsolfree[0]:NULL);
03634         for( std::size_t j = 0; j < sol.size(); ++j)
03635             printf("%.15f, ", sol[j]);
03636         printf("\n");
03637     }
03638     return 0;
03639 }
03640 
03641 #endif
03642 
03643 //#include "ikbase.h"
03644 //namespace IKFAST_NAMESPACE {
03645 //#ifdef RAVE_REGISTER_BOOST
03646 //#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
03647 //BOOST_TYPEOF_REGISTER_TYPE(IKSolution)
03648 //#endif
03649 //IkSolverBasePtr CreateIkSolver(EnvironmentBasePtr penv, const std::vector<dReal>& vfreeinc) {
03650 //    std::vector<int> vfree(getNumFreeParameters());
03651 //    for(size_t i = 0; i < vfree.size(); ++i) {
03652 //        vfree[i] = getFreeParameters()[i];
03653 //    }
03654 //    return IkSolverBasePtr(new IkFastSolver<IKReal,IKSolution>(ik,vfree,vfreeinc,getNumJoints(),static_cast<IkParameterizationType>(getIKType()), boost::shared_ptr<void>(), getKinematicsHash(), penv));
03655 //}
03656 //} // end namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


coverage_3d_arm_navigation
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:56