fanuc_m430ia2f_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 // check if the included ikfast version matches what this file was compiled with
00025 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00026 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00027 
00028 #include <cmath>
00029 #include <vector>
00030 #include <limits>
00031 #include <algorithm>
00032 #include <complex>
00033 
00034 #define IKFAST_STRINGIZE2(s) #s
00035 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
00036 
00037 #ifndef IKFAST_ASSERT
00038 #include <stdexcept>
00039 #include <sstream>
00040 #include <iostream>
00041 
00042 #ifdef _MSC_VER
00043 #ifndef __PRETTY_FUNCTION__
00044 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00045 #endif
00046 #endif
00047 
00048 #ifndef __PRETTY_FUNCTION__
00049 #define __PRETTY_FUNCTION__ __func__
00050 #endif
00051 
00052 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
00053 
00054 #endif
00055 
00056 #if defined(_MSC_VER)
00057 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00058 #else
00059 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00060 #endif
00061 
00062 #define IK2PI  ((IkReal)6.28318530717959)
00063 #define IKPI  ((IkReal)3.14159265358979)
00064 #define IKPI_2  ((IkReal)1.57079632679490)
00065 
00066 #ifdef _MSC_VER
00067 #ifndef isnan
00068 #define isnan _isnan
00069 #endif
00070 #endif // _MSC_VER
00071 
00072 // lapack routines
00073 extern "C" {
00074   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00075   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00076   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00077   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00078   void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
00079   void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
00080 }
00081 
00082 using namespace std; // necessary to get std math routines
00083 
00084 #ifdef IKFAST_NAMESPACE
00085 namespace IKFAST_NAMESPACE {
00086 #endif
00087 
00088 inline float IKabs(float f) { return fabsf(f); }
00089 inline double IKabs(double f) { return fabs(f); }
00090 
00091 inline float IKsqr(float f) { return f*f; }
00092 inline double IKsqr(double f) { return f*f; }
00093 
00094 inline float IKlog(float f) { return logf(f); }
00095 inline double IKlog(double f) { return log(f); }
00096 
00097 // allows asin and acos to exceed 1
00098 #ifndef IKFAST_SINCOS_THRESH
00099 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00100 #endif
00101 
00102 // used to check input to atan2 for degenerate cases
00103 #ifndef IKFAST_ATAN2_MAGTHRESH
00104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00105 #endif
00106 
00107 // minimum distance of separate solutions
00108 #ifndef IKFAST_SOLUTION_THRESH
00109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00110 #endif
00111 
00112 inline float IKasin(float f)
00113 {
00114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00115 if( f <= -1 ) return float(-IKPI_2);
00116 else if( f >= 1 ) return float(IKPI_2);
00117 return asinf(f);
00118 }
00119 inline double IKasin(double f)
00120 {
00121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00122 if( f <= -1 ) return -IKPI_2;
00123 else if( f >= 1 ) return IKPI_2;
00124 return asin(f);
00125 }
00126 
00127 // return positive value in [0,y)
00128 inline float IKfmod(float x, float y)
00129 {
00130     while(x < 0) {
00131         x += y;
00132     }
00133     return fmodf(x,y);
00134 }
00135 
00136 // return positive value in [0,y)
00137 inline double IKfmod(double x, double y)
00138 {
00139     while(x < 0) {
00140         x += y;
00141     }
00142     return fmod(x,y);
00143 }
00144 
00145 inline float IKacos(float f)
00146 {
00147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00148 if( f <= -1 ) return float(IKPI);
00149 else if( f >= 1 ) return float(0);
00150 return acosf(f);
00151 }
00152 inline double IKacos(double f)
00153 {
00154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00155 if( f <= -1 ) return IKPI;
00156 else if( f >= 1 ) return 0;
00157 return acos(f);
00158 }
00159 inline float IKsin(float f) { return sinf(f); }
00160 inline double IKsin(double f) { return sin(f); }
00161 inline float IKcos(float f) { return cosf(f); }
00162 inline double IKcos(double f) { return cos(f); }
00163 inline float IKtan(float f) { return tanf(f); }
00164 inline double IKtan(double f) { return tan(f); }
00165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00167 inline float IKatan2(float fy, float fx) {
00168     if( isnan(fy) ) {
00169         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00170         return float(IKPI_2);
00171     }
00172     else if( isnan(fx) ) {
00173         return 0;
00174     }
00175     return atan2f(fy,fx);
00176 }
00177 inline double IKatan2(double fy, double fx) {
00178     if( isnan(fy) ) {
00179         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00180         return IKPI_2;
00181     }
00182     else if( isnan(fx) ) {
00183         return 0;
00184     }
00185     return atan2(fy,fx);
00186 }
00187 
00188 inline float IKsign(float f) {
00189     if( f > 0 ) {
00190         return float(1);
00191     }
00192     else if( f < 0 ) {
00193         return float(-1);
00194     }
00195     return 0;
00196 }
00197 
00198 inline double IKsign(double f) {
00199     if( f > 0 ) {
00200         return 1.0;
00201     }
00202     else if( f < 0 ) {
00203         return -1.0;
00204     }
00205     return 0;
00206 }
00207 
00210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00211 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20;
00212 x0=IKcos(j[0]);
00213 x1=IKcos(j[2]);
00214 x2=IKsin(j[1]);
00215 x3=IKcos(j[1]);
00216 x4=IKsin(j[2]);
00217 x5=IKsin(j[0]);
00218 x6=IKcos(j[3]);
00219 x7=IKsin(j[3]);
00220 x8=IKcos(j[4]);
00221 x9=IKsin(j[4]);
00222 x10=((IkReal(1.00000000000000))*(x2));
00223 x11=((IkReal(0.550000000000000))*(x3));
00224 x12=((IkReal(0.0650000000000000))*(x3));
00225 x13=((IkReal(0.550000000000000))*(x2));
00226 x14=((IkReal(1.00000000000000))*(x3));
00227 x15=((IkReal(0.0650000000000000))*(x2));
00228 x16=((IkReal(0.350000000000000))*(x2));
00229 x17=((x4)*(x5));
00230 x18=((x0)*(x4));
00231 x19=((x0)*(x1));
00232 x20=((x1)*(x5));
00233 eetrans[0]=((((x0)*(x16)))+(((x11)*(x18)))+(((IkReal(0.0950000000000000))*(x5)))+(((x6)*(((((x12)*(x18)))+(((x15)*(x19)))))))+(((x13)*(x19)))+(((x7)*(((((x12)*(x19)))+(((IkReal(-1.00000000000000))*(x15)*(x18))))))));
00234 eetrans[1]=((((x16)*(x5)))+(((x7)*(((((x12)*(x20)))+(((IkReal(-1.00000000000000))*(x15)*(x17)))))))+(((IkReal(-0.0950000000000000))*(x0)))+(((x11)*(x17)))+(((x6)*(((((x15)*(x20)))+(((x12)*(x17)))))))+(((x13)*(x20))));
00235 IkReal x21=((IkReal(1.00000000000000))*(x4));
00236 eetrans[2]=((IkReal(0.440000000000000))+(((IkReal(0.350000000000000))*(x3)))+(((x7)*(((((IkReal(-1.00000000000000))*(x1)*(x15)))+(((IkReal(-1.00000000000000))*(x12)*(x21)))))))+(((x1)*(x11)))+(((x6)*(((((x1)*(x12)))+(((IkReal(-1.00000000000000))*(x15)*(x21)))))))+(((IkReal(-1.00000000000000))*(x13)*(x21))));
00237 IkReal x22=((IkReal(1.00000000000000))*(x10));
00238 eerot[0]=((((IkReal(-1.00000000000000))*(x5)*(x9)))+(((x8)*(((((x7)*(((((IkReal(-1.00000000000000))*(x19)*(x22)))+(((IkReal(-1.00000000000000))*(x14)*(x18)))))))+(((x6)*(((((x19)*(x3)))+(((IkReal(-1.00000000000000))*(x18)*(x22))))))))))));
00239 IkReal x23=((IkReal(1.00000000000000))*(x10));
00240 eerot[1]=((((x0)*(x9)))+(((x8)*(((((x6)*(((((x20)*(x3)))+(((IkReal(-1.00000000000000))*(x17)*(x23)))))))+(((x7)*(((((IkReal(-1.00000000000000))*(x20)*(x23)))+(((IkReal(-1.00000000000000))*(x14)*(x17))))))))))));
00241 IkReal x24=((IkReal(1.00000000000000))*(x1));
00242 eerot[2]=((x8)*(((((x6)*(((((IkReal(-1.00000000000000))*(x14)*(x4)))+(((IkReal(-1.00000000000000))*(x10)*(x24)))))))+(((x7)*(((((x2)*(x4)))+(((IkReal(-1.00000000000000))*(x14)*(x24))))))))));
00243 }
00244 
00245 IKFAST_API int GetNumFreeParameters() { return 0; }
00246 IKFAST_API int* GetFreeParameters() { return NULL; }
00247 IKFAST_API int GetNumJoints() { return 5; }
00248 
00249 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00250 
00251 IKFAST_API int GetIkType() { return 0x56000007; }
00252 
00253 class IKSolver {
00254 public:
00255 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
00256 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4;
00257 
00258 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00259 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; 
00260 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00261     solutions.Clear();
00262 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00263 
00264 r00 = eerot[0];
00265 r01 = eerot[1];
00266 r02 = eerot[2];
00267 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00268 new_r00=r00;
00269 new_px=px;
00270 new_r01=r01;
00271 new_py=py;
00272 new_r02=r02;
00273 new_pz=((IkReal(-0.440000000000000))+(pz));
00274 r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz;
00275 
00276 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
00277 {
00278 IkReal dummyeval[1];
00279 dummyeval[0]=(((px)*(px))+((py)*(py)));
00280 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00281 {
00282 continue;
00283 
00284 } else
00285 {
00286 {
00287 IkReal j0array[2], cj0array[2], sj0array[2];
00288 bool j0valid[2]={false};
00289 _nj0 = 2;
00290 if( IKabs(py) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(px))) < IKFAST_ATAN2_MAGTHRESH )
00291     continue;
00292 IkReal x25=((IkReal(1.00000000000000))*(IKatan2(py, ((IkReal(-1.00000000000000))*(px)))));
00293 if( ((((px)*(px))+((py)*(py)))) < (IkReal)-0.00001 )
00294     continue;
00295 if( (((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
00296     continue;
00297 IkReal x26=IKasin(((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30))));
00298 j0array[0]=((((IkReal(-1.00000000000000))*(x26)))+(((IkReal(-1.00000000000000))*(x25))));
00299 sj0array[0]=IKsin(j0array[0]);
00300 cj0array[0]=IKcos(j0array[0]);
00301 j0array[1]=((IkReal(3.14159265358979))+(x26)+(((IkReal(-1.00000000000000))*(x25))));
00302 sj0array[1]=IKsin(j0array[1]);
00303 cj0array[1]=IKcos(j0array[1]);
00304 if( j0array[0] > IKPI )
00305 {
00306     j0array[0]-=IK2PI;
00307 }
00308 else if( j0array[0] < -IKPI )
00309 {    j0array[0]+=IK2PI;
00310 }
00311 j0valid[0] = true;
00312 if( j0array[1] > IKPI )
00313 {
00314     j0array[1]-=IK2PI;
00315 }
00316 else if( j0array[1] < -IKPI )
00317 {    j0array[1]+=IK2PI;
00318 }
00319 j0valid[1] = true;
00320 for(int ij0 = 0; ij0 < 2; ++ij0)
00321 {
00322 if( !j0valid[ij0] )
00323 {
00324     continue;
00325 }
00326 _ij0[0] = ij0; _ij0[1] = -1;
00327 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
00328 {
00329 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00330 {
00331     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00332 }
00333 }
00334 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00335 
00336 IkReal op[4+1], zeror[4];
00337 int numroots;
00338 op[0]=((((IkReal(-2.80000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.163324000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.266000000000000))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-0.369600000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.133000000000000))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.240800000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.0341510400000000))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.258720000000000))*(py)*(r01)*(r02)))+(((IkReal(0.279200000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00442225000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0228760000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.266000000000000))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.0252700000000000))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.266000000000000))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.532000000000000))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(0.0931000000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.51220000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.720000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.0722000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.0702240000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.0702240000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0228760000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(1.71920000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.760000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.266000000000000))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-0.0641615800000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.541100000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.0365030400000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.80000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.245280000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(-0.270550000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-0.245280000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.240800000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.720000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.859600000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(0.279200000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.0252700000000000))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.490000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(1.40000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.249200000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.266000000000000))*(px)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.245280000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.207000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0361000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.207000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.266000000000000))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.258720000000000))*(pz)*((r02)*(r02))))+(((IkReal(0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.258720000000000))*(px)*(r00)*(r02)))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.270550000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.907800000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(1.52000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(0.760000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.380000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.163324000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(-0.859600000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(1.40000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(-1.71920000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(-0.133000000000000))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.760000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(1.40000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.0252700000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.490000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.51220000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0931000000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-5.60000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.532000000000000))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-0.266000000000000))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(2.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.245280000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.0722000000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(1.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(2.80000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-5.60000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.0365030400000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.0361000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0252700000000000))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.0702240000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.0245784000000000))*(cj0)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.0702240000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00442225000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(cj0)*(py)*(pz)*((r00)*(r00))))+(((IkReal(1.40000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-0.0245784000000000))*(r00)*(r02)*(sj0)))+(((IkReal(1.40000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0)))));
00339 op[1]=((((IkReal(5.60000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(11.2000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(pp)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(0.186200000000000))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(5.60000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.186200000000000))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.490560000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(5.60000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(1.96000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.186200000000000))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.0491568000000000))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(2.80000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(1.96000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)))+(((IkReal(5.60000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.0505400000000000))*(cj0)*(px)*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.00940800000000000))*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))))+(((IkReal(5.60000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.96000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.266000000000000))*(cj0)*(pp)*(sj0)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(0.0505400000000000))*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0491568000000000))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(1.96000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.186200000000000))*(py)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.517440000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(1.96000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.567980000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0491568000000000))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-5.60000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-2.80000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(1.96000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(cj0)*(pp)*(sj0)*((r01)*(r01))))+(((IkReal(-0.490560000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.186200000000000))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(0.266000000000000))*(pp)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.186200000000000))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.96000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(1.96000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.0268800000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((r01)*(r01))))+(((IkReal(-0.0268800000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(11.2000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.00940800000000000))*(cj0)*(r00)*(r02)))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.517440000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.567980000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(5.60000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0491568000000000))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.186200000000000))*(px)*(r01)*(r02)))+(((IkReal(1.96000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((px)*(px))))+(((IkReal(5.60000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(1.96000000000000))*(cj0)*(px)*(py)*(r01)*(r02))));
00340 op[2]=((((IkReal(-16.0000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.0722000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-32.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.44000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.44000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.231952000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-1.52000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0777100800000000))*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.760000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(1.52000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(8.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.481600000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.760000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(-0.481600000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.52000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-1.52000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(3.43840000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.326648000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.326648000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0635980800000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.52000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.240800000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.144400000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.481600000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-1.52000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-16.0000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.0635980800000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.481600000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(-32.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.481600000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.41840000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.52000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(-0.00884450000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.512848000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.109507160000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(1.52000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00884450000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-4.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(1.52000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.481600000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(3.02440000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-3.50600000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.980000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.52000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-3.36160000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-16.0000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(1.52000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-1.52000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(-0.0722000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(3.02440000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-1.81560000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-1.52000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0457520000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.186200000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.144400000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.481600000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(3.04000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-2.69920000000000))*(pp)*((r02)*(r02))))+(((IkReal(1.44000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(3.43840000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.52000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(1.52000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-16.0000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.512848000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-3.36160000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.52000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.980000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0457520000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(1.52000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-32.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(1.44000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.481600000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.231952000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-0.0722000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.481600000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.240800000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(-1.52000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-3.04000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-8.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-3.50600000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02))));
00341 op[3]=((((IkReal(5.60000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(11.2000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(pp)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(5.60000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.490560000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.186200000000000))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(5.60000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0491568000000000))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.186200000000000))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.00940800000000000))*(cj0)*(r00)*(r02)))+(((IkReal(-1.96000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(1.96000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.186200000000000))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)))+(((IkReal(5.60000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-1.96000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.0505400000000000))*(cj0)*(px)*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-1.96000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))))+(((IkReal(5.60000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(0.186200000000000))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.266000000000000))*(cj0)*(pp)*(sj0)*((r00)*(r00))))+(((IkReal(-1.96000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0505400000000000))*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0491568000000000))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.517440000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.186200000000000))*(px)*(r01)*(r02)))+(((IkReal(-0.567980000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0491568000000000))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(0.186200000000000))*(py)*(r00)*(r02)))+(((IkReal(-5.60000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-2.80000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(5.60000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(cj0)*(pp)*(sj0)*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-1.96000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-0.490560000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.266000000000000))*(pp)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.186200000000000))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0268800000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(1.96000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((r01)*(r01))))+(((IkReal(-0.0268800000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(11.2000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.517440000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.567980000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(5.60000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0491568000000000))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.00940800000000000))*(r01)*(r02)*(sj0)))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.186200000000000))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((px)*(px))))+(((IkReal(5.60000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(-1.96000000000000))*(py)*(pz)*(sj0)*((r02)*(r02)))));
00342 op[4]=((((IkReal(0.163324000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.369600000000000))*(pp)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.240800000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(0.0252700000000000))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.0341510400000000))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.258720000000000))*(px)*(r00)*(r02)))+(((IkReal(0.279200000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00442225000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0252700000000000))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.0228760000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-2.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.0931000000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(0.270550000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.51220000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.532000000000000))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.266000000000000))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(-0.258720000000000))*(pz)*((r02)*(r02))))+(((IkReal(-1.40000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.720000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.0722000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.258720000000000))*(py)*(r01)*(r02)))+(((IkReal(-1.52000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.0702240000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.266000000000000))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.532000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.0702240000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0228760000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(0.532000000000000))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(1.71920000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.760000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0641615800000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(0.270550000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0365030400000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.40000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.0252700000000000))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-0.240800000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(px)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.532000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(0.720000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.40000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.859600000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.245280000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(0.279200000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.541100000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.490000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(0.249200000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.0252700000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.207000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0361000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.207000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.133000000000000))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.907800000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-1.40000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(1.52000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(0.760000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.380000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.245280000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.245280000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.0245784000000000))*(cj0)*(r01)*(r02)))+(((IkReal(-0.163324000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(-0.859600000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.266000000000000))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.0245784000000000))*(r00)*(r02)*(sj0)))+(((IkReal(-1.71920000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(5.60000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.40000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.490000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-1.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.51220000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0931000000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.266000000000000))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(2.80000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.133000000000000))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(0.0722000000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.266000000000000))*(cj0)*(py)*(pz)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.245280000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.0365030400000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-0.0361000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0702240000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.0702240000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00442225000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.266000000000000))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)))));
00343 polyroots4(op,zeror,numroots);
00344 IkReal j1array[4], cj1array[4], sj1array[4], tempj1array[1];
00345 int numsolutions = 0;
00346 for(int ij1 = 0; ij1 < numroots; ++ij1)
00347 {
00348 IkReal htj1 = zeror[ij1];
00349 tempj1array[0]=((IkReal(2.00000000000000))*(atan(htj1)));
00350 for(int kj1 = 0; kj1 < 1; ++kj1)
00351 {
00352 j1array[numsolutions] = tempj1array[kj1];
00353 if( j1array[numsolutions] > IKPI )
00354 {
00355     j1array[numsolutions]-=IK2PI;
00356 }
00357 else if( j1array[numsolutions] < -IKPI )
00358 {
00359     j1array[numsolutions]+=IK2PI;
00360 }
00361 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
00362 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
00363 numsolutions++;
00364 }
00365 }
00366 bool j1valid[4]={true,true,true,true};
00367 _nj1 = 4;
00368 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
00369     {
00370 if( !j1valid[ij1] )
00371 {
00372     continue;
00373 }
00374     j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00375 htj1 = IKtan(j1/2);
00376 
00377 _ij1[0] = ij1; _ij1[1] = -1;
00378 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
00379 {
00380 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00381 {
00382     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00383 }
00384 }
00385 {
00386 IkReal j4array[2], cj4array[2], sj4array[2];
00387 bool j4valid[2]={false};
00388 _nj4 = 2;
00389 sj4array[0]=((((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
00390 if( sj4array[0] >= -1-IKFAST_SINCOS_THRESH && sj4array[0] <= 1+IKFAST_SINCOS_THRESH )
00391 {
00392     j4valid[0] = j4valid[1] = true;
00393     j4array[0] = IKasin(sj4array[0]);
00394     cj4array[0] = IKcos(j4array[0]);
00395     sj4array[1] = sj4array[0];
00396     j4array[1] = j4array[0] > 0 ? (IKPI-j4array[0]) : (-IKPI-j4array[0]);
00397     cj4array[1] = -cj4array[0];
00398 }
00399 else if( isnan(sj4array[0]) )
00400 {
00401     // probably any value will work
00402     j4valid[0] = true;
00403     cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
00404 }
00405 for(int ij4 = 0; ij4 < 2; ++ij4)
00406 {
00407 if( !j4valid[ij4] )
00408 {
00409     continue;
00410 }
00411 _ij4[0] = ij4; _ij4[1] = -1;
00412 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
00413 {
00414 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00415 {
00416     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00417 }
00418 }
00419 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00420 
00421 {
00422 IkReal dummyeval[1];
00423 IkReal gconst0;
00424 gconst0=IKsign(cj4);
00425 dummyeval[0]=cj4;
00426 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00427 {
00428 {
00429 IkReal dummyeval[1];
00430 dummyeval[0]=cj4;
00431 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00432 {
00433 {
00434 IkReal evalcond[9];
00435 IkReal x27=(py)*(py);
00436 IkReal x28=(px)*(px);
00437 IkReal x29=(pz)*(pz);
00438 IkReal x30=((cj1)*(r02));
00439 IkReal x31=((IkReal(0.190000000000000))*(pz));
00440 IkReal x32=((cj0)*(sj1));
00441 IkReal x33=((cj1)*(r00));
00442 IkReal x34=((IkReal(0.190000000000000))*(py));
00443 IkReal x35=((px)*(sj0));
00444 IkReal x36=((IkReal(2.00000000000000))*(r00));
00445 IkReal x37=((r00)*(sj1));
00446 IkReal x38=((px)*(r01));
00447 IkReal x39=((IkReal(0.190000000000000))*(sj1));
00448 IkReal x40=((IkReal(2.00000000000000))*(py));
00449 IkReal x41=((py)*(r01));
00450 IkReal x42=((IkReal(1.00000000000000))*(cj0));
00451 IkReal x43=((r02)*(sj1));
00452 IkReal x44=((r00)*(sj0));
00453 IkReal x45=((IkReal(0.700000000000000))*(r00));
00454 IkReal x46=((pz)*(r01));
00455 IkReal x47=((IkReal(2.00000000000000))*(pz));
00456 IkReal x48=((cj0)*(r01));
00457 IkReal x49=((pz)*(r02));
00458 IkReal x50=((cj0)*(pz));
00459 IkReal x51=((r01)*(sj0)*(sj1));
00460 IkReal x52=((cj0)*(px)*(r02));
00461 IkReal x53=((sj1)*(x47));
00462 IkReal x54=((py)*(r02)*(sj0));
00463 IkReal x55=((cj1)*(r01)*(sj0));
00464 IkReal x56=((IkReal(2.00000000000000))*(x27));
00465 IkReal x57=((IkReal(2.00000000000000))*(x29));
00466 IkReal x58=((sj0)*(x40)*(x49));
00467 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
00468 evalcond[1]=((IkReal(-0.0950000000000000))+(x35)+(((IkReal(-1.00000000000000))*(py)*(x42))));
00469 evalcond[2]=((IkReal(1.00000000000000))+(x44)+(((IkReal(-1.00000000000000))*(r01)*(x42))));
00470 evalcond[3]=((x51)+(x30)+(((r00)*(x32))));
00471 evalcond[4]=((((IkReal(-1.00000000000000))*(x55)))+(x43)+(((IkReal(-1.00000000000000))*(x33)*(x42))));
00472 evalcond[5]=((((IkReal(-0.0950000000000000))*(x48)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.350000000000000))*(x51)))+(((IkReal(-1.00000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(x49)))+(((IkReal(0.350000000000000))*(r00)*(x32)))+(((IkReal(0.0950000000000000))*(x44)))+(((IkReal(0.350000000000000))*(x30))));
00473 evalcond[6]=((((IkReal(0.350000000000000))*(x43)))+(((IkReal(-1.00000000000000))*(x54)))+(((sj0)*(x46)))+(((IkReal(-0.350000000000000))*(x55)))+(((r00)*(x50)))+(((IkReal(-0.350000000000000))*(cj0)*(x33)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x42))));
00474 evalcond[7]=((((IkReal(-1.00000000000000))*(pz)*(sj0)*(x40)*(x43)))+(((pp)*(x51)))+(((IkReal(-1.00000000000000))*(x38)*(x39)))+(((sj0)*(x31)*(x33)))+(((IkReal(-0.113475000000000))*(x30)))+(((x34)*(x37)))+(((IkReal(0.700000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(py)*(sj1)*(x35)*(x36)))+(((IkReal(0.0665000000000000))*(x48)))+(((IkReal(-1.00000000000000))*(cj1)*(x31)*(x48)))+(((IkReal(-1.00000000000000))*(x30)*(x57)))+(((IkReal(-0.190000000000000))*(x30)*(x35)))+(((IkReal(-0.0665000000000000))*(x44)))+(((IkReal(-0.113475000000000))*(r00)*(x32)))+(((IkReal(-1.00000000000000))*(x28)*(x32)*(x36)))+(((px)*(x45)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x32)*(x47)))+(((cj0)*(x30)*(x34)))+(((pp)*(x30)))+(((IkReal(-1.00000000000000))*(px)*(x33)*(x47)))+(((IkReal(0.700000000000000))*(x49)))+(((IkReal(-1.00000000000000))*(x32)*(x38)*(x40)))+(((IkReal(-1.00000000000000))*(x51)*(x56)))+(((IkReal(-0.113475000000000))*(x51)))+(((pp)*(r00)*(x32)))+(((IkReal(-1.00000000000000))*(cj1)*(x40)*(x46))));
00475 evalcond[8]=((((IkReal(0.131525000000000))*(x43)))+(((IkReal(2.00000000000000))*(cj0)*(x28)*(x33)))+(((r02)*(x32)*(x34)))+(((cj0)*(px)*(x30)*(x47)))+(((IkReal(-0.131525000000000))*(cj0)*(x33)))+(((pp)*(x43)))+(((IkReal(0.700000000000000))*(sj0)*(x46)))+(((pz)*(sj0)*(x30)*(x40)))+(((cj0)*(cj1)*(x38)*(x40)))+(((IkReal(-1.00000000000000))*(r02)*(x35)*(x39)))+(((IkReal(0.190000000000000))*(cj1)*(x38)))+(((IkReal(-1.00000000000000))*(x33)*(x34)))+(((IkReal(-1.00000000000000))*(x43)*(x57)))+(((IkReal(-1.00000000000000))*(px)*(pz)*(sj1)*(x36)))+(((IkReal(-1.00000000000000))*(pp)*(x55)))+(((sj0)*(x31)*(x37)))+(((IkReal(-1.00000000000000))*(r01)*(x31)*(x32)))+(((IkReal(-1.00000000000000))*(sj1)*(x40)*(x46)))+(((IkReal(-0.131525000000000))*(x55)))+(((IkReal(-1.00000000000000))*(pp)*(x33)*(x42)))+(((IkReal(-0.700000000000000))*(x52)))+(((IkReal(-0.700000000000000))*(x54)))+(((x45)*(x50)))+(((x33)*(x35)*(x40)))+(((x55)*(x56))));
00476 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  )
00477 {
00478 {
00479 IkReal j3array[2], cj3array[2], sj3array[2];
00480 bool j3valid[2]={false};
00481 _nj3 = 2;
00482 IkReal x59=((IkReal(9.79020979020979))*(sj1));
00483 cj3array[0]=((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x59)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x59)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp))));
00484 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
00485 {
00486     j3valid[0] = j3valid[1] = true;
00487     j3array[0] = IKacos(cj3array[0]);
00488     sj3array[0] = IKsin(j3array[0]);
00489     cj3array[1] = cj3array[0];
00490     j3array[1] = -j3array[0];
00491     sj3array[1] = -sj3array[0];
00492 }
00493 else if( isnan(cj3array[0]) )
00494 {
00495     // probably any value will work
00496     j3valid[0] = true;
00497     cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
00498 }
00499 for(int ij3 = 0; ij3 < 2; ++ij3)
00500 {
00501 if( !j3valid[ij3] )
00502 {
00503     continue;
00504 }
00505 _ij3[0] = ij3; _ij3[1] = -1;
00506 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00507 {
00508 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00509 {
00510     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00511 }
00512 }
00513 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00514 {
00515 IkReal evalcond[1];
00516 IkReal x60=((IkReal(2.00000000000000))*(sj0));
00517 IkReal x61=((px)*(r01));
00518 IkReal x62=((r00)*(sj1));
00519 IkReal x63=((IkReal(0.700000000000000))*(py));
00520 IkReal x64=((r00)*(sj0));
00521 IkReal x65=((IkReal(0.700000000000000))*(cj1));
00522 IkReal x66=((px)*(r02));
00523 IkReal x67=((IkReal(2.00000000000000))*(cj0));
00524 IkReal x68=((px)*(r00));
00525 IkReal x69=((cj0)*(r01));
00526 IkReal x70=((pz)*(r02));
00527 IkReal x71=((cj1)*(r02));
00528 evalcond[0]=((IkReal(0.306725000000000))+(((IkReal(0.190000000000000))*(x70)))+(((IkReal(-1.00000000000000))*(pp)*(x69)))+(((IkReal(-1.00000000000000))*(pz)*(x60)*(x66)))+(((sj0)*(x65)*(x66)))+(((IkReal(-1.00000000000000))*(x62)*(x63)))+(((IkReal(0.0715000000000000))*(IKcos(j3))))+(((IkReal(0.190000000000000))*(py)*(r01)))+(((IkReal(-0.113475000000000))*(x69)))+(((IkReal(0.113475000000000))*(x64)))+(((r01)*(x67)*((py)*(py))))+(((IkReal(-0.0665000000000000))*(r01)*(sj0)*(sj1)))+(((pp)*(x64)))+(((IkReal(-1.00000000000000))*(py)*(x60)*(x61)))+(((IkReal(-1.00000000000000))*(pz)*(x64)*(x65)))+(((IkReal(-0.0665000000000000))*(cj0)*(x62)))+(((pz)*(x65)*(x69)))+(((IkReal(-1.00000000000000))*(px)*(x60)*(x68)))+(((py)*(x67)*(x68)))+(((IkReal(0.700000000000000))*(sj1)*(x61)))+(((py)*(x67)*(x70)))+(((IkReal(-0.0665000000000000))*(x71)))+(((IkReal(-1.00000000000000))*(cj0)*(x63)*(x71)))+(((IkReal(0.190000000000000))*(x68))));
00529 if( IKabs(evalcond[0]) > 0.000001  )
00530 {
00531 continue;
00532 }
00533 }
00534 
00535 {
00536 IkReal dummyeval[1];
00537 IkReal gconst9;
00538 gconst9=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
00539 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
00540 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00541 {
00542 {
00543 IkReal dummyeval[1];
00544 IkReal gconst10;
00545 gconst10=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
00546 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
00547 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00548 {
00549 continue;
00550 
00551 } else
00552 {
00553 {
00554 IkReal j2array[1], cj2array[1], sj2array[1];
00555 bool j2valid[1]={false};
00556 _nj2 = 1;
00557 IkReal x72=((cj1)*(cj3));
00558 IkReal x73=((sj0)*(sj1));
00559 IkReal x74=((IkReal(22000.0000000000))*(pz));
00560 IkReal x75=((IkReal(2600.00000000000))*(pz));
00561 IkReal x76=((r02)*(sj1));
00562 IkReal x77=((IkReal(2600.00000000000))*(sj3));
00563 IkReal x78=((cj1)*(sj3));
00564 IkReal x79=((r01)*(sj0));
00565 IkReal x80=((cj0)*(sj1));
00566 IkReal x81=((IkReal(2600.00000000000))*(cj3));
00567 IkReal x82=((IkReal(2090.00000000000))*(cj1));
00568 IkReal x83=((IkReal(22000.0000000000))*(px));
00569 IkReal x84=((px)*(r02));
00570 IkReal x85=((IkReal(22000.0000000000))*(py));
00571 IkReal x86=((py)*(r00));
00572 IkReal x87=((px)*(r01));
00573 IkReal x88=((IkReal(247.000000000000))*(cj0)*(r00));
00574 IkReal x89=((x77)*(x80));
00575 if( IKabs(((gconst10)*(((((cj0)*(r00)*(x82)))+(((py)*(x73)*(x77)))+(((px)*(x89)))+(((cj1)*(r00)*(x85)))+(((IkReal(-1.00000000000000))*(cj0)*(x76)*(x85)))+(((x72)*(x88)))+(((IkReal(-247.000000000000))*(cj3)*(x76)))+(((r02)*(x73)*(x83)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x73)*(x75)))+(((IkReal(247.000000000000))*(x72)*(x79)))+(((IkReal(2600.00000000000))*(x72)*(x86)))+(((x79)*(x82)))+(((r01)*(x74)*(x80)))+(((IkReal(-910.000000000000))*(sj3)))+(((cj3)*(r01)*(x75)*(x80)))+(((IkReal(-2090.00000000000))*(x76)))+(((IkReal(-2600.00000000000))*(x72)*(x87)))+(((x75)*(x78)))+(((IkReal(-1.00000000000000))*(r00)*(x73)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x83)))+(((x73)*(x81)*(x84)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x81))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((IkReal(7700.00000000000))+(((x78)*(x88)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x77)))+(((IkReal(-1.00000000000000))*(x72)*(x75)))+(((r01)*(sj3)*(x75)*(x80)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x73)*(x75)))+(((cj1)*(x77)*(x86)))+(((IkReal(-1.00000000000000))*(px)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(x80)*(x83)))+(((IkReal(-1.00000000000000))*(py)*(x73)*(x81)))+(((x73)*(x77)*(x84)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x73)*(x85)))+(((IkReal(-247.000000000000))*(sj3)*(x76)))+(((IkReal(247.000000000000))*(x78)*(x79)))+(((IkReal(-1.00000000000000))*(cj1)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(x77)*(x87))))))) < IKFAST_ATAN2_MAGTHRESH )
00576     continue;
00577 j2array[0]=IKatan2(((gconst10)*(((((cj0)*(r00)*(x82)))+(((py)*(x73)*(x77)))+(((px)*(x89)))+(((cj1)*(r00)*(x85)))+(((IkReal(-1.00000000000000))*(cj0)*(x76)*(x85)))+(((x72)*(x88)))+(((IkReal(-247.000000000000))*(cj3)*(x76)))+(((r02)*(x73)*(x83)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x73)*(x75)))+(((IkReal(247.000000000000))*(x72)*(x79)))+(((IkReal(2600.00000000000))*(x72)*(x86)))+(((x79)*(x82)))+(((r01)*(x74)*(x80)))+(((IkReal(-910.000000000000))*(sj3)))+(((cj3)*(r01)*(x75)*(x80)))+(((IkReal(-2090.00000000000))*(x76)))+(((IkReal(-2600.00000000000))*(x72)*(x87)))+(((x75)*(x78)))+(((IkReal(-1.00000000000000))*(r00)*(x73)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x83)))+(((x73)*(x81)*(x84)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x81)))))), ((gconst10)*(((IkReal(7700.00000000000))+(((x78)*(x88)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x77)))+(((IkReal(-1.00000000000000))*(x72)*(x75)))+(((r01)*(sj3)*(x75)*(x80)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x73)*(x75)))+(((cj1)*(x77)*(x86)))+(((IkReal(-1.00000000000000))*(px)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(x80)*(x83)))+(((IkReal(-1.00000000000000))*(py)*(x73)*(x81)))+(((x73)*(x77)*(x84)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x73)*(x85)))+(((IkReal(-247.000000000000))*(sj3)*(x76)))+(((IkReal(247.000000000000))*(x78)*(x79)))+(((IkReal(-1.00000000000000))*(cj1)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(x77)*(x87)))))));
00578 sj2array[0]=IKsin(j2array[0]);
00579 cj2array[0]=IKcos(j2array[0]);
00580 if( j2array[0] > IKPI )
00581 {
00582     j2array[0]-=IK2PI;
00583 }
00584 else if( j2array[0] < -IKPI )
00585 {    j2array[0]+=IK2PI;
00586 }
00587 j2valid[0] = true;
00588 for(int ij2 = 0; ij2 < 1; ++ij2)
00589 {
00590 if( !j2valid[ij2] )
00591 {
00592     continue;
00593 }
00594 _ij2[0] = ij2; _ij2[1] = -1;
00595 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
00596 {
00597 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00598 {
00599     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00600 }
00601 }
00602 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00603 {
00604 IkReal evalcond[4];
00605 IkReal x90=IKcos(j2);
00606 IkReal x91=IKsin(j2);
00607 IkReal x92=((px)*(sj1));
00608 IkReal x93=((py)*(sj1));
00609 IkReal x94=((cj1)*(sj0));
00610 IkReal x95=((IkReal(1.00000000000000))*(px));
00611 IkReal x96=((r00)*(sj0));
00612 IkReal x97=((IkReal(0.0950000000000000))*(r01));
00613 IkReal x98=((IkReal(0.0650000000000000))*(cj3));
00614 IkReal x99=((cj0)*(r01));
00615 IkReal x100=((IkReal(0.0950000000000000))*(cj1));
00616 IkReal x101=((pz)*(sj1));
00617 IkReal x102=((cj0)*(r00));
00618 IkReal x103=((IkReal(0.0950000000000000))*(sj1));
00619 IkReal x104=((cj0)*(cj1));
00620 IkReal x105=((cj1)*(pz));
00621 IkReal x106=((IkReal(0.0650000000000000))*(sj3));
00622 IkReal x107=((IkReal(0.550000000000000))*(x91));
00623 IkReal x108=((IkReal(0.550000000000000))*(x90));
00624 IkReal x109=((x91)*(x98));
00625 IkReal x110=((x106)*(x90));
00626 IkReal x111=((x90)*(x98));
00627 IkReal x112=((x106)*(x91));
00628 IkReal x113=((x108)+(x111));
00629 IkReal x114=((x109)+(x107)+(x110));
00630 evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x93)))+(((cj0)*(x92)))+(x105)+(x112)+(((IkReal(-1.00000000000000))*(x113))));
00631 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x94)))+(x101)+(x114)+(((IkReal(-1.00000000000000))*(x104)*(x95))));
00632 evalcond[2]=((((r02)*(sj0)*(x92)))+(((IkReal(-1.00000000000000))*(r02)*(x103)))+(((cj1)*(py)*(r00)))+(((x100)*(x102)))+(((IkReal(-1.00000000000000))*(x101)*(x96)))+(x114)+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x95)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x93)))+(((x101)*(x99)))+(((x94)*(x97))));
00633 evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x94)*(x95)))+(((IkReal(-0.350000000000000))*(x96)))+(((sj0)*(sj1)*(x97)))+(((IkReal(-1.00000000000000))*(r01)*(x92)))+(((IkReal(-1.00000000000000))*(x105)*(x99)))+(((r00)*(x93)))+(((x102)*(x103)))+(((py)*(r02)*(x104)))+(((IkReal(0.350000000000000))*(x99)))+(((IkReal(-1.00000000000000))*(x112)))+(x113)+(((r02)*(x100)))+(((pz)*(r00)*(x94))));
00634 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00635 {
00636 continue;
00637 }
00638 }
00639 
00640 {
00641 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
00642 vinfos[0].jointtype = 1;
00643 vinfos[0].foffset = j0;
00644 vinfos[0].indices[0] = _ij0[0];
00645 vinfos[0].indices[1] = _ij0[1];
00646 vinfos[0].maxsolutions = _nj0;
00647 vinfos[1].jointtype = 1;
00648 vinfos[1].foffset = j1;
00649 vinfos[1].indices[0] = _ij1[0];
00650 vinfos[1].indices[1] = _ij1[1];
00651 vinfos[1].maxsolutions = _nj1;
00652 vinfos[2].jointtype = 1;
00653 vinfos[2].foffset = j2;
00654 vinfos[2].indices[0] = _ij2[0];
00655 vinfos[2].indices[1] = _ij2[1];
00656 vinfos[2].maxsolutions = _nj2;
00657 vinfos[3].jointtype = 1;
00658 vinfos[3].foffset = j3;
00659 vinfos[3].indices[0] = _ij3[0];
00660 vinfos[3].indices[1] = _ij3[1];
00661 vinfos[3].maxsolutions = _nj3;
00662 vinfos[4].jointtype = 1;
00663 vinfos[4].foffset = j4;
00664 vinfos[4].indices[0] = _ij4[0];
00665 vinfos[4].indices[1] = _ij4[1];
00666 vinfos[4].maxsolutions = _nj4;
00667 std::vector<int> vfree(0);
00668 solutions.AddSolution(vinfos,vfree);
00669 }
00670 }
00671 }
00672 
00673 }
00674 
00675 }
00676 
00677 } else
00678 {
00679 {
00680 IkReal j2array[1], cj2array[1], sj2array[1];
00681 bool j2valid[1]={false};
00682 _nj2 = 1;
00683 IkReal x115=((IkReal(2600.00000000000))*(sj3));
00684 IkReal x116=((py)*(sj0));
00685 IkReal x117=((pz)*(sj1));
00686 IkReal x118=((IkReal(2600.00000000000))*(cj3));
00687 IkReal x119=((cj1)*(pz));
00688 IkReal x120=((IkReal(22000.0000000000))*(cj1));
00689 IkReal x121=((cj0)*(px));
00690 IkReal x122=((sj1)*(x121));
00691 if( IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(x116)*(x120)))+(((x117)*(x118)))+(((IkReal(-1.00000000000000))*(cj1)*(x118)*(x121)))+(((x115)*(x122)))+(((IkReal(22000.0000000000))*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x116)*(x118)))+(((IkReal(-910.000000000000))*(sj3)))+(((sj1)*(x115)*(x116)))+(((IkReal(-1.00000000000000))*(x120)*(x121)))+(((x115)*(x119))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x122)))+(((x115)*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x116)))+(((IkReal(-22000.0000000000))*(sj1)*(x116)))+(((IkReal(-1.00000000000000))*(x118)*(x119)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x121)))+(((IkReal(-1.00000000000000))*(x118)*(x122)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x119)))+(((IkReal(-1.00000000000000))*(sj1)*(x116)*(x118))))))) < IKFAST_ATAN2_MAGTHRESH )
00692     continue;
00693 j2array[0]=IKatan2(((gconst9)*(((((IkReal(-1.00000000000000))*(x116)*(x120)))+(((x117)*(x118)))+(((IkReal(-1.00000000000000))*(cj1)*(x118)*(x121)))+(((x115)*(x122)))+(((IkReal(22000.0000000000))*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x116)*(x118)))+(((IkReal(-910.000000000000))*(sj3)))+(((sj1)*(x115)*(x116)))+(((IkReal(-1.00000000000000))*(x120)*(x121)))+(((x115)*(x119)))))), ((gconst9)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x122)))+(((x115)*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x116)))+(((IkReal(-22000.0000000000))*(sj1)*(x116)))+(((IkReal(-1.00000000000000))*(x118)*(x119)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x121)))+(((IkReal(-1.00000000000000))*(x118)*(x122)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x119)))+(((IkReal(-1.00000000000000))*(sj1)*(x116)*(x118)))))));
00694 sj2array[0]=IKsin(j2array[0]);
00695 cj2array[0]=IKcos(j2array[0]);
00696 if( j2array[0] > IKPI )
00697 {
00698     j2array[0]-=IK2PI;
00699 }
00700 else if( j2array[0] < -IKPI )
00701 {    j2array[0]+=IK2PI;
00702 }
00703 j2valid[0] = true;
00704 for(int ij2 = 0; ij2 < 1; ++ij2)
00705 {
00706 if( !j2valid[ij2] )
00707 {
00708     continue;
00709 }
00710 _ij2[0] = ij2; _ij2[1] = -1;
00711 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
00712 {
00713 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00714 {
00715     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00716 }
00717 }
00718 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00719 {
00720 IkReal evalcond[4];
00721 IkReal x123=IKcos(j2);
00722 IkReal x124=IKsin(j2);
00723 IkReal x125=((px)*(sj1));
00724 IkReal x126=((py)*(sj1));
00725 IkReal x127=((cj1)*(sj0));
00726 IkReal x128=((IkReal(1.00000000000000))*(px));
00727 IkReal x129=((r00)*(sj0));
00728 IkReal x130=((IkReal(0.0950000000000000))*(r01));
00729 IkReal x131=((IkReal(0.0650000000000000))*(cj3));
00730 IkReal x132=((cj0)*(r01));
00731 IkReal x133=((IkReal(0.0950000000000000))*(cj1));
00732 IkReal x134=((pz)*(sj1));
00733 IkReal x135=((cj0)*(r00));
00734 IkReal x136=((IkReal(0.0950000000000000))*(sj1));
00735 IkReal x137=((cj0)*(cj1));
00736 IkReal x138=((cj1)*(pz));
00737 IkReal x139=((IkReal(0.0650000000000000))*(sj3));
00738 IkReal x140=((IkReal(0.550000000000000))*(x124));
00739 IkReal x141=((IkReal(0.550000000000000))*(x123));
00740 IkReal x142=((x124)*(x131));
00741 IkReal x143=((x123)*(x139));
00742 IkReal x144=((x123)*(x131));
00743 IkReal x145=((x124)*(x139));
00744 IkReal x146=((x141)+(x144));
00745 IkReal x147=((x140)+(x142)+(x143));
00746 evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x126)))+(((IkReal(-1.00000000000000))*(x146)))+(((cj0)*(x125)))+(x138)+(x145));
00747 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x127)))+(x134)+(x147)+(((IkReal(-1.00000000000000))*(x128)*(x137))));
00748 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x136)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x128)))+(((r02)*(sj0)*(x125)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x126)))+(((IkReal(-1.00000000000000))*(x129)*(x134)))+(((x133)*(x135)))+(((cj1)*(py)*(r00)))+(((x127)*(x130)))+(((x132)*(x134)))+(x147));
00749 evalcond[3]=((((r00)*(x126)))+(((IkReal(-1.00000000000000))*(x132)*(x138)))+(((IkReal(-0.350000000000000))*(x129)))+(((IkReal(0.350000000000000))*(x132)))+(((IkReal(-1.00000000000000))*(x145)))+(((x135)*(x136)))+(((r02)*(x133)))+(((IkReal(-1.00000000000000))*(r01)*(x125)))+(((IkReal(-1.00000000000000))*(r02)*(x127)*(x128)))+(((py)*(r02)*(x137)))+(x146)+(((pz)*(r00)*(x127)))+(((sj0)*(sj1)*(x130))));
00750 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00751 {
00752 continue;
00753 }
00754 }
00755 
00756 {
00757 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
00758 vinfos[0].jointtype = 1;
00759 vinfos[0].foffset = j0;
00760 vinfos[0].indices[0] = _ij0[0];
00761 vinfos[0].indices[1] = _ij0[1];
00762 vinfos[0].maxsolutions = _nj0;
00763 vinfos[1].jointtype = 1;
00764 vinfos[1].foffset = j1;
00765 vinfos[1].indices[0] = _ij1[0];
00766 vinfos[1].indices[1] = _ij1[1];
00767 vinfos[1].maxsolutions = _nj1;
00768 vinfos[2].jointtype = 1;
00769 vinfos[2].foffset = j2;
00770 vinfos[2].indices[0] = _ij2[0];
00771 vinfos[2].indices[1] = _ij2[1];
00772 vinfos[2].maxsolutions = _nj2;
00773 vinfos[3].jointtype = 1;
00774 vinfos[3].foffset = j3;
00775 vinfos[3].indices[0] = _ij3[0];
00776 vinfos[3].indices[1] = _ij3[1];
00777 vinfos[3].maxsolutions = _nj3;
00778 vinfos[4].jointtype = 1;
00779 vinfos[4].foffset = j4;
00780 vinfos[4].indices[0] = _ij4[0];
00781 vinfos[4].indices[1] = _ij4[1];
00782 vinfos[4].maxsolutions = _nj4;
00783 std::vector<int> vfree(0);
00784 solutions.AddSolution(vinfos,vfree);
00785 }
00786 }
00787 }
00788 
00789 }
00790 
00791 }
00792 }
00793 }
00794 
00795 } else
00796 {
00797 IkReal x148=(py)*(py);
00798 IkReal x149=(px)*(px);
00799 IkReal x150=(pz)*(pz);
00800 IkReal x151=((cj1)*(r02));
00801 IkReal x152=((IkReal(0.190000000000000))*(pz));
00802 IkReal x153=((cj0)*(sj1));
00803 IkReal x154=((cj1)*(r00));
00804 IkReal x155=((IkReal(0.190000000000000))*(py));
00805 IkReal x156=((px)*(sj0));
00806 IkReal x157=((IkReal(2.00000000000000))*(r00));
00807 IkReal x158=((r00)*(sj1));
00808 IkReal x159=((px)*(r01));
00809 IkReal x160=((IkReal(0.190000000000000))*(sj1));
00810 IkReal x161=((IkReal(2.00000000000000))*(py));
00811 IkReal x162=((py)*(r01));
00812 IkReal x163=((IkReal(1.00000000000000))*(cj0));
00813 IkReal x164=((r02)*(sj1));
00814 IkReal x165=((r00)*(sj0));
00815 IkReal x166=((IkReal(0.700000000000000))*(r00));
00816 IkReal x167=((pz)*(r01));
00817 IkReal x168=((IkReal(2.00000000000000))*(pz));
00818 IkReal x169=((cj0)*(r01));
00819 IkReal x170=((pz)*(r02));
00820 IkReal x171=((cj0)*(pz));
00821 IkReal x172=((r01)*(sj0)*(sj1));
00822 IkReal x173=((cj0)*(px)*(r02));
00823 IkReal x174=((sj1)*(x168));
00824 IkReal x175=((py)*(r02)*(sj0));
00825 IkReal x176=((cj1)*(r01)*(sj0));
00826 IkReal x177=((IkReal(2.00000000000000))*(x148));
00827 IkReal x178=((IkReal(2.00000000000000))*(x150));
00828 IkReal x179=((sj0)*(x161)*(x170));
00829 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
00830 evalcond[1]=((IkReal(-0.0950000000000000))+(x156)+(((IkReal(-1.00000000000000))*(py)*(x163))));
00831 evalcond[2]=((IkReal(-1.00000000000000))+(x165)+(((IkReal(-1.00000000000000))*(r01)*(x163))));
00832 evalcond[3]=((x172)+(x151)+(((r00)*(x153))));
00833 evalcond[4]=((((IkReal(-1.00000000000000))*(x154)*(x163)))+(x164)+(((IkReal(-1.00000000000000))*(x176))));
00834 evalcond[5]=((((IkReal(-1.00000000000000))*(x170)))+(((IkReal(0.0950000000000000))*(x165)))+(((IkReal(0.350000000000000))*(x151)))+(((IkReal(-1.00000000000000))*(x162)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.350000000000000))*(r00)*(x153)))+(((IkReal(-0.0950000000000000))*(x169)))+(((IkReal(0.350000000000000))*(x172))));
00835 evalcond[6]=((((sj0)*(x167)))+(((IkReal(-1.00000000000000))*(x175)))+(((IkReal(-0.350000000000000))*(x176)))+(((r00)*(x171)))+(((IkReal(-0.350000000000000))*(cj0)*(x154)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x163)))+(((IkReal(0.350000000000000))*(x164))));
00836 evalcond[7]=((((IkReal(-1.00000000000000))*(x172)*(x177)))+(((sj0)*(x152)*(x154)))+(((IkReal(-0.113475000000000))*(x151)))+(((IkReal(-1.00000000000000))*(pz)*(sj0)*(x161)*(x164)))+(((pp)*(x151)))+(((IkReal(-1.00000000000000))*(x149)*(x153)*(x157)))+(((IkReal(0.700000000000000))*(x170)))+(((IkReal(-1.00000000000000))*(x159)*(x160)))+(((pp)*(x172)))+(((px)*(x166)))+(((x155)*(x158)))+(((IkReal(-1.00000000000000))*(cj1)*(x161)*(x167)))+(((IkReal(0.0665000000000000))*(x169)))+(((IkReal(-1.00000000000000))*(cj1)*(x152)*(x169)))+(((cj0)*(x151)*(x155)))+(((pp)*(r00)*(x153)))+(((IkReal(-1.00000000000000))*(x153)*(x159)*(x161)))+(((IkReal(-1.00000000000000))*(x151)*(x178)))+(((IkReal(0.700000000000000))*(x162)))+(((IkReal(-1.00000000000000))*(px)*(x154)*(x168)))+(((IkReal(-0.113475000000000))*(r00)*(x153)))+(((IkReal(-1.00000000000000))*(py)*(sj1)*(x156)*(x157)))+(((IkReal(-0.113475000000000))*(x172)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x153)*(x168)))+(((IkReal(-0.190000000000000))*(x151)*(x156)))+(((IkReal(-0.0665000000000000))*(x165))));
00837 evalcond[8]=((((IkReal(0.700000000000000))*(sj0)*(x167)))+(((IkReal(2.00000000000000))*(cj0)*(x149)*(x154)))+(((IkReal(-1.00000000000000))*(px)*(pz)*(sj1)*(x157)))+(((IkReal(-1.00000000000000))*(x154)*(x155)))+(((cj0)*(cj1)*(x159)*(x161)))+(((x176)*(x177)))+(((IkReal(-1.00000000000000))*(r01)*(x152)*(x153)))+(((IkReal(-1.00000000000000))*(pp)*(x154)*(x163)))+(((IkReal(-1.00000000000000))*(r02)*(x156)*(x160)))+(((pz)*(sj0)*(x151)*(x161)))+(((IkReal(-0.700000000000000))*(x175)))+(((IkReal(0.190000000000000))*(cj1)*(x159)))+(((IkReal(-1.00000000000000))*(pp)*(x176)))+(((IkReal(0.131525000000000))*(x164)))+(((r02)*(x153)*(x155)))+(((IkReal(-1.00000000000000))*(sj1)*(x161)*(x167)))+(((IkReal(-0.131525000000000))*(x176)))+(((IkReal(-0.700000000000000))*(x173)))+(((sj0)*(x152)*(x158)))+(((IkReal(-0.131525000000000))*(cj0)*(x154)))+(((x166)*(x171)))+(((pp)*(x164)))+(((IkReal(-1.00000000000000))*(x164)*(x178)))+(((cj0)*(px)*(x151)*(x168)))+(((x154)*(x156)*(x161))));
00838 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  )
00839 {
00840 {
00841 IkReal j3array[2], cj3array[2], sj3array[2];
00842 bool j3valid[2]={false};
00843 _nj3 = 2;
00844 IkReal x180=((IkReal(9.79020979020979))*(sj1));
00845 cj3array[0]=((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x180)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x180))));
00846 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
00847 {
00848     j3valid[0] = j3valid[1] = true;
00849     j3array[0] = IKacos(cj3array[0]);
00850     sj3array[0] = IKsin(j3array[0]);
00851     cj3array[1] = cj3array[0];
00852     j3array[1] = -j3array[0];
00853     sj3array[1] = -sj3array[0];
00854 }
00855 else if( isnan(cj3array[0]) )
00856 {
00857     // probably any value will work
00858     j3valid[0] = true;
00859     cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
00860 }
00861 for(int ij3 = 0; ij3 < 2; ++ij3)
00862 {
00863 if( !j3valid[ij3] )
00864 {
00865     continue;
00866 }
00867 _ij3[0] = ij3; _ij3[1] = -1;
00868 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00869 {
00870 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00871 {
00872     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00873 }
00874 }
00875 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00876 {
00877 IkReal evalcond[1];
00878 IkReal x181=((IkReal(2.00000000000000))*(sj0));
00879 IkReal x182=((px)*(r01));
00880 IkReal x183=((r00)*(sj1));
00881 IkReal x184=((IkReal(0.700000000000000))*(py));
00882 IkReal x185=((r00)*(sj0));
00883 IkReal x186=((IkReal(0.700000000000000))*(cj1));
00884 IkReal x187=((px)*(r02));
00885 IkReal x188=((IkReal(2.00000000000000))*(cj0));
00886 IkReal x189=((px)*(r00));
00887 IkReal x190=((cj0)*(r01));
00888 IkReal x191=((pz)*(r02));
00889 IkReal x192=((cj1)*(r02));
00890 evalcond[0]=((IkReal(-0.306725000000000))+(((py)*(x188)*(x191)))+(((IkReal(-1.00000000000000))*(pz)*(x181)*(x187)))+(((IkReal(-0.0665000000000000))*(cj0)*(x183)))+(((IkReal(-1.00000000000000))*(cj0)*(x184)*(x192)))+(((IkReal(-0.113475000000000))*(x190)))+(((IkReal(-1.00000000000000))*(py)*(x181)*(x182)))+(((IkReal(0.190000000000000))*(py)*(r01)))+(((IkReal(0.700000000000000))*(sj1)*(x182)))+(((IkReal(0.190000000000000))*(x189)))+(((IkReal(-1.00000000000000))*(pp)*(x190)))+(((IkReal(0.190000000000000))*(x191)))+(((IkReal(-1.00000000000000))*(x183)*(x184)))+(((r01)*(x188)*((py)*(py))))+(((IkReal(-0.0665000000000000))*(r01)*(sj0)*(sj1)))+(((pz)*(x186)*(x190)))+(((IkReal(0.113475000000000))*(x185)))+(((IkReal(-1.00000000000000))*(pz)*(x185)*(x186)))+(((py)*(x188)*(x189)))+(((IkReal(-0.0665000000000000))*(x192)))+(((pp)*(x185)))+(((sj0)*(x186)*(x187)))+(((IkReal(-1.00000000000000))*(px)*(x181)*(x189)))+(((IkReal(-0.0715000000000000))*(IKcos(j3)))));
00891 if( IKabs(evalcond[0]) > 0.000001  )
00892 {
00893 continue;
00894 }
00895 }
00896 
00897 {
00898 IkReal dummyeval[1];
00899 IkReal gconst11;
00900 gconst11=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
00901 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
00902 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00903 {
00904 {
00905 IkReal dummyeval[1];
00906 IkReal gconst12;
00907 gconst12=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
00908 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
00909 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00910 {
00911 continue;
00912 
00913 } else
00914 {
00915 {
00916 IkReal j2array[1], cj2array[1], sj2array[1];
00917 bool j2valid[1]={false};
00918 _nj2 = 1;
00919 IkReal x193=((cj1)*(cj3));
00920 IkReal x194=((sj0)*(sj1));
00921 IkReal x195=((IkReal(22000.0000000000))*(pz));
00922 IkReal x196=((IkReal(2600.00000000000))*(pz));
00923 IkReal x197=((r02)*(sj1));
00924 IkReal x198=((IkReal(2600.00000000000))*(sj3));
00925 IkReal x199=((cj1)*(sj3));
00926 IkReal x200=((r01)*(sj0));
00927 IkReal x201=((cj0)*(sj1));
00928 IkReal x202=((IkReal(2600.00000000000))*(cj3));
00929 IkReal x203=((IkReal(2090.00000000000))*(cj1));
00930 IkReal x204=((IkReal(22000.0000000000))*(px));
00931 IkReal x205=((px)*(r02));
00932 IkReal x206=((IkReal(22000.0000000000))*(py));
00933 IkReal x207=((py)*(r00));
00934 IkReal x208=((px)*(r01));
00935 IkReal x209=((IkReal(247.000000000000))*(cj0)*(r00));
00936 IkReal x210=((x198)*(x201));
00937 if( IKabs(((gconst12)*(((((px)*(x210)))+(((IkReal(-1.00000000000000))*(x193)*(x209)))+(((py)*(x194)*(x198)))+(((cj3)*(r00)*(x194)*(x196)))+(((x196)*(x199)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x206)))+(((IkReal(2600.00000000000))*(x193)*(x208)))+(((cj0)*(py)*(x197)*(x202)))+(((cj1)*(r01)*(x204)))+(((IkReal(-1.00000000000000))*(x194)*(x202)*(x205)))+(((IkReal(247.000000000000))*(cj3)*(x197)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x193)*(x200)))+(((IkReal(2090.00000000000))*(x197)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x203)))+(((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(r02)*(x194)*(x204)))+(((IkReal(-2600.00000000000))*(x193)*(x207)))+(((cj0)*(x197)*(x206)))+(((r00)*(x194)*(x195)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x201)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x196)*(x201))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x194)*(x198)*(x205)))+(((IkReal(-1.00000000000000))*(x193)*(x196)))+(((IkReal(-1.00000000000000))*(x199)*(x209)))+(((IkReal(-1.00000000000000))*(cj1)*(x195)))+(((cj0)*(py)*(x197)*(x198)))+(((IkReal(-1.00000000000000))*(px)*(x201)*(x202)))+(((r00)*(sj3)*(x194)*(x196)))+(((IkReal(-247.000000000000))*(x199)*(x200)))+(((cj1)*(x198)*(x208)))+(((IkReal(-1.00000000000000))*(cj1)*(x198)*(x207)))+(((IkReal(247.000000000000))*(sj3)*(x197)))+(((IkReal(-1.00000000000000))*(x194)*(x206)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(py)*(x194)*(x202)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x196)*(x201)))+(((IkReal(-1.00000000000000))*(x201)*(x204))))))) < IKFAST_ATAN2_MAGTHRESH )
00938     continue;
00939 j2array[0]=IKatan2(((gconst12)*(((((px)*(x210)))+(((IkReal(-1.00000000000000))*(x193)*(x209)))+(((py)*(x194)*(x198)))+(((cj3)*(r00)*(x194)*(x196)))+(((x196)*(x199)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x206)))+(((IkReal(2600.00000000000))*(x193)*(x208)))+(((cj0)*(py)*(x197)*(x202)))+(((cj1)*(r01)*(x204)))+(((IkReal(-1.00000000000000))*(x194)*(x202)*(x205)))+(((IkReal(247.000000000000))*(cj3)*(x197)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x193)*(x200)))+(((IkReal(2090.00000000000))*(x197)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x203)))+(((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(r02)*(x194)*(x204)))+(((IkReal(-2600.00000000000))*(x193)*(x207)))+(((cj0)*(x197)*(x206)))+(((r00)*(x194)*(x195)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x201)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x196)*(x201)))))), ((gconst12)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x194)*(x198)*(x205)))+(((IkReal(-1.00000000000000))*(x193)*(x196)))+(((IkReal(-1.00000000000000))*(x199)*(x209)))+(((IkReal(-1.00000000000000))*(cj1)*(x195)))+(((cj0)*(py)*(x197)*(x198)))+(((IkReal(-1.00000000000000))*(px)*(x201)*(x202)))+(((r00)*(sj3)*(x194)*(x196)))+(((IkReal(-247.000000000000))*(x199)*(x200)))+(((cj1)*(x198)*(x208)))+(((IkReal(-1.00000000000000))*(cj1)*(x198)*(x207)))+(((IkReal(247.000000000000))*(sj3)*(x197)))+(((IkReal(-1.00000000000000))*(x194)*(x206)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(py)*(x194)*(x202)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x196)*(x201)))+(((IkReal(-1.00000000000000))*(x201)*(x204)))))));
00940 sj2array[0]=IKsin(j2array[0]);
00941 cj2array[0]=IKcos(j2array[0]);
00942 if( j2array[0] > IKPI )
00943 {
00944     j2array[0]-=IK2PI;
00945 }
00946 else if( j2array[0] < -IKPI )
00947 {    j2array[0]+=IK2PI;
00948 }
00949 j2valid[0] = true;
00950 for(int ij2 = 0; ij2 < 1; ++ij2)
00951 {
00952 if( !j2valid[ij2] )
00953 {
00954     continue;
00955 }
00956 _ij2[0] = ij2; _ij2[1] = -1;
00957 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
00958 {
00959 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00960 {
00961     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00962 }
00963 }
00964 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00965 {
00966 IkReal evalcond[4];
00967 IkReal x211=IKcos(j2);
00968 IkReal x212=IKsin(j2);
00969 IkReal x213=((px)*(sj1));
00970 IkReal x214=((py)*(sj1));
00971 IkReal x215=((cj1)*(sj0));
00972 IkReal x216=((IkReal(1.00000000000000))*(px));
00973 IkReal x217=((r00)*(sj0));
00974 IkReal x218=((IkReal(0.0950000000000000))*(r01));
00975 IkReal x219=((IkReal(0.0650000000000000))*(cj3));
00976 IkReal x220=((cj0)*(r01));
00977 IkReal x221=((IkReal(0.0950000000000000))*(cj1));
00978 IkReal x222=((pz)*(sj1));
00979 IkReal x223=((cj0)*(r00));
00980 IkReal x224=((IkReal(0.0950000000000000))*(sj1));
00981 IkReal x225=((cj0)*(cj1));
00982 IkReal x226=((cj1)*(pz));
00983 IkReal x227=((IkReal(0.0650000000000000))*(sj3));
00984 IkReal x228=((IkReal(0.550000000000000))*(x212));
00985 IkReal x229=((IkReal(0.550000000000000))*(x211));
00986 IkReal x230=((x212)*(x219));
00987 IkReal x231=((x211)*(x227));
00988 IkReal x232=((x212)*(x227));
00989 IkReal x233=((x211)*(x219));
00990 IkReal x234=((x233)+(x229));
00991 IkReal x235=((x230)+(x231)+(x228));
00992 evalcond[0]=((IkReal(-0.350000000000000))+(x232)+(((cj0)*(x213)))+(x226)+(((IkReal(-1.00000000000000))*(x234)))+(((sj0)*(x214))));
00993 evalcond[1]=((x235)+(((IkReal(-1.00000000000000))*(py)*(x215)))+(((IkReal(-1.00000000000000))*(x216)*(x225)))+(x222));
00994 evalcond[2]=((((IkReal(-1.00000000000000))*(x235)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x216)))+(((r02)*(sj0)*(x213)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x214)))+(((IkReal(-1.00000000000000))*(r02)*(x224)))+(((x215)*(x218)))+(((x220)*(x222)))+(((x221)*(x223)))+(((IkReal(-1.00000000000000))*(x217)*(x222))));
00995 evalcond[3]=((((IkReal(0.350000000000000))*(x220)))+(x232)+(((py)*(r02)*(x225)))+(((r02)*(x221)))+(((r00)*(x214)))+(((IkReal(-0.350000000000000))*(x217)))+(((x223)*(x224)))+(((IkReal(-1.00000000000000))*(x234)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((pz)*(r00)*(x215)))+(((IkReal(-1.00000000000000))*(r01)*(x213)))+(((IkReal(-1.00000000000000))*(r02)*(x215)*(x216)))+(((sj0)*(sj1)*(x218))));
00996 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00997 {
00998 continue;
00999 }
01000 }
01001 
01002 {
01003 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01004 vinfos[0].jointtype = 1;
01005 vinfos[0].foffset = j0;
01006 vinfos[0].indices[0] = _ij0[0];
01007 vinfos[0].indices[1] = _ij0[1];
01008 vinfos[0].maxsolutions = _nj0;
01009 vinfos[1].jointtype = 1;
01010 vinfos[1].foffset = j1;
01011 vinfos[1].indices[0] = _ij1[0];
01012 vinfos[1].indices[1] = _ij1[1];
01013 vinfos[1].maxsolutions = _nj1;
01014 vinfos[2].jointtype = 1;
01015 vinfos[2].foffset = j2;
01016 vinfos[2].indices[0] = _ij2[0];
01017 vinfos[2].indices[1] = _ij2[1];
01018 vinfos[2].maxsolutions = _nj2;
01019 vinfos[3].jointtype = 1;
01020 vinfos[3].foffset = j3;
01021 vinfos[3].indices[0] = _ij3[0];
01022 vinfos[3].indices[1] = _ij3[1];
01023 vinfos[3].maxsolutions = _nj3;
01024 vinfos[4].jointtype = 1;
01025 vinfos[4].foffset = j4;
01026 vinfos[4].indices[0] = _ij4[0];
01027 vinfos[4].indices[1] = _ij4[1];
01028 vinfos[4].maxsolutions = _nj4;
01029 std::vector<int> vfree(0);
01030 solutions.AddSolution(vinfos,vfree);
01031 }
01032 }
01033 }
01034 
01035 }
01036 
01037 }
01038 
01039 } else
01040 {
01041 {
01042 IkReal j2array[1], cj2array[1], sj2array[1];
01043 bool j2valid[1]={false};
01044 _nj2 = 1;
01045 IkReal x236=((IkReal(2600.00000000000))*(sj3));
01046 IkReal x237=((py)*(sj0));
01047 IkReal x238=((pz)*(sj1));
01048 IkReal x239=((IkReal(2600.00000000000))*(cj3));
01049 IkReal x240=((cj1)*(pz));
01050 IkReal x241=((IkReal(22000.0000000000))*(cj1));
01051 IkReal x242=((cj0)*(px));
01052 IkReal x243=((sj1)*(x242));
01053 if( IKabs(((gconst11)*(((((IkReal(22000.0000000000))*(x238)))+(((x236)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x237)*(x239)))+(((IkReal(-1.00000000000000))*(x237)*(x241)))+(((IkReal(-1.00000000000000))*(x241)*(x242)))+(((x236)*(x240)))+(((IkReal(-1.00000000000000))*(cj1)*(x239)*(x242)))+(((IkReal(-910.000000000000))*(sj3)))+(((x238)*(x239)))+(((sj1)*(x236)*(x237))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x239)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x242)))+(((IkReal(-1.00000000000000))*(x239)*(x240)))+(((IkReal(-1.00000000000000))*(sj1)*(x237)*(x239)))+(((x236)*(x238)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x237)))+(((IkReal(-22000.0000000000))*(x243)))+(((IkReal(-22000.0000000000))*(sj1)*(x237)))+(((IkReal(-22000.0000000000))*(x240))))))) < IKFAST_ATAN2_MAGTHRESH )
01054     continue;
01055 j2array[0]=IKatan2(((gconst11)*(((((IkReal(22000.0000000000))*(x238)))+(((x236)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x237)*(x239)))+(((IkReal(-1.00000000000000))*(x237)*(x241)))+(((IkReal(-1.00000000000000))*(x241)*(x242)))+(((x236)*(x240)))+(((IkReal(-1.00000000000000))*(cj1)*(x239)*(x242)))+(((IkReal(-910.000000000000))*(sj3)))+(((x238)*(x239)))+(((sj1)*(x236)*(x237)))))), ((gconst11)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x239)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x242)))+(((IkReal(-1.00000000000000))*(x239)*(x240)))+(((IkReal(-1.00000000000000))*(sj1)*(x237)*(x239)))+(((x236)*(x238)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x237)))+(((IkReal(-22000.0000000000))*(x243)))+(((IkReal(-22000.0000000000))*(sj1)*(x237)))+(((IkReal(-22000.0000000000))*(x240)))))));
01056 sj2array[0]=IKsin(j2array[0]);
01057 cj2array[0]=IKcos(j2array[0]);
01058 if( j2array[0] > IKPI )
01059 {
01060     j2array[0]-=IK2PI;
01061 }
01062 else if( j2array[0] < -IKPI )
01063 {    j2array[0]+=IK2PI;
01064 }
01065 j2valid[0] = true;
01066 for(int ij2 = 0; ij2 < 1; ++ij2)
01067 {
01068 if( !j2valid[ij2] )
01069 {
01070     continue;
01071 }
01072 _ij2[0] = ij2; _ij2[1] = -1;
01073 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01074 {
01075 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01076 {
01077     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01078 }
01079 }
01080 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01081 {
01082 IkReal evalcond[4];
01083 IkReal x244=IKcos(j2);
01084 IkReal x245=IKsin(j2);
01085 IkReal x246=((px)*(sj1));
01086 IkReal x247=((py)*(sj1));
01087 IkReal x248=((cj1)*(sj0));
01088 IkReal x249=((IkReal(1.00000000000000))*(px));
01089 IkReal x250=((r00)*(sj0));
01090 IkReal x251=((IkReal(0.0950000000000000))*(r01));
01091 IkReal x252=((IkReal(0.0650000000000000))*(cj3));
01092 IkReal x253=((cj0)*(r01));
01093 IkReal x254=((IkReal(0.0950000000000000))*(cj1));
01094 IkReal x255=((pz)*(sj1));
01095 IkReal x256=((cj0)*(r00));
01096 IkReal x257=((IkReal(0.0950000000000000))*(sj1));
01097 IkReal x258=((cj0)*(cj1));
01098 IkReal x259=((cj1)*(pz));
01099 IkReal x260=((IkReal(0.0650000000000000))*(sj3));
01100 IkReal x261=((IkReal(0.550000000000000))*(x245));
01101 IkReal x262=((IkReal(0.550000000000000))*(x244));
01102 IkReal x263=((x245)*(x252));
01103 IkReal x264=((x244)*(x260));
01104 IkReal x265=((x245)*(x260));
01105 IkReal x266=((x244)*(x252));
01106 IkReal x267=((x266)+(x262));
01107 IkReal x268=((x264)+(x261)+(x263));
01108 evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x267)))+(x259)+(((cj0)*(x246)))+(x265)+(((sj0)*(x247))));
01109 evalcond[1]=((x255)+(x268)+(((IkReal(-1.00000000000000))*(x249)*(x258)))+(((IkReal(-1.00000000000000))*(py)*(x248))));
01110 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(r02)*(x247)))+(((x253)*(x255)))+(((r02)*(sj0)*(x246)))+(((IkReal(-1.00000000000000))*(x250)*(x255)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(x268)))+(((x254)*(x256)))+(((x248)*(x251)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x249)))+(((IkReal(-1.00000000000000))*(r02)*(x257))));
01111 evalcond[3]=((((r02)*(x254)))+(((py)*(r02)*(x258)))+(((IkReal(-1.00000000000000))*(x267)))+(((IkReal(-1.00000000000000))*(x253)*(x259)))+(((IkReal(-0.350000000000000))*(x250)))+(((IkReal(0.350000000000000))*(x253)))+(((x256)*(x257)))+(((pz)*(r00)*(x248)))+(((r00)*(x247)))+(((IkReal(-1.00000000000000))*(r01)*(x246)))+(x265)+(((IkReal(-1.00000000000000))*(r02)*(x248)*(x249)))+(((sj0)*(sj1)*(x251))));
01112 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01113 {
01114 continue;
01115 }
01116 }
01117 
01118 {
01119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01120 vinfos[0].jointtype = 1;
01121 vinfos[0].foffset = j0;
01122 vinfos[0].indices[0] = _ij0[0];
01123 vinfos[0].indices[1] = _ij0[1];
01124 vinfos[0].maxsolutions = _nj0;
01125 vinfos[1].jointtype = 1;
01126 vinfos[1].foffset = j1;
01127 vinfos[1].indices[0] = _ij1[0];
01128 vinfos[1].indices[1] = _ij1[1];
01129 vinfos[1].maxsolutions = _nj1;
01130 vinfos[2].jointtype = 1;
01131 vinfos[2].foffset = j2;
01132 vinfos[2].indices[0] = _ij2[0];
01133 vinfos[2].indices[1] = _ij2[1];
01134 vinfos[2].maxsolutions = _nj2;
01135 vinfos[3].jointtype = 1;
01136 vinfos[3].foffset = j3;
01137 vinfos[3].indices[0] = _ij3[0];
01138 vinfos[3].indices[1] = _ij3[1];
01139 vinfos[3].maxsolutions = _nj3;
01140 vinfos[4].jointtype = 1;
01141 vinfos[4].foffset = j4;
01142 vinfos[4].indices[0] = _ij4[0];
01143 vinfos[4].indices[1] = _ij4[1];
01144 vinfos[4].maxsolutions = _nj4;
01145 std::vector<int> vfree(0);
01146 solutions.AddSolution(vinfos,vfree);
01147 }
01148 }
01149 }
01150 
01151 }
01152 
01153 }
01154 }
01155 }
01156 
01157 } else
01158 {
01159 if( 1 )
01160 {
01161 continue;
01162 
01163 } else
01164 {
01165 }
01166 }
01167 }
01168 }
01169 
01170 } else
01171 {
01172 {
01173 IkReal j3array[1], cj3array[1], sj3array[1];
01174 bool j3valid[1]={false};
01175 _nj3 = 1;
01176 IkReal x269=((sj0)*(sj1));
01177 IkReal x270=((cj0)*(sj1));
01178 if( IKabs(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))))+IKsqr(((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270)))))-1) <= IKFAST_SINCOS_THRESH )
01179     continue;
01180 j3array[0]=IKatan2(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))), ((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270)))));
01181 sj3array[0]=IKsin(j3array[0]);
01182 cj3array[0]=IKcos(j3array[0]);
01183 if( j3array[0] > IKPI )
01184 {
01185     j3array[0]-=IK2PI;
01186 }
01187 else if( j3array[0] < -IKPI )
01188 {    j3array[0]+=IK2PI;
01189 }
01190 j3valid[0] = true;
01191 for(int ij3 = 0; ij3 < 1; ++ij3)
01192 {
01193 if( !j3valid[ij3] )
01194 {
01195     continue;
01196 }
01197 _ij3[0] = ij3; _ij3[1] = -1;
01198 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01199 {
01200 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01201 {
01202     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01203 }
01204 }
01205 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01206 {
01207 IkReal evalcond[4];
01208 IkReal x271=IKcos(j3);
01209 IkReal x272=((r01)*(sj1));
01210 IkReal x273=((IkReal(0.350000000000000))*(sj0));
01211 IkReal x274=((cj0)*(r00));
01212 IkReal x275=((IkReal(0.350000000000000))*(cj1));
01213 IkReal x276=((px)*(r02));
01214 IkReal x277=((IkReal(1.00000000000000))*(cj0));
01215 IkReal x278=((IkReal(0.190000000000000))*(px));
01216 IkReal x279=((cj0)*(r01));
01217 IkReal x280=((r00)*(sj0));
01218 IkReal x281=((IkReal(0.700000000000000))*(sj1));
01219 IkReal x282=((IkReal(0.550000000000000))*(cj4));
01220 IkReal x283=((IkReal(0.700000000000000))*(cj1));
01221 IkReal x284=((py)*(sj0));
01222 IkReal x285=((IkReal(1.00000000000000))*(r02));
01223 IkReal x286=((cj0)*(py));
01224 IkReal x287=((IkReal(2.00000000000000))*(px));
01225 IkReal x288=((pz)*(sj0));
01226 IkReal x289=((pz)*(r02));
01227 IkReal x290=((IkReal(0.350000000000000))*(sj1));
01228 IkReal x291=((py)*(r01));
01229 IkReal x292=((IkReal(0.0715000000000000))*(x271));
01230 evalcond[0]=((IkReal(0.175200000000000))+(((x281)*(x284)))+(((sj0)*(x278)))+(((IkReal(-1.00000000000000))*(pp)))+(x292)+(((pz)*(x283)))+(((IkReal(-0.190000000000000))*(x286)))+(((cj0)*(px)*(x281))));
01231 evalcond[1]=((((IkReal(-1.00000000000000))*(x282)*(IKsin(j3))))+(((x272)*(x273)))+(((IkReal(0.0950000000000000))*(x280)))+(((x274)*(x290)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x291)))+(((r02)*(x275)))+(((IkReal(-1.00000000000000))*(pz)*(x285)))+(((IkReal(-0.0950000000000000))*(x279))));
01232 evalcond[2]=((((r02)*(x290)))+(((r01)*(x288)))+(((IkReal(-1.00000000000000))*(x274)*(x275)))+(((IkReal(-1.00000000000000))*(x276)*(x277)))+(((IkReal(-0.0650000000000000))*(cj4)))+(((pz)*(x274)))+(((IkReal(-1.00000000000000))*(x284)*(x285)))+(((IkReal(-1.00000000000000))*(x271)*(x282)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x273))));
01233 evalcond[3]=((((IkReal(-2.00000000000000))*(x276)*(x288)))+(((IkReal(0.306725000000000))*(sj4)))+(((pp)*(x280)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x277)))+(((IkReal(-0.0665000000000000))*(sj1)*(x274)))+(((IkReal(0.190000000000000))*(x289)))+(((IkReal(-1.00000000000000))*(r01)*(x284)*(x287)))+(((IkReal(-0.113475000000000))*(x279)))+(((IkReal(0.700000000000000))*(px)*(x272)))+(((IkReal(2.00000000000000))*(x286)*(x289)))+(((IkReal(-1.00000000000000))*(pz)*(x280)*(x283)))+(((IkReal(0.113475000000000))*(x280)))+(((sj4)*(x292)))+(((IkReal(-1.00000000000000))*(px)*(x280)*(x287)))+(((IkReal(2.00000000000000))*(x279)*((py)*(py))))+(((r00)*(x278)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x281)))+(((IkReal(-0.0665000000000000))*(sj0)*(x272)))+(((sj0)*(x276)*(x283)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((pz)*(x279)*(x283)))+(((IkReal(-1.00000000000000))*(r02)*(x283)*(x286)))+(((py)*(x274)*(x287)))+(((IkReal(0.190000000000000))*(x291))));
01234 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01235 {
01236 continue;
01237 }
01238 }
01239 
01240 {
01241 IkReal dummyeval[1];
01242 IkReal gconst1;
01243 gconst1=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
01244 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
01245 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01246 {
01247 {
01248 IkReal dummyeval[1];
01249 IkReal gconst2;
01250 IkReal x293=((IkReal(13.0000000000000))*(cj4));
01251 gconst2=IKsign(((((IkReal(-1.00000000000000))*(x293)*((sj3)*(sj3))))+(((IkReal(-110.000000000000))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x293)*((cj3)*(cj3))))));
01252 IkReal x294=((IkReal(1.00000000000000))*(cj4));
01253 dummyeval[0]=((((IkReal(-1.00000000000000))*(x294)*((cj3)*(cj3))))+(((IkReal(-8.46153846153846))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x294)*((sj3)*(sj3)))));
01254 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01255 {
01256 {
01257 IkReal evalcond[11];
01258 IkReal x295=((IkReal(0.0715000000000000))*(cj3));
01259 IkReal x296=(py)*(py);
01260 IkReal x297=(px)*(px);
01261 IkReal x298=(pz)*(pz);
01262 IkReal x299=((r01)*(sj0));
01263 IkReal x300=((px)*(sj0));
01264 IkReal x301=((cj1)*(r00));
01265 IkReal x302=((IkReal(0.350000000000000))*(cj0));
01266 IkReal x303=((IkReal(0.113475000000000))*(sj1));
01267 IkReal x304=((r01)*(sj1));
01268 IkReal x305=((IkReal(2.00000000000000))*(pz));
01269 IkReal x306=((IkReal(0.190000000000000))*(px));
01270 IkReal x307=((py)*(sj0));
01271 IkReal x308=((IkReal(1.00000000000000))*(r02));
01272 IkReal x309=((IkReal(0.190000000000000))*(py));
01273 IkReal x310=((cj0)*(sj1));
01274 IkReal x311=((px)*(r02));
01275 IkReal x312=((px)*(r00));
01276 IkReal x313=((r02)*(sj1));
01277 IkReal x314=((IkReal(2.00000000000000))*(py));
01278 IkReal x315=((pp)*(r00));
01279 IkReal x316=((cj0)*(r01));
01280 IkReal x317=((IkReal(0.700000000000000))*(sj1));
01281 IkReal x318=((IkReal(0.190000000000000))*(pz));
01282 IkReal x319=((cj1)*(r01));
01283 IkReal x320=((cj0)*(r00));
01284 IkReal x321=((IkReal(0.700000000000000))*(pz));
01285 IkReal x322=((cj0)*(px));
01286 IkReal x323=((r00)*(sj0));
01287 IkReal x324=((cj1)*(r02));
01288 IkReal x325=((IkReal(1.00000000000000))*(pp));
01289 IkReal x326=((IkReal(1.00000000000000))*(py));
01290 IkReal x327=((IkReal(0.700000000000000))*(cj0));
01291 IkReal x328=((r00)*(sj1));
01292 IkReal x329=((IkReal(0.700000000000000))*(px));
01293 IkReal x330=((cj0)*(cj1));
01294 IkReal x331=((cj1)*(x325));
01295 IkReal x332=((IkReal(2.00000000000000))*(x298));
01296 IkReal x333=((IkReal(2.00000000000000))*(x296));
01297 IkReal x334=((IkReal(2.00000000000000))*(r00)*(x297));
01298 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
01299 evalcond[1]=((IkReal(-0.0950000000000000))+(x300)+(((IkReal(-1.00000000000000))*(cj0)*(x326))));
01300 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x316)))+(x323));
01301 evalcond[3]=((x324)+(((sj1)*(x299)))+(((r00)*(x310))));
01302 evalcond[4]=((((IkReal(-1.00000000000000))*(cj1)*(x299)))+(((IkReal(-1.00000000000000))*(cj0)*(x301)))+(x313));
01303 evalcond[5]=((IkReal(0.175200000000000))+(((IkReal(-1.00000000000000))*(x325)))+(((x307)*(x317)))+(((IkReal(-1.00000000000000))*(cj0)*(x309)))+(x295)+(((x310)*(x329)))+(((IkReal(0.190000000000000))*(x300)))+(((cj1)*(x321))));
01304 evalcond[6]=((((x302)*(x328)))+(((IkReal(-1.00000000000000))*(pz)*(x308)))+(((IkReal(-1.00000000000000))*(x312)))+(((IkReal(0.350000000000000))*(sj1)*(x299)))+(((IkReal(0.350000000000000))*(x324)))+(((IkReal(0.0950000000000000))*(x323)))+(((IkReal(-0.0950000000000000))*(x316)))+(((IkReal(-1.00000000000000))*(r01)*(x326))));
01305 evalcond[7]=((((IkReal(0.350000000000000))*(x313)))+(((IkReal(-1.00000000000000))*(x307)*(x308)))+(((IkReal(-1.00000000000000))*(x308)*(x322)))+(((pz)*(x320)))+(((IkReal(-1.00000000000000))*(x301)*(x302)))+(((pz)*(x299)))+(((IkReal(-0.350000000000000))*(cj1)*(x299))));
01306 evalcond[8]=((IkReal(0.306725000000000))+(((x304)*(x329)))+(((IkReal(-1.00000000000000))*(x316)*(x325)))+(((r02)*(x318)))+(((IkReal(0.700000000000000))*(x300)*(x324)))+(((IkReal(-0.0665000000000000))*(sj1)*(x299)))+(((sj0)*(x315)))+(((r00)*(x306)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x317)))+(((IkReal(0.113475000000000))*(x323)))+(((cj0)*(x312)*(x314)))+(((x316)*(x333)))+(((IkReal(-2.00000000000000))*(x297)*(x323)))+(x295)+(((r01)*(x309)))+(((IkReal(-1.00000000000000))*(px)*(x299)*(x314)))+(((cj0)*(py)*(r02)*(x305)))+(((cj1)*(x316)*(x321)))+(((IkReal(-0.0665000000000000))*(x324)))+(((IkReal(-1.00000000000000))*(py)*(x324)*(x327)))+(((IkReal(-0.113475000000000))*(x316)))+(((IkReal(-1.00000000000000))*(r02)*(x300)*(x305)))+(((IkReal(-0.0665000000000000))*(r00)*(x310)))+(((IkReal(-1.00000000000000))*(sj0)*(x301)*(x321))));
01307 evalcond[9]=((((IkReal(-1.00000000000000))*(cj1)*(x316)*(x318)))+(((IkReal(-1.00000000000000))*(py)*(x305)*(x319)))+(((IkReal(0.0665000000000000))*(x316)))+(((sj0)*(x301)*(x318)))+(((IkReal(-0.0665000000000000))*(x323)))+(((IkReal(-1.00000000000000))*(x324)*(x332)))+(((IkReal(0.700000000000000))*(x312)))+(((IkReal(-1.00000000000000))*(x304)*(x306)))+(((IkReal(-1.00000000000000))*(x310)*(x334)))+(((IkReal(-0.190000000000000))*(x300)*(x324)))+(((IkReal(-1.00000000000000))*(px)*(x301)*(x305)))+(((IkReal(-1.00000000000000))*(sj1)*(x299)*(x333)))+(((cj0)*(x309)*(x324)))+(((r02)*(x321)))+(((IkReal(-1.00000000000000))*(x304)*(x314)*(x322)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x303)*(x320)))+(((IkReal(-1.00000000000000))*(x305)*(x307)*(x313)))+(((x310)*(x315)))+(((pp)*(sj1)*(x299)))+(((IkReal(-1.00000000000000))*(x299)*(x303)))+(((IkReal(-1.00000000000000))*(x305)*(x310)*(x311)))+(((IkReal(-1.00000000000000))*(x300)*(x314)*(x328)))+(((IkReal(-0.113475000000000))*(x324)))+(((pp)*(x324)))+(((x309)*(x328))));
01308 evalcond[10]=((((IkReal(-1.00000000000000))*(sj1)*(x305)*(x312)))+(((x320)*(x321)))+(((x306)*(x319)))+(((IkReal(-1.00000000000000))*(x301)*(x309)))+(((pp)*(x313)))+(((x299)*(x321)))+(((IkReal(-0.131525000000000))*(cj0)*(x301)))+(((x305)*(x311)*(x330)))+(((r02)*(x309)*(x310)))+(((IkReal(-1.00000000000000))*(cj0)*(x301)*(x325)))+(((IkReal(-0.190000000000000))*(x300)*(x313)))+(((IkReal(-0.131525000000000))*(cj1)*(x299)))+(((IkReal(0.131525000000000))*(x313)))+(((IkReal(-1.00000000000000))*(x299)*(x331)))+(((IkReal(-1.00000000000000))*(py)*(x304)*(x305)))+(((IkReal(-1.00000000000000))*(x311)*(x327)))+(((IkReal(-0.700000000000000))*(r02)*(x307)))+(((x300)*(x301)*(x314)))+(((IkReal(2.00000000000000))*(cj0)*(x297)*(x301)))+(((IkReal(-1.00000000000000))*(x313)*(x332)))+(((cj1)*(x299)*(x333)))+(((x305)*(x307)*(x324)))+(((IkReal(-1.00000000000000))*(cj0)*(x304)*(x318)))+(((sj1)*(x318)*(x323)))+(((cj1)*(px)*(x314)*(x316))));
01309 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  )
01310 {
01311 {
01312 IkReal dummyeval[1];
01313 IkReal gconst3;
01314 gconst3=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
01315 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
01316 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01317 {
01318 {
01319 IkReal dummyeval[1];
01320 IkReal gconst4;
01321 gconst4=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
01322 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
01323 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01324 {
01325 continue;
01326 
01327 } else
01328 {
01329 {
01330 IkReal j2array[1], cj2array[1], sj2array[1];
01331 bool j2valid[1]={false};
01332 _nj2 = 1;
01333 IkReal x335=((cj1)*(cj3));
01334 IkReal x336=((sj0)*(sj1));
01335 IkReal x337=((IkReal(22000.0000000000))*(pz));
01336 IkReal x338=((IkReal(2600.00000000000))*(pz));
01337 IkReal x339=((r02)*(sj1));
01338 IkReal x340=((cj1)*(sj3));
01339 IkReal x341=((cj1)*(r00));
01340 IkReal x342=((r01)*(sj0));
01341 IkReal x343=((cj0)*(sj1));
01342 IkReal x344=((IkReal(2600.00000000000))*(px));
01343 IkReal x345=((IkReal(22000.0000000000))*(px));
01344 IkReal x346=((IkReal(22000.0000000000))*(py));
01345 IkReal x347=((IkReal(2600.00000000000))*(py));
01346 IkReal x348=((IkReal(247.000000000000))*(cj0)*(r00));
01347 IkReal x349=((sj3)*(x347));
01348 IkReal x350=((IkReal(2600.00000000000))*(sj3)*(x343));
01349 if( IKabs(((gconst4)*(((((IkReal(2090.00000000000))*(cj1)*(x342)))+(((IkReal(-2090.00000000000))*(x339)))+(((IkReal(247.000000000000))*(x335)*(x342)))+(((r00)*(x335)*(x347)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x336)*(x338)))+(((IkReal(2090.00000000000))*(cj0)*(x341)))+(((x336)*(x349)))+(((cj3)*(r02)*(x336)*(x344)))+(((IkReal(-1.00000000000000))*(r00)*(x336)*(x337)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x345)))+(((r01)*(x337)*(x343)))+(((x338)*(x340)))+(((sj3)*(x343)*(x344)))+(((IkReal(-247.000000000000))*(cj3)*(x339)))+(((x341)*(x346)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x335)*(x344)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x346)))+(((r02)*(x336)*(x345)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x339)*(x347)))+(((cj3)*(r01)*(x338)*(x343)))+(((x335)*(x348))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj1)*(x337)))+(((IkReal(-1.00000000000000))*(cj3)*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)*(x345)))+(((IkReal(-1.00000000000000))*(r01)*(x340)*(x344)))+(((IkReal(-1.00000000000000))*(x336)*(x346)))+(((IkReal(-1.00000000000000))*(cj3)*(x336)*(x347)))+(((x340)*(x348)))+(((r01)*(sj3)*(x338)*(x343)))+(((IkReal(-247.000000000000))*(sj3)*(x339)))+(((IkReal(-1.00000000000000))*(x335)*(x338)))+(((r02)*(sj3)*(x336)*(x344)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x349)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x336)*(x338)))+(((IkReal(247.000000000000))*(x340)*(x342)))+(((r00)*(x340)*(x347))))))) < IKFAST_ATAN2_MAGTHRESH )
01350     continue;
01351 j2array[0]=IKatan2(((gconst4)*(((((IkReal(2090.00000000000))*(cj1)*(x342)))+(((IkReal(-2090.00000000000))*(x339)))+(((IkReal(247.000000000000))*(x335)*(x342)))+(((r00)*(x335)*(x347)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x336)*(x338)))+(((IkReal(2090.00000000000))*(cj0)*(x341)))+(((x336)*(x349)))+(((cj3)*(r02)*(x336)*(x344)))+(((IkReal(-1.00000000000000))*(r00)*(x336)*(x337)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x345)))+(((r01)*(x337)*(x343)))+(((x338)*(x340)))+(((sj3)*(x343)*(x344)))+(((IkReal(-247.000000000000))*(cj3)*(x339)))+(((x341)*(x346)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x335)*(x344)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x346)))+(((r02)*(x336)*(x345)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x339)*(x347)))+(((cj3)*(r01)*(x338)*(x343)))+(((x335)*(x348)))))), ((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj1)*(x337)))+(((IkReal(-1.00000000000000))*(cj3)*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)*(x345)))+(((IkReal(-1.00000000000000))*(r01)*(x340)*(x344)))+(((IkReal(-1.00000000000000))*(x336)*(x346)))+(((IkReal(-1.00000000000000))*(cj3)*(x336)*(x347)))+(((x340)*(x348)))+(((r01)*(sj3)*(x338)*(x343)))+(((IkReal(-247.000000000000))*(sj3)*(x339)))+(((IkReal(-1.00000000000000))*(x335)*(x338)))+(((r02)*(sj3)*(x336)*(x344)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x349)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x336)*(x338)))+(((IkReal(247.000000000000))*(x340)*(x342)))+(((r00)*(x340)*(x347)))))));
01352 sj2array[0]=IKsin(j2array[0]);
01353 cj2array[0]=IKcos(j2array[0]);
01354 if( j2array[0] > IKPI )
01355 {
01356     j2array[0]-=IK2PI;
01357 }
01358 else if( j2array[0] < -IKPI )
01359 {    j2array[0]+=IK2PI;
01360 }
01361 j2valid[0] = true;
01362 for(int ij2 = 0; ij2 < 1; ++ij2)
01363 {
01364 if( !j2valid[ij2] )
01365 {
01366     continue;
01367 }
01368 _ij2[0] = ij2; _ij2[1] = -1;
01369 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01370 {
01371 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01372 {
01373     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01374 }
01375 }
01376 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01377 {
01378 IkReal evalcond[4];
01379 IkReal x351=IKcos(j2);
01380 IkReal x352=IKsin(j2);
01381 IkReal x353=((px)*(sj1));
01382 IkReal x354=((py)*(sj1));
01383 IkReal x355=((cj1)*(sj0));
01384 IkReal x356=((IkReal(1.00000000000000))*(px));
01385 IkReal x357=((r00)*(sj0));
01386 IkReal x358=((IkReal(0.0950000000000000))*(r01));
01387 IkReal x359=((IkReal(0.0650000000000000))*(cj3));
01388 IkReal x360=((cj0)*(r01));
01389 IkReal x361=((IkReal(0.0950000000000000))*(cj1));
01390 IkReal x362=((pz)*(sj1));
01391 IkReal x363=((cj0)*(r00));
01392 IkReal x364=((IkReal(0.0950000000000000))*(sj1));
01393 IkReal x365=((cj0)*(cj1));
01394 IkReal x366=((cj1)*(pz));
01395 IkReal x367=((IkReal(0.0650000000000000))*(sj3));
01396 IkReal x368=((IkReal(0.550000000000000))*(x352));
01397 IkReal x369=((IkReal(0.550000000000000))*(x351));
01398 IkReal x370=((x352)*(x359));
01399 IkReal x371=((x351)*(x367));
01400 IkReal x372=((x351)*(x359));
01401 IkReal x373=((x352)*(x367));
01402 IkReal x374=((x372)+(x369));
01403 IkReal x375=((x371)+(x370)+(x368));
01404 evalcond[0]=((IkReal(-0.350000000000000))+(x373)+(((IkReal(-1.00000000000000))*(x374)))+(x366)+(((cj0)*(x353)))+(((sj0)*(x354))));
01405 evalcond[1]=((x375)+(((IkReal(-1.00000000000000))*(py)*(x355)))+(x362)+(((IkReal(-1.00000000000000))*(x356)*(x365))));
01406 evalcond[2]=((x375)+(((r02)*(sj0)*(x353)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x356)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x354)))+(((cj1)*(py)*(r00)))+(((x360)*(x362)))+(((IkReal(-1.00000000000000))*(x357)*(x362)))+(((IkReal(-1.00000000000000))*(r02)*(x364)))+(((x355)*(x358)))+(((x361)*(x363))));
01407 evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x355)*(x356)))+(((r02)*(x361)))+(((IkReal(0.350000000000000))*(x360)))+(((py)*(r02)*(x365)))+(x374)+(((IkReal(-1.00000000000000))*(x360)*(x366)))+(((IkReal(-0.350000000000000))*(x357)))+(((IkReal(-1.00000000000000))*(r01)*(x353)))+(((x363)*(x364)))+(((r00)*(x354)))+(((sj0)*(sj1)*(x358)))+(((pz)*(r00)*(x355)))+(((IkReal(-1.00000000000000))*(x373))));
01408 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01409 {
01410 continue;
01411 }
01412 }
01413 
01414 {
01415 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01416 vinfos[0].jointtype = 1;
01417 vinfos[0].foffset = j0;
01418 vinfos[0].indices[0] = _ij0[0];
01419 vinfos[0].indices[1] = _ij0[1];
01420 vinfos[0].maxsolutions = _nj0;
01421 vinfos[1].jointtype = 1;
01422 vinfos[1].foffset = j1;
01423 vinfos[1].indices[0] = _ij1[0];
01424 vinfos[1].indices[1] = _ij1[1];
01425 vinfos[1].maxsolutions = _nj1;
01426 vinfos[2].jointtype = 1;
01427 vinfos[2].foffset = j2;
01428 vinfos[2].indices[0] = _ij2[0];
01429 vinfos[2].indices[1] = _ij2[1];
01430 vinfos[2].maxsolutions = _nj2;
01431 vinfos[3].jointtype = 1;
01432 vinfos[3].foffset = j3;
01433 vinfos[3].indices[0] = _ij3[0];
01434 vinfos[3].indices[1] = _ij3[1];
01435 vinfos[3].maxsolutions = _nj3;
01436 vinfos[4].jointtype = 1;
01437 vinfos[4].foffset = j4;
01438 vinfos[4].indices[0] = _ij4[0];
01439 vinfos[4].indices[1] = _ij4[1];
01440 vinfos[4].maxsolutions = _nj4;
01441 std::vector<int> vfree(0);
01442 solutions.AddSolution(vinfos,vfree);
01443 }
01444 }
01445 }
01446 
01447 }
01448 
01449 }
01450 
01451 } else
01452 {
01453 {
01454 IkReal j2array[1], cj2array[1], sj2array[1];
01455 bool j2valid[1]={false};
01456 _nj2 = 1;
01457 IkReal x376=((IkReal(2600.00000000000))*(sj3));
01458 IkReal x377=((py)*(sj0));
01459 IkReal x378=((pz)*(sj1));
01460 IkReal x379=((IkReal(2600.00000000000))*(cj3));
01461 IkReal x380=((cj1)*(pz));
01462 IkReal x381=((IkReal(22000.0000000000))*(cj1));
01463 IkReal x382=((cj0)*(px));
01464 IkReal x383=((sj1)*(x382));
01465 if( IKabs(((gconst3)*(((((x376)*(x383)))+(((IkReal(22000.0000000000))*(x378)))+(((IkReal(-1.00000000000000))*(x381)*(x382)))+(((sj1)*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(cj1)*(x379)*(x382)))+(((x378)*(x379)))+(((IkReal(-1.00000000000000))*(cj1)*(x377)*(x379)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x377)*(x381)))+(((x376)*(x380))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x377)*(x379)))+(((IkReal(-1.00000000000000))*(x379)*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x377)))+(((IkReal(-22000.0000000000))*(sj1)*(x377)))+(((IkReal(-22000.0000000000))*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x382)))+(((IkReal(-22000.0000000000))*(x383)))+(((IkReal(910.000000000000))*(cj3)))+(((x376)*(x378)))+(((IkReal(-1.00000000000000))*(x379)*(x383))))))) < IKFAST_ATAN2_MAGTHRESH )
01466     continue;
01467 j2array[0]=IKatan2(((gconst3)*(((((x376)*(x383)))+(((IkReal(22000.0000000000))*(x378)))+(((IkReal(-1.00000000000000))*(x381)*(x382)))+(((sj1)*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(cj1)*(x379)*(x382)))+(((x378)*(x379)))+(((IkReal(-1.00000000000000))*(cj1)*(x377)*(x379)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x377)*(x381)))+(((x376)*(x380)))))), ((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x377)*(x379)))+(((IkReal(-1.00000000000000))*(x379)*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x377)))+(((IkReal(-22000.0000000000))*(sj1)*(x377)))+(((IkReal(-22000.0000000000))*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x382)))+(((IkReal(-22000.0000000000))*(x383)))+(((IkReal(910.000000000000))*(cj3)))+(((x376)*(x378)))+(((IkReal(-1.00000000000000))*(x379)*(x383)))))));
01468 sj2array[0]=IKsin(j2array[0]);
01469 cj2array[0]=IKcos(j2array[0]);
01470 if( j2array[0] > IKPI )
01471 {
01472     j2array[0]-=IK2PI;
01473 }
01474 else if( j2array[0] < -IKPI )
01475 {    j2array[0]+=IK2PI;
01476 }
01477 j2valid[0] = true;
01478 for(int ij2 = 0; ij2 < 1; ++ij2)
01479 {
01480 if( !j2valid[ij2] )
01481 {
01482     continue;
01483 }
01484 _ij2[0] = ij2; _ij2[1] = -1;
01485 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01486 {
01487 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01488 {
01489     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01490 }
01491 }
01492 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01493 {
01494 IkReal evalcond[4];
01495 IkReal x384=IKcos(j2);
01496 IkReal x385=IKsin(j2);
01497 IkReal x386=((px)*(sj1));
01498 IkReal x387=((py)*(sj1));
01499 IkReal x388=((cj1)*(sj0));
01500 IkReal x389=((IkReal(1.00000000000000))*(px));
01501 IkReal x390=((r00)*(sj0));
01502 IkReal x391=((IkReal(0.0950000000000000))*(r01));
01503 IkReal x392=((IkReal(0.0650000000000000))*(cj3));
01504 IkReal x393=((cj0)*(r01));
01505 IkReal x394=((IkReal(0.0950000000000000))*(cj1));
01506 IkReal x395=((pz)*(sj1));
01507 IkReal x396=((cj0)*(r00));
01508 IkReal x397=((IkReal(0.0950000000000000))*(sj1));
01509 IkReal x398=((cj0)*(cj1));
01510 IkReal x399=((cj1)*(pz));
01511 IkReal x400=((IkReal(0.0650000000000000))*(sj3));
01512 IkReal x401=((IkReal(0.550000000000000))*(x385));
01513 IkReal x402=((IkReal(0.550000000000000))*(x384));
01514 IkReal x403=((x385)*(x392));
01515 IkReal x404=((x384)*(x400));
01516 IkReal x405=((x384)*(x392));
01517 IkReal x406=((x385)*(x400));
01518 IkReal x407=((x402)+(x405));
01519 IkReal x408=((x401)+(x403)+(x404));
01520 evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x387)))+(((IkReal(-1.00000000000000))*(x407)))+(((cj0)*(x386)))+(x406)+(x399));
01521 evalcond[1]=((((IkReal(-1.00000000000000))*(x389)*(x398)))+(((IkReal(-1.00000000000000))*(py)*(x388)))+(x408)+(x395));
01522 evalcond[2]=((((IkReal(-1.00000000000000))*(x390)*(x395)))+(((x388)*(x391)))+(((x393)*(x395)))+(((cj1)*(py)*(r00)))+(((r02)*(sj0)*(x386)))+(((x394)*(x396)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x389)))+(((IkReal(-1.00000000000000))*(r02)*(x397)))+(x408)+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x387))));
01523 evalcond[3]=((((pz)*(r00)*(x388)))+(((IkReal(-1.00000000000000))*(r01)*(x386)))+(((IkReal(-1.00000000000000))*(r02)*(x388)*(x389)))+(((r00)*(x387)))+(((r02)*(x394)))+(((sj0)*(sj1)*(x391)))+(((IkReal(-0.350000000000000))*(x390)))+(((IkReal(-1.00000000000000))*(x406)))+(((IkReal(-1.00000000000000))*(x393)*(x399)))+(x407)+(((x396)*(x397)))+(((py)*(r02)*(x398)))+(((IkReal(0.350000000000000))*(x393))));
01524 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01525 {
01526 continue;
01527 }
01528 }
01529 
01530 {
01531 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01532 vinfos[0].jointtype = 1;
01533 vinfos[0].foffset = j0;
01534 vinfos[0].indices[0] = _ij0[0];
01535 vinfos[0].indices[1] = _ij0[1];
01536 vinfos[0].maxsolutions = _nj0;
01537 vinfos[1].jointtype = 1;
01538 vinfos[1].foffset = j1;
01539 vinfos[1].indices[0] = _ij1[0];
01540 vinfos[1].indices[1] = _ij1[1];
01541 vinfos[1].maxsolutions = _nj1;
01542 vinfos[2].jointtype = 1;
01543 vinfos[2].foffset = j2;
01544 vinfos[2].indices[0] = _ij2[0];
01545 vinfos[2].indices[1] = _ij2[1];
01546 vinfos[2].maxsolutions = _nj2;
01547 vinfos[3].jointtype = 1;
01548 vinfos[3].foffset = j3;
01549 vinfos[3].indices[0] = _ij3[0];
01550 vinfos[3].indices[1] = _ij3[1];
01551 vinfos[3].maxsolutions = _nj3;
01552 vinfos[4].jointtype = 1;
01553 vinfos[4].foffset = j4;
01554 vinfos[4].indices[0] = _ij4[0];
01555 vinfos[4].indices[1] = _ij4[1];
01556 vinfos[4].maxsolutions = _nj4;
01557 std::vector<int> vfree(0);
01558 solutions.AddSolution(vinfos,vfree);
01559 }
01560 }
01561 }
01562 
01563 }
01564 
01565 }
01566 
01567 } else
01568 {
01569 IkReal x409=((IkReal(0.0715000000000000))*(cj3));
01570 IkReal x410=(py)*(py);
01571 IkReal x411=(px)*(px);
01572 IkReal x412=(pz)*(pz);
01573 IkReal x413=((r01)*(sj0));
01574 IkReal x414=((px)*(sj0));
01575 IkReal x415=((cj1)*(r00));
01576 IkReal x416=((IkReal(0.350000000000000))*(cj0));
01577 IkReal x417=((IkReal(0.113475000000000))*(sj1));
01578 IkReal x418=((r01)*(sj1));
01579 IkReal x419=((IkReal(2.00000000000000))*(pz));
01580 IkReal x420=((IkReal(0.190000000000000))*(px));
01581 IkReal x421=((py)*(sj0));
01582 IkReal x422=((IkReal(1.00000000000000))*(r02));
01583 IkReal x423=((IkReal(0.190000000000000))*(py));
01584 IkReal x424=((cj0)*(sj1));
01585 IkReal x425=((px)*(r02));
01586 IkReal x426=((px)*(r00));
01587 IkReal x427=((r02)*(sj1));
01588 IkReal x428=((IkReal(2.00000000000000))*(py));
01589 IkReal x429=((pp)*(r00));
01590 IkReal x430=((cj0)*(r01));
01591 IkReal x431=((IkReal(0.700000000000000))*(sj1));
01592 IkReal x432=((IkReal(0.190000000000000))*(pz));
01593 IkReal x433=((cj1)*(r01));
01594 IkReal x434=((cj0)*(r00));
01595 IkReal x435=((IkReal(0.700000000000000))*(pz));
01596 IkReal x436=((cj0)*(px));
01597 IkReal x437=((r00)*(sj0));
01598 IkReal x438=((cj1)*(r02));
01599 IkReal x439=((IkReal(1.00000000000000))*(pp));
01600 IkReal x440=((IkReal(1.00000000000000))*(py));
01601 IkReal x441=((IkReal(0.700000000000000))*(cj0));
01602 IkReal x442=((r00)*(sj1));
01603 IkReal x443=((IkReal(0.700000000000000))*(px));
01604 IkReal x444=((cj0)*(cj1));
01605 IkReal x445=((cj1)*(x439));
01606 IkReal x446=((IkReal(2.00000000000000))*(x412));
01607 IkReal x447=((IkReal(2.00000000000000))*(x410));
01608 IkReal x448=((IkReal(2.00000000000000))*(r00)*(x411));
01609 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
01610 evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x440)))+(x414));
01611 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x430)))+(x437));
01612 evalcond[3]=((x438)+(((sj1)*(x413)))+(((r00)*(x424))));
01613 evalcond[4]=((((IkReal(-1.00000000000000))*(cj0)*(x415)))+(x427)+(((IkReal(-1.00000000000000))*(cj1)*(x413))));
01614 evalcond[5]=((IkReal(0.175200000000000))+(((x424)*(x443)))+(((cj1)*(x435)))+(((x421)*(x431)))+(x409)+(((IkReal(-1.00000000000000))*(cj0)*(x423)))+(((IkReal(-1.00000000000000))*(x439)))+(((IkReal(0.190000000000000))*(x414))));
01615 evalcond[6]=((((IkReal(-1.00000000000000))*(x426)))+(((IkReal(0.0950000000000000))*(x437)))+(((IkReal(0.350000000000000))*(x438)))+(((IkReal(-1.00000000000000))*(pz)*(x422)))+(((x416)*(x442)))+(((IkReal(-1.00000000000000))*(r01)*(x440)))+(((IkReal(-0.0950000000000000))*(x430)))+(((IkReal(0.350000000000000))*(sj1)*(x413))));
01616 evalcond[7]=((((IkReal(-1.00000000000000))*(x415)*(x416)))+(((pz)*(x413)))+(((IkReal(0.350000000000000))*(x427)))+(((IkReal(-1.00000000000000))*(x421)*(x422)))+(((IkReal(-0.350000000000000))*(cj1)*(x413)))+(((pz)*(x434)))+(((IkReal(-1.00000000000000))*(x422)*(x436))));
01617 evalcond[8]=((IkReal(-0.306725000000000))+(((IkReal(0.113475000000000))*(x437)))+(((IkReal(-0.113475000000000))*(x430)))+(((x418)*(x443)))+(((r02)*(x432)))+(((IkReal(-1.00000000000000))*(x430)*(x439)))+(((IkReal(-2.00000000000000))*(x411)*(x437)))+(((sj0)*(x429)))+(((IkReal(-0.0665000000000000))*(x438)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x431)))+(((r00)*(x420)))+(((IkReal(-1.00000000000000))*(x409)))+(((cj0)*(py)*(r02)*(x419)))+(((IkReal(0.700000000000000))*(x414)*(x438)))+(((cj1)*(x430)*(x435)))+(((IkReal(-1.00000000000000))*(sj0)*(x415)*(x435)))+(((IkReal(-1.00000000000000))*(r02)*(x414)*(x419)))+(((r01)*(x423)))+(((x430)*(x447)))+(((IkReal(-1.00000000000000))*(px)*(x413)*(x428)))+(((IkReal(-0.0665000000000000))*(sj1)*(x413)))+(((IkReal(-1.00000000000000))*(py)*(x438)*(x441)))+(((cj0)*(x426)*(x428)))+(((IkReal(-0.0665000000000000))*(r00)*(x424))));
01618 evalcond[9]=((((IkReal(-1.00000000000000))*(x438)*(x446)))+(((IkReal(-1.00000000000000))*(py)*(x419)*(x433)))+(((IkReal(-0.190000000000000))*(x414)*(x438)))+(((IkReal(-1.00000000000000))*(sj1)*(x413)*(x447)))+(((IkReal(-1.00000000000000))*(x419)*(x421)*(x427)))+(((x423)*(x442)))+(((pp)*(x438)))+(((IkReal(-1.00000000000000))*(x418)*(x428)*(x436)))+(((x424)*(x429)))+(((IkReal(-1.00000000000000))*(px)*(x415)*(x419)))+(((IkReal(-1.00000000000000))*(x419)*(x424)*(x425)))+(((IkReal(-1.00000000000000))*(x414)*(x428)*(x442)))+(((IkReal(-0.113475000000000))*(x438)))+(((IkReal(0.0665000000000000))*(x430)))+(((cj0)*(x423)*(x438)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x424)*(x448)))+(((sj0)*(x415)*(x432)))+(((r02)*(x435)))+(((IkReal(-1.00000000000000))*(x413)*(x417)))+(((IkReal(0.700000000000000))*(x426)))+(((pp)*(sj1)*(x413)))+(((IkReal(-1.00000000000000))*(cj1)*(x430)*(x432)))+(((IkReal(-1.00000000000000))*(x417)*(x434)))+(((IkReal(-1.00000000000000))*(x418)*(x420)))+(((IkReal(-0.0665000000000000))*(x437))));
01619 evalcond[10]=((((IkReal(-1.00000000000000))*(x427)*(x446)))+(((sj1)*(x432)*(x437)))+(((IkReal(-0.131525000000000))*(cj1)*(x413)))+(((x434)*(x435)))+(((IkReal(-1.00000000000000))*(x413)*(x445)))+(((cj1)*(px)*(x428)*(x430)))+(((IkReal(-1.00000000000000))*(sj1)*(x419)*(x426)))+(((r02)*(x423)*(x424)))+(((pp)*(x427)))+(((IkReal(-0.131525000000000))*(cj0)*(x415)))+(((x414)*(x415)*(x428)))+(((x420)*(x433)))+(((IkReal(-1.00000000000000))*(cj0)*(x415)*(x439)))+(((IkReal(-1.00000000000000))*(x425)*(x441)))+(((IkReal(-1.00000000000000))*(x415)*(x423)))+(((x413)*(x435)))+(((IkReal(-0.190000000000000))*(x414)*(x427)))+(((x419)*(x425)*(x444)))+(((IkReal(2.00000000000000))*(cj0)*(x411)*(x415)))+(((IkReal(-1.00000000000000))*(cj0)*(x418)*(x432)))+(((IkReal(-1.00000000000000))*(py)*(x418)*(x419)))+(((x419)*(x421)*(x438)))+(((IkReal(0.131525000000000))*(x427)))+(((IkReal(-0.700000000000000))*(r02)*(x421)))+(((cj1)*(x413)*(x447))));
01620 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  )
01621 {
01622 {
01623 IkReal dummyeval[1];
01624 IkReal gconst5;
01625 gconst5=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
01626 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
01627 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01628 {
01629 {
01630 IkReal dummyeval[1];
01631 IkReal gconst6;
01632 gconst6=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
01633 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
01634 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01635 {
01636 continue;
01637 
01638 } else
01639 {
01640 {
01641 IkReal j2array[1], cj2array[1], sj2array[1];
01642 bool j2valid[1]={false};
01643 _nj2 = 1;
01644 IkReal x449=((cj1)*(cj3));
01645 IkReal x450=((sj0)*(sj1));
01646 IkReal x451=((IkReal(22000.0000000000))*(pz));
01647 IkReal x452=((IkReal(2600.00000000000))*(pz));
01648 IkReal x453=((r02)*(sj1));
01649 IkReal x454=((IkReal(2600.00000000000))*(sj3));
01650 IkReal x455=((cj1)*(sj3));
01651 IkReal x456=((r01)*(sj0));
01652 IkReal x457=((cj0)*(sj1));
01653 IkReal x458=((IkReal(2600.00000000000))*(cj3));
01654 IkReal x459=((IkReal(2090.00000000000))*(cj1));
01655 IkReal x460=((IkReal(22000.0000000000))*(px));
01656 IkReal x461=((px)*(r02));
01657 IkReal x462=((IkReal(22000.0000000000))*(py));
01658 IkReal x463=((py)*(r00));
01659 IkReal x464=((px)*(r01));
01660 IkReal x465=((IkReal(247.000000000000))*(cj0)*(r00));
01661 IkReal x466=((x454)*(x457));
01662 if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x450)*(x458)*(x461)))+(((cj3)*(r00)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(r02)*(x450)*(x460)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x462)))+(((IkReal(247.000000000000))*(cj3)*(x453)))+(((IkReal(-1.00000000000000))*(x456)*(x459)))+(((IkReal(2600.00000000000))*(x449)*(x464)))+(((px)*(x466)))+(((IkReal(2090.00000000000))*(x453)))+(((IkReal(-2600.00000000000))*(x449)*(x463)))+(((IkReal(-1.00000000000000))*(r01)*(x451)*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x465)))+(((cj0)*(x453)*(x462)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x449)*(x456)))+(((cj1)*(r01)*(x460)))+(((x452)*(x455)))+(((py)*(x450)*(x454)))+(((r00)*(x450)*(x451)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x459)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x452)*(x457)))+(((cj0)*(py)*(x453)*(x458))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x454)*(x464)))+(((IkReal(-1.00000000000000))*(x450)*(x454)*(x461)))+(((IkReal(-1.00000000000000))*(x449)*(x452)))+(((r00)*(sj3)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(py)*(x450)*(x458)))+(((IkReal(-247.000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(px)*(x457)*(x458)))+(((IkReal(247.000000000000))*(sj3)*(x453)))+(((IkReal(-1.00000000000000))*(x455)*(x465)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x452)*(x457)))+(((IkReal(-1.00000000000000))*(x457)*(x460)))+(((IkReal(-1.00000000000000))*(x450)*(x462)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x451)))+(((IkReal(-1.00000000000000))*(cj1)*(x454)*(x463)))+(((cj0)*(py)*(x453)*(x454))))))) < IKFAST_ATAN2_MAGTHRESH )
01663     continue;
01664 j2array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(x450)*(x458)*(x461)))+(((cj3)*(r00)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(r02)*(x450)*(x460)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x462)))+(((IkReal(247.000000000000))*(cj3)*(x453)))+(((IkReal(-1.00000000000000))*(x456)*(x459)))+(((IkReal(2600.00000000000))*(x449)*(x464)))+(((px)*(x466)))+(((IkReal(2090.00000000000))*(x453)))+(((IkReal(-2600.00000000000))*(x449)*(x463)))+(((IkReal(-1.00000000000000))*(r01)*(x451)*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x465)))+(((cj0)*(x453)*(x462)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x449)*(x456)))+(((cj1)*(r01)*(x460)))+(((x452)*(x455)))+(((py)*(x450)*(x454)))+(((r00)*(x450)*(x451)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x459)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x452)*(x457)))+(((cj0)*(py)*(x453)*(x458)))))), ((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x454)*(x464)))+(((IkReal(-1.00000000000000))*(x450)*(x454)*(x461)))+(((IkReal(-1.00000000000000))*(x449)*(x452)))+(((r00)*(sj3)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(py)*(x450)*(x458)))+(((IkReal(-247.000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(px)*(x457)*(x458)))+(((IkReal(247.000000000000))*(sj3)*(x453)))+(((IkReal(-1.00000000000000))*(x455)*(x465)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x452)*(x457)))+(((IkReal(-1.00000000000000))*(x457)*(x460)))+(((IkReal(-1.00000000000000))*(x450)*(x462)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x451)))+(((IkReal(-1.00000000000000))*(cj1)*(x454)*(x463)))+(((cj0)*(py)*(x453)*(x454)))))));
01665 sj2array[0]=IKsin(j2array[0]);
01666 cj2array[0]=IKcos(j2array[0]);
01667 if( j2array[0] > IKPI )
01668 {
01669     j2array[0]-=IK2PI;
01670 }
01671 else if( j2array[0] < -IKPI )
01672 {    j2array[0]+=IK2PI;
01673 }
01674 j2valid[0] = true;
01675 for(int ij2 = 0; ij2 < 1; ++ij2)
01676 {
01677 if( !j2valid[ij2] )
01678 {
01679     continue;
01680 }
01681 _ij2[0] = ij2; _ij2[1] = -1;
01682 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01683 {
01684 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01685 {
01686     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01687 }
01688 }
01689 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01690 {
01691 IkReal evalcond[4];
01692 IkReal x467=IKcos(j2);
01693 IkReal x468=IKsin(j2);
01694 IkReal x469=((px)*(sj1));
01695 IkReal x470=((py)*(sj1));
01696 IkReal x471=((cj1)*(sj0));
01697 IkReal x472=((IkReal(1.00000000000000))*(px));
01698 IkReal x473=((r00)*(sj0));
01699 IkReal x474=((IkReal(0.0950000000000000))*(r01));
01700 IkReal x475=((IkReal(0.0650000000000000))*(cj3));
01701 IkReal x476=((cj0)*(r01));
01702 IkReal x477=((IkReal(0.0950000000000000))*(cj1));
01703 IkReal x478=((pz)*(sj1));
01704 IkReal x479=((cj0)*(r00));
01705 IkReal x480=((IkReal(0.0950000000000000))*(sj1));
01706 IkReal x481=((cj0)*(cj1));
01707 IkReal x482=((cj1)*(pz));
01708 IkReal x483=((IkReal(0.0650000000000000))*(sj3));
01709 IkReal x484=((IkReal(0.550000000000000))*(x468));
01710 IkReal x485=((IkReal(0.550000000000000))*(x467));
01711 IkReal x486=((x468)*(x475));
01712 IkReal x487=((x467)*(x483));
01713 IkReal x488=((x468)*(x483));
01714 IkReal x489=((x467)*(x475));
01715 IkReal x490=((x489)+(x485));
01716 IkReal x491=((x484)+(x487)+(x486));
01717 evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x490)))+(((sj0)*(x470)))+(x488)+(x482)+(((cj0)*(x469))));
01718 evalcond[1]=((x478)+(((IkReal(-1.00000000000000))*(py)*(x471)))+(x491)+(((IkReal(-1.00000000000000))*(x472)*(x481))));
01719 evalcond[2]=((((IkReal(-1.00000000000000))*(x491)))+(((cj1)*(py)*(r00)))+(((x476)*(x478)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x470)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x472)))+(((IkReal(-1.00000000000000))*(r02)*(x480)))+(((IkReal(-1.00000000000000))*(x473)*(x478)))+(((x477)*(x479)))+(((x471)*(x474)))+(((r02)*(sj0)*(x469))));
01720 evalcond[3]=((((x479)*(x480)))+(((IkReal(-1.00000000000000))*(r01)*(x469)))+(((pz)*(r00)*(x471)))+(((IkReal(-1.00000000000000))*(x490)))+(((IkReal(-1.00000000000000))*(x476)*(x482)))+(((r02)*(x477)))+(((py)*(r02)*(x481)))+(((r00)*(x470)))+(((IkReal(-0.350000000000000))*(x473)))+(x488)+(((IkReal(0.350000000000000))*(x476)))+(((IkReal(-1.00000000000000))*(r02)*(x471)*(x472)))+(((sj0)*(sj1)*(x474))));
01721 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01722 {
01723 continue;
01724 }
01725 }
01726 
01727 {
01728 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01729 vinfos[0].jointtype = 1;
01730 vinfos[0].foffset = j0;
01731 vinfos[0].indices[0] = _ij0[0];
01732 vinfos[0].indices[1] = _ij0[1];
01733 vinfos[0].maxsolutions = _nj0;
01734 vinfos[1].jointtype = 1;
01735 vinfos[1].foffset = j1;
01736 vinfos[1].indices[0] = _ij1[0];
01737 vinfos[1].indices[1] = _ij1[1];
01738 vinfos[1].maxsolutions = _nj1;
01739 vinfos[2].jointtype = 1;
01740 vinfos[2].foffset = j2;
01741 vinfos[2].indices[0] = _ij2[0];
01742 vinfos[2].indices[1] = _ij2[1];
01743 vinfos[2].maxsolutions = _nj2;
01744 vinfos[3].jointtype = 1;
01745 vinfos[3].foffset = j3;
01746 vinfos[3].indices[0] = _ij3[0];
01747 vinfos[3].indices[1] = _ij3[1];
01748 vinfos[3].maxsolutions = _nj3;
01749 vinfos[4].jointtype = 1;
01750 vinfos[4].foffset = j4;
01751 vinfos[4].indices[0] = _ij4[0];
01752 vinfos[4].indices[1] = _ij4[1];
01753 vinfos[4].maxsolutions = _nj4;
01754 std::vector<int> vfree(0);
01755 solutions.AddSolution(vinfos,vfree);
01756 }
01757 }
01758 }
01759 
01760 }
01761 
01762 }
01763 
01764 } else
01765 {
01766 {
01767 IkReal j2array[1], cj2array[1], sj2array[1];
01768 bool j2valid[1]={false};
01769 _nj2 = 1;
01770 IkReal x492=((IkReal(2600.00000000000))*(sj3));
01771 IkReal x493=((py)*(sj0));
01772 IkReal x494=((pz)*(sj1));
01773 IkReal x495=((IkReal(2600.00000000000))*(cj3));
01774 IkReal x496=((cj1)*(pz));
01775 IkReal x497=((IkReal(22000.0000000000))*(cj1));
01776 IkReal x498=((cj0)*(px));
01777 IkReal x499=((sj1)*(x498));
01778 if( IKabs(((gconst5)*(((((x492)*(x499)))+(((sj1)*(x492)*(x493)))+(((x494)*(x495)))+(((x492)*(x496)))+(((IkReal(22000.0000000000))*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x495)*(x498)))+(((IkReal(-1.00000000000000))*(cj1)*(x493)*(x495)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x497)*(x498)))+(((IkReal(-1.00000000000000))*(x493)*(x497))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x496)))+(((IkReal(-1.00000000000000))*(x495)*(x499)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x493)))+(((IkReal(-1.00000000000000))*(x495)*(x496)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(sj1)*(x493)))+(((IkReal(-1.00000000000000))*(sj1)*(x493)*(x495)))+(((IkReal(-22000.0000000000))*(x499)))+(((x492)*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x498))))))) < IKFAST_ATAN2_MAGTHRESH )
01779     continue;
01780 j2array[0]=IKatan2(((gconst5)*(((((x492)*(x499)))+(((sj1)*(x492)*(x493)))+(((x494)*(x495)))+(((x492)*(x496)))+(((IkReal(22000.0000000000))*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x495)*(x498)))+(((IkReal(-1.00000000000000))*(cj1)*(x493)*(x495)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x497)*(x498)))+(((IkReal(-1.00000000000000))*(x493)*(x497)))))), ((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x496)))+(((IkReal(-1.00000000000000))*(x495)*(x499)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x493)))+(((IkReal(-1.00000000000000))*(x495)*(x496)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(sj1)*(x493)))+(((IkReal(-1.00000000000000))*(sj1)*(x493)*(x495)))+(((IkReal(-22000.0000000000))*(x499)))+(((x492)*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x498)))))));
01781 sj2array[0]=IKsin(j2array[0]);
01782 cj2array[0]=IKcos(j2array[0]);
01783 if( j2array[0] > IKPI )
01784 {
01785     j2array[0]-=IK2PI;
01786 }
01787 else if( j2array[0] < -IKPI )
01788 {    j2array[0]+=IK2PI;
01789 }
01790 j2valid[0] = true;
01791 for(int ij2 = 0; ij2 < 1; ++ij2)
01792 {
01793 if( !j2valid[ij2] )
01794 {
01795     continue;
01796 }
01797 _ij2[0] = ij2; _ij2[1] = -1;
01798 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01799 {
01800 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01801 {
01802     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01803 }
01804 }
01805 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01806 {
01807 IkReal evalcond[4];
01808 IkReal x500=IKcos(j2);
01809 IkReal x501=IKsin(j2);
01810 IkReal x502=((px)*(sj1));
01811 IkReal x503=((py)*(sj1));
01812 IkReal x504=((cj1)*(sj0));
01813 IkReal x505=((IkReal(1.00000000000000))*(px));
01814 IkReal x506=((r00)*(sj0));
01815 IkReal x507=((IkReal(0.0950000000000000))*(r01));
01816 IkReal x508=((IkReal(0.0650000000000000))*(cj3));
01817 IkReal x509=((cj0)*(r01));
01818 IkReal x510=((IkReal(0.0950000000000000))*(cj1));
01819 IkReal x511=((pz)*(sj1));
01820 IkReal x512=((cj0)*(r00));
01821 IkReal x513=((IkReal(0.0950000000000000))*(sj1));
01822 IkReal x514=((cj0)*(cj1));
01823 IkReal x515=((cj1)*(pz));
01824 IkReal x516=((IkReal(0.0650000000000000))*(sj3));
01825 IkReal x517=((IkReal(0.550000000000000))*(x501));
01826 IkReal x518=((IkReal(0.550000000000000))*(x500));
01827 IkReal x519=((x501)*(x508));
01828 IkReal x520=((x500)*(x516));
01829 IkReal x521=((x501)*(x516));
01830 IkReal x522=((x500)*(x508));
01831 IkReal x523=((x518)+(x522));
01832 IkReal x524=((x519)+(x517)+(x520));
01833 evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x503)))+(x515)+(((cj0)*(x502)))+(((IkReal(-1.00000000000000))*(x523)))+(x521));
01834 evalcond[1]=((x511)+(((IkReal(-1.00000000000000))*(x505)*(x514)))+(((IkReal(-1.00000000000000))*(py)*(x504)))+(x524));
01835 evalcond[2]=((((x509)*(x511)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x505)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x503)))+(((IkReal(-1.00000000000000))*(x524)))+(((IkReal(-1.00000000000000))*(x506)*(x511)))+(((cj1)*(py)*(r00)))+(((x510)*(x512)))+(((x504)*(x507)))+(((IkReal(-1.00000000000000))*(r02)*(x513)))+(((r02)*(sj0)*(x502))));
01836 evalcond[3]=((((sj0)*(sj1)*(x507)))+(((r02)*(x510)))+(((pz)*(r00)*(x504)))+(((r00)*(x503)))+(((py)*(r02)*(x514)))+(((x512)*(x513)))+(((IkReal(-1.00000000000000))*(x509)*(x515)))+(((IkReal(-0.350000000000000))*(x506)))+(((IkReal(-1.00000000000000))*(r02)*(x504)*(x505)))+(((IkReal(0.350000000000000))*(x509)))+(((IkReal(-1.00000000000000))*(x523)))+(x521)+(((IkReal(-1.00000000000000))*(r01)*(x502))));
01837 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01838 {
01839 continue;
01840 }
01841 }
01842 
01843 {
01844 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
01845 vinfos[0].jointtype = 1;
01846 vinfos[0].foffset = j0;
01847 vinfos[0].indices[0] = _ij0[0];
01848 vinfos[0].indices[1] = _ij0[1];
01849 vinfos[0].maxsolutions = _nj0;
01850 vinfos[1].jointtype = 1;
01851 vinfos[1].foffset = j1;
01852 vinfos[1].indices[0] = _ij1[0];
01853 vinfos[1].indices[1] = _ij1[1];
01854 vinfos[1].maxsolutions = _nj1;
01855 vinfos[2].jointtype = 1;
01856 vinfos[2].foffset = j2;
01857 vinfos[2].indices[0] = _ij2[0];
01858 vinfos[2].indices[1] = _ij2[1];
01859 vinfos[2].maxsolutions = _nj2;
01860 vinfos[3].jointtype = 1;
01861 vinfos[3].foffset = j3;
01862 vinfos[3].indices[0] = _ij3[0];
01863 vinfos[3].indices[1] = _ij3[1];
01864 vinfos[3].maxsolutions = _nj3;
01865 vinfos[4].jointtype = 1;
01866 vinfos[4].foffset = j4;
01867 vinfos[4].indices[0] = _ij4[0];
01868 vinfos[4].indices[1] = _ij4[1];
01869 vinfos[4].maxsolutions = _nj4;
01870 std::vector<int> vfree(0);
01871 solutions.AddSolution(vinfos,vfree);
01872 }
01873 }
01874 }
01875 
01876 }
01877 
01878 }
01879 
01880 } else
01881 {
01882 IkReal x525=((r01)*(sj1));
01883 IkReal x526=((IkReal(0.350000000000000))*(sj0));
01884 IkReal x527=((px)*(r02));
01885 IkReal x528=((IkReal(1.00000000000000))*(cj0));
01886 IkReal x529=((cj0)*(r00));
01887 IkReal x530=((IkReal(0.350000000000000))*(cj1));
01888 IkReal x531=((IkReal(0.700000000000000))*(sj1));
01889 IkReal x532=((IkReal(0.190000000000000))*(px));
01890 IkReal x533=((r00)*(sj0));
01891 IkReal x534=((px)*(sj0));
01892 IkReal x535=((IkReal(0.700000000000000))*(cj1));
01893 IkReal x536=((py)*(r02));
01894 IkReal x537=((IkReal(0.190000000000000))*(py));
01895 IkReal x538=((IkReal(2.00000000000000))*(py));
01896 IkReal x539=((pz)*(r01));
01897 IkReal x540=((IkReal(2.00000000000000))*(cj0));
01898 IkReal x541=((IkReal(0.350000000000000))*(sj1));
01899 IkReal x542=((pz)*(r02));
01900 IkReal x543=((cj0)*(r01));
01901 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.45233765858574))+(j3)), IkReal(6.28318530717959))));
01902 evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x528)))+(x534));
01903 evalcond[2]=((sj4)+(((IkReal(-1.00000000000000))*(r01)*(x528)))+(x533));
01904 evalcond[3]=((IkReal(0.166749999870000))+(((cj0)*(px)*(x531)))+(((sj0)*(x532)))+(((pz)*(x535)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj0)*(x537)))+(((py)*(sj0)*(x531))));
01905 evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-0.546145583500000))*(cj4)))+(((r02)*(x530)))+(((IkReal(0.0950000000000000))*(x533)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x542)))+(((x529)*(x541)))+(((IkReal(-0.0950000000000000))*(x543)))+(((x525)*(x526))));
01906 evalcond[5]=((((IkReal(-1.00000000000000))*(sj0)*(x536)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(x527)*(x528)))+(((pz)*(x529)))+(((sj0)*(x539)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x526)))+(((r02)*(x541)))+(((IkReal(-1.00000000000000))*(x529)*(x530))));
01907 evalcond[6]=((((px)*(x529)*(x538)))+(((IkReal(-0.0665000000000000))*(sj0)*(x525)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x528)))+(((pz)*(x536)*(x540)))+(((sj0)*(x527)*(x535)))+(((IkReal(-0.113475000000000))*(x543)))+(((pp)*(x533)))+(((IkReal(-2.00000000000000))*(x533)*((px)*(px))))+(((IkReal(-1.00000000000000))*(r01)*(x534)*(x538)))+(((IkReal(-0.0665000000000000))*(sj1)*(x529)))+(((IkReal(-1.00000000000000))*(cj0)*(x535)*(x536)))+(((IkReal(-1.00000000000000))*(pz)*(x533)*(x535)))+(((IkReal(0.298274999870000))*(sj4)))+(((r01)*(x537)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x527)))+(((IkReal(0.113475000000000))*(x533)))+(((IkReal(0.700000000000000))*(px)*(x525)))+(((cj0)*(x535)*(x539)))+(((r00)*(x532)))+(((py)*(x538)*(x543)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x531)))+(((IkReal(0.190000000000000))*(x542))));
01908 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  )
01909 {
01910 {
01911 IkReal j2array[1], cj2array[1], sj2array[1];
01912 bool j2valid[1]={false};
01913 _nj2 = 1;
01914 IkReal x544=((IkReal(1.81818181870518))*(sj1));
01915 IkReal x545=((py)*(sj0));
01916 IkReal x546=((IkReal(0.216392517249668))*(sj1));
01917 IkReal x547=((cj0)*(px));
01918 IkReal x548=((IkReal(0.216392517249668))*(cj1));
01919 IkReal x549=((IkReal(1.81818181870518))*(cj1));
01920 if( IKabs(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547)))))+IKsqr(((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545)))))-1) <= IKFAST_SINCOS_THRESH )
01921     continue;
01922 j2array[0]=IKatan2(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547)))), ((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545)))));
01923 sj2array[0]=IKsin(j2array[0]);
01924 cj2array[0]=IKcos(j2array[0]);
01925 if( j2array[0] > IKPI )
01926 {
01927     j2array[0]-=IK2PI;
01928 }
01929 else if( j2array[0] < -IKPI )
01930 {    j2array[0]+=IK2PI;
01931 }
01932 j2valid[0] = true;
01933 for(int ij2 = 0; ij2 < 1; ++ij2)
01934 {
01935 if( !j2valid[ij2] )
01936 {
01937     continue;
01938 }
01939 _ij2[0] = ij2; _ij2[1] = -1;
01940 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
01941 {
01942 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
01943 {
01944     j2valid[iij2]=false; _ij2[1] = iij2; break; 
01945 }
01946 }
01947 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
01948 {
01949 IkReal evalcond[8];
01950 IkReal x550=IKsin(j2);
01951 IkReal x551=IKcos(j2);
01952 IkReal x552=(py)*(py);
01953 IkReal x553=(px)*(px);
01954 IkReal x554=(pz)*(pz);
01955 IkReal x555=((sj0)*(sj1));
01956 IkReal x556=((py)*(r00));
01957 IkReal x557=((IkReal(0.190000000000000))*(cj1));
01958 IkReal x558=((cj0)*(r02));
01959 IkReal x559=((py)*(r01));
01960 IkReal x560=((px)*(r01));
01961 IkReal x561=((IkReal(0.190000000000000))*(sj1));
01962 IkReal x562=((cj1)*(r02));
01963 IkReal x563=((pz)*(r00));
01964 IkReal x564=((cj1)*(sj0));
01965 IkReal x565=((cj0)*(r00));
01966 IkReal x566=((IkReal(2.00000000000000))*(sj1));
01967 IkReal x567=((IkReal(0.700000000000000))*(px));
01968 IkReal x568=((IkReal(1.00000000000000))*(cj1));
01969 IkReal x569=((IkReal(2.00000000000000))*(px));
01970 IkReal x570=((pp)*(r01));
01971 IkReal x571=((pp)*(sj1));
01972 IkReal x572=((r02)*(sj1));
01973 IkReal x573=((pz)*(r01));
01974 IkReal x574=((px)*(r02));
01975 IkReal x575=((IkReal(0.700000000000000))*(sj0));
01976 IkReal x576=((cj0)*(sj1));
01977 IkReal x577=((r00)*(sj0));
01978 IkReal x578=((cj0)*(r01));
01979 IkReal x579=((IkReal(1.00000000000000))*(py));
01980 IkReal x580=((pz)*(r02));
01981 IkReal x581=((IkReal(2.00000000000000))*(py));
01982 IkReal x582=((cj1)*(pz));
01983 IkReal x583=((IkReal(0.0950000000000000))*(r01));
01984 IkReal x584=((cj0)*(px));
01985 IkReal x585=((pz)*(x566));
01986 IkReal x586=((cj4)*(x550));
01987 IkReal x587=((cj4)*(x551));
01988 IkReal x588=((sj4)*(x550));
01989 IkReal x589=((IkReal(0.0645444780500000))*(x551));
01990 IkReal x590=((IkReal(0.542318181700000))*(x551));
01991 IkReal x591=((IkReal(2.00000000000000))*(r01)*(x552));
01992 evalcond[0]=((x562)+(((r01)*(x555)))+(((sj1)*(x565)))+(((IkReal(0.992991970000000))*(x587)))+(((IkReal(-0.118181820000000))*(x586))));
01993 evalcond[1]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x590)))+(((py)*(x555)))+(((px)*(x576)))+(((IkReal(0.0645444780500000))*(x550)))+(x582));
01994 evalcond[2]=((x572)+(((IkReal(-0.118181820000000))*(x587)))+(((IkReal(-1.00000000000000))*(x565)*(x568)))+(((IkReal(-0.992991970000000))*(x586)))+(((IkReal(-1.00000000000000))*(r01)*(x564))));
01995 evalcond[3]=((((IkReal(-1.00000000000000))*(x564)*(x579)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x568)*(x584)))+(((IkReal(0.542318181700000))*(x550)))+(x589));
01996 evalcond[4]=((((IkReal(-0.0950000000000000))*(x572)))+(((IkReal(-1.00000000000000))*(x560)*(x568)))+(((x573)*(x576)))+(((IkReal(-1.00000000000000))*(sj1)*(x558)*(x579)))+(((sj4)*(x589)))+(((x555)*(x574)))+(((IkReal(-1.00000000000000))*(x555)*(x563)))+(((IkReal(0.0950000000000000))*(cj1)*(x565)))+(((IkReal(0.542318181700000))*(x588)))+(((x564)*(x583)))+(((cj1)*(x556))));
01997 evalcond[5]=((((IkReal(0.0950000000000000))*(x562)))+(((IkReal(0.350000000000000))*(x578)))+(((sj4)*(x590)))+(((sj1)*(x556)))+(((x563)*(x564)))+(((x555)*(x583)))+(((IkReal(-1.00000000000000))*(cj0)*(x568)*(x573)))+(((IkReal(-0.350000000000000))*(x577)))+(((IkReal(-0.0645444780500000))*(x588)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x562)))+(((IkReal(-1.00000000000000))*(sj1)*(x560)))+(((cj1)*(py)*(x558)))+(((IkReal(0.0950000000000000))*(sj1)*(x565))));
01998 evalcond[6]=((((IkReal(-1.00000000000000))*(cj1)*(x563)*(x569)))+(((IkReal(-0.0665000000000000))*(x577)))+(((py)*(x557)*(x558)))+(((IkReal(-0.113475000000000))*(sj1)*(x565)))+(((IkReal(-1.00000000000000))*(px)*(x558)*(x585)))+(((IkReal(-0.296184679851750))*(x587)))+(((x556)*(x561)))+(((IkReal(-0.113475000000000))*(x562)))+(((IkReal(-0.113475000000000))*(r01)*(x555)))+(((x565)*(x571)))+(((x555)*(x570)))+(((IkReal(-1.00000000000000))*(x555)*(x556)*(x569)))+(((sj0)*(x557)*(x563)))+(((IkReal(-1.00000000000000))*(cj0)*(x557)*(x573)))+(((IkReal(-2.00000000000000))*(x554)*(x562)))+(((IkReal(0.700000000000000))*(x559)))+(((IkReal(-1.00000000000000))*(x553)*(x565)*(x566)))+(((IkReal(-1.00000000000000))*(sj0)*(x557)*(x574)))+(((IkReal(-1.00000000000000))*(x555)*(x580)*(x581)))+(((IkReal(0.0352506812605000))*(x586)))+(((IkReal(-1.00000000000000))*(x560)*(x561)))+(((IkReal(0.0665000000000000))*(x578)))+(((IkReal(-1.00000000000000))*(x559)*(x566)*(x584)))+(((IkReal(-2.00000000000000))*(x559)*(x582)))+(((IkReal(0.700000000000000))*(x580)))+(((IkReal(-1.00000000000000))*(x555)*(x591)))+(((r00)*(x567)))+(((pp)*(x562))));
01999 evalcond[7]=((((IkReal(-0.131525000000000))*(cj1)*(x565)))+(((py)*(x558)*(x561)))+(((r02)*(x571)))+(((IkReal(0.296184679851750))*(x586)))+(((IkReal(-1.00000000000000))*(x556)*(x557)))+(((IkReal(-1.00000000000000))*(x558)*(x567)))+(((x558)*(x569)*(x582)))+(((x556)*(x564)*(x569)))+(((IkReal(-1.00000000000000))*(x559)*(x585)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x575)))+(((x564)*(x591)))+(((IkReal(0.0352506812605000))*(x587)))+(((IkReal(-1.00000000000000))*(px)*(x563)*(x566)))+(((IkReal(-0.190000000000000))*(x555)*(x574)))+(((IkReal(2.00000000000000))*(cj1)*(x553)*(x565)))+(((x557)*(x560)))+(((IkReal(-1.00000000000000))*(x564)*(x570)))+(((IkReal(0.190000000000000))*(x555)*(x563)))+(((cj0)*(cj1)*(x559)*(x569)))+(((x573)*(x575)))+(((IkReal(-1.00000000000000))*(r02)*(x554)*(x566)))+(((IkReal(-0.131525000000000))*(r01)*(x564)))+(((IkReal(-1.00000000000000))*(pp)*(x565)*(x568)))+(((IkReal(-1.00000000000000))*(cj0)*(x561)*(x573)))+(((IkReal(0.700000000000000))*(cj0)*(x563)))+(((pz)*(sj0)*(x562)*(x581)))+(((IkReal(0.131525000000000))*(x572))));
02000 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  )
02001 {
02002 continue;
02003 }
02004 }
02005 
02006 {
02007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02008 vinfos[0].jointtype = 1;
02009 vinfos[0].foffset = j0;
02010 vinfos[0].indices[0] = _ij0[0];
02011 vinfos[0].indices[1] = _ij0[1];
02012 vinfos[0].maxsolutions = _nj0;
02013 vinfos[1].jointtype = 1;
02014 vinfos[1].foffset = j1;
02015 vinfos[1].indices[0] = _ij1[0];
02016 vinfos[1].indices[1] = _ij1[1];
02017 vinfos[1].maxsolutions = _nj1;
02018 vinfos[2].jointtype = 1;
02019 vinfos[2].foffset = j2;
02020 vinfos[2].indices[0] = _ij2[0];
02021 vinfos[2].indices[1] = _ij2[1];
02022 vinfos[2].maxsolutions = _nj2;
02023 vinfos[3].jointtype = 1;
02024 vinfos[3].foffset = j3;
02025 vinfos[3].indices[0] = _ij3[0];
02026 vinfos[3].indices[1] = _ij3[1];
02027 vinfos[3].maxsolutions = _nj3;
02028 vinfos[4].jointtype = 1;
02029 vinfos[4].foffset = j4;
02030 vinfos[4].indices[0] = _ij4[0];
02031 vinfos[4].indices[1] = _ij4[1];
02032 vinfos[4].maxsolutions = _nj4;
02033 std::vector<int> vfree(0);
02034 solutions.AddSolution(vinfos,vfree);
02035 }
02036 }
02037 }
02038 
02039 } else
02040 {
02041 IkReal x592=((r01)*(sj1));
02042 IkReal x593=((IkReal(0.350000000000000))*(sj0));
02043 IkReal x594=((px)*(r02));
02044 IkReal x595=((IkReal(1.00000000000000))*(cj0));
02045 IkReal x596=((cj0)*(r00));
02046 IkReal x597=((IkReal(0.350000000000000))*(cj1));
02047 IkReal x598=((IkReal(0.700000000000000))*(sj1));
02048 IkReal x599=((IkReal(0.190000000000000))*(px));
02049 IkReal x600=((r00)*(sj0));
02050 IkReal x601=((px)*(sj0));
02051 IkReal x602=((IkReal(0.700000000000000))*(cj1));
02052 IkReal x603=((py)*(r02));
02053 IkReal x604=((IkReal(0.190000000000000))*(py));
02054 IkReal x605=((IkReal(2.00000000000000))*(py));
02055 IkReal x606=((pz)*(r01));
02056 IkReal x607=((IkReal(2.00000000000000))*(cj0));
02057 IkReal x608=((IkReal(0.350000000000000))*(sj1));
02058 IkReal x609=((pz)*(r02));
02059 IkReal x610=((cj0)*(r01));
02060 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.83084764859385))+(j3)), IkReal(6.28318530717959))));
02061 evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x595)))+(x601));
02062 evalcond[2]=((sj4)+(((IkReal(-1.00000000000000))*(r01)*(x595)))+(x600));
02063 evalcond[3]=((IkReal(0.166749999870000))+(((pz)*(x602)))+(((IkReal(-1.00000000000000))*(cj0)*(x604)))+(((IkReal(-1.00000000000000))*(pp)))+(((cj0)*(px)*(x598)))+(((py)*(sj0)*(x598)))+(((sj0)*(x599))));
02064 evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x609)))+(((r02)*(x597)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.0950000000000000))*(x600)))+(((x592)*(x593)))+(((IkReal(-0.0950000000000000))*(x610)))+(((x596)*(x608)))+(((IkReal(0.546145583500000))*(cj4))));
02065 evalcond[5]=((((sj0)*(x606)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((r02)*(x608)))+(((IkReal(-1.00000000000000))*(sj0)*(x603)))+(((IkReal(-1.00000000000000))*(x596)*(x597)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x593)))+(((pz)*(x596)))+(((IkReal(-1.00000000000000))*(x594)*(x595))));
02066 evalcond[6]=((((IkReal(0.700000000000000))*(px)*(x592)))+(((pz)*(x603)*(x607)))+(((IkReal(0.190000000000000))*(x609)))+(((sj0)*(x594)*(x602)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x595)))+(((IkReal(-0.0665000000000000))*(sj0)*(x592)))+(((r01)*(x604)))+(((IkReal(-1.00000000000000))*(r01)*(x601)*(x605)))+(((px)*(x596)*(x605)))+(((IkReal(0.113475000000000))*(x600)))+(((r00)*(x599)))+(((cj0)*(x602)*(x606)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x598)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x594)))+(((py)*(x605)*(x610)))+(((IkReal(-0.113475000000000))*(x610)))+(((pp)*(x600)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-1.00000000000000))*(cj0)*(x602)*(x603)))+(((IkReal(-1.00000000000000))*(pz)*(x600)*(x602)))+(((IkReal(-0.0665000000000000))*(sj1)*(x596)))+(((IkReal(-2.00000000000000))*(x600)*((px)*(px)))));
02067 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  )
02068 {
02069 {
02070 IkReal j2array[1], cj2array[1], sj2array[1];
02071 bool j2valid[1]={false};
02072 _nj2 = 1;
02073 IkReal x611=((IkReal(1.81818181870518))*(sj1));
02074 IkReal x612=((py)*(sj0));
02075 IkReal x613=((IkReal(0.216392517249668))*(sj1));
02076 IkReal x614=((cj0)*(px));
02077 IkReal x615=((IkReal(0.216392517249668))*(cj1));
02078 IkReal x616=((IkReal(1.81818181870518))*(cj1));
02079 if( IKabs(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614)))))+IKsqr(((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612)))))-1) <= IKFAST_SINCOS_THRESH )
02080     continue;
02081 j2array[0]=IKatan2(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614)))), ((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612)))));
02082 sj2array[0]=IKsin(j2array[0]);
02083 cj2array[0]=IKcos(j2array[0]);
02084 if( j2array[0] > IKPI )
02085 {
02086     j2array[0]-=IK2PI;
02087 }
02088 else if( j2array[0] < -IKPI )
02089 {    j2array[0]+=IK2PI;
02090 }
02091 j2valid[0] = true;
02092 for(int ij2 = 0; ij2 < 1; ++ij2)
02093 {
02094 if( !j2valid[ij2] )
02095 {
02096     continue;
02097 }
02098 _ij2[0] = ij2; _ij2[1] = -1;
02099 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02100 {
02101 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02102 {
02103     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02104 }
02105 }
02106 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02107 {
02108 IkReal evalcond[8];
02109 IkReal x617=IKsin(j2);
02110 IkReal x618=IKcos(j2);
02111 IkReal x619=(py)*(py);
02112 IkReal x620=(px)*(px);
02113 IkReal x621=(pz)*(pz);
02114 IkReal x622=((sj0)*(sj1));
02115 IkReal x623=((py)*(r00));
02116 IkReal x624=((IkReal(0.190000000000000))*(cj1));
02117 IkReal x625=((cj0)*(r02));
02118 IkReal x626=((py)*(r01));
02119 IkReal x627=((px)*(r01));
02120 IkReal x628=((IkReal(0.190000000000000))*(sj1));
02121 IkReal x629=((cj1)*(r02));
02122 IkReal x630=((pz)*(r00));
02123 IkReal x631=((cj1)*(sj0));
02124 IkReal x632=((cj0)*(r00));
02125 IkReal x633=((IkReal(2.00000000000000))*(sj1));
02126 IkReal x634=((IkReal(0.700000000000000))*(px));
02127 IkReal x635=((IkReal(1.00000000000000))*(cj1));
02128 IkReal x636=((IkReal(2.00000000000000))*(px));
02129 IkReal x637=((pp)*(r01));
02130 IkReal x638=((pp)*(sj1));
02131 IkReal x639=((r02)*(sj1));
02132 IkReal x640=((pz)*(r01));
02133 IkReal x641=((px)*(r02));
02134 IkReal x642=((IkReal(0.700000000000000))*(sj0));
02135 IkReal x643=((cj0)*(sj1));
02136 IkReal x644=((r00)*(sj0));
02137 IkReal x645=((cj0)*(r01));
02138 IkReal x646=((IkReal(1.00000000000000))*(py));
02139 IkReal x647=((pz)*(r02));
02140 IkReal x648=((IkReal(2.00000000000000))*(py));
02141 IkReal x649=((cj1)*(pz));
02142 IkReal x650=((IkReal(0.0950000000000000))*(r01));
02143 IkReal x651=((cj0)*(px));
02144 IkReal x652=((pz)*(x633));
02145 IkReal x653=((cj4)*(x617));
02146 IkReal x654=((cj4)*(x618));
02147 IkReal x655=((sj4)*(x617));
02148 IkReal x656=((IkReal(0.0645444780500000))*(x618));
02149 IkReal x657=((IkReal(0.542318181700000))*(x618));
02150 IkReal x658=((IkReal(2.00000000000000))*(r01)*(x619));
02151 evalcond[0]=((((sj1)*(x632)))+(((r01)*(x622)))+(((IkReal(-0.118181820000000))*(x653)))+(x629)+(((IkReal(-0.992991970000000))*(x654))));
02152 evalcond[1]=((IkReal(-0.350000000000000))+(x649)+(((py)*(x622)))+(((IkReal(-0.0645444780500000))*(x617)))+(((px)*(x643)))+(((IkReal(-1.00000000000000))*(x657))));
02153 evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x631)))+(((IkReal(-1.00000000000000))*(x632)*(x635)))+(((IkReal(0.992991970000000))*(x653)))+(x639)+(((IkReal(-0.118181820000000))*(x654))));
02154 evalcond[3]=((((pz)*(sj1)))+(((IkReal(0.542318181700000))*(x617)))+(((IkReal(-1.00000000000000))*(x635)*(x651)))+(((IkReal(-1.00000000000000))*(x656)))+(((IkReal(-1.00000000000000))*(x631)*(x646))));
02155 evalcond[4]=((((IkReal(0.542318181700000))*(x655)))+(((IkReal(-1.00000000000000))*(x627)*(x635)))+(((IkReal(-1.00000000000000))*(x622)*(x630)))+(((IkReal(-1.00000000000000))*(sj4)*(x656)))+(((IkReal(-1.00000000000000))*(sj1)*(x625)*(x646)))+(((x640)*(x643)))+(((x631)*(x650)))+(((IkReal(0.0950000000000000))*(cj1)*(x632)))+(((x622)*(x641)))+(((cj1)*(x623)))+(((IkReal(-0.0950000000000000))*(x639))));
02156 evalcond[5]=((((IkReal(0.0950000000000000))*(x629)))+(((x630)*(x631)))+(((sj1)*(x623)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x629)))+(((sj4)*(x657)))+(((x622)*(x650)))+(((IkReal(-0.350000000000000))*(x644)))+(((IkReal(0.0645444780500000))*(x655)))+(((cj1)*(py)*(x625)))+(((IkReal(0.0950000000000000))*(sj1)*(x632)))+(((IkReal(-1.00000000000000))*(sj1)*(x627)))+(((IkReal(0.350000000000000))*(x645)))+(((IkReal(-1.00000000000000))*(cj0)*(x635)*(x640))));
02157 evalcond[6]=((((IkReal(-1.00000000000000))*(x627)*(x628)))+(((IkReal(-1.00000000000000))*(x626)*(x633)*(x651)))+(((IkReal(-0.113475000000000))*(x629)))+(((IkReal(0.296184679851750))*(x654)))+(((IkReal(-0.113475000000000))*(sj1)*(x632)))+(((IkReal(-1.00000000000000))*(x622)*(x623)*(x636)))+(((IkReal(0.700000000000000))*(x647)))+(((IkReal(-1.00000000000000))*(x622)*(x658)))+(((IkReal(-2.00000000000000))*(x621)*(x629)))+(((sj0)*(x624)*(x630)))+(((py)*(x624)*(x625)))+(((pp)*(x629)))+(((x622)*(x637)))+(((x632)*(x638)))+(((IkReal(-0.113475000000000))*(r01)*(x622)))+(((IkReal(-1.00000000000000))*(cj1)*(x630)*(x636)))+(((IkReal(-1.00000000000000))*(x622)*(x647)*(x648)))+(((IkReal(0.0665000000000000))*(x645)))+(((IkReal(-1.00000000000000))*(x620)*(x632)*(x633)))+(((IkReal(-1.00000000000000))*(cj0)*(x624)*(x640)))+(((IkReal(0.0352506812605000))*(x653)))+(((x623)*(x628)))+(((IkReal(-1.00000000000000))*(sj0)*(x624)*(x641)))+(((IkReal(-1.00000000000000))*(px)*(x625)*(x652)))+(((IkReal(-0.0665000000000000))*(x644)))+(((IkReal(0.700000000000000))*(x626)))+(((IkReal(-2.00000000000000))*(x626)*(x649)))+(((r00)*(x634))));
02158 evalcond[7]=((((x623)*(x631)*(x636)))+(((x625)*(x636)*(x649)))+(((IkReal(-1.00000000000000))*(x626)*(x652)))+(((x631)*(x658)))+(((IkReal(-0.296184679851750))*(x653)))+(((IkReal(-0.131525000000000))*(r01)*(x631)))+(((r02)*(x638)))+(((IkReal(0.131525000000000))*(x639)))+(((IkReal(-0.131525000000000))*(cj1)*(x632)))+(((x640)*(x642)))+(((x624)*(x627)))+(((pz)*(sj0)*(x629)*(x648)))+(((IkReal(-1.00000000000000))*(r02)*(x621)*(x633)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x642)))+(((IkReal(-1.00000000000000))*(x631)*(x637)))+(((IkReal(0.190000000000000))*(x622)*(x630)))+(((IkReal(-1.00000000000000))*(pp)*(x632)*(x635)))+(((IkReal(-1.00000000000000))*(x623)*(x624)))+(((IkReal(-0.190000000000000))*(x622)*(x641)))+(((IkReal(0.700000000000000))*(cj0)*(x630)))+(((IkReal(-1.00000000000000))*(px)*(x630)*(x633)))+(((IkReal(0.0352506812605000))*(x654)))+(((IkReal(-1.00000000000000))*(cj0)*(x628)*(x640)))+(((IkReal(2.00000000000000))*(cj1)*(x620)*(x632)))+(((IkReal(-1.00000000000000))*(x625)*(x634)))+(((cj0)*(cj1)*(x626)*(x636)))+(((py)*(x625)*(x628))));
02159 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  )
02160 {
02161 continue;
02162 }
02163 }
02164 
02165 {
02166 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02167 vinfos[0].jointtype = 1;
02168 vinfos[0].foffset = j0;
02169 vinfos[0].indices[0] = _ij0[0];
02170 vinfos[0].indices[1] = _ij0[1];
02171 vinfos[0].maxsolutions = _nj0;
02172 vinfos[1].jointtype = 1;
02173 vinfos[1].foffset = j1;
02174 vinfos[1].indices[0] = _ij1[0];
02175 vinfos[1].indices[1] = _ij1[1];
02176 vinfos[1].maxsolutions = _nj1;
02177 vinfos[2].jointtype = 1;
02178 vinfos[2].foffset = j2;
02179 vinfos[2].indices[0] = _ij2[0];
02180 vinfos[2].indices[1] = _ij2[1];
02181 vinfos[2].maxsolutions = _nj2;
02182 vinfos[3].jointtype = 1;
02183 vinfos[3].foffset = j3;
02184 vinfos[3].indices[0] = _ij3[0];
02185 vinfos[3].indices[1] = _ij3[1];
02186 vinfos[3].maxsolutions = _nj3;
02187 vinfos[4].jointtype = 1;
02188 vinfos[4].foffset = j4;
02189 vinfos[4].indices[0] = _ij4[0];
02190 vinfos[4].indices[1] = _ij4[1];
02191 vinfos[4].maxsolutions = _nj4;
02192 std::vector<int> vfree(0);
02193 solutions.AddSolution(vinfos,vfree);
02194 }
02195 }
02196 }
02197 
02198 } else
02199 {
02200 if( 1 )
02201 {
02202 continue;
02203 
02204 } else
02205 {
02206 }
02207 }
02208 }
02209 }
02210 }
02211 }
02212 
02213 } else
02214 {
02215 {
02216 IkReal j2array[1], cj2array[1], sj2array[1];
02217 bool j2valid[1]={false};
02218 _nj2 = 1;
02219 IkReal x659=((cj0)*(sj1));
02220 IkReal x660=((IkReal(13.0000000000000))*(r00));
02221 IkReal x661=((cj1)*(pz));
02222 IkReal x662=((sj0)*(sj1));
02223 IkReal x663=((IkReal(13.0000000000000))*(sj3));
02224 IkReal x664=((cj1)*(r02));
02225 IkReal x665=((IkReal(70.0000000000000))*(cj4));
02226 IkReal x666=((IkReal(13.0000000000000))*(cj3));
02227 IkReal x667=((IkReal(200.000000000000))*(cj4)*(sj3));
02228 IkReal x668=((IkReal(200.000000000000))*(cj3)*(cj4));
02229 if( IKabs(((gconst2)*(((((r01)*(x662)*(x666)))+(((IkReal(110.000000000000))*(r00)*(x659)))+(((IkReal(110.000000000000))*(r01)*(x662)))+(((x661)*(x667)))+(((IkReal(110.000000000000))*(x664)))+(((IkReal(-1.00000000000000))*(sj3)*(x665)))+(((cj3)*(x659)*(x660)))+(((x664)*(x666)))+(((px)*(x659)*(x667)))+(((py)*(x662)*(x667))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((r01)*(x662)*(x663)))+(((cj3)*(x665)))+(((sj3)*(x659)*(x660)))+(((IkReal(-1.00000000000000))*(px)*(x659)*(x668)))+(((x663)*(x664)))+(((IkReal(-1.00000000000000))*(x661)*(x668)))+(((IkReal(-1.00000000000000))*(py)*(x662)*(x668))))))) < IKFAST_ATAN2_MAGTHRESH )
02230     continue;
02231 j2array[0]=IKatan2(((gconst2)*(((((r01)*(x662)*(x666)))+(((IkReal(110.000000000000))*(r00)*(x659)))+(((IkReal(110.000000000000))*(r01)*(x662)))+(((x661)*(x667)))+(((IkReal(110.000000000000))*(x664)))+(((IkReal(-1.00000000000000))*(sj3)*(x665)))+(((cj3)*(x659)*(x660)))+(((x664)*(x666)))+(((px)*(x659)*(x667)))+(((py)*(x662)*(x667)))))), ((gconst2)*(((((r01)*(x662)*(x663)))+(((cj3)*(x665)))+(((sj3)*(x659)*(x660)))+(((IkReal(-1.00000000000000))*(px)*(x659)*(x668)))+(((x663)*(x664)))+(((IkReal(-1.00000000000000))*(x661)*(x668)))+(((IkReal(-1.00000000000000))*(py)*(x662)*(x668)))))));
02232 sj2array[0]=IKsin(j2array[0]);
02233 cj2array[0]=IKcos(j2array[0]);
02234 if( j2array[0] > IKPI )
02235 {
02236     j2array[0]-=IK2PI;
02237 }
02238 else if( j2array[0] < -IKPI )
02239 {    j2array[0]+=IK2PI;
02240 }
02241 j2valid[0] = true;
02242 for(int ij2 = 0; ij2 < 1; ++ij2)
02243 {
02244 if( !j2valid[ij2] )
02245 {
02246     continue;
02247 }
02248 _ij2[0] = ij2; _ij2[1] = -1;
02249 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02250 {
02251 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02252 {
02253     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02254 }
02255 }
02256 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02257 {
02258 IkReal evalcond[8];
02259 IkReal x669=IKsin(j2);
02260 IkReal x670=IKcos(j2);
02261 IkReal x671=(py)*(py);
02262 IkReal x672=(px)*(px);
02263 IkReal x673=(pz)*(pz);
02264 IkReal x674=((sj0)*(sj1));
02265 IkReal x675=((py)*(r00));
02266 IkReal x676=((IkReal(0.190000000000000))*(cj1));
02267 IkReal x677=((cj0)*(r02));
02268 IkReal x678=((py)*(r01));
02269 IkReal x679=((r01)*(sj1));
02270 IkReal x680=((IkReal(0.190000000000000))*(px));
02271 IkReal x681=((cj1)*(r02));
02272 IkReal x682=((pz)*(r00));
02273 IkReal x683=((cj1)*(sj0));
02274 IkReal x684=((cj0)*(r00));
02275 IkReal x685=((IkReal(2.00000000000000))*(sj1));
02276 IkReal x686=((cj0)*(pz));
02277 IkReal x687=((IkReal(1.00000000000000))*(r01));
02278 IkReal x688=((cj1)*(px));
02279 IkReal x689=((IkReal(2.00000000000000))*(px));
02280 IkReal x690=((pp)*(sj1));
02281 IkReal x691=((r02)*(sj1));
02282 IkReal x692=((px)*(r02));
02283 IkReal x693=((IkReal(0.700000000000000))*(pz));
02284 IkReal x694=((cj0)*(px));
02285 IkReal x695=((r00)*(sj0));
02286 IkReal x696=((IkReal(1.00000000000000))*(px));
02287 IkReal x697=((cj0)*(r01));
02288 IkReal x698=((IkReal(1.00000000000000))*(cj1));
02289 IkReal x699=((IkReal(1.00000000000000))*(py));
02290 IkReal x700=((IkReal(0.700000000000000))*(px));
02291 IkReal x701=((IkReal(0.190000000000000))*(sj1));
02292 IkReal x702=((IkReal(0.298275000000000))*(sj3));
02293 IkReal x703=((cj1)*(pz));
02294 IkReal x704=((IkReal(0.0950000000000000))*(r01));
02295 IkReal x705=((IkReal(0.306725000000000))*(cj3)*(cj4));
02296 IkReal x706=((pz)*(x685));
02297 IkReal x707=((IkReal(0.550000000000000))*(x670));
02298 IkReal x708=((cj4)*(x670));
02299 IkReal x709=((IkReal(0.550000000000000))*(x669));
02300 IkReal x710=((IkReal(2.00000000000000))*(py)*(pz));
02301 IkReal x711=((IkReal(0.0650000000000000))*(cj3)*(sj4));
02302 IkReal x712=((IkReal(0.0650000000000000))*(x669));
02303 IkReal x713=((cj4)*(x669));
02304 IkReal x714=((IkReal(2.00000000000000))*(r01)*(x671));
02305 IkReal x715=((IkReal(0.0650000000000000))*(sj3)*(x670));
02306 evalcond[0]=((((r01)*(x674)))+(((sj1)*(x684)))+(((cj3)*(x713)))+(x681)+(((sj3)*(x708))));
02307 evalcond[1]=((IkReal(-0.350000000000000))+(((sj1)*(x694)))+(((IkReal(-0.0650000000000000))*(cj3)*(x670)))+(((sj3)*(x712)))+(x703)+(((py)*(x674)))+(((IkReal(-1.00000000000000))*(x707))));
02308 evalcond[2]=((((IkReal(-1.00000000000000))*(x683)*(x687)))+(x691)+(((IkReal(-1.00000000000000))*(x684)*(x698)))+(((cj3)*(x708)))+(((IkReal(-1.00000000000000))*(sj3)*(x713))));
02309 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(x688)))+(((pz)*(sj1)))+(x709)+(((cj3)*(x712)))+(x715)+(((IkReal(-1.00000000000000))*(x683)*(x699))));
02310 evalcond[4]=((((x679)*(x686)))+(((IkReal(-0.0950000000000000))*(x691)))+(((IkReal(0.0950000000000000))*(cj1)*(x684)))+(((x674)*(x692)))+(((IkReal(-1.00000000000000))*(x674)*(x682)))+(((IkReal(-1.00000000000000))*(sj1)*(x677)*(x699)))+(((sj4)*(x709)))+(((x683)*(x704)))+(((sj4)*(x715)))+(((cj1)*(x675)))+(((IkReal(-1.00000000000000))*(x687)*(x688)))+(((x669)*(x711))));
02311 evalcond[5]=((((sj1)*(x675)))+(((x670)*(x711)))+(((IkReal(-1.00000000000000))*(cj1)*(x686)*(x687)))+(((IkReal(0.0950000000000000))*(sj1)*(x684)))+(((IkReal(-1.00000000000000))*(x679)*(x696)))+(((x674)*(x704)))+(((IkReal(-0.350000000000000))*(x695)))+(((x682)*(x683)))+(((IkReal(-1.00000000000000))*(sj0)*(x681)*(x696)))+(((cj1)*(py)*(x677)))+(((IkReal(0.350000000000000))*(x697)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x712)))+(((IkReal(0.0950000000000000))*(x681)))+(((sj4)*(x707))));
02312 evalcond[6]=((((x684)*(x690)))+(((py)*(x676)*(x677)))+(((r00)*(x700)))+(((IkReal(-1.00000000000000))*(x674)*(x675)*(x689)))+(((IkReal(-1.00000000000000))*(x702)*(x708)))+(((x669)*(x705)))+(((IkReal(-0.113475000000000))*(r01)*(x674)))+(((IkReal(-2.00000000000000))*(x673)*(x681)))+(((IkReal(-1.00000000000000))*(r02)*(x674)*(x710)))+(((IkReal(-1.00000000000000))*(x674)*(x714)))+(((sj0)*(x676)*(x682)))+(((IkReal(-1.00000000000000))*(sj0)*(x676)*(x692)))+(((IkReal(-0.113475000000000))*(x681)))+(((IkReal(-2.00000000000000))*(x678)*(x703)))+(((IkReal(-2.00000000000000))*(x682)*(x688)))+(((IkReal(0.0665000000000000))*(x697)))+(((IkReal(-1.00000000000000))*(r01)*(x676)*(x686)))+(((IkReal(0.0715000000000000))*(x713)))+(((IkReal(-1.00000000000000))*(x672)*(x684)*(x685)))+(((r02)*(x693)))+(((pp)*(x681)))+(((IkReal(-1.00000000000000))*(px)*(x677)*(x706)))+(((IkReal(0.700000000000000))*(x678)))+(((IkReal(-1.00000000000000))*(x679)*(x680)))+(((x675)*(x701)))+(((IkReal(-1.00000000000000))*(x678)*(x685)*(x694)))+(((IkReal(-0.113475000000000))*(sj1)*(x684)))+(((pp)*(r01)*(x674)))+(((IkReal(-0.0665000000000000))*(x695))));
02313 evalcond[7]=((((IkReal(-1.00000000000000))*(r02)*(x674)*(x680)))+(((IkReal(-1.00000000000000))*(x678)*(x706)))+(((x670)*(x705)))+(((IkReal(0.190000000000000))*(x674)*(x682)))+(((px)*(r01)*(x676)))+(((IkReal(2.00000000000000))*(cj1)*(x672)*(x684)))+(((IkReal(0.0715000000000000))*(x708)))+(((x683)*(x714)))+(((IkReal(0.700000000000000))*(cj0)*(x682)))+(((IkReal(-0.190000000000000))*(x679)*(x686)))+(((r01)*(sj0)*(x693)))+(((x702)*(x713)))+(((IkReal(-1.00000000000000))*(x675)*(x676)))+(((py)*(x677)*(x701)))+(((sj0)*(x681)*(x710)))+(((IkReal(-1.00000000000000))*(r02)*(x673)*(x685)))+(((IkReal(-1.00000000000000))*(px)*(x682)*(x685)))+(((IkReal(-1.00000000000000))*(x677)*(x700)))+(((IkReal(-0.131525000000000))*(r01)*(x683)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((IkReal(2.00000000000000))*(pz)*(x677)*(x688)))+(((x675)*(x683)*(x689)))+(((IkReal(-1.00000000000000))*(pp)*(x683)*(x687)))+(((IkReal(-0.131525000000000))*(cj1)*(x684)))+(((r02)*(x690)))+(((IkReal(-1.00000000000000))*(pp)*(x684)*(x698)))+(((IkReal(2.00000000000000))*(cj0)*(x678)*(x688)))+(((IkReal(0.131525000000000))*(x691))));
02314 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  )
02315 {
02316 continue;
02317 }
02318 }
02319 
02320 {
02321 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02322 vinfos[0].jointtype = 1;
02323 vinfos[0].foffset = j0;
02324 vinfos[0].indices[0] = _ij0[0];
02325 vinfos[0].indices[1] = _ij0[1];
02326 vinfos[0].maxsolutions = _nj0;
02327 vinfos[1].jointtype = 1;
02328 vinfos[1].foffset = j1;
02329 vinfos[1].indices[0] = _ij1[0];
02330 vinfos[1].indices[1] = _ij1[1];
02331 vinfos[1].maxsolutions = _nj1;
02332 vinfos[2].jointtype = 1;
02333 vinfos[2].foffset = j2;
02334 vinfos[2].indices[0] = _ij2[0];
02335 vinfos[2].indices[1] = _ij2[1];
02336 vinfos[2].maxsolutions = _nj2;
02337 vinfos[3].jointtype = 1;
02338 vinfos[3].foffset = j3;
02339 vinfos[3].indices[0] = _ij3[0];
02340 vinfos[3].indices[1] = _ij3[1];
02341 vinfos[3].maxsolutions = _nj3;
02342 vinfos[4].jointtype = 1;
02343 vinfos[4].foffset = j4;
02344 vinfos[4].indices[0] = _ij4[0];
02345 vinfos[4].indices[1] = _ij4[1];
02346 vinfos[4].maxsolutions = _nj4;
02347 std::vector<int> vfree(0);
02348 solutions.AddSolution(vinfos,vfree);
02349 }
02350 }
02351 }
02352 
02353 }
02354 
02355 }
02356 
02357 } else
02358 {
02359 {
02360 IkReal j2array[1], cj2array[1], sj2array[1];
02361 bool j2valid[1]={false};
02362 _nj2 = 1;
02363 IkReal x716=((IkReal(1.00000000000000))*(sj1));
02364 IkReal x717=((IkReal(1.00000000000000))*(sj3));
02365 IkReal x718=((cj0)*(r00));
02366 IkReal x719=((r01)*(sj0));
02367 IkReal x720=((cj3)*(r02));
02368 IkReal x721=((cj3)*(x719));
02369 IkReal x722=((cj1)*(x718));
02370 if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(cj3)*(x716)*(x718)))+(((IkReal(-1.00000000000000))*(x716)*(x721)))+(((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x717)*(x719)))+(((IkReal(-1.00000000000000))*(cj1)*(x720)))+(((IkReal(-1.00000000000000))*(x717)*(x722))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(sj3)*(x716)*(x718)))+(((cj3)*(x722)))+(((cj1)*(x721)))+(((IkReal(-1.00000000000000))*(sj3)*(x716)*(x719)))+(((IkReal(-1.00000000000000))*(x716)*(x720)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x717))))))) < IKFAST_ATAN2_MAGTHRESH )
02371     continue;
02372 j2array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(cj3)*(x716)*(x718)))+(((IkReal(-1.00000000000000))*(x716)*(x721)))+(((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x717)*(x719)))+(((IkReal(-1.00000000000000))*(cj1)*(x720)))+(((IkReal(-1.00000000000000))*(x717)*(x722)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(sj3)*(x716)*(x718)))+(((cj3)*(x722)))+(((cj1)*(x721)))+(((IkReal(-1.00000000000000))*(sj3)*(x716)*(x719)))+(((IkReal(-1.00000000000000))*(x716)*(x720)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x717)))))));
02373 sj2array[0]=IKsin(j2array[0]);
02374 cj2array[0]=IKcos(j2array[0]);
02375 if( j2array[0] > IKPI )
02376 {
02377     j2array[0]-=IK2PI;
02378 }
02379 else if( j2array[0] < -IKPI )
02380 {    j2array[0]+=IK2PI;
02381 }
02382 j2valid[0] = true;
02383 for(int ij2 = 0; ij2 < 1; ++ij2)
02384 {
02385 if( !j2valid[ij2] )
02386 {
02387     continue;
02388 }
02389 _ij2[0] = ij2; _ij2[1] = -1;
02390 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02391 {
02392 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02393 {
02394     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02395 }
02396 }
02397 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02398 {
02399 IkReal evalcond[8];
02400 IkReal x723=IKsin(j2);
02401 IkReal x724=IKcos(j2);
02402 IkReal x725=(py)*(py);
02403 IkReal x726=(px)*(px);
02404 IkReal x727=(pz)*(pz);
02405 IkReal x728=((sj0)*(sj1));
02406 IkReal x729=((py)*(r00));
02407 IkReal x730=((IkReal(0.190000000000000))*(cj1));
02408 IkReal x731=((cj0)*(r02));
02409 IkReal x732=((py)*(r01));
02410 IkReal x733=((r01)*(sj1));
02411 IkReal x734=((IkReal(0.190000000000000))*(px));
02412 IkReal x735=((cj1)*(r02));
02413 IkReal x736=((pz)*(r00));
02414 IkReal x737=((cj1)*(sj0));
02415 IkReal x738=((cj0)*(r00));
02416 IkReal x739=((IkReal(2.00000000000000))*(sj1));
02417 IkReal x740=((cj0)*(pz));
02418 IkReal x741=((IkReal(1.00000000000000))*(r01));
02419 IkReal x742=((cj1)*(px));
02420 IkReal x743=((IkReal(2.00000000000000))*(px));
02421 IkReal x744=((pp)*(sj1));
02422 IkReal x745=((r02)*(sj1));
02423 IkReal x746=((px)*(r02));
02424 IkReal x747=((IkReal(0.700000000000000))*(pz));
02425 IkReal x748=((cj0)*(px));
02426 IkReal x749=((r00)*(sj0));
02427 IkReal x750=((IkReal(1.00000000000000))*(px));
02428 IkReal x751=((cj0)*(r01));
02429 IkReal x752=((IkReal(1.00000000000000))*(cj1));
02430 IkReal x753=((IkReal(1.00000000000000))*(py));
02431 IkReal x754=((IkReal(0.700000000000000))*(px));
02432 IkReal x755=((IkReal(0.190000000000000))*(sj1));
02433 IkReal x756=((IkReal(0.298275000000000))*(sj3));
02434 IkReal x757=((cj1)*(pz));
02435 IkReal x758=((IkReal(0.0950000000000000))*(r01));
02436 IkReal x759=((IkReal(0.306725000000000))*(cj3)*(cj4));
02437 IkReal x760=((pz)*(x739));
02438 IkReal x761=((IkReal(0.550000000000000))*(x724));
02439 IkReal x762=((cj4)*(x724));
02440 IkReal x763=((IkReal(0.550000000000000))*(x723));
02441 IkReal x764=((IkReal(2.00000000000000))*(py)*(pz));
02442 IkReal x765=((IkReal(0.0650000000000000))*(cj3)*(sj4));
02443 IkReal x766=((IkReal(0.0650000000000000))*(x723));
02444 IkReal x767=((cj4)*(x723));
02445 IkReal x768=((IkReal(2.00000000000000))*(r01)*(x725));
02446 IkReal x769=((IkReal(0.0650000000000000))*(sj3)*(x724));
02447 evalcond[0]=((((sj1)*(x738)))+(x735)+(((cj3)*(x767)))+(((r01)*(x728)))+(((sj3)*(x762))));
02448 evalcond[1]=((IkReal(-0.350000000000000))+(((sj1)*(x748)))+(((py)*(x728)))+(x757)+(((sj3)*(x766)))+(((IkReal(-0.0650000000000000))*(cj3)*(x724)))+(((IkReal(-1.00000000000000))*(x761))));
02449 evalcond[2]=((((cj3)*(x762)))+(((IkReal(-1.00000000000000))*(sj3)*(x767)))+(((IkReal(-1.00000000000000))*(x738)*(x752)))+(x745)+(((IkReal(-1.00000000000000))*(x737)*(x741))));
02450 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(x742)))+(((pz)*(sj1)))+(x769)+(x763)+(((IkReal(-1.00000000000000))*(x737)*(x753)))+(((cj3)*(x766))));
02451 evalcond[4]=((((sj4)*(x769)))+(((IkReal(-1.00000000000000))*(sj1)*(x731)*(x753)))+(((x728)*(x746)))+(((x737)*(x758)))+(((x723)*(x765)))+(((IkReal(-1.00000000000000))*(x741)*(x742)))+(((cj1)*(x729)))+(((IkReal(0.0950000000000000))*(cj1)*(x738)))+(((IkReal(-1.00000000000000))*(x728)*(x736)))+(((sj4)*(x763)))+(((x733)*(x740)))+(((IkReal(-0.0950000000000000))*(x745))));
02452 evalcond[5]=((((cj1)*(py)*(x731)))+(((IkReal(-0.350000000000000))*(x749)))+(((IkReal(-1.00000000000000))*(sj0)*(x735)*(x750)))+(((IkReal(0.0950000000000000))*(x735)))+(((x736)*(x737)))+(((IkReal(0.350000000000000))*(x751)))+(((sj1)*(x729)))+(((x724)*(x765)))+(((IkReal(-1.00000000000000))*(cj1)*(x740)*(x741)))+(((IkReal(0.0950000000000000))*(sj1)*(x738)))+(((IkReal(-1.00000000000000))*(x733)*(x750)))+(((x728)*(x758)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x766)))+(((sj4)*(x761))));
02453 evalcond[6]=((((x723)*(x759)))+(((sj0)*(x730)*(x736)))+(((pp)*(x735)))+(((IkReal(-1.00000000000000))*(r02)*(x728)*(x764)))+(((IkReal(-1.00000000000000))*(px)*(x731)*(x760)))+(((IkReal(-1.00000000000000))*(x728)*(x768)))+(((IkReal(-1.00000000000000))*(r01)*(x730)*(x740)))+(((IkReal(-1.00000000000000))*(sj0)*(x730)*(x746)))+(((IkReal(0.0715000000000000))*(x767)))+(((pp)*(r01)*(x728)))+(((py)*(x730)*(x731)))+(((IkReal(-2.00000000000000))*(x727)*(x735)))+(((IkReal(-1.00000000000000))*(x756)*(x762)))+(((r02)*(x747)))+(((IkReal(-1.00000000000000))*(x733)*(x734)))+(((IkReal(-1.00000000000000))*(x732)*(x739)*(x748)))+(((IkReal(-1.00000000000000))*(x726)*(x738)*(x739)))+(((IkReal(-0.113475000000000))*(x735)))+(((IkReal(-2.00000000000000))*(x732)*(x757)))+(((r00)*(x754)))+(((IkReal(-1.00000000000000))*(x728)*(x729)*(x743)))+(((IkReal(-0.0665000000000000))*(x749)))+(((IkReal(0.0665000000000000))*(x751)))+(((IkReal(-2.00000000000000))*(x736)*(x742)))+(((IkReal(-0.113475000000000))*(r01)*(x728)))+(((x738)*(x744)))+(((x729)*(x755)))+(((IkReal(-0.113475000000000))*(sj1)*(x738)))+(((IkReal(0.700000000000000))*(x732))));
02454 evalcond[7]=((((IkReal(2.00000000000000))*(pz)*(x731)*(x742)))+(((sj0)*(x735)*(x764)))+(((IkReal(0.190000000000000))*(x728)*(x736)))+(((x756)*(x767)))+(((IkReal(-1.00000000000000))*(pp)*(x738)*(x752)))+(((py)*(x731)*(x755)))+(((IkReal(-0.131525000000000))*(r01)*(x737)))+(((IkReal(-0.190000000000000))*(x733)*(x740)))+(((IkReal(2.00000000000000))*(cj0)*(x732)*(x742)))+(((IkReal(-0.131525000000000))*(cj1)*(x738)))+(((IkReal(0.700000000000000))*(cj0)*(x736)))+(((IkReal(-1.00000000000000))*(x732)*(x760)))+(((IkReal(-1.00000000000000))*(pp)*(x737)*(x741)))+(((IkReal(-1.00000000000000))*(x731)*(x754)))+(((r01)*(sj0)*(x747)))+(((IkReal(-1.00000000000000))*(r02)*(x728)*(x734)))+(((IkReal(-1.00000000000000))*(r02)*(x727)*(x739)))+(((IkReal(-1.00000000000000))*(x729)*(x730)))+(((px)*(r01)*(x730)))+(((IkReal(-1.00000000000000))*(px)*(x736)*(x739)))+(((IkReal(0.131525000000000))*(x745)))+(((r02)*(x744)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x737)*(x768)))+(((IkReal(0.0715000000000000))*(x762)))+(((x729)*(x737)*(x743)))+(((IkReal(2.00000000000000))*(cj1)*(x726)*(x738)))+(((x724)*(x759))));
02455 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  )
02456 {
02457 continue;
02458 }
02459 }
02460 
02461 {
02462 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02463 vinfos[0].jointtype = 1;
02464 vinfos[0].foffset = j0;
02465 vinfos[0].indices[0] = _ij0[0];
02466 vinfos[0].indices[1] = _ij0[1];
02467 vinfos[0].maxsolutions = _nj0;
02468 vinfos[1].jointtype = 1;
02469 vinfos[1].foffset = j1;
02470 vinfos[1].indices[0] = _ij1[0];
02471 vinfos[1].indices[1] = _ij1[1];
02472 vinfos[1].maxsolutions = _nj1;
02473 vinfos[2].jointtype = 1;
02474 vinfos[2].foffset = j2;
02475 vinfos[2].indices[0] = _ij2[0];
02476 vinfos[2].indices[1] = _ij2[1];
02477 vinfos[2].maxsolutions = _nj2;
02478 vinfos[3].jointtype = 1;
02479 vinfos[3].foffset = j3;
02480 vinfos[3].indices[0] = _ij3[0];
02481 vinfos[3].indices[1] = _ij3[1];
02482 vinfos[3].maxsolutions = _nj3;
02483 vinfos[4].jointtype = 1;
02484 vinfos[4].foffset = j4;
02485 vinfos[4].indices[0] = _ij4[0];
02486 vinfos[4].indices[1] = _ij4[1];
02487 vinfos[4].maxsolutions = _nj4;
02488 std::vector<int> vfree(0);
02489 solutions.AddSolution(vinfos,vfree);
02490 }
02491 }
02492 }
02493 
02494 }
02495 
02496 }
02497 }
02498 }
02499 
02500 }
02501 
02502 }
02503 
02504 } else
02505 {
02506 {
02507 IkReal j3array[1], cj3array[1], sj3array[1];
02508 bool j3valid[1]={false};
02509 _nj3 = 1;
02510 IkReal x770=((IkReal(20.0000000000000))*(r02));
02511 IkReal x771=((IkReal(7.00000000000000))*(sj1));
02512 IkReal x772=((r01)*(sj0));
02513 IkReal x773=((cj0)*(r00));
02514 IkReal x774=((IkReal(20.0000000000000))*(pz));
02515 IkReal x775=((IkReal(7.00000000000000))*(cj1));
02516 if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(pz)*(x770)))+(((r02)*(x775)))+(((x771)*(x773)))+(((IkReal(-20.0000000000000))*(py)*(r01)))+(((x771)*(x772)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(1.90000000000000))*(r00)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(sj0)*(x770)))+(((r02)*(x771)))+(((x772)*(x774)))+(((x773)*(x774)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x770)))+(((IkReal(-1.00000000000000))*(x772)*(x775)))+(((IkReal(-1.00000000000000))*(x773)*(x775)))+(((IkReal(-1.30000000000000))*(cj4))))))) < IKFAST_ATAN2_MAGTHRESH )
02517     continue;
02518 j3array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(pz)*(x770)))+(((r02)*(x775)))+(((x771)*(x773)))+(((IkReal(-20.0000000000000))*(py)*(r01)))+(((x771)*(x772)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(sj0)*(x770)))+(((r02)*(x771)))+(((x772)*(x774)))+(((x773)*(x774)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x770)))+(((IkReal(-1.00000000000000))*(x772)*(x775)))+(((IkReal(-1.00000000000000))*(x773)*(x775)))+(((IkReal(-1.30000000000000))*(cj4)))))));
02519 sj3array[0]=IKsin(j3array[0]);
02520 cj3array[0]=IKcos(j3array[0]);
02521 if( j3array[0] > IKPI )
02522 {
02523     j3array[0]-=IK2PI;
02524 }
02525 else if( j3array[0] < -IKPI )
02526 {    j3array[0]+=IK2PI;
02527 }
02528 j3valid[0] = true;
02529 for(int ij3 = 0; ij3 < 1; ++ij3)
02530 {
02531 if( !j3valid[ij3] )
02532 {
02533     continue;
02534 }
02535 _ij3[0] = ij3; _ij3[1] = -1;
02536 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02537 {
02538 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02539 {
02540     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02541 }
02542 }
02543 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02544 {
02545 IkReal evalcond[4];
02546 IkReal x776=IKcos(j3);
02547 IkReal x777=((r01)*(sj1));
02548 IkReal x778=((IkReal(0.350000000000000))*(sj0));
02549 IkReal x779=((cj0)*(r00));
02550 IkReal x780=((IkReal(0.350000000000000))*(cj1));
02551 IkReal x781=((px)*(r02));
02552 IkReal x782=((IkReal(1.00000000000000))*(cj0));
02553 IkReal x783=((IkReal(0.190000000000000))*(px));
02554 IkReal x784=((cj0)*(r01));
02555 IkReal x785=((r00)*(sj0));
02556 IkReal x786=((IkReal(0.700000000000000))*(sj1));
02557 IkReal x787=((IkReal(0.550000000000000))*(cj4));
02558 IkReal x788=((IkReal(0.700000000000000))*(cj1));
02559 IkReal x789=((py)*(sj0));
02560 IkReal x790=((IkReal(1.00000000000000))*(r02));
02561 IkReal x791=((cj0)*(py));
02562 IkReal x792=((IkReal(2.00000000000000))*(px));
02563 IkReal x793=((pz)*(sj0));
02564 IkReal x794=((pz)*(r02));
02565 IkReal x795=((IkReal(0.350000000000000))*(sj1));
02566 IkReal x796=((py)*(r01));
02567 IkReal x797=((IkReal(0.0715000000000000))*(x776));
02568 evalcond[0]=((IkReal(0.175200000000000))+(((IkReal(-0.190000000000000))*(x791)))+(((pz)*(x788)))+(((IkReal(-1.00000000000000))*(pp)))+(x797)+(((cj0)*(px)*(x786)))+(((sj0)*(x783)))+(((x786)*(x789))));
02569 evalcond[1]=((((r02)*(x780)))+(((x779)*(x795)))+(((x777)*(x778)))+(((IkReal(-1.00000000000000))*(x787)*(IKsin(j3))))+(((IkReal(0.0950000000000000))*(x785)))+(((IkReal(-1.00000000000000))*(pz)*(x790)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-0.0950000000000000))*(x784)))+(((IkReal(-1.00000000000000))*(x796))));
02570 evalcond[2]=((((IkReal(-1.00000000000000))*(x789)*(x790)))+(((r02)*(x795)))+(((IkReal(-1.00000000000000))*(x781)*(x782)))+(((IkReal(-1.00000000000000))*(x779)*(x780)))+(((IkReal(-0.0650000000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x776)*(x787)))+(((pz)*(x779)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x778)))+(((r01)*(x793))));
02571 evalcond[3]=((((sj0)*(x781)*(x788)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x782)))+(((IkReal(0.306725000000000))*(sj4)))+(((IkReal(-1.00000000000000))*(r01)*(x789)*(x792)))+(((IkReal(0.700000000000000))*(px)*(x777)))+(((py)*(x779)*(x792)))+(((IkReal(2.00000000000000))*(x784)*((py)*(py))))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x786)))+(((IkReal(-1.00000000000000))*(px)*(x785)*(x792)))+(((pz)*(x784)*(x788)))+(((pp)*(x785)))+(((sj4)*(x797)))+(((r00)*(x783)))+(((IkReal(0.113475000000000))*(x785)))+(((IkReal(-1.00000000000000))*(pz)*(x785)*(x788)))+(((IkReal(-0.0665000000000000))*(sj1)*(x779)))+(((IkReal(-1.00000000000000))*(r02)*(x788)*(x791)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-0.113475000000000))*(x784)))+(((IkReal(-2.00000000000000))*(x781)*(x793)))+(((IkReal(0.190000000000000))*(x794)))+(((IkReal(-0.0665000000000000))*(sj0)*(x777)))+(((IkReal(0.190000000000000))*(x796)))+(((IkReal(2.00000000000000))*(x791)*(x794))));
02572 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02573 {
02574 continue;
02575 }
02576 }
02577 
02578 {
02579 IkReal dummyeval[1];
02580 IkReal gconst1;
02581 gconst1=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
02582 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
02583 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02584 {
02585 {
02586 IkReal dummyeval[1];
02587 IkReal gconst2;
02588 IkReal x798=((IkReal(13.0000000000000))*(cj4));
02589 gconst2=IKsign(((((IkReal(-1.00000000000000))*(x798)*((sj3)*(sj3))))+(((IkReal(-1.00000000000000))*(x798)*((cj3)*(cj3))))+(((IkReal(-110.000000000000))*(cj3)*(cj4)))));
02590 IkReal x799=((IkReal(1.00000000000000))*(cj4));
02591 dummyeval[0]=((((IkReal(-1.00000000000000))*(x799)*((cj3)*(cj3))))+(((IkReal(-8.46153846153846))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x799)*((sj3)*(sj3)))));
02592 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02593 {
02594 {
02595 IkReal evalcond[11];
02596 IkReal x800=((IkReal(0.0715000000000000))*(cj3));
02597 IkReal x801=(py)*(py);
02598 IkReal x802=(px)*(px);
02599 IkReal x803=(pz)*(pz);
02600 IkReal x804=((r01)*(sj0));
02601 IkReal x805=((px)*(sj0));
02602 IkReal x806=((cj1)*(r00));
02603 IkReal x807=((IkReal(0.350000000000000))*(cj0));
02604 IkReal x808=((IkReal(0.113475000000000))*(sj1));
02605 IkReal x809=((r01)*(sj1));
02606 IkReal x810=((IkReal(2.00000000000000))*(pz));
02607 IkReal x811=((IkReal(0.190000000000000))*(px));
02608 IkReal x812=((py)*(sj0));
02609 IkReal x813=((IkReal(1.00000000000000))*(r02));
02610 IkReal x814=((IkReal(0.190000000000000))*(py));
02611 IkReal x815=((cj0)*(sj1));
02612 IkReal x816=((px)*(r02));
02613 IkReal x817=((px)*(r00));
02614 IkReal x818=((r02)*(sj1));
02615 IkReal x819=((IkReal(2.00000000000000))*(py));
02616 IkReal x820=((pp)*(r00));
02617 IkReal x821=((cj0)*(r01));
02618 IkReal x822=((IkReal(0.700000000000000))*(sj1));
02619 IkReal x823=((IkReal(0.190000000000000))*(pz));
02620 IkReal x824=((cj1)*(r01));
02621 IkReal x825=((cj0)*(r00));
02622 IkReal x826=((IkReal(0.700000000000000))*(pz));
02623 IkReal x827=((cj0)*(px));
02624 IkReal x828=((r00)*(sj0));
02625 IkReal x829=((cj1)*(r02));
02626 IkReal x830=((IkReal(1.00000000000000))*(pp));
02627 IkReal x831=((IkReal(1.00000000000000))*(py));
02628 IkReal x832=((IkReal(0.700000000000000))*(cj0));
02629 IkReal x833=((r00)*(sj1));
02630 IkReal x834=((IkReal(0.700000000000000))*(px));
02631 IkReal x835=((cj0)*(cj1));
02632 IkReal x836=((cj1)*(x830));
02633 IkReal x837=((IkReal(2.00000000000000))*(x803));
02634 IkReal x838=((IkReal(2.00000000000000))*(x801));
02635 IkReal x839=((IkReal(2.00000000000000))*(r00)*(x802));
02636 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
02637 evalcond[1]=((IkReal(-0.0950000000000000))+(x805)+(((IkReal(-1.00000000000000))*(cj0)*(x831))));
02638 evalcond[2]=((IkReal(1.00000000000000))+(x828)+(((IkReal(-1.00000000000000))*(x821))));
02639 evalcond[3]=((x829)+(((r00)*(x815)))+(((sj1)*(x804))));
02640 evalcond[4]=((((IkReal(-1.00000000000000))*(cj0)*(x806)))+(((IkReal(-1.00000000000000))*(cj1)*(x804)))+(x818));
02641 evalcond[5]=((IkReal(0.175200000000000))+(x800)+(((IkReal(-1.00000000000000))*(x830)))+(((IkReal(-1.00000000000000))*(cj0)*(x814)))+(((x815)*(x834)))+(((cj1)*(x826)))+(((x812)*(x822)))+(((IkReal(0.190000000000000))*(x805))));
02642 evalcond[6]=((((IkReal(-1.00000000000000))*(pz)*(x813)))+(((IkReal(0.350000000000000))*(sj1)*(x804)))+(((IkReal(-1.00000000000000))*(x817)))+(((IkReal(-1.00000000000000))*(r01)*(x831)))+(((IkReal(0.350000000000000))*(x829)))+(((IkReal(-0.0950000000000000))*(x821)))+(((IkReal(0.0950000000000000))*(x828)))+(((x807)*(x833))));
02643 evalcond[7]=((((IkReal(-1.00000000000000))*(x806)*(x807)))+(((IkReal(-1.00000000000000))*(x813)*(x827)))+(((IkReal(-0.350000000000000))*(cj1)*(x804)))+(((IkReal(-1.00000000000000))*(x812)*(x813)))+(((IkReal(0.350000000000000))*(x818)))+(((pz)*(x825)))+(((pz)*(x804))));
02644 evalcond[8]=((IkReal(0.306725000000000))+(((cj1)*(x821)*(x826)))+(((r02)*(x823)))+(x800)+(((IkReal(-1.00000000000000))*(py)*(x829)*(x832)))+(((IkReal(-1.00000000000000))*(r02)*(x805)*(x810)))+(((cj0)*(py)*(r02)*(x810)))+(((IkReal(0.700000000000000))*(x805)*(x829)))+(((cj0)*(x817)*(x819)))+(((r00)*(x811)))+(((r01)*(x814)))+(((IkReal(-0.0665000000000000))*(r00)*(x815)))+(((sj0)*(x820)))+(((IkReal(0.113475000000000))*(x828)))+(((IkReal(-0.0665000000000000))*(x829)))+(((x809)*(x834)))+(((IkReal(-1.00000000000000))*(x821)*(x830)))+(((IkReal(-1.00000000000000))*(sj0)*(x806)*(x826)))+(((IkReal(-2.00000000000000))*(x802)*(x828)))+(((IkReal(-0.113475000000000))*(x821)))+(((x821)*(x838)))+(((IkReal(-0.0665000000000000))*(sj1)*(x804)))+(((IkReal(-1.00000000000000))*(px)*(x804)*(x819)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x822))));
02645 evalcond[9]=((((pp)*(sj1)*(x804)))+(((sj0)*(x806)*(x823)))+(((r02)*(x826)))+(((IkReal(-1.00000000000000))*(cj1)*(x821)*(x823)))+(((IkReal(-1.00000000000000))*(x810)*(x812)*(x818)))+(((IkReal(-1.00000000000000))*(x815)*(x839)))+(((IkReal(-1.00000000000000))*(x829)*(x837)))+(((pp)*(x829)))+(((x815)*(x820)))+(((IkReal(0.0665000000000000))*(x821)))+(((IkReal(-0.190000000000000))*(x805)*(x829)))+(((IkReal(-1.00000000000000))*(x810)*(x815)*(x816)))+(((cj0)*(x814)*(x829)))+(((IkReal(-1.00000000000000))*(x809)*(x819)*(x827)))+(((IkReal(-1.00000000000000))*(x804)*(x808)))+(((IkReal(0.700000000000000))*(x817)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((x814)*(x833)))+(((IkReal(-0.0665000000000000))*(x828)))+(((IkReal(-1.00000000000000))*(sj1)*(x804)*(x838)))+(((IkReal(-1.00000000000000))*(py)*(x810)*(x824)))+(((IkReal(-1.00000000000000))*(x809)*(x811)))+(((IkReal(-1.00000000000000))*(x808)*(x825)))+(((IkReal(-1.00000000000000))*(x805)*(x819)*(x833)))+(((IkReal(-0.113475000000000))*(x829)))+(((IkReal(-1.00000000000000))*(px)*(x806)*(x810))));
02646 evalcond[10]=((((x825)*(x826)))+(((IkReal(-1.00000000000000))*(sj1)*(x810)*(x817)))+(((IkReal(-0.131525000000000))*(cj0)*(x806)))+(((pp)*(x818)))+(((cj1)*(px)*(x819)*(x821)))+(((x805)*(x806)*(x819)))+(((sj1)*(x823)*(x828)))+(((x811)*(x824)))+(((IkReal(0.131525000000000))*(x818)))+(((IkReal(-0.190000000000000))*(x805)*(x818)))+(((IkReal(-0.700000000000000))*(r02)*(x812)))+(((IkReal(-1.00000000000000))*(x804)*(x836)))+(((IkReal(-1.00000000000000))*(x806)*(x814)))+(((IkReal(-1.00000000000000))*(py)*(x809)*(x810)))+(((cj1)*(x804)*(x838)))+(((IkReal(-0.131525000000000))*(cj1)*(x804)))+(((x810)*(x816)*(x835)))+(((IkReal(-1.00000000000000))*(x818)*(x837)))+(((r02)*(x814)*(x815)))+(((IkReal(-1.00000000000000))*(x816)*(x832)))+(((IkReal(2.00000000000000))*(cj0)*(x802)*(x806)))+(((x804)*(x826)))+(((IkReal(-1.00000000000000))*(cj0)*(x806)*(x830)))+(((IkReal(-1.00000000000000))*(cj0)*(x809)*(x823)))+(((x810)*(x812)*(x829))));
02647 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  )
02648 {
02649 {
02650 IkReal dummyeval[1];
02651 IkReal gconst3;
02652 gconst3=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
02653 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
02654 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02655 {
02656 {
02657 IkReal dummyeval[1];
02658 IkReal gconst4;
02659 gconst4=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
02660 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
02661 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02662 {
02663 continue;
02664 
02665 } else
02666 {
02667 {
02668 IkReal j2array[1], cj2array[1], sj2array[1];
02669 bool j2valid[1]={false};
02670 _nj2 = 1;
02671 IkReal x840=((cj1)*(cj3));
02672 IkReal x841=((sj0)*(sj1));
02673 IkReal x842=((IkReal(22000.0000000000))*(pz));
02674 IkReal x843=((IkReal(2600.00000000000))*(pz));
02675 IkReal x844=((r02)*(sj1));
02676 IkReal x845=((cj1)*(sj3));
02677 IkReal x846=((cj1)*(r00));
02678 IkReal x847=((r01)*(sj0));
02679 IkReal x848=((cj0)*(sj1));
02680 IkReal x849=((IkReal(2600.00000000000))*(px));
02681 IkReal x850=((IkReal(22000.0000000000))*(px));
02682 IkReal x851=((IkReal(22000.0000000000))*(py));
02683 IkReal x852=((IkReal(2600.00000000000))*(py));
02684 IkReal x853=((IkReal(247.000000000000))*(cj0)*(r00));
02685 IkReal x854=((sj3)*(x852));
02686 IkReal x855=((IkReal(2600.00000000000))*(sj3)*(x848));
02687 if( IKabs(((gconst4)*(((((r02)*(x841)*(x850)))+(((x843)*(x845)))+(((r01)*(x842)*(x848)))+(((IkReal(-1.00000000000000))*(r01)*(x840)*(x849)))+(((IkReal(-247.000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x841)*(x842)))+(((sj3)*(x848)*(x849)))+(((IkReal(2090.00000000000))*(cj1)*(x847)))+(((cj3)*(r02)*(x841)*(x849)))+(((IkReal(2090.00000000000))*(cj0)*(x846)))+(((IkReal(247.000000000000))*(x840)*(x847)))+(((r00)*(x840)*(x852)))+(((IkReal(-2090.00000000000))*(x844)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x844)*(x852)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x850)))+(((cj3)*(r01)*(x843)*(x848)))+(((x846)*(x851)))+(((IkReal(-910.000000000000))*(sj3)))+(((x840)*(x853)))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x851)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x841)*(x843)))+(((x841)*(x854))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x854)))+(((r00)*(x845)*(x852)))+(((IkReal(-1.00000000000000))*(cj3)*(x841)*(x852)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x841)*(x843)))+(((IkReal(-1.00000000000000))*(cj3)*(x848)*(x849)))+(((IkReal(-1.00000000000000))*(cj1)*(x842)))+(((IkReal(-1.00000000000000))*(x840)*(x843)))+(((r02)*(sj3)*(x841)*(x849)))+(((IkReal(-1.00000000000000))*(x841)*(x851)))+(((x845)*(x853)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(247.000000000000))*(x845)*(x847)))+(((IkReal(-1.00000000000000))*(r01)*(x845)*(x849)))+(((r01)*(sj3)*(x843)*(x848)))+(((IkReal(-1.00000000000000))*(x848)*(x850)))+(((IkReal(-247.000000000000))*(sj3)*(x844))))))) < IKFAST_ATAN2_MAGTHRESH )
02688     continue;
02689 j2array[0]=IKatan2(((gconst4)*(((((r02)*(x841)*(x850)))+(((x843)*(x845)))+(((r01)*(x842)*(x848)))+(((IkReal(-1.00000000000000))*(r01)*(x840)*(x849)))+(((IkReal(-247.000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x841)*(x842)))+(((sj3)*(x848)*(x849)))+(((IkReal(2090.00000000000))*(cj1)*(x847)))+(((cj3)*(r02)*(x841)*(x849)))+(((IkReal(2090.00000000000))*(cj0)*(x846)))+(((IkReal(247.000000000000))*(x840)*(x847)))+(((r00)*(x840)*(x852)))+(((IkReal(-2090.00000000000))*(x844)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x844)*(x852)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x850)))+(((cj3)*(r01)*(x843)*(x848)))+(((x846)*(x851)))+(((IkReal(-910.000000000000))*(sj3)))+(((x840)*(x853)))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x851)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x841)*(x843)))+(((x841)*(x854)))))), ((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x854)))+(((r00)*(x845)*(x852)))+(((IkReal(-1.00000000000000))*(cj3)*(x841)*(x852)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x841)*(x843)))+(((IkReal(-1.00000000000000))*(cj3)*(x848)*(x849)))+(((IkReal(-1.00000000000000))*(cj1)*(x842)))+(((IkReal(-1.00000000000000))*(x840)*(x843)))+(((r02)*(sj3)*(x841)*(x849)))+(((IkReal(-1.00000000000000))*(x841)*(x851)))+(((x845)*(x853)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(247.000000000000))*(x845)*(x847)))+(((IkReal(-1.00000000000000))*(r01)*(x845)*(x849)))+(((r01)*(sj3)*(x843)*(x848)))+(((IkReal(-1.00000000000000))*(x848)*(x850)))+(((IkReal(-247.000000000000))*(sj3)*(x844)))))));
02690 sj2array[0]=IKsin(j2array[0]);
02691 cj2array[0]=IKcos(j2array[0]);
02692 if( j2array[0] > IKPI )
02693 {
02694     j2array[0]-=IK2PI;
02695 }
02696 else if( j2array[0] < -IKPI )
02697 {    j2array[0]+=IK2PI;
02698 }
02699 j2valid[0] = true;
02700 for(int ij2 = 0; ij2 < 1; ++ij2)
02701 {
02702 if( !j2valid[ij2] )
02703 {
02704     continue;
02705 }
02706 _ij2[0] = ij2; _ij2[1] = -1;
02707 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02708 {
02709 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02710 {
02711     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02712 }
02713 }
02714 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02715 {
02716 IkReal evalcond[4];
02717 IkReal x856=IKcos(j2);
02718 IkReal x857=IKsin(j2);
02719 IkReal x858=((px)*(sj1));
02720 IkReal x859=((py)*(sj1));
02721 IkReal x860=((cj1)*(sj0));
02722 IkReal x861=((IkReal(1.00000000000000))*(px));
02723 IkReal x862=((r00)*(sj0));
02724 IkReal x863=((IkReal(0.0950000000000000))*(r01));
02725 IkReal x864=((IkReal(0.0650000000000000))*(cj3));
02726 IkReal x865=((cj0)*(r01));
02727 IkReal x866=((IkReal(0.0950000000000000))*(cj1));
02728 IkReal x867=((pz)*(sj1));
02729 IkReal x868=((cj0)*(r00));
02730 IkReal x869=((IkReal(0.0950000000000000))*(sj1));
02731 IkReal x870=((cj0)*(cj1));
02732 IkReal x871=((cj1)*(pz));
02733 IkReal x872=((IkReal(0.0650000000000000))*(sj3));
02734 IkReal x873=((IkReal(0.550000000000000))*(x857));
02735 IkReal x874=((IkReal(0.550000000000000))*(x856));
02736 IkReal x875=((x857)*(x864));
02737 IkReal x876=((x856)*(x872));
02738 IkReal x877=((x856)*(x864));
02739 IkReal x878=((x857)*(x872));
02740 IkReal x879=((x874)+(x877));
02741 IkReal x880=((x873)+(x875)+(x876));
02742 evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x859)))+(((cj0)*(x858)))+(x878)+(x871)+(((IkReal(-1.00000000000000))*(x879))));
02743 evalcond[1]=((x880)+(((IkReal(-1.00000000000000))*(py)*(x860)))+(((IkReal(-1.00000000000000))*(x861)*(x870)))+(x867));
02744 evalcond[2]=((x880)+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x861)))+(((x860)*(x863)))+(((IkReal(-1.00000000000000))*(x862)*(x867)))+(((cj1)*(py)*(r00)))+(((x866)*(x868)))+(((x865)*(x867)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x859)))+(((r02)*(sj0)*(x858)))+(((IkReal(-1.00000000000000))*(r02)*(x869))));
02745 evalcond[3]=((((IkReal(-1.00000000000000))*(x865)*(x871)))+(((IkReal(-0.350000000000000))*(x862)))+(((IkReal(-1.00000000000000))*(r01)*(x858)))+(((IkReal(-1.00000000000000))*(r02)*(x860)*(x861)))+(((sj0)*(sj1)*(x863)))+(((x868)*(x869)))+(((IkReal(0.350000000000000))*(x865)))+(((r00)*(x859)))+(((IkReal(-1.00000000000000))*(x878)))+(x879)+(((pz)*(r00)*(x860)))+(((r02)*(x866)))+(((py)*(r02)*(x870))));
02746 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02747 {
02748 continue;
02749 }
02750 }
02751 
02752 {
02753 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02754 vinfos[0].jointtype = 1;
02755 vinfos[0].foffset = j0;
02756 vinfos[0].indices[0] = _ij0[0];
02757 vinfos[0].indices[1] = _ij0[1];
02758 vinfos[0].maxsolutions = _nj0;
02759 vinfos[1].jointtype = 1;
02760 vinfos[1].foffset = j1;
02761 vinfos[1].indices[0] = _ij1[0];
02762 vinfos[1].indices[1] = _ij1[1];
02763 vinfos[1].maxsolutions = _nj1;
02764 vinfos[2].jointtype = 1;
02765 vinfos[2].foffset = j2;
02766 vinfos[2].indices[0] = _ij2[0];
02767 vinfos[2].indices[1] = _ij2[1];
02768 vinfos[2].maxsolutions = _nj2;
02769 vinfos[3].jointtype = 1;
02770 vinfos[3].foffset = j3;
02771 vinfos[3].indices[0] = _ij3[0];
02772 vinfos[3].indices[1] = _ij3[1];
02773 vinfos[3].maxsolutions = _nj3;
02774 vinfos[4].jointtype = 1;
02775 vinfos[4].foffset = j4;
02776 vinfos[4].indices[0] = _ij4[0];
02777 vinfos[4].indices[1] = _ij4[1];
02778 vinfos[4].maxsolutions = _nj4;
02779 std::vector<int> vfree(0);
02780 solutions.AddSolution(vinfos,vfree);
02781 }
02782 }
02783 }
02784 
02785 }
02786 
02787 }
02788 
02789 } else
02790 {
02791 {
02792 IkReal j2array[1], cj2array[1], sj2array[1];
02793 bool j2valid[1]={false};
02794 _nj2 = 1;
02795 IkReal x881=((IkReal(2600.00000000000))*(sj3));
02796 IkReal x882=((py)*(sj0));
02797 IkReal x883=((pz)*(sj1));
02798 IkReal x884=((IkReal(2600.00000000000))*(cj3));
02799 IkReal x885=((cj1)*(pz));
02800 IkReal x886=((IkReal(22000.0000000000))*(cj1));
02801 IkReal x887=((cj0)*(px));
02802 IkReal x888=((sj1)*(x887));
02803 if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x882)*(x886)))+(((IkReal(-1.00000000000000))*(cj1)*(x884)*(x887)))+(((IkReal(22000.0000000000))*(x883)))+(((sj1)*(x881)*(x882)))+(((IkReal(-1.00000000000000))*(cj1)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(x886)*(x887)))+(((IkReal(-910.000000000000))*(sj3)))+(((x881)*(x885)))+(((x881)*(x888)))+(((x883)*(x884))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x885)))+(((IkReal(-1.00000000000000))*(x884)*(x885)))+(((x881)*(x883)))+(((IkReal(-1.00000000000000))*(sj1)*(x882)*(x884)))+(((IkReal(-22000.0000000000))*(sj1)*(x882)))+(((IkReal(-1.00000000000000))*(x884)*(x888)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x888)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x887)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x882))))))) < IKFAST_ATAN2_MAGTHRESH )
02804     continue;
02805 j2array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x882)*(x886)))+(((IkReal(-1.00000000000000))*(cj1)*(x884)*(x887)))+(((IkReal(22000.0000000000))*(x883)))+(((sj1)*(x881)*(x882)))+(((IkReal(-1.00000000000000))*(cj1)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(x886)*(x887)))+(((IkReal(-910.000000000000))*(sj3)))+(((x881)*(x885)))+(((x881)*(x888)))+(((x883)*(x884)))))), ((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x885)))+(((IkReal(-1.00000000000000))*(x884)*(x885)))+(((x881)*(x883)))+(((IkReal(-1.00000000000000))*(sj1)*(x882)*(x884)))+(((IkReal(-22000.0000000000))*(sj1)*(x882)))+(((IkReal(-1.00000000000000))*(x884)*(x888)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x888)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x887)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x882)))))));
02806 sj2array[0]=IKsin(j2array[0]);
02807 cj2array[0]=IKcos(j2array[0]);
02808 if( j2array[0] > IKPI )
02809 {
02810     j2array[0]-=IK2PI;
02811 }
02812 else if( j2array[0] < -IKPI )
02813 {    j2array[0]+=IK2PI;
02814 }
02815 j2valid[0] = true;
02816 for(int ij2 = 0; ij2 < 1; ++ij2)
02817 {
02818 if( !j2valid[ij2] )
02819 {
02820     continue;
02821 }
02822 _ij2[0] = ij2; _ij2[1] = -1;
02823 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
02824 {
02825 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
02826 {
02827     j2valid[iij2]=false; _ij2[1] = iij2; break; 
02828 }
02829 }
02830 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
02831 {
02832 IkReal evalcond[4];
02833 IkReal x889=IKcos(j2);
02834 IkReal x890=IKsin(j2);
02835 IkReal x891=((px)*(sj1));
02836 IkReal x892=((py)*(sj1));
02837 IkReal x893=((cj1)*(sj0));
02838 IkReal x894=((IkReal(1.00000000000000))*(px));
02839 IkReal x895=((r00)*(sj0));
02840 IkReal x896=((IkReal(0.0950000000000000))*(r01));
02841 IkReal x897=((IkReal(0.0650000000000000))*(cj3));
02842 IkReal x898=((cj0)*(r01));
02843 IkReal x899=((IkReal(0.0950000000000000))*(cj1));
02844 IkReal x900=((pz)*(sj1));
02845 IkReal x901=((cj0)*(r00));
02846 IkReal x902=((IkReal(0.0950000000000000))*(sj1));
02847 IkReal x903=((cj0)*(cj1));
02848 IkReal x904=((cj1)*(pz));
02849 IkReal x905=((IkReal(0.0650000000000000))*(sj3));
02850 IkReal x906=((IkReal(0.550000000000000))*(x890));
02851 IkReal x907=((IkReal(0.550000000000000))*(x889));
02852 IkReal x908=((x890)*(x897));
02853 IkReal x909=((x889)*(x905));
02854 IkReal x910=((x889)*(x897));
02855 IkReal x911=((x890)*(x905));
02856 IkReal x912=((x907)+(x910));
02857 IkReal x913=((x906)+(x908)+(x909));
02858 evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x912)))+(x904)+(((cj0)*(x891)))+(x911)+(((sj0)*(x892))));
02859 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x893)))+(x900)+(((IkReal(-1.00000000000000))*(x894)*(x903)))+(x913));
02860 evalcond[2]=((((x893)*(x896)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x894)))+(((r02)*(sj0)*(x891)))+(((IkReal(-1.00000000000000))*(x895)*(x900)))+(((IkReal(-1.00000000000000))*(r02)*(x902)))+(x913)+(((x898)*(x900)))+(((x899)*(x901)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x892))));
02861 evalcond[3]=((((sj0)*(sj1)*(x896)))+(((IkReal(0.350000000000000))*(x898)))+(((x901)*(x902)))+(((IkReal(-1.00000000000000))*(x911)))+(((IkReal(-1.00000000000000))*(r01)*(x891)))+(((r02)*(x899)))+(((pz)*(r00)*(x893)))+(((r00)*(x892)))+(x912)+(((IkReal(-0.350000000000000))*(x895)))+(((IkReal(-1.00000000000000))*(x898)*(x904)))+(((IkReal(-1.00000000000000))*(r02)*(x893)*(x894)))+(((py)*(r02)*(x903))));
02862 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02863 {
02864 continue;
02865 }
02866 }
02867 
02868 {
02869 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
02870 vinfos[0].jointtype = 1;
02871 vinfos[0].foffset = j0;
02872 vinfos[0].indices[0] = _ij0[0];
02873 vinfos[0].indices[1] = _ij0[1];
02874 vinfos[0].maxsolutions = _nj0;
02875 vinfos[1].jointtype = 1;
02876 vinfos[1].foffset = j1;
02877 vinfos[1].indices[0] = _ij1[0];
02878 vinfos[1].indices[1] = _ij1[1];
02879 vinfos[1].maxsolutions = _nj1;
02880 vinfos[2].jointtype = 1;
02881 vinfos[2].foffset = j2;
02882 vinfos[2].indices[0] = _ij2[0];
02883 vinfos[2].indices[1] = _ij2[1];
02884 vinfos[2].maxsolutions = _nj2;
02885 vinfos[3].jointtype = 1;
02886 vinfos[3].foffset = j3;
02887 vinfos[3].indices[0] = _ij3[0];
02888 vinfos[3].indices[1] = _ij3[1];
02889 vinfos[3].maxsolutions = _nj3;
02890 vinfos[4].jointtype = 1;
02891 vinfos[4].foffset = j4;
02892 vinfos[4].indices[0] = _ij4[0];
02893 vinfos[4].indices[1] = _ij4[1];
02894 vinfos[4].maxsolutions = _nj4;
02895 std::vector<int> vfree(0);
02896 solutions.AddSolution(vinfos,vfree);
02897 }
02898 }
02899 }
02900 
02901 }
02902 
02903 }
02904 
02905 } else
02906 {
02907 IkReal x914=((IkReal(0.0715000000000000))*(cj3));
02908 IkReal x915=(py)*(py);
02909 IkReal x916=(px)*(px);
02910 IkReal x917=(pz)*(pz);
02911 IkReal x918=((r01)*(sj0));
02912 IkReal x919=((px)*(sj0));
02913 IkReal x920=((cj1)*(r00));
02914 IkReal x921=((IkReal(0.350000000000000))*(cj0));
02915 IkReal x922=((IkReal(0.113475000000000))*(sj1));
02916 IkReal x923=((r01)*(sj1));
02917 IkReal x924=((IkReal(2.00000000000000))*(pz));
02918 IkReal x925=((IkReal(0.190000000000000))*(px));
02919 IkReal x926=((py)*(sj0));
02920 IkReal x927=((IkReal(1.00000000000000))*(r02));
02921 IkReal x928=((IkReal(0.190000000000000))*(py));
02922 IkReal x929=((cj0)*(sj1));
02923 IkReal x930=((px)*(r02));
02924 IkReal x931=((px)*(r00));
02925 IkReal x932=((r02)*(sj1));
02926 IkReal x933=((IkReal(2.00000000000000))*(py));
02927 IkReal x934=((pp)*(r00));
02928 IkReal x935=((cj0)*(r01));
02929 IkReal x936=((IkReal(0.700000000000000))*(sj1));
02930 IkReal x937=((IkReal(0.190000000000000))*(pz));
02931 IkReal x938=((cj1)*(r01));
02932 IkReal x939=((cj0)*(r00));
02933 IkReal x940=((IkReal(0.700000000000000))*(pz));
02934 IkReal x941=((cj0)*(px));
02935 IkReal x942=((r00)*(sj0));
02936 IkReal x943=((cj1)*(r02));
02937 IkReal x944=((IkReal(1.00000000000000))*(pp));
02938 IkReal x945=((IkReal(1.00000000000000))*(py));
02939 IkReal x946=((IkReal(0.700000000000000))*(cj0));
02940 IkReal x947=((r00)*(sj1));
02941 IkReal x948=((IkReal(0.700000000000000))*(px));
02942 IkReal x949=((cj0)*(cj1));
02943 IkReal x950=((cj1)*(x944));
02944 IkReal x951=((IkReal(2.00000000000000))*(x917));
02945 IkReal x952=((IkReal(2.00000000000000))*(x915));
02946 IkReal x953=((IkReal(2.00000000000000))*(r00)*(x916));
02947 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
02948 evalcond[1]=((IkReal(-0.0950000000000000))+(x919)+(((IkReal(-1.00000000000000))*(cj0)*(x945))));
02949 evalcond[2]=((IkReal(-1.00000000000000))+(x942)+(((IkReal(-1.00000000000000))*(x935))));
02950 evalcond[3]=((((r00)*(x929)))+(x943)+(((sj1)*(x918))));
02951 evalcond[4]=((x932)+(((IkReal(-1.00000000000000))*(cj1)*(x918)))+(((IkReal(-1.00000000000000))*(cj0)*(x920))));
02952 evalcond[5]=((IkReal(0.175200000000000))+(((x929)*(x948)))+(((cj1)*(x940)))+(((IkReal(-1.00000000000000))*(cj0)*(x928)))+(((IkReal(-1.00000000000000))*(x944)))+(((IkReal(0.190000000000000))*(x919)))+(x914)+(((x926)*(x936))));
02953 evalcond[6]=((((IkReal(-1.00000000000000))*(pz)*(x927)))+(((IkReal(0.350000000000000))*(sj1)*(x918)))+(((IkReal(0.350000000000000))*(x943)))+(((IkReal(-1.00000000000000))*(x931)))+(((x921)*(x947)))+(((IkReal(-1.00000000000000))*(r01)*(x945)))+(((IkReal(0.0950000000000000))*(x942)))+(((IkReal(-0.0950000000000000))*(x935))));
02954 evalcond[7]=((((IkReal(-1.00000000000000))*(x927)*(x941)))+(((IkReal(-1.00000000000000))*(x920)*(x921)))+(((pz)*(x918)))+(((IkReal(-0.350000000000000))*(cj1)*(x918)))+(((pz)*(x939)))+(((IkReal(-1.00000000000000))*(x926)*(x927)))+(((IkReal(0.350000000000000))*(x932))));
02955 evalcond[8]=((IkReal(-0.306725000000000))+(((IkReal(-2.00000000000000))*(x916)*(x942)))+(((IkReal(-0.113475000000000))*(x935)))+(((IkReal(0.113475000000000))*(x942)))+(((IkReal(-1.00000000000000))*(py)*(x943)*(x946)))+(((r01)*(x928)))+(((r02)*(x937)))+(((x935)*(x952)))+(((cj0)*(py)*(r02)*(x924)))+(((IkReal(-1.00000000000000))*(r02)*(x919)*(x924)))+(((IkReal(-1.00000000000000))*(px)*(x918)*(x933)))+(((cj0)*(x931)*(x933)))+(((IkReal(-1.00000000000000))*(x935)*(x944)))+(((IkReal(-0.0665000000000000))*(sj1)*(x918)))+(((IkReal(-1.00000000000000))*(sj0)*(x920)*(x940)))+(((IkReal(0.700000000000000))*(x919)*(x943)))+(((x923)*(x948)))+(((IkReal(-1.00000000000000))*(x914)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x936)))+(((IkReal(-0.0665000000000000))*(x943)))+(((sj0)*(x934)))+(((r00)*(x925)))+(((cj1)*(x935)*(x940)))+(((IkReal(-0.0665000000000000))*(r00)*(x929))));
02956 evalcond[9]=((((IkReal(-1.00000000000000))*(x943)*(x951)))+(((IkReal(-1.00000000000000))*(x924)*(x926)*(x932)))+(((pp)*(x943)))+(((IkReal(-1.00000000000000))*(py)*(x924)*(x938)))+(((IkReal(-1.00000000000000))*(x918)*(x922)))+(((sj0)*(x920)*(x937)))+(((IkReal(-0.190000000000000))*(x919)*(x943)))+(((x929)*(x934)))+(((pp)*(sj1)*(x918)))+(((IkReal(-1.00000000000000))*(sj1)*(x918)*(x952)))+(((IkReal(-0.113475000000000))*(x943)))+(((IkReal(-1.00000000000000))*(x923)*(x925)))+(((cj0)*(x928)*(x943)))+(((IkReal(-1.00000000000000))*(x929)*(x953)))+(((IkReal(-0.0665000000000000))*(x942)))+(((IkReal(0.700000000000000))*(x931)))+(((IkReal(-1.00000000000000))*(cj1)*(x935)*(x937)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x919)*(x933)*(x947)))+(((IkReal(-1.00000000000000))*(x923)*(x933)*(x941)))+(((IkReal(-1.00000000000000))*(x924)*(x929)*(x930)))+(((r02)*(x940)))+(((IkReal(0.0665000000000000))*(x935)))+(((x928)*(x947)))+(((IkReal(-1.00000000000000))*(px)*(x920)*(x924)))+(((IkReal(-1.00000000000000))*(x922)*(x939))));
02957 evalcond[10]=((((IkReal(-1.00000000000000))*(cj0)*(x920)*(x944)))+(((x925)*(x938)))+(((IkReal(-1.00000000000000))*(x932)*(x951)))+(((cj1)*(x918)*(x952)))+(((IkReal(-0.131525000000000))*(cj1)*(x918)))+(((IkReal(2.00000000000000))*(cj0)*(x916)*(x920)))+(((pp)*(x932)))+(((x919)*(x920)*(x933)))+(((x939)*(x940)))+(((x924)*(x930)*(x949)))+(((IkReal(-1.00000000000000))*(py)*(x923)*(x924)))+(((IkReal(-1.00000000000000))*(x930)*(x946)))+(((sj1)*(x937)*(x942)))+(((x924)*(x926)*(x943)))+(((IkReal(-1.00000000000000))*(cj0)*(x923)*(x937)))+(((cj1)*(px)*(x933)*(x935)))+(((IkReal(-0.700000000000000))*(r02)*(x926)))+(((x918)*(x940)))+(((IkReal(-0.131525000000000))*(cj0)*(x920)))+(((r02)*(x928)*(x929)))+(((IkReal(-1.00000000000000))*(sj1)*(x924)*(x931)))+(((IkReal(-1.00000000000000))*(x920)*(x928)))+(((IkReal(-0.190000000000000))*(x919)*(x932)))+(((IkReal(-1.00000000000000))*(x918)*(x950)))+(((IkReal(0.131525000000000))*(x932))));
02958 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  )
02959 {
02960 {
02961 IkReal dummyeval[1];
02962 IkReal gconst5;
02963 gconst5=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
02964 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
02965 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02966 {
02967 {
02968 IkReal dummyeval[1];
02969 IkReal gconst6;
02970 gconst6=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
02971 dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
02972 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02973 {
02974 continue;
02975 
02976 } else
02977 {
02978 {
02979 IkReal j2array[1], cj2array[1], sj2array[1];
02980 bool j2valid[1]={false};
02981 _nj2 = 1;
02982 IkReal x954=((cj1)*(cj3));
02983 IkReal x955=((sj0)*(sj1));
02984 IkReal x956=((IkReal(22000.0000000000))*(pz));
02985 IkReal x957=((IkReal(2600.00000000000))*(pz));
02986 IkReal x958=((r02)*(sj1));
02987 IkReal x959=((IkReal(2600.00000000000))*(sj3));
02988 IkReal x960=((cj1)*(sj3));
02989 IkReal x961=((r01)*(sj0));
02990 IkReal x962=((cj0)*(sj1));
02991 IkReal x963=((IkReal(2600.00000000000))*(cj3));
02992 IkReal x964=((IkReal(2090.00000000000))*(cj1));
02993 IkReal x965=((IkReal(22000.0000000000))*(px));
02994 IkReal x966=((px)*(r02));
02995 IkReal x967=((IkReal(22000.0000000000))*(py));
02996 IkReal x968=((py)*(r00));
02997 IkReal x969=((px)*(r01));
02998 IkReal x970=((IkReal(247.000000000000))*(cj0)*(r00));
02999 IkReal x971=((x959)*(x962));
03000 if( IKabs(((gconst6)*(((((cj0)*(py)*(x958)*(x963)))+(((cj3)*(r00)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x967)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x964)))+(((IkReal(2090.00000000000))*(x958)))+(((IkReal(-2600.00000000000))*(x954)*(x968)))+(((x957)*(x960)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(-1.00000000000000))*(x955)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(r02)*(x955)*(x965)))+(((cj0)*(x958)*(x967)))+(((IkReal(-1.00000000000000))*(x954)*(x970)))+(((px)*(x971)))+(((py)*(x955)*(x959)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(2600.00000000000))*(x954)*(x969)))+(((r00)*(x955)*(x956)))+(((IkReal(247.000000000000))*(cj3)*(x958)))+(((cj1)*(r01)*(x965)))+(((IkReal(-247.000000000000))*(x954)*(x961)))+(((IkReal(-1.00000000000000))*(r01)*(x956)*(x962))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x959)*(x969)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x966)))+(((r00)*(sj3)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(x959)*(x968)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x962)*(x965)))+(((IkReal(-1.00000000000000))*(x955)*(x967)))+(((IkReal(-1.00000000000000))*(cj1)*(x956)))+(((IkReal(247.000000000000))*(sj3)*(x958)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x962)*(x963)))+(((IkReal(-1.00000000000000))*(x960)*(x970)))+(((cj0)*(py)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(py)*(x955)*(x963)))+(((IkReal(-247.000000000000))*(x960)*(x961))))))) < IKFAST_ATAN2_MAGTHRESH )
03001     continue;
03002 j2array[0]=IKatan2(((gconst6)*(((((cj0)*(py)*(x958)*(x963)))+(((cj3)*(r00)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x967)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x964)))+(((IkReal(2090.00000000000))*(x958)))+(((IkReal(-2600.00000000000))*(x954)*(x968)))+(((x957)*(x960)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(-1.00000000000000))*(x955)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(r02)*(x955)*(x965)))+(((cj0)*(x958)*(x967)))+(((IkReal(-1.00000000000000))*(x954)*(x970)))+(((px)*(x971)))+(((py)*(x955)*(x959)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(2600.00000000000))*(x954)*(x969)))+(((r00)*(x955)*(x956)))+(((IkReal(247.000000000000))*(cj3)*(x958)))+(((cj1)*(r01)*(x965)))+(((IkReal(-247.000000000000))*(x954)*(x961)))+(((IkReal(-1.00000000000000))*(r01)*(x956)*(x962)))))), ((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x959)*(x969)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x966)))+(((r00)*(sj3)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(x959)*(x968)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x962)*(x965)))+(((IkReal(-1.00000000000000))*(x955)*(x967)))+(((IkReal(-1.00000000000000))*(cj1)*(x956)))+(((IkReal(247.000000000000))*(sj3)*(x958)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x962)*(x963)))+(((IkReal(-1.00000000000000))*(x960)*(x970)))+(((cj0)*(py)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(py)*(x955)*(x963)))+(((IkReal(-247.000000000000))*(x960)*(x961)))))));
03003 sj2array[0]=IKsin(j2array[0]);
03004 cj2array[0]=IKcos(j2array[0]);
03005 if( j2array[0] > IKPI )
03006 {
03007     j2array[0]-=IK2PI;
03008 }
03009 else if( j2array[0] < -IKPI )
03010 {    j2array[0]+=IK2PI;
03011 }
03012 j2valid[0] = true;
03013 for(int ij2 = 0; ij2 < 1; ++ij2)
03014 {
03015 if( !j2valid[ij2] )
03016 {
03017     continue;
03018 }
03019 _ij2[0] = ij2; _ij2[1] = -1;
03020 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03021 {
03022 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03023 {
03024     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03025 }
03026 }
03027 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03028 {
03029 IkReal evalcond[4];
03030 IkReal x972=IKcos(j2);
03031 IkReal x973=IKsin(j2);
03032 IkReal x974=((px)*(sj1));
03033 IkReal x975=((py)*(sj1));
03034 IkReal x976=((cj1)*(sj0));
03035 IkReal x977=((IkReal(1.00000000000000))*(px));
03036 IkReal x978=((r00)*(sj0));
03037 IkReal x979=((IkReal(0.0950000000000000))*(r01));
03038 IkReal x980=((IkReal(0.0650000000000000))*(cj3));
03039 IkReal x981=((cj0)*(r01));
03040 IkReal x982=((IkReal(0.0950000000000000))*(cj1));
03041 IkReal x983=((pz)*(sj1));
03042 IkReal x984=((cj0)*(r00));
03043 IkReal x985=((IkReal(0.0950000000000000))*(sj1));
03044 IkReal x986=((cj0)*(cj1));
03045 IkReal x987=((cj1)*(pz));
03046 IkReal x988=((IkReal(0.0650000000000000))*(sj3));
03047 IkReal x989=((IkReal(0.550000000000000))*(x973));
03048 IkReal x990=((IkReal(0.550000000000000))*(x972));
03049 IkReal x991=((x973)*(x980));
03050 IkReal x992=((x972)*(x988));
03051 IkReal x993=((x973)*(x988));
03052 IkReal x994=((x972)*(x980));
03053 IkReal x995=((x990)+(x994));
03054 IkReal x996=((x989)+(x992)+(x991));
03055 evalcond[0]=((IkReal(-0.350000000000000))+(x987)+(((IkReal(-1.00000000000000))*(x995)))+(x993)+(((cj0)*(x974)))+(((sj0)*(x975))));
03056 evalcond[1]=((((IkReal(-1.00000000000000))*(x977)*(x986)))+(x983)+(((IkReal(-1.00000000000000))*(py)*(x976)))+(x996));
03057 evalcond[2]=((((x981)*(x983)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x977)))+(((x982)*(x984)))+(((IkReal(-1.00000000000000))*(x978)*(x983)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(r02)*(x985)))+(((x976)*(x979)))+(((IkReal(-1.00000000000000))*(x996)))+(((r02)*(sj0)*(x974)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x975))));
03058 evalcond[3]=((((r00)*(x975)))+(((IkReal(-0.350000000000000))*(x978)))+(((x984)*(x985)))+(((IkReal(-1.00000000000000))*(x981)*(x987)))+(((IkReal(0.350000000000000))*(x981)))+(((pz)*(r00)*(x976)))+(((py)*(r02)*(x986)))+(((IkReal(-1.00000000000000))*(r01)*(x974)))+(((sj0)*(sj1)*(x979)))+(((r02)*(x982)))+(((IkReal(-1.00000000000000))*(r02)*(x976)*(x977)))+(((IkReal(-1.00000000000000))*(x995)))+(x993));
03059 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03060 {
03061 continue;
03062 }
03063 }
03064 
03065 {
03066 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03067 vinfos[0].jointtype = 1;
03068 vinfos[0].foffset = j0;
03069 vinfos[0].indices[0] = _ij0[0];
03070 vinfos[0].indices[1] = _ij0[1];
03071 vinfos[0].maxsolutions = _nj0;
03072 vinfos[1].jointtype = 1;
03073 vinfos[1].foffset = j1;
03074 vinfos[1].indices[0] = _ij1[0];
03075 vinfos[1].indices[1] = _ij1[1];
03076 vinfos[1].maxsolutions = _nj1;
03077 vinfos[2].jointtype = 1;
03078 vinfos[2].foffset = j2;
03079 vinfos[2].indices[0] = _ij2[0];
03080 vinfos[2].indices[1] = _ij2[1];
03081 vinfos[2].maxsolutions = _nj2;
03082 vinfos[3].jointtype = 1;
03083 vinfos[3].foffset = j3;
03084 vinfos[3].indices[0] = _ij3[0];
03085 vinfos[3].indices[1] = _ij3[1];
03086 vinfos[3].maxsolutions = _nj3;
03087 vinfos[4].jointtype = 1;
03088 vinfos[4].foffset = j4;
03089 vinfos[4].indices[0] = _ij4[0];
03090 vinfos[4].indices[1] = _ij4[1];
03091 vinfos[4].maxsolutions = _nj4;
03092 std::vector<int> vfree(0);
03093 solutions.AddSolution(vinfos,vfree);
03094 }
03095 }
03096 }
03097 
03098 }
03099 
03100 }
03101 
03102 } else
03103 {
03104 {
03105 IkReal j2array[1], cj2array[1], sj2array[1];
03106 bool j2valid[1]={false};
03107 _nj2 = 1;
03108 IkReal x997=((IkReal(2600.00000000000))*(sj3));
03109 IkReal x998=((py)*(sj0));
03110 IkReal x999=((pz)*(sj1));
03111 IkReal x1000=((IkReal(2600.00000000000))*(cj3));
03112 IkReal x1001=((cj1)*(pz));
03113 IkReal x1002=((IkReal(22000.0000000000))*(cj1));
03114 IkReal x1003=((cj0)*(px));
03115 IkReal x1004=((sj1)*(x1003));
03116 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x1003)))+(((x1000)*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((sj1)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x998)))+(((IkReal(-910.000000000000))*(sj3)))+(((x1001)*(x997)))+(((x1004)*(x997)))+(((IkReal(22000.0000000000))*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x998))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1000)*(x998)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001)))+(((IkReal(-1.00000000000000))*(cj1)*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1000)*(x1004)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x997)*(x998)))+(((IkReal(-22000.0000000000))*(x1001)))+(((IkReal(-22000.0000000000))*(sj1)*(x998)))+(((IkReal(-22000.0000000000))*(x1004)))+(((x997)*(x999))))))) < IKFAST_ATAN2_MAGTHRESH )
03117     continue;
03118 j2array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x1003)))+(((x1000)*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((sj1)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x998)))+(((IkReal(-910.000000000000))*(sj3)))+(((x1001)*(x997)))+(((x1004)*(x997)))+(((IkReal(22000.0000000000))*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x998)))))), ((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1000)*(x998)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001)))+(((IkReal(-1.00000000000000))*(cj1)*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1000)*(x1004)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x997)*(x998)))+(((IkReal(-22000.0000000000))*(x1001)))+(((IkReal(-22000.0000000000))*(sj1)*(x998)))+(((IkReal(-22000.0000000000))*(x1004)))+(((x997)*(x999)))))));
03119 sj2array[0]=IKsin(j2array[0]);
03120 cj2array[0]=IKcos(j2array[0]);
03121 if( j2array[0] > IKPI )
03122 {
03123     j2array[0]-=IK2PI;
03124 }
03125 else if( j2array[0] < -IKPI )
03126 {    j2array[0]+=IK2PI;
03127 }
03128 j2valid[0] = true;
03129 for(int ij2 = 0; ij2 < 1; ++ij2)
03130 {
03131 if( !j2valid[ij2] )
03132 {
03133     continue;
03134 }
03135 _ij2[0] = ij2; _ij2[1] = -1;
03136 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03137 {
03138 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03139 {
03140     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03141 }
03142 }
03143 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03144 {
03145 IkReal evalcond[4];
03146 IkReal x1005=IKcos(j2);
03147 IkReal x1006=IKsin(j2);
03148 IkReal x1007=((px)*(sj1));
03149 IkReal x1008=((py)*(sj1));
03150 IkReal x1009=((cj1)*(sj0));
03151 IkReal x1010=((IkReal(1.00000000000000))*(px));
03152 IkReal x1011=((r00)*(sj0));
03153 IkReal x1012=((IkReal(0.0950000000000000))*(r01));
03154 IkReal x1013=((IkReal(0.0650000000000000))*(cj3));
03155 IkReal x1014=((cj0)*(r01));
03156 IkReal x1015=((IkReal(0.0950000000000000))*(cj1));
03157 IkReal x1016=((pz)*(sj1));
03158 IkReal x1017=((cj0)*(r00));
03159 IkReal x1018=((IkReal(0.0950000000000000))*(sj1));
03160 IkReal x1019=((cj0)*(cj1));
03161 IkReal x1020=((cj1)*(pz));
03162 IkReal x1021=((IkReal(0.0650000000000000))*(sj3));
03163 IkReal x1022=((IkReal(0.550000000000000))*(x1006));
03164 IkReal x1023=((IkReal(0.550000000000000))*(x1005));
03165 IkReal x1024=((x1006)*(x1013));
03166 IkReal x1025=((x1005)*(x1021));
03167 IkReal x1026=((x1006)*(x1021));
03168 IkReal x1027=((x1005)*(x1013));
03169 IkReal x1028=((x1027)+(x1023));
03170 IkReal x1029=((x1025)+(x1024)+(x1022));
03171 evalcond[0]=((IkReal(-0.350000000000000))+(((cj0)*(x1007)))+(((IkReal(-1.00000000000000))*(x1028)))+(((sj0)*(x1008)))+(x1026)+(x1020));
03172 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x1009)))+(((IkReal(-1.00000000000000))*(x1010)*(x1019)))+(x1016)+(x1029));
03173 evalcond[2]=((((x1014)*(x1016)))+(((x1009)*(x1012)))+(((r02)*(sj0)*(x1007)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1010)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1008)))+(((IkReal(-1.00000000000000))*(x1011)*(x1016)))+(((IkReal(-1.00000000000000))*(r02)*(x1018)))+(((IkReal(-1.00000000000000))*(x1029)))+(((x1015)*(x1017))));
03174 evalcond[3]=((((r02)*(x1015)))+(((IkReal(-1.00000000000000))*(x1014)*(x1020)))+(((IkReal(-0.350000000000000))*(x1011)))+(((IkReal(-1.00000000000000))*(x1028)))+(((IkReal(0.350000000000000))*(x1014)))+(((IkReal(-1.00000000000000))*(r01)*(x1007)))+(((x1017)*(x1018)))+(((py)*(r02)*(x1019)))+(((pz)*(r00)*(x1009)))+(x1026)+(((sj0)*(sj1)*(x1012)))+(((r00)*(x1008)))+(((IkReal(-1.00000000000000))*(r02)*(x1009)*(x1010))));
03175 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03176 {
03177 continue;
03178 }
03179 }
03180 
03181 {
03182 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03183 vinfos[0].jointtype = 1;
03184 vinfos[0].foffset = j0;
03185 vinfos[0].indices[0] = _ij0[0];
03186 vinfos[0].indices[1] = _ij0[1];
03187 vinfos[0].maxsolutions = _nj0;
03188 vinfos[1].jointtype = 1;
03189 vinfos[1].foffset = j1;
03190 vinfos[1].indices[0] = _ij1[0];
03191 vinfos[1].indices[1] = _ij1[1];
03192 vinfos[1].maxsolutions = _nj1;
03193 vinfos[2].jointtype = 1;
03194 vinfos[2].foffset = j2;
03195 vinfos[2].indices[0] = _ij2[0];
03196 vinfos[2].indices[1] = _ij2[1];
03197 vinfos[2].maxsolutions = _nj2;
03198 vinfos[3].jointtype = 1;
03199 vinfos[3].foffset = j3;
03200 vinfos[3].indices[0] = _ij3[0];
03201 vinfos[3].indices[1] = _ij3[1];
03202 vinfos[3].maxsolutions = _nj3;
03203 vinfos[4].jointtype = 1;
03204 vinfos[4].foffset = j4;
03205 vinfos[4].indices[0] = _ij4[0];
03206 vinfos[4].indices[1] = _ij4[1];
03207 vinfos[4].maxsolutions = _nj4;
03208 std::vector<int> vfree(0);
03209 solutions.AddSolution(vinfos,vfree);
03210 }
03211 }
03212 }
03213 
03214 }
03215 
03216 }
03217 
03218 } else
03219 {
03220 IkReal x1030=((r01)*(sj1));
03221 IkReal x1031=((IkReal(0.350000000000000))*(sj0));
03222 IkReal x1032=((px)*(r02));
03223 IkReal x1033=((IkReal(1.00000000000000))*(cj0));
03224 IkReal x1034=((cj0)*(r00));
03225 IkReal x1035=((IkReal(0.350000000000000))*(cj1));
03226 IkReal x1036=((IkReal(0.700000000000000))*(sj1));
03227 IkReal x1037=((IkReal(0.190000000000000))*(px));
03228 IkReal x1038=((r00)*(sj0));
03229 IkReal x1039=((px)*(sj0));
03230 IkReal x1040=((IkReal(0.700000000000000))*(cj1));
03231 IkReal x1041=((py)*(r02));
03232 IkReal x1042=((IkReal(0.190000000000000))*(py));
03233 IkReal x1043=((IkReal(2.00000000000000))*(py));
03234 IkReal x1044=((pz)*(r01));
03235 IkReal x1045=((IkReal(2.00000000000000))*(cj0));
03236 IkReal x1046=((IkReal(0.350000000000000))*(sj1));
03237 IkReal x1047=((pz)*(r02));
03238 IkReal x1048=((cj0)*(r01));
03239 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.45233765858574))+(j3)), IkReal(6.28318530717959))));
03240 evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x1033)))+(x1039));
03241 evalcond[2]=((sj4)+(x1038)+(((IkReal(-1.00000000000000))*(r01)*(x1033))));
03242 evalcond[3]=((IkReal(0.166749999870000))+(((pz)*(x1040)))+(((py)*(sj0)*(x1036)))+(((cj0)*(px)*(x1036)))+(((IkReal(-1.00000000000000))*(pp)))+(((sj0)*(x1037)))+(((IkReal(-1.00000000000000))*(cj0)*(x1042))));
03243 evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-0.546145583500000))*(cj4)))+(((x1034)*(x1046)))+(((IkReal(-0.0950000000000000))*(x1048)))+(((x1030)*(x1031)))+(((r02)*(x1035)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.0950000000000000))*(x1038)))+(((IkReal(-1.00000000000000))*(x1047))));
03244 evalcond[5]=((((r02)*(x1046)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(x1034)*(x1035)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1031)))+(((sj0)*(x1044)))+(((IkReal(-1.00000000000000))*(x1032)*(x1033)))+(((pz)*(x1034)))+(((IkReal(-1.00000000000000))*(sj0)*(x1041))));
03245 evalcond[6]=((((py)*(x1043)*(x1048)))+(((IkReal(-1.00000000000000))*(r01)*(x1039)*(x1043)))+(((IkReal(0.190000000000000))*(x1047)))+(((IkReal(-1.00000000000000))*(cj0)*(x1040)*(x1041)))+(((IkReal(0.700000000000000))*(px)*(x1030)))+(((pz)*(x1041)*(x1045)))+(((IkReal(-0.113475000000000))*(x1048)))+(((sj0)*(x1032)*(x1040)))+(((IkReal(-1.00000000000000))*(pz)*(x1038)*(x1040)))+(((IkReal(-2.00000000000000))*(x1038)*((px)*(px))))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x1032)))+(((pp)*(x1038)))+(((IkReal(-0.0665000000000000))*(sj1)*(x1034)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1036)))+(((r00)*(x1037)))+(((IkReal(0.113475000000000))*(x1038)))+(((px)*(x1034)*(x1043)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x1033)))+(((IkReal(-0.0665000000000000))*(sj0)*(x1030)))+(((r01)*(x1042)))+(((cj0)*(x1040)*(x1044))));
03246 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  )
03247 {
03248 {
03249 IkReal j2array[1], cj2array[1], sj2array[1];
03250 bool j2valid[1]={false};
03251 _nj2 = 1;
03252 IkReal x1049=((IkReal(1.81818181870518))*(sj1));
03253 IkReal x1050=((py)*(sj0));
03254 IkReal x1051=((IkReal(0.216392517249668))*(sj1));
03255 IkReal x1052=((cj0)*(px));
03256 IkReal x1053=((IkReal(0.216392517249668))*(cj1));
03257 IkReal x1054=((IkReal(1.81818181870518))*(cj1));
03258 if( IKabs(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051)))))+IKsqr(((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051)))))-1) <= IKFAST_SINCOS_THRESH )
03259     continue;
03260 j2array[0]=IKatan2(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051)))), ((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051)))));
03261 sj2array[0]=IKsin(j2array[0]);
03262 cj2array[0]=IKcos(j2array[0]);
03263 if( j2array[0] > IKPI )
03264 {
03265     j2array[0]-=IK2PI;
03266 }
03267 else if( j2array[0] < -IKPI )
03268 {    j2array[0]+=IK2PI;
03269 }
03270 j2valid[0] = true;
03271 for(int ij2 = 0; ij2 < 1; ++ij2)
03272 {
03273 if( !j2valid[ij2] )
03274 {
03275     continue;
03276 }
03277 _ij2[0] = ij2; _ij2[1] = -1;
03278 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03279 {
03280 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03281 {
03282     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03283 }
03284 }
03285 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03286 {
03287 IkReal evalcond[8];
03288 IkReal x1055=IKsin(j2);
03289 IkReal x1056=IKcos(j2);
03290 IkReal x1057=(py)*(py);
03291 IkReal x1058=(px)*(px);
03292 IkReal x1059=(pz)*(pz);
03293 IkReal x1060=((sj0)*(sj1));
03294 IkReal x1061=((py)*(r00));
03295 IkReal x1062=((IkReal(0.190000000000000))*(cj1));
03296 IkReal x1063=((cj0)*(r02));
03297 IkReal x1064=((py)*(r01));
03298 IkReal x1065=((px)*(r01));
03299 IkReal x1066=((IkReal(0.190000000000000))*(sj1));
03300 IkReal x1067=((cj1)*(r02));
03301 IkReal x1068=((pz)*(r00));
03302 IkReal x1069=((cj1)*(sj0));
03303 IkReal x1070=((cj0)*(r00));
03304 IkReal x1071=((IkReal(2.00000000000000))*(sj1));
03305 IkReal x1072=((IkReal(0.700000000000000))*(px));
03306 IkReal x1073=((IkReal(1.00000000000000))*(cj1));
03307 IkReal x1074=((IkReal(2.00000000000000))*(px));
03308 IkReal x1075=((pp)*(r01));
03309 IkReal x1076=((pp)*(sj1));
03310 IkReal x1077=((r02)*(sj1));
03311 IkReal x1078=((pz)*(r01));
03312 IkReal x1079=((px)*(r02));
03313 IkReal x1080=((IkReal(0.700000000000000))*(sj0));
03314 IkReal x1081=((cj0)*(sj1));
03315 IkReal x1082=((r00)*(sj0));
03316 IkReal x1083=((cj0)*(r01));
03317 IkReal x1084=((IkReal(1.00000000000000))*(py));
03318 IkReal x1085=((pz)*(r02));
03319 IkReal x1086=((IkReal(2.00000000000000))*(py));
03320 IkReal x1087=((cj1)*(pz));
03321 IkReal x1088=((IkReal(0.0950000000000000))*(r01));
03322 IkReal x1089=((cj0)*(px));
03323 IkReal x1090=((pz)*(x1071));
03324 IkReal x1091=((cj4)*(x1055));
03325 IkReal x1092=((cj4)*(x1056));
03326 IkReal x1093=((sj4)*(x1055));
03327 IkReal x1094=((IkReal(0.0645444780500000))*(x1056));
03328 IkReal x1095=((IkReal(0.542318181700000))*(x1056));
03329 IkReal x1096=((IkReal(2.00000000000000))*(r01)*(x1057));
03330 evalcond[0]=((((sj1)*(x1070)))+(((IkReal(0.992991970000000))*(x1092)))+(x1067)+(((r01)*(x1060)))+(((IkReal(-0.118181820000000))*(x1091))));
03331 evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1060)))+(x1087)+(((px)*(x1081)))+(((IkReal(-1.00000000000000))*(x1095)))+(((IkReal(0.0645444780500000))*(x1055))));
03332 evalcond[2]=((((IkReal(-1.00000000000000))*(x1070)*(x1073)))+(((IkReal(-0.992991970000000))*(x1091)))+(((IkReal(-0.118181820000000))*(x1092)))+(((IkReal(-1.00000000000000))*(r01)*(x1069)))+(x1077));
03333 evalcond[3]=((((IkReal(0.542318181700000))*(x1055)))+(((IkReal(-1.00000000000000))*(x1069)*(x1084)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x1073)*(x1089)))+(x1094));
03334 evalcond[4]=((((IkReal(-1.00000000000000))*(x1060)*(x1068)))+(((x1078)*(x1081)))+(((IkReal(0.0950000000000000))*(cj1)*(x1070)))+(((x1060)*(x1079)))+(((IkReal(-1.00000000000000))*(x1065)*(x1073)))+(((IkReal(-0.0950000000000000))*(x1077)))+(((sj4)*(x1094)))+(((cj1)*(x1061)))+(((IkReal(0.542318181700000))*(x1093)))+(((x1069)*(x1088)))+(((IkReal(-1.00000000000000))*(sj1)*(x1063)*(x1084))));
03335 evalcond[5]=((((sj1)*(x1061)))+(((x1068)*(x1069)))+(((IkReal(-0.0645444780500000))*(x1093)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x1067)))+(((IkReal(0.0950000000000000))*(x1067)))+(((IkReal(0.350000000000000))*(x1083)))+(((x1060)*(x1088)))+(((IkReal(-1.00000000000000))*(cj0)*(x1073)*(x1078)))+(((IkReal(-0.350000000000000))*(x1082)))+(((cj1)*(py)*(x1063)))+(((IkReal(-1.00000000000000))*(sj1)*(x1065)))+(((IkReal(0.0950000000000000))*(sj1)*(x1070)))+(((sj4)*(x1095))));
03336 evalcond[6]=((((IkReal(-0.0665000000000000))*(x1082)))+(((IkReal(-1.00000000000000))*(x1058)*(x1070)*(x1071)))+(((py)*(x1062)*(x1063)))+(((IkReal(-0.113475000000000))*(sj1)*(x1070)))+(((IkReal(-1.00000000000000))*(px)*(x1063)*(x1090)))+(((IkReal(-0.296184679851750))*(x1092)))+(((IkReal(0.0665000000000000))*(x1083)))+(((IkReal(0.700000000000000))*(x1064)))+(((IkReal(-1.00000000000000))*(sj0)*(x1062)*(x1079)))+(((pp)*(x1067)))+(((sj0)*(x1062)*(x1068)))+(((x1061)*(x1066)))+(((IkReal(0.700000000000000))*(x1085)))+(((IkReal(-1.00000000000000))*(x1064)*(x1071)*(x1089)))+(((IkReal(-1.00000000000000))*(cj1)*(x1068)*(x1074)))+(((IkReal(-1.00000000000000))*(x1060)*(x1061)*(x1074)))+(((IkReal(-1.00000000000000))*(x1060)*(x1085)*(x1086)))+(((IkReal(-1.00000000000000))*(x1060)*(x1096)))+(((x1060)*(x1075)))+(((IkReal(0.0352506812605000))*(x1091)))+(((IkReal(-0.113475000000000))*(x1067)))+(((r00)*(x1072)))+(((IkReal(-2.00000000000000))*(x1064)*(x1087)))+(((IkReal(-2.00000000000000))*(x1059)*(x1067)))+(((IkReal(-1.00000000000000))*(x1065)*(x1066)))+(((IkReal(-0.113475000000000))*(r01)*(x1060)))+(((IkReal(-1.00000000000000))*(cj0)*(x1062)*(x1078)))+(((x1070)*(x1076))));
03337 evalcond[7]=((((IkReal(-1.00000000000000))*(py)*(r02)*(x1080)))+(((IkReal(0.296184679851750))*(x1091)))+(((IkReal(0.700000000000000))*(cj0)*(x1068)))+(((IkReal(-1.00000000000000))*(x1064)*(x1090)))+(((IkReal(-0.131525000000000))*(cj1)*(x1070)))+(((py)*(x1063)*(x1066)))+(((IkReal(-1.00000000000000))*(px)*(x1068)*(x1071)))+(((cj0)*(cj1)*(x1064)*(x1074)))+(((x1061)*(x1069)*(x1074)))+(((x1078)*(x1080)))+(((IkReal(0.131525000000000))*(x1077)))+(((IkReal(-1.00000000000000))*(r02)*(x1059)*(x1071)))+(((x1069)*(x1096)))+(((IkReal(-1.00000000000000))*(pp)*(x1070)*(x1073)))+(((IkReal(-1.00000000000000))*(x1063)*(x1072)))+(((IkReal(-1.00000000000000))*(cj0)*(x1066)*(x1078)))+(((IkReal(-1.00000000000000))*(x1069)*(x1075)))+(((x1062)*(x1065)))+(((IkReal(0.190000000000000))*(x1060)*(x1068)))+(((IkReal(-1.00000000000000))*(x1061)*(x1062)))+(((x1063)*(x1074)*(x1087)))+(((IkReal(2.00000000000000))*(cj1)*(x1058)*(x1070)))+(((IkReal(-0.131525000000000))*(r01)*(x1069)))+(((r02)*(x1076)))+(((pz)*(sj0)*(x1067)*(x1086)))+(((IkReal(0.0352506812605000))*(x1092)))+(((IkReal(-0.190000000000000))*(x1060)*(x1079))));
03338 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  )
03339 {
03340 continue;
03341 }
03342 }
03343 
03344 {
03345 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03346 vinfos[0].jointtype = 1;
03347 vinfos[0].foffset = j0;
03348 vinfos[0].indices[0] = _ij0[0];
03349 vinfos[0].indices[1] = _ij0[1];
03350 vinfos[0].maxsolutions = _nj0;
03351 vinfos[1].jointtype = 1;
03352 vinfos[1].foffset = j1;
03353 vinfos[1].indices[0] = _ij1[0];
03354 vinfos[1].indices[1] = _ij1[1];
03355 vinfos[1].maxsolutions = _nj1;
03356 vinfos[2].jointtype = 1;
03357 vinfos[2].foffset = j2;
03358 vinfos[2].indices[0] = _ij2[0];
03359 vinfos[2].indices[1] = _ij2[1];
03360 vinfos[2].maxsolutions = _nj2;
03361 vinfos[3].jointtype = 1;
03362 vinfos[3].foffset = j3;
03363 vinfos[3].indices[0] = _ij3[0];
03364 vinfos[3].indices[1] = _ij3[1];
03365 vinfos[3].maxsolutions = _nj3;
03366 vinfos[4].jointtype = 1;
03367 vinfos[4].foffset = j4;
03368 vinfos[4].indices[0] = _ij4[0];
03369 vinfos[4].indices[1] = _ij4[1];
03370 vinfos[4].maxsolutions = _nj4;
03371 std::vector<int> vfree(0);
03372 solutions.AddSolution(vinfos,vfree);
03373 }
03374 }
03375 }
03376 
03377 } else
03378 {
03379 IkReal x1097=((r01)*(sj1));
03380 IkReal x1098=((IkReal(0.350000000000000))*(sj0));
03381 IkReal x1099=((px)*(r02));
03382 IkReal x1100=((IkReal(1.00000000000000))*(cj0));
03383 IkReal x1101=((cj0)*(r00));
03384 IkReal x1102=((IkReal(0.350000000000000))*(cj1));
03385 IkReal x1103=((IkReal(0.700000000000000))*(sj1));
03386 IkReal x1104=((IkReal(0.190000000000000))*(px));
03387 IkReal x1105=((r00)*(sj0));
03388 IkReal x1106=((px)*(sj0));
03389 IkReal x1107=((IkReal(0.700000000000000))*(cj1));
03390 IkReal x1108=((py)*(r02));
03391 IkReal x1109=((IkReal(0.190000000000000))*(py));
03392 IkReal x1110=((IkReal(2.00000000000000))*(py));
03393 IkReal x1111=((pz)*(r01));
03394 IkReal x1112=((IkReal(2.00000000000000))*(cj0));
03395 IkReal x1113=((IkReal(0.350000000000000))*(sj1));
03396 IkReal x1114=((pz)*(r02));
03397 IkReal x1115=((cj0)*(r01));
03398 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.83084764859385))+(j3)), IkReal(6.28318530717959))));
03399 evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x1100)))+(x1106));
03400 evalcond[2]=((x1105)+(sj4)+(((IkReal(-1.00000000000000))*(r01)*(x1100))));
03401 evalcond[3]=((IkReal(0.166749999870000))+(((py)*(sj0)*(x1103)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj0)*(x1109)))+(((sj0)*(x1104)))+(((pz)*(x1107)))+(((cj0)*(px)*(x1103))));
03402 evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((r02)*(x1102)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((x1101)*(x1113)))+(((x1097)*(x1098)))+(((IkReal(-1.00000000000000))*(x1114)))+(((IkReal(0.546145583500000))*(cj4)))+(((IkReal(-0.0950000000000000))*(x1115)))+(((IkReal(0.0950000000000000))*(x1105))));
03403 evalcond[5]=((((r02)*(x1113)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(sj0)*(x1108)))+(((pz)*(x1101)))+(((IkReal(-1.00000000000000))*(x1101)*(x1102)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1098)))+(((IkReal(-1.00000000000000))*(x1099)*(x1100)))+(((sj0)*(x1111))));
03404 evalcond[6]=((((IkReal(0.190000000000000))*(x1114)))+(((IkReal(-0.0665000000000000))*(sj0)*(x1097)))+(((pz)*(x1108)*(x1112)))+(((IkReal(-1.00000000000000))*(cj0)*(x1107)*(x1108)))+(((IkReal(-1.00000000000000))*(r01)*(x1106)*(x1110)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1103)))+(((IkReal(-0.113475000000000))*(x1115)))+(((IkReal(-2.00000000000000))*(x1105)*((px)*(px))))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x1100)))+(((pp)*(x1105)))+(((IkReal(0.113475000000000))*(x1105)))+(((py)*(x1110)*(x1115)))+(((px)*(x1101)*(x1110)))+(((IkReal(-1.00000000000000))*(pz)*(x1105)*(x1107)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x1099)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((cj0)*(x1107)*(x1111)))+(((IkReal(0.700000000000000))*(px)*(x1097)))+(((r01)*(x1109)))+(((r00)*(x1104)))+(((sj0)*(x1099)*(x1107)))+(((IkReal(-0.0665000000000000))*(sj1)*(x1101))));
03405 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  )
03406 {
03407 {
03408 IkReal j2array[1], cj2array[1], sj2array[1];
03409 bool j2valid[1]={false};
03410 _nj2 = 1;
03411 IkReal x1116=((IkReal(1.81818181870518))*(sj1));
03412 IkReal x1117=((py)*(sj0));
03413 IkReal x1118=((IkReal(0.216392517249668))*(sj1));
03414 IkReal x1119=((cj0)*(px));
03415 IkReal x1120=((IkReal(0.216392517249668))*(cj1));
03416 IkReal x1121=((IkReal(1.81818181870518))*(cj1));
03417 if( IKabs(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121)))))+IKsqr(((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119)))))-1) <= IKFAST_SINCOS_THRESH )
03418     continue;
03419 j2array[0]=IKatan2(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121)))), ((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119)))));
03420 sj2array[0]=IKsin(j2array[0]);
03421 cj2array[0]=IKcos(j2array[0]);
03422 if( j2array[0] > IKPI )
03423 {
03424     j2array[0]-=IK2PI;
03425 }
03426 else if( j2array[0] < -IKPI )
03427 {    j2array[0]+=IK2PI;
03428 }
03429 j2valid[0] = true;
03430 for(int ij2 = 0; ij2 < 1; ++ij2)
03431 {
03432 if( !j2valid[ij2] )
03433 {
03434     continue;
03435 }
03436 _ij2[0] = ij2; _ij2[1] = -1;
03437 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03438 {
03439 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03440 {
03441     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03442 }
03443 }
03444 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03445 {
03446 IkReal evalcond[8];
03447 IkReal x1122=IKsin(j2);
03448 IkReal x1123=IKcos(j2);
03449 IkReal x1124=(py)*(py);
03450 IkReal x1125=(px)*(px);
03451 IkReal x1126=(pz)*(pz);
03452 IkReal x1127=((sj0)*(sj1));
03453 IkReal x1128=((py)*(r00));
03454 IkReal x1129=((IkReal(0.190000000000000))*(cj1));
03455 IkReal x1130=((cj0)*(r02));
03456 IkReal x1131=((py)*(r01));
03457 IkReal x1132=((px)*(r01));
03458 IkReal x1133=((IkReal(0.190000000000000))*(sj1));
03459 IkReal x1134=((cj1)*(r02));
03460 IkReal x1135=((pz)*(r00));
03461 IkReal x1136=((cj1)*(sj0));
03462 IkReal x1137=((cj0)*(r00));
03463 IkReal x1138=((IkReal(2.00000000000000))*(sj1));
03464 IkReal x1139=((IkReal(0.700000000000000))*(px));
03465 IkReal x1140=((IkReal(1.00000000000000))*(cj1));
03466 IkReal x1141=((IkReal(2.00000000000000))*(px));
03467 IkReal x1142=((pp)*(r01));
03468 IkReal x1143=((pp)*(sj1));
03469 IkReal x1144=((r02)*(sj1));
03470 IkReal x1145=((pz)*(r01));
03471 IkReal x1146=((px)*(r02));
03472 IkReal x1147=((IkReal(0.700000000000000))*(sj0));
03473 IkReal x1148=((cj0)*(sj1));
03474 IkReal x1149=((r00)*(sj0));
03475 IkReal x1150=((cj0)*(r01));
03476 IkReal x1151=((IkReal(1.00000000000000))*(py));
03477 IkReal x1152=((pz)*(r02));
03478 IkReal x1153=((IkReal(2.00000000000000))*(py));
03479 IkReal x1154=((cj1)*(pz));
03480 IkReal x1155=((IkReal(0.0950000000000000))*(r01));
03481 IkReal x1156=((cj0)*(px));
03482 IkReal x1157=((pz)*(x1138));
03483 IkReal x1158=((cj4)*(x1122));
03484 IkReal x1159=((cj4)*(x1123));
03485 IkReal x1160=((sj4)*(x1122));
03486 IkReal x1161=((IkReal(0.0645444780500000))*(x1123));
03487 IkReal x1162=((IkReal(0.542318181700000))*(x1123));
03488 IkReal x1163=((IkReal(2.00000000000000))*(r01)*(x1124));
03489 evalcond[0]=((x1134)+(((IkReal(-0.992991970000000))*(x1159)))+(((r01)*(x1127)))+(((IkReal(-0.118181820000000))*(x1158)))+(((sj1)*(x1137))));
03490 evalcond[1]=((IkReal(-0.350000000000000))+(((px)*(x1148)))+(((IkReal(-0.0645444780500000))*(x1122)))+(((IkReal(-1.00000000000000))*(x1162)))+(((py)*(x1127)))+(x1154));
03491 evalcond[2]=((x1144)+(((IkReal(0.992991970000000))*(x1158)))+(((IkReal(-0.118181820000000))*(x1159)))+(((IkReal(-1.00000000000000))*(r01)*(x1136)))+(((IkReal(-1.00000000000000))*(x1137)*(x1140))));
03492 evalcond[3]=((((IkReal(-1.00000000000000))*(x1161)))+(((IkReal(0.542318181700000))*(x1122)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x1136)*(x1151)))+(((IkReal(-1.00000000000000))*(x1140)*(x1156))));
03493 evalcond[4]=((((x1145)*(x1148)))+(((IkReal(-1.00000000000000))*(x1127)*(x1135)))+(((IkReal(0.0950000000000000))*(cj1)*(x1137)))+(((IkReal(-0.0950000000000000))*(x1144)))+(((x1136)*(x1155)))+(((IkReal(-1.00000000000000))*(sj4)*(x1161)))+(((IkReal(-1.00000000000000))*(x1132)*(x1140)))+(((x1127)*(x1146)))+(((cj1)*(x1128)))+(((IkReal(-1.00000000000000))*(sj1)*(x1130)*(x1151)))+(((IkReal(0.542318181700000))*(x1160))));
03494 evalcond[5]=((((x1135)*(x1136)))+(((sj4)*(x1162)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x1134)))+(((IkReal(-0.350000000000000))*(x1149)))+(((IkReal(0.350000000000000))*(x1150)))+(((x1127)*(x1155)))+(((IkReal(-1.00000000000000))*(sj1)*(x1132)))+(((IkReal(-1.00000000000000))*(cj0)*(x1140)*(x1145)))+(((IkReal(0.0950000000000000))*(sj1)*(x1137)))+(((sj1)*(x1128)))+(((cj1)*(py)*(x1130)))+(((IkReal(0.0950000000000000))*(x1134)))+(((IkReal(0.0645444780500000))*(x1160))));
03495 evalcond[6]=((((IkReal(-0.113475000000000))*(x1134)))+(((IkReal(-0.113475000000000))*(sj1)*(x1137)))+(((IkReal(-2.00000000000000))*(x1131)*(x1154)))+(((IkReal(0.0665000000000000))*(x1150)))+(((IkReal(-1.00000000000000))*(cj0)*(x1129)*(x1145)))+(((IkReal(-1.00000000000000))*(x1127)*(x1163)))+(((IkReal(-1.00000000000000))*(px)*(x1130)*(x1157)))+(((IkReal(-1.00000000000000))*(x1127)*(x1152)*(x1153)))+(((IkReal(-1.00000000000000))*(sj0)*(x1129)*(x1146)))+(((sj0)*(x1129)*(x1135)))+(((IkReal(-1.00000000000000))*(cj1)*(x1135)*(x1141)))+(((IkReal(-1.00000000000000))*(x1131)*(x1138)*(x1156)))+(((IkReal(-0.0665000000000000))*(x1149)))+(((x1127)*(x1142)))+(((IkReal(0.700000000000000))*(x1152)))+(((pp)*(x1134)))+(((x1137)*(x1143)))+(((r00)*(x1139)))+(((IkReal(-0.113475000000000))*(r01)*(x1127)))+(((x1128)*(x1133)))+(((py)*(x1129)*(x1130)))+(((IkReal(0.296184679851750))*(x1159)))+(((IkReal(-1.00000000000000))*(x1125)*(x1137)*(x1138)))+(((IkReal(-2.00000000000000))*(x1126)*(x1134)))+(((IkReal(0.0352506812605000))*(x1158)))+(((IkReal(-1.00000000000000))*(x1132)*(x1133)))+(((IkReal(0.700000000000000))*(x1131)))+(((IkReal(-1.00000000000000))*(x1127)*(x1128)*(x1141))));
03496 evalcond[7]=((((IkReal(-0.190000000000000))*(x1127)*(x1146)))+(((IkReal(0.190000000000000))*(x1127)*(x1135)))+(((py)*(x1130)*(x1133)))+(((IkReal(2.00000000000000))*(cj1)*(x1125)*(x1137)))+(((IkReal(-1.00000000000000))*(x1130)*(x1139)))+(((IkReal(-0.296184679851750))*(x1158)))+(((IkReal(-1.00000000000000))*(px)*(x1135)*(x1138)))+(((x1129)*(x1132)))+(((IkReal(-0.131525000000000))*(cj1)*(x1137)))+(((x1145)*(x1147)))+(((cj0)*(cj1)*(x1131)*(x1141)))+(((IkReal(-0.131525000000000))*(r01)*(x1136)))+(((x1136)*(x1163)))+(((x1128)*(x1136)*(x1141)))+(((IkReal(0.131525000000000))*(x1144)))+(((r02)*(x1143)))+(((IkReal(-1.00000000000000))*(x1131)*(x1157)))+(((IkReal(-1.00000000000000))*(cj0)*(x1133)*(x1145)))+(((x1130)*(x1141)*(x1154)))+(((IkReal(0.700000000000000))*(cj0)*(x1135)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x1147)))+(((IkReal(-1.00000000000000))*(pp)*(x1137)*(x1140)))+(((IkReal(0.0352506812605000))*(x1159)))+(((pz)*(sj0)*(x1134)*(x1153)))+(((IkReal(-1.00000000000000))*(x1136)*(x1142)))+(((IkReal(-1.00000000000000))*(x1128)*(x1129)))+(((IkReal(-1.00000000000000))*(r02)*(x1126)*(x1138))));
03497 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  )
03498 {
03499 continue;
03500 }
03501 }
03502 
03503 {
03504 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03505 vinfos[0].jointtype = 1;
03506 vinfos[0].foffset = j0;
03507 vinfos[0].indices[0] = _ij0[0];
03508 vinfos[0].indices[1] = _ij0[1];
03509 vinfos[0].maxsolutions = _nj0;
03510 vinfos[1].jointtype = 1;
03511 vinfos[1].foffset = j1;
03512 vinfos[1].indices[0] = _ij1[0];
03513 vinfos[1].indices[1] = _ij1[1];
03514 vinfos[1].maxsolutions = _nj1;
03515 vinfos[2].jointtype = 1;
03516 vinfos[2].foffset = j2;
03517 vinfos[2].indices[0] = _ij2[0];
03518 vinfos[2].indices[1] = _ij2[1];
03519 vinfos[2].maxsolutions = _nj2;
03520 vinfos[3].jointtype = 1;
03521 vinfos[3].foffset = j3;
03522 vinfos[3].indices[0] = _ij3[0];
03523 vinfos[3].indices[1] = _ij3[1];
03524 vinfos[3].maxsolutions = _nj3;
03525 vinfos[4].jointtype = 1;
03526 vinfos[4].foffset = j4;
03527 vinfos[4].indices[0] = _ij4[0];
03528 vinfos[4].indices[1] = _ij4[1];
03529 vinfos[4].maxsolutions = _nj4;
03530 std::vector<int> vfree(0);
03531 solutions.AddSolution(vinfos,vfree);
03532 }
03533 }
03534 }
03535 
03536 } else
03537 {
03538 if( 1 )
03539 {
03540 continue;
03541 
03542 } else
03543 {
03544 }
03545 }
03546 }
03547 }
03548 }
03549 }
03550 
03551 } else
03552 {
03553 {
03554 IkReal j2array[1], cj2array[1], sj2array[1];
03555 bool j2valid[1]={false};
03556 _nj2 = 1;
03557 IkReal x1164=((cj0)*(sj1));
03558 IkReal x1165=((IkReal(13.0000000000000))*(r00));
03559 IkReal x1166=((cj1)*(pz));
03560 IkReal x1167=((sj0)*(sj1));
03561 IkReal x1168=((IkReal(13.0000000000000))*(sj3));
03562 IkReal x1169=((cj1)*(r02));
03563 IkReal x1170=((IkReal(70.0000000000000))*(cj4));
03564 IkReal x1171=((IkReal(13.0000000000000))*(cj3));
03565 IkReal x1172=((IkReal(200.000000000000))*(cj4)*(sj3));
03566 IkReal x1173=((IkReal(200.000000000000))*(cj3)*(cj4));
03567 if( IKabs(((gconst2)*(((((cj3)*(x1164)*(x1165)))+(((py)*(x1167)*(x1172)))+(((IkReal(110.000000000000))*(r01)*(x1167)))+(((px)*(x1164)*(x1172)))+(((IkReal(-1.00000000000000))*(sj3)*(x1170)))+(((IkReal(110.000000000000))*(x1169)))+(((x1169)*(x1171)))+(((r01)*(x1167)*(x1171)))+(((x1166)*(x1172)))+(((IkReal(110.000000000000))*(r00)*(x1164))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj3)*(x1170)))+(((r01)*(x1167)*(x1168)))+(((x1168)*(x1169)))+(((IkReal(-1.00000000000000))*(px)*(x1164)*(x1173)))+(((IkReal(-1.00000000000000))*(x1166)*(x1173)))+(((IkReal(-1.00000000000000))*(py)*(x1167)*(x1173)))+(((sj3)*(x1164)*(x1165))))))) < IKFAST_ATAN2_MAGTHRESH )
03568     continue;
03569 j2array[0]=IKatan2(((gconst2)*(((((cj3)*(x1164)*(x1165)))+(((py)*(x1167)*(x1172)))+(((IkReal(110.000000000000))*(r01)*(x1167)))+(((px)*(x1164)*(x1172)))+(((IkReal(-1.00000000000000))*(sj3)*(x1170)))+(((IkReal(110.000000000000))*(x1169)))+(((x1169)*(x1171)))+(((r01)*(x1167)*(x1171)))+(((x1166)*(x1172)))+(((IkReal(110.000000000000))*(r00)*(x1164)))))), ((gconst2)*(((((cj3)*(x1170)))+(((r01)*(x1167)*(x1168)))+(((x1168)*(x1169)))+(((IkReal(-1.00000000000000))*(px)*(x1164)*(x1173)))+(((IkReal(-1.00000000000000))*(x1166)*(x1173)))+(((IkReal(-1.00000000000000))*(py)*(x1167)*(x1173)))+(((sj3)*(x1164)*(x1165)))))));
03570 sj2array[0]=IKsin(j2array[0]);
03571 cj2array[0]=IKcos(j2array[0]);
03572 if( j2array[0] > IKPI )
03573 {
03574     j2array[0]-=IK2PI;
03575 }
03576 else if( j2array[0] < -IKPI )
03577 {    j2array[0]+=IK2PI;
03578 }
03579 j2valid[0] = true;
03580 for(int ij2 = 0; ij2 < 1; ++ij2)
03581 {
03582 if( !j2valid[ij2] )
03583 {
03584     continue;
03585 }
03586 _ij2[0] = ij2; _ij2[1] = -1;
03587 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03588 {
03589 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03590 {
03591     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03592 }
03593 }
03594 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03595 {
03596 IkReal evalcond[8];
03597 IkReal x1174=IKsin(j2);
03598 IkReal x1175=IKcos(j2);
03599 IkReal x1176=(py)*(py);
03600 IkReal x1177=(px)*(px);
03601 IkReal x1178=(pz)*(pz);
03602 IkReal x1179=((sj0)*(sj1));
03603 IkReal x1180=((py)*(r00));
03604 IkReal x1181=((IkReal(0.190000000000000))*(cj1));
03605 IkReal x1182=((cj0)*(r02));
03606 IkReal x1183=((py)*(r01));
03607 IkReal x1184=((r01)*(sj1));
03608 IkReal x1185=((IkReal(0.190000000000000))*(px));
03609 IkReal x1186=((cj1)*(r02));
03610 IkReal x1187=((pz)*(r00));
03611 IkReal x1188=((cj1)*(sj0));
03612 IkReal x1189=((cj0)*(r00));
03613 IkReal x1190=((IkReal(2.00000000000000))*(sj1));
03614 IkReal x1191=((cj0)*(pz));
03615 IkReal x1192=((IkReal(1.00000000000000))*(r01));
03616 IkReal x1193=((cj1)*(px));
03617 IkReal x1194=((IkReal(2.00000000000000))*(px));
03618 IkReal x1195=((pp)*(sj1));
03619 IkReal x1196=((r02)*(sj1));
03620 IkReal x1197=((px)*(r02));
03621 IkReal x1198=((IkReal(0.700000000000000))*(pz));
03622 IkReal x1199=((cj0)*(px));
03623 IkReal x1200=((r00)*(sj0));
03624 IkReal x1201=((IkReal(1.00000000000000))*(px));
03625 IkReal x1202=((cj0)*(r01));
03626 IkReal x1203=((IkReal(1.00000000000000))*(cj1));
03627 IkReal x1204=((IkReal(1.00000000000000))*(py));
03628 IkReal x1205=((IkReal(0.700000000000000))*(px));
03629 IkReal x1206=((IkReal(0.190000000000000))*(sj1));
03630 IkReal x1207=((IkReal(0.298275000000000))*(sj3));
03631 IkReal x1208=((cj1)*(pz));
03632 IkReal x1209=((IkReal(0.0950000000000000))*(r01));
03633 IkReal x1210=((IkReal(0.306725000000000))*(cj3)*(cj4));
03634 IkReal x1211=((pz)*(x1190));
03635 IkReal x1212=((IkReal(0.550000000000000))*(x1175));
03636 IkReal x1213=((cj4)*(x1175));
03637 IkReal x1214=((IkReal(0.550000000000000))*(x1174));
03638 IkReal x1215=((IkReal(2.00000000000000))*(py)*(pz));
03639 IkReal x1216=((IkReal(0.0650000000000000))*(cj3)*(sj4));
03640 IkReal x1217=((IkReal(0.0650000000000000))*(x1174));
03641 IkReal x1218=((cj4)*(x1174));
03642 IkReal x1219=((IkReal(2.00000000000000))*(r01)*(x1176));
03643 IkReal x1220=((IkReal(0.0650000000000000))*(sj3)*(x1175));
03644 evalcond[0]=((((sj3)*(x1213)))+(((r01)*(x1179)))+(x1186)+(((cj3)*(x1218)))+(((sj1)*(x1189))));
03645 evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1179)))+(((IkReal(-0.0650000000000000))*(cj3)*(x1175)))+(((sj1)*(x1199)))+(((sj3)*(x1217)))+(((IkReal(-1.00000000000000))*(x1212)))+(x1208));
03646 evalcond[2]=((x1196)+(((IkReal(-1.00000000000000))*(x1189)*(x1203)))+(((IkReal(-1.00000000000000))*(sj3)*(x1218)))+(((IkReal(-1.00000000000000))*(x1188)*(x1192)))+(((cj3)*(x1213))));
03647 evalcond[3]=((((pz)*(sj1)))+(x1220)+(((IkReal(-1.00000000000000))*(x1188)*(x1204)))+(((cj3)*(x1217)))+(x1214)+(((IkReal(-1.00000000000000))*(cj0)*(x1193))));
03648 evalcond[4]=((((IkReal(-0.0950000000000000))*(x1196)))+(((sj4)*(x1214)))+(((cj1)*(x1180)))+(((x1174)*(x1216)))+(((x1188)*(x1209)))+(((x1184)*(x1191)))+(((sj4)*(x1220)))+(((IkReal(-1.00000000000000))*(x1179)*(x1187)))+(((IkReal(-1.00000000000000))*(x1192)*(x1193)))+(((x1179)*(x1197)))+(((IkReal(0.0950000000000000))*(cj1)*(x1189)))+(((IkReal(-1.00000000000000))*(sj1)*(x1182)*(x1204))));
03649 evalcond[5]=((((IkReal(0.0950000000000000))*(x1186)))+(((IkReal(-1.00000000000000))*(x1184)*(x1201)))+(((IkReal(0.350000000000000))*(x1202)))+(((sj1)*(x1180)))+(((x1179)*(x1209)))+(((IkReal(-0.350000000000000))*(x1200)))+(((sj4)*(x1212)))+(((cj1)*(py)*(x1182)))+(((IkReal(-1.00000000000000))*(cj1)*(x1191)*(x1192)))+(((x1175)*(x1216)))+(((x1187)*(x1188)))+(((IkReal(0.0950000000000000))*(sj1)*(x1189)))+(((IkReal(-1.00000000000000))*(sj0)*(x1186)*(x1201)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x1217))));
03650 evalcond[6]=((((IkReal(-0.113475000000000))*(sj1)*(x1189)))+(((IkReal(-1.00000000000000))*(x1179)*(x1180)*(x1194)))+(((IkReal(-2.00000000000000))*(x1183)*(x1208)))+(((pp)*(r01)*(x1179)))+(((r00)*(x1205)))+(((IkReal(-2.00000000000000))*(x1187)*(x1193)))+(((sj0)*(x1181)*(x1187)))+(((pp)*(x1186)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185)))+(((x1174)*(x1210)))+(((IkReal(-0.113475000000000))*(x1186)))+(((IkReal(-1.00000000000000))*(x1177)*(x1189)*(x1190)))+(((IkReal(-1.00000000000000))*(x1179)*(x1219)))+(((IkReal(-1.00000000000000))*(x1207)*(x1213)))+(((IkReal(-1.00000000000000))*(r01)*(x1181)*(x1191)))+(((x1189)*(x1195)))+(((IkReal(-0.0665000000000000))*(x1200)))+(((r02)*(x1198)))+(((IkReal(-1.00000000000000))*(sj0)*(x1181)*(x1197)))+(((IkReal(0.700000000000000))*(x1183)))+(((IkReal(-1.00000000000000))*(x1183)*(x1190)*(x1199)))+(((IkReal(-1.00000000000000))*(px)*(x1182)*(x1211)))+(((IkReal(0.0715000000000000))*(x1218)))+(((IkReal(0.0665000000000000))*(x1202)))+(((x1180)*(x1206)))+(((py)*(x1181)*(x1182)))+(((IkReal(-2.00000000000000))*(x1178)*(x1186)))+(((IkReal(-0.113475000000000))*(r01)*(x1179)))+(((IkReal(-1.00000000000000))*(r02)*(x1179)*(x1215))));
03651 evalcond[7]=((((IkReal(-1.00000000000000))*(r02)*(x1178)*(x1190)))+(((IkReal(-0.131525000000000))*(cj1)*(x1189)))+(((IkReal(0.0715000000000000))*(x1213)))+(((IkReal(-0.131525000000000))*(r01)*(x1188)))+(((IkReal(0.131525000000000))*(x1196)))+(((x1188)*(x1219)))+(((IkReal(-1.00000000000000))*(px)*(x1187)*(x1190)))+(((IkReal(-1.00000000000000))*(pp)*(x1188)*(x1192)))+(((IkReal(-1.00000000000000))*(r02)*(x1179)*(x1185)))+(((IkReal(-1.00000000000000))*(x1180)*(x1181)))+(((py)*(x1182)*(x1206)))+(((r02)*(x1195)))+(((IkReal(-1.00000000000000))*(pp)*(x1189)*(x1203)))+(((sj0)*(x1186)*(x1215)))+(((x1207)*(x1218)))+(((IkReal(-1.00000000000000))*(x1183)*(x1211)))+(((IkReal(-1.00000000000000))*(x1182)*(x1205)))+(((IkReal(2.00000000000000))*(cj1)*(x1177)*(x1189)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x1180)*(x1188)*(x1194)))+(((IkReal(2.00000000000000))*(pz)*(x1182)*(x1193)))+(((IkReal(0.700000000000000))*(cj0)*(x1187)))+(((IkReal(0.190000000000000))*(x1179)*(x1187)))+(((px)*(r01)*(x1181)))+(((IkReal(-0.190000000000000))*(x1184)*(x1191)))+(((x1175)*(x1210)))+(((r01)*(sj0)*(x1198)))+(((IkReal(2.00000000000000))*(cj0)*(x1183)*(x1193))));
03652 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  )
03653 {
03654 continue;
03655 }
03656 }
03657 
03658 {
03659 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03660 vinfos[0].jointtype = 1;
03661 vinfos[0].foffset = j0;
03662 vinfos[0].indices[0] = _ij0[0];
03663 vinfos[0].indices[1] = _ij0[1];
03664 vinfos[0].maxsolutions = _nj0;
03665 vinfos[1].jointtype = 1;
03666 vinfos[1].foffset = j1;
03667 vinfos[1].indices[0] = _ij1[0];
03668 vinfos[1].indices[1] = _ij1[1];
03669 vinfos[1].maxsolutions = _nj1;
03670 vinfos[2].jointtype = 1;
03671 vinfos[2].foffset = j2;
03672 vinfos[2].indices[0] = _ij2[0];
03673 vinfos[2].indices[1] = _ij2[1];
03674 vinfos[2].maxsolutions = _nj2;
03675 vinfos[3].jointtype = 1;
03676 vinfos[3].foffset = j3;
03677 vinfos[3].indices[0] = _ij3[0];
03678 vinfos[3].indices[1] = _ij3[1];
03679 vinfos[3].maxsolutions = _nj3;
03680 vinfos[4].jointtype = 1;
03681 vinfos[4].foffset = j4;
03682 vinfos[4].indices[0] = _ij4[0];
03683 vinfos[4].indices[1] = _ij4[1];
03684 vinfos[4].maxsolutions = _nj4;
03685 std::vector<int> vfree(0);
03686 solutions.AddSolution(vinfos,vfree);
03687 }
03688 }
03689 }
03690 
03691 }
03692 
03693 }
03694 
03695 } else
03696 {
03697 {
03698 IkReal j2array[1], cj2array[1], sj2array[1];
03699 bool j2valid[1]={false};
03700 _nj2 = 1;
03701 IkReal x1221=((IkReal(1.00000000000000))*(sj1));
03702 IkReal x1222=((IkReal(1.00000000000000))*(sj3));
03703 IkReal x1223=((cj0)*(r00));
03704 IkReal x1224=((r01)*(sj0));
03705 IkReal x1225=((cj3)*(r02));
03706 IkReal x1226=((cj3)*(x1224));
03707 IkReal x1227=((cj1)*(x1223));
03708 if( IKabs(((gconst1)*(((((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x1225)))+(((IkReal(-1.00000000000000))*(x1222)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(x1222)*(x1224)))+(((IkReal(-1.00000000000000))*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(cj3)*(x1221)*(x1223))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((cj3)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x1222)))+(((IkReal(-1.00000000000000))*(x1221)*(x1225)))+(((cj1)*(x1226)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1223)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1224))))))) < IKFAST_ATAN2_MAGTHRESH )
03709     continue;
03710 j2array[0]=IKatan2(((gconst1)*(((((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x1225)))+(((IkReal(-1.00000000000000))*(x1222)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(x1222)*(x1224)))+(((IkReal(-1.00000000000000))*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(cj3)*(x1221)*(x1223)))))), ((gconst1)*(((((cj3)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x1222)))+(((IkReal(-1.00000000000000))*(x1221)*(x1225)))+(((cj1)*(x1226)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1223)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1224)))))));
03711 sj2array[0]=IKsin(j2array[0]);
03712 cj2array[0]=IKcos(j2array[0]);
03713 if( j2array[0] > IKPI )
03714 {
03715     j2array[0]-=IK2PI;
03716 }
03717 else if( j2array[0] < -IKPI )
03718 {    j2array[0]+=IK2PI;
03719 }
03720 j2valid[0] = true;
03721 for(int ij2 = 0; ij2 < 1; ++ij2)
03722 {
03723 if( !j2valid[ij2] )
03724 {
03725     continue;
03726 }
03727 _ij2[0] = ij2; _ij2[1] = -1;
03728 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
03729 {
03730 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
03731 {
03732     j2valid[iij2]=false; _ij2[1] = iij2; break; 
03733 }
03734 }
03735 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
03736 {
03737 IkReal evalcond[8];
03738 IkReal x1228=IKsin(j2);
03739 IkReal x1229=IKcos(j2);
03740 IkReal x1230=(py)*(py);
03741 IkReal x1231=(px)*(px);
03742 IkReal x1232=(pz)*(pz);
03743 IkReal x1233=((sj0)*(sj1));
03744 IkReal x1234=((py)*(r00));
03745 IkReal x1235=((IkReal(0.190000000000000))*(cj1));
03746 IkReal x1236=((cj0)*(r02));
03747 IkReal x1237=((py)*(r01));
03748 IkReal x1238=((r01)*(sj1));
03749 IkReal x1239=((IkReal(0.190000000000000))*(px));
03750 IkReal x1240=((cj1)*(r02));
03751 IkReal x1241=((pz)*(r00));
03752 IkReal x1242=((cj1)*(sj0));
03753 IkReal x1243=((cj0)*(r00));
03754 IkReal x1244=((IkReal(2.00000000000000))*(sj1));
03755 IkReal x1245=((cj0)*(pz));
03756 IkReal x1246=((IkReal(1.00000000000000))*(r01));
03757 IkReal x1247=((cj1)*(px));
03758 IkReal x1248=((IkReal(2.00000000000000))*(px));
03759 IkReal x1249=((pp)*(sj1));
03760 IkReal x1250=((r02)*(sj1));
03761 IkReal x1251=((px)*(r02));
03762 IkReal x1252=((IkReal(0.700000000000000))*(pz));
03763 IkReal x1253=((cj0)*(px));
03764 IkReal x1254=((r00)*(sj0));
03765 IkReal x1255=((IkReal(1.00000000000000))*(px));
03766 IkReal x1256=((cj0)*(r01));
03767 IkReal x1257=((IkReal(1.00000000000000))*(cj1));
03768 IkReal x1258=((IkReal(1.00000000000000))*(py));
03769 IkReal x1259=((IkReal(0.700000000000000))*(px));
03770 IkReal x1260=((IkReal(0.190000000000000))*(sj1));
03771 IkReal x1261=((IkReal(0.298275000000000))*(sj3));
03772 IkReal x1262=((cj1)*(pz));
03773 IkReal x1263=((IkReal(0.0950000000000000))*(r01));
03774 IkReal x1264=((IkReal(0.306725000000000))*(cj3)*(cj4));
03775 IkReal x1265=((pz)*(x1244));
03776 IkReal x1266=((IkReal(0.550000000000000))*(x1229));
03777 IkReal x1267=((cj4)*(x1229));
03778 IkReal x1268=((IkReal(0.550000000000000))*(x1228));
03779 IkReal x1269=((IkReal(2.00000000000000))*(py)*(pz));
03780 IkReal x1270=((IkReal(0.0650000000000000))*(cj3)*(sj4));
03781 IkReal x1271=((IkReal(0.0650000000000000))*(x1228));
03782 IkReal x1272=((cj4)*(x1228));
03783 IkReal x1273=((IkReal(2.00000000000000))*(r01)*(x1230));
03784 IkReal x1274=((IkReal(0.0650000000000000))*(sj3)*(x1229));
03785 evalcond[0]=((x1240)+(((r01)*(x1233)))+(((cj3)*(x1272)))+(((sj1)*(x1243)))+(((sj3)*(x1267))));
03786 evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1233)))+(((IkReal(-1.00000000000000))*(x1266)))+(((sj3)*(x1271)))+(((IkReal(-0.0650000000000000))*(cj3)*(x1229)))+(((sj1)*(x1253)))+(x1262));
03787 evalcond[2]=((((IkReal(-1.00000000000000))*(x1243)*(x1257)))+(((IkReal(-1.00000000000000))*(sj3)*(x1272)))+(((cj3)*(x1267)))+(x1250)+(((IkReal(-1.00000000000000))*(x1242)*(x1246))));
03788 evalcond[3]=((((cj3)*(x1271)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(cj0)*(x1247)))+(x1274)+(((IkReal(-1.00000000000000))*(x1242)*(x1258)))+(x1268));
03789 evalcond[4]=((((cj1)*(x1234)))+(((x1228)*(x1270)))+(((x1242)*(x1263)))+(((IkReal(-1.00000000000000))*(x1246)*(x1247)))+(((IkReal(-1.00000000000000))*(sj1)*(x1236)*(x1258)))+(((x1233)*(x1251)))+(((IkReal(-0.0950000000000000))*(x1250)))+(((IkReal(0.0950000000000000))*(cj1)*(x1243)))+(((x1238)*(x1245)))+(((IkReal(-1.00000000000000))*(x1233)*(x1241)))+(((sj4)*(x1268)))+(((sj4)*(x1274))));
03790 evalcond[5]=((((IkReal(-1.00000000000000))*(cj1)*(x1245)*(x1246)))+(((x1229)*(x1270)))+(((IkReal(0.350000000000000))*(x1256)))+(((IkReal(0.0950000000000000))*(sj1)*(x1243)))+(((IkReal(-1.00000000000000))*(sj0)*(x1240)*(x1255)))+(((sj1)*(x1234)))+(((sj4)*(x1266)))+(((x1241)*(x1242)))+(((IkReal(0.0950000000000000))*(x1240)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x1271)))+(((cj1)*(py)*(x1236)))+(((IkReal(-1.00000000000000))*(x1238)*(x1255)))+(((IkReal(-0.350000000000000))*(x1254)))+(((x1233)*(x1263))));
03791 evalcond[6]=((((IkReal(-1.00000000000000))*(px)*(x1236)*(x1265)))+(((IkReal(-2.00000000000000))*(x1241)*(x1247)))+(((x1228)*(x1264)))+(((x1234)*(x1260)))+(((IkReal(-1.00000000000000))*(r01)*(x1235)*(x1245)))+(((IkReal(-1.00000000000000))*(x1231)*(x1243)*(x1244)))+(((r02)*(x1252)))+(((IkReal(-0.113475000000000))*(x1240)))+(((IkReal(-1.00000000000000))*(x1233)*(x1273)))+(((IkReal(-2.00000000000000))*(x1237)*(x1262)))+(((r00)*(x1259)))+(((IkReal(-0.113475000000000))*(r01)*(x1233)))+(((IkReal(0.0665000000000000))*(x1256)))+(((IkReal(0.0715000000000000))*(x1272)))+(((IkReal(-1.00000000000000))*(x1233)*(x1234)*(x1248)))+(((IkReal(-1.00000000000000))*(r02)*(x1233)*(x1269)))+(((IkReal(-1.00000000000000))*(x1238)*(x1239)))+(((pp)*(x1240)))+(((IkReal(0.700000000000000))*(x1237)))+(((IkReal(-1.00000000000000))*(x1261)*(x1267)))+(((sj0)*(x1235)*(x1241)))+(((pp)*(r01)*(x1233)))+(((IkReal(-0.113475000000000))*(sj1)*(x1243)))+(((IkReal(-2.00000000000000))*(x1232)*(x1240)))+(((IkReal(-1.00000000000000))*(x1237)*(x1244)*(x1253)))+(((IkReal(-0.0665000000000000))*(x1254)))+(((x1243)*(x1249)))+(((py)*(x1235)*(x1236)))+(((IkReal(-1.00000000000000))*(sj0)*(x1235)*(x1251))));
03792 evalcond[7]=((((IkReal(0.190000000000000))*(x1233)*(x1241)))+(((r01)*(sj0)*(x1252)))+(((x1261)*(x1272)))+(((IkReal(0.131525000000000))*(x1250)))+(((r02)*(x1249)))+(((x1242)*(x1273)))+(((IkReal(-1.00000000000000))*(x1236)*(x1259)))+(((sj0)*(x1240)*(x1269)))+(((IkReal(2.00000000000000))*(cj0)*(x1237)*(x1247)))+(((IkReal(2.00000000000000))*(pz)*(x1236)*(x1247)))+(((IkReal(-1.00000000000000))*(r02)*(x1232)*(x1244)))+(((IkReal(0.700000000000000))*(cj0)*(x1241)))+(((IkReal(-1.00000000000000))*(pp)*(x1243)*(x1257)))+(((IkReal(-1.00000000000000))*(x1237)*(x1265)))+(((IkReal(-1.00000000000000))*(r02)*(x1233)*(x1239)))+(((IkReal(-0.190000000000000))*(x1238)*(x1245)))+(((IkReal(-0.131525000000000))*(r01)*(x1242)))+(((IkReal(0.0715000000000000))*(x1267)))+(((IkReal(-1.00000000000000))*(pp)*(x1242)*(x1246)))+(((IkReal(-0.131525000000000))*(cj1)*(x1243)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x1234)*(x1242)*(x1248)))+(((px)*(r01)*(x1235)))+(((IkReal(-1.00000000000000))*(x1234)*(x1235)))+(((py)*(x1236)*(x1260)))+(((IkReal(2.00000000000000))*(cj1)*(x1231)*(x1243)))+(((x1229)*(x1264)))+(((IkReal(-1.00000000000000))*(px)*(x1241)*(x1244))));
03793 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  )
03794 {
03795 continue;
03796 }
03797 }
03798 
03799 {
03800 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
03801 vinfos[0].jointtype = 1;
03802 vinfos[0].foffset = j0;
03803 vinfos[0].indices[0] = _ij0[0];
03804 vinfos[0].indices[1] = _ij0[1];
03805 vinfos[0].maxsolutions = _nj0;
03806 vinfos[1].jointtype = 1;
03807 vinfos[1].foffset = j1;
03808 vinfos[1].indices[0] = _ij1[0];
03809 vinfos[1].indices[1] = _ij1[1];
03810 vinfos[1].maxsolutions = _nj1;
03811 vinfos[2].jointtype = 1;
03812 vinfos[2].foffset = j2;
03813 vinfos[2].indices[0] = _ij2[0];
03814 vinfos[2].indices[1] = _ij2[1];
03815 vinfos[2].maxsolutions = _nj2;
03816 vinfos[3].jointtype = 1;
03817 vinfos[3].foffset = j3;
03818 vinfos[3].indices[0] = _ij3[0];
03819 vinfos[3].indices[1] = _ij3[1];
03820 vinfos[3].maxsolutions = _nj3;
03821 vinfos[4].jointtype = 1;
03822 vinfos[4].foffset = j4;
03823 vinfos[4].indices[0] = _ij4[0];
03824 vinfos[4].indices[1] = _ij4[1];
03825 vinfos[4].maxsolutions = _nj4;
03826 std::vector<int> vfree(0);
03827 solutions.AddSolution(vinfos,vfree);
03828 }
03829 }
03830 }
03831 
03832 }
03833 
03834 }
03835 }
03836 }
03837 
03838 }
03839 
03840 }
03841 }
03842 }
03843     }
03844 }
03845 }
03846 
03847 }
03848 
03849 }
03850 }
03851 return solutions.GetNumSolutions()>0;
03852 }
03853 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
03854 {
03855     using std::complex;
03856     IKFAST_ASSERT(rawcoeffs[0] != 0);
03857     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
03858     const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
03859     complex<IkReal> coeffs[4];
03860     const int maxsteps = 110;
03861     for(int i = 0; i < 4; ++i) {
03862         coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
03863     }
03864     complex<IkReal> roots[4];
03865     IkReal err[4];
03866     roots[0] = complex<IkReal>(1,0);
03867     roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
03868     err[0] = 1.0;
03869     err[1] = 1.0;
03870     for(int i = 2; i < 4; ++i) {
03871         roots[i] = roots[i-1]*roots[1];
03872         err[i] = 1.0;
03873     }
03874     for(int step = 0; step < maxsteps; ++step) {
03875         bool changed = false;
03876         for(int i = 0; i < 4; ++i) {
03877             if ( err[i] >= tol ) {
03878                 changed = true;
03879                 // evaluate
03880                 complex<IkReal> x = roots[i] + coeffs[0];
03881                 for(int j = 1; j < 4; ++j) {
03882                     x = roots[i] * x + coeffs[j];
03883                 }
03884                 for(int j = 0; j < 4; ++j) {
03885                     if( i != j ) {
03886                         if( roots[i] != roots[j] ) {
03887                             x /= (roots[i] - roots[j]);
03888                         }
03889                     }
03890                 }
03891                 roots[i] -= x;
03892                 err[i] = abs(x);
03893             }
03894         }
03895         if( !changed ) {
03896             break;
03897         }
03898     }
03899 
03900     numroots = 0;
03901     bool visited[4] = {false};
03902     for(int i = 0; i < 4; ++i) {
03903         if( !visited[i] ) {
03904             // might be a multiple root, in which case it will have more error than the other roots
03905             // find any neighboring roots, and take the average
03906             complex<IkReal> newroot=roots[i];
03907             int n = 1;
03908             for(int j = i+1; j < 4; ++j) {
03909                 if( abs(roots[i]-roots[j]) < 8*tolsqrt ) {
03910                     newroot += roots[j];
03911                     n += 1;
03912                     visited[j] = true;
03913                 }
03914             }
03915             if( n > 1 ) {
03916                 newroot /= n;
03917             }
03918             // 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
03919             if( IKabs(imag(newroot)) < tolsqrt ) {
03920                 rawroots[numroots++] = real(newroot);
03921             }
03922         }
03923     }
03924 }
03925 };
03926 
03927 
03930 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
03931 IKSolver solver;
03932 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
03933 }
03934 
03935 IKFAST_API const char* GetKinematicsHash() { return "d2942246f1616a588c32a0fec55a0c4b"; }
03936 
03937 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
03938 
03939 #ifdef IKFAST_NAMESPACE
03940 } // end namespace
03941 #endif
03942 
03943 #ifndef IKFAST_NO_MAIN
03944 #include <stdio.h>
03945 #include <stdlib.h>
03946 #ifdef IKFAST_NAMESPACE
03947 using namespace IKFAST_NAMESPACE;
03948 #endif
03949 int main(int argc, char** argv)
03950 {
03951     if( argc != 12+GetNumFreeParameters()+1 ) {
03952         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
03953                "Returns the ik solutions given the transformation of the end effector specified by\n"
03954                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
03955                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
03956         return 1;
03957     }
03958 
03959     IkSolutionList<IkReal> solutions;
03960     std::vector<IkReal> vfree(GetNumFreeParameters());
03961     IkReal eerot[9],eetrans[3];
03962     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
03963     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
03964     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
03965     for(std::size_t i = 0; i < vfree.size(); ++i)
03966         vfree[i] = atof(argv[13+i]);
03967     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
03968 
03969     if( !bSuccess ) {
03970         fprintf(stderr,"Failed to get ik solution\n");
03971         return -1;
03972     }
03973 
03974     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
03975     std::vector<IkReal> solvalues(GetNumJoints());
03976     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
03977         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
03978         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
03979         std::vector<IkReal> vsolfree(sol.GetFree().size());
03980         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
03981         for( std::size_t j = 0; j < solvalues.size(); ++j)
03982             printf("%.15f, ", solvalues[j]);
03983         printf("\n");
03984     }
03985     return 0;
03986 }
03987 
03988 #endif


fanuc_m430ia_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Wed Aug 2 2017 03:13:50