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