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

fanuc_lrmate200id_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Fri Aug 28 2015 10:39:24