ikfast_ur5.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 // check if the included ikfast version matches what this file was compiled with
00025 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00026 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00027 
00028 #include <cmath>
00029 #include <vector>
00030 #include <limits>
00031 #include <algorithm>
00032 #include <complex>
00033 
00034 #define IKFAST_STRINGIZE2(s) #s
00035 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
00036 
00037 #ifndef IKFAST_ASSERT
00038 #include <stdexcept>
00039 #include <sstream>
00040 #include <iostream>
00041 
00042 #ifdef _MSC_VER
00043 #ifndef __PRETTY_FUNCTION__
00044 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00045 #endif
00046 #endif
00047 
00048 #ifndef __PRETTY_FUNCTION__
00049 #define __PRETTY_FUNCTION__ __func__
00050 #endif
00051 
00052 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
00053 
00054 #endif
00055 
00056 #if defined(_MSC_VER)
00057 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00058 #else
00059 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00060 #endif
00061 
00062 #define IK2PI  ((IkReal)6.28318530717959)
00063 #define IKPI  ((IkReal)3.14159265358979)
00064 #define IKPI_2  ((IkReal)1.57079632679490)
00065 
00066 #ifdef _MSC_VER
00067 #ifndef isnan
00068 #define isnan _isnan
00069 #endif
00070 #endif // _MSC_VER
00071 
00072 // lapack routines
00073 extern "C" {
00074   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00075   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00076   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00077   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00078   void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
00079   void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
00080 }
00081 
00082 using namespace std; // necessary to get std math routines
00083 
00084 #ifdef IKFAST_NAMESPACE
00085 namespace IKFAST_NAMESPACE {
00086 #endif
00087 
00088 inline float IKabs(float f) { return fabsf(f); }
00089 inline double IKabs(double f) { return fabs(f); }
00090 
00091 inline float IKsqr(float f) { return f*f; }
00092 inline double IKsqr(double f) { return f*f; }
00093 
00094 inline float IKlog(float f) { return logf(f); }
00095 inline double IKlog(double f) { return log(f); }
00096 
00097 // allows asin and acos to exceed 1
00098 #ifndef IKFAST_SINCOS_THRESH
00099 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00100 #endif
00101 
00102 // used to check input to atan2 for degenerate cases
00103 #ifndef IKFAST_ATAN2_MAGTHRESH
00104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00105 #endif
00106 
00107 // minimum distance of separate solutions
00108 #ifndef IKFAST_SOLUTION_THRESH
00109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
00110 #endif
00111 
00112 inline float IKasin(float f)
00113 {
00114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00115 if( f <= -1 ) return float(-IKPI_2);
00116 else if( f >= 1 ) return float(IKPI_2);
00117 return asinf(f);
00118 }
00119 inline double IKasin(double f)
00120 {
00121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00122 if( f <= -1 ) return -IKPI_2;
00123 else if( f >= 1 ) return IKPI_2;
00124 return asin(f);
00125 }
00126 
00127 // return positive value in [0,y)
00128 inline float IKfmod(float x, float y)
00129 {
00130     while(x < 0) {
00131         x += y;
00132     }
00133     return fmodf(x,y);
00134 }
00135 
00136 // return positive value in [0,y)
00137 inline double IKfmod(double x, double y)
00138 {
00139     while(x < 0) {
00140         x += y;
00141     }
00142     return fmod(x,y);
00143 }
00144 
00145 inline float IKacos(float f)
00146 {
00147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00148 if( f <= -1 ) return float(IKPI);
00149 else if( f >= 1 ) return float(0);
00150 return acosf(f);
00151 }
00152 inline double IKacos(double f)
00153 {
00154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00155 if( f <= -1 ) return IKPI;
00156 else if( f >= 1 ) return 0;
00157 return acos(f);
00158 }
00159 inline float IKsin(float f) { return sinf(f); }
00160 inline double IKsin(double f) { return sin(f); }
00161 inline float IKcos(float f) { return cosf(f); }
00162 inline double IKcos(double f) { return cos(f); }
00163 inline float IKtan(float f) { return tanf(f); }
00164 inline double IKtan(double f) { return tan(f); }
00165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00167 inline float IKatan2(float fy, float fx) {
00168     if( isnan(fy) ) {
00169         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00170         return float(IKPI_2);
00171     }
00172     else if( isnan(fx) ) {
00173         return 0;
00174     }
00175     return atan2f(fy,fx);
00176 }
00177 inline double IKatan2(double fy, double fx) {
00178     if( isnan(fy) ) {
00179         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00180         return IKPI_2;
00181     }
00182     else if( isnan(fx) ) {
00183         return 0;
00184     }
00185     return atan2(fy,fx);
00186 }
00187 
00188 inline float IKsign(float f) {
00189     if( f > 0 ) {
00190         return float(1);
00191     }
00192     else if( f < 0 ) {
00193         return float(-1);
00194     }
00195     return 0;
00196 }
00197 
00198 inline double IKsign(double f) {
00199     if( f > 0 ) {
00200         return 1.0;
00201     }
00202     else if( f < 0 ) {
00203         return -1.0;
00204     }
00205     return 0;
00206 }
00207 
00210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
00211 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41;
00212 x0=IKcos(j[0]);
00213 x1=IKcos(j[1]);
00214 x2=IKsin(j[2]);
00215 x3=IKcos(j[2]);
00216 x4=IKsin(j[1]);
00217 x5=IKcos(j[3]);
00218 x6=IKsin(j[3]);
00219 x7=IKsin(j[4]);
00220 x8=IKcos(j[4]);
00221 x9=IKsin(j[0]);
00222 x10=IKsin(j[5]);
00223 x11=IKcos(j[5]);
00224 x12=((IkReal(0.392250000000000))*(x0));
00225 x13=((IkReal(0.0946500000000000))*(x0));
00226 x14=((IkReal(1.00000000000000))*(x9));
00227 x15=((IkReal(0.0946500000000000))*(x9));
00228 x16=((IkReal(1.00000000000000))*(x6));
00229 x17=((IkReal(0.392250000000000))*(x9));
00230 x18=((IkReal(1.00000000000000))*(x5));
00231 x19=((IkReal(0.425000000000000))*(x1));
00232 x20=((IkReal(1.00000000000000))*(x0));
00233 x21=((x1)*(x3));
00234 x22=((x2)*(x4));
00235 x23=((x1)*(x2));
00236 x24=((x3)*(x4));
00237 x25=((x20)*(x21));
00238 x26=((x14)*(x21));
00239 x27=((x24)+(x23));
00240 x28=((((IkReal(-1.00000000000000))*(x22)))+(((IkReal(1.00000000000000))*(x21))));
00241 x29=((((IkReal(-1.00000000000000))*(x23)))+(((IkReal(-1.00000000000000))*(x24))));
00242 x30=((x28)*(x5));
00243 x31=((((x0)*(x22)))+(((IkReal(-1.00000000000000))*(x25))));
00244 x32=((((x22)*(x9)))+(((IkReal(-1.00000000000000))*(x26))));
00245 x33=((x29)*(x6));
00246 x34=((x25)+(((IkReal(-1.00000000000000))*(x20)*(x22))));
00247 x35=((((IkReal(-1.00000000000000))*(x14)*(x22)))+(x26));
00248 x36=((x20)*(((x24)+(x23))));
00249 x37=((x14)*(((x24)+(x23))));
00250 x38=((((x27)*(x5)))+(((x28)*(x6))));
00251 x39=((x38)*(x8));
00252 x40=((((IkReal(-1.00000000000000))*(x14)*(x7)))+(((x8)*(((((x31)*(x5)))+(((x36)*(x6))))))));
00253 x41=((((x0)*(x7)))+(((x8)*(((((x32)*(x5)))+(((x37)*(x6))))))));
00254 eerot[0]=((((x11)*(x40)))+(((x10)*(((((x34)*(x6)))+(((x36)*(x5))))))));
00255 eerot[1]=((((IkReal(-1.00000000000000))*(x14)*(x8)))+(((x7)*(((((IkReal(-1.00000000000000))*(x18)*(x31)))+(((IkReal(-1.00000000000000))*(x16)*(x36))))))));
00256 eerot[2]=((((x10)*(x40)))+(((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x36)))+(((IkReal(-1.00000000000000))*(x16)*(x34))))))));
00257 IkReal x42=((IkReal(1.00000000000000))*(x13));
00258 eetrans[0]=((((x0)*(x19)))+(((IkReal(-1.00000000000000))*(x12)*(x22)))+(((IkReal(-0.109150000000000))*(x9)))+(((x6)*(((((IkReal(-1.00000000000000))*(x21)*(x42)))+(((x13)*(x22)))))))+(((x5)*(((((IkReal(-1.00000000000000))*(x23)*(x42)))+(((IkReal(-1.00000000000000))*(x24)*(x42)))))))+(((x12)*(x21))));
00259 eerot[3]=((((x11)*(x41)))+(((x10)*(((((x35)*(x6)))+(((x37)*(x5))))))));
00260 eerot[4]=((((x0)*(x8)))+(((x7)*(((((IkReal(-1.00000000000000))*(x18)*(x32)))+(((IkReal(-1.00000000000000))*(x16)*(x37))))))));
00261 eerot[5]=((((x10)*(x41)))+(((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x37)))+(((IkReal(-1.00000000000000))*(x16)*(x35))))))));
00262 IkReal x43=((IkReal(1.00000000000000))*(x15));
00263 eetrans[1]=((((IkReal(0.109150000000000))*(x0)))+(((x19)*(x9)))+(((IkReal(-1.00000000000000))*(x17)*(x22)))+(((x17)*(x21)))+(((x5)*(((((IkReal(-1.00000000000000))*(x23)*(x43)))+(((IkReal(-1.00000000000000))*(x24)*(x43)))))))+(((x6)*(((((IkReal(-1.00000000000000))*(x21)*(x43)))+(((x15)*(x22))))))));
00264 eerot[6]=((((x11)*(x39)))+(((x10)*(((x33)+(x30))))));
00265 eerot[7]=((x7)*(((((IkReal(-1.00000000000000))*(x16)*(x28)))+(((IkReal(-1.00000000000000))*(x18)*(x27))))));
00266 eerot[8]=((((x11)*(((((IkReal(-1.00000000000000))*(x33)))+(((IkReal(-1.00000000000000))*(x30)))))))+(((x10)*(x39))));
00267 eetrans[2]=((IkReal(0.0891590000000000))+(((x5)*(((((IkReal(-0.0946500000000000))*(x21)))+(((IkReal(0.0946500000000000))*(x22)))))))+(((IkReal(-0.425000000000000))*(x4)))+(((x6)*(((((IkReal(0.0946500000000000))*(x23)))+(((IkReal(0.0946500000000000))*(x24)))))))+(((IkReal(-0.392250000000000))*(x23)))+(((IkReal(-0.392250000000000))*(x24))));
00268 }
00269 
00270 IKFAST_API int GetNumFreeParameters() { return 0; }
00271 IKFAST_API int* GetFreeParameters() { return NULL; }
00272 IKFAST_API int GetNumJoints() { return 6; }
00273 
00274 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00275 
00276 IKFAST_API int GetIkType() { return 0x67000001; }
00277 
00278 class IKSolver {
00279 public:
00280 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
00281 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
00282 
00283 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00284 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; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; 
00285 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00286     solutions.Clear();
00287 r00 = eerot[0*3+0];
00288 r01 = eerot[0*3+1];
00289 r02 = eerot[0*3+2];
00290 r10 = eerot[1*3+0];
00291 r11 = eerot[1*3+1];
00292 r12 = eerot[1*3+2];
00293 r20 = eerot[2*3+0];
00294 r21 = eerot[2*3+1];
00295 r22 = eerot[2*3+2];
00296 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00297 
00298 new_r00=r00;
00299 new_r01=((IkReal(-1.00000000000000))*(r02));
00300 new_r02=r01;
00301 new_px=px;
00302 new_r10=r10;
00303 new_r11=((IkReal(-1.00000000000000))*(r12));
00304 new_r12=r11;
00305 new_py=py;
00306 new_r20=r20;
00307 new_r21=((IkReal(-1.00000000000000))*(r22));
00308 new_r22=r21;
00309 new_pz=((IkReal(-0.0891590000000000))+(pz));
00310 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
00311 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00312 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00313 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00314 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00315 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00316 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00317 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00318 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00319 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00320 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00321 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00322 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00323 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00324 IkReal IKFAST_ALIGNED16(matrixinvcoeffs[64]);
00325 IkReal x44=((IkReal(0.189300000000000))*(py));
00326 IkReal x45=((IkReal(0.0946500000000000))*(r10));
00327 IkReal x46=((IkReal(0.0946500000000000))*(r11));
00328 IkReal x47=((IkReal(2.00000000000000))*(npx));
00329 IkReal x48=((IkReal(2.00000000000000))*(npy));
00330 IkReal x49=((pp)*(r01));
00331 IkReal x50=((pp)*(r00));
00332 IkReal x51=((IkReal(1.00000000000000))*(pp));
00333 IkReal x52=((px)*(x47));
00334 IkReal x53=((py)*(x47));
00335 IkReal x54=((r10)*(x51));
00336 IkReal x55=((px)*(x48));
00337 IkReal x56=((py)*(x48));
00338 IkReal x57=((r11)*(x51));
00339 matrixinvcoeffs[0]=((IkReal(0.271700000000000))*(rxp0_2));
00340 matrixinvcoeffs[1]=((IkReal(-0.271700000000000))*(npx));
00341 matrixinvcoeffs[2]=((IkReal(0.189300000000000))*(npx));
00342 matrixinvcoeffs[3]=IkReal(0);
00343 matrixinvcoeffs[4]=IkReal(0);
00344 matrixinvcoeffs[5]=((IkReal(-0.135850000000000))*(r20));
00345 matrixinvcoeffs[6]=rxp0_2;
00346 matrixinvcoeffs[7]=((((pp)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(x47)))+(((IkReal(0.00949660000000000))*(r20))));
00347 matrixinvcoeffs[8]=((IkReal(0.271700000000000))*(rxp1_2));
00348 matrixinvcoeffs[9]=((IkReal(-0.271700000000000))*(npy));
00349 matrixinvcoeffs[10]=((IkReal(0.189300000000000))*(npy));
00350 matrixinvcoeffs[11]=IkReal(0);
00351 matrixinvcoeffs[12]=IkReal(0);
00352 matrixinvcoeffs[13]=((IkReal(-0.135850000000000))*(r21));
00353 matrixinvcoeffs[14]=rxp1_2;
00354 matrixinvcoeffs[15]=((((pp)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(x48)))+(((IkReal(0.00949660000000000))*(r21))));
00355 matrixinvcoeffs[16]=x44;
00356 matrixinvcoeffs[17]=((IkReal(-0.189300000000000))*(px));
00357 matrixinvcoeffs[18]=((IkReal(0.271700000000000))*(px));
00358 matrixinvcoeffs[19]=py;
00359 matrixinvcoeffs[20]=((IkReal(-1.00000000000000))*(px));
00360 matrixinvcoeffs[21]=IkReal(0);
00361 matrixinvcoeffs[22]=IkReal(0);
00362 matrixinvcoeffs[23]=IkReal(0);
00363 matrixinvcoeffs[24]=((((IkReal(-0.00949660000000000))*(r10)))+(x53)+(((IkReal(-1.00000000000000))*(x54))));
00364 matrixinvcoeffs[25]=((((IkReal(-0.0274138450000000))*(r00)))+(x50)+(((IkReal(-1.00000000000000))*(x52))));
00365 matrixinvcoeffs[26]=((IkReal(0.0257164050000000))*(r00));
00366 matrixinvcoeffs[27]=x45;
00367 matrixinvcoeffs[28]=((IkReal(-0.0946500000000000))*(r00));
00368 matrixinvcoeffs[29]=((IkReal(-1.00000000000000))*(rxp0_1));
00369 matrixinvcoeffs[30]=((IkReal(-0.135850000000000))*(r10));
00370 matrixinvcoeffs[31]=((IkReal(0.271700000000000))*(rxp0_1));
00371 matrixinvcoeffs[32]=((((IkReal(-0.00949660000000000))*(r11)))+(x56)+(((IkReal(-1.00000000000000))*(x57))));
00372 matrixinvcoeffs[33]=((((IkReal(-0.0274138450000000))*(r01)))+(x49)+(((IkReal(-1.00000000000000))*(x55))));
00373 matrixinvcoeffs[34]=((IkReal(0.0257164050000000))*(r01));
00374 matrixinvcoeffs[35]=x46;
00375 matrixinvcoeffs[36]=((IkReal(-0.0946500000000000))*(r01));
00376 matrixinvcoeffs[37]=((IkReal(-1.00000000000000))*(rxp1_1));
00377 matrixinvcoeffs[38]=((IkReal(-0.135850000000000))*(r11));
00378 matrixinvcoeffs[39]=((IkReal(0.271700000000000))*(rxp1_1));
00379 matrixinvcoeffs[40]=((IkReal(0.189300000000000))*(px));
00380 matrixinvcoeffs[41]=x44;
00381 matrixinvcoeffs[42]=((IkReal(-0.271700000000000))*(py));
00382 matrixinvcoeffs[43]=px;
00383 matrixinvcoeffs[44]=py;
00384 matrixinvcoeffs[45]=IkReal(0);
00385 matrixinvcoeffs[46]=IkReal(0);
00386 matrixinvcoeffs[47]=IkReal(0);
00387 matrixinvcoeffs[48]=((((IkReal(-0.00949660000000000))*(r00)))+(x52)+(((IkReal(-1.00000000000000))*(x50))));
00388 matrixinvcoeffs[49]=((x53)+(((IkReal(0.0274138450000000))*(r10)))+(((IkReal(-1.00000000000000))*(x54))));
00389 matrixinvcoeffs[50]=((IkReal(-0.0257164050000000))*(r10));
00390 matrixinvcoeffs[51]=((IkReal(0.0946500000000000))*(r00));
00391 matrixinvcoeffs[52]=x45;
00392 matrixinvcoeffs[53]=((IkReal(-1.00000000000000))*(rxp0_0));
00393 matrixinvcoeffs[54]=((IkReal(-0.135850000000000))*(r00));
00394 matrixinvcoeffs[55]=((IkReal(0.271700000000000))*(rxp0_0));
00395 matrixinvcoeffs[56]=((((IkReal(-1.00000000000000))*(x49)))+(((IkReal(-0.00949660000000000))*(r01)))+(x55));
00396 matrixinvcoeffs[57]=((x56)+(((IkReal(0.0274138450000000))*(r11)))+(((IkReal(-1.00000000000000))*(x57))));
00397 matrixinvcoeffs[58]=((IkReal(-0.0257164050000000))*(r11));
00398 matrixinvcoeffs[59]=((IkReal(0.0946500000000000))*(r01));
00399 matrixinvcoeffs[60]=x46;
00400 matrixinvcoeffs[61]=((IkReal(-1.00000000000000))*(rxp1_0));
00401 matrixinvcoeffs[62]=((IkReal(-0.135850000000000))*(r01));
00402 matrixinvcoeffs[63]=((IkReal(0.271700000000000))*(rxp1_0));
00403 if( !matrixinverse<8>(matrixinvcoeffs) ) {
00404 continue;
00405 }
00406 IkReal gconst0_0=matrixinvcoeffs[0], gconst0_1=matrixinvcoeffs[8], gconst0_2=matrixinvcoeffs[16], gconst0_3=matrixinvcoeffs[24], gconst0_4=matrixinvcoeffs[32], gconst0_5=matrixinvcoeffs[40], gconst0_6=matrixinvcoeffs[48], gconst0_7=matrixinvcoeffs[56], gconst1_0=matrixinvcoeffs[1], gconst1_1=matrixinvcoeffs[9], gconst1_2=matrixinvcoeffs[17], gconst1_3=matrixinvcoeffs[25], gconst1_4=matrixinvcoeffs[33], gconst1_5=matrixinvcoeffs[41], gconst1_6=matrixinvcoeffs[49], gconst1_7=matrixinvcoeffs[57], gconst2_0=matrixinvcoeffs[2], gconst2_1=matrixinvcoeffs[10], gconst2_2=matrixinvcoeffs[18], gconst2_3=matrixinvcoeffs[26], gconst2_4=matrixinvcoeffs[34], gconst2_5=matrixinvcoeffs[42], gconst2_6=matrixinvcoeffs[50], gconst2_7=matrixinvcoeffs[58], gconst3_0=matrixinvcoeffs[3], gconst3_1=matrixinvcoeffs[11], gconst3_2=matrixinvcoeffs[19], gconst3_3=matrixinvcoeffs[27], gconst3_4=matrixinvcoeffs[35], gconst3_5=matrixinvcoeffs[43], gconst3_6=matrixinvcoeffs[51], gconst3_7=matrixinvcoeffs[59], gconst4_0=matrixinvcoeffs[4], gconst4_1=matrixinvcoeffs[12], gconst4_2=matrixinvcoeffs[20], gconst4_3=matrixinvcoeffs[28], gconst4_4=matrixinvcoeffs[36], gconst4_5=matrixinvcoeffs[44], gconst4_6=matrixinvcoeffs[52], gconst4_7=matrixinvcoeffs[60], gconst5_0=matrixinvcoeffs[5], gconst5_1=matrixinvcoeffs[13], gconst5_2=matrixinvcoeffs[21], gconst5_3=matrixinvcoeffs[29], gconst5_4=matrixinvcoeffs[37], gconst5_5=matrixinvcoeffs[45], gconst5_6=matrixinvcoeffs[53], gconst5_7=matrixinvcoeffs[61], gconst6_0=matrixinvcoeffs[6], gconst6_1=matrixinvcoeffs[14], gconst6_2=matrixinvcoeffs[22], gconst6_3=matrixinvcoeffs[30], gconst6_4=matrixinvcoeffs[38], gconst6_5=matrixinvcoeffs[46], gconst6_6=matrixinvcoeffs[54], gconst6_7=matrixinvcoeffs[62], gconst7_0=matrixinvcoeffs[7], gconst7_1=matrixinvcoeffs[15], gconst7_2=matrixinvcoeffs[23], gconst7_3=matrixinvcoeffs[31], gconst7_4=matrixinvcoeffs[39], gconst7_5=matrixinvcoeffs[47], gconst7_6=matrixinvcoeffs[55], gconst7_7=matrixinvcoeffs[63];
00407 IkReal op[162], zeror[48];
00408 int numroots;
00409 IkReal x58=((gconst7_7)*(r01));
00410 IkReal x59=((gconst1_3)*(r21));
00411 IkReal x60=((gconst0_1)*(npx));
00412 IkReal x61=((gconst1_3)*(npy));
00413 IkReal x62=((IkReal(0.0521040950000000))*(gconst4_0));
00414 IkReal x63=((IkReal(0.109150000000000))*(gconst1_4));
00415 IkReal x64=((gconst3_1)*(rxp0_0));
00416 IkReal x65=((IkReal(0.0257164050000000))*(r01));
00417 IkReal x66=((gconst4_7)*(pz));
00418 IkReal x67=((gconst3_0)*(r00));
00419 IkReal x68=((IkReal(0.109911310000000))*(gconst4_7));
00420 IkReal x69=((gconst6_1)*(r10));
00421 IkReal x70=((gconst0_3)*(r20));
00422 IkReal x71=((IkReal(0.0327500000000000))*(gconst7_3));
00423 IkReal x72=((gconst4_2)*(r01));
00424 IkReal x73=((gconst4_1)*(r01));
00425 IkReal x74=((gconst7_7)*(r11));
00426 IkReal x75=((IkReal(0.135850000000000))*(pp));
00427 IkReal x76=((gconst1_7)*(r21));
00428 IkReal x77=((gconst4_1)*(rxp1_0));
00429 IkReal x78=((IkReal(0.00178545250000000))*(r10));
00430 IkReal x79=((gconst0_1)*(r20));
00431 IkReal x80=((gconst3_2)*(r00));
00432 IkReal x81=((gconst3_7)*(r00));
00433 IkReal x82=((gconst0_5)*(npx));
00434 IkReal x83=((IkReal(1.33436934500000))*(gconst7_0));
00435 IkReal x84=((gconst7_0)*(r11));
00436 IkReal x85=((IkReal(0.109150000000000))*(gconst3_4));
00437 IkReal x86=((IkReal(0.668610452500000))*(gconst3_7));
00438 IkReal x87=((gconst4_3)*(r01));
00439 IkReal x88=((IkReal(0.189300000000000))*(pz));
00440 IkReal x89=((gconst0_7)*(npx));
00441 IkReal x90=((gconst6_2)*(r10));
00442 IkReal x91=((gconst1_2)*(npy));
00443 IkReal x92=((IkReal(0.0257164050000000))*(pz));
00444 IkReal x93=((IkReal(0.668610452500000))*(gconst6_7));
00445 IkReal x94=((IkReal(0.109150000000000))*(gconst7_4));
00446 IkReal x95=((IkReal(0.0534000000000000))*(gconst0_6));
00447 IkReal x96=((IkReal(0.106800000000000))*(gconst4_5));
00448 IkReal x97=((gconst0_2)*(r20));
00449 IkReal x98=((gconst1_1)*(npy));
00450 IkReal x99=((gconst0_0)*(r20));
00451 IkReal x100=((gconst6_0)*(r10));
00452 IkReal x101=((gconst0_3)*(npx));
00453 IkReal x102=((gconst3_0)*(rxp0_0));
00454 IkReal x103=((IkReal(0.0534000000000000))*(gconst1_6));
00455 IkReal x104=((gconst4_7)*(r01));
00456 IkReal x105=((gconst3_1)*(r00));
00457 IkReal x106=((gconst7_5)*(rxp1_1));
00458 IkReal x107=((gconst6_2)*(rxp0_1));
00459 IkReal x108=((gconst6_3)*(r10));
00460 IkReal x109=((IkReal(0.817250000000000))*(r11));
00461 IkReal x110=((gconst1_0)*(r21));
00462 IkReal x111=((IkReal(0.0267000000000000))*(gconst3_5));
00463 IkReal x112=((IkReal(0.641196607500000))*(gconst7_2));
00464 IkReal x113=((IkReal(0.0327500000000000))*(r00));
00465 IkReal x114=((gconst0_0)*(npx));
00466 IkReal x115=((gconst0_7)*(r20));
00467 IkReal x116=((IkReal(0.0534000000000000))*(gconst3_6));
00468 IkReal x117=((IkReal(1.33436934500000))*(gconst6_0));
00469 IkReal x118=((gconst6_7)*(r10));
00470 IkReal x119=((gconst3_2)*(rxp0_0));
00471 IkReal x120=((IkReal(0.668610452500000))*(gconst4_7));
00472 IkReal x121=((gconst0_4)*(r20));
00473 IkReal x122=((IkReal(0.0256283925000000))*(r11));
00474 IkReal x123=((gconst7_1)*(r11));
00475 IkReal x124=((IkReal(0.106800000000000))*(gconst6_5));
00476 IkReal x125=((gconst6_5)*(r10));
00477 IkReal x126=((gconst3_1)*(r10));
00478 IkReal x127=((gconst6_1)*(r00));
00479 IkReal x128=((gconst6_0)*(r00));
00480 IkReal x129=((IkReal(0.817250000000000))*(gconst7_3));
00481 IkReal x130=((gconst7_5)*(r01));
00482 IkReal x131=((gconst6_2)*(r00));
00483 IkReal x132=((IkReal(0.000719345000000000))*(gconst4_0));
00484 IkReal x133=((IkReal(0.109150000000000))*(gconst4_4));
00485 IkReal x134=((IkReal(1.00000000000000))*(pp));
00486 IkReal x135=((gconst7_2)*(rxp1_1));
00487 IkReal x136=((gconst1_1)*(r21));
00488 IkReal x137=((gconst1_0)*(npy));
00489 IkReal x138=((gconst6_1)*(rxp0_1));
00490 IkReal x139=((gconst3_3)*(r00));
00491 IkReal x140=((IkReal(0.817250000000000))*(gconst3_3));
00492 IkReal x141=((IkReal(0.0534000000000000))*(gconst7_6));
00493 IkReal x142=((gconst6_7)*(rxp0_1));
00494 IkReal x143=((gconst7_2)*(r11));
00495 IkReal x144=((gconst7_3)*(r11));
00496 IkReal x145=((gconst3_7)*(rxp0_0));
00497 IkReal x146=((gconst7_7)*(rxp1_1));
00498 IkReal x147=((gconst3_7)*(r10));
00499 IkReal x148=((IkReal(0.0534000000000000))*(gconst4_6));
00500 IkReal x149=((gconst0_2)*(npx));
00501 IkReal x150=((gconst0_5)*(r20));
00502 IkReal x151=((gconst4_5)*(r01));
00503 IkReal x152=((gconst4_2)*(rxp1_0));
00504 IkReal x153=((gconst7_5)*(r11));
00505 IkReal x154=((IkReal(0.0946500000000000))*(pp));
00506 IkReal x155=((gconst4_2)*(r11));
00507 IkReal x156=((gconst1_2)*(r21));
00508 IkReal x157=((gconst7_1)*(r01));
00509 IkReal x158=((gconst6_4)*(r10));
00510 IkReal x159=((gconst7_1)*(rxp1_1));
00511 IkReal x160=((gconst4_1)*(r11));
00512 IkReal x161=((gconst4_0)*(r01));
00513 IkReal x162=((gconst1_5)*(r21));
00514 IkReal x163=((gconst6_0)*(rxp0_1));
00515 IkReal x164=((gconst1_5)*(npy));
00516 IkReal x165=((gconst3_5)*(r00));
00517 IkReal x166=((IkReal(1.33436934500000))*(gconst4_0));
00518 IkReal x167=((IkReal(0.106800000000000))*(gconst3_5));
00519 IkReal x168=((IkReal(0.00178545250000000))*(gconst4_7));
00520 IkReal x169=((gconst6_6)*(r10));
00521 IkReal x170=((gconst7_2)*(r01));
00522 IkReal x171=((gconst1_7)*(npy));
00523 IkReal x172=((gconst4_3)*(rxp1_0));
00524 IkReal x173=((gconst7_0)*(r01));
00525 IkReal x174=((gconst7_0)*(rxp1_1));
00526 IkReal x175=((IkReal(0.817250000000000))*(gconst6_3));
00527 IkReal x176=((gconst3_2)*(r10));
00528 IkReal x177=((IkReal(0.0327500000000000))*(gconst3_3));
00529 IkReal x178=((IkReal(0.0179172450000000))*(pz));
00530 IkReal x179=((gconst6_7)*(r00));
00531 IkReal x180=((gconst3_0)*(r10));
00532 IkReal x181=((IkReal(0.0534000000000000))*(gconst6_6));
00533 IkReal x182=((IkReal(0.0267000000000000))*(gconst6_5));
00534 IkReal x183=((IkReal(0.0267000000000000))*(gconst4_5));
00535 IkReal x184=((IkReal(0.109150000000000))*(gconst6_4));
00536 IkReal x185=((IkReal(0.00362719500000000))*(x151));
00537 IkReal x186=((IkReal(0.0148280275000000))*(gconst4_4)*(r01));
00538 IkReal x187=((IkReal(0.0257164050000000))*(x60));
00539 IkReal x188=((IkReal(0.00349357361925000))*(x73));
00540 IkReal x189=((IkReal(0.817250000000000))*(x61));
00541 IkReal x190=((IkReal(0.0267000000000000))*(x82));
00542 IkReal x191=((IkReal(0.0267000000000000))*(x164));
00543 IkReal x192=((IkReal(0.00362719500000000))*(x165));
00544 IkReal x193=((IkReal(0.641196607500000))*(x91));
00545 IkReal x194=((IkReal(0.0871065591288750))*(x80));
00546 IkReal x195=((IkReal(0.0257164050000000))*(x98));
00547 IkReal x196=((IkReal(0.111023412500000))*(x139));
00548 IkReal x197=((IkReal(0.0871065591288750))*(x72));
00549 IkReal x198=((IkReal(0.0148280275000000))*(gconst3_4)*(r00));
00550 IkReal x199=((IkReal(0.641196607500000))*(x149));
00551 IkReal x200=((IkReal(0.00349357361925000))*(x105));
00552 IkReal x201=((IkReal(0.0908307299721250))*(x118));
00553 IkReal x202=((IkReal(0.111023412500000))*(x87));
00554 IkReal x203=((IkReal(0.817250000000000))*(x101));
00555 IkReal x204=((npy)*(x63));
00556 IkReal x205=((IkReal(0.109150000000000))*(gconst0_4)*(npx));
00557 IkReal x206=((IkReal(0.0908307299721250))*(x74));
00558 IkReal x207=((IkReal(0.0148280275000000))*(x158));
00559 IkReal x208=((IkReal(0.0871065591288750))*(x143));
00560 IkReal x209=((IkReal(0.111023412500000))*(x108));
00561 IkReal x210=((IkReal(0.00362719500000000))*(x125));
00562 IkReal x211=((IkReal(0.668610452500000))*(x89));
00563 IkReal x212=((IkReal(0.0148280275000000))*(gconst7_4)*(r11));
00564 IkReal x213=((IkReal(0.111023412500000))*(x144));
00565 IkReal x214=((IkReal(0.0871065591288750))*(x90));
00566 IkReal x215=((IkReal(0.00349357361925000))*(x123));
00567 IkReal x216=((IkReal(0.00362719500000000))*(x153));
00568 IkReal x217=((IkReal(0.0908307299721250))*(x81));
00569 IkReal x218=((IkReal(0.00349357361925000))*(x69));
00570 IkReal x219=((IkReal(0.0908307299721250))*(x104));
00571 IkReal x220=((IkReal(0.668610452500000))*(x171));
00572 IkReal x221=((IkReal(0.00725439000000000))*(gconst7_6)*(r11));
00573 IkReal x222=((IkReal(0.00725439000000000))*(x169));
00574 IkReal x223=((IkReal(0.00725439000000000))*(gconst3_6)*(r00));
00575 IkReal x224=((IkReal(0.00725439000000000))*(gconst4_6)*(r01));
00576 IkReal x225=((npx)*(x95));
00577 IkReal x226=((npy)*(x103));
00578 IkReal x227=((IkReal(0.00616623150000000))*(x105));
00579 IkReal x228=((IkReal(0.0453900000000000))*(x60));
00580 IkReal x229=((IkReal(0.00707834130575000))*(x84));
00581 IkReal x230=((IkReal(0.00616623150000000))*(x73));
00582 IkReal x231=((IkReal(0.00707834130575000))*(x100));
00583 IkReal x232=((IkReal(0.0453900000000000))*(x98));
00584 IkReal x233=((IkReal(0.0521040950000000))*(x137));
00585 IkReal x234=((IkReal(0.00707834130575000))*(x161));
00586 IkReal x235=((IkReal(0.00707834130575000))*(x67));
00587 IkReal x236=((IkReal(0.00616623150000000))*(x69));
00588 IkReal x237=((IkReal(0.0521040950000000))*(x114));
00589 IkReal x238=((IkReal(0.00616623150000000))*(x123));
00590 IkReal x239=((IkReal(0.00444908750000000))*(x87));
00591 IkReal x240=((IkReal(0.00348161712112500))*(x143));
00592 IkReal x241=((IkReal(0.00444908750000000))*(x139));
00593 IkReal x242=((IkReal(0.0327500000000000))*(x101));
00594 IkReal x243=((IkReal(0.000242553722125000))*(x81));
00595 IkReal x244=((IkReal(0.000242553722125000))*(x104));
00596 IkReal x245=((IkReal(0.00348161712112500))*(x90));
00597 IkReal x246=((IkReal(0.00178545250000000))*(x171));
00598 IkReal x247=((IkReal(0.00178545250000000))*(x89));
00599 IkReal x248=((IkReal(0.0327500000000000))*(x61));
00600 IkReal x249=((IkReal(0.000242553722125000))*(x74));
00601 IkReal x250=((IkReal(0.00348161712112500))*(x72));
00602 IkReal x251=((IkReal(0.0256283925000000))*(x149));
00603 IkReal x252=((IkReal(0.000242553722125000))*(x118));
00604 IkReal x253=((IkReal(0.0256283925000000))*(x91));
00605 IkReal x254=((IkReal(0.00444908750000000))*(x144));
00606 IkReal x255=((IkReal(0.00444908750000000))*(x108));
00607 IkReal x256=((IkReal(0.00348161712112500))*(x80));
00608 IkReal x257=((r00)*(x175));
00609 IkReal x258=((r00)*(x184));
00610 IkReal x259=((r00)*(x182));
00611 IkReal x260=((r01)*(x129));
00612 IkReal x261=((r11)*(x133));
00613 IkReal x262=((r10)*(x111));
00614 IkReal x263=((IkReal(0.0257164050000000))*(x160));
00615 IkReal x264=((IkReal(0.0257164050000000))*(x126));
00616 IkReal x265=((IkReal(0.641196607500000))*(x176));
00617 IkReal x266=((gconst4_3)*(x109));
00618 IkReal x267=((IkReal(0.641196607500000))*(x155));
00619 IkReal x268=((IkReal(0.0257164050000000))*(x127));
00620 IkReal x269=((r10)*(x140));
00621 IkReal x270=((r11)*(x183));
00622 IkReal x271=((gconst7_1)*(x65));
00623 IkReal x272=((r01)*(x94));
00624 IkReal x273=((IkReal(0.641196607500000))*(x131));
00625 IkReal x274=((r01)*(x112));
00626 IkReal x275=((r10)*(x85));
00627 IkReal x276=((IkReal(0.0267000000000000))*(x130));
00628 IkReal x277=((r00)*(x93));
00629 IkReal x278=((IkReal(0.668610452500000))*(x58));
00630 IkReal x279=((r11)*(x120));
00631 IkReal x280=((r10)*(x86));
00632 IkReal x281=((r11)*(x148));
00633 IkReal x282=((r01)*(x141));
00634 IkReal x283=((r00)*(x181));
00635 IkReal x284=((r10)*(x116));
00636 IkReal x285=((IkReal(0.0453900000000000))*(x160));
00637 IkReal x286=((IkReal(0.0453900000000000))*(x157));
00638 IkReal x287=((IkReal(0.0453900000000000))*(x127));
00639 IkReal x288=((IkReal(0.0453900000000000))*(x126));
00640 IkReal x289=((IkReal(0.0521040950000000))*(x128));
00641 IkReal x290=((IkReal(0.0521040950000000))*(x180));
00642 IkReal x291=((IkReal(0.0521040950000000))*(x173));
00643 IkReal x292=((r11)*(x62));
00644 IkReal x293=((IkReal(0.0327500000000000))*(gconst4_3)*(r11));
00645 IkReal x294=((IkReal(0.00178545250000000))*(x179));
00646 IkReal x295=((IkReal(0.00178545250000000))*(x58));
00647 IkReal x296=((gconst3_7)*(x78));
00648 IkReal x297=((r11)*(x168));
00649 IkReal x298=((r01)*(x71));
00650 IkReal x299=((gconst6_3)*(x113));
00651 IkReal x300=((r10)*(x177));
00652 IkReal x301=((IkReal(0.0256283925000000))*(x170));
00653 IkReal x302=((IkReal(0.0256283925000000))*(x176));
00654 IkReal x303=((gconst4_2)*(x122));
00655 IkReal x304=((IkReal(0.0256283925000000))*(x131));
00656 IkReal x305=((IkReal(0.0267000000000000))*(x125));
00657 IkReal x306=((IkReal(0.668610452500000))*(x104));
00658 IkReal x307=((gconst7_3)*(x109));
00659 IkReal x308=((IkReal(0.817250000000000))*(x108));
00660 IkReal x309=((pp)*(x72));
00661 IkReal x310=((pp)*(x80));
00662 IkReal x311=((IkReal(0.0257164050000000))*(x69));
00663 IkReal x312=((IkReal(0.668610452500000))*(x81));
00664 IkReal x313=((r11)*(x94));
00665 IkReal x314=((r11)*(x112));
00666 IkReal x315=((IkReal(0.0257164050000000))*(x123));
00667 IkReal x316=((IkReal(0.641196607500000))*(x90));
00668 IkReal x317=((IkReal(0.0267000000000000))*(x153));
00669 IkReal x318=((IkReal(0.109150000000000))*(x158));
00670 IkReal x319=((IkReal(0.668610452500000))*(x74));
00671 IkReal x320=((r10)*(x93));
00672 IkReal x321=((gconst4_1)*(x65));
00673 IkReal x322=((r00)*(x111));
00674 IkReal x323=((r01)*(x133));
00675 IkReal x324=((IkReal(0.817250000000000))*(x87));
00676 IkReal x325=((IkReal(0.641196607500000))*(x72));
00677 IkReal x326=((IkReal(0.817250000000000))*(x139));
00678 IkReal x327=((IkReal(0.0257164050000000))*(x105));
00679 IkReal x328=((IkReal(0.641196607500000))*(x80));
00680 IkReal x329=((IkReal(0.0267000000000000))*(x151));
00681 IkReal x330=((r00)*(x85));
00682 IkReal x331=((r01)*(x148));
00683 IkReal x332=((r00)*(x116));
00684 IkReal x333=((IkReal(0.0534000000000000))*(x169));
00685 IkReal x334=((r11)*(x141));
00686 IkReal x335=((IkReal(0.0256283925000000))*(x80));
00687 IkReal x336=((gconst6_7)*(x78));
00688 IkReal x337=((IkReal(0.00178545250000000))*(x74));
00689 IkReal x338=((r11)*(x71));
00690 IkReal x339=((IkReal(0.0327500000000000))*(x108));
00691 IkReal x340=((IkReal(0.0256283925000000))*(x72));
00692 IkReal x341=((IkReal(0.0327500000000000))*(x87));
00693 IkReal x342=((gconst7_2)*(x122));
00694 IkReal x343=((IkReal(0.00178545250000000))*(x104));
00695 IkReal x344=((IkReal(0.00178545250000000))*(x81));
00696 IkReal x345=((IkReal(0.0256283925000000))*(x90));
00697 IkReal x346=((gconst3_3)*(x113));
00698 IkReal x347=((IkReal(0.668610452500000))*(x115));
00699 IkReal x348=((pp)*(x97));
00700 IkReal x349=((pp)*(x156));
00701 IkReal x350=((IkReal(0.668610452500000))*(x76));
00702 IkReal x351=((IkReal(0.0267000000000000))*(x150));
00703 IkReal x352=((IkReal(0.817250000000000))*(x59));
00704 IkReal x353=((IkReal(0.109150000000000))*(x121));
00705 IkReal x354=((IkReal(0.0257164050000000))*(x136));
00706 IkReal x355=((IkReal(0.817250000000000))*(x70));
00707 IkReal x356=((IkReal(0.641196607500000))*(x156));
00708 IkReal x357=((IkReal(0.641196607500000))*(x97));
00709 IkReal x358=((IkReal(0.0257164050000000))*(x79));
00710 IkReal x359=((r21)*(x63));
00711 IkReal x360=((IkReal(0.0267000000000000))*(x162));
00712 IkReal x361=((r20)*(x95));
00713 IkReal x362=((r21)*(x103));
00714 IkReal x363=((IkReal(0.0256283925000000))*(x97));
00715 IkReal x364=((IkReal(0.0256283925000000))*(x156));
00716 IkReal x365=((IkReal(0.00178545250000000))*(x115));
00717 IkReal x366=((IkReal(0.0327500000000000))*(x70));
00718 IkReal x367=((IkReal(0.0327500000000000))*(x59));
00719 IkReal x368=((IkReal(0.00178545250000000))*(x76));
00720 IkReal x369=((rxp0_1)*(x184));
00721 IkReal x370=((rxp1_0)*(x120));
00722 IkReal x371=((rxp1_1)*(x112));
00723 IkReal x372=((rxp0_1)*(x182));
00724 IkReal x373=((rxp0_1)*(x175));
00725 IkReal x374=((pp)*(x152));
00726 IkReal x375=((IkReal(0.0257164050000000))*(x138));
00727 IkReal x376=((IkReal(0.0267000000000000))*(x106));
00728 IkReal x377=((rxp1_1)*(x129));
00729 IkReal x378=((rxp1_1)*(x94));
00730 IkReal x379=((pp)*(x119));
00731 IkReal x380=((rxp0_0)*(x86));
00732 IkReal x381=((IkReal(0.0257164050000000))*(x159));
00733 IkReal x382=((IkReal(0.641196607500000))*(x107));
00734 IkReal x383=((rxp0_0)*(x85));
00735 IkReal x384=((rxp1_0)*(x183));
00736 IkReal x385=((IkReal(0.641196607500000))*(x119));
00737 IkReal x386=((rxp0_0)*(x111));
00738 IkReal x387=((IkReal(0.668610452500000))*(x146));
00739 IkReal x388=((IkReal(0.0257164050000000))*(x77));
00740 IkReal x389=((rxp0_0)*(x140));
00741 IkReal x390=((IkReal(0.0257164050000000))*(x64));
00742 IkReal x391=((rxp1_0)*(x133));
00743 IkReal x392=((IkReal(0.817250000000000))*(x172));
00744 IkReal x393=((rxp0_1)*(x93));
00745 IkReal x394=((IkReal(0.641196607500000))*(x152));
00746 IkReal x395=((rxp0_0)*(x116));
00747 IkReal x396=((rxp1_0)*(x148));
00748 IkReal x397=((rxp1_1)*(x141));
00749 IkReal x398=((rxp0_1)*(x181));
00750 IkReal x399=((IkReal(0.0256283925000000))*(x152));
00751 IkReal x400=((IkReal(0.00178545250000000))*(x146));
00752 IkReal x401=((rxp1_1)*(x71));
00753 IkReal x402=((IkReal(0.0327500000000000))*(gconst6_3)*(rxp0_1));
00754 IkReal x403=((IkReal(0.0256283925000000))*(x119));
00755 IkReal x404=((IkReal(0.00178545250000000))*(x142));
00756 IkReal x405=((rxp0_0)*(x177));
00757 IkReal x406=((rxp1_0)*(x168));
00758 IkReal x407=((IkReal(0.0256283925000000))*(x107));
00759 IkReal x408=((IkReal(0.00178545250000000))*(x145));
00760 IkReal x409=((IkReal(0.0327500000000000))*(x172));
00761 IkReal x410=((IkReal(0.0256283925000000))*(x135));
00762 IkReal x411=((IkReal(0.00243405773325000))*(x136));
00763 IkReal x412=((IkReal(0.00252715500000000))*(x162));
00764 IkReal x413=((IkReal(0.0103310475000000))*(gconst1_4)*(r21));
00765 IkReal x414=((IkReal(0.0606892588998750))*(x156));
00766 IkReal x415=((IkReal(0.00243405773325000))*(x79));
00767 IkReal x416=((IkReal(0.0773527125000000))*(x59));
00768 IkReal x417=((IkReal(0.0606892588998750))*(x97));
00769 IkReal x418=((IkReal(0.0103310475000000))*(x121));
00770 IkReal x419=((IkReal(0.00252715500000000))*(x150));
00771 IkReal x420=((IkReal(0.0773527125000000))*(x70));
00772 IkReal x421=((IkReal(0.0632839793291250))*(x76));
00773 IkReal x422=((IkReal(0.0632839793291250))*(x115));
00774 IkReal x423=((IkReal(0.00505431000000000))*(gconst0_6)*(r20));
00775 IkReal x424=((IkReal(0.00505431000000000))*(gconst1_6)*(r21));
00776 IkReal x425=((IkReal(0.00429616350000000))*(x79));
00777 IkReal x426=((IkReal(0.00429616350000000))*(x136));
00778 IkReal x427=((IkReal(0.00493165259175000))*(x110));
00779 IkReal x428=((IkReal(0.00493165259175000))*(x99));
00780 IkReal x429=((r00)*(x88));
00781 IkReal x430=((x88)*(x89));
00782 IkReal x431=((x65)*(x66));
00783 IkReal x432=((x81)*(x92));
00784 IkReal x433=((x171)*(x88));
00785 IkReal x434=((x75)*(x90));
00786 IkReal x435=((x143)*(x75));
00787 IkReal x436=((x74)*(x92));
00788 IkReal x437=((x118)*(x92));
00789 IkReal x438=((x72)*(x75));
00790 IkReal x439=((x134)*(x91));
00791 IkReal x440=((x134)*(x149));
00792 IkReal x441=((x75)*(x80));
00793 IkReal x442=((x179)*(x88));
00794 IkReal x443=((IkReal(0.189300000000000))*(r11)*(x66));
00795 IkReal x444=((x147)*(x88));
00796 IkReal x445=((x58)*(x88));
00797 IkReal x446=((x134)*(x155));
00798 IkReal x447=((x131)*(x134));
00799 IkReal x448=((x134)*(x170));
00800 IkReal x449=((x134)*(x176));
00801 IkReal x450=((x118)*(x88));
00802 IkReal x451=((x74)*(x88));
00803 IkReal x452=((IkReal(0.189300000000000))*(r01)*(x66));
00804 IkReal x453=((x81)*(x88));
00805 IkReal x454=((x134)*(x90));
00806 IkReal x455=((x134)*(x143));
00807 IkReal x456=((x115)*(x88));
00808 IkReal x457=((x76)*(x88));
00809 IkReal x458=((x146)*(x88));
00810 IkReal x459=((x142)*(x88));
00811 IkReal x460=((x134)*(x135));
00812 IkReal x461=((x107)*(x134));
00813 IkReal x462=((IkReal(0.189300000000000))*(rxp1_0)*(x66));
00814 IkReal x463=((x145)*(x88));
00815 IkReal x464=((x115)*(x178));
00816 IkReal x465=((x178)*(x76));
00817 IkReal x466=((x154)*(x156));
00818 IkReal x467=((x154)*(x97));
00819 IkReal x468=((((IkReal(0.0521040950000000))*(x110)))+(((IkReal(0.0521040950000000))*(x99))));
00820 IkReal x469=((x221)+(x222));
00821 IkReal x470=((x350)+(x347));
00822 IkReal x471=((x348)+(x349));
00823 IkReal x472=((x366)+(x367));
00824 IkReal x473=((x368)+(x365));
00825 IkReal x474=((x424)+(x423));
00826 IkReal x475=((x332)+(x331));
00827 IkReal x476=((x351)+(x360));
00828 IkReal x477=((x333)+(x334));
00829 IkReal x478=((x376)+(x372));
00830 IkReal x479=((x380)+(x370));
00831 IkReal x480=((x305)+(x317));
00832 IkReal x481=((x343)+(x344));
00833 IkReal x482=((x363)+(x364));
00834 IkReal x483=((x426)+(x425));
00835 IkReal x484=((x397)+(x398));
00836 IkReal x485=((x422)+(x421));
00837 IkReal x486=((x428)+(x427));
00838 IkReal x487=((x361)+(x362));
00839 IkReal x488=((x406)+(x408));
00840 IkReal x489=((x395)+(x396));
00841 IkReal x490=((x306)+(x312));
00842 IkReal x491=((x412)+(x419));
00843 IkReal x492=((x467)+(x466));
00844 IkReal x493=((((IkReal(0.0453900000000000))*(x136)))+(((IkReal(0.0453900000000000))*(x79)))+(x487));
00845 IkReal x494=((x337)+(x336)+(x322)+(x329));
00846 IkReal x495=((x342)+(x341)+(x346)+(x345));
00847 IkReal x496=((x294)+(x295)+(x296)+(x297));
00848 IkReal x497=((x300)+(x298)+(x299)+(x293));
00849 IkReal x498=((x339)+(x338)+(x335)+(x340));
00850 IkReal x499=((x304)+(x302)+(x303)+(x301));
00851 IkReal x500=((x352)+(x355)+(x357)+(x356));
00852 IkReal x501=((x308)+(x307)+(x314)+(x316));
00853 IkReal x502=((x320)+(x322)+(x329)+(x319));
00854 IkReal x503=((x277)+(x278)+(x279)+(x280));
00855 IkReal x504=((x416)+(x417)+(x414)+(x420));
00856 IkReal x505=((x386)+(x387)+(x384)+(x393));
00857 IkReal x506=((x213)+(x214)+(x209)+(x208));
00858 IkReal x507=((x385)+(x389)+(x394)+(x392));
00859 IkReal x508=((x290)+(x291)+(x292)+(x289));
00860 IkReal x509=((x259)+(x262)+(x276)+(x270));
00861 IkReal x510=((x284)+(x283)+(x282)+(x281));
00862 IkReal x511=((x405)+(x407)+(x409)+(x410));
00863 IkReal x512=((x288)+(x287)+(x286)+(x285));
00864 IkReal x513=((x324)+(x325)+(x326)+(x328));
00865 IkReal x514=((x386)+(x384)+(x400)+(x404));
00866 IkReal x515=((x223)+(x225)+(x224)+(x226));
00867 IkReal x516=((x449)+(x448)+(x447)+(x446));
00868 IkReal x517=((((IkReal(0.0521040950000000))*(x67)))+(((IkReal(0.0453900000000000))*(x69)))+(((r01)*(x62)))+(x477)+(((IkReal(0.0453900000000000))*(x123))));
00869 IkReal x518=((((IkReal(0.0521040950000000))*(x100)))+(((IkReal(0.0521040950000000))*(x84)))+(((IkReal(0.0453900000000000))*(x73)))+(x475)+(((IkReal(0.0453900000000000))*(x105))));
00870 IkReal x519=((((IkReal(0.0453900000000000))*(x138)))+(((IkReal(0.0453900000000000))*(x159)))+(((IkReal(0.0521040950000000))*(x102)))+(x484)+(((rxp1_0)*(x62))));
00871 IkReal x520=((((IkReal(0.0453900000000000))*(x64)))+(((IkReal(0.0521040950000000))*(x163)))+(((IkReal(0.0521040950000000))*(x174)))+(((IkReal(0.0453900000000000))*(x77)))+(x489));
00872 IkReal x521=((x238)+(x233)+(x236)+(x237)+(x234)+(x235));
00873 IkReal x522=((x210)+(x211)+(x216)+(x217)+(x219)+(x220));
00874 IkReal x523=((x192)+(x191)+(x190)+(x185)+(x206)+(x201));
00875 IkReal x524=((x210)+(x216)+(x243)+(x247)+(x246)+(x244));
00876 IkReal x525=((x254)+(x255)+(x256)+(x250)+(x251)+(x253));
00877 IkReal x526=((x192)+(x191)+(x190)+(x185)+(x252)+(x249));
00878 IkReal x527=((x353)+(x354)+(x359)+(x358)+(x456)+(x457));
00879 IkReal x528=((x413)+(x411)+(x415)+(x418)+(x465)+(x464)+(pz));
00880 IkReal x529=((x257)+(x265)+(x267)+(x266)+(x260)+(x269)+(x274)+(x273));
00881 IkReal x530=((x309)+(x318)+(x315)+(x311)+(x310)+(x313)+(x450)+(x451));
00882 IkReal x531=((x330)+(x327)+(x321)+(x323)+(x454)+(x455)+(x452)+(x453));
00883 IkReal x532=((x383)+(x388)+(x391)+(x390)+(x463)+(x462)+(x461)+(x460));
00884 IkReal x533=((x229)+(x228)+(x227)+(x232)+(x230)+(x231)+(x515));
00885 IkReal x534=((x212)+(x215)+(x218)+(x207)+(x438)+(x439)+(x436)+(x437)+(x441)+(x440));
00886 IkReal x535=((x382)+(x381)+(x379)+(x378)+(x377)+(x375)+(x374)+(x373)+(x371)+(x369)+(x458)+(x459));
00887 IkReal x536=((x381)+(x379)+(x378)+(x375)+(x374)+(x369)+(x399)+(x401)+(x403)+(x402)+(x458)+(x459));
00888 IkReal x537=((x258)+(x264)+(x261)+(x263)+(x268)+(x275)+(x272)+(x271)+(x445)+(x444)+(x443)+(x442));
00889 IkReal x538=((IkReal(0.0946500000000000))+(x198)+(x195)+(x186)+(x187)+(x188)+(x239)+(x205)+(x204)+(x200)+(x242)+(x241)+(x240)+(x245)+(x248)+(x430)+(x431)+(x432)+(x433)+(x434)+(x435));
00890 IkReal x539=((IkReal(0.0946500000000000))+(x199)+(x198)+(x193)+(x197)+(x196)+(x195)+(x194)+(x186)+(x187)+(x188)+(x189)+(x205)+(x204)+(x203)+(x202)+(x200)+(x430)+(x431)+(x432)+(x433)+(x434)+(x435));
00891 op[0]=((((IkReal(-1.00000000000000))*(x506)))+(((IkReal(-1.00000000000000))*(x534)))+(((IkReal(-1.00000000000000))*(x523)))+(x539)+(x522));
00892 op[1]=((IkReal(-1.63450000000000))+(((IkReal(-0.0118573004550000))*(x73)))+(((IkReal(1.33436934500000))*(x137)))+(((IkReal(-0.181274075518250))*(x100)))+(((IkReal(1.33436934500000))*(x114)))+(((IkReal(0.181274075518250))*(x161)))+(((IkReal(-0.0872823000000000))*(x60)))+(((IkReal(-0.0872823000000000))*(x98)))+(((IkReal(0.181274075518250))*(x67)))+(x469)+(((IkReal(-0.0118573004550000))*(x105)))+(((IkReal(0.0118573004550000))*(x69)))+(((IkReal(-0.181274075518250))*(x84)))+(((IkReal(-1.00000000000000))*(x515)))+(((IkReal(0.0118573004550000))*(x123))));
00893 op[2]=((((IkReal(-1.00000000000000))*(x506)))+(((IkReal(-1.00000000000000))*(x534)))+(((IkReal(-1.00000000000000))*(x522)))+(x539)+(x523));
00894 op[3]=((IkReal(-0.850000000000000))+(((IkReal(-1.00000000000000))*(x533)))+(x469)+(x521));
00895 op[4]=((((IkReal(0.106800000000000))*(x164)))+(((IkReal(0.0145087800000000))*(x151)))+(((IkReal(-0.0149314514635000))*(x81)))+(((IkReal(0.0149314514635000))*(x74)))+(((IkReal(-0.0145087800000000))*(x125)))+(((IkReal(0.0145087800000000))*(x165)))+(((IkReal(0.0149314514635000))*(x118)))+(((IkReal(-0.0149314514635000))*(x104)))+(((IkReal(0.106800000000000))*(x82)))+(((IkReal(-0.0145087800000000))*(x153)))+(((IkReal(-0.109911310000000))*(x171)))+(((IkReal(-0.109911310000000))*(x89))));
00896 op[5]=((IkReal(0.850000000000000))+(((IkReal(-1.00000000000000))*(x469)))+(((IkReal(-1.00000000000000))*(x521)))+(x533));
00897 op[6]=((((IkReal(-1.00000000000000))*(x534)))+(((IkReal(-1.00000000000000))*(x525)))+(((IkReal(-1.00000000000000))*(x524)))+(x538)+(x526));
00898 op[7]=((IkReal(0.0655000000000000))+(((IkReal(-9.77230182500000e-5))*(x67)))+(((IkReal(-9.77230182500000e-5))*(x161)))+(((IkReal(0.00349770000000000))*(x60)))+(((IkReal(-0.000475162545000000))*(x69)))+(((IkReal(-1.00000000000000))*(x469)))+(((IkReal(-0.000719345000000000))*(x114)))+(((IkReal(9.77230182500000e-5))*(x84)))+(x515)+(((IkReal(-0.000719345000000000))*(x137)))+(((IkReal(-0.000475162545000000))*(x123)))+(((IkReal(0.000475162545000000))*(x73)))+(((IkReal(0.000475162545000000))*(x105)))+(((IkReal(0.00349770000000000))*(x98)))+(((IkReal(9.77230182500000e-5))*(x100))));
00899 op[8]=((((IkReal(-1.00000000000000))*(x534)))+(((IkReal(-1.00000000000000))*(x526)))+(((IkReal(-1.00000000000000))*(x525)))+(x538)+(x524));
00900 op[9]=((((IkReal(-1.00000000000000))*(x509)))+(x503)+(x537)+(x529)+(((IkReal(-1.00000000000000))*(x516))));
00901 op[10]=((IkReal(-2.00000000000000))+(((IkReal(-0.0872823000000000))*(x160)))+(((r01)*(x83)))+(((IkReal(-0.0872823000000000))*(x157)))+(((IkReal(1.33436934500000))*(x180)))+(((r00)*(x117)))+(((IkReal(-1.00000000000000))*(x510)))+(((IkReal(-0.0872823000000000))*(x126)))+(((IkReal(-0.0872823000000000))*(x127)))+(((r11)*(x166))));
00902 op[11]=((((IkReal(-1.00000000000000))*(x503)))+(x509)+(x537)+(x529)+(((IkReal(-1.00000000000000))*(x516))));
00903 op[12]=((IkReal(-2.00000000000000))+(x508)+(((IkReal(-1.00000000000000))*(x512)))+(((IkReal(-1.00000000000000))*(x510))));
00904 op[13]=((((r11)*(x96)))+(((IkReal(0.106800000000000))*(x130)))+(((r00)*(x124)))+(((IkReal(-1.00000000000000))*(r11)*(x68)))+(((IkReal(-0.109911310000000))*(x58)))+(((r10)*(x167)))+(((IkReal(-0.109911310000000))*(x147)))+(((IkReal(-0.109911310000000))*(x179))));
00905 op[14]=((IkReal(2.00000000000000))+(((IkReal(-1.00000000000000))*(x508)))+(x512)+(x510));
00906 op[15]=((((IkReal(-1.00000000000000))*(x499)))+(((IkReal(-1.00000000000000))*(x496)))+(x497)+(x509)+(x537)+(((IkReal(-1.00000000000000))*(x516))));
00907 op[16]=((IkReal(2.00000000000000))+(((IkReal(0.00349770000000000))*(x160)))+(((IkReal(0.00349770000000000))*(x127)))+(((IkReal(0.00349770000000000))*(x126)))+(((IkReal(-1.00000000000000))*(r11)*(x132)))+(((IkReal(-0.000719345000000000))*(x173)))+(((IkReal(-0.000719345000000000))*(x180)))+(x510)+(((IkReal(-0.000719345000000000))*(x128)))+(((IkReal(0.00349770000000000))*(x157))));
00908 op[17]=((((IkReal(-1.00000000000000))*(x499)))+(((IkReal(-1.00000000000000))*(x509)))+(x496)+(x497)+(x537)+(((IkReal(-1.00000000000000))*(x516))));
00909 op[18]=((((IkReal(-1.00000000000000))*(x490)))+(((IkReal(-1.00000000000000))*(x480)))+(((IkReal(-1.00000000000000))*(x531)))+(x501)+(x502)+(x530)+(((IkReal(-1.00000000000000))*(x513))));
00910 op[19]=((((IkReal(0.0872823000000000))*(x73)))+(((r11)*(x83)))+(((IkReal(1.33436934500000))*(x100)))+(((IkReal(0.0872823000000000))*(x105)))+(((IkReal(-0.0872823000000000))*(x69)))+(((IkReal(-1.00000000000000))*(x477)))+(x475)+(((IkReal(-1.33436934500000))*(x161)))+(((IkReal(-1.33436934500000))*(x67)))+(((IkReal(-0.0872823000000000))*(x123))));
00911 op[20]=((((IkReal(-1.00000000000000))*(x502)))+(((IkReal(-1.00000000000000))*(x531)))+(x480)+(x490)+(x501)+(x530)+(((IkReal(-1.00000000000000))*(x513))));
00912 op[21]=((x518)+(((IkReal(-1.00000000000000))*(x517))));
00913 op[22]=((((IkReal(-0.106800000000000))*(x165)))+(((IkReal(0.109911310000000))*(x81)))+(((IkReal(-0.109911310000000))*(x118)))+(((IkReal(-0.109911310000000))*(x74)))+(((r01)*(x68)))+(((IkReal(-1.00000000000000))*(r01)*(x96)))+(((IkReal(0.106800000000000))*(x153)))+(((r10)*(x124))));
00914 op[23]=((x517)+(((IkReal(-1.00000000000000))*(x518))));
00915 op[24]=((((IkReal(-1.00000000000000))*(x495)))+(((IkReal(-1.00000000000000))*(x494)))+(((IkReal(-1.00000000000000))*(x531)))+(x481)+(x480)+(x498)+(x530));
00916 op[25]=((((IkReal(0.00349770000000000))*(x69)))+(((IkReal(-0.00349770000000000))*(x73)))+(((IkReal(0.00349770000000000))*(x123)))+(((IkReal(-1.00000000000000))*(x475)))+(((r01)*(x132)))+(((IkReal(-0.000719345000000000))*(x84)))+(((IkReal(-0.000719345000000000))*(x100)))+(x477)+(((IkReal(-0.00349770000000000))*(x105)))+(((IkReal(0.000719345000000000))*(x67))));
00917 op[26]=((((IkReal(-1.00000000000000))*(x495)))+(((IkReal(-1.00000000000000))*(x480)))+(((IkReal(-1.00000000000000))*(x481)))+(((IkReal(-1.00000000000000))*(x531)))+(x498)+(x494)+(x530));
00918 op[27]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x500)))+(((IkReal(-1.00000000000000))*(x470)))+(((IkReal(-1.00000000000000))*(x527)))+(x476)+(x471));
00919 op[28]=((((IkReal(0.0872823000000000))*(x79)))+(((IkReal(-1.33436934500000))*(x99)))+(((IkReal(0.0872823000000000))*(x136)))+(((IkReal(-1.33436934500000))*(x110)))+(x487));
00920 op[29]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x500)))+(((IkReal(-1.00000000000000))*(x476)))+(((IkReal(-1.00000000000000))*(x527)))+(x470)+(x471));
00921 op[30]=((((IkReal(-1.00000000000000))*(x468)))+(x493));
00922 op[31]=((IkReal(-4.00000000000000))+(((IkReal(-0.106800000000000))*(x162)))+(((IkReal(0.109911310000000))*(x115)))+(((IkReal(-0.106800000000000))*(x150)))+(((IkReal(0.109911310000000))*(x76))));
00923 op[32]=((((IkReal(-1.00000000000000))*(x493)))+(x468));
00924 op[33]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x476)))+(((IkReal(-1.00000000000000))*(x472)))+(((IkReal(-1.00000000000000))*(x527)))+(x482)+(x471)+(x473));
00925 op[34]=((((IkReal(-1.00000000000000))*(x487)))+(((IkReal(-0.00349770000000000))*(x79)))+(((IkReal(0.000719345000000000))*(x99)))+(((IkReal(-0.00349770000000000))*(x136)))+(((IkReal(0.000719345000000000))*(x110))));
00926 op[35]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x473)))+(((IkReal(-1.00000000000000))*(x472)))+(((IkReal(-1.00000000000000))*(x527)))+(x482)+(x476)+(x471));
00927 op[36]=((IkReal(-0.817250000000000))+(((IkReal(-1.00000000000000))*(x507)))+(((IkReal(-1.00000000000000))*(x479)))+(((IkReal(-1.00000000000000))*(x478)))+(((IkReal(-1.00000000000000))*(x532)))+(x505)+(x535));
00928 op[37]=((((IkReal(0.0872823000000000))*(x77)))+(((IkReal(-0.0872823000000000))*(x138)))+(((IkReal(-1.00000000000000))*(x484)))+(((IkReal(0.0872823000000000))*(x64)))+(((IkReal(-0.0872823000000000))*(x159)))+(((rxp1_1)*(x83)))+(x489)+(((IkReal(-1.33436934500000))*(x102)))+(((rxp0_1)*(x117)))+(((IkReal(-1.00000000000000))*(rxp1_0)*(x166))));
00929 op[38]=((IkReal(0.817250000000000))+(((IkReal(-1.00000000000000))*(x507)))+(((IkReal(-1.00000000000000))*(x505)))+(((IkReal(-1.00000000000000))*(x532)))+(x478)+(x479)+(x535));
00930 op[39]=((x520)+(((IkReal(-1.00000000000000))*(x519))));
00931 op[40]=((IkReal(1.70000000000000))+(((IkReal(0.106800000000000))*(x106)))+(((IkReal(-1.00000000000000))*(rxp1_0)*(x96)))+(((rxp0_1)*(x124)))+(((rxp1_0)*(x68)))+(((IkReal(0.109911310000000))*(x145)))+(((IkReal(-0.109911310000000))*(x142)))+(((IkReal(-0.109911310000000))*(x146)))+(((IkReal(-1.00000000000000))*(rxp0_0)*(x167))));
00932 op[41]=((((IkReal(-1.00000000000000))*(x520)))+(x519));
00933 op[42]=((IkReal(0.0327500000000000))+(((IkReal(-1.00000000000000))*(x532)))+(x488)+(x478)+(x536)+(((IkReal(-1.00000000000000))*(x514)))+(((IkReal(-1.00000000000000))*(x511))));
00934 op[43]=((((IkReal(-0.00349770000000000))*(x64)))+(((rxp1_0)*(x132)))+(((IkReal(-1.00000000000000))*(x489)))+(((IkReal(-0.00349770000000000))*(x77)))+(((IkReal(-0.000719345000000000))*(x174)))+(((IkReal(0.00349770000000000))*(x138)))+(((IkReal(-0.000719345000000000))*(x163)))+(x484)+(((IkReal(0.000719345000000000))*(x102)))+(((IkReal(0.00349770000000000))*(x159))));
00935 op[44]=((IkReal(-0.0327500000000000))+(((IkReal(-1.00000000000000))*(x488)))+(((IkReal(-1.00000000000000))*(x478)))+(((IkReal(-1.00000000000000))*(x532)))+(x536)+(x514)+(((IkReal(-1.00000000000000))*(x511))));
00936 op[45]=((((IkReal(-1.00000000000000))*(x491)))+(((IkReal(-1.00000000000000))*(x492)))+(x485)+(x504)+(x528));
00937 op[46]=((((IkReal(-0.00826126969500000))*(x136)))+(((IkReal(-1.00000000000000))*(x474)))+(((IkReal(0.126298058504250))*(x110)))+(((IkReal(0.126298058504250))*(x99)))+(((IkReal(-0.00826126969500000))*(x79))));
00938 op[47]=((((IkReal(-1.00000000000000))*(x492)))+(((IkReal(-1.00000000000000))*(x485)))+(x491)+(x504)+(x528));
00939 op[48]=((IkReal(0.784500000000000))+(((IkReal(-1.00000000000000))*(x483)))+(((IkReal(-1.00000000000000))*(x474)))+(x486));
00940 op[49]=((((IkReal(-0.0104031054915000))*(x76)))+(((IkReal(0.0101086200000000))*(x162)))+(((IkReal(-0.0104031054915000))*(x115)))+(((IkReal(0.0101086200000000))*(x150))));
00941 op[50]=((IkReal(0.784500000000000))+(((IkReal(-1.00000000000000))*(x486)))+(x483)+(x474));
00942 op[51]=((((IkReal(-1.00000000000000))*(x492)))+(((IkReal(-0.00242572735012500))*(x97)))+(((IkReal(-0.00242572735012500))*(x156)))+(x491)+(x528)+(((IkReal(0.00309978750000000))*(x59)))+(((IkReal(-0.000168993079125000))*(x76)))+(((IkReal(0.00309978750000000))*(x70)))+(((IkReal(-0.000168993079125000))*(x115))));
00943 op[52]=((((IkReal(0.000331057305000000))*(x79)))+(x474)+(((IkReal(-6.80860042500000e-5))*(x110)))+(((IkReal(-6.80860042500000e-5))*(x99)))+(((IkReal(0.000331057305000000))*(x136))));
00944 IkReal x540=((gconst7_7)*(r01));
00945 IkReal x541=((gconst0_3)*(r20));
00946 IkReal x542=((gconst1_3)*(r21));
00947 IkReal x543=((gconst4_0)*(r11));
00948 IkReal x544=((gconst0_1)*(npx));
00949 IkReal x545=((gconst1_3)*(npy));
00950 IkReal x546=((gconst4_0)*(r01));
00951 IkReal x547=((gconst6_7)*(r10));
00952 IkReal x548=((gconst0_7)*(npx));
00953 IkReal x549=((gconst6_0)*(r10));
00954 IkReal x550=((gconst7_0)*(r01));
00955 IkReal x551=((IkReal(0.0257164050000000))*(r01));
00956 IkReal x552=((gconst4_7)*(pz));
00957 IkReal x553=((gconst3_0)*(r00));
00958 IkReal x554=((gconst4_7)*(r11));
00959 IkReal x555=((gconst3_0)*(r10));
00960 IkReal x556=((gconst6_3)*(r10));
00961 IkReal x557=((gconst0_0)*(r20));
00962 IkReal x558=((gconst3_2)*(r00));
00963 IkReal x559=((gconst4_1)*(r01));
00964 IkReal x560=((IkReal(2.66873869000000))*(gconst7_7));
00965 IkReal x561=((gconst7_7)*(r11));
00966 IkReal x562=((gconst4_2)*(r01));
00967 IkReal x563=((IkReal(0.135850000000000))*(pp));
00968 IkReal x564=((gconst0_6)*(npx));
00969 IkReal x565=((gconst0_5)*(npx));
00970 IkReal x566=((IkReal(0.189300000000000))*(pz));
00971 IkReal x567=((gconst3_7)*(r00));
00972 IkReal x568=((gconst7_0)*(r11));
00973 IkReal x569=((gconst4_7)*(r01));
00974 IkReal x570=((gconst6_1)*(r10));
00975 IkReal x571=((gconst1_0)*(npy));
00976 IkReal x572=((gconst6_2)*(r10));
00977 IkReal x573=((gconst1_2)*(npy));
00978 IkReal x574=((IkReal(0.0257164050000000))*(pz));
00979 IkReal x575=((gconst4_3)*(r01));
00980 IkReal x576=((gconst0_6)*(r20));
00981 IkReal x577=((IkReal(0.213600000000000))*(gconst6_6));
00982 IkReal x578=((IkReal(0.106800000000000))*(gconst4_5));
00983 IkReal x579=((gconst0_2)*(r20));
00984 IkReal x580=((gconst1_1)*(npy));
00985 IkReal x581=((gconst0_3)*(npx));
00986 IkReal x582=((gconst1_6)*(r21));
00987 IkReal x583=((gconst3_7)*(r10));
00988 IkReal x584=((gconst1_7)*(npy));
00989 IkReal x585=((gconst1_7)*(r21));
00990 IkReal x586=((gconst3_1)*(r00));
00991 IkReal x587=((gconst6_1)*(r00));
00992 IkReal x588=((IkReal(1.56900000000000))*(gconst4_3));
00993 IkReal x589=((IkReal(0.817250000000000))*(r11));
00994 IkReal x590=((IkReal(0.0267000000000000))*(gconst3_5));
00995 IkReal x591=((gconst0_2)*(npx));
00996 IkReal x592=((gconst7_2)*(r01));
00997 IkReal x593=((gconst0_7)*(r20));
00998 IkReal x594=((gconst6_6)*(r10));
00999 IkReal x595=((gconst0_0)*(npx));
01000 IkReal x596=((IkReal(0.0534000000000000))*(gconst3_6));
01001 IkReal x597=((gconst6_0)*(r00));
01002 IkReal x598=((gconst4_6)*(r01));
01003 IkReal x599=((gconst6_7)*(r00));
01004 IkReal x600=((gconst3_3)*(r10));
01005 IkReal x601=((gconst0_5)*(r20));
01006 IkReal x602=((gconst7_1)*(r11));
01007 IkReal x603=((gconst6_3)*(r00));
01008 IkReal x604=((IkReal(0.106800000000000))*(gconst6_5));
01009 IkReal x605=((gconst6_5)*(r10));
01010 IkReal x606=((gconst3_1)*(r10));
01011 IkReal x607=((IkReal(0.106800000000000))*(gconst7_5));
01012 IkReal x608=((gconst3_2)*(r10));
01013 IkReal x609=((gconst3_6)*(r00));
01014 IkReal x610=((gconst6_2)*(r00));
01015 IkReal x611=((gconst3_5)*(r00));
01016 IkReal x612=((gconst7_4)*(r11));
01017 IkReal x613=((gconst3_3)*(r00));
01018 IkReal x614=((IkReal(1.56900000000000))*(gconst7_3));
01019 IkReal x615=((IkReal(0.0534000000000000))*(gconst7_6));
01020 IkReal x616=((gconst1_6)*(npy));
01021 IkReal x617=((gconst7_6)*(r11));
01022 IkReal x618=((gconst7_2)*(r11));
01023 IkReal x619=((gconst7_3)*(r11));
01024 IkReal x620=((gconst7_0)*(rxp1_1));
01025 IkReal x621=((gconst6_7)*(rxp0_1));
01026 IkReal x622=((gconst4_6)*(r11));
01027 IkReal x623=((IkReal(1.00000000000000))*(pp));
01028 IkReal x624=((IkReal(0.213600000000000))*(gconst3_6));
01029 IkReal x625=((gconst4_5)*(r01));
01030 IkReal x626=((gconst7_5)*(r11));
01031 IkReal x627=((gconst3_0)*(rxp0_0));
01032 IkReal x628=((IkReal(0.0946500000000000))*(pp));
01033 IkReal x629=((gconst3_7)*(rxp0_0));
01034 IkReal x630=((gconst4_2)*(r11));
01035 IkReal x631=((gconst7_1)*(r01));
01036 IkReal x632=((gconst6_4)*(r10));
01037 IkReal x633=((gconst6_0)*(rxp0_1));
01038 IkReal x634=((gconst1_2)*(r21));
01039 IkReal x635=((IkReal(0.109150000000000))*(r01));
01040 IkReal x636=((gconst1_0)*(r21));
01041 IkReal x637=((gconst1_5)*(r21));
01042 IkReal x638=((gconst1_5)*(npy));
01043 IkReal x639=((gconst4_7)*(rxp1_0));
01044 IkReal x640=((IkReal(0.106800000000000))*(gconst3_5));
01045 IkReal x641=((gconst4_1)*(r11));
01046 IkReal x642=((gconst3_4)*(r00));
01047 IkReal x643=((IkReal(0.213600000000000))*(gconst7_6));
01048 IkReal x644=((IkReal(0.0179172450000000))*(pz));
01049 IkReal x645=((gconst4_0)*(rxp1_0));
01050 IkReal x646=((gconst4_6)*(rxp1_0));
01051 IkReal x647=((gconst7_3)*(r01));
01052 IkReal x648=((IkReal(0.0534000000000000))*(gconst6_6));
01053 IkReal x649=((gconst7_7)*(rxp1_1));
01054 IkReal x650=((IkReal(0.0145087800000000))*(x611));
01055 IkReal x651=((IkReal(0.106800000000000))*(x638));
01056 IkReal x652=((IkReal(0.106800000000000))*(x565));
01057 IkReal x653=((IkReal(0.0145087800000000))*(x625));
01058 IkReal x654=((IkReal(0.0145087800000000))*(x605));
01059 IkReal x655=((IkReal(0.0145087800000000))*(x626));
01060 IkReal x656=((IkReal(0.0141566826115000))*(x561));
01061 IkReal x657=((IkReal(0.213148650000000))*(x619));
01062 IkReal x658=((IkReal(0.213148650000000))*(x556));
01063 IkReal x659=((IkReal(0.0141566826115000))*(x547));
01064 IkReal x660=((IkReal(0.104208190000000))*(x548));
01065 IkReal x661=((IkReal(0.213148650000000))*(x575));
01066 IkReal x662=((IkReal(0.213148650000000))*(x613));
01067 IkReal x663=((IkReal(0.0141566826115000))*(x567));
01068 IkReal x664=((IkReal(0.104208190000000))*(x584));
01069 IkReal x665=((IkReal(1.56900000000000))*(x545));
01070 IkReal x666=((IkReal(0.0141566826115000))*(x569));
01071 IkReal x667=((IkReal(1.56900000000000))*(x581));
01072 IkReal x668=((IkReal(0.00725439000000000))*(x609));
01073 IkReal x669=((IkReal(0.00725439000000000))*(x598));
01074 IkReal x670=((IkReal(0.0534000000000000))*(x564));
01075 IkReal x671=((IkReal(0.0534000000000000))*(x616));
01076 IkReal x672=((IkReal(0.00725439000000000))*(x617));
01077 IkReal x673=((IkReal(0.00725439000000000))*(x594));
01078 IkReal x674=((IkReal(0.0534000000000000))*(x622));
01079 IkReal x675=((r01)*(x615));
01080 IkReal x676=((IkReal(1.33722090500000))*(x597));
01081 IkReal x677=((IkReal(1.33722090500000))*(x543));
01082 IkReal x678=((r00)*(x648));
01083 IkReal x679=((r10)*(x596));
01084 IkReal x680=((IkReal(1.33722090500000))*(x555));
01085 IkReal x681=((IkReal(1.33722090500000))*(x550));
01086 IkReal x682=((r10)*(x640));
01087 IkReal x683=((r00)*(x604));
01088 IkReal x684=((r11)*(x578));
01089 IkReal x685=((r01)*(x607));
01090 IkReal x686=((IkReal(0.104208190000000))*(x540));
01091 IkReal x687=((IkReal(1.56900000000000))*(x603));
01092 IkReal x688=((IkReal(0.104208190000000))*(x583));
01093 IkReal x689=((IkReal(0.104208190000000))*(x554));
01094 IkReal x690=((r01)*(x614));
01095 IkReal x691=((IkReal(0.104208190000000))*(x599));
01096 IkReal x692=((r11)*(x588));
01097 IkReal x693=((IkReal(1.56900000000000))*(x600));
01098 IkReal x694=((IkReal(0.00357090500000000))*(x550));
01099 IkReal x695=((IkReal(0.00357090500000000))*(x597));
01100 IkReal x696=((IkReal(0.00357090500000000))*(x555));
01101 IkReal x697=((IkReal(0.00357090500000000))*(x543));
01102 IkReal x698=((r10)*(x604));
01103 IkReal x699=((r11)*(x607));
01104 IkReal x700=((r01)*(x578));
01105 IkReal x701=((IkReal(0.106800000000000))*(x611));
01106 IkReal x702=((IkReal(0.104208190000000))*(x567));
01107 IkReal x703=((IkReal(0.104208190000000))*(x569));
01108 IkReal x704=((IkReal(1.56900000000000))*(x613));
01109 IkReal x705=((IkReal(1.56900000000000))*(x575));
01110 IkReal x706=((IkReal(1.56900000000000))*(x556));
01111 IkReal x707=((r11)*(x614));
01112 IkReal x708=((IkReal(0.104208190000000))*(x547));
01113 IkReal x709=((IkReal(0.104208190000000))*(x561));
01114 IkReal x710=((IkReal(0.0534000000000000))*(x594));
01115 IkReal x711=((r11)*(x615));
01116 IkReal x712=((IkReal(0.0534000000000000))*(x598));
01117 IkReal x713=((r00)*(x596));
01118 IkReal x714=((IkReal(0.106800000000000))*(x637));
01119 IkReal x715=((IkReal(0.106800000000000))*(x601));
01120 IkReal x716=((IkReal(0.104208190000000))*(x593));
01121 IkReal x717=((IkReal(1.56900000000000))*(x541));
01122 IkReal x718=((IkReal(0.104208190000000))*(x585));
01123 IkReal x719=((IkReal(1.56900000000000))*(x542));
01124 IkReal x720=((IkReal(0.0534000000000000))*(x576));
01125 IkReal x721=((IkReal(0.0534000000000000))*(x582));
01126 IkReal x722=((rxp1_1)*(x607));
01127 IkReal x723=((rxp0_1)*(x604));
01128 IkReal x724=((rxp1_0)*(x578));
01129 IkReal x725=((rxp0_0)*(x640));
01130 IkReal x726=((IkReal(1.56900000000000))*(gconst3_3)*(rxp0_0));
01131 IkReal x727=((IkReal(0.104208190000000))*(x639));
01132 IkReal x728=((IkReal(0.104208190000000))*(x629));
01133 IkReal x729=((rxp1_0)*(x588));
01134 IkReal x730=((IkReal(0.104208190000000))*(x649));
01135 IkReal x731=((IkReal(0.104208190000000))*(x621));
01136 IkReal x732=((IkReal(1.56900000000000))*(gconst6_3)*(rxp0_1));
01137 IkReal x733=((rxp1_1)*(x614));
01138 IkReal x734=((rxp1_1)*(x615));
01139 IkReal x735=((rxp0_1)*(x648));
01140 IkReal x736=((rxp0_0)*(x596));
01141 IkReal x737=((IkReal(0.0534000000000000))*(x646));
01142 IkReal x738=((IkReal(0.126567958658250))*(x557));
01143 IkReal x739=((IkReal(0.126567958658250))*(x636));
01144 IkReal x740=((IkReal(0.00505431000000000))*(x576));
01145 IkReal x741=((IkReal(0.00505431000000000))*(x582));
01146 IkReal x742=((IkReal(0.0101086200000000))*(x637));
01147 IkReal x743=((IkReal(0.0101086200000000))*(x601));
01148 IkReal x744=((IkReal(0.148505850000000))*(x541));
01149 IkReal x745=((IkReal(0.00986330518350000))*(x585));
01150 IkReal x746=((IkReal(0.00986330518350000))*(x593));
01151 IkReal x747=((IkReal(0.148505850000000))*(x542));
01152 IkReal x748=((IkReal(0.000337986158250000))*(x557));
01153 IkReal x749=((IkReal(0.000337986158250000))*(x636));
01154 IkReal x750=((IkReal(0.111023412500000))*(x556));
01155 IkReal x751=((IkReal(0.0148280275000000))*(gconst4_4)*(r01));
01156 IkReal x752=((IkReal(0.0257164050000000))*(x544));
01157 IkReal x753=((IkReal(0.00349357361925000))*(x559));
01158 IkReal x754=((IkReal(0.00362719500000000))*(x605));
01159 IkReal x755=((IkReal(0.668610452500000))*(x548));
01160 IkReal x756=((IkReal(0.641196607500000))*(x573));
01161 IkReal x757=((IkReal(0.0871065591288750))*(x558));
01162 IkReal x758=((IkReal(0.0257164050000000))*(x580));
01163 IkReal x759=((IkReal(0.0871065591288750))*(x562));
01164 IkReal x760=((IkReal(0.111023412500000))*(x619));
01165 IkReal x761=((IkReal(0.0148280275000000))*(x642));
01166 IkReal x762=((IkReal(0.00362719500000000))*(x626));
01167 IkReal x763=((IkReal(0.641196607500000))*(x591));
01168 IkReal x764=((IkReal(0.00349357361925000))*(x586));
01169 IkReal x765=((IkReal(0.0908307299721250))*(x567));
01170 IkReal x766=((IkReal(0.109150000000000))*(gconst1_4)*(npy));
01171 IkReal x767=((IkReal(0.0908307299721250))*(x569));
01172 IkReal x768=((IkReal(0.668610452500000))*(x584));
01173 IkReal x769=((IkReal(0.109150000000000))*(gconst0_4)*(npx));
01174 IkReal x770=((IkReal(0.0148280275000000))*(x632));
01175 IkReal x771=((IkReal(0.0871065591288750))*(x618));
01176 IkReal x772=((IkReal(0.00362719500000000))*(x625));
01177 IkReal x773=((IkReal(0.817250000000000))*(x545));
01178 IkReal x774=((IkReal(0.0267000000000000))*(x565));
01179 IkReal x775=((IkReal(0.0267000000000000))*(x638));
01180 IkReal x776=((IkReal(0.00362719500000000))*(x611));
01181 IkReal x777=((IkReal(0.0148280275000000))*(x612));
01182 IkReal x778=((IkReal(0.111023412500000))*(x613));
01183 IkReal x779=((IkReal(0.0871065591288750))*(x572));
01184 IkReal x780=((IkReal(0.00349357361925000))*(x602));
01185 IkReal x781=((IkReal(0.0908307299721250))*(x547));
01186 IkReal x782=((IkReal(0.111023412500000))*(x575));
01187 IkReal x783=((IkReal(0.817250000000000))*(x581));
01188 IkReal x784=((IkReal(0.00349357361925000))*(x570));
01189 IkReal x785=((IkReal(0.0908307299721250))*(x561));
01190 IkReal x786=((IkReal(0.00616623150000000))*(x586));
01191 IkReal x787=((IkReal(0.0453900000000000))*(x544));
01192 IkReal x788=((IkReal(0.0521040950000000))*(x571));
01193 IkReal x789=((IkReal(0.00707834130575000))*(x546));
01194 IkReal x790=((IkReal(0.00707834130575000))*(x553));
01195 IkReal x791=((IkReal(0.0521040950000000))*(x595));
01196 IkReal x792=((IkReal(0.00616623150000000))*(x559));
01197 IkReal x793=((IkReal(0.0453900000000000))*(x580));
01198 IkReal x794=((IkReal(0.00616623150000000))*(x570));
01199 IkReal x795=((IkReal(0.00707834130575000))*(x568));
01200 IkReal x796=((IkReal(0.00707834130575000))*(x549));
01201 IkReal x797=((IkReal(0.00616623150000000))*(x602));
01202 IkReal x798=((IkReal(0.000242553722125000))*(x561));
01203 IkReal x799=((IkReal(0.00348161712112500))*(x618));
01204 IkReal x800=((IkReal(0.000242553722125000))*(x547));
01205 IkReal x801=((IkReal(0.00348161712112500))*(x572));
01206 IkReal x802=((IkReal(0.00444908750000000))*(x619));
01207 IkReal x803=((IkReal(0.00444908750000000))*(x556));
01208 IkReal x804=((IkReal(0.00444908750000000))*(x575));
01209 IkReal x805=((IkReal(0.00348161712112500))*(x562));
01210 IkReal x806=((IkReal(0.00444908750000000))*(x613));
01211 IkReal x807=((IkReal(0.0327500000000000))*(x581));
01212 IkReal x808=((IkReal(0.0256283925000000))*(x591));
01213 IkReal x809=((IkReal(0.000242553722125000))*(x567));
01214 IkReal x810=((IkReal(0.000242553722125000))*(x569));
01215 IkReal x811=((IkReal(0.0256283925000000))*(x573));
01216 IkReal x812=((IkReal(0.00178545250000000))*(x584));
01217 IkReal x813=((IkReal(0.00178545250000000))*(x548));
01218 IkReal x814=((IkReal(0.00348161712112500))*(x558));
01219 IkReal x815=((IkReal(0.0327500000000000))*(x545));
01220 IkReal x816=((IkReal(0.109150000000000))*(gconst6_4)*(r00));
01221 IkReal x817=((IkReal(0.668610452500000))*(x599));
01222 IkReal x818=((IkReal(0.668610452500000))*(x540));
01223 IkReal x819=((IkReal(0.668610452500000))*(x554));
01224 IkReal x820=((IkReal(0.109150000000000))*(gconst4_4)*(r11));
01225 IkReal x821=((IkReal(0.0257164050000000))*(x641));
01226 IkReal x822=((IkReal(0.0257164050000000))*(x606));
01227 IkReal x823=((IkReal(0.641196607500000))*(x608));
01228 IkReal x824=((IkReal(0.641196607500000))*(x630));
01229 IkReal x825=((IkReal(0.0257164050000000))*(x587));
01230 IkReal x826=((IkReal(0.668610452500000))*(x583));
01231 IkReal x827=((gconst7_1)*(x551));
01232 IkReal x828=((gconst7_4)*(x635));
01233 IkReal x829=((IkReal(0.641196607500000))*(x610));
01234 IkReal x830=((IkReal(0.641196607500000))*(x592));
01235 IkReal x831=((IkReal(0.109150000000000))*(gconst3_4)*(r10));
01236 IkReal x832=((IkReal(0.817250000000000))*(x603));
01237 IkReal x833=((IkReal(0.0267000000000000))*(gconst6_5)*(r00));
01238 IkReal x834=((IkReal(0.817250000000000))*(x647));
01239 IkReal x835=((r10)*(x590));
01240 IkReal x836=((gconst4_3)*(x589));
01241 IkReal x837=((IkReal(0.817250000000000))*(x600));
01242 IkReal x838=((IkReal(0.0267000000000000))*(gconst4_5)*(r11));
01243 IkReal x839=((IkReal(0.0267000000000000))*(gconst7_5)*(r01));
01244 IkReal x840=((IkReal(0.0453900000000000))*(x641));
01245 IkReal x841=((IkReal(0.0521040950000000))*(x597));
01246 IkReal x842=((IkReal(0.0521040950000000))*(x555));
01247 IkReal x843=((IkReal(0.0453900000000000))*(x631));
01248 IkReal x844=((IkReal(0.0521040950000000))*(x550));
01249 IkReal x845=((IkReal(0.0521040950000000))*(x543));
01250 IkReal x846=((IkReal(0.0453900000000000))*(x587));
01251 IkReal x847=((IkReal(0.0453900000000000))*(x606));
01252 IkReal x848=((IkReal(0.0327500000000000))*(gconst4_3)*(r11));
01253 IkReal x849=((IkReal(0.0256283925000000))*(x592));
01254 IkReal x850=((IkReal(0.0256283925000000))*(x608));
01255 IkReal x851=((IkReal(0.00178545250000000))*(x599));
01256 IkReal x852=((IkReal(0.00178545250000000))*(x540));
01257 IkReal x853=((IkReal(0.00178545250000000))*(x583));
01258 IkReal x854=((IkReal(0.00178545250000000))*(x554));
01259 IkReal x855=((IkReal(0.0327500000000000))*(x647));
01260 IkReal x856=((IkReal(0.0327500000000000))*(x603));
01261 IkReal x857=((IkReal(0.0256283925000000))*(x630));
01262 IkReal x858=((IkReal(0.0256283925000000))*(x610));
01263 IkReal x859=((IkReal(0.0327500000000000))*(x600));
01264 IkReal x860=((IkReal(0.668610452500000))*(x561));
01265 IkReal x861=((IkReal(0.668610452500000))*(x547));
01266 IkReal x862=((pp)*(x562));
01267 IkReal x863=((r00)*(x590));
01268 IkReal x864=((IkReal(0.817250000000000))*(x575));
01269 IkReal x865=((pp)*(x558));
01270 IkReal x866=((IkReal(0.817250000000000))*(x613));
01271 IkReal x867=((IkReal(0.0257164050000000))*(x570));
01272 IkReal x868=((IkReal(0.109150000000000))*(x612));
01273 IkReal x869=((IkReal(0.641196607500000))*(x618));
01274 IkReal x870=((IkReal(0.0257164050000000))*(x602));
01275 IkReal x871=((IkReal(0.0267000000000000))*(x625));
01276 IkReal x872=((IkReal(0.641196607500000))*(x572));
01277 IkReal x873=((IkReal(0.109150000000000))*(x632));
01278 IkReal x874=((IkReal(0.0267000000000000))*(x605));
01279 IkReal x875=((IkReal(0.668610452500000))*(x569));
01280 IkReal x876=((gconst7_3)*(x589));
01281 IkReal x877=((IkReal(0.817250000000000))*(x556));
01282 IkReal x878=((gconst4_1)*(x551));
01283 IkReal x879=((gconst4_4)*(x635));
01284 IkReal x880=((IkReal(0.641196607500000))*(x562));
01285 IkReal x881=((IkReal(0.0257164050000000))*(x586));
01286 IkReal x882=((IkReal(0.641196607500000))*(x558));
01287 IkReal x883=((IkReal(0.668610452500000))*(x567));
01288 IkReal x884=((IkReal(0.109150000000000))*(x642));
01289 IkReal x885=((IkReal(0.0267000000000000))*(x626));
01290 IkReal x886=((x548)*(x566));
01291 IkReal x887=((x551)*(x552));
01292 IkReal x888=((x567)*(x574));
01293 IkReal x889=((x566)*(x584));
01294 IkReal x890=((x563)*(x572));
01295 IkReal x891=((x563)*(x618));
01296 IkReal x892=((x561)*(x574));
01297 IkReal x893=((x547)*(x574));
01298 IkReal x894=((x562)*(x563));
01299 IkReal x895=((x573)*(x623));
01300 IkReal x896=((x591)*(x623));
01301 IkReal x897=((x558)*(x563));
01302 IkReal x898=((x566)*(x599));
01303 IkReal x899=((IkReal(0.189300000000000))*(r11)*(x552));
01304 IkReal x900=((x566)*(x583));
01305 IkReal x901=((x540)*(x566));
01306 IkReal x902=((x623)*(x630));
01307 IkReal x903=((x610)*(x623));
01308 IkReal x904=((x592)*(x623));
01309 IkReal x905=((x608)*(x623));
01310 IkReal x906=((x547)*(x566));
01311 IkReal x907=((x561)*(x566));
01312 IkReal x908=((IkReal(0.189300000000000))*(r01)*(x552));
01313 IkReal x909=((x566)*(x567));
01314 IkReal x910=((x572)*(x623));
01315 IkReal x911=((x618)*(x623));
01316 IkReal x912=((x798)+(x800));
01317 IkReal x913=((x754)+(x762));
01318 IkReal x914=((x672)+(x673));
01319 IkReal x915=((x700)+(x701));
01320 IkReal x916=((x654)+(x655));
01321 IkReal x917=((x720)+(x721));
01322 IkReal x918=((x781)+(x785));
01323 IkReal x919=((x747)+(x744));
01324 IkReal x920=((x704)+(x705));
01325 IkReal x921=((x718)+(x716));
01326 IkReal x922=((x719)+(x717));
01327 IkReal x923=((x737)+(x736));
01328 IkReal x924=((x702)+(x703));
01329 IkReal x925=((x724)+(x725));
01330 IkReal x926=((x727)+(x728));
01331 IkReal x927=((x733)+(x732));
01332 IkReal x928=((x722)+(x723));
01333 IkReal x929=((x726)+(x729));
01334 IkReal x930=((x731)+(x730));
01335 IkReal x931=((x711)+(x710));
01336 IkReal x932=((x706)+(x707));
01337 IkReal x933=((x715)+(x714));
01338 IkReal x934=((x742)+(x743));
01339 IkReal x935=((x746)+(x745));
01340 IkReal x936=((x713)+(x712));
01341 IkReal x937=((x656)+(x659));
01342 IkReal x938=((x657)+(x658));
01343 IkReal x939=((x698)+(x699));
01344 IkReal x940=((x735)+(x734));
01345 IkReal x941=((IkReal(4.00000000000000))+(x933));
01346 IkReal x942=((((IkReal(0.181661459944250))*(x549)))+(x914)+(((IkReal(0.181661459944250))*(x568))));
01347 IkReal x943=((((IkReal(0.000485107444250000))*(x549)))+(((IkReal(0.000485107444250000))*(x568)))+(x914));
01348 IkReal x944=((((IkReal(1.33722090500000))*(x549)))+(((IkReal(1.33722090500000))*(x568)))+(x931));
01349 IkReal x945=((((IkReal(1.33722090500000))*(x546)))+(x936)+(((IkReal(1.33722090500000))*(x553))));
01350 IkReal x946=((((IkReal(0.00357090500000000))*(x553)))+(x936)+(((IkReal(0.00357090500000000))*(x546))));
01351 IkReal x947=((((IkReal(0.00357090500000000))*(x568)))+(x931)+(((IkReal(0.00357090500000000))*(x549))));
01352 IkReal x948=((((IkReal(1.33722090500000))*(x636)))+(x917)+(((IkReal(1.33722090500000))*(x557))));
01353 IkReal x949=((((IkReal(0.00357090500000000))*(x636)))+(((IkReal(0.00357090500000000))*(x557)))+(x917));
01354 IkReal x950=((((IkReal(1.33722090500000))*(x620)))+(((IkReal(1.33722090500000))*(x633)))+(x940));
01355 IkReal x951=((((IkReal(1.33722090500000))*(x645)))+(((IkReal(1.33722090500000))*(x627)))+(x923));
01356 IkReal x952=((((IkReal(0.00357090500000000))*(x627)))+(x923)+(((IkReal(0.00357090500000000))*(x645))));
01357 IkReal x953=((((IkReal(0.00357090500000000))*(x620)))+(((IkReal(0.00357090500000000))*(x633)))+(x940));
01358 IkReal x954=((x678)+(x679)+(x674)+(x675));
01359 IkReal x955=((x772)+(x776)+(x775)+(x774));
01360 IkReal x956=((x683)+(x682)+(x685)+(x684));
01361 IkReal x957=((x799)+(x801)+(x803)+(x802));
01362 IkReal x958=((x812)+(x813)+(x810)+(x809));
01363 IkReal x959=((x650)+(x651)+(x652)+(x653));
01364 IkReal x960=((x708)+(x709)+(x939));
01365 IkReal x961=((x863)+(x861)+(x860)+(x871));
01366 IkReal x962=((x687)+(x690)+(x692)+(x693));
01367 IkReal x963=((x689)+(x688)+(x686)+(x691));
01368 IkReal x964=((x739)+(x738)+(x740)+(x741));
01369 IkReal x965=((x835)+(x833)+(x838)+(x839));
01370 IkReal x966=((x874)+(x875)+(x885)+(x883));
01371 IkReal x967=((x755)+(x768)+(x765)+(x767));
01372 IkReal x968=((x740)+(x741)+(x748)+(x749));
01373 IkReal x969=((x669)+(x668)+(x670)+(x671));
01374 IkReal x970=((x795)+(x794)+(x797)+(x796));
01375 IkReal x971=((x852)+(x853)+(x851)+(x854));
01376 IkReal x972=((x661)+(x662)+(x665)+(x667));
01377 IkReal x973=((x829)+(x823)+(x824)+(x830));
01378 IkReal x974=((x660)+(x663)+(x664)+(x666));
01379 IkReal x975=((x676)+(x677)+(x681)+(x680));
01380 IkReal x976=((x818)+(x819)+(x817)+(x826));
01381 IkReal x977=((IkReal(2.00000000000000))+(x954));
01382 IkReal x978=((x757)+(x756)+(x750)+(x759)+(x760)+(x763));
01383 IkReal x979=((x773)+(x771)+(x779)+(x778)+(x782)+(x783));
01384 IkReal x980=((((IkReal(0.181661459944250))*(x553)))+(((IkReal(0.181661459944250))*(x546)))+(((IkReal(1.33722090500000))*(x595)))+(((IkReal(1.33722090500000))*(x571)))+(x969));
01385 IkReal x981=((((IkReal(0.000485107444250000))*(x546)))+(((IkReal(0.000485107444250000))*(x553)))+(((IkReal(0.00357090500000000))*(x595)))+(x969)+(((IkReal(0.00357090500000000))*(x571))));
01386 IkReal x982=((x814)+(x815)+(x811)+(x808)+(x805)+(x804)+(x807)+(x806));
01387 IkReal x983=((x791)+(x790)+(x793)+(x792)+(x786)+(x787)+(x788)+(x789));
01388 IkReal x984=((x694)+(x695)+(x696)+(x697)+(x954));
01389 IkReal x985=((x841)+(x840)+(x843)+(x842)+(x845)+(x844)+(x847)+(x846));
01390 IkReal x986=((x834)+(x836)+(x837)+(x832)+(x904)+(x905)+(x902)+(x903));
01391 IkReal x987=((x770)+(x777)+(x780)+(x784)+(x896)+(x897)+(x894)+(x895)+(x892)+(x893));
01392 IkReal x988=((x869)+(x868)+(x867)+(x866)+(x865)+(x864)+(x862)+(x870)+(x872)+(x873)+(x906)+(x907));
01393 IkReal x989=((x878)+(x879)+(x876)+(x877)+(x884)+(x881)+(x880)+(x882)+(x911)+(x910)+(x908)+(x909));
01394 IkReal x990=((x850)+(x856)+(x857)+(x855)+(x858)+(x859)+(x849)+(x848)+(x904)+(x905)+(x902)+(x903));
01395 IkReal x991=((x898)+(x899)+(x816)+(x828)+(x822)+(x821)+(x820)+(x827)+(x825)+(x831)+(x900)+(x901));
01396 IkReal x992=((IkReal(0.0946500000000000))+(x751)+(x753)+(x752)+(x758)+(x769)+(x764)+(x766)+(x761)+(x887)+(x886)+(x889)+(x888)+(x890)+(x891));
01397 op[53]=((((IkReal(-1.00000000000000))*(x579)*(x628)))+(((IkReal(0.00243405773325000))*(gconst1_1)*(r21)))+(((x585)*(x644)))+(((IkReal(-0.00252715500000000))*(x637)))+(((IkReal(-1.00000000000000))*(x628)*(x634)))+(((IkReal(0.0103310475000000))*(gconst1_4)*(r21)))+(((IkReal(-0.00242572735012500))*(x634)))+(((IkReal(0.00243405773325000))*(gconst0_1)*(r20)))+(((x593)*(x644)))+(((IkReal(-0.00242572735012500))*(x579)))+(((IkReal(0.000168993079125000))*(x585)))+(pz)+(((IkReal(0.0103310475000000))*(gconst0_4)*(r20)))+(((IkReal(-0.00252715500000000))*(x601)))+(((IkReal(0.00309978750000000))*(x541)))+(((IkReal(0.00309978750000000))*(x542)))+(((IkReal(0.000168993079125000))*(x593))));
01398 op[54]=((((IkReal(-1.00000000000000))*(x980)))+(x942));
01399 op[55]=((((IkReal(-1.00000000000000))*(x916)))+(((IkReal(-0.362548151036500))*(x561)))+(((IkReal(0.362548151036500))*(x569)))+(((IkReal(0.362548151036500))*(x567)))+(((IkReal(-0.362548151036500))*(x547)))+(x959)+(((IkReal(2.66873869000000))*(x584)))+(((IkReal(2.66873869000000))*(x548))));
01400 op[56]=((((IkReal(-1.00000000000000))*(x942)))+(x980));
01401 op[57]=((((IkReal(-1.00000000000000))*(x916)))+(x938)+(x974)+(x959)+(((IkReal(-1.00000000000000))*(x972)))+(((IkReal(-1.00000000000000))*(x937))));
01402 op[58]=((((IkReal(0.219822620000000))*(x571)))+(((IkReal(0.0298629029270000))*(x553)))+(((IkReal(-0.0290175600000000))*(x617)))+(((IkReal(-0.0298629029270000))*(x568)))+(((IkReal(0.0298629029270000))*(x546)))+(((IkReal(0.213600000000000))*(x616)))+(((IkReal(-0.0298629029270000))*(x549)))+(((IkReal(0.213600000000000))*(x564)))+(((IkReal(-0.0290175600000000))*(x594)))+(((IkReal(0.0290175600000000))*(x609)))+(((IkReal(0.219822620000000))*(x595)))+(((IkReal(0.0290175600000000))*(x598))));
01403 op[59]=((((IkReal(-1.00000000000000))*(x959)))+(x938)+(x937)+(x916)+(((IkReal(-1.00000000000000))*(x974)))+(((IkReal(-1.00000000000000))*(x972))));
01404 op[60]=((((IkReal(-1.00000000000000))*(x943)))+(x981));
01405 op[61]=((((IkReal(-0.00143869000000000))*(x548)))+(((IkReal(0.000195446036500000))*(x547)))+(((IkReal(-0.000195446036500000))*(x567)))+(((IkReal(-0.000195446036500000))*(x569)))+(((IkReal(0.000195446036500000))*(x561)))+(((IkReal(-0.00143869000000000))*(x584)))+(((IkReal(-1.00000000000000))*(x959)))+(x916));
01406 op[62]=((((IkReal(-1.00000000000000))*(x981)))+(x943));
01407 op[63]=((IkReal(-2.00000000000000))+(((IkReal(-1.00000000000000))*(x954)))+(((IkReal(-1.00000000000000))*(x975))));
01408 op[64]=((((IkReal(2.66873869000000))*(x599)))+(x956)+(((IkReal(2.66873869000000))*(x583)))+(((IkReal(2.66873869000000))*(x540)))+(((IkReal(2.66873869000000))*(x554))));
01409 op[65]=((x975)+(x977));
01410 op[66]=((x963)+(x956)+(((IkReal(-1.00000000000000))*(x962))));
01411 op[67]=((IkReal(8.00000000000000))+(((IkReal(0.219822620000000))*(x543)))+(((r00)*(x577)))+(((IkReal(0.213600000000000))*(x622)))+(((IkReal(0.219822620000000))*(x550)))+(((IkReal(0.219822620000000))*(x555)))+(((r10)*(x624)))+(((IkReal(0.219822620000000))*(x597)))+(((r01)*(x643))));
01412 op[68]=((((IkReal(-1.00000000000000))*(x956)))+(((IkReal(-1.00000000000000))*(x962)))+(((IkReal(-1.00000000000000))*(x963))));
01413 op[69]=((x694)+(x695)+(x696)+(x697)+(x977));
01414 op[70]=((((IkReal(-0.00143869000000000))*(x540)))+(((IkReal(-0.00143869000000000))*(x599)))+(((IkReal(-0.00143869000000000))*(x583)))+(((IkReal(-1.00000000000000))*(x956)))+(((IkReal(-0.00143869000000000))*(x554))));
01415 op[71]=((IkReal(-2.00000000000000))+(((IkReal(-1.00000000000000))*(x984))));
01416 op[72]=((((IkReal(-1.00000000000000))*(x944)))+(x945));
01417 op[73]=((((IkReal(-2.66873869000000))*(x567)))+(((IkReal(-2.66873869000000))*(x569)))+(((IkReal(-1.00000000000000))*(x915)))+(((r11)*(x560)))+(x939)+(((IkReal(2.66873869000000))*(x547))));
01418 op[74]=((((IkReal(-1.00000000000000))*(x945)))+(x944));
01419 op[75]=((((IkReal(-1.00000000000000))*(x924)))+(((IkReal(-1.00000000000000))*(x915)))+(x920)+(x960)+(((IkReal(-1.00000000000000))*(x932))));
01420 op[76]=((((IkReal(0.219822620000000))*(x549)))+(((IkReal(-0.219822620000000))*(x546)))+(((IkReal(0.213600000000000))*(x617)))+(((IkReal(-0.213600000000000))*(x598)))+(((IkReal(-0.219822620000000))*(x553)))+(((IkReal(-0.213600000000000))*(x609)))+(((r10)*(x577)))+(((IkReal(0.219822620000000))*(x568))));
01421 op[77]=((x924)+(x920)+(x915)+(((IkReal(-1.00000000000000))*(x960)))+(((IkReal(-1.00000000000000))*(x932))));
01422 op[78]=((((IkReal(-1.00000000000000))*(x946)))+(x947));
01423 op[79]=((((IkReal(-0.00143869000000000))*(x547)))+(((IkReal(0.00143869000000000))*(x569)))+(((IkReal(0.00143869000000000))*(x567)))+(((IkReal(-0.00143869000000000))*(x561)))+(x915)+(((IkReal(-1.00000000000000))*(x939))));
01424 op[80]=((((IkReal(-1.00000000000000))*(x947)))+(x946));
01425 op[81]=x948;
01426 op[82]=((IkReal(-4.00000000000000))+(((IkReal(-2.66873869000000))*(x593)))+(((IkReal(-2.66873869000000))*(x585)))+(((IkReal(-1.00000000000000))*(x933))));
01427 op[83]=((IkReal(-1.00000000000000))*(x948));
01428 op[84]=((IkReal(-4.00000000000000))+(((IkReal(-1.00000000000000))*(x921)))+(x922)+(((IkReal(-1.00000000000000))*(x933))));
01429 op[85]=((((IkReal(-0.213600000000000))*(x582)))+(((IkReal(-0.213600000000000))*(x576)))+(((IkReal(-0.219822620000000))*(x557)))+(((IkReal(-0.219822620000000))*(x636))));
01430 op[86]=((x922)+(x921)+(x941));
01431 op[87]=((IkReal(-1.00000000000000))*(x949));
01432 op[88]=((((IkReal(0.00143869000000000))*(x585)))+(((IkReal(0.00143869000000000))*(x593)))+(x941));
01433 op[89]=x949;
01434 op[90]=((((IkReal(-1.00000000000000))*(x950)))+(x951));
01435 op[91]=((((IkReal(-1.00000000000000))*(x925)))+(((IkReal(2.66873869000000))*(x621)))+(((rxp1_1)*(x560)))+(((IkReal(-2.66873869000000))*(x629)))+(((IkReal(-2.66873869000000))*(x639)))+(x928));
01436 op[92]=((((IkReal(-1.00000000000000))*(x951)))+(x950));
01437 op[93]=((((IkReal(-1.00000000000000))*(x927)))+(((IkReal(-1.00000000000000))*(x926)))+(((IkReal(-1.00000000000000))*(x925)))+(x930)+(x928)+(x929));
01438 op[94]=((((IkReal(-0.213600000000000))*(x646)))+(((rxp1_1)*(x643)))+(((IkReal(0.219822620000000))*(x620)))+(((IkReal(-1.00000000000000))*(rxp0_0)*(x624)))+(((IkReal(0.219822620000000))*(x633)))+(((IkReal(-0.219822620000000))*(x627)))+(((rxp0_1)*(x577)))+(((IkReal(-0.219822620000000))*(x645))));
01439 op[95]=((((IkReal(-1.00000000000000))*(x927)))+(((IkReal(-1.00000000000000))*(x928)))+(x929)+(x926)+(x925)+(((IkReal(-1.00000000000000))*(x930))));
01440 op[96]=((((IkReal(-1.00000000000000))*(x952)))+(x953));
01441 op[97]=((((IkReal(-1.00000000000000))*(x928)))+(((IkReal(0.00143869000000000))*(x629)))+(((IkReal(0.00143869000000000))*(x639)))+(((IkReal(-0.00143869000000000))*(x621)))+(((IkReal(-0.00143869000000000))*(x649)))+(x925));
01442 op[98]=((((IkReal(-1.00000000000000))*(x953)))+(x952));
01443 op[99]=((IkReal(1.63450000000000))+(((IkReal(-1.00000000000000))*(x964))));
01444 op[100]=((((IkReal(0.252596117008500))*(x593)))+(x934)+(((IkReal(0.252596117008500))*(x585))));
01445 op[101]=((IkReal(1.63450000000000))+(x964));
01446 op[102]=((((IkReal(-1.00000000000000))*(x919)))+(x935)+(x934));
01447 op[103]=((((IkReal(0.0202172400000000))*(x576)))+(((IkReal(0.0208062109830000))*(x636)))+(((IkReal(0.0208062109830000))*(x557)))+(((IkReal(0.0202172400000000))*(x582))));
01448 op[104]=((((IkReal(-1.00000000000000))*(x919)))+(((IkReal(-1.00000000000000))*(x934)))+(((IkReal(-1.00000000000000))*(x935))));
01449 op[105]=((IkReal(0.0655000000000000))+(x968));
01450 op[106]=((((IkReal(-0.000136172008500000))*(x585)))+(((IkReal(-0.000136172008500000))*(x593)))+(((IkReal(-1.00000000000000))*(x934))));
01451 op[107]=((IkReal(0.0655000000000000))+(((IkReal(-1.00000000000000))*(x968))));
01452 op[108]=((((IkReal(-1.00000000000000))*(x987)))+(((IkReal(-1.00000000000000))*(x913)))+(x918)+(x992)+(x978)+(x955)+(((IkReal(-1.00000000000000))*(x967)))+(((IkReal(-1.00000000000000))*(x979))));
01453 op[109]=((IkReal(-1.63450000000000))+(((IkReal(-1.33436934500000))*(x571)))+(((IkReal(-0.0872823000000000))*(x544)))+(((IkReal(-0.181274075518250))*(x546)))+(((IkReal(0.0118573004550000))*(x602)))+(((IkReal(0.181274075518250))*(x568)))+(((IkReal(-1.00000000000000))*(x914)))+(((IkReal(-0.0872823000000000))*(x580)))+(((IkReal(-0.0118573004550000))*(x586)))+(((IkReal(-1.33436934500000))*(x595)))+(((IkReal(-0.0118573004550000))*(x559)))+(((IkReal(0.0118573004550000))*(x570)))+(x969)+(((IkReal(0.181274075518250))*(x549)))+(((IkReal(-0.181274075518250))*(x553))));
01454 op[110]=((((IkReal(-1.00000000000000))*(x987)))+(((IkReal(-1.00000000000000))*(x918)))+(((IkReal(-1.00000000000000))*(x955)))+(x913)+(x992)+(x978)+(x967)+(((IkReal(-1.00000000000000))*(x979))));
01455 op[111]=((IkReal(-0.850000000000000))+(((IkReal(-1.00000000000000))*(x983)))+(((IkReal(-1.00000000000000))*(x914)))+(x970)+(x969));
01456 op[112]=((((IkReal(-0.0149314514635000))*(x561)))+(((IkReal(0.0149314514635000))*(x567)))+(((IkReal(0.0149314514635000))*(x569)))+(((IkReal(0.109911310000000))*(x584)))+(((IkReal(-1.00000000000000))*(x959)))+(x916)+(((IkReal(0.109911310000000))*(x548)))+(((IkReal(-0.0149314514635000))*(x547))));
01457 op[113]=((IkReal(0.850000000000000))+(x914)+(x983)+(((IkReal(-1.00000000000000))*(x969)))+(((IkReal(-1.00000000000000))*(x970))));
01458 op[114]=((((IkReal(-1.00000000000000))*(x982)))+(((IkReal(-1.00000000000000))*(x987)))+(((IkReal(-1.00000000000000))*(x912)))+(((IkReal(-1.00000000000000))*(x955)))+(x913)+(x992)+(x958)+(x957));
01459 op[115]=((IkReal(0.0655000000000000))+(((IkReal(0.000475162545000000))*(x586)))+(((IkReal(-9.77230182500000e-5))*(x568)))+(((IkReal(0.000719345000000000))*(x595)))+(((IkReal(9.77230182500000e-5))*(x553)))+(((IkReal(0.000719345000000000))*(x571)))+(((IkReal(0.00349770000000000))*(x580)))+(((IkReal(9.77230182500000e-5))*(x546)))+(((IkReal(0.00349770000000000))*(x544)))+(((IkReal(-9.77230182500000e-5))*(x549)))+(((IkReal(-0.000475162545000000))*(x602)))+(((IkReal(0.000475162545000000))*(x559)))+(x914)+(((IkReal(-1.00000000000000))*(x969)))+(((IkReal(-0.000475162545000000))*(x570))));
01460 op[116]=((((IkReal(-1.00000000000000))*(x982)))+(((IkReal(-1.00000000000000))*(x987)))+(((IkReal(-1.00000000000000))*(x913)))+(((IkReal(-1.00000000000000))*(x958)))+(x912)+(x992)+(x957)+(x955));
01461 op[117]=((((IkReal(-1.00000000000000))*(x986)))+(x991)+(x973)+(x965)+(((IkReal(-1.00000000000000))*(x976))));
01462 op[118]=((((IkReal(-1.33436934500000))*(x550)))+(((IkReal(-1.33436934500000))*(x555)))+(((IkReal(-1.33436934500000))*(x543)))+(((IkReal(-0.0872823000000000))*(x587)))+(((IkReal(-1.33436934500000))*(x597)))+(((IkReal(-0.0872823000000000))*(x606)))+(((IkReal(-0.0872823000000000))*(x641)))+(x977)+(((IkReal(-0.0872823000000000))*(x631))));
01463 op[119]=((((IkReal(-1.00000000000000))*(x986)))+(x991)+(x973)+(x976)+(((IkReal(-1.00000000000000))*(x965))));
01464 op[120]=((((IkReal(-1.00000000000000))*(x985)))+(x977));
01465 op[121]=((((IkReal(0.109911310000000))*(x599)))+(((IkReal(0.109911310000000))*(x583)))+(((IkReal(0.109911310000000))*(x554)))+(((IkReal(-1.00000000000000))*(x956)))+(((IkReal(0.109911310000000))*(x540))));
01466 op[122]=((IkReal(-2.00000000000000))+(((IkReal(-1.00000000000000))*(x954)))+(x985));
01467 op[123]=((((IkReal(-1.00000000000000))*(x990)))+(x991)+(x971)+(((IkReal(-1.00000000000000))*(x965))));
01468 op[124]=((IkReal(-2.00000000000000))+(((IkReal(0.00349770000000000))*(x631)))+(((IkReal(0.000719345000000000))*(x543)))+(((IkReal(0.000719345000000000))*(x597)))+(((IkReal(0.000719345000000000))*(x555)))+(((IkReal(0.000719345000000000))*(x550)))+(((IkReal(0.00349770000000000))*(x587)))+(((IkReal(0.00349770000000000))*(x641)))+(((IkReal(0.00349770000000000))*(x606)))+(((IkReal(-1.00000000000000))*(x954))));
01469 op[125]=((((IkReal(-1.00000000000000))*(x990)))+(x991)+(x965)+(((IkReal(-1.00000000000000))*(x971))));
01470 op[126]=((((IkReal(-1.00000000000000))*(x989)))+(x988)+(x966)+(((IkReal(-1.00000000000000))*(x961))));
01471 op[127]=((((IkReal(-1.33436934500000))*(x549)))+(((IkReal(1.33436934500000))*(x546)))+(((IkReal(0.0872823000000000))*(x559)))+(((IkReal(0.0872823000000000))*(x586)))+(((IkReal(-1.33436934500000))*(x568)))+(((IkReal(-0.0872823000000000))*(x602)))+(x931)+(((IkReal(-0.0872823000000000))*(x570)))+(((IkReal(1.33436934500000))*(x553)))+(((IkReal(-1.00000000000000))*(x936))));
01472 op[128]=((((IkReal(-1.00000000000000))*(x989)))+(x988)+(x961)+(((IkReal(-1.00000000000000))*(x966))));
01473 op[129]=((((IkReal(-0.0453900000000000))*(x570)))+(((IkReal(0.0521040950000000))*(x546)))+(((IkReal(-0.0521040950000000))*(x568)))+(((IkReal(-0.0453900000000000))*(x602)))+(((IkReal(0.0453900000000000))*(x559)))+(((IkReal(0.0453900000000000))*(x586)))+(x931)+(((IkReal(0.0521040950000000))*(x553)))+(((IkReal(-0.0521040950000000))*(x549)))+(((IkReal(-1.00000000000000))*(x936))));
01474 IkReal x993=((gconst7_7)*(rxp1_1));
01475 IkReal x994=((IkReal(0.106800000000000))*(gconst7_5));
01476 IkReal x995=((gconst0_3)*(r20));
01477 IkReal x996=((gconst6_2)*(r10));
01478 IkReal x997=((IkReal(1.00000000000000))*(pp));
01479 IkReal x998=((gconst4_0)*(r01));
01480 IkReal x999=((gconst1_2)*(r21));
01481 IkReal x1000=((IkReal(0.0946500000000000))*(pp));
01482 IkReal x1001=((gconst1_4)*(r21));
01483 IkReal x1002=((gconst0_2)*(r20));
01484 IkReal x1003=((gconst0_1)*(r20));
01485 IkReal x1004=((IkReal(0.189300000000000))*(pz));
01486 IkReal x1005=((gconst3_7)*(rxp0_0));
01487 IkReal x1006=((gconst3_1)*(r00));
01488 IkReal x1007=((gconst1_3)*(r21));
01489 IkReal x1008=((gconst1_1)*(r21));
01490 IkReal x1009=((gconst3_1)*(rxp0_0));
01491 IkReal x1010=((gconst6_3)*(rxp0_1));
01492 IkReal x1011=((gconst6_1)*(r10));
01493 IkReal x1012=((IkReal(0.0267000000000000))*(gconst6_5));
01494 IkReal x1013=((gconst3_2)*(r00));
01495 IkReal x1014=((gconst4_0)*(rxp1_0));
01496 IkReal x1015=((gconst0_5)*(r20));
01497 IkReal x1016=((gconst1_5)*(r21));
01498 IkReal x1017=((gconst7_2)*(rxp1_1));
01499 IkReal x1018=((IkReal(0.0257164050000000))*(gconst7_1));
01500 IkReal x1019=((gconst4_2)*(rxp1_0));
01501 IkReal x1020=((gconst1_0)*(r21));
01502 IkReal x1021=((gconst0_0)*(r20));
01503 IkReal x1022=((gconst4_7)*(rxp1_0));
01504 IkReal x1023=((gconst6_7)*(r10));
01505 IkReal x1024=((gconst0_7)*(r20));
01506 IkReal x1025=((gconst6_1)*(rxp0_1));
01507 IkReal x1026=((IkReal(0.0534000000000000))*(gconst6_6));
01508 IkReal x1027=((gconst3_7)*(r00));
01509 IkReal x1028=((gconst4_1)*(rxp1_0));
01510 IkReal x1029=((gconst1_7)*(r21));
01511 IkReal x1030=((gconst7_0)*(rxp1_1));
01512 IkReal x1031=((gconst7_0)*(r11));
01513 IkReal x1032=((gconst7_3)*(rxp1_1));
01514 IkReal x1033=((IkReal(0.0534000000000000))*(gconst3_6));
01515 IkReal x1034=((IkReal(0.106800000000000))*(gconst6_5));
01516 IkReal x1035=((IkReal(0.109150000000000))*(gconst3_4));
01517 IkReal x1036=((gconst3_2)*(rxp0_0));
01518 IkReal x1037=((gconst0_6)*(r20));
01519 IkReal x1038=((gconst7_1)*(r11));
01520 IkReal x1039=((IkReal(0.0327500000000000))*(gconst4_3));
01521 IkReal x1040=((gconst6_7)*(rxp0_1));
01522 IkReal x1041=((IkReal(0.0521040950000000))*(gconst6_0));
01523 IkReal x1042=((IkReal(0.109150000000000))*(gconst7_4));
01524 IkReal x1043=((gconst7_7)*(r11));
01525 IkReal x1044=((IkReal(0.0534000000000000))*(gconst7_6));
01526 IkReal x1045=((gconst4_5)*(r01));
01527 IkReal x1046=((IkReal(0.0534000000000000))*(gconst4_6));
01528 IkReal x1047=((gconst4_7)*(r01));
01529 IkReal x1048=((gconst3_0)*(rxp0_0));
01530 IkReal x1049=((gconst4_5)*(rxp1_0));
01531 IkReal x1050=((gconst3_0)*(r00));
01532 IkReal x1051=((IkReal(0.106800000000000))*(gconst3_5));
01533 IkReal x1052=((gconst1_6)*(r21));
01534 IkReal x1053=((gconst6_2)*(rxp0_1));
01535 IkReal x1054=((IkReal(0.0179172450000000))*(pz));
01536 IkReal x1055=((gconst3_3)*(rxp0_0));
01537 IkReal x1056=((IkReal(0.0267000000000000))*(gconst7_5));
01538 IkReal x1057=((IkReal(0.109150000000000))*(gconst6_4));
01539 IkReal x1058=((gconst6_0)*(rxp0_1));
01540 IkReal x1059=((IkReal(0.109150000000000))*(gconst4_4));
01541 IkReal x1060=((gconst4_1)*(r01));
01542 IkReal x1061=((gconst7_1)*(rxp1_1));
01543 IkReal x1062=((gconst7_2)*(r11));
01544 IkReal x1063=((gconst4_2)*(r01));
01545 IkReal x1064=((IkReal(0.0267000000000000))*(gconst3_5));
01546 IkReal x1065=((gconst0_4)*(r20));
01547 IkReal x1066=((r01)*(x1046));
01548 IkReal x1067=((r00)*(x1033));
01549 IkReal x1068=((r10)*(x1026));
01550 IkReal x1069=((r11)*(x1044));
01551 IkReal x1070=((r10)*(x1012));
01552 IkReal x1071=((r01)*(x1039));
01553 IkReal x1072=((IkReal(0.0256283925000000))*(x1013));
01554 IkReal x1073=((IkReal(0.00178545250000000))*(x1047));
01555 IkReal x1074=((pp)*(x1063));
01556 IkReal x1075=((IkReal(0.00178545250000000))*(x1027));
01557 IkReal x1076=((pp)*(x1013));
01558 IkReal x1077=((IkReal(0.0257164050000000))*(x1011));
01559 IkReal x1078=((r11)*(x1042));
01560 IkReal x1079=((IkReal(0.0256283925000000))*(x1063));
01561 IkReal x1080=((r11)*(x1018));
01562 IkReal x1081=((IkReal(0.0327500000000000))*(gconst3_3)*(r00));
01563 IkReal x1082=((r11)*(x1056));
01564 IkReal x1083=((r10)*(x1057));
01565 IkReal x1084=((IkReal(0.0256283925000000))*(x1062));
01566 IkReal x1085=((IkReal(0.00178545250000000))*(x1023));
01567 IkReal x1086=((IkReal(0.00178545250000000))*(x1043));
01568 IkReal x1087=((IkReal(0.0257164050000000))*(x1060));
01569 IkReal x1088=((r00)*(x1064));
01570 IkReal x1089=((r01)*(x1059));
01571 IkReal x1090=((IkReal(0.0327500000000000))*(gconst7_3)*(r11));
01572 IkReal x1091=((IkReal(0.0257164050000000))*(x1006));
01573 IkReal x1092=((IkReal(0.0327500000000000))*(gconst6_3)*(r10));
01574 IkReal x1093=((IkReal(0.0267000000000000))*(x1045));
01575 IkReal x1094=((IkReal(0.0256283925000000))*(x996));
01576 IkReal x1095=((r00)*(x1035));
01577 IkReal x1096=((pp)*(x1002));
01578 IkReal x1097=((IkReal(0.0267000000000000))*(x1015));
01579 IkReal x1098=((IkReal(0.817250000000000))*(x1007));
01580 IkReal x1099=((IkReal(0.817250000000000))*(x995));
01581 IkReal x1100=((pp)*(x999));
01582 IkReal x1101=((IkReal(0.0267000000000000))*(x1016));
01583 IkReal x1102=((IkReal(0.668610452500000))*(x1024));
01584 IkReal x1103=((IkReal(0.109150000000000))*(x1065));
01585 IkReal x1104=((IkReal(0.0257164050000000))*(x1008));
01586 IkReal x1105=((IkReal(0.641196607500000))*(x999));
01587 IkReal x1106=((IkReal(0.641196607500000))*(x1002));
01588 IkReal x1107=((IkReal(0.0257164050000000))*(x1003));
01589 IkReal x1108=((IkReal(0.109150000000000))*(x1001));
01590 IkReal x1109=((IkReal(0.668610452500000))*(x1029));
01591 IkReal x1110=((IkReal(0.00178545250000000))*(x1024));
01592 IkReal x1111=((IkReal(0.0327500000000000))*(x995));
01593 IkReal x1112=((IkReal(0.0327500000000000))*(x1007));
01594 IkReal x1113=((IkReal(0.0256283925000000))*(x1002));
01595 IkReal x1114=((IkReal(0.00178545250000000))*(x1029));
01596 IkReal x1115=((IkReal(0.0256283925000000))*(x999));
01597 IkReal x1116=((rxp0_1)*(x1057));
01598 IkReal x1117=((IkReal(0.641196607500000))*(x1017));
01599 IkReal x1118=((IkReal(0.0267000000000000))*(x1049));
01600 IkReal x1119=((rxp0_0)*(x1064));
01601 IkReal x1120=((IkReal(0.668610452500000))*(x993));
01602 IkReal x1121=((pp)*(x1019));
01603 IkReal x1122=((IkReal(0.0257164050000000))*(x1025));
01604 IkReal x1123=((IkReal(0.817250000000000))*(x1055));
01605 IkReal x1124=((rxp1_1)*(x1042));
01606 IkReal x1125=((pp)*(x1036));
01607 IkReal x1126=((rxp1_1)*(x1018));
01608 IkReal x1127=((IkReal(0.641196607500000))*(x1053));
01609 IkReal x1128=((IkReal(0.817250000000000))*(gconst4_3)*(rxp1_0));
01610 IkReal x1129=((IkReal(0.668610452500000))*(x1040));
01611 IkReal x1130=((IkReal(0.668610452500000))*(x1022));
01612 IkReal x1131=((rxp0_0)*(x1035));
01613 IkReal x1132=((IkReal(0.641196607500000))*(x1036));
01614 IkReal x1133=((rxp0_1)*(x1012));
01615 IkReal x1134=((IkReal(0.817250000000000))*(x1010));
01616 IkReal x1135=((rxp1_1)*(x1056));
01617 IkReal x1136=((IkReal(0.0257164050000000))*(x1028));
01618 IkReal x1137=((IkReal(0.817250000000000))*(x1032));
01619 IkReal x1138=((IkReal(0.0257164050000000))*(x1009));
01620 IkReal x1139=((IkReal(0.668610452500000))*(x1005));
01621 IkReal x1140=((rxp1_0)*(x1059));
01622 IkReal x1141=((IkReal(0.641196607500000))*(x1019));
01623 IkReal x1142=((rxp1_1)*(x1044));
01624 IkReal x1143=((rxp0_1)*(x1026));
01625 IkReal x1144=((rxp0_0)*(x1033));
01626 IkReal x1145=((rxp1_0)*(x1046));
01627 IkReal x1146=((IkReal(0.0327500000000000))*(x1055));
01628 IkReal x1147=((IkReal(0.00178545250000000))*(x1022));
01629 IkReal x1148=((IkReal(0.0256283925000000))*(x1019));
01630 IkReal x1149=((IkReal(0.00178545250000000))*(x1005));
01631 IkReal x1150=((rxp1_0)*(x1039));
01632 IkReal x1151=((IkReal(0.0256283925000000))*(x1036));
01633 IkReal x1152=((IkReal(0.00178545250000000))*(x993));
01634 IkReal x1153=((IkReal(0.0256283925000000))*(x1053));
01635 IkReal x1154=((IkReal(0.0327500000000000))*(x1032));
01636 IkReal x1155=((IkReal(0.0256283925000000))*(x1017));
01637 IkReal x1156=((IkReal(0.0327500000000000))*(x1010));
01638 IkReal x1157=((IkReal(0.00178545250000000))*(x1040));
01639 IkReal x1158=((IkReal(0.00243405773325000))*(x1008));
01640 IkReal x1159=((IkReal(0.0632839793291250))*(x1029));
01641 IkReal x1160=((IkReal(0.0103310475000000))*(x1001));
01642 IkReal x1161=((IkReal(0.0606892588998750))*(x999));
01643 IkReal x1162=((IkReal(0.00243405773325000))*(x1003));
01644 IkReal x1163=((IkReal(0.0632839793291250))*(x1024));
01645 IkReal x1164=((IkReal(0.0606892588998750))*(x1002));
01646 IkReal x1165=((IkReal(0.0103310475000000))*(x1065));
01647 IkReal x1166=((IkReal(0.00252715500000000))*(x1016));
01648 IkReal x1167=((IkReal(0.0773527125000000))*(x1007));
01649 IkReal x1168=((IkReal(0.00252715500000000))*(x1015));
01650 IkReal x1169=((IkReal(0.0773527125000000))*(x995));
01651 IkReal x1170=((IkReal(0.00505431000000000))*(x1037));
01652 IkReal x1171=((IkReal(0.00505431000000000))*(x1052));
01653 IkReal x1172=((IkReal(0.00493165259175000))*(x1020));
01654 IkReal x1173=((IkReal(0.00429616350000000))*(x1003));
01655 IkReal x1174=((IkReal(0.00429616350000000))*(x1008));
01656 IkReal x1175=((IkReal(0.00493165259175000))*(x1021));
01657 IkReal x1176=((IkReal(0.00309978750000000))*(x1007));
01658 IkReal x1177=((IkReal(0.00242572735012500))*(x1002));
01659 IkReal x1178=((IkReal(0.000168993079125000))*(x1024));
01660 IkReal x1179=((IkReal(0.000168993079125000))*(x1029));
01661 IkReal x1180=((IkReal(0.00242572735012500))*(x999));
01662 IkReal x1181=((IkReal(0.00309978750000000))*(x995));
01663 IkReal x1182=((x1004)*(x1023));
01664 IkReal x1183=((x1004)*(x1043));
01665 IkReal x1184=((x1004)*(x1047));
01666 IkReal x1185=((x1004)*(x1027));
01667 IkReal x1186=((x996)*(x997));
01668 IkReal x1187=((x1062)*(x997));
01669 IkReal x1188=((x1004)*(x1024));
01670 IkReal x1189=((x1004)*(x1029));
01671 IkReal x1190=((x1004)*(x993));
01672 IkReal x1191=((x1004)*(x1040));
01673 IkReal x1192=((x1017)*(x997));
01674 IkReal x1193=((x1053)*(x997));
01675 IkReal x1194=((x1004)*(x1022));
01676 IkReal x1195=((x1004)*(x1005));
01677 IkReal x1196=((x1024)*(x1054));
01678 IkReal x1197=((x1029)*(x1054));
01679 IkReal x1198=((x1000)*(x999));
01680 IkReal x1199=((x1000)*(x1002));
01681 IkReal x1200=((((IkReal(0.0534000000000000))*(x1037)))+(((IkReal(0.0534000000000000))*(x1052))));
01682 IkReal x1201=((x1171)+(x1170));
01683 IkReal x1202=((x1067)+(x1066));
01684 IkReal x1203=((x1142)+(x1143));
01685 IkReal x1204=((x1164)+(x1161));
01686 IkReal x1205=((x1102)+(x1109));
01687 IkReal x1206=((x1101)+(x1097));
01688 IkReal x1207=((x1179)+(x1178));
01689 IkReal x1208=((x1100)+(x1096));
01690 IkReal x1209=((x1098)+(x1099));
01691 IkReal x1210=((x1167)+(x1169));
01692 IkReal x1211=((x1069)+(x1068));
01693 IkReal x1212=((x1159)+(x1163));
01694 IkReal x1213=((x1144)+(x1145));
01695 IkReal x1214=((x1110)+(x1114));
01696 IkReal x1215=((x1199)+(x1198));
01697 IkReal x1216=((((IkReal(0.0521040950000000))*(x1020)))+(((IkReal(0.0521040950000000))*(x1021)))+(((IkReal(0.0453900000000000))*(x1003)))+(((IkReal(0.0453900000000000))*(x1008))));
01698 IkReal x1217=((x1139)+(x1133)+(x1130)+(x1135));
01699 IkReal x1218=((x1117)+(x1128)+(x1123)+(x1127));
01700 IkReal x1219=((x1075)+(x1073)+(x1070)+(x1082));
01701 IkReal x1220=((x1141)+(x1132)+(x1137)+(x1134));
01702 IkReal x1221=((x1147)+(x1149)+(x1133)+(x1135));
01703 IkReal x1222=((x1155)+(x1154)+(x1156)+(x1153));
01704 IkReal x1223=((x1146)+(x1148)+(x1151)+(x1150));
01705 IkReal x1224=((x1119)+(x1118)+(x1157)+(x1152));
01706 IkReal x1225=((x1180)+(x1181)+(x1177)+(x1176));
01707 IkReal x1226=((x1093)+(x1086)+(x1085)+(x1088));
01708 IkReal x1227=((x1111)+(x1113)+(x1112)+(x1115));
01709 IkReal x1228=((x1175)+(x1174)+(x1173)+(x1172));
01710 IkReal x1229=((x1119)+(x1118)+(x1129)+(x1120));
01711 IkReal x1230=((x1166)+(x1168)+(x1215));
01712 IkReal x1231=((x1213)+(((IkReal(0.0453900000000000))*(x1025)))+(((IkReal(0.0521040950000000))*(x1030)))+(((IkReal(0.0453900000000000))*(x1061)))+(((rxp0_1)*(x1041))));
01713 IkReal x1232=((x1203)+(((IkReal(0.0453900000000000))*(x1028)))+(((IkReal(0.0521040950000000))*(x1014)))+(((IkReal(0.0453900000000000))*(x1009)))+(((IkReal(0.0521040950000000))*(x1048))));
01714 IkReal x1233=((x1107)+(x1104)+(x1103)+(x1108)+(x1188)+(x1189));
01715 IkReal x1234=((x1158)+(x1197)+(x1196)+(x1165)+(x1160)+(x1162)+(pz));
01716 IkReal x1235=((x1116)+(x1121)+(x1122)+(x1124)+(x1125)+(x1126)+(x1191)+(x1190));
01717 IkReal x1236=((x1106)+(x1105)+(x1233));
01718 IkReal x1237=((x1166)+(x1168)+(x1234));
01719 IkReal x1238=((x1140)+(x1138)+(x1131)+(x1136)+(x1193)+(x1192)+(x1195)+(x1194));
01720 IkReal x1239=((x1182)+(x1183)+(x1078)+(x1079)+(x1076)+(x1077)+(x1074)+(x1072)+(x1071)+(x1083)+(x1081)+(x1080));
01721 IkReal x1240=((x1186)+(x1187)+(x1184)+(x1185)+(x1094)+(x1095)+(x1090)+(x1091)+(x1092)+(x1087)+(x1084)+(x1089));
01722 op[130]=((((IkReal(-1.00000000000000))*(r10)*(x1034)))+(((IkReal(0.109911310000000))*(x1023)))+(((IkReal(0.106800000000000))*(x1045)))+(((IkReal(-1.00000000000000))*(r11)*(x994)))+(((r00)*(x1051)))+(((IkReal(0.109911310000000))*(x1043)))+(((IkReal(-0.109911310000000))*(x1047)))+(((IkReal(-0.109911310000000))*(x1027))));
01723 op[131]=((((IkReal(-0.0521040950000000))*(x1050)))+(x1202)+(((IkReal(-0.0521040950000000))*(x998)))+(((IkReal(-0.0453900000000000))*(x1006)))+(((IkReal(-0.0453900000000000))*(x1060)))+(((r10)*(x1041)))+(((IkReal(0.0453900000000000))*(x1038)))+(((IkReal(0.0453900000000000))*(x1011)))+(((IkReal(-1.00000000000000))*(x1211)))+(((IkReal(0.0521040950000000))*(x1031))));
01724 op[132]=((x1239)+(x1226)+(((IkReal(-1.00000000000000))*(x1240)))+(((IkReal(-1.00000000000000))*(x1219))));
01725 op[133]=((((IkReal(0.00349770000000000))*(x1038)))+(x1202)+(((IkReal(-0.000719345000000000))*(x998)))+(((IkReal(-0.000719345000000000))*(x1050)))+(((IkReal(0.00349770000000000))*(x1011)))+(((IkReal(0.000719345000000000))*(x1031)))+(((IkReal(-0.00349770000000000))*(x1006)))+(((IkReal(0.000719345000000000))*(gconst6_0)*(r10)))+(((IkReal(-1.00000000000000))*(x1211)))+(((IkReal(-0.00349770000000000))*(x1060))));
01726 op[134]=((x1239)+(x1219)+(((IkReal(-1.00000000000000))*(x1226)))+(((IkReal(-1.00000000000000))*(x1240))));
01727 op[135]=((IkReal(-1.00000000000000))+(x1209)+(x1208)+(x1205)+(((IkReal(-1.00000000000000))*(x1236)))+(((IkReal(-1.00000000000000))*(x1206))));
01728 op[136]=((((IkReal(0.0872823000000000))*(x1008)))+(((IkReal(0.0872823000000000))*(x1003)))+(((IkReal(-1.00000000000000))*(x1200)))+(((IkReal(1.33436934500000))*(x1021)))+(((IkReal(1.33436934500000))*(x1020))));
01729 op[137]=((IkReal(1.00000000000000))+(x1209)+(x1208)+(x1206)+(((IkReal(-1.00000000000000))*(x1236)))+(((IkReal(-1.00000000000000))*(x1205))));
01730 op[138]=((x1216)+(((IkReal(-1.00000000000000))*(x1200))));
01731 op[139]=((IkReal(4.00000000000000))+(((IkReal(0.106800000000000))*(x1015)))+(((IkReal(0.106800000000000))*(x1016)))+(((IkReal(-0.109911310000000))*(x1024)))+(((IkReal(-0.109911310000000))*(x1029))));
01732 op[140]=((x1200)+(((IkReal(-1.00000000000000))*(x1216))));
01733 op[141]=((IkReal(1.00000000000000))+(x1208)+(x1206)+(x1227)+(((IkReal(-1.00000000000000))*(x1233)))+(((IkReal(-1.00000000000000))*(x1214))));
01734 op[142]=((x1200)+(((IkReal(-0.00349770000000000))*(x1008)))+(((IkReal(-0.00349770000000000))*(x1003)))+(((IkReal(-0.000719345000000000))*(x1020)))+(((IkReal(-0.000719345000000000))*(x1021))));
01735 op[143]=((IkReal(-1.00000000000000))+(x1208)+(x1227)+(x1214)+(((IkReal(-1.00000000000000))*(x1233)))+(((IkReal(-1.00000000000000))*(x1206))));
01736 op[144]=((IkReal(-0.817250000000000))+(x1235)+(x1217)+(x1218)+(((IkReal(-1.00000000000000))*(x1220)))+(((IkReal(-1.00000000000000))*(x1229)))+(((IkReal(-1.00000000000000))*(x1238))));
01737 op[145]=((((IkReal(-1.33436934500000))*(x1030)))+(((IkReal(-1.33436934500000))*(x1058)))+(((IkReal(1.33436934500000))*(x1014)))+(x1203)+(((IkReal(-0.0872823000000000))*(x1025)))+(((IkReal(1.33436934500000))*(x1048)))+(((IkReal(0.0872823000000000))*(x1009)))+(((IkReal(-0.0872823000000000))*(x1061)))+(((IkReal(0.0872823000000000))*(x1028)))+(((IkReal(-1.00000000000000))*(x1213))));
01738 op[146]=((IkReal(0.817250000000000))+(x1235)+(x1229)+(x1218)+(((IkReal(-1.00000000000000))*(x1220)))+(((IkReal(-1.00000000000000))*(x1238)))+(((IkReal(-1.00000000000000))*(x1217))));
01739 op[147]=((x1232)+(((IkReal(-1.00000000000000))*(x1231))));
01740 op[148]=((IkReal(1.70000000000000))+(((IkReal(-0.109911310000000))*(x1005)))+(((IkReal(0.106800000000000))*(x1049)))+(((IkReal(-1.00000000000000))*(rxp1_1)*(x994)))+(((IkReal(0.109911310000000))*(x993)))+(((IkReal(0.109911310000000))*(x1040)))+(((rxp0_0)*(x1051)))+(((IkReal(-1.00000000000000))*(rxp0_1)*(x1034)))+(((IkReal(-0.109911310000000))*(x1022))));
01741 op[149]=((x1231)+(((IkReal(-1.00000000000000))*(x1232))));
01742 op[150]=((IkReal(0.0327500000000000))+(x1235)+(x1224)+(x1223)+(((IkReal(-1.00000000000000))*(x1221)))+(((IkReal(-1.00000000000000))*(x1222)))+(((IkReal(-1.00000000000000))*(x1238))));
01743 op[151]=((x1213)+(((IkReal(0.000719345000000000))*(x1058)))+(((IkReal(0.000719345000000000))*(x1030)))+(((IkReal(-0.000719345000000000))*(x1048)))+(((IkReal(-0.00349770000000000))*(x1009)))+(((IkReal(0.00349770000000000))*(x1025)))+(((IkReal(-0.00349770000000000))*(x1028)))+(((IkReal(-0.000719345000000000))*(x1014)))+(((IkReal(-1.00000000000000))*(x1203)))+(((IkReal(0.00349770000000000))*(x1061))));
01744 op[152]=((IkReal(-0.0327500000000000))+(x1235)+(x1223)+(x1221)+(((IkReal(-1.00000000000000))*(x1222)))+(((IkReal(-1.00000000000000))*(x1224)))+(((IkReal(-1.00000000000000))*(x1238))));
01745 op[153]=((x1204)+(x1237)+(((IkReal(-1.00000000000000))*(x1215)))+(((IkReal(-1.00000000000000))*(x1210)))+(((IkReal(-1.00000000000000))*(x1212))));
01746 op[154]=((x1201)+(((IkReal(-0.00826126969500000))*(x1008)))+(((IkReal(-0.00826126969500000))*(x1003)))+(((IkReal(-0.126298058504250))*(x1020)))+(((IkReal(-0.126298058504250))*(x1021))));
01747 op[155]=((x1204)+(x1234)+(x1212)+(((IkReal(-1.00000000000000))*(x1230)))+(((IkReal(-1.00000000000000))*(x1210))));
01748 op[156]=((IkReal(-0.784500000000000))+(x1201)+(((IkReal(-1.00000000000000))*(x1228))));
01749 op[157]=((((IkReal(0.0104031054915000))*(x1029)))+(((IkReal(0.0104031054915000))*(x1024)))+(((IkReal(-0.0101086200000000))*(x1016)))+(((IkReal(-0.0101086200000000))*(x1015))));
01750 op[158]=((IkReal(-0.784500000000000))+(x1228)+(((IkReal(-1.00000000000000))*(x1201))));
01751 op[159]=((x1207)+(x1234)+(((IkReal(-1.00000000000000))*(x1225)))+(((IkReal(-1.00000000000000))*(x1230))));
01752 op[160]=((((IkReal(0.000331057305000000))*(x1003)))+(((IkReal(0.000331057305000000))*(x1008)))+(((IkReal(6.80860042500000e-5))*(x1021)))+(((IkReal(6.80860042500000e-5))*(x1020)))+(((IkReal(-1.00000000000000))*(x1201))));
01753 op[161]=((x1237)+(((IkReal(-1.00000000000000))*(x1225)))+(((IkReal(-1.00000000000000))*(x1207)))+(((IkReal(-1.00000000000000))*(x1215))));
01754 solvedialyticpoly12qep(op,zeror,numroots);
01755 IkReal j1array[16], cj1array[16], sj1array[16], j2array[16], cj2array[16], sj2array[16], j3array[16], cj3array[16], sj3array[16];
01756 int numsolutions = 0;
01757 for(int ij1 = 0; ij1 < numroots; ij1 += 3)
01758 {
01759 IkReal htj1 = zeror[ij1+0], htj2 = zeror[ij1+1], htj3 = zeror[ij1+2];
01760 j1array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj1)));
01761 j2array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj2)));
01762 j3array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj3)));
01763 IkReal x1241=(htj1)*(htj1);
01764 IkReal x1242=(htj2)*(htj2);
01765 IkReal x1243=(htj3)*(htj3);
01766 cj1array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x1241))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x1241)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1241))))));
01767 cj2array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x1242))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x1242)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1242))))));
01768 cj3array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x1243))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x1243)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1243))))));
01769 sj1array[numsolutions]=((IkReal(2.00000000000000))*(htj1)*(((IKabs(((IkReal(1.00000000000000))+((htj1)*(htj1)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj1)*(htj1))))):(IkReal)1.0e30)));
01770 sj2array[numsolutions]=((IkReal(2.00000000000000))*(htj2)*(((IKabs(((IkReal(1.00000000000000))+((htj2)*(htj2)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj2)*(htj2))))):(IkReal)1.0e30)));
01771 sj3array[numsolutions]=((IkReal(2.00000000000000))*(htj3)*(((IKabs(((IkReal(1.00000000000000))+((htj3)*(htj3)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj3)*(htj3))))):(IkReal)1.0e30)));
01772 if( j1array[numsolutions] > IKPI )
01773 {
01774     j1array[numsolutions]-=IK2PI;
01775 }
01776 else if( j1array[numsolutions] < -IKPI )
01777 {
01778     j1array[numsolutions]+=IK2PI;
01779 }
01780 if( j2array[numsolutions] > IKPI )
01781 {
01782     j2array[numsolutions]-=IK2PI;
01783 }
01784 else if( j2array[numsolutions] < -IKPI )
01785 {
01786     j2array[numsolutions]+=IK2PI;
01787 }
01788 if( j3array[numsolutions] > IKPI )
01789 {
01790     j3array[numsolutions]-=IK2PI;
01791 }
01792 else if( j3array[numsolutions] < -IKPI )
01793 {
01794     j3array[numsolutions]+=IK2PI;
01795 }
01796 numsolutions++;
01797 }
01798 bool j1valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
01799 _nj1 = 16;
01800 _nj2 = 1;
01801 _nj3 = 1;
01802 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
01803     {
01804 if( !j1valid[ij1] )
01805 {
01806     continue;
01807 }
01808 _ij1[0] = ij1; _ij1[1] = -1;
01809 _ij2[0] = 0; _ij2[1] = -1;
01810 _ij3[0] = 0; _ij3[1] = -1;
01811 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
01812 {
01813 if( !j1valid[iij1] ) { continue; }
01814 if( IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(cj2array[ij1]-cj2array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij1]-sj2array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(cj3array[ij1]-cj3array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij1]-sj3array[iij1]) < IKFAST_SOLUTION_THRESH &&  1 )
01815 {
01816     j1valid[iij1]=false; _ij1[1] = iij1; _ij2[1] = 0; _ij3[1] = 0;  break; 
01817 }
01818 }
01819     j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
01820 
01821     j2 = j2array[ij1]; cj2 = cj2array[ij1]; sj2 = sj2array[ij1];
01822 
01823     j3 = j3array[ij1]; cj3 = cj3array[ij1]; sj3 = sj3array[ij1];
01824 
01825 {
01826 IkReal dummyeval[1];
01827 dummyeval[0]=(((r20)*(r20))+((r21)*(r21)));
01828 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01829 {
01830 {
01831 IkReal dummyeval[1];
01832 dummyeval[0]=(((r20)*(r20))+((r21)*(r21)));
01833 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01834 {
01835 {
01836 IkReal dummyeval[1];
01837 dummyeval[0]=(((r20)*(r20))+((r21)*(r21)));
01838 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01839 {
01840 continue;
01841 
01842 } else
01843 {
01844 {
01845 IkReal j5array[2], cj5array[2], sj5array[2];
01846 bool j5valid[2]={false};
01847 _nj5 = 2;
01848 IkReal x1244=((IkReal(1.00000000000000))*(sj3));
01849 if( IKabs(((IkReal(-1.00000000000000))*(r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(r20))) < IKFAST_ATAN2_MAGTHRESH )
01850     continue;
01851 IkReal x1245=((IkReal(1.00000000000000))*(IKatan2(((IkReal(-1.00000000000000))*(r21)), ((IkReal(-1.00000000000000))*(r20)))));
01852 if( ((((r20)*(r20))+((r21)*(r21)))) < (IkReal)-0.00001 )
01853     continue;
01854 if( (((((IKabs(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21))))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(cj3)*(sj1)*(sj2)))+(((cj1)*(cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x1244)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)*(x1244))))))) < -1-IKFAST_SINCOS_THRESH || (((((IKabs(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21))))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(cj3)*(sj1)*(sj2)))+(((cj1)*(cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x1244)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)*(x1244))))))) > 1+IKFAST_SINCOS_THRESH )
01855     continue;
01856 IkReal x1246=IKasin(((((IKabs(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((r20)*(r20))+((r21)*(r21))))))):(IkReal)1.0e30))*(((((IkReal(-1.00000000000000))*(cj3)*(sj1)*(sj2)))+(((cj1)*(cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x1244)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)*(x1244)))))));
01857 j5array[0]=((((IkReal(-1.00000000000000))*(x1246)))+(((IkReal(-1.00000000000000))*(x1245))));
01858 sj5array[0]=IKsin(j5array[0]);
01859 cj5array[0]=IKcos(j5array[0]);
01860 j5array[1]=((IkReal(3.14159265358979))+(x1246)+(((IkReal(-1.00000000000000))*(x1245))));
01861 sj5array[1]=IKsin(j5array[1]);
01862 cj5array[1]=IKcos(j5array[1]);
01863 if( j5array[0] > IKPI )
01864 {
01865     j5array[0]-=IK2PI;
01866 }
01867 else if( j5array[0] < -IKPI )
01868 {    j5array[0]+=IK2PI;
01869 }
01870 j5valid[0] = true;
01871 if( j5array[1] > IKPI )
01872 {
01873     j5array[1]-=IK2PI;
01874 }
01875 else if( j5array[1] < -IKPI )
01876 {    j5array[1]+=IK2PI;
01877 }
01878 j5valid[1] = true;
01879 for(int ij5 = 0; ij5 < 2; ++ij5)
01880 {
01881 if( !j5valid[ij5] )
01882 {
01883     continue;
01884 }
01885 _ij5[0] = ij5; _ij5[1] = -1;
01886 for(int iij5 = ij5+1; iij5 < 2; ++iij5)
01887 {
01888 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01889 {
01890     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01891 }
01892 }
01893 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01894 {
01895 IkReal evalcond[1];
01896 evalcond[0]=((((IkReal(0.0946500000000000))*(r21)*(IKcos(j5))))+(pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1)))+(((IkReal(0.0946500000000000))*(r20)*(IKsin(j5)))));
01897 if( IKabs(evalcond[0]) > 0.000001  )
01898 {
01899 continue;
01900 }
01901 }
01902 
01903 {
01904 IkReal dummyeval[1];
01905 IkReal gconst0;
01906 IkReal x1247=((IkReal(20000.0000000000))*(cj5));
01907 IkReal x1248=((IkReal(20000.0000000000))*(sj5));
01908 gconst0=IKsign(((((IkReal(-1.00000000000000))*(px)*(r11)*(x1247)))+(((py)*(r01)*(x1247)))+(((py)*(r00)*(x1248)))+(((IkReal(-1.00000000000000))*(px)*(r10)*(x1248)))));
01909 IkReal x1249=((IkReal(1.00000000000000))*(px));
01910 dummyeval[0]=((((cj5)*(py)*(r01)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1249)))+(((py)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1249))));
01911 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01912 {
01913 {
01914 IkReal dummyeval[1];
01915 IkReal gconst2;
01916 gconst2=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
01917 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
01918 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01919 {
01920 {
01921 IkReal dummyeval[1];
01922 IkReal gconst1;
01923 IkReal x1250=(cj5)*(cj5);
01924 IkReal x1251=(sj5)*(sj5);
01925 IkReal x1252=((IkReal(1.00000000000000))*(x1251));
01926 IkReal x1253=((IkReal(2.00000000000000))*(cj5)*(sj5));
01927 IkReal x1254=((IkReal(1.00000000000000))*(x1250));
01928 gconst1=IKsign(((((IkReal(-1.00000000000000))*(x1252)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1253)))+(((IkReal(-1.00000000000000))*(x1254)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1253)))+(((IkReal(-1.00000000000000))*(x1254)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1252)*((r00)*(r00))))));
01929 IkReal x1255=(cj5)*(cj5);
01930 IkReal x1256=(sj5)*(sj5);
01931 IkReal x1257=((IkReal(1.00000000000000))*(x1256));
01932 IkReal x1258=((IkReal(2.00000000000000))*(cj5)*(sj5));
01933 IkReal x1259=((IkReal(1.00000000000000))*(x1255));
01934 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r01)*(x1258)))+(((IkReal(-1.00000000000000))*(x1259)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1258)))+(((IkReal(-1.00000000000000))*(x1257)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1259)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1257)*((r10)*(r10)))));
01935 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01936 {
01937 continue;
01938 
01939 } else
01940 {
01941 {
01942 IkReal j0array[1], cj0array[1], sj0array[1];
01943 bool j0valid[1]={false};
01944 _nj0 = 1;
01945 IkReal x1260=((sj2)*(sj3));
01946 IkReal x1261=((cj5)*(r01));
01947 IkReal x1262=((cj5)*(r11));
01948 IkReal x1263=((r00)*(sj5));
01949 IkReal x1264=((r10)*(sj5));
01950 IkReal x1265=((sj1)*(x1263));
01951 IkReal x1266=((IkReal(1.00000000000000))*(cj2)*(cj3));
01952 IkReal x1267=((sj1)*(x1264));
01953 IkReal x1268=((IkReal(1.00000000000000))*(cj1)*(cj2)*(sj3));
01954 IkReal x1269=((IkReal(1.00000000000000))*(cj1)*(cj3)*(sj2));
01955 if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x1264)*(x1268)))+(((IkReal(-1.00000000000000))*(x1264)*(x1269)))+(((IkReal(-1.00000000000000))*(x1266)*(x1267)))+(((IkReal(-1.00000000000000))*(x1262)*(x1269)))+(((IkReal(-1.00000000000000))*(x1262)*(x1268)))+(((x1260)*(x1267)))+(((sj1)*(x1260)*(x1262)))+(((IkReal(-1.00000000000000))*(sj1)*(x1262)*(x1266))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x1261)*(x1269)))+(((IkReal(-1.00000000000000))*(x1261)*(x1268)))+(((IkReal(-1.00000000000000))*(sj1)*(x1261)*(x1266)))+(((IkReal(-1.00000000000000))*(x1263)*(x1269)))+(((IkReal(-1.00000000000000))*(x1263)*(x1268)))+(((x1260)*(x1265)))+(((IkReal(-1.00000000000000))*(x1265)*(x1266)))+(((sj1)*(x1260)*(x1261))))))) < IKFAST_ATAN2_MAGTHRESH )
01956     continue;
01957 j0array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(x1264)*(x1268)))+(((IkReal(-1.00000000000000))*(x1264)*(x1269)))+(((IkReal(-1.00000000000000))*(x1266)*(x1267)))+(((IkReal(-1.00000000000000))*(x1262)*(x1269)))+(((IkReal(-1.00000000000000))*(x1262)*(x1268)))+(((x1260)*(x1267)))+(((sj1)*(x1260)*(x1262)))+(((IkReal(-1.00000000000000))*(sj1)*(x1262)*(x1266)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x1261)*(x1269)))+(((IkReal(-1.00000000000000))*(x1261)*(x1268)))+(((IkReal(-1.00000000000000))*(sj1)*(x1261)*(x1266)))+(((IkReal(-1.00000000000000))*(x1263)*(x1269)))+(((IkReal(-1.00000000000000))*(x1263)*(x1268)))+(((x1260)*(x1265)))+(((IkReal(-1.00000000000000))*(x1265)*(x1266)))+(((sj1)*(x1260)*(x1261)))))));
01958 sj0array[0]=IKsin(j0array[0]);
01959 cj0array[0]=IKcos(j0array[0]);
01960 if( j0array[0] > IKPI )
01961 {
01962     j0array[0]-=IK2PI;
01963 }
01964 else if( j0array[0] < -IKPI )
01965 {    j0array[0]+=IK2PI;
01966 }
01967 j0valid[0] = true;
01968 for(int ij0 = 0; ij0 < 1; ++ij0)
01969 {
01970 if( !j0valid[ij0] )
01971 {
01972     continue;
01973 }
01974 _ij0[0] = ij0; _ij0[1] = -1;
01975 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
01976 {
01977 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
01978 {
01979     j0valid[iij0]=false; _ij0[1] = iij0; break; 
01980 }
01981 }
01982 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
01983 {
01984 IkReal evalcond[4];
01985 IkReal x1270=IKcos(j0);
01986 IkReal x1271=IKsin(j0);
01987 IkReal x1272=((r00)*(sj5));
01988 IkReal x1273=((IkReal(1.00000000000000))*(cj2));
01989 IkReal x1274=((cj5)*(r11));
01990 IkReal x1275=((sj1)*(sj2));
01991 IkReal x1276=((r10)*(sj5));
01992 IkReal x1277=((IkReal(0.0946500000000000))*(x1271));
01993 IkReal x1278=((IkReal(0.0946500000000000))*(x1270));
01994 IkReal x1279=((IkReal(1.00000000000000))*(x1271));
01995 IkReal x1280=((IkReal(1.00000000000000))*(x1270));
01996 IkReal x1281=((cj5)*(r01)*(x1271));
01997 IkReal x1282=((cj5)*(x1278));
01998 evalcond[0]=((((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1279)))+(((IkReal(-1.00000000000000))*(x1272)*(x1279)))+(((x1270)*(x1276)))+(((x1270)*(x1274))));
01999 evalcond[1]=((IkReal(0.109150000000000))+(((x1272)*(x1277)))+(((IkReal(-1.00000000000000))*(x1276)*(x1278)))+(((IkReal(-1.00000000000000))*(x1274)*(x1278)))+(((cj5)*(r01)*(x1277)))+(((px)*(x1271)))+(((IkReal(-1.00000000000000))*(py)*(x1280))));
02000 evalcond[2]=((((IkReal(-1.00000000000000))*(py)*(x1279)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(px)*(x1280)))+(((IkReal(-1.00000000000000))*(x1276)*(x1277)))+(((IkReal(-0.392250000000000))*(x1275)))+(((IkReal(-1.00000000000000))*(r01)*(x1282)))+(((IkReal(-1.00000000000000))*(x1272)*(x1278)))+(((IkReal(-1.00000000000000))*(x1274)*(x1277))));
02001 evalcond[3]=((((sj3)*(x1275)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1273)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1273)))+(((x1270)*(x1272)))+(((x1271)*(x1276)))+(((x1271)*(x1274)))+(((cj5)*(r01)*(x1270))));
02002 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02003 {
02004 continue;
02005 }
02006 }
02007 
02008 {
02009 IkReal dummyeval[1];
02010 IkReal gconst3;
02011 IkReal x1283=((IkReal(1.00000000000000))*(sj0));
02012 IkReal x1284=((cj5)*(r22));
02013 IkReal x1285=((r21)*(sj5));
02014 IkReal x1286=((IkReal(1.00000000000000))*(cj0));
02015 IkReal x1287=((r22)*(sj5));
02016 IkReal x1288=((cj5)*(r20));
02017 gconst3=IKsign(((((r02)*(sj0)*(x1288)))+(((IkReal(-1.00000000000000))*(r02)*(x1283)*(x1285)))+(((cj0)*(r10)*(x1284)))+(((IkReal(-1.00000000000000))*(r00)*(x1283)*(x1284)))+(((r01)*(sj0)*(x1287)))+(((IkReal(-1.00000000000000))*(r12)*(x1286)*(x1288)))+(((IkReal(-1.00000000000000))*(r11)*(x1286)*(x1287)))+(((cj0)*(r12)*(x1285)))));
02018 IkReal x1289=((IkReal(1.00000000000000))*(sj0));
02019 IkReal x1290=((cj5)*(r22));
02020 IkReal x1291=((r21)*(sj5));
02021 IkReal x1292=((IkReal(1.00000000000000))*(cj0));
02022 IkReal x1293=((r22)*(sj5));
02023 IkReal x1294=((cj5)*(r20));
02024 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(x1289)*(x1291)))+(((IkReal(-1.00000000000000))*(r00)*(x1289)*(x1290)))+(((r01)*(sj0)*(x1293)))+(((cj0)*(r12)*(x1291)))+(((IkReal(-1.00000000000000))*(r12)*(x1292)*(x1294)))+(((r02)*(sj0)*(x1294)))+(((cj0)*(r10)*(x1290)))+(((IkReal(-1.00000000000000))*(r11)*(x1292)*(x1293))));
02025 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02026 {
02027 {
02028 IkReal dummyeval[1];
02029 IkReal gconst4;
02030 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
02031 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
02032 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02033 {
02034 continue;
02035 
02036 } else
02037 {
02038 {
02039 IkReal j4array[1], cj4array[1], sj4array[1];
02040 bool j4valid[1]={false};
02041 _nj4 = 1;
02042 IkReal x1295=((cj3)*(sj2));
02043 IkReal x1296=((cj2)*(cj3));
02044 IkReal x1297=((cj2)*(sj3));
02045 IkReal x1298=((sj2)*(sj3));
02046 IkReal x1299=((r21)*(sj5));
02047 IkReal x1300=((IkReal(1.00000000000000))*(cj1));
02048 IkReal x1301=((IkReal(1.00000000000000))*(sj1));
02049 IkReal x1302=((r22)*(x1300));
02050 IkReal x1303=((cj5)*(r20)*(sj1));
02051 IkReal x1304=((cj1)*(cj5)*(r20));
02052 if( IKabs(((gconst4)*(((((r22)*(sj1)*(x1298)))+(((IkReal(-1.00000000000000))*(x1297)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1296)*(x1301)))+(((IkReal(-1.00000000000000))*(x1295)*(x1302))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1296)*(x1299)*(x1301)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1298)*(x1301)))+(((IkReal(-1.00000000000000))*(x1297)*(x1299)*(x1300)))+(((x1295)*(x1304)))+(((IkReal(-1.00000000000000))*(x1295)*(x1299)*(x1300)))+(((x1297)*(x1304)))+(((x1296)*(x1303)))+(((sj1)*(x1298)*(x1299))))))) < IKFAST_ATAN2_MAGTHRESH )
02053     continue;
02054 j4array[0]=IKatan2(((gconst4)*(((((r22)*(sj1)*(x1298)))+(((IkReal(-1.00000000000000))*(x1297)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1296)*(x1301)))+(((IkReal(-1.00000000000000))*(x1295)*(x1302)))))), ((gconst4)*(((((IkReal(-1.00000000000000))*(x1296)*(x1299)*(x1301)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1298)*(x1301)))+(((IkReal(-1.00000000000000))*(x1297)*(x1299)*(x1300)))+(((x1295)*(x1304)))+(((IkReal(-1.00000000000000))*(x1295)*(x1299)*(x1300)))+(((x1297)*(x1304)))+(((x1296)*(x1303)))+(((sj1)*(x1298)*(x1299)))))));
02055 sj4array[0]=IKsin(j4array[0]);
02056 cj4array[0]=IKcos(j4array[0]);
02057 if( j4array[0] > IKPI )
02058 {
02059     j4array[0]-=IK2PI;
02060 }
02061 else if( j4array[0] < -IKPI )
02062 {    j4array[0]+=IK2PI;
02063 }
02064 j4valid[0] = true;
02065 for(int ij4 = 0; ij4 < 1; ++ij4)
02066 {
02067 if( !j4valid[ij4] )
02068 {
02069     continue;
02070 }
02071 _ij4[0] = ij4; _ij4[1] = -1;
02072 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02073 {
02074 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02075 {
02076     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02077 }
02078 }
02079 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02080 {
02081 IkReal evalcond[6];
02082 IkReal x1305=IKsin(j4);
02083 IkReal x1306=IKcos(j4);
02084 IkReal x1307=((IkReal(1.00000000000000))*(cj2));
02085 IkReal x1308=((cj3)*(sj1));
02086 IkReal x1309=((cj1)*(sj3));
02087 IkReal x1310=((IkReal(1.00000000000000))*(cj5));
02088 IkReal x1311=((IkReal(1.00000000000000))*(r01));
02089 IkReal x1312=((cj1)*(cj3));
02090 IkReal x1313=((r11)*(sj5));
02091 IkReal x1314=((sj1)*(sj3));
02092 IkReal x1315=((r01)*(sj5));
02093 IkReal x1316=((r21)*(sj5));
02094 IkReal x1317=((cj0)*(x1305));
02095 IkReal x1318=((cj5)*(x1305));
02096 IkReal x1319=((sj0)*(x1306));
02097 IkReal x1320=((IkReal(1.00000000000000))*(x1305));
02098 IkReal x1321=((cj0)*(x1306));
02099 IkReal x1322=((sj0)*(sj5)*(x1305));
02100 IkReal x1323=((IkReal(1.00000000000000))*(x1321));
02101 evalcond[0]=((((r22)*(x1306)))+(((r20)*(x1318)))+(((IkReal(-1.00000000000000))*(x1316)*(x1320))));
02102 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x1311)*(x1319)))+(((x1313)*(x1321)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1320)))+(((cj5)*(r00)*(x1319)))+(((r12)*(x1317)))+(((IkReal(-1.00000000000000))*(r10)*(x1310)*(x1321))));
02103 evalcond[2]=((((sj0)*(x1305)*(x1313)))+(((IkReal(-1.00000000000000))*(r12)*(x1319)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1305)*(x1310)))+(((IkReal(-1.00000000000000))*(r02)*(x1323)))+(((IkReal(-1.00000000000000))*(r00)*(x1310)*(x1317)))+(((x1315)*(x1317))));
02104 evalcond[3]=((IkReal(1.00000000000000))+(((x1313)*(x1317)))+(((IkReal(-1.00000000000000))*(r12)*(x1323)))+(((r02)*(x1319)))+(((IkReal(-1.00000000000000))*(r10)*(x1310)*(x1317)))+(((r00)*(sj0)*(x1318)))+(((IkReal(-1.00000000000000))*(x1311)*(x1322))));
02105 evalcond[4]=((((cj5)*(r20)*(x1306)))+(((IkReal(-1.00000000000000))*(x1307)*(x1308)))+(((IkReal(-1.00000000000000))*(x1307)*(x1309)))+(((sj2)*(x1314)))+(((IkReal(-1.00000000000000))*(r22)*(x1320)))+(((IkReal(-1.00000000000000))*(x1306)*(x1316)))+(((IkReal(-1.00000000000000))*(sj2)*(x1312))));
02106 evalcond[5]=((((x1313)*(x1319)))+(((sj2)*(x1309)))+(((sj2)*(x1308)))+(((r12)*(sj0)*(x1305)))+(((IkReal(-1.00000000000000))*(x1307)*(x1312)))+(((cj2)*(x1314)))+(((r02)*(x1317)))+(((IkReal(-1.00000000000000))*(r10)*(x1310)*(x1319)))+(((IkReal(-1.00000000000000))*(r00)*(x1310)*(x1321)))+(((x1315)*(x1321))));
02107 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  )
02108 {
02109 continue;
02110 }
02111 }
02112 
02113 {
02114 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02115 vinfos[0].jointtype = 1;
02116 vinfos[0].foffset = j0;
02117 vinfos[0].indices[0] = _ij0[0];
02118 vinfos[0].indices[1] = _ij0[1];
02119 vinfos[0].maxsolutions = _nj0;
02120 vinfos[1].jointtype = 1;
02121 vinfos[1].foffset = j1;
02122 vinfos[1].indices[0] = _ij1[0];
02123 vinfos[1].indices[1] = _ij1[1];
02124 vinfos[1].maxsolutions = _nj1;
02125 vinfos[2].jointtype = 1;
02126 vinfos[2].foffset = j2;
02127 vinfos[2].indices[0] = _ij2[0];
02128 vinfos[2].indices[1] = _ij2[1];
02129 vinfos[2].maxsolutions = _nj2;
02130 vinfos[3].jointtype = 1;
02131 vinfos[3].foffset = j3;
02132 vinfos[3].indices[0] = _ij3[0];
02133 vinfos[3].indices[1] = _ij3[1];
02134 vinfos[3].maxsolutions = _nj3;
02135 vinfos[4].jointtype = 1;
02136 vinfos[4].foffset = j4;
02137 vinfos[4].indices[0] = _ij4[0];
02138 vinfos[4].indices[1] = _ij4[1];
02139 vinfos[4].maxsolutions = _nj4;
02140 vinfos[5].jointtype = 1;
02141 vinfos[5].foffset = j5;
02142 vinfos[5].indices[0] = _ij5[0];
02143 vinfos[5].indices[1] = _ij5[1];
02144 vinfos[5].maxsolutions = _nj5;
02145 std::vector<int> vfree(0);
02146 solutions.AddSolution(vinfos,vfree);
02147 }
02148 }
02149 }
02150 
02151 }
02152 
02153 }
02154 
02155 } else
02156 {
02157 {
02158 IkReal j4array[1], cj4array[1], sj4array[1];
02159 bool j4valid[1]={false};
02160 _nj4 = 1;
02161 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
02162     continue;
02163 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
02164 sj4array[0]=IKsin(j4array[0]);
02165 cj4array[0]=IKcos(j4array[0]);
02166 if( j4array[0] > IKPI )
02167 {
02168     j4array[0]-=IK2PI;
02169 }
02170 else if( j4array[0] < -IKPI )
02171 {    j4array[0]+=IK2PI;
02172 }
02173 j4valid[0] = true;
02174 for(int ij4 = 0; ij4 < 1; ++ij4)
02175 {
02176 if( !j4valid[ij4] )
02177 {
02178     continue;
02179 }
02180 _ij4[0] = ij4; _ij4[1] = -1;
02181 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02182 {
02183 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02184 {
02185     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02186 }
02187 }
02188 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02189 {
02190 IkReal evalcond[6];
02191 IkReal x1324=IKsin(j4);
02192 IkReal x1325=IKcos(j4);
02193 IkReal x1326=((IkReal(1.00000000000000))*(cj2));
02194 IkReal x1327=((cj3)*(sj1));
02195 IkReal x1328=((cj1)*(sj3));
02196 IkReal x1329=((IkReal(1.00000000000000))*(cj5));
02197 IkReal x1330=((IkReal(1.00000000000000))*(r01));
02198 IkReal x1331=((cj1)*(cj3));
02199 IkReal x1332=((r11)*(sj5));
02200 IkReal x1333=((sj1)*(sj3));
02201 IkReal x1334=((r01)*(sj5));
02202 IkReal x1335=((r21)*(sj5));
02203 IkReal x1336=((cj0)*(x1324));
02204 IkReal x1337=((cj5)*(x1324));
02205 IkReal x1338=((sj0)*(x1325));
02206 IkReal x1339=((IkReal(1.00000000000000))*(x1324));
02207 IkReal x1340=((cj0)*(x1325));
02208 IkReal x1341=((sj0)*(sj5)*(x1324));
02209 IkReal x1342=((IkReal(1.00000000000000))*(x1340));
02210 evalcond[0]=((((IkReal(-1.00000000000000))*(x1335)*(x1339)))+(((r20)*(x1337)))+(((r22)*(x1325))));
02211 evalcond[1]=((((IkReal(-1.00000000000000))*(r10)*(x1329)*(x1340)))+(((r12)*(x1336)))+(((cj5)*(r00)*(x1338)))+(((x1332)*(x1340)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1339)))+(((IkReal(-1.00000000000000))*(sj5)*(x1330)*(x1338))));
02212 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x1342)))+(((IkReal(-1.00000000000000))*(r12)*(x1338)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1324)*(x1329)))+(((IkReal(-1.00000000000000))*(r00)*(x1329)*(x1336)))+(((sj0)*(x1324)*(x1332)))+(((x1334)*(x1336))));
02213 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(sj0)*(x1337)))+(((IkReal(-1.00000000000000))*(r12)*(x1342)))+(((IkReal(-1.00000000000000))*(r10)*(x1329)*(x1336)))+(((IkReal(-1.00000000000000))*(x1330)*(x1341)))+(((r02)*(x1338)))+(((x1332)*(x1336))));
02214 evalcond[4]=((((IkReal(-1.00000000000000))*(r22)*(x1339)))+(((IkReal(-1.00000000000000))*(x1325)*(x1335)))+(((IkReal(-1.00000000000000))*(x1326)*(x1327)))+(((IkReal(-1.00000000000000))*(x1326)*(x1328)))+(((sj2)*(x1333)))+(((IkReal(-1.00000000000000))*(sj2)*(x1331)))+(((cj5)*(r20)*(x1325))));
02215 evalcond[5]=((((IkReal(-1.00000000000000))*(x1326)*(x1331)))+(((sj2)*(x1327)))+(((sj2)*(x1328)))+(((IkReal(-1.00000000000000))*(r10)*(x1329)*(x1338)))+(((r12)*(sj0)*(x1324)))+(((x1334)*(x1340)))+(((cj2)*(x1333)))+(((IkReal(-1.00000000000000))*(r00)*(x1329)*(x1340)))+(((r02)*(x1336)))+(((x1332)*(x1338))));
02216 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  )
02217 {
02218 continue;
02219 }
02220 }
02221 
02222 {
02223 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02224 vinfos[0].jointtype = 1;
02225 vinfos[0].foffset = j0;
02226 vinfos[0].indices[0] = _ij0[0];
02227 vinfos[0].indices[1] = _ij0[1];
02228 vinfos[0].maxsolutions = _nj0;
02229 vinfos[1].jointtype = 1;
02230 vinfos[1].foffset = j1;
02231 vinfos[1].indices[0] = _ij1[0];
02232 vinfos[1].indices[1] = _ij1[1];
02233 vinfos[1].maxsolutions = _nj1;
02234 vinfos[2].jointtype = 1;
02235 vinfos[2].foffset = j2;
02236 vinfos[2].indices[0] = _ij2[0];
02237 vinfos[2].indices[1] = _ij2[1];
02238 vinfos[2].maxsolutions = _nj2;
02239 vinfos[3].jointtype = 1;
02240 vinfos[3].foffset = j3;
02241 vinfos[3].indices[0] = _ij3[0];
02242 vinfos[3].indices[1] = _ij3[1];
02243 vinfos[3].maxsolutions = _nj3;
02244 vinfos[4].jointtype = 1;
02245 vinfos[4].foffset = j4;
02246 vinfos[4].indices[0] = _ij4[0];
02247 vinfos[4].indices[1] = _ij4[1];
02248 vinfos[4].maxsolutions = _nj4;
02249 vinfos[5].jointtype = 1;
02250 vinfos[5].foffset = j5;
02251 vinfos[5].indices[0] = _ij5[0];
02252 vinfos[5].indices[1] = _ij5[1];
02253 vinfos[5].maxsolutions = _nj5;
02254 std::vector<int> vfree(0);
02255 solutions.AddSolution(vinfos,vfree);
02256 }
02257 }
02258 }
02259 
02260 }
02261 
02262 }
02263 }
02264 }
02265 
02266 }
02267 
02268 }
02269 
02270 } else
02271 {
02272 {
02273 IkReal j4array[1], cj4array[1], sj4array[1];
02274 bool j4valid[1]={false};
02275 _nj4 = 1;
02276 IkReal x1343=((cj3)*(sj2));
02277 IkReal x1344=((cj2)*(sj3));
02278 IkReal x1345=((cj5)*(r20));
02279 IkReal x1346=((r21)*(sj5));
02280 IkReal x1347=((IkReal(1.00000000000000))*(cj1));
02281 IkReal x1348=((r22)*(x1347));
02282 IkReal x1349=((sj1)*(sj2)*(sj3));
02283 IkReal x1350=((cj2)*(cj3)*(sj1));
02284 if( IKabs(((gconst2)*(((((r22)*(x1349)))+(((IkReal(-1.00000000000000))*(r22)*(x1350)))+(((IkReal(-1.00000000000000))*(x1343)*(x1348)))+(((IkReal(-1.00000000000000))*(x1344)*(x1348))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x1346)*(x1350)))+(((x1345)*(x1350)))+(((IkReal(-1.00000000000000))*(x1344)*(x1346)*(x1347)))+(((x1346)*(x1349)))+(((cj1)*(x1344)*(x1345)))+(((IkReal(-1.00000000000000))*(x1345)*(x1349)))+(((cj1)*(x1343)*(x1345)))+(((IkReal(-1.00000000000000))*(x1343)*(x1346)*(x1347))))))) < IKFAST_ATAN2_MAGTHRESH )
02285     continue;
02286 j4array[0]=IKatan2(((gconst2)*(((((r22)*(x1349)))+(((IkReal(-1.00000000000000))*(r22)*(x1350)))+(((IkReal(-1.00000000000000))*(x1343)*(x1348)))+(((IkReal(-1.00000000000000))*(x1344)*(x1348)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(x1346)*(x1350)))+(((x1345)*(x1350)))+(((IkReal(-1.00000000000000))*(x1344)*(x1346)*(x1347)))+(((x1346)*(x1349)))+(((cj1)*(x1344)*(x1345)))+(((IkReal(-1.00000000000000))*(x1345)*(x1349)))+(((cj1)*(x1343)*(x1345)))+(((IkReal(-1.00000000000000))*(x1343)*(x1346)*(x1347)))))));
02287 sj4array[0]=IKsin(j4array[0]);
02288 cj4array[0]=IKcos(j4array[0]);
02289 if( j4array[0] > IKPI )
02290 {
02291     j4array[0]-=IK2PI;
02292 }
02293 else if( j4array[0] < -IKPI )
02294 {    j4array[0]+=IK2PI;
02295 }
02296 j4valid[0] = true;
02297 for(int ij4 = 0; ij4 < 1; ++ij4)
02298 {
02299 if( !j4valid[ij4] )
02300 {
02301     continue;
02302 }
02303 _ij4[0] = ij4; _ij4[1] = -1;
02304 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02305 {
02306 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02307 {
02308     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02309 }
02310 }
02311 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02312 {
02313 IkReal evalcond[2];
02314 IkReal x1351=IKsin(j4);
02315 IkReal x1352=IKcos(j4);
02316 IkReal x1353=((IkReal(1.00000000000000))*(cj1));
02317 IkReal x1354=((cj5)*(r20));
02318 IkReal x1355=((IkReal(1.00000000000000))*(r21)*(sj5));
02319 evalcond[0]=((((r22)*(x1352)))+(((x1351)*(x1354)))+(((IkReal(-1.00000000000000))*(x1351)*(x1355))));
02320 evalcond[1]=((((IkReal(-1.00000000000000))*(x1352)*(x1355)))+(((IkReal(-1.00000000000000))*(r22)*(x1351)))+(((IkReal(-1.00000000000000))*(cj2)*(cj3)*(sj1)))+(((IkReal(-1.00000000000000))*(cj2)*(sj3)*(x1353)))+(((sj1)*(sj2)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(sj2)*(x1353)))+(((x1352)*(x1354))));
02321 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02322 {
02323 continue;
02324 }
02325 }
02326 
02327 {
02328 IkReal dummyeval[1];
02329 IkReal gconst5;
02330 IkReal x1356=((IkReal(20000.0000000000))*(cj5));
02331 IkReal x1357=((IkReal(20000.0000000000))*(sj5));
02332 gconst5=IKsign(((((IkReal(-1.00000000000000))*(px)*(r11)*(x1356)))+(((IkReal(-1.00000000000000))*(px)*(r10)*(x1357)))+(((py)*(r00)*(x1357)))+(((py)*(r01)*(x1356)))));
02333 IkReal x1358=((IkReal(1.00000000000000))*(px));
02334 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1358)))+(((cj5)*(py)*(r01)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1358)))+(((py)*(r00)*(sj5))));
02335 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02336 {
02337 {
02338 IkReal dummyeval[1];
02339 IkReal gconst6;
02340 IkReal x1359=(sj5)*(sj5);
02341 IkReal x1360=(cj5)*(cj5);
02342 IkReal x1361=((cj4)*(sj5));
02343 IkReal x1362=((IkReal(1.00000000000000))*(r02));
02344 IkReal x1363=((r01)*(r10));
02345 IkReal x1364=((cj4)*(cj5));
02346 IkReal x1365=((sj4)*(x1360));
02347 IkReal x1366=((IkReal(1.00000000000000))*(r00)*(r11));
02348 IkReal x1367=((sj4)*(x1359));
02349 gconst6=IKsign(((((IkReal(-1.00000000000000))*(x1366)*(x1367)))+(((x1363)*(x1367)))+(((x1363)*(x1365)))+(((IkReal(-1.00000000000000))*(r11)*(x1362)*(x1364)))+(((IkReal(-1.00000000000000))*(r10)*(x1361)*(x1362)))+(((r00)*(r12)*(x1361)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)))+(((r01)*(r12)*(x1364)))));
02350 IkReal x1368=(sj5)*(sj5);
02351 IkReal x1369=(cj5)*(cj5);
02352 IkReal x1370=((cj4)*(sj5));
02353 IkReal x1371=((IkReal(1.00000000000000))*(r02));
02354 IkReal x1372=((r01)*(r10));
02355 IkReal x1373=((cj4)*(cj5));
02356 IkReal x1374=((sj4)*(x1369));
02357 IkReal x1375=((IkReal(1.00000000000000))*(r00)*(r11));
02358 IkReal x1376=((sj4)*(x1368));
02359 dummyeval[0]=((((x1372)*(x1376)))+(((x1372)*(x1374)))+(((IkReal(-1.00000000000000))*(r11)*(x1371)*(x1373)))+(((IkReal(-1.00000000000000))*(x1375)*(x1376)))+(((r01)*(r12)*(x1373)))+(((IkReal(-1.00000000000000))*(x1374)*(x1375)))+(((IkReal(-1.00000000000000))*(r10)*(x1370)*(x1371)))+(((r00)*(r12)*(x1370))));
02360 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02361 {
02362 continue;
02363 
02364 } else
02365 {
02366 {
02367 IkReal j0array[1], cj0array[1], sj0array[1];
02368 bool j0valid[1]={false};
02369 _nj0 = 1;
02370 if( IKabs(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH )
02371     continue;
02372 j0array[0]=IKatan2(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5)))))), ((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01)))))));
02373 sj0array[0]=IKsin(j0array[0]);
02374 cj0array[0]=IKcos(j0array[0]);
02375 if( j0array[0] > IKPI )
02376 {
02377     j0array[0]-=IK2PI;
02378 }
02379 else if( j0array[0] < -IKPI )
02380 {    j0array[0]+=IK2PI;
02381 }
02382 j0valid[0] = true;
02383 for(int ij0 = 0; ij0 < 1; ++ij0)
02384 {
02385 if( !j0valid[ij0] )
02386 {
02387     continue;
02388 }
02389 _ij0[0] = ij0; _ij0[1] = -1;
02390 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
02391 {
02392 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
02393 {
02394     j0valid[iij0]=false; _ij0[1] = iij0; break; 
02395 }
02396 }
02397 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
02398 {
02399 IkReal evalcond[8];
02400 IkReal x1377=IKcos(j0);
02401 IkReal x1378=IKsin(j0);
02402 IkReal x1379=((IkReal(1.00000000000000))*(cj5));
02403 IkReal x1380=((IkReal(0.0946500000000000))*(sj5));
02404 IkReal x1381=((IkReal(1.00000000000000))*(cj2));
02405 IkReal x1382=((cj3)*(sj1));
02406 IkReal x1383=((cj1)*(sj3));
02407 IkReal x1384=((IkReal(0.0946500000000000))*(cj5));
02408 IkReal x1385=((r02)*(sj4));
02409 IkReal x1386=((cj1)*(cj3));
02410 IkReal x1387=((sj1)*(sj2));
02411 IkReal x1388=((sj4)*(sj5));
02412 IkReal x1389=((IkReal(1.00000000000000))*(sj5));
02413 IkReal x1390=((cj4)*(r02));
02414 IkReal x1391=((cj4)*(r12));
02415 IkReal x1392=((cj4)*(sj5));
02416 IkReal x1393=((r12)*(sj4));
02417 IkReal x1394=((r01)*(x1378));
02418 IkReal x1395=((r00)*(x1377));
02419 IkReal x1396=((r00)*(x1378));
02420 IkReal x1397=((r11)*(x1377));
02421 IkReal x1398=((r01)*(x1377));
02422 IkReal x1399=((r11)*(x1378));
02423 IkReal x1400=((r10)*(x1378));
02424 IkReal x1401=((IkReal(1.00000000000000))*(x1378));
02425 IkReal x1402=((r10)*(x1377));
02426 IkReal x1403=((IkReal(1.00000000000000))*(x1377));
02427 evalcond[0]=((((IkReal(-1.00000000000000))*(x1389)*(x1396)))+(((sj5)*(x1402)))+(((cj5)*(x1397)))+(((IkReal(-1.00000000000000))*(x1379)*(x1394))));
02428 evalcond[1]=((IkReal(0.109150000000000))+(((IkReal(-1.00000000000000))*(x1384)*(x1397)))+(((px)*(x1378)))+(((IkReal(-1.00000000000000))*(x1380)*(x1402)))+(((x1384)*(x1394)))+(((IkReal(-1.00000000000000))*(py)*(x1403)))+(((x1380)*(x1396))));
02429 evalcond[2]=((((IkReal(-1.00000000000000))*(cj4)*(x1379)*(x1402)))+(((IkReal(-1.00000000000000))*(cj4)*(x1389)*(x1394)))+(((x1377)*(x1393)))+(((IkReal(-1.00000000000000))*(x1385)*(x1401)))+(((cj4)*(cj5)*(x1396)))+(((x1392)*(x1397))));
02430 evalcond[3]=((((x1388)*(x1398)))+(((x1388)*(x1399)))+(((IkReal(-1.00000000000000))*(sj4)*(x1379)*(x1400)))+(((IkReal(-1.00000000000000))*(x1390)*(x1403)))+(((IkReal(-1.00000000000000))*(x1391)*(x1401)))+(((IkReal(-1.00000000000000))*(sj4)*(x1379)*(x1395))));
02431 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1388)*(x1394)))+(((x1388)*(x1397)))+(((IkReal(-1.00000000000000))*(sj4)*(x1379)*(x1402)))+(((cj5)*(sj4)*(x1396)))+(((IkReal(-1.00000000000000))*(x1391)*(x1403)))+(((x1378)*(x1390))));
02432 evalcond[5]=((((IkReal(-1.00000000000000))*(x1384)*(x1399)))+(((IkReal(-1.00000000000000))*(x1384)*(x1398)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-0.392250000000000))*(x1387)))+(((IkReal(-1.00000000000000))*(x1380)*(x1400)))+(((IkReal(-1.00000000000000))*(px)*(x1403)))+(((IkReal(-1.00000000000000))*(x1380)*(x1395)))+(((IkReal(-1.00000000000000))*(py)*(x1401))));
02433 evalcond[6]=((((IkReal(-1.00000000000000))*(sj2)*(x1386)))+(((sj5)*(x1400)))+(((cj5)*(x1398)))+(((cj5)*(x1399)))+(((sj3)*(x1387)))+(((IkReal(-1.00000000000000))*(x1381)*(x1383)))+(((IkReal(-1.00000000000000))*(x1381)*(x1382)))+(((sj5)*(x1395))));
02434 evalcond[7]=((((IkReal(-1.00000000000000))*(cj4)*(x1379)*(x1400)))+(((IkReal(-1.00000000000000))*(x1381)*(x1386)))+(((sj2)*(x1383)))+(((sj2)*(x1382)))+(((x1377)*(x1385)))+(((IkReal(-1.00000000000000))*(cj4)*(x1379)*(x1395)))+(((x1392)*(x1399)))+(((x1392)*(x1398)))+(((cj2)*(sj1)*(sj3)))+(((x1378)*(x1393))));
02435 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  || IKabs(evalcond[6]) > 0.000001  || IKabs(evalcond[7]) > 0.000001  )
02436 {
02437 continue;
02438 }
02439 }
02440 
02441 {
02442 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02443 vinfos[0].jointtype = 1;
02444 vinfos[0].foffset = j0;
02445 vinfos[0].indices[0] = _ij0[0];
02446 vinfos[0].indices[1] = _ij0[1];
02447 vinfos[0].maxsolutions = _nj0;
02448 vinfos[1].jointtype = 1;
02449 vinfos[1].foffset = j1;
02450 vinfos[1].indices[0] = _ij1[0];
02451 vinfos[1].indices[1] = _ij1[1];
02452 vinfos[1].maxsolutions = _nj1;
02453 vinfos[2].jointtype = 1;
02454 vinfos[2].foffset = j2;
02455 vinfos[2].indices[0] = _ij2[0];
02456 vinfos[2].indices[1] = _ij2[1];
02457 vinfos[2].maxsolutions = _nj2;
02458 vinfos[3].jointtype = 1;
02459 vinfos[3].foffset = j3;
02460 vinfos[3].indices[0] = _ij3[0];
02461 vinfos[3].indices[1] = _ij3[1];
02462 vinfos[3].maxsolutions = _nj3;
02463 vinfos[4].jointtype = 1;
02464 vinfos[4].foffset = j4;
02465 vinfos[4].indices[0] = _ij4[0];
02466 vinfos[4].indices[1] = _ij4[1];
02467 vinfos[4].maxsolutions = _nj4;
02468 vinfos[5].jointtype = 1;
02469 vinfos[5].foffset = j5;
02470 vinfos[5].indices[0] = _ij5[0];
02471 vinfos[5].indices[1] = _ij5[1];
02472 vinfos[5].maxsolutions = _nj5;
02473 std::vector<int> vfree(0);
02474 solutions.AddSolution(vinfos,vfree);
02475 }
02476 }
02477 }
02478 
02479 }
02480 
02481 }
02482 
02483 } else
02484 {
02485 {
02486 IkReal j0array[1], cj0array[1], sj0array[1];
02487 bool j0valid[1]={false};
02488 _nj0 = 1;
02489 IkReal x1404=((IkReal(2183.00000000000))*(sj5));
02490 IkReal x1405=((IkReal(2183.00000000000))*(cj5));
02491 if( IKabs(((gconst5)*(((((r11)*(x1405)))+(((r10)*(x1404))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((r00)*(x1404)))+(((r01)*(x1405))))))) < IKFAST_ATAN2_MAGTHRESH )
02492     continue;
02493 j0array[0]=IKatan2(((gconst5)*(((((r11)*(x1405)))+(((r10)*(x1404)))))), ((gconst5)*(((((r00)*(x1404)))+(((r01)*(x1405)))))));
02494 sj0array[0]=IKsin(j0array[0]);
02495 cj0array[0]=IKcos(j0array[0]);
02496 if( j0array[0] > IKPI )
02497 {
02498     j0array[0]-=IK2PI;
02499 }
02500 else if( j0array[0] < -IKPI )
02501 {    j0array[0]+=IK2PI;
02502 }
02503 j0valid[0] = true;
02504 for(int ij0 = 0; ij0 < 1; ++ij0)
02505 {
02506 if( !j0valid[ij0] )
02507 {
02508     continue;
02509 }
02510 _ij0[0] = ij0; _ij0[1] = -1;
02511 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
02512 {
02513 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
02514 {
02515     j0valid[iij0]=false; _ij0[1] = iij0; break; 
02516 }
02517 }
02518 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
02519 {
02520 IkReal evalcond[8];
02521 IkReal x1406=IKcos(j0);
02522 IkReal x1407=IKsin(j0);
02523 IkReal x1408=((IkReal(1.00000000000000))*(cj5));
02524 IkReal x1409=((IkReal(0.0946500000000000))*(sj5));
02525 IkReal x1410=((IkReal(1.00000000000000))*(cj2));
02526 IkReal x1411=((cj3)*(sj1));
02527 IkReal x1412=((cj1)*(sj3));
02528 IkReal x1413=((IkReal(0.0946500000000000))*(cj5));
02529 IkReal x1414=((r02)*(sj4));
02530 IkReal x1415=((cj1)*(cj3));
02531 IkReal x1416=((sj1)*(sj2));
02532 IkReal x1417=((sj4)*(sj5));
02533 IkReal x1418=((IkReal(1.00000000000000))*(sj5));
02534 IkReal x1419=((cj4)*(r02));
02535 IkReal x1420=((cj4)*(r12));
02536 IkReal x1421=((cj4)*(sj5));
02537 IkReal x1422=((r12)*(sj4));
02538 IkReal x1423=((r01)*(x1407));
02539 IkReal x1424=((r00)*(x1406));
02540 IkReal x1425=((r00)*(x1407));
02541 IkReal x1426=((r11)*(x1406));
02542 IkReal x1427=((r01)*(x1406));
02543 IkReal x1428=((r11)*(x1407));
02544 IkReal x1429=((r10)*(x1407));
02545 IkReal x1430=((IkReal(1.00000000000000))*(x1407));
02546 IkReal x1431=((r10)*(x1406));
02547 IkReal x1432=((IkReal(1.00000000000000))*(x1406));
02548 evalcond[0]=((((sj5)*(x1431)))+(((IkReal(-1.00000000000000))*(x1408)*(x1423)))+(((IkReal(-1.00000000000000))*(x1418)*(x1425)))+(((cj5)*(x1426))));
02549 evalcond[1]=((IkReal(0.109150000000000))+(((px)*(x1407)))+(((x1409)*(x1425)))+(((IkReal(-1.00000000000000))*(x1409)*(x1431)))+(((IkReal(-1.00000000000000))*(x1413)*(x1426)))+(((IkReal(-1.00000000000000))*(py)*(x1432)))+(((x1413)*(x1423))));
02550 evalcond[2]=((((x1406)*(x1422)))+(((IkReal(-1.00000000000000))*(cj4)*(x1408)*(x1431)))+(((cj4)*(cj5)*(x1425)))+(((IkReal(-1.00000000000000))*(x1414)*(x1430)))+(((IkReal(-1.00000000000000))*(cj4)*(x1418)*(x1423)))+(((x1421)*(x1426))));
02551 evalcond[3]=((((IkReal(-1.00000000000000))*(x1420)*(x1430)))+(((IkReal(-1.00000000000000))*(x1419)*(x1432)))+(((IkReal(-1.00000000000000))*(sj4)*(x1408)*(x1429)))+(((IkReal(-1.00000000000000))*(sj4)*(x1408)*(x1424)))+(((x1417)*(x1427)))+(((x1417)*(x1428))));
02552 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1420)*(x1432)))+(((x1407)*(x1419)))+(((IkReal(-1.00000000000000))*(x1417)*(x1423)))+(((IkReal(-1.00000000000000))*(sj4)*(x1408)*(x1431)))+(((x1417)*(x1426)))+(((cj5)*(sj4)*(x1425))));
02553 evalcond[5]=((((IkReal(-0.392250000000000))*(x1416)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x1409)*(x1429)))+(((IkReal(-1.00000000000000))*(x1409)*(x1424)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(px)*(x1432)))+(((IkReal(-1.00000000000000))*(x1413)*(x1427)))+(((IkReal(-1.00000000000000))*(x1413)*(x1428)))+(((IkReal(-1.00000000000000))*(py)*(x1430))));
02554 evalcond[6]=((((IkReal(-1.00000000000000))*(sj2)*(x1415)))+(((IkReal(-1.00000000000000))*(x1410)*(x1411)))+(((IkReal(-1.00000000000000))*(x1410)*(x1412)))+(((sj5)*(x1424)))+(((sj5)*(x1429)))+(((cj5)*(x1428)))+(((cj5)*(x1427)))+(((sj3)*(x1416))));
02555 evalcond[7]=((((sj2)*(x1411)))+(((sj2)*(x1412)))+(((x1406)*(x1414)))+(((x1407)*(x1422)))+(((IkReal(-1.00000000000000))*(x1410)*(x1415)))+(((IkReal(-1.00000000000000))*(cj4)*(x1408)*(x1429)))+(((IkReal(-1.00000000000000))*(cj4)*(x1408)*(x1424)))+(((cj2)*(sj1)*(sj3)))+(((x1421)*(x1427)))+(((x1421)*(x1428))));
02556 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  )
02557 {
02558 continue;
02559 }
02560 }
02561 
02562 {
02563 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02564 vinfos[0].jointtype = 1;
02565 vinfos[0].foffset = j0;
02566 vinfos[0].indices[0] = _ij0[0];
02567 vinfos[0].indices[1] = _ij0[1];
02568 vinfos[0].maxsolutions = _nj0;
02569 vinfos[1].jointtype = 1;
02570 vinfos[1].foffset = j1;
02571 vinfos[1].indices[0] = _ij1[0];
02572 vinfos[1].indices[1] = _ij1[1];
02573 vinfos[1].maxsolutions = _nj1;
02574 vinfos[2].jointtype = 1;
02575 vinfos[2].foffset = j2;
02576 vinfos[2].indices[0] = _ij2[0];
02577 vinfos[2].indices[1] = _ij2[1];
02578 vinfos[2].maxsolutions = _nj2;
02579 vinfos[3].jointtype = 1;
02580 vinfos[3].foffset = j3;
02581 vinfos[3].indices[0] = _ij3[0];
02582 vinfos[3].indices[1] = _ij3[1];
02583 vinfos[3].maxsolutions = _nj3;
02584 vinfos[4].jointtype = 1;
02585 vinfos[4].foffset = j4;
02586 vinfos[4].indices[0] = _ij4[0];
02587 vinfos[4].indices[1] = _ij4[1];
02588 vinfos[4].maxsolutions = _nj4;
02589 vinfos[5].jointtype = 1;
02590 vinfos[5].foffset = j5;
02591 vinfos[5].indices[0] = _ij5[0];
02592 vinfos[5].indices[1] = _ij5[1];
02593 vinfos[5].maxsolutions = _nj5;
02594 std::vector<int> vfree(0);
02595 solutions.AddSolution(vinfos,vfree);
02596 }
02597 }
02598 }
02599 
02600 }
02601 
02602 }
02603 }
02604 }
02605 
02606 }
02607 
02608 }
02609 
02610 } else
02611 {
02612 {
02613 IkReal j0array[1], cj0array[1], sj0array[1];
02614 bool j0valid[1]={false};
02615 _nj0 = 1;
02616 IkReal x1433=((IkReal(2183.00000000000))*(sj5));
02617 IkReal x1434=((IkReal(2183.00000000000))*(cj5));
02618 if( IKabs(((gconst0)*(((((r11)*(x1434)))+(((r10)*(x1433))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((r01)*(x1434)))+(((r00)*(x1433))))))) < IKFAST_ATAN2_MAGTHRESH )
02619     continue;
02620 j0array[0]=IKatan2(((gconst0)*(((((r11)*(x1434)))+(((r10)*(x1433)))))), ((gconst0)*(((((r01)*(x1434)))+(((r00)*(x1433)))))));
02621 sj0array[0]=IKsin(j0array[0]);
02622 cj0array[0]=IKcos(j0array[0]);
02623 if( j0array[0] > IKPI )
02624 {
02625     j0array[0]-=IK2PI;
02626 }
02627 else if( j0array[0] < -IKPI )
02628 {    j0array[0]+=IK2PI;
02629 }
02630 j0valid[0] = true;
02631 for(int ij0 = 0; ij0 < 1; ++ij0)
02632 {
02633 if( !j0valid[ij0] )
02634 {
02635     continue;
02636 }
02637 _ij0[0] = ij0; _ij0[1] = -1;
02638 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
02639 {
02640 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
02641 {
02642     j0valid[iij0]=false; _ij0[1] = iij0; break; 
02643 }
02644 }
02645 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
02646 {
02647 IkReal evalcond[4];
02648 IkReal x1435=IKcos(j0);
02649 IkReal x1436=IKsin(j0);
02650 IkReal x1437=((r00)*(sj5));
02651 IkReal x1438=((IkReal(1.00000000000000))*(cj2));
02652 IkReal x1439=((cj5)*(r11));
02653 IkReal x1440=((sj1)*(sj2));
02654 IkReal x1441=((r10)*(sj5));
02655 IkReal x1442=((IkReal(0.0946500000000000))*(x1436));
02656 IkReal x1443=((IkReal(0.0946500000000000))*(x1435));
02657 IkReal x1444=((IkReal(1.00000000000000))*(x1436));
02658 IkReal x1445=((IkReal(1.00000000000000))*(x1435));
02659 IkReal x1446=((cj5)*(r01)*(x1436));
02660 IkReal x1447=((cj5)*(x1443));
02661 evalcond[0]=((((x1435)*(x1441)))+(((x1435)*(x1439)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1444)))+(((IkReal(-1.00000000000000))*(x1437)*(x1444))));
02662 evalcond[1]=((IkReal(0.109150000000000))+(((x1437)*(x1442)))+(((px)*(x1436)))+(((IkReal(-1.00000000000000))*(x1439)*(x1443)))+(((IkReal(-1.00000000000000))*(x1441)*(x1443)))+(((IkReal(-1.00000000000000))*(py)*(x1445)))+(((cj5)*(r01)*(x1442))));
02663 evalcond[2]=((((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(r01)*(x1447)))+(((IkReal(-1.00000000000000))*(px)*(x1445)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1437)*(x1443)))+(((IkReal(-1.00000000000000))*(x1439)*(x1442)))+(((IkReal(-1.00000000000000))*(x1441)*(x1442)))+(((IkReal(-1.00000000000000))*(py)*(x1444)))+(((IkReal(-0.392250000000000))*(x1440))));
02664 evalcond[3]=((((x1436)*(x1439)))+(((cj5)*(r01)*(x1435)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((x1435)*(x1437)))+(((x1436)*(x1441)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1438)))+(((sj3)*(x1440)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1438))));
02665 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
02666 {
02667 continue;
02668 }
02669 }
02670 
02671 {
02672 IkReal dummyeval[1];
02673 IkReal gconst3;
02674 IkReal x1448=((IkReal(1.00000000000000))*(sj0));
02675 IkReal x1449=((cj5)*(r22));
02676 IkReal x1450=((r21)*(sj5));
02677 IkReal x1451=((IkReal(1.00000000000000))*(cj0));
02678 IkReal x1452=((r22)*(sj5));
02679 IkReal x1453=((cj5)*(r20));
02680 gconst3=IKsign(((((r02)*(sj0)*(x1453)))+(((IkReal(-1.00000000000000))*(r11)*(x1451)*(x1452)))+(((IkReal(-1.00000000000000))*(r02)*(x1448)*(x1450)))+(((IkReal(-1.00000000000000))*(r12)*(x1451)*(x1453)))+(((cj0)*(r12)*(x1450)))+(((r01)*(sj0)*(x1452)))+(((cj0)*(r10)*(x1449)))+(((IkReal(-1.00000000000000))*(r00)*(x1448)*(x1449)))));
02681 IkReal x1454=((IkReal(1.00000000000000))*(sj0));
02682 IkReal x1455=((cj5)*(r22));
02683 IkReal x1456=((r21)*(sj5));
02684 IkReal x1457=((IkReal(1.00000000000000))*(cj0));
02685 IkReal x1458=((r22)*(sj5));
02686 IkReal x1459=((cj5)*(r20));
02687 dummyeval[0]=((((r02)*(sj0)*(x1459)))+(((IkReal(-1.00000000000000))*(r12)*(x1457)*(x1459)))+(((IkReal(-1.00000000000000))*(r11)*(x1457)*(x1458)))+(((cj0)*(r12)*(x1456)))+(((r01)*(sj0)*(x1458)))+(((IkReal(-1.00000000000000))*(r02)*(x1454)*(x1456)))+(((IkReal(-1.00000000000000))*(r00)*(x1454)*(x1455)))+(((cj0)*(r10)*(x1455))));
02688 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02689 {
02690 {
02691 IkReal dummyeval[1];
02692 IkReal gconst4;
02693 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
02694 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
02695 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02696 {
02697 continue;
02698 
02699 } else
02700 {
02701 {
02702 IkReal j4array[1], cj4array[1], sj4array[1];
02703 bool j4valid[1]={false};
02704 _nj4 = 1;
02705 IkReal x1460=((cj3)*(sj2));
02706 IkReal x1461=((cj2)*(cj3));
02707 IkReal x1462=((cj2)*(sj3));
02708 IkReal x1463=((sj2)*(sj3));
02709 IkReal x1464=((r21)*(sj5));
02710 IkReal x1465=((IkReal(1.00000000000000))*(cj1));
02711 IkReal x1466=((IkReal(1.00000000000000))*(sj1));
02712 IkReal x1467=((r22)*(x1465));
02713 IkReal x1468=((cj5)*(r20)*(sj1));
02714 IkReal x1469=((cj1)*(cj5)*(r20));
02715 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1462)*(x1467)))+(((r22)*(sj1)*(x1463)))+(((IkReal(-1.00000000000000))*(x1460)*(x1467)))+(((IkReal(-1.00000000000000))*(r22)*(x1461)*(x1466))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1463)*(x1466)))+(((x1461)*(x1468)))+(((IkReal(-1.00000000000000))*(x1460)*(x1464)*(x1465)))+(((IkReal(-1.00000000000000))*(x1462)*(x1464)*(x1465)))+(((x1460)*(x1469)))+(((x1462)*(x1469)))+(((sj1)*(x1463)*(x1464)))+(((IkReal(-1.00000000000000))*(x1461)*(x1464)*(x1466))))))) < IKFAST_ATAN2_MAGTHRESH )
02716     continue;
02717 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(x1462)*(x1467)))+(((r22)*(sj1)*(x1463)))+(((IkReal(-1.00000000000000))*(x1460)*(x1467)))+(((IkReal(-1.00000000000000))*(r22)*(x1461)*(x1466)))))), ((gconst4)*(((((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1463)*(x1466)))+(((x1461)*(x1468)))+(((IkReal(-1.00000000000000))*(x1460)*(x1464)*(x1465)))+(((IkReal(-1.00000000000000))*(x1462)*(x1464)*(x1465)))+(((x1460)*(x1469)))+(((x1462)*(x1469)))+(((sj1)*(x1463)*(x1464)))+(((IkReal(-1.00000000000000))*(x1461)*(x1464)*(x1466)))))));
02718 sj4array[0]=IKsin(j4array[0]);
02719 cj4array[0]=IKcos(j4array[0]);
02720 if( j4array[0] > IKPI )
02721 {
02722     j4array[0]-=IK2PI;
02723 }
02724 else if( j4array[0] < -IKPI )
02725 {    j4array[0]+=IK2PI;
02726 }
02727 j4valid[0] = true;
02728 for(int ij4 = 0; ij4 < 1; ++ij4)
02729 {
02730 if( !j4valid[ij4] )
02731 {
02732     continue;
02733 }
02734 _ij4[0] = ij4; _ij4[1] = -1;
02735 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02736 {
02737 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02738 {
02739     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02740 }
02741 }
02742 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02743 {
02744 IkReal evalcond[6];
02745 IkReal x1470=IKsin(j4);
02746 IkReal x1471=IKcos(j4);
02747 IkReal x1472=((IkReal(1.00000000000000))*(cj2));
02748 IkReal x1473=((cj3)*(sj1));
02749 IkReal x1474=((cj1)*(sj3));
02750 IkReal x1475=((IkReal(1.00000000000000))*(cj5));
02751 IkReal x1476=((IkReal(1.00000000000000))*(r01));
02752 IkReal x1477=((cj1)*(cj3));
02753 IkReal x1478=((r11)*(sj5));
02754 IkReal x1479=((sj1)*(sj3));
02755 IkReal x1480=((r01)*(sj5));
02756 IkReal x1481=((r21)*(sj5));
02757 IkReal x1482=((cj0)*(x1470));
02758 IkReal x1483=((cj5)*(x1470));
02759 IkReal x1484=((sj0)*(x1471));
02760 IkReal x1485=((IkReal(1.00000000000000))*(x1470));
02761 IkReal x1486=((cj0)*(x1471));
02762 IkReal x1487=((sj0)*(sj5)*(x1470));
02763 IkReal x1488=((IkReal(1.00000000000000))*(x1486));
02764 evalcond[0]=((((r20)*(x1483)))+(((r22)*(x1471)))+(((IkReal(-1.00000000000000))*(x1481)*(x1485))));
02765 evalcond[1]=((((r12)*(x1482)))+(((IkReal(-1.00000000000000))*(r10)*(x1475)*(x1486)))+(((x1478)*(x1486)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1485)))+(((IkReal(-1.00000000000000))*(sj5)*(x1476)*(x1484)))+(((cj5)*(r00)*(x1484))));
02766 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1475)*(x1482)))+(((x1480)*(x1482)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1470)*(x1475)))+(((IkReal(-1.00000000000000))*(r02)*(x1488)))+(((sj0)*(x1470)*(x1478)))+(((IkReal(-1.00000000000000))*(r12)*(x1484))));
02767 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r12)*(x1488)))+(((r00)*(sj0)*(x1483)))+(((IkReal(-1.00000000000000))*(r10)*(x1475)*(x1482)))+(((x1478)*(x1482)))+(((r02)*(x1484)))+(((IkReal(-1.00000000000000))*(x1476)*(x1487))));
02768 evalcond[4]=((((cj5)*(r20)*(x1471)))+(((IkReal(-1.00000000000000))*(x1471)*(x1481)))+(((sj2)*(x1479)))+(((IkReal(-1.00000000000000))*(r22)*(x1485)))+(((IkReal(-1.00000000000000))*(x1472)*(x1474)))+(((IkReal(-1.00000000000000))*(x1472)*(x1473)))+(((IkReal(-1.00000000000000))*(sj2)*(x1477))));
02769 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x1475)*(x1486)))+(((x1480)*(x1486)))+(((cj2)*(x1479)))+(((sj2)*(x1473)))+(((sj2)*(x1474)))+(((IkReal(-1.00000000000000))*(r10)*(x1475)*(x1484)))+(((x1478)*(x1484)))+(((r12)*(sj0)*(x1470)))+(((r02)*(x1482)))+(((IkReal(-1.00000000000000))*(x1472)*(x1477))));
02770 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  )
02771 {
02772 continue;
02773 }
02774 }
02775 
02776 {
02777 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02778 vinfos[0].jointtype = 1;
02779 vinfos[0].foffset = j0;
02780 vinfos[0].indices[0] = _ij0[0];
02781 vinfos[0].indices[1] = _ij0[1];
02782 vinfos[0].maxsolutions = _nj0;
02783 vinfos[1].jointtype = 1;
02784 vinfos[1].foffset = j1;
02785 vinfos[1].indices[0] = _ij1[0];
02786 vinfos[1].indices[1] = _ij1[1];
02787 vinfos[1].maxsolutions = _nj1;
02788 vinfos[2].jointtype = 1;
02789 vinfos[2].foffset = j2;
02790 vinfos[2].indices[0] = _ij2[0];
02791 vinfos[2].indices[1] = _ij2[1];
02792 vinfos[2].maxsolutions = _nj2;
02793 vinfos[3].jointtype = 1;
02794 vinfos[3].foffset = j3;
02795 vinfos[3].indices[0] = _ij3[0];
02796 vinfos[3].indices[1] = _ij3[1];
02797 vinfos[3].maxsolutions = _nj3;
02798 vinfos[4].jointtype = 1;
02799 vinfos[4].foffset = j4;
02800 vinfos[4].indices[0] = _ij4[0];
02801 vinfos[4].indices[1] = _ij4[1];
02802 vinfos[4].maxsolutions = _nj4;
02803 vinfos[5].jointtype = 1;
02804 vinfos[5].foffset = j5;
02805 vinfos[5].indices[0] = _ij5[0];
02806 vinfos[5].indices[1] = _ij5[1];
02807 vinfos[5].maxsolutions = _nj5;
02808 std::vector<int> vfree(0);
02809 solutions.AddSolution(vinfos,vfree);
02810 }
02811 }
02812 }
02813 
02814 }
02815 
02816 }
02817 
02818 } else
02819 {
02820 {
02821 IkReal j4array[1], cj4array[1], sj4array[1];
02822 bool j4valid[1]={false};
02823 _nj4 = 1;
02824 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
02825     continue;
02826 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
02827 sj4array[0]=IKsin(j4array[0]);
02828 cj4array[0]=IKcos(j4array[0]);
02829 if( j4array[0] > IKPI )
02830 {
02831     j4array[0]-=IK2PI;
02832 }
02833 else if( j4array[0] < -IKPI )
02834 {    j4array[0]+=IK2PI;
02835 }
02836 j4valid[0] = true;
02837 for(int ij4 = 0; ij4 < 1; ++ij4)
02838 {
02839 if( !j4valid[ij4] )
02840 {
02841     continue;
02842 }
02843 _ij4[0] = ij4; _ij4[1] = -1;
02844 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02845 {
02846 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02847 {
02848     j4valid[iij4]=false; _ij4[1] = iij4; break; 
02849 }
02850 }
02851 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02852 {
02853 IkReal evalcond[6];
02854 IkReal x1489=IKsin(j4);
02855 IkReal x1490=IKcos(j4);
02856 IkReal x1491=((IkReal(1.00000000000000))*(cj2));
02857 IkReal x1492=((cj3)*(sj1));
02858 IkReal x1493=((cj1)*(sj3));
02859 IkReal x1494=((IkReal(1.00000000000000))*(cj5));
02860 IkReal x1495=((IkReal(1.00000000000000))*(r01));
02861 IkReal x1496=((cj1)*(cj3));
02862 IkReal x1497=((r11)*(sj5));
02863 IkReal x1498=((sj1)*(sj3));
02864 IkReal x1499=((r01)*(sj5));
02865 IkReal x1500=((r21)*(sj5));
02866 IkReal x1501=((cj0)*(x1489));
02867 IkReal x1502=((cj5)*(x1489));
02868 IkReal x1503=((sj0)*(x1490));
02869 IkReal x1504=((IkReal(1.00000000000000))*(x1489));
02870 IkReal x1505=((cj0)*(x1490));
02871 IkReal x1506=((sj0)*(sj5)*(x1489));
02872 IkReal x1507=((IkReal(1.00000000000000))*(x1505));
02873 evalcond[0]=((((r20)*(x1502)))+(((IkReal(-1.00000000000000))*(x1500)*(x1504)))+(((r22)*(x1490))));
02874 evalcond[1]=((((IkReal(-1.00000000000000))*(sj5)*(x1495)*(x1503)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1504)))+(((r12)*(x1501)))+(((cj5)*(r00)*(x1503)))+(((IkReal(-1.00000000000000))*(r10)*(x1494)*(x1505)))+(((x1497)*(x1505))));
02875 evalcond[2]=((((x1499)*(x1501)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1489)*(x1494)))+(((IkReal(-1.00000000000000))*(r00)*(x1494)*(x1501)))+(((IkReal(-1.00000000000000))*(r02)*(x1507)))+(((IkReal(-1.00000000000000))*(r12)*(x1503)))+(((sj0)*(x1489)*(x1497))));
02876 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x1494)*(x1501)))+(((r00)*(sj0)*(x1502)))+(((x1497)*(x1501)))+(((r02)*(x1503)))+(((IkReal(-1.00000000000000))*(x1495)*(x1506)))+(((IkReal(-1.00000000000000))*(r12)*(x1507))));
02877 evalcond[4]=((((sj2)*(x1498)))+(((IkReal(-1.00000000000000))*(r22)*(x1504)))+(((IkReal(-1.00000000000000))*(x1490)*(x1500)))+(((cj5)*(r20)*(x1490)))+(((IkReal(-1.00000000000000))*(sj2)*(x1496)))+(((IkReal(-1.00000000000000))*(x1491)*(x1493)))+(((IkReal(-1.00000000000000))*(x1491)*(x1492))));
02878 evalcond[5]=((((x1499)*(x1505)))+(((sj2)*(x1493)))+(((sj2)*(x1492)))+(((IkReal(-1.00000000000000))*(r00)*(x1494)*(x1505)))+(((cj2)*(x1498)))+(((IkReal(-1.00000000000000))*(r10)*(x1494)*(x1503)))+(((x1497)*(x1503)))+(((IkReal(-1.00000000000000))*(x1491)*(x1496)))+(((r02)*(x1501)))+(((r12)*(sj0)*(x1489))));
02879 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  )
02880 {
02881 continue;
02882 }
02883 }
02884 
02885 {
02886 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02887 vinfos[0].jointtype = 1;
02888 vinfos[0].foffset = j0;
02889 vinfos[0].indices[0] = _ij0[0];
02890 vinfos[0].indices[1] = _ij0[1];
02891 vinfos[0].maxsolutions = _nj0;
02892 vinfos[1].jointtype = 1;
02893 vinfos[1].foffset = j1;
02894 vinfos[1].indices[0] = _ij1[0];
02895 vinfos[1].indices[1] = _ij1[1];
02896 vinfos[1].maxsolutions = _nj1;
02897 vinfos[2].jointtype = 1;
02898 vinfos[2].foffset = j2;
02899 vinfos[2].indices[0] = _ij2[0];
02900 vinfos[2].indices[1] = _ij2[1];
02901 vinfos[2].maxsolutions = _nj2;
02902 vinfos[3].jointtype = 1;
02903 vinfos[3].foffset = j3;
02904 vinfos[3].indices[0] = _ij3[0];
02905 vinfos[3].indices[1] = _ij3[1];
02906 vinfos[3].maxsolutions = _nj3;
02907 vinfos[4].jointtype = 1;
02908 vinfos[4].foffset = j4;
02909 vinfos[4].indices[0] = _ij4[0];
02910 vinfos[4].indices[1] = _ij4[1];
02911 vinfos[4].maxsolutions = _nj4;
02912 vinfos[5].jointtype = 1;
02913 vinfos[5].foffset = j5;
02914 vinfos[5].indices[0] = _ij5[0];
02915 vinfos[5].indices[1] = _ij5[1];
02916 vinfos[5].maxsolutions = _nj5;
02917 std::vector<int> vfree(0);
02918 solutions.AddSolution(vinfos,vfree);
02919 }
02920 }
02921 }
02922 
02923 }
02924 
02925 }
02926 }
02927 }
02928 
02929 }
02930 
02931 }
02932 }
02933 }
02934 
02935 }
02936 
02937 }
02938 
02939 } else
02940 {
02941 {
02942 IkReal j5array[2], cj5array[2], sj5array[2];
02943 bool j5valid[2]={false};
02944 _nj5 = 2;
02945 if( IKabs(((IkReal(0.0946500000000000))*(r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(0.0946500000000000))*(r20))) < IKFAST_ATAN2_MAGTHRESH )
02946     continue;
02947 IkReal x1508=((IkReal(1.00000000000000))*(IKatan2(((IkReal(0.0946500000000000))*(r21)), ((IkReal(0.0946500000000000))*(r20)))));
02948 if( (((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))) < (IkReal)-0.00001 )
02949     continue;
02950 if( (((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1))))))) < -1-IKFAST_SINCOS_THRESH || (((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1))))))) > 1+IKFAST_SINCOS_THRESH )
02951     continue;
02952 IkReal x1509=IKasin(((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1)))))));
02953 j5array[0]=((((IkReal(-1.00000000000000))*(x1508)))+(((IkReal(-1.00000000000000))*(x1509))));
02954 sj5array[0]=IKsin(j5array[0]);
02955 cj5array[0]=IKcos(j5array[0]);
02956 j5array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x1508)))+(x1509));
02957 sj5array[1]=IKsin(j5array[1]);
02958 cj5array[1]=IKcos(j5array[1]);
02959 if( j5array[0] > IKPI )
02960 {
02961     j5array[0]-=IK2PI;
02962 }
02963 else if( j5array[0] < -IKPI )
02964 {    j5array[0]+=IK2PI;
02965 }
02966 j5valid[0] = true;
02967 if( j5array[1] > IKPI )
02968 {
02969     j5array[1]-=IK2PI;
02970 }
02971 else if( j5array[1] < -IKPI )
02972 {    j5array[1]+=IK2PI;
02973 }
02974 j5valid[1] = true;
02975 for(int ij5 = 0; ij5 < 2; ++ij5)
02976 {
02977 if( !j5valid[ij5] )
02978 {
02979     continue;
02980 }
02981 _ij5[0] = ij5; _ij5[1] = -1;
02982 for(int iij5 = ij5+1; iij5 < 2; ++iij5)
02983 {
02984 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02985 {
02986     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02987 }
02988 }
02989 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02990 {
02991 IkReal evalcond[1];
02992 IkReal x1510=((IkReal(1.00000000000000))*(sj3));
02993 evalcond[0]=((((IkReal(-1.00000000000000))*(cj3)*(sj1)*(sj2)))+(((cj1)*(cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x1510)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)*(x1510)))+(((IkReal(-1.00000000000000))*(r21)*(IKcos(j5))))+(((IkReal(-1.00000000000000))*(r20)*(IKsin(j5)))));
02994 if( IKabs(evalcond[0]) > 0.000001  )
02995 {
02996 continue;
02997 }
02998 }
02999 
03000 {
03001 IkReal dummyeval[1];
03002 IkReal gconst0;
03003 IkReal x1511=((IkReal(20000.0000000000))*(cj5));
03004 IkReal x1512=((IkReal(20000.0000000000))*(sj5));
03005 gconst0=IKsign(((((IkReal(-1.00000000000000))*(px)*(r11)*(x1511)))+(((py)*(r01)*(x1511)))+(((IkReal(-1.00000000000000))*(px)*(r10)*(x1512)))+(((py)*(r00)*(x1512)))));
03006 IkReal x1513=((IkReal(1.00000000000000))*(px));
03007 dummyeval[0]=((((cj5)*(py)*(r01)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1513)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1513)))+(((py)*(r00)*(sj5))));
03008 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03009 {
03010 {
03011 IkReal dummyeval[1];
03012 IkReal gconst2;
03013 gconst2=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
03014 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
03015 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03016 {
03017 {
03018 IkReal dummyeval[1];
03019 IkReal gconst1;
03020 IkReal x1514=(cj5)*(cj5);
03021 IkReal x1515=(sj5)*(sj5);
03022 IkReal x1516=((IkReal(1.00000000000000))*(x1515));
03023 IkReal x1517=((IkReal(2.00000000000000))*(cj5)*(sj5));
03024 IkReal x1518=((IkReal(1.00000000000000))*(x1514));
03025 gconst1=IKsign(((((IkReal(-1.00000000000000))*(x1516)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1518)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1516)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1517)))+(((IkReal(-1.00000000000000))*(x1518)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1517)))));
03026 IkReal x1519=(cj5)*(cj5);
03027 IkReal x1520=(sj5)*(sj5);
03028 IkReal x1521=((IkReal(1.00000000000000))*(x1520));
03029 IkReal x1522=((IkReal(2.00000000000000))*(cj5)*(sj5));
03030 IkReal x1523=((IkReal(1.00000000000000))*(x1519));
03031 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1521)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1522)))+(((IkReal(-1.00000000000000))*(x1521)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1523)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1522)))+(((IkReal(-1.00000000000000))*(x1523)*((r11)*(r11)))));
03032 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03033 {
03034 continue;
03035 
03036 } else
03037 {
03038 {
03039 IkReal j0array[1], cj0array[1], sj0array[1];
03040 bool j0valid[1]={false};
03041 _nj0 = 1;
03042 IkReal x1524=((sj2)*(sj3));
03043 IkReal x1525=((cj5)*(r01));
03044 IkReal x1526=((cj5)*(r11));
03045 IkReal x1527=((r00)*(sj5));
03046 IkReal x1528=((r10)*(sj5));
03047 IkReal x1529=((sj1)*(x1527));
03048 IkReal x1530=((IkReal(1.00000000000000))*(cj2)*(cj3));
03049 IkReal x1531=((sj1)*(x1528));
03050 IkReal x1532=((IkReal(1.00000000000000))*(cj1)*(cj2)*(sj3));
03051 IkReal x1533=((IkReal(1.00000000000000))*(cj1)*(cj3)*(sj2));
03052 if( IKabs(((gconst1)*(((((x1524)*(x1531)))+(((IkReal(-1.00000000000000))*(x1530)*(x1531)))+(((sj1)*(x1524)*(x1526)))+(((IkReal(-1.00000000000000))*(x1528)*(x1533)))+(((IkReal(-1.00000000000000))*(x1528)*(x1532)))+(((IkReal(-1.00000000000000))*(x1526)*(x1532)))+(((IkReal(-1.00000000000000))*(x1526)*(x1533)))+(((IkReal(-1.00000000000000))*(sj1)*(x1526)*(x1530))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x1529)*(x1530)))+(((x1524)*(x1529)))+(((IkReal(-1.00000000000000))*(sj1)*(x1525)*(x1530)))+(((IkReal(-1.00000000000000))*(x1525)*(x1532)))+(((IkReal(-1.00000000000000))*(x1525)*(x1533)))+(((sj1)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1527)*(x1532)))+(((IkReal(-1.00000000000000))*(x1527)*(x1533))))))) < IKFAST_ATAN2_MAGTHRESH )
03053     continue;
03054 j0array[0]=IKatan2(((gconst1)*(((((x1524)*(x1531)))+(((IkReal(-1.00000000000000))*(x1530)*(x1531)))+(((sj1)*(x1524)*(x1526)))+(((IkReal(-1.00000000000000))*(x1528)*(x1533)))+(((IkReal(-1.00000000000000))*(x1528)*(x1532)))+(((IkReal(-1.00000000000000))*(x1526)*(x1532)))+(((IkReal(-1.00000000000000))*(x1526)*(x1533)))+(((IkReal(-1.00000000000000))*(sj1)*(x1526)*(x1530)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x1529)*(x1530)))+(((x1524)*(x1529)))+(((IkReal(-1.00000000000000))*(sj1)*(x1525)*(x1530)))+(((IkReal(-1.00000000000000))*(x1525)*(x1532)))+(((IkReal(-1.00000000000000))*(x1525)*(x1533)))+(((sj1)*(x1524)*(x1525)))+(((IkReal(-1.00000000000000))*(x1527)*(x1532)))+(((IkReal(-1.00000000000000))*(x1527)*(x1533)))))));
03055 sj0array[0]=IKsin(j0array[0]);
03056 cj0array[0]=IKcos(j0array[0]);
03057 if( j0array[0] > IKPI )
03058 {
03059     j0array[0]-=IK2PI;
03060 }
03061 else if( j0array[0] < -IKPI )
03062 {    j0array[0]+=IK2PI;
03063 }
03064 j0valid[0] = true;
03065 for(int ij0 = 0; ij0 < 1; ++ij0)
03066 {
03067 if( !j0valid[ij0] )
03068 {
03069     continue;
03070 }
03071 _ij0[0] = ij0; _ij0[1] = -1;
03072 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
03073 {
03074 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
03075 {
03076     j0valid[iij0]=false; _ij0[1] = iij0; break; 
03077 }
03078 }
03079 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
03080 {
03081 IkReal evalcond[4];
03082 IkReal x1534=IKcos(j0);
03083 IkReal x1535=IKsin(j0);
03084 IkReal x1536=((r00)*(sj5));
03085 IkReal x1537=((IkReal(1.00000000000000))*(cj2));
03086 IkReal x1538=((cj5)*(r11));
03087 IkReal x1539=((sj1)*(sj2));
03088 IkReal x1540=((r10)*(sj5));
03089 IkReal x1541=((IkReal(0.0946500000000000))*(x1535));
03090 IkReal x1542=((IkReal(0.0946500000000000))*(x1534));
03091 IkReal x1543=((IkReal(1.00000000000000))*(x1535));
03092 IkReal x1544=((IkReal(1.00000000000000))*(x1534));
03093 IkReal x1545=((cj5)*(r01)*(x1535));
03094 IkReal x1546=((cj5)*(x1542));
03095 evalcond[0]=((((x1534)*(x1540)))+(((IkReal(-1.00000000000000))*(x1536)*(x1543)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1543)))+(((x1534)*(x1538))));
03096 evalcond[1]=((IkReal(0.109150000000000))+(((x1536)*(x1541)))+(((IkReal(-1.00000000000000))*(x1540)*(x1542)))+(((IkReal(-1.00000000000000))*(py)*(x1544)))+(((px)*(x1535)))+(((cj5)*(r01)*(x1541)))+(((IkReal(-1.00000000000000))*(x1538)*(x1542))));
03097 evalcond[2]=((((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x1540)*(x1541)))+(((IkReal(-0.392250000000000))*(x1539)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)*(x1543)))+(((IkReal(-1.00000000000000))*(px)*(x1544)))+(((IkReal(-1.00000000000000))*(x1536)*(x1542)))+(((IkReal(-1.00000000000000))*(r01)*(x1546)))+(((IkReal(-1.00000000000000))*(x1538)*(x1541))));
03098 evalcond[3]=((((sj3)*(x1539)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1537)))+(((cj5)*(r01)*(x1534)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1537)))+(((x1535)*(x1538)))+(((x1535)*(x1540)))+(((x1534)*(x1536))));
03099 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03100 {
03101 continue;
03102 }
03103 }
03104 
03105 {
03106 IkReal dummyeval[1];
03107 IkReal gconst3;
03108 IkReal x1547=((IkReal(1.00000000000000))*(sj0));
03109 IkReal x1548=((cj5)*(r22));
03110 IkReal x1549=((r21)*(sj5));
03111 IkReal x1550=((IkReal(1.00000000000000))*(cj0));
03112 IkReal x1551=((r22)*(sj5));
03113 IkReal x1552=((cj5)*(r20));
03114 gconst3=IKsign(((((r01)*(sj0)*(x1551)))+(((IkReal(-1.00000000000000))*(r02)*(x1547)*(x1549)))+(((cj0)*(r12)*(x1549)))+(((IkReal(-1.00000000000000))*(r00)*(x1547)*(x1548)))+(((IkReal(-1.00000000000000))*(r12)*(x1550)*(x1552)))+(((r02)*(sj0)*(x1552)))+(((IkReal(-1.00000000000000))*(r11)*(x1550)*(x1551)))+(((cj0)*(r10)*(x1548)))));
03115 IkReal x1553=((IkReal(1.00000000000000))*(sj0));
03116 IkReal x1554=((cj5)*(r22));
03117 IkReal x1555=((r21)*(sj5));
03118 IkReal x1556=((IkReal(1.00000000000000))*(cj0));
03119 IkReal x1557=((r22)*(sj5));
03120 IkReal x1558=((cj5)*(r20));
03121 dummyeval[0]=((((r01)*(sj0)*(x1557)))+(((IkReal(-1.00000000000000))*(r02)*(x1553)*(x1555)))+(((IkReal(-1.00000000000000))*(r12)*(x1556)*(x1558)))+(((IkReal(-1.00000000000000))*(r11)*(x1556)*(x1557)))+(((cj0)*(r12)*(x1555)))+(((IkReal(-1.00000000000000))*(r00)*(x1553)*(x1554)))+(((r02)*(sj0)*(x1558)))+(((cj0)*(r10)*(x1554))));
03122 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03123 {
03124 {
03125 IkReal dummyeval[1];
03126 IkReal gconst4;
03127 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
03128 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
03129 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03130 {
03131 continue;
03132 
03133 } else
03134 {
03135 {
03136 IkReal j4array[1], cj4array[1], sj4array[1];
03137 bool j4valid[1]={false};
03138 _nj4 = 1;
03139 IkReal x1559=((cj3)*(sj2));
03140 IkReal x1560=((cj2)*(cj3));
03141 IkReal x1561=((cj2)*(sj3));
03142 IkReal x1562=((sj2)*(sj3));
03143 IkReal x1563=((r21)*(sj5));
03144 IkReal x1564=((IkReal(1.00000000000000))*(cj1));
03145 IkReal x1565=((IkReal(1.00000000000000))*(sj1));
03146 IkReal x1566=((r22)*(x1564));
03147 IkReal x1567=((cj5)*(r20)*(sj1));
03148 IkReal x1568=((cj1)*(cj5)*(r20));
03149 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1559)*(x1566)))+(((IkReal(-1.00000000000000))*(r22)*(x1560)*(x1565)))+(((r22)*(sj1)*(x1562)))+(((IkReal(-1.00000000000000))*(x1561)*(x1566))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((x1560)*(x1567)))+(((IkReal(-1.00000000000000))*(x1561)*(x1563)*(x1564)))+(((IkReal(-1.00000000000000))*(x1560)*(x1563)*(x1565)))+(((sj1)*(x1562)*(x1563)))+(((x1561)*(x1568)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1562)*(x1565)))+(((x1559)*(x1568)))+(((IkReal(-1.00000000000000))*(x1559)*(x1563)*(x1564))))))) < IKFAST_ATAN2_MAGTHRESH )
03150     continue;
03151 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(x1559)*(x1566)))+(((IkReal(-1.00000000000000))*(r22)*(x1560)*(x1565)))+(((r22)*(sj1)*(x1562)))+(((IkReal(-1.00000000000000))*(x1561)*(x1566)))))), ((gconst4)*(((((x1560)*(x1567)))+(((IkReal(-1.00000000000000))*(x1561)*(x1563)*(x1564)))+(((IkReal(-1.00000000000000))*(x1560)*(x1563)*(x1565)))+(((sj1)*(x1562)*(x1563)))+(((x1561)*(x1568)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1562)*(x1565)))+(((x1559)*(x1568)))+(((IkReal(-1.00000000000000))*(x1559)*(x1563)*(x1564)))))));
03152 sj4array[0]=IKsin(j4array[0]);
03153 cj4array[0]=IKcos(j4array[0]);
03154 if( j4array[0] > IKPI )
03155 {
03156     j4array[0]-=IK2PI;
03157 }
03158 else if( j4array[0] < -IKPI )
03159 {    j4array[0]+=IK2PI;
03160 }
03161 j4valid[0] = true;
03162 for(int ij4 = 0; ij4 < 1; ++ij4)
03163 {
03164 if( !j4valid[ij4] )
03165 {
03166     continue;
03167 }
03168 _ij4[0] = ij4; _ij4[1] = -1;
03169 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03170 {
03171 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03172 {
03173     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03174 }
03175 }
03176 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03177 {
03178 IkReal evalcond[6];
03179 IkReal x1569=IKsin(j4);
03180 IkReal x1570=IKcos(j4);
03181 IkReal x1571=((IkReal(1.00000000000000))*(cj2));
03182 IkReal x1572=((cj3)*(sj1));
03183 IkReal x1573=((cj1)*(sj3));
03184 IkReal x1574=((IkReal(1.00000000000000))*(cj5));
03185 IkReal x1575=((IkReal(1.00000000000000))*(r01));
03186 IkReal x1576=((cj1)*(cj3));
03187 IkReal x1577=((r11)*(sj5));
03188 IkReal x1578=((sj1)*(sj3));
03189 IkReal x1579=((r01)*(sj5));
03190 IkReal x1580=((r21)*(sj5));
03191 IkReal x1581=((cj0)*(x1569));
03192 IkReal x1582=((cj5)*(x1569));
03193 IkReal x1583=((sj0)*(x1570));
03194 IkReal x1584=((IkReal(1.00000000000000))*(x1569));
03195 IkReal x1585=((cj0)*(x1570));
03196 IkReal x1586=((sj0)*(sj5)*(x1569));
03197 IkReal x1587=((IkReal(1.00000000000000))*(x1585));
03198 evalcond[0]=((((IkReal(-1.00000000000000))*(x1580)*(x1584)))+(((r20)*(x1582)))+(((r22)*(x1570))));
03199 evalcond[1]=((((cj5)*(r00)*(x1583)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1584)))+(((IkReal(-1.00000000000000))*(r10)*(x1574)*(x1585)))+(((r12)*(x1581)))+(((IkReal(-1.00000000000000))*(sj5)*(x1575)*(x1583)))+(((x1577)*(x1585))));
03200 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1569)*(x1574)))+(((IkReal(-1.00000000000000))*(r00)*(x1574)*(x1581)))+(((IkReal(-1.00000000000000))*(r02)*(x1587)))+(((x1579)*(x1581)))+(((sj0)*(x1569)*(x1577)))+(((IkReal(-1.00000000000000))*(r12)*(x1583))));
03201 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x1574)*(x1581)))+(((r00)*(sj0)*(x1582)))+(((r02)*(x1583)))+(((x1577)*(x1581)))+(((IkReal(-1.00000000000000))*(r12)*(x1587)))+(((IkReal(-1.00000000000000))*(x1575)*(x1586))));
03202 evalcond[4]=((((IkReal(-1.00000000000000))*(sj2)*(x1576)))+(((IkReal(-1.00000000000000))*(x1571)*(x1572)))+(((IkReal(-1.00000000000000))*(x1571)*(x1573)))+(((cj5)*(r20)*(x1570)))+(((IkReal(-1.00000000000000))*(x1570)*(x1580)))+(((IkReal(-1.00000000000000))*(r22)*(x1584)))+(((sj2)*(x1578))));
03203 evalcond[5]=((((IkReal(-1.00000000000000))*(r10)*(x1574)*(x1583)))+(((IkReal(-1.00000000000000))*(x1571)*(x1576)))+(((IkReal(-1.00000000000000))*(r00)*(x1574)*(x1585)))+(((r12)*(sj0)*(x1569)))+(((cj2)*(x1578)))+(((r02)*(x1581)))+(((x1577)*(x1583)))+(((x1579)*(x1585)))+(((sj2)*(x1572)))+(((sj2)*(x1573))));
03204 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  )
03205 {
03206 continue;
03207 }
03208 }
03209 
03210 {
03211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03212 vinfos[0].jointtype = 1;
03213 vinfos[0].foffset = j0;
03214 vinfos[0].indices[0] = _ij0[0];
03215 vinfos[0].indices[1] = _ij0[1];
03216 vinfos[0].maxsolutions = _nj0;
03217 vinfos[1].jointtype = 1;
03218 vinfos[1].foffset = j1;
03219 vinfos[1].indices[0] = _ij1[0];
03220 vinfos[1].indices[1] = _ij1[1];
03221 vinfos[1].maxsolutions = _nj1;
03222 vinfos[2].jointtype = 1;
03223 vinfos[2].foffset = j2;
03224 vinfos[2].indices[0] = _ij2[0];
03225 vinfos[2].indices[1] = _ij2[1];
03226 vinfos[2].maxsolutions = _nj2;
03227 vinfos[3].jointtype = 1;
03228 vinfos[3].foffset = j3;
03229 vinfos[3].indices[0] = _ij3[0];
03230 vinfos[3].indices[1] = _ij3[1];
03231 vinfos[3].maxsolutions = _nj3;
03232 vinfos[4].jointtype = 1;
03233 vinfos[4].foffset = j4;
03234 vinfos[4].indices[0] = _ij4[0];
03235 vinfos[4].indices[1] = _ij4[1];
03236 vinfos[4].maxsolutions = _nj4;
03237 vinfos[5].jointtype = 1;
03238 vinfos[5].foffset = j5;
03239 vinfos[5].indices[0] = _ij5[0];
03240 vinfos[5].indices[1] = _ij5[1];
03241 vinfos[5].maxsolutions = _nj5;
03242 std::vector<int> vfree(0);
03243 solutions.AddSolution(vinfos,vfree);
03244 }
03245 }
03246 }
03247 
03248 }
03249 
03250 }
03251 
03252 } else
03253 {
03254 {
03255 IkReal j4array[1], cj4array[1], sj4array[1];
03256 bool j4valid[1]={false};
03257 _nj4 = 1;
03258 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
03259     continue;
03260 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
03261 sj4array[0]=IKsin(j4array[0]);
03262 cj4array[0]=IKcos(j4array[0]);
03263 if( j4array[0] > IKPI )
03264 {
03265     j4array[0]-=IK2PI;
03266 }
03267 else if( j4array[0] < -IKPI )
03268 {    j4array[0]+=IK2PI;
03269 }
03270 j4valid[0] = true;
03271 for(int ij4 = 0; ij4 < 1; ++ij4)
03272 {
03273 if( !j4valid[ij4] )
03274 {
03275     continue;
03276 }
03277 _ij4[0] = ij4; _ij4[1] = -1;
03278 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03279 {
03280 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03281 {
03282     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03283 }
03284 }
03285 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03286 {
03287 IkReal evalcond[6];
03288 IkReal x1588=IKsin(j4);
03289 IkReal x1589=IKcos(j4);
03290 IkReal x1590=((IkReal(1.00000000000000))*(cj2));
03291 IkReal x1591=((cj3)*(sj1));
03292 IkReal x1592=((cj1)*(sj3));
03293 IkReal x1593=((IkReal(1.00000000000000))*(cj5));
03294 IkReal x1594=((IkReal(1.00000000000000))*(r01));
03295 IkReal x1595=((cj1)*(cj3));
03296 IkReal x1596=((r11)*(sj5));
03297 IkReal x1597=((sj1)*(sj3));
03298 IkReal x1598=((r01)*(sj5));
03299 IkReal x1599=((r21)*(sj5));
03300 IkReal x1600=((cj0)*(x1588));
03301 IkReal x1601=((cj5)*(x1588));
03302 IkReal x1602=((sj0)*(x1589));
03303 IkReal x1603=((IkReal(1.00000000000000))*(x1588));
03304 IkReal x1604=((cj0)*(x1589));
03305 IkReal x1605=((sj0)*(sj5)*(x1588));
03306 IkReal x1606=((IkReal(1.00000000000000))*(x1604));
03307 evalcond[0]=((((IkReal(-1.00000000000000))*(x1599)*(x1603)))+(((r22)*(x1589)))+(((r20)*(x1601))));
03308 evalcond[1]=((((IkReal(-1.00000000000000))*(r10)*(x1593)*(x1604)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1603)))+(((r12)*(x1600)))+(((cj5)*(r00)*(x1602)))+(((x1596)*(x1604)))+(((IkReal(-1.00000000000000))*(sj5)*(x1594)*(x1602))));
03309 evalcond[2]=((((IkReal(-1.00000000000000))*(r12)*(x1602)))+(((IkReal(-1.00000000000000))*(r00)*(x1593)*(x1600)))+(((sj0)*(x1588)*(x1596)))+(((x1598)*(x1600)))+(((IkReal(-1.00000000000000))*(r02)*(x1606)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1588)*(x1593))));
03310 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x1593)*(x1600)))+(((IkReal(-1.00000000000000))*(x1594)*(x1605)))+(((r00)*(sj0)*(x1601)))+(((x1596)*(x1600)))+(((IkReal(-1.00000000000000))*(r12)*(x1606)))+(((r02)*(x1602))));
03311 evalcond[4]=((((IkReal(-1.00000000000000))*(x1589)*(x1599)))+(((sj2)*(x1597)))+(((cj5)*(r20)*(x1589)))+(((IkReal(-1.00000000000000))*(r22)*(x1603)))+(((IkReal(-1.00000000000000))*(x1590)*(x1592)))+(((IkReal(-1.00000000000000))*(x1590)*(x1591)))+(((IkReal(-1.00000000000000))*(sj2)*(x1595))));
03312 evalcond[5]=((((sj2)*(x1591)))+(((sj2)*(x1592)))+(((IkReal(-1.00000000000000))*(r10)*(x1593)*(x1602)))+(((r12)*(sj0)*(x1588)))+(((cj2)*(x1597)))+(((IkReal(-1.00000000000000))*(r00)*(x1593)*(x1604)))+(((x1598)*(x1604)))+(((IkReal(-1.00000000000000))*(x1590)*(x1595)))+(((x1596)*(x1602)))+(((r02)*(x1600))));
03313 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  || IKabs(evalcond[4]) > 0.000001  || IKabs(evalcond[5]) > 0.000001  )
03314 {
03315 continue;
03316 }
03317 }
03318 
03319 {
03320 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03321 vinfos[0].jointtype = 1;
03322 vinfos[0].foffset = j0;
03323 vinfos[0].indices[0] = _ij0[0];
03324 vinfos[0].indices[1] = _ij0[1];
03325 vinfos[0].maxsolutions = _nj0;
03326 vinfos[1].jointtype = 1;
03327 vinfos[1].foffset = j1;
03328 vinfos[1].indices[0] = _ij1[0];
03329 vinfos[1].indices[1] = _ij1[1];
03330 vinfos[1].maxsolutions = _nj1;
03331 vinfos[2].jointtype = 1;
03332 vinfos[2].foffset = j2;
03333 vinfos[2].indices[0] = _ij2[0];
03334 vinfos[2].indices[1] = _ij2[1];
03335 vinfos[2].maxsolutions = _nj2;
03336 vinfos[3].jointtype = 1;
03337 vinfos[3].foffset = j3;
03338 vinfos[3].indices[0] = _ij3[0];
03339 vinfos[3].indices[1] = _ij3[1];
03340 vinfos[3].maxsolutions = _nj3;
03341 vinfos[4].jointtype = 1;
03342 vinfos[4].foffset = j4;
03343 vinfos[4].indices[0] = _ij4[0];
03344 vinfos[4].indices[1] = _ij4[1];
03345 vinfos[4].maxsolutions = _nj4;
03346 vinfos[5].jointtype = 1;
03347 vinfos[5].foffset = j5;
03348 vinfos[5].indices[0] = _ij5[0];
03349 vinfos[5].indices[1] = _ij5[1];
03350 vinfos[5].maxsolutions = _nj5;
03351 std::vector<int> vfree(0);
03352 solutions.AddSolution(vinfos,vfree);
03353 }
03354 }
03355 }
03356 
03357 }
03358 
03359 }
03360 }
03361 }
03362 
03363 }
03364 
03365 }
03366 
03367 } else
03368 {
03369 {
03370 IkReal j4array[1], cj4array[1], sj4array[1];
03371 bool j4valid[1]={false};
03372 _nj4 = 1;
03373 IkReal x1607=((cj3)*(sj2));
03374 IkReal x1608=((cj2)*(sj3));
03375 IkReal x1609=((cj5)*(r20));
03376 IkReal x1610=((r21)*(sj5));
03377 IkReal x1611=((IkReal(1.00000000000000))*(cj1));
03378 IkReal x1612=((r22)*(x1611));
03379 IkReal x1613=((sj1)*(sj2)*(sj3));
03380 IkReal x1614=((cj2)*(cj3)*(sj1));
03381 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(r22)*(x1614)))+(((IkReal(-1.00000000000000))*(x1608)*(x1612)))+(((r22)*(x1613)))+(((IkReal(-1.00000000000000))*(x1607)*(x1612))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x1609)*(x1613)))+(((IkReal(-1.00000000000000))*(x1607)*(x1610)*(x1611)))+(((IkReal(-1.00000000000000))*(x1610)*(x1614)))+(((IkReal(-1.00000000000000))*(x1608)*(x1610)*(x1611)))+(((x1609)*(x1614)))+(((cj1)*(x1607)*(x1609)))+(((cj1)*(x1608)*(x1609)))+(((x1610)*(x1613))))))) < IKFAST_ATAN2_MAGTHRESH )
03382     continue;
03383 j4array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(r22)*(x1614)))+(((IkReal(-1.00000000000000))*(x1608)*(x1612)))+(((r22)*(x1613)))+(((IkReal(-1.00000000000000))*(x1607)*(x1612)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(x1609)*(x1613)))+(((IkReal(-1.00000000000000))*(x1607)*(x1610)*(x1611)))+(((IkReal(-1.00000000000000))*(x1610)*(x1614)))+(((IkReal(-1.00000000000000))*(x1608)*(x1610)*(x1611)))+(((x1609)*(x1614)))+(((cj1)*(x1607)*(x1609)))+(((cj1)*(x1608)*(x1609)))+(((x1610)*(x1613)))))));
03384 sj4array[0]=IKsin(j4array[0]);
03385 cj4array[0]=IKcos(j4array[0]);
03386 if( j4array[0] > IKPI )
03387 {
03388     j4array[0]-=IK2PI;
03389 }
03390 else if( j4array[0] < -IKPI )
03391 {    j4array[0]+=IK2PI;
03392 }
03393 j4valid[0] = true;
03394 for(int ij4 = 0; ij4 < 1; ++ij4)
03395 {
03396 if( !j4valid[ij4] )
03397 {
03398     continue;
03399 }
03400 _ij4[0] = ij4; _ij4[1] = -1;
03401 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03402 {
03403 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03404 {
03405     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03406 }
03407 }
03408 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03409 {
03410 IkReal evalcond[2];
03411 IkReal x1615=IKsin(j4);
03412 IkReal x1616=IKcos(j4);
03413 IkReal x1617=((IkReal(1.00000000000000))*(cj1));
03414 IkReal x1618=((cj5)*(r20));
03415 IkReal x1619=((IkReal(1.00000000000000))*(r21)*(sj5));
03416 evalcond[0]=((((x1615)*(x1618)))+(((r22)*(x1616)))+(((IkReal(-1.00000000000000))*(x1615)*(x1619))));
03417 evalcond[1]=((((IkReal(-1.00000000000000))*(x1616)*(x1619)))+(((IkReal(-1.00000000000000))*(r22)*(x1615)))+(((IkReal(-1.00000000000000))*(cj3)*(sj2)*(x1617)))+(((IkReal(-1.00000000000000))*(cj2)*(sj3)*(x1617)))+(((IkReal(-1.00000000000000))*(cj2)*(cj3)*(sj1)))+(((x1616)*(x1618)))+(((sj1)*(sj2)*(sj3))));
03418 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
03419 {
03420 continue;
03421 }
03422 }
03423 
03424 {
03425 IkReal dummyeval[1];
03426 IkReal gconst5;
03427 IkReal x1620=((IkReal(20000.0000000000))*(cj5));
03428 IkReal x1621=((IkReal(20000.0000000000))*(sj5));
03429 gconst5=IKsign(((((IkReal(-1.00000000000000))*(px)*(r10)*(x1621)))+(((IkReal(-1.00000000000000))*(px)*(r11)*(x1620)))+(((py)*(r00)*(x1621)))+(((py)*(r01)*(x1620)))));
03430 IkReal x1622=((IkReal(1.00000000000000))*(px));
03431 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1622)))+(((cj5)*(py)*(r01)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1622)))+(((py)*(r00)*(sj5))));
03432 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03433 {
03434 {
03435 IkReal dummyeval[1];
03436 IkReal gconst6;
03437 IkReal x1623=(sj5)*(sj5);
03438 IkReal x1624=(cj5)*(cj5);
03439 IkReal x1625=((cj4)*(sj5));
03440 IkReal x1626=((IkReal(1.00000000000000))*(r02));
03441 IkReal x1627=((r01)*(r10));
03442 IkReal x1628=((cj4)*(cj5));
03443 IkReal x1629=((sj4)*(x1624));
03444 IkReal x1630=((IkReal(1.00000000000000))*(r00)*(r11));
03445 IkReal x1631=((sj4)*(x1623));
03446 gconst6=IKsign(((((x1627)*(x1631)))+(((IkReal(-1.00000000000000))*(r10)*(x1625)*(x1626)))+(((x1627)*(x1629)))+(((r00)*(r12)*(x1625)))+(((IkReal(-1.00000000000000))*(r11)*(x1626)*(x1628)))+(((r01)*(r12)*(x1628)))+(((IkReal(-1.00000000000000))*(x1629)*(x1630)))+(((IkReal(-1.00000000000000))*(x1630)*(x1631)))));
03447 IkReal x1632=(sj5)*(sj5);
03448 IkReal x1633=(cj5)*(cj5);
03449 IkReal x1634=((cj4)*(sj5));
03450 IkReal x1635=((IkReal(1.00000000000000))*(r02));
03451 IkReal x1636=((r01)*(r10));
03452 IkReal x1637=((cj4)*(cj5));
03453 IkReal x1638=((sj4)*(x1633));
03454 IkReal x1639=((IkReal(1.00000000000000))*(r00)*(r11));
03455 IkReal x1640=((sj4)*(x1632));
03456 dummyeval[0]=((((x1636)*(x1640)))+(((x1636)*(x1638)))+(((r01)*(r12)*(x1637)))+(((IkReal(-1.00000000000000))*(r10)*(x1634)*(x1635)))+(((IkReal(-1.00000000000000))*(x1638)*(x1639)))+(((IkReal(-1.00000000000000))*(x1639)*(x1640)))+(((r00)*(r12)*(x1634)))+(((IkReal(-1.00000000000000))*(r11)*(x1635)*(x1637))));
03457 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03458 {
03459 continue;
03460 
03461 } else
03462 {
03463 {
03464 IkReal j0array[1], cj0array[1], sj0array[1];
03465 bool j0valid[1]={false};
03466 _nj0 = 1;
03467 if( IKabs(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH )
03468     continue;
03469 j0array[0]=IKatan2(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5)))))), ((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01)))))));
03470 sj0array[0]=IKsin(j0array[0]);
03471 cj0array[0]=IKcos(j0array[0]);
03472 if( j0array[0] > IKPI )
03473 {
03474     j0array[0]-=IK2PI;
03475 }
03476 else if( j0array[0] < -IKPI )
03477 {    j0array[0]+=IK2PI;
03478 }
03479 j0valid[0] = true;
03480 for(int ij0 = 0; ij0 < 1; ++ij0)
03481 {
03482 if( !j0valid[ij0] )
03483 {
03484     continue;
03485 }
03486 _ij0[0] = ij0; _ij0[1] = -1;
03487 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
03488 {
03489 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
03490 {
03491     j0valid[iij0]=false; _ij0[1] = iij0; break; 
03492 }
03493 }
03494 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
03495 {
03496 IkReal evalcond[8];
03497 IkReal x1641=IKcos(j0);
03498 IkReal x1642=IKsin(j0);
03499 IkReal x1643=((IkReal(1.00000000000000))*(cj5));
03500 IkReal x1644=((IkReal(0.0946500000000000))*(sj5));
03501 IkReal x1645=((IkReal(1.00000000000000))*(cj2));
03502 IkReal x1646=((cj3)*(sj1));
03503 IkReal x1647=((cj1)*(sj3));
03504 IkReal x1648=((IkReal(0.0946500000000000))*(cj5));
03505 IkReal x1649=((r02)*(sj4));
03506 IkReal x1650=((cj1)*(cj3));
03507 IkReal x1651=((sj1)*(sj2));
03508 IkReal x1652=((sj4)*(sj5));
03509 IkReal x1653=((IkReal(1.00000000000000))*(sj5));
03510 IkReal x1654=((cj4)*(r02));
03511 IkReal x1655=((cj4)*(r12));
03512 IkReal x1656=((cj4)*(sj5));
03513 IkReal x1657=((r12)*(sj4));
03514 IkReal x1658=((r01)*(x1642));
03515 IkReal x1659=((r00)*(x1641));
03516 IkReal x1660=((r00)*(x1642));
03517 IkReal x1661=((r11)*(x1641));
03518 IkReal x1662=((r01)*(x1641));
03519 IkReal x1663=((r11)*(x1642));
03520 IkReal x1664=((r10)*(x1642));
03521 IkReal x1665=((IkReal(1.00000000000000))*(x1642));
03522 IkReal x1666=((r10)*(x1641));
03523 IkReal x1667=((IkReal(1.00000000000000))*(x1641));
03524 evalcond[0]=((((cj5)*(x1661)))+(((IkReal(-1.00000000000000))*(x1653)*(x1660)))+(((IkReal(-1.00000000000000))*(x1643)*(x1658)))+(((sj5)*(x1666))));
03525 evalcond[1]=((IkReal(0.109150000000000))+(((IkReal(-1.00000000000000))*(x1644)*(x1666)))+(((IkReal(-1.00000000000000))*(py)*(x1667)))+(((x1648)*(x1658)))+(((px)*(x1642)))+(((x1644)*(x1660)))+(((IkReal(-1.00000000000000))*(x1648)*(x1661))));
03526 evalcond[2]=((((IkReal(-1.00000000000000))*(cj4)*(x1643)*(x1666)))+(((IkReal(-1.00000000000000))*(cj4)*(x1653)*(x1658)))+(((x1641)*(x1657)))+(((cj4)*(cj5)*(x1660)))+(((IkReal(-1.00000000000000))*(x1649)*(x1665)))+(((x1656)*(x1661))));
03527 evalcond[3]=((((IkReal(-1.00000000000000))*(sj4)*(x1643)*(x1659)))+(((IkReal(-1.00000000000000))*(x1654)*(x1667)))+(((IkReal(-1.00000000000000))*(sj4)*(x1643)*(x1664)))+(((IkReal(-1.00000000000000))*(x1655)*(x1665)))+(((x1652)*(x1663)))+(((x1652)*(x1662))));
03528 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(sj4)*(x1643)*(x1666)))+(((IkReal(-1.00000000000000))*(x1655)*(x1667)))+(((cj5)*(sj4)*(x1660)))+(((IkReal(-1.00000000000000))*(x1652)*(x1658)))+(((x1652)*(x1661)))+(((x1642)*(x1654))));
03529 evalcond[5]=((((IkReal(-1.00000000000000))*(x1644)*(x1659)))+(((IkReal(-1.00000000000000))*(x1644)*(x1664)))+(((IkReal(-1.00000000000000))*(py)*(x1665)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(px)*(x1667)))+(((IkReal(-0.392250000000000))*(x1651)))+(((IkReal(-1.00000000000000))*(x1648)*(x1662)))+(((IkReal(-1.00000000000000))*(x1648)*(x1663))));
03530 evalcond[6]=((((cj5)*(x1663)))+(((cj5)*(x1662)))+(((IkReal(-1.00000000000000))*(x1645)*(x1646)))+(((IkReal(-1.00000000000000))*(x1645)*(x1647)))+(((sj5)*(x1659)))+(((sj5)*(x1664)))+(((sj3)*(x1651)))+(((IkReal(-1.00000000000000))*(sj2)*(x1650))));
03531 evalcond[7]=((((IkReal(-1.00000000000000))*(cj4)*(x1643)*(x1659)))+(((IkReal(-1.00000000000000))*(cj4)*(x1643)*(x1664)))+(((sj2)*(x1647)))+(((sj2)*(x1646)))+(((IkReal(-1.00000000000000))*(x1645)*(x1650)))+(((x1641)*(x1649)))+(((x1642)*(x1657)))+(((cj2)*(sj1)*(sj3)))+(((x1656)*(x1663)))+(((x1656)*(x1662))));
03532 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  )
03533 {
03534 continue;
03535 }
03536 }
03537 
03538 {
03539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03540 vinfos[0].jointtype = 1;
03541 vinfos[0].foffset = j0;
03542 vinfos[0].indices[0] = _ij0[0];
03543 vinfos[0].indices[1] = _ij0[1];
03544 vinfos[0].maxsolutions = _nj0;
03545 vinfos[1].jointtype = 1;
03546 vinfos[1].foffset = j1;
03547 vinfos[1].indices[0] = _ij1[0];
03548 vinfos[1].indices[1] = _ij1[1];
03549 vinfos[1].maxsolutions = _nj1;
03550 vinfos[2].jointtype = 1;
03551 vinfos[2].foffset = j2;
03552 vinfos[2].indices[0] = _ij2[0];
03553 vinfos[2].indices[1] = _ij2[1];
03554 vinfos[2].maxsolutions = _nj2;
03555 vinfos[3].jointtype = 1;
03556 vinfos[3].foffset = j3;
03557 vinfos[3].indices[0] = _ij3[0];
03558 vinfos[3].indices[1] = _ij3[1];
03559 vinfos[3].maxsolutions = _nj3;
03560 vinfos[4].jointtype = 1;
03561 vinfos[4].foffset = j4;
03562 vinfos[4].indices[0] = _ij4[0];
03563 vinfos[4].indices[1] = _ij4[1];
03564 vinfos[4].maxsolutions = _nj4;
03565 vinfos[5].jointtype = 1;
03566 vinfos[5].foffset = j5;
03567 vinfos[5].indices[0] = _ij5[0];
03568 vinfos[5].indices[1] = _ij5[1];
03569 vinfos[5].maxsolutions = _nj5;
03570 std::vector<int> vfree(0);
03571 solutions.AddSolution(vinfos,vfree);
03572 }
03573 }
03574 }
03575 
03576 }
03577 
03578 }
03579 
03580 } else
03581 {
03582 {
03583 IkReal j0array[1], cj0array[1], sj0array[1];
03584 bool j0valid[1]={false};
03585 _nj0 = 1;
03586 IkReal x1668=((IkReal(2183.00000000000))*(sj5));
03587 IkReal x1669=((IkReal(2183.00000000000))*(cj5));
03588 if( IKabs(((gconst5)*(((((r10)*(x1668)))+(((r11)*(x1669))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((r00)*(x1668)))+(((r01)*(x1669))))))) < IKFAST_ATAN2_MAGTHRESH )
03589     continue;
03590 j0array[0]=IKatan2(((gconst5)*(((((r10)*(x1668)))+(((r11)*(x1669)))))), ((gconst5)*(((((r00)*(x1668)))+(((r01)*(x1669)))))));
03591 sj0array[0]=IKsin(j0array[0]);
03592 cj0array[0]=IKcos(j0array[0]);
03593 if( j0array[0] > IKPI )
03594 {
03595     j0array[0]-=IK2PI;
03596 }
03597 else if( j0array[0] < -IKPI )
03598 {    j0array[0]+=IK2PI;
03599 }
03600 j0valid[0] = true;
03601 for(int ij0 = 0; ij0 < 1; ++ij0)
03602 {
03603 if( !j0valid[ij0] )
03604 {
03605     continue;
03606 }
03607 _ij0[0] = ij0; _ij0[1] = -1;
03608 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
03609 {
03610 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
03611 {
03612     j0valid[iij0]=false; _ij0[1] = iij0; break; 
03613 }
03614 }
03615 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
03616 {
03617 IkReal evalcond[8];
03618 IkReal x1670=IKcos(j0);
03619 IkReal x1671=IKsin(j0);
03620 IkReal x1672=((IkReal(1.00000000000000))*(cj5));
03621 IkReal x1673=((IkReal(0.0946500000000000))*(sj5));
03622 IkReal x1674=((IkReal(1.00000000000000))*(cj2));
03623 IkReal x1675=((cj3)*(sj1));
03624 IkReal x1676=((cj1)*(sj3));
03625 IkReal x1677=((IkReal(0.0946500000000000))*(cj5));
03626 IkReal x1678=((r02)*(sj4));
03627 IkReal x1679=((cj1)*(cj3));
03628 IkReal x1680=((sj1)*(sj2));
03629 IkReal x1681=((sj4)*(sj5));
03630 IkReal x1682=((IkReal(1.00000000000000))*(sj5));
03631 IkReal x1683=((cj4)*(r02));
03632 IkReal x1684=((cj4)*(r12));
03633 IkReal x1685=((cj4)*(sj5));
03634 IkReal x1686=((r12)*(sj4));
03635 IkReal x1687=((r01)*(x1671));
03636 IkReal x1688=((r00)*(x1670));
03637 IkReal x1689=((r00)*(x1671));
03638 IkReal x1690=((r11)*(x1670));
03639 IkReal x1691=((r01)*(x1670));
03640 IkReal x1692=((r11)*(x1671));
03641 IkReal x1693=((r10)*(x1671));
03642 IkReal x1694=((IkReal(1.00000000000000))*(x1671));
03643 IkReal x1695=((r10)*(x1670));
03644 IkReal x1696=((IkReal(1.00000000000000))*(x1670));
03645 evalcond[0]=((((IkReal(-1.00000000000000))*(x1682)*(x1689)))+(((IkReal(-1.00000000000000))*(x1672)*(x1687)))+(((cj5)*(x1690)))+(((sj5)*(x1695))));
03646 evalcond[1]=((IkReal(0.109150000000000))+(((x1673)*(x1689)))+(((IkReal(-1.00000000000000))*(py)*(x1696)))+(((x1677)*(x1687)))+(((px)*(x1671)))+(((IkReal(-1.00000000000000))*(x1673)*(x1695)))+(((IkReal(-1.00000000000000))*(x1677)*(x1690))));
03647 evalcond[2]=((((IkReal(-1.00000000000000))*(cj4)*(x1682)*(x1687)))+(((x1670)*(x1686)))+(((cj4)*(cj5)*(x1689)))+(((IkReal(-1.00000000000000))*(x1678)*(x1694)))+(((x1685)*(x1690)))+(((IkReal(-1.00000000000000))*(cj4)*(x1672)*(x1695))));
03648 evalcond[3]=((((IkReal(-1.00000000000000))*(x1684)*(x1694)))+(((IkReal(-1.00000000000000))*(sj4)*(x1672)*(x1688)))+(((IkReal(-1.00000000000000))*(x1683)*(x1696)))+(((IkReal(-1.00000000000000))*(sj4)*(x1672)*(x1693)))+(((x1681)*(x1692)))+(((x1681)*(x1691))));
03649 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1684)*(x1696)))+(((x1671)*(x1683)))+(((IkReal(-1.00000000000000))*(x1681)*(x1687)))+(((IkReal(-1.00000000000000))*(sj4)*(x1672)*(x1695)))+(((cj5)*(sj4)*(x1689)))+(((x1681)*(x1690))));
03650 evalcond[5]=((((IkReal(-1.00000000000000))*(py)*(x1694)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1673)*(x1688)))+(((IkReal(-1.00000000000000))*(x1673)*(x1693)))+(((IkReal(-1.00000000000000))*(x1677)*(x1692)))+(((IkReal(-1.00000000000000))*(x1677)*(x1691)))+(((IkReal(-1.00000000000000))*(px)*(x1696)))+(((IkReal(-0.392250000000000))*(x1680))));
03651 evalcond[6]=((((sj5)*(x1688)))+(((cj5)*(x1691)))+(((cj5)*(x1692)))+(((IkReal(-1.00000000000000))*(x1674)*(x1675)))+(((IkReal(-1.00000000000000))*(x1674)*(x1676)))+(((sj5)*(x1693)))+(((sj3)*(x1680)))+(((IkReal(-1.00000000000000))*(sj2)*(x1679))));
03652 evalcond[7]=((((x1671)*(x1686)))+(((sj2)*(x1676)))+(((sj2)*(x1675)))+(((x1685)*(x1692)))+(((x1685)*(x1691)))+(((IkReal(-1.00000000000000))*(cj4)*(x1672)*(x1693)))+(((IkReal(-1.00000000000000))*(x1674)*(x1679)))+(((cj2)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj4)*(x1672)*(x1688)))+(((x1670)*(x1678))));
03653 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  )
03654 {
03655 continue;
03656 }
03657 }
03658 
03659 {
03660 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03661 vinfos[0].jointtype = 1;
03662 vinfos[0].foffset = j0;
03663 vinfos[0].indices[0] = _ij0[0];
03664 vinfos[0].indices[1] = _ij0[1];
03665 vinfos[0].maxsolutions = _nj0;
03666 vinfos[1].jointtype = 1;
03667 vinfos[1].foffset = j1;
03668 vinfos[1].indices[0] = _ij1[0];
03669 vinfos[1].indices[1] = _ij1[1];
03670 vinfos[1].maxsolutions = _nj1;
03671 vinfos[2].jointtype = 1;
03672 vinfos[2].foffset = j2;
03673 vinfos[2].indices[0] = _ij2[0];
03674 vinfos[2].indices[1] = _ij2[1];
03675 vinfos[2].maxsolutions = _nj2;
03676 vinfos[3].jointtype = 1;
03677 vinfos[3].foffset = j3;
03678 vinfos[3].indices[0] = _ij3[0];
03679 vinfos[3].indices[1] = _ij3[1];
03680 vinfos[3].maxsolutions = _nj3;
03681 vinfos[4].jointtype = 1;
03682 vinfos[4].foffset = j4;
03683 vinfos[4].indices[0] = _ij4[0];
03684 vinfos[4].indices[1] = _ij4[1];
03685 vinfos[4].maxsolutions = _nj4;
03686 vinfos[5].jointtype = 1;
03687 vinfos[5].foffset = j5;
03688 vinfos[5].indices[0] = _ij5[0];
03689 vinfos[5].indices[1] = _ij5[1];
03690 vinfos[5].maxsolutions = _nj5;
03691 std::vector<int> vfree(0);
03692 solutions.AddSolution(vinfos,vfree);
03693 }
03694 }
03695 }
03696 
03697 }
03698 
03699 }
03700 }
03701 }
03702 
03703 }
03704 
03705 }
03706 
03707 } else
03708 {
03709 {
03710 IkReal j0array[1], cj0array[1], sj0array[1];
03711 bool j0valid[1]={false};
03712 _nj0 = 1;
03713 IkReal x1697=((IkReal(2183.00000000000))*(sj5));
03714 IkReal x1698=((IkReal(2183.00000000000))*(cj5));
03715 if( IKabs(((gconst0)*(((((r10)*(x1697)))+(((r11)*(x1698))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((r01)*(x1698)))+(((r00)*(x1697))))))) < IKFAST_ATAN2_MAGTHRESH )
03716     continue;
03717 j0array[0]=IKatan2(((gconst0)*(((((r10)*(x1697)))+(((r11)*(x1698)))))), ((gconst0)*(((((r01)*(x1698)))+(((r00)*(x1697)))))));
03718 sj0array[0]=IKsin(j0array[0]);
03719 cj0array[0]=IKcos(j0array[0]);
03720 if( j0array[0] > IKPI )
03721 {
03722     j0array[0]-=IK2PI;
03723 }
03724 else if( j0array[0] < -IKPI )
03725 {    j0array[0]+=IK2PI;
03726 }
03727 j0valid[0] = true;
03728 for(int ij0 = 0; ij0 < 1; ++ij0)
03729 {
03730 if( !j0valid[ij0] )
03731 {
03732     continue;
03733 }
03734 _ij0[0] = ij0; _ij0[1] = -1;
03735 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
03736 {
03737 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
03738 {
03739     j0valid[iij0]=false; _ij0[1] = iij0; break; 
03740 }
03741 }
03742 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
03743 {
03744 IkReal evalcond[4];
03745 IkReal x1699=IKcos(j0);
03746 IkReal x1700=IKsin(j0);
03747 IkReal x1701=((r00)*(sj5));
03748 IkReal x1702=((IkReal(1.00000000000000))*(cj2));
03749 IkReal x1703=((cj5)*(r11));
03750 IkReal x1704=((sj1)*(sj2));
03751 IkReal x1705=((r10)*(sj5));
03752 IkReal x1706=((IkReal(0.0946500000000000))*(x1700));
03753 IkReal x1707=((IkReal(0.0946500000000000))*(x1699));
03754 IkReal x1708=((IkReal(1.00000000000000))*(x1700));
03755 IkReal x1709=((IkReal(1.00000000000000))*(x1699));
03756 IkReal x1710=((cj5)*(r01)*(x1700));
03757 IkReal x1711=((cj5)*(x1707));
03758 evalcond[0]=((((IkReal(-1.00000000000000))*(x1701)*(x1708)))+(((x1699)*(x1703)))+(((x1699)*(x1705)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1708))));
03759 evalcond[1]=((IkReal(0.109150000000000))+(((IkReal(-1.00000000000000))*(py)*(x1709)))+(((px)*(x1700)))+(((IkReal(-1.00000000000000))*(x1705)*(x1707)))+(((x1701)*(x1706)))+(((cj5)*(r01)*(x1706)))+(((IkReal(-1.00000000000000))*(x1703)*(x1707))));
03760 evalcond[2]=((((IkReal(-1.00000000000000))*(py)*(x1708)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(px)*(x1709)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1705)*(x1706)))+(((IkReal(-1.00000000000000))*(x1703)*(x1706)))+(((IkReal(-0.392250000000000))*(x1704)))+(((IkReal(-1.00000000000000))*(x1701)*(x1707)))+(((IkReal(-1.00000000000000))*(r01)*(x1711))));
03761 evalcond[3]=((((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1702)))+(((cj5)*(r01)*(x1699)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((x1700)*(x1705)))+(((x1700)*(x1703)))+(((sj3)*(x1704)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1702)))+(((x1699)*(x1701))));
03762 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
03763 {
03764 continue;
03765 }
03766 }
03767 
03768 {
03769 IkReal dummyeval[1];
03770 IkReal gconst3;
03771 IkReal x1712=((IkReal(1.00000000000000))*(sj0));
03772 IkReal x1713=((cj5)*(r22));
03773 IkReal x1714=((r21)*(sj5));
03774 IkReal x1715=((IkReal(1.00000000000000))*(cj0));
03775 IkReal x1716=((r22)*(sj5));
03776 IkReal x1717=((cj5)*(r20));
03777 gconst3=IKsign(((((cj0)*(r12)*(x1714)))+(((r01)*(sj0)*(x1716)))+(((cj0)*(r10)*(x1713)))+(((IkReal(-1.00000000000000))*(r02)*(x1712)*(x1714)))+(((IkReal(-1.00000000000000))*(r11)*(x1715)*(x1716)))+(((IkReal(-1.00000000000000))*(r00)*(x1712)*(x1713)))+(((r02)*(sj0)*(x1717)))+(((IkReal(-1.00000000000000))*(r12)*(x1715)*(x1717)))));
03778 IkReal x1718=((IkReal(1.00000000000000))*(sj0));
03779 IkReal x1719=((cj5)*(r22));
03780 IkReal x1720=((r21)*(sj5));
03781 IkReal x1721=((IkReal(1.00000000000000))*(cj0));
03782 IkReal x1722=((r22)*(sj5));
03783 IkReal x1723=((cj5)*(r20));
03784 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(x1718)*(x1719)))+(((r02)*(sj0)*(x1723)))+(((cj0)*(r10)*(x1719)))+(((IkReal(-1.00000000000000))*(r12)*(x1721)*(x1723)))+(((IkReal(-1.00000000000000))*(r02)*(x1718)*(x1720)))+(((r01)*(sj0)*(x1722)))+(((cj0)*(r12)*(x1720)))+(((IkReal(-1.00000000000000))*(r11)*(x1721)*(x1722))));
03785 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03786 {
03787 {
03788 IkReal dummyeval[1];
03789 IkReal gconst4;
03790 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
03791 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
03792 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
03793 {
03794 continue;
03795 
03796 } else
03797 {
03798 {
03799 IkReal j4array[1], cj4array[1], sj4array[1];
03800 bool j4valid[1]={false};
03801 _nj4 = 1;
03802 IkReal x1724=((cj3)*(sj2));
03803 IkReal x1725=((cj2)*(cj3));
03804 IkReal x1726=((cj2)*(sj3));
03805 IkReal x1727=((sj2)*(sj3));
03806 IkReal x1728=((r21)*(sj5));
03807 IkReal x1729=((IkReal(1.00000000000000))*(cj1));
03808 IkReal x1730=((IkReal(1.00000000000000))*(sj1));
03809 IkReal x1731=((r22)*(x1729));
03810 IkReal x1732=((cj5)*(r20)*(sj1));
03811 IkReal x1733=((cj1)*(cj5)*(r20));
03812 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1726)*(x1731)))+(((IkReal(-1.00000000000000))*(x1724)*(x1731)))+(((IkReal(-1.00000000000000))*(r22)*(x1725)*(x1730)))+(((r22)*(sj1)*(x1727))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((x1724)*(x1733)))+(((x1725)*(x1732)))+(((x1726)*(x1733)))+(((IkReal(-1.00000000000000))*(x1726)*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(x1724)*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1727)*(x1730)))+(((sj1)*(x1727)*(x1728)))+(((IkReal(-1.00000000000000))*(x1725)*(x1728)*(x1730))))))) < IKFAST_ATAN2_MAGTHRESH )
03813     continue;
03814 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(x1726)*(x1731)))+(((IkReal(-1.00000000000000))*(x1724)*(x1731)))+(((IkReal(-1.00000000000000))*(r22)*(x1725)*(x1730)))+(((r22)*(sj1)*(x1727)))))), ((gconst4)*(((((x1724)*(x1733)))+(((x1725)*(x1732)))+(((x1726)*(x1733)))+(((IkReal(-1.00000000000000))*(x1726)*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(x1724)*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1727)*(x1730)))+(((sj1)*(x1727)*(x1728)))+(((IkReal(-1.00000000000000))*(x1725)*(x1728)*(x1730)))))));
03815 sj4array[0]=IKsin(j4array[0]);
03816 cj4array[0]=IKcos(j4array[0]);
03817 if( j4array[0] > IKPI )
03818 {
03819     j4array[0]-=IK2PI;
03820 }
03821 else if( j4array[0] < -IKPI )
03822 {    j4array[0]+=IK2PI;
03823 }
03824 j4valid[0] = true;
03825 for(int ij4 = 0; ij4 < 1; ++ij4)
03826 {
03827 if( !j4valid[ij4] )
03828 {
03829     continue;
03830 }
03831 _ij4[0] = ij4; _ij4[1] = -1;
03832 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03833 {
03834 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03835 {
03836     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03837 }
03838 }
03839 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03840 {
03841 IkReal evalcond[6];
03842 IkReal x1734=IKsin(j4);
03843 IkReal x1735=IKcos(j4);
03844 IkReal x1736=((IkReal(1.00000000000000))*(cj2));
03845 IkReal x1737=((cj3)*(sj1));
03846 IkReal x1738=((cj1)*(sj3));
03847 IkReal x1739=((IkReal(1.00000000000000))*(cj5));
03848 IkReal x1740=((IkReal(1.00000000000000))*(r01));
03849 IkReal x1741=((cj1)*(cj3));
03850 IkReal x1742=((r11)*(sj5));
03851 IkReal x1743=((sj1)*(sj3));
03852 IkReal x1744=((r01)*(sj5));
03853 IkReal x1745=((r21)*(sj5));
03854 IkReal x1746=((cj0)*(x1734));
03855 IkReal x1747=((cj5)*(x1734));
03856 IkReal x1748=((sj0)*(x1735));
03857 IkReal x1749=((IkReal(1.00000000000000))*(x1734));
03858 IkReal x1750=((cj0)*(x1735));
03859 IkReal x1751=((sj0)*(sj5)*(x1734));
03860 IkReal x1752=((IkReal(1.00000000000000))*(x1750));
03861 evalcond[0]=((((r22)*(x1735)))+(((IkReal(-1.00000000000000))*(x1745)*(x1749)))+(((r20)*(x1747))));
03862 evalcond[1]=((((r12)*(x1746)))+(((IkReal(-1.00000000000000))*(sj5)*(x1740)*(x1748)))+(((IkReal(-1.00000000000000))*(r10)*(x1739)*(x1750)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1749)))+(((x1742)*(x1750)))+(((cj5)*(r00)*(x1748))));
03863 evalcond[2]=((((x1744)*(x1746)))+(((IkReal(-1.00000000000000))*(r12)*(x1748)))+(((IkReal(-1.00000000000000))*(r00)*(x1739)*(x1746)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1734)*(x1739)))+(((IkReal(-1.00000000000000))*(r02)*(x1752)))+(((sj0)*(x1734)*(x1742))));
03864 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1740)*(x1751)))+(((IkReal(-1.00000000000000))*(r12)*(x1752)))+(((r02)*(x1748)))+(((IkReal(-1.00000000000000))*(r10)*(x1739)*(x1746)))+(((x1742)*(x1746)))+(((r00)*(sj0)*(x1747))));
03865 evalcond[4]=((((cj5)*(r20)*(x1735)))+(((IkReal(-1.00000000000000))*(x1735)*(x1745)))+(((IkReal(-1.00000000000000))*(sj2)*(x1741)))+(((IkReal(-1.00000000000000))*(r22)*(x1749)))+(((IkReal(-1.00000000000000))*(x1736)*(x1738)))+(((IkReal(-1.00000000000000))*(x1736)*(x1737)))+(((sj2)*(x1743))));
03866 evalcond[5]=((((x1744)*(x1750)))+(((IkReal(-1.00000000000000))*(x1736)*(x1741)))+(((r02)*(x1746)))+(((cj2)*(x1743)))+(((IkReal(-1.00000000000000))*(r00)*(x1739)*(x1750)))+(((IkReal(-1.00000000000000))*(r10)*(x1739)*(x1748)))+(((x1742)*(x1748)))+(((r12)*(sj0)*(x1734)))+(((sj2)*(x1738)))+(((sj2)*(x1737))));
03867 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  )
03868 {
03869 continue;
03870 }
03871 }
03872 
03873 {
03874 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03875 vinfos[0].jointtype = 1;
03876 vinfos[0].foffset = j0;
03877 vinfos[0].indices[0] = _ij0[0];
03878 vinfos[0].indices[1] = _ij0[1];
03879 vinfos[0].maxsolutions = _nj0;
03880 vinfos[1].jointtype = 1;
03881 vinfos[1].foffset = j1;
03882 vinfos[1].indices[0] = _ij1[0];
03883 vinfos[1].indices[1] = _ij1[1];
03884 vinfos[1].maxsolutions = _nj1;
03885 vinfos[2].jointtype = 1;
03886 vinfos[2].foffset = j2;
03887 vinfos[2].indices[0] = _ij2[0];
03888 vinfos[2].indices[1] = _ij2[1];
03889 vinfos[2].maxsolutions = _nj2;
03890 vinfos[3].jointtype = 1;
03891 vinfos[3].foffset = j3;
03892 vinfos[3].indices[0] = _ij3[0];
03893 vinfos[3].indices[1] = _ij3[1];
03894 vinfos[3].maxsolutions = _nj3;
03895 vinfos[4].jointtype = 1;
03896 vinfos[4].foffset = j4;
03897 vinfos[4].indices[0] = _ij4[0];
03898 vinfos[4].indices[1] = _ij4[1];
03899 vinfos[4].maxsolutions = _nj4;
03900 vinfos[5].jointtype = 1;
03901 vinfos[5].foffset = j5;
03902 vinfos[5].indices[0] = _ij5[0];
03903 vinfos[5].indices[1] = _ij5[1];
03904 vinfos[5].maxsolutions = _nj5;
03905 std::vector<int> vfree(0);
03906 solutions.AddSolution(vinfos,vfree);
03907 }
03908 }
03909 }
03910 
03911 }
03912 
03913 }
03914 
03915 } else
03916 {
03917 {
03918 IkReal j4array[1], cj4array[1], sj4array[1];
03919 bool j4valid[1]={false};
03920 _nj4 = 1;
03921 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
03922     continue;
03923 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
03924 sj4array[0]=IKsin(j4array[0]);
03925 cj4array[0]=IKcos(j4array[0]);
03926 if( j4array[0] > IKPI )
03927 {
03928     j4array[0]-=IK2PI;
03929 }
03930 else if( j4array[0] < -IKPI )
03931 {    j4array[0]+=IK2PI;
03932 }
03933 j4valid[0] = true;
03934 for(int ij4 = 0; ij4 < 1; ++ij4)
03935 {
03936 if( !j4valid[ij4] )
03937 {
03938     continue;
03939 }
03940 _ij4[0] = ij4; _ij4[1] = -1;
03941 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03942 {
03943 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03944 {
03945     j4valid[iij4]=false; _ij4[1] = iij4; break; 
03946 }
03947 }
03948 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03949 {
03950 IkReal evalcond[6];
03951 IkReal x1753=IKsin(j4);
03952 IkReal x1754=IKcos(j4);
03953 IkReal x1755=((IkReal(1.00000000000000))*(cj2));
03954 IkReal x1756=((cj3)*(sj1));
03955 IkReal x1757=((cj1)*(sj3));
03956 IkReal x1758=((IkReal(1.00000000000000))*(cj5));
03957 IkReal x1759=((IkReal(1.00000000000000))*(r01));
03958 IkReal x1760=((cj1)*(cj3));
03959 IkReal x1761=((r11)*(sj5));
03960 IkReal x1762=((sj1)*(sj3));
03961 IkReal x1763=((r01)*(sj5));
03962 IkReal x1764=((r21)*(sj5));
03963 IkReal x1765=((cj0)*(x1753));
03964 IkReal x1766=((cj5)*(x1753));
03965 IkReal x1767=((sj0)*(x1754));
03966 IkReal x1768=((IkReal(1.00000000000000))*(x1753));
03967 IkReal x1769=((cj0)*(x1754));
03968 IkReal x1770=((sj0)*(sj5)*(x1753));
03969 IkReal x1771=((IkReal(1.00000000000000))*(x1769));
03970 evalcond[0]=((((r20)*(x1766)))+(((r22)*(x1754)))+(((IkReal(-1.00000000000000))*(x1764)*(x1768))));
03971 evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1768)))+(((IkReal(-1.00000000000000))*(sj5)*(x1759)*(x1767)))+(((cj5)*(r00)*(x1767)))+(((r12)*(x1765)))+(((x1761)*(x1769)))+(((IkReal(-1.00000000000000))*(r10)*(x1758)*(x1769))));
03972 evalcond[2]=((((sj0)*(x1753)*(x1761)))+(((IkReal(-1.00000000000000))*(r00)*(x1758)*(x1765)))+(((IkReal(-1.00000000000000))*(r02)*(x1771)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1753)*(x1758)))+(((IkReal(-1.00000000000000))*(r12)*(x1767)))+(((x1763)*(x1765))));
03973 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(sj0)*(x1766)))+(((IkReal(-1.00000000000000))*(x1759)*(x1770)))+(((r02)*(x1767)))+(((x1761)*(x1765)))+(((IkReal(-1.00000000000000))*(r12)*(x1771)))+(((IkReal(-1.00000000000000))*(r10)*(x1758)*(x1765))));
03974 evalcond[4]=((((sj2)*(x1762)))+(((IkReal(-1.00000000000000))*(x1754)*(x1764)))+(((IkReal(-1.00000000000000))*(sj2)*(x1760)))+(((IkReal(-1.00000000000000))*(r22)*(x1768)))+(((cj5)*(r20)*(x1754)))+(((IkReal(-1.00000000000000))*(x1755)*(x1757)))+(((IkReal(-1.00000000000000))*(x1755)*(x1756))));
03975 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x1758)*(x1769)))+(((IkReal(-1.00000000000000))*(x1755)*(x1760)))+(((r02)*(x1765)))+(((sj2)*(x1757)))+(((sj2)*(x1756)))+(((r12)*(sj0)*(x1753)))+(((cj2)*(x1762)))+(((x1761)*(x1767)))+(((IkReal(-1.00000000000000))*(r10)*(x1758)*(x1767)))+(((x1763)*(x1769))));
03976 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  )
03977 {
03978 continue;
03979 }
03980 }
03981 
03982 {
03983 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03984 vinfos[0].jointtype = 1;
03985 vinfos[0].foffset = j0;
03986 vinfos[0].indices[0] = _ij0[0];
03987 vinfos[0].indices[1] = _ij0[1];
03988 vinfos[0].maxsolutions = _nj0;
03989 vinfos[1].jointtype = 1;
03990 vinfos[1].foffset = j1;
03991 vinfos[1].indices[0] = _ij1[0];
03992 vinfos[1].indices[1] = _ij1[1];
03993 vinfos[1].maxsolutions = _nj1;
03994 vinfos[2].jointtype = 1;
03995 vinfos[2].foffset = j2;
03996 vinfos[2].indices[0] = _ij2[0];
03997 vinfos[2].indices[1] = _ij2[1];
03998 vinfos[2].maxsolutions = _nj2;
03999 vinfos[3].jointtype = 1;
04000 vinfos[3].foffset = j3;
04001 vinfos[3].indices[0] = _ij3[0];
04002 vinfos[3].indices[1] = _ij3[1];
04003 vinfos[3].maxsolutions = _nj3;
04004 vinfos[4].jointtype = 1;
04005 vinfos[4].foffset = j4;
04006 vinfos[4].indices[0] = _ij4[0];
04007 vinfos[4].indices[1] = _ij4[1];
04008 vinfos[4].maxsolutions = _nj4;
04009 vinfos[5].jointtype = 1;
04010 vinfos[5].foffset = j5;
04011 vinfos[5].indices[0] = _ij5[0];
04012 vinfos[5].indices[1] = _ij5[1];
04013 vinfos[5].maxsolutions = _nj5;
04014 std::vector<int> vfree(0);
04015 solutions.AddSolution(vinfos,vfree);
04016 }
04017 }
04018 }
04019 
04020 }
04021 
04022 }
04023 }
04024 }
04025 
04026 }
04027 
04028 }
04029 }
04030 }
04031 
04032 }
04033 
04034 }
04035 
04036 } else
04037 {
04038 {
04039 IkReal j5array[2], cj5array[2], sj5array[2];
04040 bool j5valid[2]={false};
04041 _nj5 = 2;
04042 if( IKabs(((IkReal(0.0946500000000000))*(r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(0.0946500000000000))*(r20))) < IKFAST_ATAN2_MAGTHRESH )
04043     continue;
04044 IkReal x1772=((IkReal(1.00000000000000))*(IKatan2(((IkReal(0.0946500000000000))*(r21)), ((IkReal(0.0946500000000000))*(r20)))));
04045 if( (((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))) < (IkReal)-0.00001 )
04046     continue;
04047 if( (((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1))))))) < -1-IKFAST_SINCOS_THRESH || (((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1))))))) > 1+IKFAST_SINCOS_THRESH )
04048     continue;
04049 IkReal x1773=IKasin(((((IKabs(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt(((((IkReal(0.00895862250000000))*((r21)*(r21))))+(((IkReal(0.00895862250000000))*((r20)*(r20))))))))):(IkReal)1.0e30))*(((pz)+(((IkReal(0.392250000000000))*(cj1)*(sj2)))+(((IkReal(0.425000000000000))*(sj1)))+(((IkReal(0.392250000000000))*(cj2)*(sj1)))))));
04050 j5array[0]=((((IkReal(-1.00000000000000))*(x1773)))+(((IkReal(-1.00000000000000))*(x1772))));
04051 sj5array[0]=IKsin(j5array[0]);
04052 cj5array[0]=IKcos(j5array[0]);
04053 j5array[1]=((IkReal(3.14159265358979))+(x1773)+(((IkReal(-1.00000000000000))*(x1772))));
04054 sj5array[1]=IKsin(j5array[1]);
04055 cj5array[1]=IKcos(j5array[1]);
04056 if( j5array[0] > IKPI )
04057 {
04058     j5array[0]-=IK2PI;
04059 }
04060 else if( j5array[0] < -IKPI )
04061 {    j5array[0]+=IK2PI;
04062 }
04063 j5valid[0] = true;
04064 if( j5array[1] > IKPI )
04065 {
04066     j5array[1]-=IK2PI;
04067 }
04068 else if( j5array[1] < -IKPI )
04069 {    j5array[1]+=IK2PI;
04070 }
04071 j5valid[1] = true;
04072 for(int ij5 = 0; ij5 < 2; ++ij5)
04073 {
04074 if( !j5valid[ij5] )
04075 {
04076     continue;
04077 }
04078 _ij5[0] = ij5; _ij5[1] = -1;
04079 for(int iij5 = ij5+1; iij5 < 2; ++iij5)
04080 {
04081 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
04082 {
04083     j5valid[iij5]=false; _ij5[1] = iij5; break; 
04084 }
04085 }
04086 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
04087 {
04088 IkReal evalcond[1];
04089 IkReal x1774=((IkReal(1.00000000000000))*(sj3));
04090 evalcond[0]=((((IkReal(-1.00000000000000))*(cj2)*(sj1)*(x1774)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(sj2)))+(((cj1)*(cj2)*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)*(x1774)))+(((IkReal(-1.00000000000000))*(r21)*(IKcos(j5))))+(((IkReal(-1.00000000000000))*(r20)*(IKsin(j5)))));
04091 if( IKabs(evalcond[0]) > 0.000001  )
04092 {
04093 continue;
04094 }
04095 }
04096 
04097 {
04098 IkReal dummyeval[1];
04099 IkReal gconst0;
04100 IkReal x1775=((IkReal(20000.0000000000))*(cj5));
04101 IkReal x1776=((IkReal(20000.0000000000))*(sj5));
04102 gconst0=IKsign(((((py)*(r00)*(x1776)))+(((py)*(r01)*(x1775)))+(((IkReal(-1.00000000000000))*(px)*(r11)*(x1775)))+(((IkReal(-1.00000000000000))*(px)*(r10)*(x1776)))));
04103 IkReal x1777=((IkReal(1.00000000000000))*(px));
04104 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1777)))+(((cj5)*(py)*(r01)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1777)))+(((py)*(r00)*(sj5))));
04105 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04106 {
04107 {
04108 IkReal dummyeval[1];
04109 IkReal gconst2;
04110 gconst2=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
04111 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
04112 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04113 {
04114 {
04115 IkReal dummyeval[1];
04116 IkReal gconst1;
04117 IkReal x1778=(cj5)*(cj5);
04118 IkReal x1779=(sj5)*(sj5);
04119 IkReal x1780=((IkReal(1.00000000000000))*(x1779));
04120 IkReal x1781=((IkReal(2.00000000000000))*(cj5)*(sj5));
04121 IkReal x1782=((IkReal(1.00000000000000))*(x1778));
04122 gconst1=IKsign(((((IkReal(-1.00000000000000))*(x1782)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1781)))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1781)))+(((IkReal(-1.00000000000000))*(x1780)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1780)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1782)*((r01)*(r01))))));
04123 IkReal x1783=(cj5)*(cj5);
04124 IkReal x1784=(sj5)*(sj5);
04125 IkReal x1785=((IkReal(1.00000000000000))*(x1784));
04126 IkReal x1786=((IkReal(2.00000000000000))*(cj5)*(sj5));
04127 IkReal x1787=((IkReal(1.00000000000000))*(x1783));
04128 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1787)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1785)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1787)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1785)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1786)))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1786))));
04129 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04130 {
04131 continue;
04132 
04133 } else
04134 {
04135 {
04136 IkReal j0array[1], cj0array[1], sj0array[1];
04137 bool j0valid[1]={false};
04138 _nj0 = 1;
04139 IkReal x1788=((sj2)*(sj3));
04140 IkReal x1789=((cj5)*(r01));
04141 IkReal x1790=((cj5)*(r11));
04142 IkReal x1791=((r00)*(sj5));
04143 IkReal x1792=((r10)*(sj5));
04144 IkReal x1793=((sj1)*(x1791));
04145 IkReal x1794=((IkReal(1.00000000000000))*(cj2)*(cj3));
04146 IkReal x1795=((sj1)*(x1792));
04147 IkReal x1796=((IkReal(1.00000000000000))*(cj1)*(cj2)*(sj3));
04148 IkReal x1797=((IkReal(1.00000000000000))*(cj1)*(cj3)*(sj2));
04149 if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x1792)*(x1797)))+(((IkReal(-1.00000000000000))*(x1792)*(x1796)))+(((IkReal(-1.00000000000000))*(x1790)*(x1797)))+(((IkReal(-1.00000000000000))*(x1790)*(x1796)))+(((x1788)*(x1795)))+(((IkReal(-1.00000000000000))*(sj1)*(x1790)*(x1794)))+(((sj1)*(x1788)*(x1790)))+(((IkReal(-1.00000000000000))*(x1794)*(x1795))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x1793)*(x1794)))+(((IkReal(-1.00000000000000))*(x1789)*(x1796)))+(((IkReal(-1.00000000000000))*(x1789)*(x1797)))+(((IkReal(-1.00000000000000))*(sj1)*(x1789)*(x1794)))+(((sj1)*(x1788)*(x1789)))+(((x1788)*(x1793)))+(((IkReal(-1.00000000000000))*(x1791)*(x1796)))+(((IkReal(-1.00000000000000))*(x1791)*(x1797))))))) < IKFAST_ATAN2_MAGTHRESH )
04150     continue;
04151 j0array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(x1792)*(x1797)))+(((IkReal(-1.00000000000000))*(x1792)*(x1796)))+(((IkReal(-1.00000000000000))*(x1790)*(x1797)))+(((IkReal(-1.00000000000000))*(x1790)*(x1796)))+(((x1788)*(x1795)))+(((IkReal(-1.00000000000000))*(sj1)*(x1790)*(x1794)))+(((sj1)*(x1788)*(x1790)))+(((IkReal(-1.00000000000000))*(x1794)*(x1795)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x1793)*(x1794)))+(((IkReal(-1.00000000000000))*(x1789)*(x1796)))+(((IkReal(-1.00000000000000))*(x1789)*(x1797)))+(((IkReal(-1.00000000000000))*(sj1)*(x1789)*(x1794)))+(((sj1)*(x1788)*(x1789)))+(((x1788)*(x1793)))+(((IkReal(-1.00000000000000))*(x1791)*(x1796)))+(((IkReal(-1.00000000000000))*(x1791)*(x1797)))))));
04152 sj0array[0]=IKsin(j0array[0]);
04153 cj0array[0]=IKcos(j0array[0]);
04154 if( j0array[0] > IKPI )
04155 {
04156     j0array[0]-=IK2PI;
04157 }
04158 else if( j0array[0] < -IKPI )
04159 {    j0array[0]+=IK2PI;
04160 }
04161 j0valid[0] = true;
04162 for(int ij0 = 0; ij0 < 1; ++ij0)
04163 {
04164 if( !j0valid[ij0] )
04165 {
04166     continue;
04167 }
04168 _ij0[0] = ij0; _ij0[1] = -1;
04169 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
04170 {
04171 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
04172 {
04173     j0valid[iij0]=false; _ij0[1] = iij0; break; 
04174 }
04175 }
04176 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
04177 {
04178 IkReal evalcond[4];
04179 IkReal x1798=IKcos(j0);
04180 IkReal x1799=IKsin(j0);
04181 IkReal x1800=((r00)*(sj5));
04182 IkReal x1801=((IkReal(1.00000000000000))*(cj2));
04183 IkReal x1802=((cj5)*(r11));
04184 IkReal x1803=((sj1)*(sj2));
04185 IkReal x1804=((r10)*(sj5));
04186 IkReal x1805=((IkReal(0.0946500000000000))*(x1799));
04187 IkReal x1806=((IkReal(0.0946500000000000))*(x1798));
04188 IkReal x1807=((IkReal(1.00000000000000))*(x1799));
04189 IkReal x1808=((IkReal(1.00000000000000))*(x1798));
04190 IkReal x1809=((cj5)*(r01)*(x1799));
04191 IkReal x1810=((cj5)*(x1806));
04192 evalcond[0]=((((x1798)*(x1804)))+(((x1798)*(x1802)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1807)))+(((IkReal(-1.00000000000000))*(x1800)*(x1807))));
04193 evalcond[1]=((IkReal(0.109150000000000))+(((cj5)*(r01)*(x1805)))+(((x1800)*(x1805)))+(((IkReal(-1.00000000000000))*(x1804)*(x1806)))+(((IkReal(-1.00000000000000))*(x1802)*(x1806)))+(((IkReal(-1.00000000000000))*(py)*(x1808)))+(((px)*(x1799))));
04194 evalcond[2]=((((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1804)*(x1805)))+(((IkReal(-1.00000000000000))*(x1802)*(x1805)))+(((IkReal(-1.00000000000000))*(r01)*(x1810)))+(((IkReal(-1.00000000000000))*(py)*(x1807)))+(((IkReal(-1.00000000000000))*(x1800)*(x1806)))+(((IkReal(-1.00000000000000))*(px)*(x1808)))+(((IkReal(-0.392250000000000))*(x1803))));
04195 evalcond[3]=((((x1799)*(x1802)))+(((x1799)*(x1804)))+(((x1798)*(x1800)))+(((cj5)*(r01)*(x1798)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((sj3)*(x1803)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1801)))+(((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1801))));
04196 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
04197 {
04198 continue;
04199 }
04200 }
04201 
04202 {
04203 IkReal dummyeval[1];
04204 IkReal gconst3;
04205 IkReal x1811=((IkReal(1.00000000000000))*(sj0));
04206 IkReal x1812=((cj5)*(r22));
04207 IkReal x1813=((r21)*(sj5));
04208 IkReal x1814=((IkReal(1.00000000000000))*(cj0));
04209 IkReal x1815=((r22)*(sj5));
04210 IkReal x1816=((cj5)*(r20));
04211 gconst3=IKsign(((((IkReal(-1.00000000000000))*(r00)*(x1811)*(x1812)))+(((cj0)*(r12)*(x1813)))+(((cj0)*(r10)*(x1812)))+(((IkReal(-1.00000000000000))*(r11)*(x1814)*(x1815)))+(((IkReal(-1.00000000000000))*(r12)*(x1814)*(x1816)))+(((r01)*(sj0)*(x1815)))+(((r02)*(sj0)*(x1816)))+(((IkReal(-1.00000000000000))*(r02)*(x1811)*(x1813)))));
04212 IkReal x1817=((IkReal(1.00000000000000))*(sj0));
04213 IkReal x1818=((cj5)*(r22));
04214 IkReal x1819=((r21)*(sj5));
04215 IkReal x1820=((IkReal(1.00000000000000))*(cj0));
04216 IkReal x1821=((r22)*(sj5));
04217 IkReal x1822=((cj5)*(r20));
04218 dummyeval[0]=((((cj0)*(r12)*(x1819)))+(((r01)*(sj0)*(x1821)))+(((cj0)*(r10)*(x1818)))+(((IkReal(-1.00000000000000))*(r11)*(x1820)*(x1821)))+(((IkReal(-1.00000000000000))*(r00)*(x1817)*(x1818)))+(((IkReal(-1.00000000000000))*(r12)*(x1820)*(x1822)))+(((IkReal(-1.00000000000000))*(r02)*(x1817)*(x1819)))+(((r02)*(sj0)*(x1822))));
04219 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04220 {
04221 {
04222 IkReal dummyeval[1];
04223 IkReal gconst4;
04224 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
04225 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
04226 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04227 {
04228 continue;
04229 
04230 } else
04231 {
04232 {
04233 IkReal j4array[1], cj4array[1], sj4array[1];
04234 bool j4valid[1]={false};
04235 _nj4 = 1;
04236 IkReal x1823=((cj3)*(sj2));
04237 IkReal x1824=((cj2)*(cj3));
04238 IkReal x1825=((cj2)*(sj3));
04239 IkReal x1826=((sj2)*(sj3));
04240 IkReal x1827=((r21)*(sj5));
04241 IkReal x1828=((IkReal(1.00000000000000))*(cj1));
04242 IkReal x1829=((IkReal(1.00000000000000))*(sj1));
04243 IkReal x1830=((r22)*(x1828));
04244 IkReal x1831=((cj5)*(r20)*(sj1));
04245 IkReal x1832=((cj1)*(cj5)*(r20));
04246 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1825)*(x1830)))+(((r22)*(sj1)*(x1826)))+(((IkReal(-1.00000000000000))*(x1823)*(x1830)))+(((IkReal(-1.00000000000000))*(r22)*(x1824)*(x1829))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((sj1)*(x1826)*(x1827)))+(((x1824)*(x1831)))+(((IkReal(-1.00000000000000))*(x1825)*(x1827)*(x1828)))+(((IkReal(-1.00000000000000))*(x1823)*(x1827)*(x1828)))+(((x1823)*(x1832)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1826)*(x1829)))+(((x1825)*(x1832)))+(((IkReal(-1.00000000000000))*(x1824)*(x1827)*(x1829))))))) < IKFAST_ATAN2_MAGTHRESH )
04247     continue;
04248 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(x1825)*(x1830)))+(((r22)*(sj1)*(x1826)))+(((IkReal(-1.00000000000000))*(x1823)*(x1830)))+(((IkReal(-1.00000000000000))*(r22)*(x1824)*(x1829)))))), ((gconst4)*(((((sj1)*(x1826)*(x1827)))+(((x1824)*(x1831)))+(((IkReal(-1.00000000000000))*(x1825)*(x1827)*(x1828)))+(((IkReal(-1.00000000000000))*(x1823)*(x1827)*(x1828)))+(((x1823)*(x1832)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1826)*(x1829)))+(((x1825)*(x1832)))+(((IkReal(-1.00000000000000))*(x1824)*(x1827)*(x1829)))))));
04249 sj4array[0]=IKsin(j4array[0]);
04250 cj4array[0]=IKcos(j4array[0]);
04251 if( j4array[0] > IKPI )
04252 {
04253     j4array[0]-=IK2PI;
04254 }
04255 else if( j4array[0] < -IKPI )
04256 {    j4array[0]+=IK2PI;
04257 }
04258 j4valid[0] = true;
04259 for(int ij4 = 0; ij4 < 1; ++ij4)
04260 {
04261 if( !j4valid[ij4] )
04262 {
04263     continue;
04264 }
04265 _ij4[0] = ij4; _ij4[1] = -1;
04266 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04267 {
04268 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04269 {
04270     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04271 }
04272 }
04273 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04274 {
04275 IkReal evalcond[6];
04276 IkReal x1833=IKsin(j4);
04277 IkReal x1834=IKcos(j4);
04278 IkReal x1835=((IkReal(1.00000000000000))*(cj2));
04279 IkReal x1836=((cj3)*(sj1));
04280 IkReal x1837=((cj1)*(sj3));
04281 IkReal x1838=((IkReal(1.00000000000000))*(cj5));
04282 IkReal x1839=((IkReal(1.00000000000000))*(r01));
04283 IkReal x1840=((cj1)*(cj3));
04284 IkReal x1841=((r11)*(sj5));
04285 IkReal x1842=((sj1)*(sj3));
04286 IkReal x1843=((r01)*(sj5));
04287 IkReal x1844=((r21)*(sj5));
04288 IkReal x1845=((cj0)*(x1833));
04289 IkReal x1846=((cj5)*(x1833));
04290 IkReal x1847=((sj0)*(x1834));
04291 IkReal x1848=((IkReal(1.00000000000000))*(x1833));
04292 IkReal x1849=((cj0)*(x1834));
04293 IkReal x1850=((sj0)*(sj5)*(x1833));
04294 IkReal x1851=((IkReal(1.00000000000000))*(x1849));
04295 evalcond[0]=((((r22)*(x1834)))+(((r20)*(x1846)))+(((IkReal(-1.00000000000000))*(x1844)*(x1848))));
04296 evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1848)))+(((cj5)*(r00)*(x1847)))+(((IkReal(-1.00000000000000))*(sj5)*(x1839)*(x1847)))+(((r12)*(x1845)))+(((x1841)*(x1849)))+(((IkReal(-1.00000000000000))*(r10)*(x1838)*(x1849))));
04297 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1833)*(x1838)))+(((IkReal(-1.00000000000000))*(r00)*(x1838)*(x1845)))+(((IkReal(-1.00000000000000))*(r12)*(x1847)))+(((IkReal(-1.00000000000000))*(r02)*(x1851)))+(((x1843)*(x1845)))+(((sj0)*(x1833)*(x1841))));
04298 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(sj0)*(x1846)))+(((r02)*(x1847)))+(((IkReal(-1.00000000000000))*(r12)*(x1851)))+(((x1841)*(x1845)))+(((IkReal(-1.00000000000000))*(x1839)*(x1850)))+(((IkReal(-1.00000000000000))*(r10)*(x1838)*(x1845))));
04299 evalcond[4]=((((IkReal(-1.00000000000000))*(r22)*(x1848)))+(((cj5)*(r20)*(x1834)))+(((sj2)*(x1842)))+(((IkReal(-1.00000000000000))*(x1835)*(x1836)))+(((IkReal(-1.00000000000000))*(x1835)*(x1837)))+(((IkReal(-1.00000000000000))*(sj2)*(x1840)))+(((IkReal(-1.00000000000000))*(x1834)*(x1844))));
04300 evalcond[5]=((((r02)*(x1845)))+(((cj2)*(x1842)))+(((sj2)*(x1836)))+(((sj2)*(x1837)))+(((IkReal(-1.00000000000000))*(r00)*(x1838)*(x1849)))+(((r12)*(sj0)*(x1833)))+(((IkReal(-1.00000000000000))*(x1835)*(x1840)))+(((x1843)*(x1849)))+(((x1841)*(x1847)))+(((IkReal(-1.00000000000000))*(r10)*(x1838)*(x1847))));
04301 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  )
04302 {
04303 continue;
04304 }
04305 }
04306 
04307 {
04308 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04309 vinfos[0].jointtype = 1;
04310 vinfos[0].foffset = j0;
04311 vinfos[0].indices[0] = _ij0[0];
04312 vinfos[0].indices[1] = _ij0[1];
04313 vinfos[0].maxsolutions = _nj0;
04314 vinfos[1].jointtype = 1;
04315 vinfos[1].foffset = j1;
04316 vinfos[1].indices[0] = _ij1[0];
04317 vinfos[1].indices[1] = _ij1[1];
04318 vinfos[1].maxsolutions = _nj1;
04319 vinfos[2].jointtype = 1;
04320 vinfos[2].foffset = j2;
04321 vinfos[2].indices[0] = _ij2[0];
04322 vinfos[2].indices[1] = _ij2[1];
04323 vinfos[2].maxsolutions = _nj2;
04324 vinfos[3].jointtype = 1;
04325 vinfos[3].foffset = j3;
04326 vinfos[3].indices[0] = _ij3[0];
04327 vinfos[3].indices[1] = _ij3[1];
04328 vinfos[3].maxsolutions = _nj3;
04329 vinfos[4].jointtype = 1;
04330 vinfos[4].foffset = j4;
04331 vinfos[4].indices[0] = _ij4[0];
04332 vinfos[4].indices[1] = _ij4[1];
04333 vinfos[4].maxsolutions = _nj4;
04334 vinfos[5].jointtype = 1;
04335 vinfos[5].foffset = j5;
04336 vinfos[5].indices[0] = _ij5[0];
04337 vinfos[5].indices[1] = _ij5[1];
04338 vinfos[5].maxsolutions = _nj5;
04339 std::vector<int> vfree(0);
04340 solutions.AddSolution(vinfos,vfree);
04341 }
04342 }
04343 }
04344 
04345 }
04346 
04347 }
04348 
04349 } else
04350 {
04351 {
04352 IkReal j4array[1], cj4array[1], sj4array[1];
04353 bool j4valid[1]={false};
04354 _nj4 = 1;
04355 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
04356     continue;
04357 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
04358 sj4array[0]=IKsin(j4array[0]);
04359 cj4array[0]=IKcos(j4array[0]);
04360 if( j4array[0] > IKPI )
04361 {
04362     j4array[0]-=IK2PI;
04363 }
04364 else if( j4array[0] < -IKPI )
04365 {    j4array[0]+=IK2PI;
04366 }
04367 j4valid[0] = true;
04368 for(int ij4 = 0; ij4 < 1; ++ij4)
04369 {
04370 if( !j4valid[ij4] )
04371 {
04372     continue;
04373 }
04374 _ij4[0] = ij4; _ij4[1] = -1;
04375 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04376 {
04377 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04378 {
04379     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04380 }
04381 }
04382 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04383 {
04384 IkReal evalcond[6];
04385 IkReal x1852=IKsin(j4);
04386 IkReal x1853=IKcos(j4);
04387 IkReal x1854=((IkReal(1.00000000000000))*(cj2));
04388 IkReal x1855=((cj3)*(sj1));
04389 IkReal x1856=((cj1)*(sj3));
04390 IkReal x1857=((IkReal(1.00000000000000))*(cj5));
04391 IkReal x1858=((IkReal(1.00000000000000))*(r01));
04392 IkReal x1859=((cj1)*(cj3));
04393 IkReal x1860=((r11)*(sj5));
04394 IkReal x1861=((sj1)*(sj3));
04395 IkReal x1862=((r01)*(sj5));
04396 IkReal x1863=((r21)*(sj5));
04397 IkReal x1864=((cj0)*(x1852));
04398 IkReal x1865=((cj5)*(x1852));
04399 IkReal x1866=((sj0)*(x1853));
04400 IkReal x1867=((IkReal(1.00000000000000))*(x1852));
04401 IkReal x1868=((cj0)*(x1853));
04402 IkReal x1869=((sj0)*(sj5)*(x1852));
04403 IkReal x1870=((IkReal(1.00000000000000))*(x1868));
04404 evalcond[0]=((((IkReal(-1.00000000000000))*(x1863)*(x1867)))+(((r22)*(x1853)))+(((r20)*(x1865))));
04405 evalcond[1]=((((r12)*(x1864)))+(((x1860)*(x1868)))+(((cj5)*(r00)*(x1866)))+(((IkReal(-1.00000000000000))*(sj5)*(x1858)*(x1866)))+(((IkReal(-1.00000000000000))*(r10)*(x1857)*(x1868)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1867))));
04406 evalcond[2]=((((IkReal(-1.00000000000000))*(r12)*(x1866)))+(((IkReal(-1.00000000000000))*(r02)*(x1870)))+(((IkReal(-1.00000000000000))*(r00)*(x1857)*(x1864)))+(((x1862)*(x1864)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1852)*(x1857)))+(((sj0)*(x1852)*(x1860))));
04407 evalcond[3]=((IkReal(1.00000000000000))+(((x1860)*(x1864)))+(((r02)*(x1866)))+(((r00)*(sj0)*(x1865)))+(((IkReal(-1.00000000000000))*(r10)*(x1857)*(x1864)))+(((IkReal(-1.00000000000000))*(r12)*(x1870)))+(((IkReal(-1.00000000000000))*(x1858)*(x1869))));
04408 evalcond[4]=((((IkReal(-1.00000000000000))*(sj2)*(x1859)))+(((IkReal(-1.00000000000000))*(x1853)*(x1863)))+(((sj2)*(x1861)))+(((cj5)*(r20)*(x1853)))+(((IkReal(-1.00000000000000))*(x1854)*(x1855)))+(((IkReal(-1.00000000000000))*(x1854)*(x1856)))+(((IkReal(-1.00000000000000))*(r22)*(x1867))));
04409 evalcond[5]=((((r12)*(sj0)*(x1852)))+(((x1860)*(x1866)))+(((IkReal(-1.00000000000000))*(r00)*(x1857)*(x1868)))+(((x1862)*(x1868)))+(((r02)*(x1864)))+(((IkReal(-1.00000000000000))*(r10)*(x1857)*(x1866)))+(((sj2)*(x1855)))+(((sj2)*(x1856)))+(((IkReal(-1.00000000000000))*(x1854)*(x1859)))+(((cj2)*(x1861))));
04410 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  )
04411 {
04412 continue;
04413 }
04414 }
04415 
04416 {
04417 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04418 vinfos[0].jointtype = 1;
04419 vinfos[0].foffset = j0;
04420 vinfos[0].indices[0] = _ij0[0];
04421 vinfos[0].indices[1] = _ij0[1];
04422 vinfos[0].maxsolutions = _nj0;
04423 vinfos[1].jointtype = 1;
04424 vinfos[1].foffset = j1;
04425 vinfos[1].indices[0] = _ij1[0];
04426 vinfos[1].indices[1] = _ij1[1];
04427 vinfos[1].maxsolutions = _nj1;
04428 vinfos[2].jointtype = 1;
04429 vinfos[2].foffset = j2;
04430 vinfos[2].indices[0] = _ij2[0];
04431 vinfos[2].indices[1] = _ij2[1];
04432 vinfos[2].maxsolutions = _nj2;
04433 vinfos[3].jointtype = 1;
04434 vinfos[3].foffset = j3;
04435 vinfos[3].indices[0] = _ij3[0];
04436 vinfos[3].indices[1] = _ij3[1];
04437 vinfos[3].maxsolutions = _nj3;
04438 vinfos[4].jointtype = 1;
04439 vinfos[4].foffset = j4;
04440 vinfos[4].indices[0] = _ij4[0];
04441 vinfos[4].indices[1] = _ij4[1];
04442 vinfos[4].maxsolutions = _nj4;
04443 vinfos[5].jointtype = 1;
04444 vinfos[5].foffset = j5;
04445 vinfos[5].indices[0] = _ij5[0];
04446 vinfos[5].indices[1] = _ij5[1];
04447 vinfos[5].maxsolutions = _nj5;
04448 std::vector<int> vfree(0);
04449 solutions.AddSolution(vinfos,vfree);
04450 }
04451 }
04452 }
04453 
04454 }
04455 
04456 }
04457 }
04458 }
04459 
04460 }
04461 
04462 }
04463 
04464 } else
04465 {
04466 {
04467 IkReal j4array[1], cj4array[1], sj4array[1];
04468 bool j4valid[1]={false};
04469 _nj4 = 1;
04470 IkReal x1871=((cj3)*(sj2));
04471 IkReal x1872=((cj2)*(sj3));
04472 IkReal x1873=((cj5)*(r20));
04473 IkReal x1874=((r21)*(sj5));
04474 IkReal x1875=((IkReal(1.00000000000000))*(cj1));
04475 IkReal x1876=((r22)*(x1875));
04476 IkReal x1877=((sj1)*(sj2)*(sj3));
04477 IkReal x1878=((cj2)*(cj3)*(sj1));
04478 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x1872)*(x1876)))+(((r22)*(x1877)))+(((IkReal(-1.00000000000000))*(r22)*(x1878)))+(((IkReal(-1.00000000000000))*(x1871)*(x1876))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((x1874)*(x1877)))+(((cj1)*(x1872)*(x1873)))+(((IkReal(-1.00000000000000))*(x1874)*(x1878)))+(((IkReal(-1.00000000000000))*(x1873)*(x1877)))+(((x1873)*(x1878)))+(((IkReal(-1.00000000000000))*(x1871)*(x1874)*(x1875)))+(((cj1)*(x1871)*(x1873)))+(((IkReal(-1.00000000000000))*(x1872)*(x1874)*(x1875))))))) < IKFAST_ATAN2_MAGTHRESH )
04479     continue;
04480 j4array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(x1872)*(x1876)))+(((r22)*(x1877)))+(((IkReal(-1.00000000000000))*(r22)*(x1878)))+(((IkReal(-1.00000000000000))*(x1871)*(x1876)))))), ((gconst2)*(((((x1874)*(x1877)))+(((cj1)*(x1872)*(x1873)))+(((IkReal(-1.00000000000000))*(x1874)*(x1878)))+(((IkReal(-1.00000000000000))*(x1873)*(x1877)))+(((x1873)*(x1878)))+(((IkReal(-1.00000000000000))*(x1871)*(x1874)*(x1875)))+(((cj1)*(x1871)*(x1873)))+(((IkReal(-1.00000000000000))*(x1872)*(x1874)*(x1875)))))));
04481 sj4array[0]=IKsin(j4array[0]);
04482 cj4array[0]=IKcos(j4array[0]);
04483 if( j4array[0] > IKPI )
04484 {
04485     j4array[0]-=IK2PI;
04486 }
04487 else if( j4array[0] < -IKPI )
04488 {    j4array[0]+=IK2PI;
04489 }
04490 j4valid[0] = true;
04491 for(int ij4 = 0; ij4 < 1; ++ij4)
04492 {
04493 if( !j4valid[ij4] )
04494 {
04495     continue;
04496 }
04497 _ij4[0] = ij4; _ij4[1] = -1;
04498 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04499 {
04500 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04501 {
04502     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04503 }
04504 }
04505 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04506 {
04507 IkReal evalcond[2];
04508 IkReal x1879=IKsin(j4);
04509 IkReal x1880=IKcos(j4);
04510 IkReal x1881=((IkReal(1.00000000000000))*(cj1));
04511 IkReal x1882=((cj5)*(r20));
04512 IkReal x1883=((IkReal(1.00000000000000))*(r21)*(sj5));
04513 evalcond[0]=((((x1879)*(x1882)))+(((IkReal(-1.00000000000000))*(x1879)*(x1883)))+(((r22)*(x1880))));
04514 evalcond[1]=((((IkReal(-1.00000000000000))*(x1880)*(x1883)))+(((x1880)*(x1882)))+(((IkReal(-1.00000000000000))*(r22)*(x1879)))+(((IkReal(-1.00000000000000))*(cj3)*(sj2)*(x1881)))+(((IkReal(-1.00000000000000))*(cj2)*(cj3)*(sj1)))+(((sj1)*(sj2)*(sj3)))+(((IkReal(-1.00000000000000))*(cj2)*(sj3)*(x1881))));
04515 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
04516 {
04517 continue;
04518 }
04519 }
04520 
04521 {
04522 IkReal dummyeval[1];
04523 IkReal gconst5;
04524 IkReal x1884=((IkReal(20000.0000000000))*(cj5));
04525 IkReal x1885=((IkReal(20000.0000000000))*(sj5));
04526 gconst5=IKsign(((((py)*(r01)*(x1884)))+(((IkReal(-1.00000000000000))*(px)*(r10)*(x1885)))+(((IkReal(-1.00000000000000))*(px)*(r11)*(x1884)))+(((py)*(r00)*(x1885)))));
04527 IkReal x1886=((IkReal(1.00000000000000))*(px));
04528 dummyeval[0]=((((cj5)*(py)*(r01)))+(((py)*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1886)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1886))));
04529 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04530 {
04531 {
04532 IkReal dummyeval[1];
04533 IkReal gconst6;
04534 IkReal x1887=(sj5)*(sj5);
04535 IkReal x1888=(cj5)*(cj5);
04536 IkReal x1889=((cj4)*(sj5));
04537 IkReal x1890=((IkReal(1.00000000000000))*(r02));
04538 IkReal x1891=((r01)*(r10));
04539 IkReal x1892=((cj4)*(cj5));
04540 IkReal x1893=((sj4)*(x1888));
04541 IkReal x1894=((IkReal(1.00000000000000))*(r00)*(r11));
04542 IkReal x1895=((sj4)*(x1887));
04543 gconst6=IKsign(((((IkReal(-1.00000000000000))*(x1893)*(x1894)))+(((r01)*(r12)*(x1892)))+(((IkReal(-1.00000000000000))*(r11)*(x1890)*(x1892)))+(((x1891)*(x1893)))+(((x1891)*(x1895)))+(((IkReal(-1.00000000000000))*(x1894)*(x1895)))+(((r00)*(r12)*(x1889)))+(((IkReal(-1.00000000000000))*(r10)*(x1889)*(x1890)))));
04544 IkReal x1896=(sj5)*(sj5);
04545 IkReal x1897=(cj5)*(cj5);
04546 IkReal x1898=((cj4)*(sj5));
04547 IkReal x1899=((IkReal(1.00000000000000))*(r02));
04548 IkReal x1900=((r01)*(r10));
04549 IkReal x1901=((cj4)*(cj5));
04550 IkReal x1902=((sj4)*(x1897));
04551 IkReal x1903=((IkReal(1.00000000000000))*(r00)*(r11));
04552 IkReal x1904=((sj4)*(x1896));
04553 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1902)*(x1903)))+(((r01)*(r12)*(x1901)))+(((IkReal(-1.00000000000000))*(x1903)*(x1904)))+(((IkReal(-1.00000000000000))*(r11)*(x1899)*(x1901)))+(((x1900)*(x1904)))+(((x1900)*(x1902)))+(((IkReal(-1.00000000000000))*(r10)*(x1898)*(x1899)))+(((r00)*(r12)*(x1898))));
04554 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04555 {
04556 continue;
04557 
04558 } else
04559 {
04560 {
04561 IkReal j0array[1], cj0array[1], sj0array[1];
04562 bool j0valid[1]={false};
04563 _nj0 = 1;
04564 if( IKabs(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH )
04565     continue;
04566 j0array[0]=IKatan2(((gconst6)*(((((cj5)*(r11)))+(((r10)*(sj5)))))), ((gconst6)*(((((r00)*(sj5)))+(((cj5)*(r01)))))));
04567 sj0array[0]=IKsin(j0array[0]);
04568 cj0array[0]=IKcos(j0array[0]);
04569 if( j0array[0] > IKPI )
04570 {
04571     j0array[0]-=IK2PI;
04572 }
04573 else if( j0array[0] < -IKPI )
04574 {    j0array[0]+=IK2PI;
04575 }
04576 j0valid[0] = true;
04577 for(int ij0 = 0; ij0 < 1; ++ij0)
04578 {
04579 if( !j0valid[ij0] )
04580 {
04581     continue;
04582 }
04583 _ij0[0] = ij0; _ij0[1] = -1;
04584 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
04585 {
04586 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
04587 {
04588     j0valid[iij0]=false; _ij0[1] = iij0; break; 
04589 }
04590 }
04591 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
04592 {
04593 IkReal evalcond[8];
04594 IkReal x1905=IKcos(j0);
04595 IkReal x1906=IKsin(j0);
04596 IkReal x1907=((IkReal(1.00000000000000))*(cj5));
04597 IkReal x1908=((IkReal(0.0946500000000000))*(sj5));
04598 IkReal x1909=((IkReal(1.00000000000000))*(cj2));
04599 IkReal x1910=((cj3)*(sj1));
04600 IkReal x1911=((cj1)*(sj3));
04601 IkReal x1912=((IkReal(0.0946500000000000))*(cj5));
04602 IkReal x1913=((r02)*(sj4));
04603 IkReal x1914=((cj1)*(cj3));
04604 IkReal x1915=((sj1)*(sj2));
04605 IkReal x1916=((sj4)*(sj5));
04606 IkReal x1917=((IkReal(1.00000000000000))*(sj5));
04607 IkReal x1918=((cj4)*(r02));
04608 IkReal x1919=((cj4)*(r12));
04609 IkReal x1920=((cj4)*(sj5));
04610 IkReal x1921=((r12)*(sj4));
04611 IkReal x1922=((r01)*(x1906));
04612 IkReal x1923=((r00)*(x1905));
04613 IkReal x1924=((r00)*(x1906));
04614 IkReal x1925=((r11)*(x1905));
04615 IkReal x1926=((r01)*(x1905));
04616 IkReal x1927=((r11)*(x1906));
04617 IkReal x1928=((r10)*(x1906));
04618 IkReal x1929=((IkReal(1.00000000000000))*(x1906));
04619 IkReal x1930=((r10)*(x1905));
04620 IkReal x1931=((IkReal(1.00000000000000))*(x1905));
04621 evalcond[0]=((((IkReal(-1.00000000000000))*(x1907)*(x1922)))+(((cj5)*(x1925)))+(((IkReal(-1.00000000000000))*(x1917)*(x1924)))+(((sj5)*(x1930))));
04622 evalcond[1]=((IkReal(0.109150000000000))+(((IkReal(-1.00000000000000))*(py)*(x1931)))+(((px)*(x1906)))+(((IkReal(-1.00000000000000))*(x1912)*(x1925)))+(((x1912)*(x1922)))+(((IkReal(-1.00000000000000))*(x1908)*(x1930)))+(((x1908)*(x1924))));
04623 evalcond[2]=((((cj4)*(cj5)*(x1924)))+(((IkReal(-1.00000000000000))*(x1913)*(x1929)))+(((IkReal(-1.00000000000000))*(cj4)*(x1907)*(x1930)))+(((x1905)*(x1921)))+(((x1920)*(x1925)))+(((IkReal(-1.00000000000000))*(cj4)*(x1917)*(x1922))));
04624 evalcond[3]=((((x1916)*(x1927)))+(((x1916)*(x1926)))+(((IkReal(-1.00000000000000))*(sj4)*(x1907)*(x1923)))+(((IkReal(-1.00000000000000))*(sj4)*(x1907)*(x1928)))+(((IkReal(-1.00000000000000))*(x1918)*(x1931)))+(((IkReal(-1.00000000000000))*(x1919)*(x1929))));
04625 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1919)*(x1931)))+(((IkReal(-1.00000000000000))*(sj4)*(x1907)*(x1930)))+(((cj5)*(sj4)*(x1924)))+(((x1916)*(x1925)))+(((IkReal(-1.00000000000000))*(x1916)*(x1922)))+(((x1906)*(x1918))));
04626 evalcond[5]=((((IkReal(-1.00000000000000))*(x1908)*(x1928)))+(((IkReal(-1.00000000000000))*(x1908)*(x1923)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-0.392250000000000))*(x1915)))+(((IkReal(-1.00000000000000))*(py)*(x1929)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1912)*(x1926)))+(((IkReal(-1.00000000000000))*(x1912)*(x1927)))+(((IkReal(-1.00000000000000))*(px)*(x1931))));
04627 evalcond[6]=((((cj5)*(x1927)))+(((cj5)*(x1926)))+(((IkReal(-1.00000000000000))*(sj2)*(x1914)))+(((sj5)*(x1928)))+(((sj5)*(x1923)))+(((IkReal(-1.00000000000000))*(x1909)*(x1910)))+(((IkReal(-1.00000000000000))*(x1909)*(x1911)))+(((sj3)*(x1915))));
04628 evalcond[7]=((((x1905)*(x1913)))+(((IkReal(-1.00000000000000))*(cj4)*(x1907)*(x1923)))+(((IkReal(-1.00000000000000))*(cj4)*(x1907)*(x1928)))+(((x1906)*(x1921)))+(((IkReal(-1.00000000000000))*(x1909)*(x1914)))+(((x1920)*(x1926)))+(((x1920)*(x1927)))+(((sj2)*(x1910)))+(((sj2)*(x1911)))+(((cj2)*(sj1)*(sj3))));
04629 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  )
04630 {
04631 continue;
04632 }
04633 }
04634 
04635 {
04636 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04637 vinfos[0].jointtype = 1;
04638 vinfos[0].foffset = j0;
04639 vinfos[0].indices[0] = _ij0[0];
04640 vinfos[0].indices[1] = _ij0[1];
04641 vinfos[0].maxsolutions = _nj0;
04642 vinfos[1].jointtype = 1;
04643 vinfos[1].foffset = j1;
04644 vinfos[1].indices[0] = _ij1[0];
04645 vinfos[1].indices[1] = _ij1[1];
04646 vinfos[1].maxsolutions = _nj1;
04647 vinfos[2].jointtype = 1;
04648 vinfos[2].foffset = j2;
04649 vinfos[2].indices[0] = _ij2[0];
04650 vinfos[2].indices[1] = _ij2[1];
04651 vinfos[2].maxsolutions = _nj2;
04652 vinfos[3].jointtype = 1;
04653 vinfos[3].foffset = j3;
04654 vinfos[3].indices[0] = _ij3[0];
04655 vinfos[3].indices[1] = _ij3[1];
04656 vinfos[3].maxsolutions = _nj3;
04657 vinfos[4].jointtype = 1;
04658 vinfos[4].foffset = j4;
04659 vinfos[4].indices[0] = _ij4[0];
04660 vinfos[4].indices[1] = _ij4[1];
04661 vinfos[4].maxsolutions = _nj4;
04662 vinfos[5].jointtype = 1;
04663 vinfos[5].foffset = j5;
04664 vinfos[5].indices[0] = _ij5[0];
04665 vinfos[5].indices[1] = _ij5[1];
04666 vinfos[5].maxsolutions = _nj5;
04667 std::vector<int> vfree(0);
04668 solutions.AddSolution(vinfos,vfree);
04669 }
04670 }
04671 }
04672 
04673 }
04674 
04675 }
04676 
04677 } else
04678 {
04679 {
04680 IkReal j0array[1], cj0array[1], sj0array[1];
04681 bool j0valid[1]={false};
04682 _nj0 = 1;
04683 IkReal x1932=((IkReal(2183.00000000000))*(sj5));
04684 IkReal x1933=((IkReal(2183.00000000000))*(cj5));
04685 if( IKabs(((gconst5)*(((((r11)*(x1933)))+(((r10)*(x1932))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((r00)*(x1932)))+(((r01)*(x1933))))))) < IKFAST_ATAN2_MAGTHRESH )
04686     continue;
04687 j0array[0]=IKatan2(((gconst5)*(((((r11)*(x1933)))+(((r10)*(x1932)))))), ((gconst5)*(((((r00)*(x1932)))+(((r01)*(x1933)))))));
04688 sj0array[0]=IKsin(j0array[0]);
04689 cj0array[0]=IKcos(j0array[0]);
04690 if( j0array[0] > IKPI )
04691 {
04692     j0array[0]-=IK2PI;
04693 }
04694 else if( j0array[0] < -IKPI )
04695 {    j0array[0]+=IK2PI;
04696 }
04697 j0valid[0] = true;
04698 for(int ij0 = 0; ij0 < 1; ++ij0)
04699 {
04700 if( !j0valid[ij0] )
04701 {
04702     continue;
04703 }
04704 _ij0[0] = ij0; _ij0[1] = -1;
04705 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
04706 {
04707 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
04708 {
04709     j0valid[iij0]=false; _ij0[1] = iij0; break; 
04710 }
04711 }
04712 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
04713 {
04714 IkReal evalcond[8];
04715 IkReal x1934=IKcos(j0);
04716 IkReal x1935=IKsin(j0);
04717 IkReal x1936=((IkReal(1.00000000000000))*(cj5));
04718 IkReal x1937=((IkReal(0.0946500000000000))*(sj5));
04719 IkReal x1938=((IkReal(1.00000000000000))*(cj2));
04720 IkReal x1939=((cj3)*(sj1));
04721 IkReal x1940=((cj1)*(sj3));
04722 IkReal x1941=((IkReal(0.0946500000000000))*(cj5));
04723 IkReal x1942=((r02)*(sj4));
04724 IkReal x1943=((cj1)*(cj3));
04725 IkReal x1944=((sj1)*(sj2));
04726 IkReal x1945=((sj4)*(sj5));
04727 IkReal x1946=((IkReal(1.00000000000000))*(sj5));
04728 IkReal x1947=((cj4)*(r02));
04729 IkReal x1948=((cj4)*(r12));
04730 IkReal x1949=((cj4)*(sj5));
04731 IkReal x1950=((r12)*(sj4));
04732 IkReal x1951=((r01)*(x1935));
04733 IkReal x1952=((r00)*(x1934));
04734 IkReal x1953=((r00)*(x1935));
04735 IkReal x1954=((r11)*(x1934));
04736 IkReal x1955=((r01)*(x1934));
04737 IkReal x1956=((r11)*(x1935));
04738 IkReal x1957=((r10)*(x1935));
04739 IkReal x1958=((IkReal(1.00000000000000))*(x1935));
04740 IkReal x1959=((r10)*(x1934));
04741 IkReal x1960=((IkReal(1.00000000000000))*(x1934));
04742 evalcond[0]=((((sj5)*(x1959)))+(((cj5)*(x1954)))+(((IkReal(-1.00000000000000))*(x1946)*(x1953)))+(((IkReal(-1.00000000000000))*(x1936)*(x1951))));
04743 evalcond[1]=((IkReal(0.109150000000000))+(((IkReal(-1.00000000000000))*(x1937)*(x1959)))+(((x1937)*(x1953)))+(((px)*(x1935)))+(((x1941)*(x1951)))+(((IkReal(-1.00000000000000))*(x1941)*(x1954)))+(((IkReal(-1.00000000000000))*(py)*(x1960))));
04744 evalcond[2]=((((IkReal(-1.00000000000000))*(x1942)*(x1958)))+(((IkReal(-1.00000000000000))*(cj4)*(x1946)*(x1951)))+(((IkReal(-1.00000000000000))*(cj4)*(x1936)*(x1959)))+(((cj4)*(cj5)*(x1953)))+(((x1934)*(x1950)))+(((x1949)*(x1954))));
04745 evalcond[3]=((((IkReal(-1.00000000000000))*(x1948)*(x1958)))+(((x1945)*(x1956)))+(((x1945)*(x1955)))+(((IkReal(-1.00000000000000))*(sj4)*(x1936)*(x1952)))+(((IkReal(-1.00000000000000))*(sj4)*(x1936)*(x1957)))+(((IkReal(-1.00000000000000))*(x1947)*(x1960))));
04746 evalcond[4]=((IkReal(1.00000000000000))+(((x1945)*(x1954)))+(((IkReal(-1.00000000000000))*(x1948)*(x1960)))+(((IkReal(-1.00000000000000))*(sj4)*(x1936)*(x1959)))+(((IkReal(-1.00000000000000))*(x1945)*(x1951)))+(((x1935)*(x1947)))+(((cj5)*(sj4)*(x1953))));
04747 evalcond[5]=((((IkReal(-1.00000000000000))*(x1937)*(x1952)))+(((IkReal(-1.00000000000000))*(x1937)*(x1957)))+(((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(-0.392250000000000))*(x1944)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(py)*(x1958)))+(((IkReal(-1.00000000000000))*(px)*(x1960)))+(((IkReal(-1.00000000000000))*(x1941)*(x1956)))+(((IkReal(-1.00000000000000))*(x1941)*(x1955))));
04748 evalcond[6]=((((sj3)*(x1944)))+(((sj5)*(x1957)))+(((sj5)*(x1952)))+(((cj5)*(x1956)))+(((cj5)*(x1955)))+(((IkReal(-1.00000000000000))*(x1938)*(x1940)))+(((IkReal(-1.00000000000000))*(x1938)*(x1939)))+(((IkReal(-1.00000000000000))*(sj2)*(x1943))));
04749 evalcond[7]=((((sj2)*(x1939)))+(((sj2)*(x1940)))+(((x1935)*(x1950)))+(((IkReal(-1.00000000000000))*(x1938)*(x1943)))+(((IkReal(-1.00000000000000))*(cj4)*(x1936)*(x1952)))+(((IkReal(-1.00000000000000))*(cj4)*(x1936)*(x1957)))+(((cj2)*(sj1)*(sj3)))+(((x1934)*(x1942)))+(((x1949)*(x1955)))+(((x1949)*(x1956))));
04750 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  )
04751 {
04752 continue;
04753 }
04754 }
04755 
04756 {
04757 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04758 vinfos[0].jointtype = 1;
04759 vinfos[0].foffset = j0;
04760 vinfos[0].indices[0] = _ij0[0];
04761 vinfos[0].indices[1] = _ij0[1];
04762 vinfos[0].maxsolutions = _nj0;
04763 vinfos[1].jointtype = 1;
04764 vinfos[1].foffset = j1;
04765 vinfos[1].indices[0] = _ij1[0];
04766 vinfos[1].indices[1] = _ij1[1];
04767 vinfos[1].maxsolutions = _nj1;
04768 vinfos[2].jointtype = 1;
04769 vinfos[2].foffset = j2;
04770 vinfos[2].indices[0] = _ij2[0];
04771 vinfos[2].indices[1] = _ij2[1];
04772 vinfos[2].maxsolutions = _nj2;
04773 vinfos[3].jointtype = 1;
04774 vinfos[3].foffset = j3;
04775 vinfos[3].indices[0] = _ij3[0];
04776 vinfos[3].indices[1] = _ij3[1];
04777 vinfos[3].maxsolutions = _nj3;
04778 vinfos[4].jointtype = 1;
04779 vinfos[4].foffset = j4;
04780 vinfos[4].indices[0] = _ij4[0];
04781 vinfos[4].indices[1] = _ij4[1];
04782 vinfos[4].maxsolutions = _nj4;
04783 vinfos[5].jointtype = 1;
04784 vinfos[5].foffset = j5;
04785 vinfos[5].indices[0] = _ij5[0];
04786 vinfos[5].indices[1] = _ij5[1];
04787 vinfos[5].maxsolutions = _nj5;
04788 std::vector<int> vfree(0);
04789 solutions.AddSolution(vinfos,vfree);
04790 }
04791 }
04792 }
04793 
04794 }
04795 
04796 }
04797 }
04798 }
04799 
04800 }
04801 
04802 }
04803 
04804 } else
04805 {
04806 {
04807 IkReal j0array[1], cj0array[1], sj0array[1];
04808 bool j0valid[1]={false};
04809 _nj0 = 1;
04810 IkReal x1961=((IkReal(2183.00000000000))*(sj5));
04811 IkReal x1962=((IkReal(2183.00000000000))*(cj5));
04812 if( IKabs(((gconst0)*(((((r10)*(x1961)))+(((r11)*(x1962))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((r00)*(x1961)))+(((r01)*(x1962))))))) < IKFAST_ATAN2_MAGTHRESH )
04813     continue;
04814 j0array[0]=IKatan2(((gconst0)*(((((r10)*(x1961)))+(((r11)*(x1962)))))), ((gconst0)*(((((r00)*(x1961)))+(((r01)*(x1962)))))));
04815 sj0array[0]=IKsin(j0array[0]);
04816 cj0array[0]=IKcos(j0array[0]);
04817 if( j0array[0] > IKPI )
04818 {
04819     j0array[0]-=IK2PI;
04820 }
04821 else if( j0array[0] < -IKPI )
04822 {    j0array[0]+=IK2PI;
04823 }
04824 j0valid[0] = true;
04825 for(int ij0 = 0; ij0 < 1; ++ij0)
04826 {
04827 if( !j0valid[ij0] )
04828 {
04829     continue;
04830 }
04831 _ij0[0] = ij0; _ij0[1] = -1;
04832 for(int iij0 = ij0+1; iij0 < 1; ++iij0)
04833 {
04834 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
04835 {
04836     j0valid[iij0]=false; _ij0[1] = iij0; break; 
04837 }
04838 }
04839 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
04840 {
04841 IkReal evalcond[4];
04842 IkReal x1963=IKcos(j0);
04843 IkReal x1964=IKsin(j0);
04844 IkReal x1965=((r00)*(sj5));
04845 IkReal x1966=((IkReal(1.00000000000000))*(cj2));
04846 IkReal x1967=((cj5)*(r11));
04847 IkReal x1968=((sj1)*(sj2));
04848 IkReal x1969=((r10)*(sj5));
04849 IkReal x1970=((IkReal(0.0946500000000000))*(x1964));
04850 IkReal x1971=((IkReal(0.0946500000000000))*(x1963));
04851 IkReal x1972=((IkReal(1.00000000000000))*(x1964));
04852 IkReal x1973=((IkReal(1.00000000000000))*(x1963));
04853 IkReal x1974=((cj5)*(r01)*(x1964));
04854 IkReal x1975=((cj5)*(x1971));
04855 evalcond[0]=((((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1972)))+(((IkReal(-1.00000000000000))*(x1965)*(x1972)))+(((x1963)*(x1967)))+(((x1963)*(x1969))));
04856 evalcond[1]=((IkReal(0.109150000000000))+(((px)*(x1964)))+(((x1965)*(x1970)))+(((IkReal(-1.00000000000000))*(py)*(x1973)))+(((IkReal(-1.00000000000000))*(x1969)*(x1971)))+(((IkReal(-1.00000000000000))*(x1967)*(x1971)))+(((cj5)*(r01)*(x1970))));
04857 evalcond[2]=((((IkReal(0.392250000000000))*(cj1)*(cj2)))+(((IkReal(0.425000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1965)*(x1971)))+(((IkReal(-0.392250000000000))*(x1968)))+(((IkReal(-1.00000000000000))*(r01)*(x1975)))+(((IkReal(-1.00000000000000))*(px)*(x1973)))+(((IkReal(-1.00000000000000))*(py)*(x1972)))+(((IkReal(-1.00000000000000))*(x1969)*(x1970)))+(((IkReal(-1.00000000000000))*(x1967)*(x1970))));
04858 evalcond[3]=((((IkReal(-1.00000000000000))*(cj1)*(sj3)*(x1966)))+(((sj3)*(x1968)))+(((x1964)*(x1967)))+(((x1964)*(x1969)))+(((IkReal(-1.00000000000000))*(cj1)*(cj3)*(sj2)))+(((IkReal(-1.00000000000000))*(cj3)*(sj1)*(x1966)))+(((x1963)*(x1965)))+(((cj5)*(r01)*(x1963))));
04859 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
04860 {
04861 continue;
04862 }
04863 }
04864 
04865 {
04866 IkReal dummyeval[1];
04867 IkReal gconst3;
04868 IkReal x1976=((IkReal(1.00000000000000))*(sj0));
04869 IkReal x1977=((cj5)*(r22));
04870 IkReal x1978=((r21)*(sj5));
04871 IkReal x1979=((IkReal(1.00000000000000))*(cj0));
04872 IkReal x1980=((r22)*(sj5));
04873 IkReal x1981=((cj5)*(r20));
04874 gconst3=IKsign(((((IkReal(-1.00000000000000))*(r11)*(x1979)*(x1980)))+(((IkReal(-1.00000000000000))*(r12)*(x1979)*(x1981)))+(((cj0)*(r10)*(x1977)))+(((r02)*(sj0)*(x1981)))+(((r01)*(sj0)*(x1980)))+(((IkReal(-1.00000000000000))*(r02)*(x1976)*(x1978)))+(((IkReal(-1.00000000000000))*(r00)*(x1976)*(x1977)))+(((cj0)*(r12)*(x1978)))));
04875 IkReal x1982=((IkReal(1.00000000000000))*(sj0));
04876 IkReal x1983=((cj5)*(r22));
04877 IkReal x1984=((r21)*(sj5));
04878 IkReal x1985=((IkReal(1.00000000000000))*(cj0));
04879 IkReal x1986=((r22)*(sj5));
04880 IkReal x1987=((cj5)*(r20));
04881 dummyeval[0]=((((IkReal(-1.00000000000000))*(r12)*(x1985)*(x1987)))+(((IkReal(-1.00000000000000))*(r11)*(x1985)*(x1986)))+(((IkReal(-1.00000000000000))*(r02)*(x1982)*(x1984)))+(((r02)*(sj0)*(x1987)))+(((r01)*(sj0)*(x1986)))+(((IkReal(-1.00000000000000))*(r00)*(x1982)*(x1983)))+(((cj0)*(r12)*(x1984)))+(((cj0)*(r10)*(x1983))));
04882 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04883 {
04884 {
04885 IkReal dummyeval[1];
04886 IkReal gconst4;
04887 gconst4=IKsign((((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5))))));
04888 dummyeval[0]=(((r22)*(r22))+((((cj5)*(cj5))*((r20)*(r20))))+(((IkReal(-2.00000000000000))*(cj5)*(r20)*(r21)*(sj5)))+((((r21)*(r21))*((sj5)*(sj5)))));
04889 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
04890 {
04891 continue;
04892 
04893 } else
04894 {
04895 {
04896 IkReal j4array[1], cj4array[1], sj4array[1];
04897 bool j4valid[1]={false};
04898 _nj4 = 1;
04899 IkReal x1988=((cj3)*(sj2));
04900 IkReal x1989=((cj2)*(cj3));
04901 IkReal x1990=((cj2)*(sj3));
04902 IkReal x1991=((sj2)*(sj3));
04903 IkReal x1992=((r21)*(sj5));
04904 IkReal x1993=((IkReal(1.00000000000000))*(cj1));
04905 IkReal x1994=((IkReal(1.00000000000000))*(sj1));
04906 IkReal x1995=((r22)*(x1993));
04907 IkReal x1996=((cj5)*(r20)*(sj1));
04908 IkReal x1997=((cj1)*(cj5)*(r20));
04909 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(r22)*(x1989)*(x1994)))+(((r22)*(sj1)*(x1991)))+(((IkReal(-1.00000000000000))*(x1990)*(x1995)))+(((IkReal(-1.00000000000000))*(x1988)*(x1995))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((sj1)*(x1991)*(x1992)))+(((x1988)*(x1997)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1991)*(x1994)))+(((IkReal(-1.00000000000000))*(x1989)*(x1992)*(x1994)))+(((x1989)*(x1996)))+(((IkReal(-1.00000000000000))*(x1990)*(x1992)*(x1993)))+(((IkReal(-1.00000000000000))*(x1988)*(x1992)*(x1993)))+(((x1990)*(x1997))))))) < IKFAST_ATAN2_MAGTHRESH )
04910     continue;
04911 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(r22)*(x1989)*(x1994)))+(((r22)*(sj1)*(x1991)))+(((IkReal(-1.00000000000000))*(x1990)*(x1995)))+(((IkReal(-1.00000000000000))*(x1988)*(x1995)))))), ((gconst4)*(((((sj1)*(x1991)*(x1992)))+(((x1988)*(x1997)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)*(x1991)*(x1994)))+(((IkReal(-1.00000000000000))*(x1989)*(x1992)*(x1994)))+(((x1989)*(x1996)))+(((IkReal(-1.00000000000000))*(x1990)*(x1992)*(x1993)))+(((IkReal(-1.00000000000000))*(x1988)*(x1992)*(x1993)))+(((x1990)*(x1997)))))));
04912 sj4array[0]=IKsin(j4array[0]);
04913 cj4array[0]=IKcos(j4array[0]);
04914 if( j4array[0] > IKPI )
04915 {
04916     j4array[0]-=IK2PI;
04917 }
04918 else if( j4array[0] < -IKPI )
04919 {    j4array[0]+=IK2PI;
04920 }
04921 j4valid[0] = true;
04922 for(int ij4 = 0; ij4 < 1; ++ij4)
04923 {
04924 if( !j4valid[ij4] )
04925 {
04926     continue;
04927 }
04928 _ij4[0] = ij4; _ij4[1] = -1;
04929 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04930 {
04931 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04932 {
04933     j4valid[iij4]=false; _ij4[1] = iij4; break; 
04934 }
04935 }
04936 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04937 {
04938 IkReal evalcond[6];
04939 IkReal x1998=IKsin(j4);
04940 IkReal x1999=IKcos(j4);
04941 IkReal x2000=((IkReal(1.00000000000000))*(cj2));
04942 IkReal x2001=((cj3)*(sj1));
04943 IkReal x2002=((cj1)*(sj3));
04944 IkReal x2003=((IkReal(1.00000000000000))*(cj5));
04945 IkReal x2004=((IkReal(1.00000000000000))*(r01));
04946 IkReal x2005=((cj1)*(cj3));
04947 IkReal x2006=((r11)*(sj5));
04948 IkReal x2007=((sj1)*(sj3));
04949 IkReal x2008=((r01)*(sj5));
04950 IkReal x2009=((r21)*(sj5));
04951 IkReal x2010=((cj0)*(x1998));
04952 IkReal x2011=((cj5)*(x1998));
04953 IkReal x2012=((sj0)*(x1999));
04954 IkReal x2013=((IkReal(1.00000000000000))*(x1998));
04955 IkReal x2014=((cj0)*(x1999));
04956 IkReal x2015=((sj0)*(sj5)*(x1998));
04957 IkReal x2016=((IkReal(1.00000000000000))*(x2014));
04958 evalcond[0]=((((IkReal(-1.00000000000000))*(x2009)*(x2013)))+(((r20)*(x2011)))+(((r22)*(x1999))));
04959 evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(sj0)*(x2013)))+(((cj5)*(r00)*(x2012)))+(((r12)*(x2010)))+(((x2006)*(x2014)))+(((IkReal(-1.00000000000000))*(r10)*(x2003)*(x2014)))+(((IkReal(-1.00000000000000))*(sj5)*(x2004)*(x2012))));
04960 evalcond[2]=((((IkReal(-1.00000000000000))*(r12)*(x2012)))+(((sj0)*(x1998)*(x2006)))+(((x2008)*(x2010)))+(((IkReal(-1.00000000000000))*(r00)*(x2003)*(x2010)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1998)*(x2003)))+(((IkReal(-1.00000000000000))*(r02)*(x2016))));
04961 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x2004)*(x2015)))+(((r00)*(sj0)*(x2011)))+(((IkReal(-1.00000000000000))*(r12)*(x2016)))+(((r02)*(x2012)))+(((x2006)*(x2010)))+(((IkReal(-1.00000000000000))*(r10)*(x2003)*(x2010))));
04962 evalcond[4]=((((cj5)*(r20)*(x1999)))+(((sj2)*(x2007)))+(((IkReal(-1.00000000000000))*(x1999)*(x2009)))+(((IkReal(-1.00000000000000))*(sj2)*(x2005)))+(((IkReal(-1.00000000000000))*(x2000)*(x2002)))+(((IkReal(-1.00000000000000))*(x2000)*(x2001)))+(((IkReal(-1.00000000000000))*(r22)*(x2013))));
04963 evalcond[5]=((((sj2)*(x2001)))+(((sj2)*(x2002)))+(((IkReal(-1.00000000000000))*(x2000)*(x2005)))+(((r12)*(sj0)*(x1998)))+(((x2008)*(x2014)))+(((cj2)*(x2007)))+(((IkReal(-1.00000000000000))*(r00)*(x2003)*(x2014)))+(((r02)*(x2010)))+(((x2006)*(x2012)))+(((IkReal(-1.00000000000000))*(r10)*(x2003)*(x2012))));
04964 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  )
04965 {
04966 continue;
04967 }
04968 }
04969 
04970 {
04971 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04972 vinfos[0].jointtype = 1;
04973 vinfos[0].foffset = j0;
04974 vinfos[0].indices[0] = _ij0[0];
04975 vinfos[0].indices[1] = _ij0[1];
04976 vinfos[0].maxsolutions = _nj0;
04977 vinfos[1].jointtype = 1;
04978 vinfos[1].foffset = j1;
04979 vinfos[1].indices[0] = _ij1[0];
04980 vinfos[1].indices[1] = _ij1[1];
04981 vinfos[1].maxsolutions = _nj1;
04982 vinfos[2].jointtype = 1;
04983 vinfos[2].foffset = j2;
04984 vinfos[2].indices[0] = _ij2[0];
04985 vinfos[2].indices[1] = _ij2[1];
04986 vinfos[2].maxsolutions = _nj2;
04987 vinfos[3].jointtype = 1;
04988 vinfos[3].foffset = j3;
04989 vinfos[3].indices[0] = _ij3[0];
04990 vinfos[3].indices[1] = _ij3[1];
04991 vinfos[3].maxsolutions = _nj3;
04992 vinfos[4].jointtype = 1;
04993 vinfos[4].foffset = j4;
04994 vinfos[4].indices[0] = _ij4[0];
04995 vinfos[4].indices[1] = _ij4[1];
04996 vinfos[4].maxsolutions = _nj4;
04997 vinfos[5].jointtype = 1;
04998 vinfos[5].foffset = j5;
04999 vinfos[5].indices[0] = _ij5[0];
05000 vinfos[5].indices[1] = _ij5[1];
05001 vinfos[5].maxsolutions = _nj5;
05002 std::vector<int> vfree(0);
05003 solutions.AddSolution(vinfos,vfree);
05004 }
05005 }
05006 }
05007 
05008 }
05009 
05010 }
05011 
05012 } else
05013 {
05014 {
05015 IkReal j4array[1], cj4array[1], sj4array[1];
05016 bool j4valid[1]={false};
05017 _nj4 = 1;
05018 if( IKabs(((gconst3)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
05019     continue;
05020 j4array[0]=IKatan2(((gconst3)*(r22)), ((gconst3)*(((((r21)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r20)))))));
05021 sj4array[0]=IKsin(j4array[0]);
05022 cj4array[0]=IKcos(j4array[0]);
05023 if( j4array[0] > IKPI )
05024 {
05025     j4array[0]-=IK2PI;
05026 }
05027 else if( j4array[0] < -IKPI )
05028 {    j4array[0]+=IK2PI;
05029 }
05030 j4valid[0] = true;
05031 for(int ij4 = 0; ij4 < 1; ++ij4)
05032 {
05033 if( !j4valid[ij4] )
05034 {
05035     continue;
05036 }
05037 _ij4[0] = ij4; _ij4[1] = -1;
05038 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05039 {
05040 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05041 {
05042     j4valid[iij4]=false; _ij4[1] = iij4; break; 
05043 }
05044 }
05045 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05046 {
05047 IkReal evalcond[6];
05048 IkReal x2017=IKsin(j4);
05049 IkReal x2018=IKcos(j4);
05050 IkReal x2019=((IkReal(1.00000000000000))*(cj2));
05051 IkReal x2020=((cj3)*(sj1));
05052 IkReal x2021=((cj1)*(sj3));
05053 IkReal x2022=((IkReal(1.00000000000000))*(cj5));
05054 IkReal x2023=((IkReal(1.00000000000000))*(r01));
05055 IkReal x2024=((cj1)*(cj3));
05056 IkReal x2025=((r11)*(sj5));
05057 IkReal x2026=((sj1)*(sj3));
05058 IkReal x2027=((r01)*(sj5));
05059 IkReal x2028=((r21)*(sj5));
05060 IkReal x2029=((cj0)*(x2017));
05061 IkReal x2030=((cj5)*(x2017));
05062 IkReal x2031=((sj0)*(x2018));
05063 IkReal x2032=((IkReal(1.00000000000000))*(x2017));
05064 IkReal x2033=((cj0)*(x2018));
05065 IkReal x2034=((sj0)*(sj5)*(x2017));
05066 IkReal x2035=((IkReal(1.00000000000000))*(x2033));
05067 evalcond[0]=((((r22)*(x2018)))+(((r20)*(x2030)))+(((IkReal(-1.00000000000000))*(x2028)*(x2032))));
05068 evalcond[1]=((((x2025)*(x2033)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x2032)))+(((cj5)*(r00)*(x2031)))+(((IkReal(-1.00000000000000))*(r10)*(x2022)*(x2033)))+(((r12)*(x2029)))+(((IkReal(-1.00000000000000))*(sj5)*(x2023)*(x2031))));
05069 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj0)*(x2017)*(x2022)))+(((IkReal(-1.00000000000000))*(r00)*(x2022)*(x2029)))+(((IkReal(-1.00000000000000))*(r02)*(x2035)))+(((sj0)*(x2017)*(x2025)))+(((x2027)*(x2029)))+(((IkReal(-1.00000000000000))*(r12)*(x2031))));
05070 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r12)*(x2035)))+(((r00)*(sj0)*(x2030)))+(((IkReal(-1.00000000000000))*(x2023)*(x2034)))+(((x2025)*(x2029)))+(((r02)*(x2031)))+(((IkReal(-1.00000000000000))*(r10)*(x2022)*(x2029))));
05071 evalcond[4]=((((IkReal(-1.00000000000000))*(r22)*(x2032)))+(((IkReal(-1.00000000000000))*(x2018)*(x2028)))+(((sj2)*(x2026)))+(((IkReal(-1.00000000000000))*(x2019)*(x2020)))+(((IkReal(-1.00000000000000))*(x2019)*(x2021)))+(((IkReal(-1.00000000000000))*(sj2)*(x2024)))+(((cj5)*(r20)*(x2018))));
05072 evalcond[5]=((((x2027)*(x2033)))+(((x2025)*(x2031)))+(((sj2)*(x2021)))+(((sj2)*(x2020)))+(((IkReal(-1.00000000000000))*(r00)*(x2022)*(x2033)))+(((IkReal(-1.00000000000000))*(x2019)*(x2024)))+(((IkReal(-1.00000000000000))*(r10)*(x2022)*(x2031)))+(((r02)*(x2029)))+(((cj2)*(x2026)))+(((r12)*(sj0)*(x2017))));
05073 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  )
05074 {
05075 continue;
05076 }
05077 }
05078 
05079 {
05080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05081 vinfos[0].jointtype = 1;
05082 vinfos[0].foffset = j0;
05083 vinfos[0].indices[0] = _ij0[0];
05084 vinfos[0].indices[1] = _ij0[1];
05085 vinfos[0].maxsolutions = _nj0;
05086 vinfos[1].jointtype = 1;
05087 vinfos[1].foffset = j1;
05088 vinfos[1].indices[0] = _ij1[0];
05089 vinfos[1].indices[1] = _ij1[1];
05090 vinfos[1].maxsolutions = _nj1;
05091 vinfos[2].jointtype = 1;
05092 vinfos[2].foffset = j2;
05093 vinfos[2].indices[0] = _ij2[0];
05094 vinfos[2].indices[1] = _ij2[1];
05095 vinfos[2].maxsolutions = _nj2;
05096 vinfos[3].jointtype = 1;
05097 vinfos[3].foffset = j3;
05098 vinfos[3].indices[0] = _ij3[0];
05099 vinfos[3].indices[1] = _ij3[1];
05100 vinfos[3].maxsolutions = _nj3;
05101 vinfos[4].jointtype = 1;
05102 vinfos[4].foffset = j4;
05103 vinfos[4].indices[0] = _ij4[0];
05104 vinfos[4].indices[1] = _ij4[1];
05105 vinfos[4].maxsolutions = _nj4;
05106 vinfos[5].jointtype = 1;
05107 vinfos[5].foffset = j5;
05108 vinfos[5].indices[0] = _ij5[0];
05109 vinfos[5].indices[1] = _ij5[1];
05110 vinfos[5].maxsolutions = _nj5;
05111 std::vector<int> vfree(0);
05112 solutions.AddSolution(vinfos,vfree);
05113 }
05114 }
05115 }
05116 
05117 }
05118 
05119 }
05120 }
05121 }
05122 
05123 }
05124 
05125 }
05126 }
05127 }
05128 
05129 }
05130 
05131 }
05132     }
05133 }
05134 return solutions.GetNumSolutions()>0;
05135 }
05136 
05137 template<int D>
05138 static inline bool matrixinverse(IkReal* A)
05139 {
05140     int n = D;
05141     int info;
05142     IkReal IKFAST_ALIGNED16(work[D*D*(D-1)]);
05143     int ipiv[D];
05144     dgetrf_(&n, &n, A, &n, &ipiv[0], &info);
05145     if( info != 0 ) {
05146         return false;
05147     }
05148     int worksize=D*D*(D-1);
05149     dgetri_(&n, A, &n, &ipiv[0], &work[0], &worksize, &info);
05150     return info==0;
05151 }
05152 
05156 static inline void solvedialyticpoly12qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
05157 {
05158     const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
05159     IkReal IKFAST_ALIGNED16(M[24*24]) = {0};
05160     IkReal IKFAST_ALIGNED16(A[12*12]);
05161     IkReal IKFAST_ALIGNED16(work[24*24*23]);
05162     int ipiv[12];
05163     int info, coeffindex;
05164     const int worksize=24*24*23;
05165     const int matrixdim = 12;
05166     const int matrixdim2 = 24;
05167     numroots = 0;
05168     // first setup M = [0 I; -C -B] and A
05169     coeffindex = 0;
05170     for(int j = 0; j < 6; ++j) {
05171         for(int k = 0; k < 9; ++k) {
05172             M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -matcoeffs[coeffindex++];
05173         }
05174     }
05175     for(int j = 0; j < 6; ++j) {
05176         for(int k = 0; k < 9; ++k) {
05177             M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
05178         }
05179     }
05180     for(int j = 0; j < 6; ++j) {
05181         for(int k = 0; k < 9; ++k) {
05182             A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = matcoeffs[coeffindex++];
05183         }
05184         for(int k = 0; k < 3; ++k) {
05185             A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
05186         }
05187     }
05188     const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
05189     int lfindex = -1;
05190     bool bsingular = true;
05191     do {
05192         dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
05193         if( info == 0 ) {
05194             bsingular = false;
05195             for(int j = 0; j < matrixdim; ++j) {
05196                 if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
05197                     bsingular = true;
05198                     break;
05199                 }
05200             }
05201             if( !bsingular ) {
05202                 break;
05203             }
05204         }
05205         if( lfindex == 3 ) {
05206             break;
05207         }
05208         // transform by the linear functional
05209         lfindex++;
05210         const IkReal* lf = lfpossibilities[lfindex];
05211         // have to reinitialize A
05212         coeffindex = 0;
05213         for(int j = 0; j < 6; ++j) {
05214             for(int k = 0; k < 9; ++k) {
05215                 IkReal a = matcoeffs[coeffindex+108], b = matcoeffs[coeffindex+54], c = matcoeffs[coeffindex];
05216                 A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
05217                 M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
05218                 M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
05219                 coeffindex++;
05220             }
05221             for(int k = 0; k < 3; ++k) {
05222                 A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
05223             }
05224         }
05225     } while(lfindex<4);
05226 
05227     if( bsingular ) {
05228         return;
05229     }
05230     dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
05231     if( info != 0 ) {
05232         return;
05233     }
05234 
05235     // set identity in upper corner
05236     for(int j = 0; j < matrixdim; ++j) {
05237         M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
05238     }
05239     IkReal IKFAST_ALIGNED16(wr[24]);
05240     IkReal IKFAST_ALIGNED16(wi[24]);
05241     IkReal IKFAST_ALIGNED16(vr[24*24]);
05242     int one=1;
05243     dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
05244     if( info != 0 ) {
05245         return;
05246     }
05247     IkReal Breal[matrixdim-1];
05248     for(int i = 0; i < matrixdim2; ++i) {
05249         if( IKabs(wi[i]) < tol*100 ) {
05250             IkReal* ev = vr+matrixdim2*i;
05251             if( IKabs(wr[i]) > 1 ) {
05252                 ev += matrixdim;
05253             }
05254             // consistency has to be checked!!
05255             if( IKabs(ev[0]) < tol ) {
05256                 continue;
05257             }
05258             IkReal iconst = 1/ev[0];
05259             for(int j = 1; j < matrixdim; ++j) {
05260                 Breal[j-1] = ev[j]*iconst;
05261             }
05262             if( checkconsistency12(Breal) ) {
05263                 if( lfindex >= 0 ) {
05264                     const IkReal* lf = lfpossibilities[lfindex];
05265                     rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
05266                 }
05267                 else {
05268                     rawroots[numroots++] = wr[i];
05269                 }
05270                 bool bsmall0=IKabs(ev[0]) > IKabs(ev[3]);
05271                 bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
05272                 if( bsmall0 && bsmall1 ) {
05273                     rawroots[numroots++] = ev[3]/ev[0];
05274                     rawroots[numroots++] = ev[1]/ev[0];
05275                 }
05276                 else if( bsmall0 && !bsmall1 ) {
05277                     rawroots[numroots++] = ev[5]/ev[2];
05278                     rawroots[numroots++] = ev[2]/ev[1];
05279                 }
05280                 else if( !bsmall0 && bsmall1 ) {
05281                     rawroots[numroots++] = ev[9]/ev[6];
05282                     rawroots[numroots++] = ev[10]/ev[9];
05283                 }
05284                 else if( !bsmall0 && !bsmall1 ) {
05285                     rawroots[numroots++] = ev[11]/ev[8];
05286                     rawroots[numroots++] = ev[11]/ev[10];
05287                 }
05288             }
05289         }
05290     }
05291 }
05292 static inline bool checkconsistency12(const IkReal* Breal)
05293 {
05294     IkReal norm = 0.1;
05295     for(int i = 0; i < 11; ++i) {
05296         norm += IKabs(Breal[i]);
05297     }
05298     IkReal tol = 1e-6*norm; // have to increase the threshold since many computations are involved
05299     return IKabs(Breal[0]*Breal[0]-Breal[1]) < tol && IKabs(Breal[0]*Breal[2]-Breal[3]) < tol && IKabs(Breal[1]*Breal[2]-Breal[4]) < tol && IKabs(Breal[2]*Breal[2]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol && IKabs(Breal[1]*Breal[5]-Breal[7]) < tol && IKabs(Breal[2]*Breal[5]-Breal[8]) < tol && IKabs(Breal[0]*Breal[8]-Breal[9]) < tol && IKabs(Breal[1]*Breal[8]-Breal[10]) < tol;
05300 }
05301 };
05302 
05303 
05306 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
05307 IKSolver solver;
05308 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
05309 }
05310 
05311 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - icob (3c4ef0b5d70ca998966ba76c38edfd2e)>"; }
05312 
05313 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
05314 
05315 #ifdef IKFAST_NAMESPACE
05316 } // end namespace
05317 #endif
05318 
05319 #ifndef IKFAST_NO_MAIN
05320 #include <stdio.h>
05321 #include <stdlib.h>
05322 #ifdef IKFAST_NAMESPACE
05323 using namespace IKFAST_NAMESPACE;
05324 #endif
05325 int main(int argc, char** argv)
05326 {
05327     if( argc != 12+GetNumFreeParameters()+1 ) {
05328         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
05329                "Returns the ik solutions given the transformation of the end effector specified by\n"
05330                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
05331                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
05332         return 1;
05333     }
05334 
05335     IkSolutionList<IkReal> solutions;
05336     std::vector<IkReal> vfree(GetNumFreeParameters());
05337     IkReal eerot[9],eetrans[3];
05338     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
05339     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
05340     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
05341     for(std::size_t i = 0; i < vfree.size(); ++i)
05342         vfree[i] = atof(argv[13+i]);
05343     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
05344 
05345     if( !bSuccess ) {
05346         fprintf(stderr,"Failed to get ik solution\n");
05347         return -1;
05348     }
05349 
05350     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
05351     std::vector<IkReal> solvalues(GetNumJoints());
05352     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
05353         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
05354         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
05355         std::vector<IkReal> vsolfree(sol.GetFree().size());
05356         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
05357         for( std::size_t j = 0; j < solvalues.size(); ++j)
05358             printf("%.15f, ", solvalues[j]);
05359         printf("\n");
05360     }
05361     return 0;
05362 }
05363 
05364 #endif


cob_kinematics
Author(s): Mathias Lüdtke
autogenerated on Thu Jun 6 2019 21:22:52