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