abb_irb2400_moveit_plugins: abb_irb2400_manipulator_ikfast

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=IKcos(j[3]);
00214 x2=IKsin(j[0]);
00215 x3=IKsin(j[3]);
00216 x4=IKsin(j[1]);
00217 x5=IKsin(j[2]);
00218 x6=IKcos(j[1]);
00219 x7=IKcos(j[2]);
00220 x8=IKsin(j[5]);
00221 x9=IKsin(j[4]);
00222 x10=IKcos(j[4]);
00223 x11=IKcos(j[5]);
00224 x12=((IkReal(0.0850000000000000))*(x1));
00225 x13=((IkReal(0.135000000000000))*(x7));
00226 x14=((IkReal(0.0850000000000000))*(x3));
00227 x15=((IkReal(1.00000000000000))*(x7));
00228 x16=((IkReal(1.00000000000000))*(x2));
00229 x17=((IkReal(0.0850000000000000))*(x5));
00230 x18=((IkReal(1.00000000000000))*(x9));
00231 x19=((IkReal(1.00000000000000))*(x10));
00232 x20=((IkReal(1.00000000000000))*(x0));
00233 x21=((IkReal(0.755000000000000))*(x5));
00234 x22=((IkReal(1.00000000000000))*(x5));
00235 x23=((x0)*(x4));
00236 x24=((x6)*(x7));
00237 x25=((x5)*(x6));
00238 x26=((x2)*(x4));
00239 x27=((x4)*(x5));
00240 x28=((x1)*(x9));
00241 x29=((x4)*(x7));
00242 x30=((x15)*(x6));
00243 x31=((x20)*(x27));
00244 x32=((x16)*(x27));
00245 x33=((x27)+(((IkReal(-1.00000000000000))*(x30))));
00246 x34=((x30)+(((IkReal(-1.00000000000000))*(x22)*(x4))));
00247 x35=((((x22)*(x6)))+(((x15)*(x4))));
00248 x36=((x1)*(x33));
00249 x37=((x3)*(x34));
00250 x38=((x10)*(x36));
00251 x39=((((IkReal(-1.00000000000000))*(x0)*(x30)))+(x31));
00252 x40=((((IkReal(-1.00000000000000))*(x2)*(x30)))+(x32));
00253 x41=((((x15)*(x23)))+(((x20)*(x25))));
00254 x42=((IkReal(-1.00000000000000))*(x41));
00255 x43=((((x15)*(x26)))+(((x16)*(x25))));
00256 x44=((IkReal(-1.00000000000000))*(x43));
00257 x45=((((x1)*(x42)))+(((IkReal(-1.00000000000000))*(x16)*(x3))));
00258 x46=((((x3)*(x41)))+(((IkReal(-1.00000000000000))*(x1)*(x16))));
00259 x47=((((x3)*(x43)))+(((x0)*(x1))));
00260 x48=((((x0)*(x3)))+(((x1)*(x44))));
00261 x49=((x10)*(x45));
00262 eerot[0]=((((x46)*(x8)))+(((x11)*(((x49)+(((x39)*(x9))))))));
00263 eerot[1]=((((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x39)))+(((IkReal(-1.00000000000000))*(x19)*(x45)))))))+(((x11)*(x46))));
00264 eerot[2]=((((x45)*(x9)))+(((x10)*(((((IkReal(-1.00000000000000))*(x31)))+(((x0)*(x24))))))));
00265 IkReal x50=((x0)*(x24));
00266 IkReal x51=((IkReal(1.00000000000000))*(x23));
00267 eetrans[0]=((((IkReal(0.135000000000000))*(x0)*(x25)))+(((IkReal(0.755000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x21)*(x51)))+(((x10)*(((((IkReal(0.0850000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x17)*(x51)))))))+(((x13)*(x23)))+(((IkReal(0.100000000000000))*(x0)))+(((x9)*(((((x12)*(x42)))+(((IkReal(-1.00000000000000))*(x14)*(x2)))))))+(((IkReal(0.705000000000000))*(x23))));
00268 eerot[3]=((((x47)*(x8)))+(((x11)*(((((x40)*(x9)))+(((x10)*(x48))))))));
00269 eerot[4]=((((x11)*(x47)))+(((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x40)))+(((IkReal(-1.00000000000000))*(x19)*(x48))))))));
00270 eerot[5]=((((x48)*(x9)))+(((x10)*(((((IkReal(-1.00000000000000))*(x32)))+(((x2)*(x24))))))));
00271 IkReal x52=((x2)*(x24));
00272 IkReal x53=((IkReal(1.00000000000000))*(x26));
00273 eetrans[1]=((((IkReal(0.755000000000000))*(x52)))+(((x13)*(x26)))+(((x10)*(((((IkReal(0.0850000000000000))*(x52)))+(((IkReal(-1.00000000000000))*(x17)*(x53)))))))+(((x9)*(((((x0)*(x14)))+(((x12)*(x44)))))))+(((IkReal(0.100000000000000))*(x2)))+(((IkReal(-1.00000000000000))*(x21)*(x53)))+(((IkReal(0.705000000000000))*(x26)))+(((IkReal(0.135000000000000))*(x2)*(x25))));
00274 eerot[6]=((((x11)*(((((x35)*(x9)))+(x38)))))+(((x37)*(x8))));
00275 eerot[7]=((((x11)*(x37)))+(((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x35)))+(((IkReal(-1.00000000000000))*(x19)*(x36))))))));
00276 eerot[8]=((((IkReal(-1.00000000000000))*(x10)*(x35)))+(((x28)*(x33))));
00277 IkReal x54=((IkReal(1.00000000000000))*(x6));
00278 eetrans[2]=((IkReal(0.615000000000000))+(((x10)*(((((IkReal(-0.0850000000000000))*(x29)))+(((IkReal(-1.00000000000000))*(x17)*(x54)))))))+(((x13)*(x6)))+(((x28)*(((((IkReal(-0.0850000000000000))*(x24)))+(((x17)*(x4)))))))+(((IkReal(-1.00000000000000))*(x21)*(x54)))+(((IkReal(0.705000000000000))*(x6)))+(((IkReal(-0.755000000000000))*(x29)))+(((IkReal(-0.135000000000000))*(x27))));
00279 }
00280 
00281 IKFAST_API int GetNumFreeParameters() { return 0; }
00282 IKFAST_API int* GetFreeParameters() { return NULL; }
00283 IKFAST_API int GetNumJoints() { return 6; }
00284 
00285 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00286 
00287 IKFAST_API int GetIkType() { return 0x67000001; }
00288 
00289 class IKSolver {
00290 public:
00291 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;
00292 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
00293 
00294 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00295 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; 
00296 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00297     solutions.Clear();
00298 r00 = eerot[0*3+0];
00299 r01 = eerot[0*3+1];
00300 r02 = eerot[0*3+2];
00301 r10 = eerot[1*3+0];
00302 r11 = eerot[1*3+1];
00303 r12 = eerot[1*3+2];
00304 r20 = eerot[2*3+0];
00305 r21 = eerot[2*3+1];
00306 r22 = eerot[2*3+2];
00307 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00308 
00309 new_r00=r00;
00310 new_r01=r01;
00311 new_r02=r02;
00312 new_px=((((IkReal(-0.0850000000000000))*(r02)))+(px));
00313 new_r10=r10;
00314 new_r11=r11;
00315 new_r12=r12;
00316 new_py=((((IkReal(-0.0850000000000000))*(r12)))+(py));
00317 new_r20=r20;
00318 new_r21=r21;
00319 new_r22=r22;
00320 new_pz=((IkReal(-0.615000000000000))+(((IkReal(-0.0850000000000000))*(r22)))+(pz));
00321 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;
00322 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
00323 npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00))));
00324 npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11))));
00325 npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02))));
00326 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00327 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00328 rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10))));
00329 rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21))));
00330 rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21))));
00331 rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11))));
00332 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00333 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00334 rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12))));
00335 {
00336 IkReal j0array[2], cj0array[2], sj0array[2];
00337 bool j0valid[2]={false};
00338 _nj0 = 2;
00339 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
00340     continue;
00341 IkReal x55=IKatan2(((IkReal(-1.00000000000000))*(py)), px);
00342 j0array[0]=((IkReal(-1.00000000000000))*(x55));
00343 sj0array[0]=IKsin(j0array[0]);
00344 cj0array[0]=IKcos(j0array[0]);
00345 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x55))));
00346 sj0array[1]=IKsin(j0array[1]);
00347 cj0array[1]=IKcos(j0array[1]);
00348 if( j0array[0] > IKPI )
00349 {
00350     j0array[0]-=IK2PI;
00351 }
00352 else if( j0array[0] < -IKPI )
00353 {    j0array[0]+=IK2PI;
00354 }
00355 j0valid[0] = true;
00356 if( j0array[1] > IKPI )
00357 {
00358     j0array[1]-=IK2PI;
00359 }
00360 else if( j0array[1] < -IKPI )
00361 {    j0array[1]+=IK2PI;
00362 }
00363 j0valid[1] = true;
00364 for(int ij0 = 0; ij0 < 2; ++ij0)
00365 {
00366 if( !j0valid[ij0] )
00367 {
00368     continue;
00369 }
00370 _ij0[0] = ij0; _ij0[1] = -1;
00371 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
00372 {
00373 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
00374 {
00375     j0valid[iij0]=false; _ij0[1] = iij0; break; 
00376 }
00377 }
00378 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00379 
00380 {
00381 IkReal j2array[2], cj2array[2], sj2array[2];
00382 bool j2valid[2]={false};
00383 _nj2 = 2;
00384 if( (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp))))) > 1+IKFAST_SINCOS_THRESH )
00385     continue;
00386 IkReal x56=IKasin(((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp)))));
00387 j2array[0]=((IkReal(-2.96465459743209))+(((IkReal(-1.00000000000000))*(x56))));
00388 sj2array[0]=IKsin(j2array[0]);
00389 cj2array[0]=IKcos(j2array[0]);
00390 j2array[1]=((IkReal(0.176938056157703))+(x56));
00391 sj2array[1]=IKsin(j2array[1]);
00392 cj2array[1]=IKcos(j2array[1]);
00393 if( j2array[0] > IKPI )
00394 {
00395     j2array[0]-=IK2PI;
00396 }
00397 else if( j2array[0] < -IKPI )
00398 {    j2array[0]+=IK2PI;
00399 }
00400 j2valid[0] = true;
00401 if( j2array[1] > IKPI )
00402 {
00403     j2array[1]-=IK2PI;
00404 }
00405 else if( j2array[1] < -IKPI )
00406 {    j2array[1]+=IK2PI;
00407 }
00408 j2valid[1] = true;
00409 for(int ij2 = 0; ij2 < 2; ++ij2)
00410 {
00411 if( !j2valid[ij2] )
00412 {
00413     continue;
00414 }
00415 _ij2[0] = ij2; _ij2[1] = -1;
00416 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
00417 {
00418 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
00419 {
00420     j2valid[iij2]=false; _ij2[1] = iij2; break; 
00421 }
00422 }
00423 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00424 
00425 {
00426 IkReal dummyeval[1];
00427 IkReal gconst1;
00428 IkReal x57=((IkReal(0.755000000000000))*(cj2));
00429 IkReal x58=((py)*(sj0));
00430 IkReal x59=((cj0)*(px));
00431 IkReal x60=((IkReal(0.135000000000000))*(sj2));
00432 gconst1=IKsign(((((IkReal(0.705000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x58)*(x60)))+(((IkReal(0.0755000000000000))*(cj2)))+(((IkReal(-0.755000000000000))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x57)*(x58)))+(((IkReal(-1.00000000000000))*(x59)*(x60)))+(((IkReal(-1.00000000000000))*(x57)*(x59)))+(((IkReal(0.0135000000000000))*(sj2)))+(((IkReal(0.135000000000000))*(cj2)*(pz)))));
00433 IkReal x61=((IkReal(10.0000000000000))*(sj2));
00434 IkReal x62=((cj0)*(px));
00435 IkReal x63=((py)*(sj0));
00436 IkReal x64=((IkReal(55.9259259259259))*(cj2));
00437 dummyeval[0]=((((IkReal(52.2222222222222))*(pz)))+(((IkReal(-1.00000000000000))*(x62)*(x64)))+(sj2)+(((IkReal(10.0000000000000))*(cj2)*(pz)))+(((IkReal(-1.00000000000000))*(x63)*(x64)))+(((IkReal(-1.00000000000000))*(x61)*(x62)))+(((IkReal(-1.00000000000000))*(x61)*(x63)))+(((IkReal(5.59259259259259))*(cj2)))+(((IkReal(-55.9259259259259))*(pz)*(sj2))));
00438 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00439 {
00440 {
00441 IkReal dummyeval[1];
00442 IkReal gconst0;
00443 IkReal x65=((IkReal(0.755000000000000))*(sj2));
00444 IkReal x66=((cj0)*(px));
00445 IkReal x67=((py)*(sj0));
00446 IkReal x68=((IkReal(0.135000000000000))*(cj2));
00447 gconst0=IKsign(((IkReal(0.0705000000000000))+(((x65)*(x66)))+(((IkReal(-0.705000000000000))*(x67)))+(((x65)*(x67)))+(((IkReal(-0.755000000000000))*(cj2)*(pz)))+(((IkReal(-0.705000000000000))*(x66)))+(((IkReal(-1.00000000000000))*(x66)*(x68)))+(((IkReal(-1.00000000000000))*(x67)*(x68)))+(((IkReal(0.0135000000000000))*(cj2)))+(((IkReal(-0.135000000000000))*(pz)*(sj2)))+(((IkReal(-0.0755000000000000))*(sj2)))));
00448 IkReal x69=((cj0)*(px));
00449 IkReal x70=((IkReal(10.0000000000000))*(cj2));
00450 IkReal x71=((IkReal(55.9259259259259))*(sj2));
00451 IkReal x72=((py)*(sj0));
00452 dummyeval[0]=((IkReal(5.22222222222222))+(((IkReal(-1.00000000000000))*(x70)*(x72)))+(((x71)*(x72)))+(((IkReal(-52.2222222222222))*(x72)))+(((IkReal(-10.0000000000000))*(pz)*(sj2)))+(cj2)+(((IkReal(-52.2222222222222))*(x69)))+(((IkReal(-1.00000000000000))*(x69)*(x70)))+(((IkReal(-5.59259259259259))*(sj2)))+(((IkReal(-55.9259259259259))*(cj2)*(pz)))+(((x69)*(x71))));
00453 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00454 {
00455 continue;
00456 
00457 } else
00458 {
00459 {
00460 IkReal j1array[1], cj1array[1], sj1array[1];
00461 bool j1valid[1]={false};
00462 _nj1 = 1;
00463 IkReal x73=(sj2)*(sj2);
00464 IkReal x74=(cj2)*(cj2);
00465 IkReal x75=((cj2)*(sj2));
00466 IkReal x76=((IkReal(1.00000000000000))*(pz));
00467 if( IKabs(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(-0.190350000000000))*(cj2)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(0.203850000000000))*(x75)))+(((IkReal(-0.570025000000000))*(x73)))+(((IkReal(-0.0182250000000000))*(x74))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(0.101925000000000))*(x73)))+(((IkReal(-0.101925000000000))*(x74)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(0.551800000000000))*(x75)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x76)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x76))))))) < IKFAST_ATAN2_MAGTHRESH )
00468     continue;
00469 j1array[0]=IKatan2(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(-0.190350000000000))*(cj2)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(0.203850000000000))*(x75)))+(((IkReal(-0.570025000000000))*(x73)))+(((IkReal(-0.0182250000000000))*(x74)))))), ((gconst0)*(((((IkReal(0.101925000000000))*(x73)))+(((IkReal(-0.101925000000000))*(x74)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(0.551800000000000))*(x75)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x76)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x76)))))));
00470 sj1array[0]=IKsin(j1array[0]);
00471 cj1array[0]=IKcos(j1array[0]);
00472 if( j1array[0] > IKPI )
00473 {
00474     j1array[0]-=IK2PI;
00475 }
00476 else if( j1array[0] < -IKPI )
00477 {    j1array[0]+=IK2PI;
00478 }
00479 j1valid[0] = true;
00480 for(int ij1 = 0; ij1 < 1; ++ij1)
00481 {
00482 if( !j1valid[ij1] )
00483 {
00484     continue;
00485 }
00486 _ij1[0] = ij1; _ij1[1] = -1;
00487 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00488 {
00489 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00490 {
00491     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00492 }
00493 }
00494 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00495 {
00496 IkReal evalcond[5];
00497 IkReal x77=IKsin(j1);
00498 IkReal x78=IKcos(j1);
00499 IkReal x79=((IkReal(0.135000000000000))*(sj2));
00500 IkReal x80=((cj0)*(px));
00501 IkReal x81=((IkReal(0.755000000000000))*(sj2));
00502 IkReal x82=((py)*(sj0));
00503 IkReal x83=((IkReal(0.755000000000000))*(cj2));
00504 IkReal x84=((IkReal(0.135000000000000))*(cj2));
00505 IkReal x85=((IkReal(0.135000000000000))*(x78));
00506 IkReal x86=((IkReal(1.00000000000000))*(x78));
00507 IkReal x87=((IkReal(1.41000000000000))*(x77));
00508 IkReal x88=((pz)*(x78));
00509 evalcond[0]=((IkReal(-0.705000000000000))+(((x77)*(x80)))+(((x77)*(x82)))+(((IkReal(-0.100000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(x84)))+(x88)+(x81));
00510 evalcond[1]=((((IkReal(0.100000000000000))*(x78)))+(((IkReal(-1.00000000000000))*(x80)*(x86)))+(x79)+(((IkReal(-1.00000000000000))*(x82)*(x86)))+(x83)+(((pz)*(x77))));
00511 evalcond[2]=((((IkReal(-0.705000000000000))*(x78)))+(((IkReal(-1.00000000000000))*(x78)*(x84)))+(((x77)*(x79)))+(pz)+(((x78)*(x81)))+(((x77)*(x83))));
00512 evalcond[3]=((IkReal(0.0812250000000000))+(((IkReal(-0.141000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(pp)))+(((x82)*(x87)))+(((IkReal(0.200000000000000))*(x82)))+(((IkReal(1.41000000000000))*(x88)))+(((x80)*(x87)))+(((IkReal(0.200000000000000))*(x80))));
00513 evalcond[4]=((IkReal(0.100000000000000))+(((IkReal(-1.00000000000000))*(x82)))+(((x77)*(x84)))+(((IkReal(-1.00000000000000))*(x77)*(x81)))+(((x78)*(x83)))+(((x78)*(x79)))+(((IkReal(0.705000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(x80))));
00514 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  )
00515 {
00516 continue;
00517 }
00518 }
00519 
00520 rotationfunction0(solutions);
00521 }
00522 }
00523 
00524 }
00525 
00526 }
00527 
00528 } else
00529 {
00530 {
00531 IkReal j1array[1], cj1array[1], sj1array[1];
00532 bool j1valid[1]={false};
00533 _nj1 = 1;
00534 IkReal x228=(sj2)*(sj2);
00535 IkReal x229=(cj2)*(cj2);
00536 IkReal x230=((cj2)*(sj2));
00537 if( IKabs(((gconst1)*(((((IkReal(0.551800000000000))*(x230)))+(((IkReal(0.101925000000000))*(x228)))+(((IkReal(-0.100000000000000))*(pz)))+(((IkReal(-0.101925000000000))*(x229)))+(((cj0)*(px)*(pz)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((py)*(pz)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-0.0182250000000000))*(x228)))+((pz)*(pz))+(((IkReal(-0.203850000000000))*(x230)))+(((IkReal(-0.570025000000000))*(x229))))))) < IKFAST_ATAN2_MAGTHRESH )
00538     continue;
00539 j1array[0]=IKatan2(((gconst1)*(((((IkReal(0.551800000000000))*(x230)))+(((IkReal(0.101925000000000))*(x228)))+(((IkReal(-0.100000000000000))*(pz)))+(((IkReal(-0.101925000000000))*(x229)))+(((cj0)*(px)*(pz)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((py)*(pz)*(sj0)))))), ((gconst1)*(((((IkReal(-0.0182250000000000))*(x228)))+((pz)*(pz))+(((IkReal(-0.203850000000000))*(x230)))+(((IkReal(-0.570025000000000))*(x229)))))));
00540 sj1array[0]=IKsin(j1array[0]);
00541 cj1array[0]=IKcos(j1array[0]);
00542 if( j1array[0] > IKPI )
00543 {
00544     j1array[0]-=IK2PI;
00545 }
00546 else if( j1array[0] < -IKPI )
00547 {    j1array[0]+=IK2PI;
00548 }
00549 j1valid[0] = true;
00550 for(int ij1 = 0; ij1 < 1; ++ij1)
00551 {
00552 if( !j1valid[ij1] )
00553 {
00554     continue;
00555 }
00556 _ij1[0] = ij1; _ij1[1] = -1;
00557 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
00558 {
00559 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
00560 {
00561     j1valid[iij1]=false; _ij1[1] = iij1; break; 
00562 }
00563 }
00564 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00565 {
00566 IkReal evalcond[5];
00567 IkReal x231=IKsin(j1);
00568 IkReal x232=IKcos(j1);
00569 IkReal x233=((IkReal(0.135000000000000))*(sj2));
00570 IkReal x234=((cj0)*(px));
00571 IkReal x235=((IkReal(0.755000000000000))*(sj2));
00572 IkReal x236=((py)*(sj0));
00573 IkReal x237=((IkReal(0.755000000000000))*(cj2));
00574 IkReal x238=((IkReal(0.135000000000000))*(cj2));
00575 IkReal x239=((IkReal(0.135000000000000))*(x232));
00576 IkReal x240=((IkReal(1.00000000000000))*(x232));
00577 IkReal x241=((IkReal(1.41000000000000))*(x231));
00578 IkReal x242=((pz)*(x232));
00579 evalcond[0]=((IkReal(-0.705000000000000))+(((IkReal(-1.00000000000000))*(x238)))+(x235)+(((x231)*(x236)))+(x242)+(((IkReal(-0.100000000000000))*(x231)))+(((x231)*(x234))));
00580 evalcond[1]=((((IkReal(0.100000000000000))*(x232)))+(((pz)*(x231)))+(x233)+(x237)+(((IkReal(-1.00000000000000))*(x234)*(x240)))+(((IkReal(-1.00000000000000))*(x236)*(x240))));
00581 evalcond[2]=((((IkReal(-0.705000000000000))*(x232)))+(((x231)*(x237)))+(((x231)*(x233)))+(pz)+(((x232)*(x235)))+(((IkReal(-1.00000000000000))*(x232)*(x238))));
00582 evalcond[3]=((IkReal(0.0812250000000000))+(((x236)*(x241)))+(((IkReal(0.200000000000000))*(x236)))+(((IkReal(0.200000000000000))*(x234)))+(((IkReal(-1.00000000000000))*(pp)))+(((x234)*(x241)))+(((IkReal(1.41000000000000))*(x242)))+(((IkReal(-0.141000000000000))*(x231))));
00583 evalcond[4]=((IkReal(0.100000000000000))+(((IkReal(-1.00000000000000))*(x231)*(x235)))+(((IkReal(0.705000000000000))*(x231)))+(((IkReal(-1.00000000000000))*(x236)))+(((x231)*(x238)))+(((x232)*(x233)))+(((x232)*(x237)))+(((IkReal(-1.00000000000000))*(x234))));
00584 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  )
00585 {
00586 continue;
00587 }
00588 }
00589 
00590 rotationfunction0(solutions);
00591 }
00592 }
00593 
00594 }
00595 
00596 }
00597 }
00598 }
00599 }
00600 }
00601 }
00602 return solutions.GetNumSolutions()>0;
00603 }
00604 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
00605 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00606 IkReal x89=((cj0)*(r00));
00607 IkReal x90=((cj0)*(r01));
00608 IkReal x91=((sj1)*(sj2));
00609 IkReal x92=((IkReal(1.00000000000000))*(sj0));
00610 IkReal x93=((r10)*(sj0));
00611 IkReal x94=((IkReal(1.00000000000000))*(cj2));
00612 IkReal x95=((r12)*(sj0));
00613 IkReal x96=((cj0)*(r02));
00614 IkReal x97=((r11)*(sj0));
00615 IkReal x98=((((IkReal(-1.00000000000000))*(cj1)*(x94)))+(x91));
00616 IkReal x99=((((IkReal(-1.00000000000000))*(x91)))+(((cj1)*(cj2))));
00617 IkReal x100=((cj0)*(x99));
00618 IkReal x101=((((IkReal(-1.00000000000000))*(sj1)*(x94)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2))));
00619 IkReal x102=((sj0)*(x101));
00620 new_r00=((((r20)*(x98)))+(((x101)*(x89)))+(((x101)*(x93))));
00621 new_r01=((((x101)*(x90)))+(((x101)*(x97)))+(((r21)*(x98))));
00622 new_r02=((((x101)*(x95)))+(((r22)*(x98)))+(((x101)*(x96))));
00623 new_r10=((((IkReal(-1.00000000000000))*(r00)*(x92)))+(((cj0)*(r10))));
00624 new_r11=((((IkReal(-1.00000000000000))*(r01)*(x92)))+(((cj0)*(r11))));
00625 new_r12=((((IkReal(-1.00000000000000))*(r02)*(x92)))+(((cj0)*(r12))));
00626 new_r20=((((x93)*(x99)))+(((x89)*(x99)))+(((r20)*(x101))));
00627 new_r21=((((r21)*(x101)))+(((x97)*(x99)))+(((x90)*(x99))));
00628 new_r22=((((x96)*(x99)))+(((r22)*(x101)))+(((x95)*(x99))));
00629 {
00630 IkReal j4array[2], cj4array[2], sj4array[2];
00631 bool j4valid[2]={false};
00632 _nj4 = 2;
00633 cj4array[0]=new_r22;
00634 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
00635 {
00636     j4valid[0] = j4valid[1] = true;
00637     j4array[0] = IKacos(cj4array[0]);
00638     sj4array[0] = IKsin(j4array[0]);
00639     cj4array[1] = cj4array[0];
00640     j4array[1] = -j4array[0];
00641     sj4array[1] = -sj4array[0];
00642 }
00643 else if( isnan(cj4array[0]) )
00644 {
00645     // probably any value will work
00646     j4valid[0] = true;
00647     cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
00648 }
00649 for(int ij4 = 0; ij4 < 2; ++ij4)
00650 {
00651 if( !j4valid[ij4] )
00652 {
00653     continue;
00654 }
00655 _ij4[0] = ij4; _ij4[1] = -1;
00656 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
00657 {
00658 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00659 {
00660     j4valid[iij4]=false; _ij4[1] = iij4; break; 
00661 }
00662 }
00663 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00664 
00665 {
00666 IkReal dummyeval[1];
00667 IkReal gconst4;
00668 gconst4=IKsign(sj4);
00669 dummyeval[0]=sj4;
00670 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00671 {
00672 {
00673 IkReal dummyeval[1];
00674 IkReal gconst2;
00675 gconst2=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00676 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00677 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00678 {
00679 {
00680 IkReal dummyeval[1];
00681 IkReal gconst3;
00682 gconst3=IKsign(((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4)))));
00683 dummyeval[0]=((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4))));
00684 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00685 {
00686 {
00687 IkReal evalcond[7];
00688 IkReal x103=((IkReal(-1.00000000000000))+(new_r22));
00689 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
00690 evalcond[1]=x103;
00691 evalcond[2]=new_r20;
00692 evalcond[3]=new_r21;
00693 evalcond[4]=new_r20;
00694 evalcond[5]=new_r21;
00695 evalcond[6]=x103;
00696 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  )
00697 {
00698 {
00699 IkReal j3array[2], cj3array[2], sj3array[2];
00700 bool j3valid[2]={false};
00701 _nj3 = 2;
00702 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
00703     continue;
00704 IkReal x104=IKatan2(new_r02, new_r12);
00705 j3array[0]=((IkReal(-1.00000000000000))*(x104));
00706 sj3array[0]=IKsin(j3array[0]);
00707 cj3array[0]=IKcos(j3array[0]);
00708 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x104))));
00709 sj3array[1]=IKsin(j3array[1]);
00710 cj3array[1]=IKcos(j3array[1]);
00711 if( j3array[0] > IKPI )
00712 {
00713     j3array[0]-=IK2PI;
00714 }
00715 else if( j3array[0] < -IKPI )
00716 {    j3array[0]+=IK2PI;
00717 }
00718 j3valid[0] = true;
00719 if( j3array[1] > IKPI )
00720 {
00721     j3array[1]-=IK2PI;
00722 }
00723 else if( j3array[1] < -IKPI )
00724 {    j3array[1]+=IK2PI;
00725 }
00726 j3valid[1] = true;
00727 for(int ij3 = 0; ij3 < 2; ++ij3)
00728 {
00729 if( !j3valid[ij3] )
00730 {
00731     continue;
00732 }
00733 _ij3[0] = ij3; _ij3[1] = -1;
00734 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00735 {
00736 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00737 {
00738     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00739 }
00740 }
00741 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00742 {
00743 IkReal evalcond[1];
00744 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
00745 if( IKabs(evalcond[0]) > 0.000001  )
00746 {
00747 continue;
00748 }
00749 }
00750 
00751 {
00752 IkReal j5array[1], cj5array[1], sj5array[1];
00753 bool j5valid[1]={false};
00754 _nj5 = 1;
00755 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
00756     continue;
00757 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
00758 sj5array[0]=IKsin(j5array[0]);
00759 cj5array[0]=IKcos(j5array[0]);
00760 if( j5array[0] > IKPI )
00761 {
00762     j5array[0]-=IK2PI;
00763 }
00764 else if( j5array[0] < -IKPI )
00765 {    j5array[0]+=IK2PI;
00766 }
00767 j5valid[0] = true;
00768 for(int ij5 = 0; ij5 < 1; ++ij5)
00769 {
00770 if( !j5valid[ij5] )
00771 {
00772     continue;
00773 }
00774 _ij5[0] = ij5; _ij5[1] = -1;
00775 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00776 {
00777 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00778 {
00779     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00780 }
00781 }
00782 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00783 {
00784 IkReal evalcond[4];
00785 IkReal x105=IKsin(j5);
00786 IkReal x106=((IkReal(1.00000000000000))*(sj3));
00787 IkReal x107=((IkReal(1.00000000000000))*(IKcos(j5)));
00788 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x106)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x105))));
00789 evalcond[1]=((((IkReal(-1.00000000000000))*(x107)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x106))));
00790 evalcond[2]=((((new_r11)*(sj3)))+(x105)+(((cj3)*(new_r01))));
00791 evalcond[3]=((((IkReal(-1.00000000000000))*(x107)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
00792 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00793 {
00794 continue;
00795 }
00796 }
00797 
00798 {
00799 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00800 vinfos[0].jointtype = 1;
00801 vinfos[0].foffset = j0;
00802 vinfos[0].indices[0] = _ij0[0];
00803 vinfos[0].indices[1] = _ij0[1];
00804 vinfos[0].maxsolutions = _nj0;
00805 vinfos[1].jointtype = 1;
00806 vinfos[1].foffset = j1;
00807 vinfos[1].indices[0] = _ij1[0];
00808 vinfos[1].indices[1] = _ij1[1];
00809 vinfos[1].maxsolutions = _nj1;
00810 vinfos[2].jointtype = 1;
00811 vinfos[2].foffset = j2;
00812 vinfos[2].indices[0] = _ij2[0];
00813 vinfos[2].indices[1] = _ij2[1];
00814 vinfos[2].maxsolutions = _nj2;
00815 vinfos[3].jointtype = 1;
00816 vinfos[3].foffset = j3;
00817 vinfos[3].indices[0] = _ij3[0];
00818 vinfos[3].indices[1] = _ij3[1];
00819 vinfos[3].maxsolutions = _nj3;
00820 vinfos[4].jointtype = 1;
00821 vinfos[4].foffset = j4;
00822 vinfos[4].indices[0] = _ij4[0];
00823 vinfos[4].indices[1] = _ij4[1];
00824 vinfos[4].maxsolutions = _nj4;
00825 vinfos[5].jointtype = 1;
00826 vinfos[5].foffset = j5;
00827 vinfos[5].indices[0] = _ij5[0];
00828 vinfos[5].indices[1] = _ij5[1];
00829 vinfos[5].maxsolutions = _nj5;
00830 std::vector<int> vfree(0);
00831 solutions.AddSolution(vinfos,vfree);
00832 }
00833 }
00834 }
00835 }
00836 }
00837 
00838 } else
00839 {
00840 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
00841 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
00842 evalcond[2]=new_r20;
00843 evalcond[3]=new_r21;
00844 evalcond[4]=((IkReal(-1.00000000000000))*(new_r20));
00845 evalcond[5]=((IkReal(-1.00000000000000))*(new_r21));
00846 evalcond[6]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
00847 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  )
00848 {
00849 {
00850 IkReal j3array[2], cj3array[2], sj3array[2];
00851 bool j3valid[2]={false};
00852 _nj3 = 2;
00853 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
00854     continue;
00855 IkReal x108=IKatan2(new_r02, new_r12);
00856 j3array[0]=((IkReal(-1.00000000000000))*(x108));
00857 sj3array[0]=IKsin(j3array[0]);
00858 cj3array[0]=IKcos(j3array[0]);
00859 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x108))));
00860 sj3array[1]=IKsin(j3array[1]);
00861 cj3array[1]=IKcos(j3array[1]);
00862 if( j3array[0] > IKPI )
00863 {
00864     j3array[0]-=IK2PI;
00865 }
00866 else if( j3array[0] < -IKPI )
00867 {    j3array[0]+=IK2PI;
00868 }
00869 j3valid[0] = true;
00870 if( j3array[1] > IKPI )
00871 {
00872     j3array[1]-=IK2PI;
00873 }
00874 else if( j3array[1] < -IKPI )
00875 {    j3array[1]+=IK2PI;
00876 }
00877 j3valid[1] = true;
00878 for(int ij3 = 0; ij3 < 2; ++ij3)
00879 {
00880 if( !j3valid[ij3] )
00881 {
00882     continue;
00883 }
00884 _ij3[0] = ij3; _ij3[1] = -1;
00885 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
00886 {
00887 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00888 {
00889     j3valid[iij3]=false; _ij3[1] = iij3; break; 
00890 }
00891 }
00892 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00893 {
00894 IkReal evalcond[1];
00895 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
00896 if( IKabs(evalcond[0]) > 0.000001  )
00897 {
00898 continue;
00899 }
00900 }
00901 
00902 {
00903 IkReal j5array[1], cj5array[1], sj5array[1];
00904 bool j5valid[1]={false};
00905 _nj5 = 1;
00906 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
00907     continue;
00908 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
00909 sj5array[0]=IKsin(j5array[0]);
00910 cj5array[0]=IKcos(j5array[0]);
00911 if( j5array[0] > IKPI )
00912 {
00913     j5array[0]-=IK2PI;
00914 }
00915 else if( j5array[0] < -IKPI )
00916 {    j5array[0]+=IK2PI;
00917 }
00918 j5valid[0] = true;
00919 for(int ij5 = 0; ij5 < 1; ++ij5)
00920 {
00921 if( !j5valid[ij5] )
00922 {
00923     continue;
00924 }
00925 _ij5[0] = ij5; _ij5[1] = -1;
00926 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00927 {
00928 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00929 {
00930     j5valid[iij5]=false; _ij5[1] = iij5; break; 
00931 }
00932 }
00933 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00934 {
00935 IkReal evalcond[4];
00936 IkReal x109=IKcos(j5);
00937 IkReal x110=((IkReal(1.00000000000000))*(sj3));
00938 IkReal x111=((IkReal(1.00000000000000))*(IKsin(j5)));
00939 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x110)))+(((IkReal(-1.00000000000000))*(x111)))+(((cj3)*(new_r10))));
00940 evalcond[1]=((((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(new_r01)*(x110)))+(((cj3)*(new_r11))));
00941 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x111)))+(((cj3)*(new_r01))));
00942 evalcond[3]=((((new_r10)*(sj3)))+(x109)+(((cj3)*(new_r00))));
00943 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
00944 {
00945 continue;
00946 }
00947 }
00948 
00949 {
00950 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00951 vinfos[0].jointtype = 1;
00952 vinfos[0].foffset = j0;
00953 vinfos[0].indices[0] = _ij0[0];
00954 vinfos[0].indices[1] = _ij0[1];
00955 vinfos[0].maxsolutions = _nj0;
00956 vinfos[1].jointtype = 1;
00957 vinfos[1].foffset = j1;
00958 vinfos[1].indices[0] = _ij1[0];
00959 vinfos[1].indices[1] = _ij1[1];
00960 vinfos[1].maxsolutions = _nj1;
00961 vinfos[2].jointtype = 1;
00962 vinfos[2].foffset = j2;
00963 vinfos[2].indices[0] = _ij2[0];
00964 vinfos[2].indices[1] = _ij2[1];
00965 vinfos[2].maxsolutions = _nj2;
00966 vinfos[3].jointtype = 1;
00967 vinfos[3].foffset = j3;
00968 vinfos[3].indices[0] = _ij3[0];
00969 vinfos[3].indices[1] = _ij3[1];
00970 vinfos[3].maxsolutions = _nj3;
00971 vinfos[4].jointtype = 1;
00972 vinfos[4].foffset = j4;
00973 vinfos[4].indices[0] = _ij4[0];
00974 vinfos[4].indices[1] = _ij4[1];
00975 vinfos[4].maxsolutions = _nj4;
00976 vinfos[5].jointtype = 1;
00977 vinfos[5].foffset = j5;
00978 vinfos[5].indices[0] = _ij5[0];
00979 vinfos[5].indices[1] = _ij5[1];
00980 vinfos[5].maxsolutions = _nj5;
00981 std::vector<int> vfree(0);
00982 solutions.AddSolution(vinfos,vfree);
00983 }
00984 }
00985 }
00986 }
00987 }
00988 
00989 } else
00990 {
00991 if( 1 )
00992 {
00993 continue;
00994 
00995 } else
00996 {
00997 }
00998 }
00999 }
01000 }
01001 
01002 } else
01003 {
01004 {
01005 IkReal j3array[1], cj3array[1], sj3array[1];
01006 bool j3valid[1]={false};
01007 _nj3 = 1;
01008 IkReal x112=((IkReal(-1.00000000000000))*(cj4)*(gconst3)*(new_r20));
01009 if( IKabs(((new_r12)*(x112))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x112))) < IKFAST_ATAN2_MAGTHRESH )
01010     continue;
01011 j3array[0]=IKatan2(((new_r12)*(x112)), ((new_r02)*(x112)));
01012 sj3array[0]=IKsin(j3array[0]);
01013 cj3array[0]=IKcos(j3array[0]);
01014 if( j3array[0] > IKPI )
01015 {
01016     j3array[0]-=IK2PI;
01017 }
01018 else if( j3array[0] < -IKPI )
01019 {    j3array[0]+=IK2PI;
01020 }
01021 j3valid[0] = true;
01022 for(int ij3 = 0; ij3 < 1; ++ij3)
01023 {
01024 if( !j3valid[ij3] )
01025 {
01026     continue;
01027 }
01028 _ij3[0] = ij3; _ij3[1] = -1;
01029 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01030 {
01031 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01032 {
01033     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01034 }
01035 }
01036 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01037 {
01038 IkReal evalcond[6];
01039 IkReal x113=IKsin(j3);
01040 IkReal x114=IKcos(j3);
01041 IkReal x115=((IkReal(1.00000000000000))*(sj4));
01042 IkReal x116=((sj4)*(x113));
01043 IkReal x117=((sj4)*(x114));
01044 IkReal x118=((new_r02)*(x114));
01045 IkReal x119=((new_r12)*(x113));
01046 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x113)))+(((new_r12)*(x114))));
01047 evalcond[1]=((((IkReal(-1.00000000000000))*(x115)))+(x119)+(x118));
01048 evalcond[2]=((((new_r00)*(x117)))+(((cj4)*(new_r20)))+(((new_r10)*(x116))));
01049 evalcond[3]=((((new_r11)*(x116)))+(((new_r01)*(x117)))+(((cj4)*(new_r21))));
01050 evalcond[4]=((IkReal(-1.00000000000000))+(((new_r02)*(x117)))+(((new_r12)*(x116)))+(((cj4)*(new_r22))));
01051 evalcond[5]=((((cj4)*(x118)))+(((cj4)*(x119)))+(((IkReal(-1.00000000000000))*(new_r22)*(x115))));
01052 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  )
01053 {
01054 continue;
01055 }
01056 }
01057 
01058 {
01059 IkReal dummyeval[1];
01060 IkReal gconst5;
01061 gconst5=IKsign(sj4);
01062 dummyeval[0]=sj4;
01063 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01064 {
01065 {
01066 IkReal dummyeval[1];
01067 dummyeval[0]=sj4;
01068 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01069 {
01070 {
01071 IkReal dummyeval[1];
01072 dummyeval[0]=sj4;
01073 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01074 {
01075 {
01076 IkReal evalcond[11];
01077 IkReal x120=((IkReal(-1.00000000000000))+(new_r22));
01078 IkReal x121=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3))));
01079 IkReal x122=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
01080 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01081 evalcond[1]=x120;
01082 evalcond[2]=new_r20;
01083 evalcond[3]=new_r21;
01084 evalcond[4]=x121;
01085 evalcond[5]=x121;
01086 evalcond[6]=x122;
01087 evalcond[7]=new_r20;
01088 evalcond[8]=new_r21;
01089 evalcond[9]=x120;
01090 evalcond[10]=x122;
01091 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  )
01092 {
01093 {
01094 IkReal j5array[1], cj5array[1], sj5array[1];
01095 bool j5valid[1]={false};
01096 _nj5 = 1;
01097 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01098     continue;
01099 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01100 sj5array[0]=IKsin(j5array[0]);
01101 cj5array[0]=IKcos(j5array[0]);
01102 if( j5array[0] > IKPI )
01103 {
01104     j5array[0]-=IK2PI;
01105 }
01106 else if( j5array[0] < -IKPI )
01107 {    j5array[0]+=IK2PI;
01108 }
01109 j5valid[0] = true;
01110 for(int ij5 = 0; ij5 < 1; ++ij5)
01111 {
01112 if( !j5valid[ij5] )
01113 {
01114     continue;
01115 }
01116 _ij5[0] = ij5; _ij5[1] = -1;
01117 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01118 {
01119 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01120 {
01121     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01122 }
01123 }
01124 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01125 {
01126 IkReal evalcond[4];
01127 IkReal x123=IKsin(j5);
01128 IkReal x124=((IkReal(1.00000000000000))*(sj3));
01129 IkReal x125=((IkReal(1.00000000000000))*(IKcos(j5)));
01130 evalcond[0]=((((IkReal(-1.00000000000000))*(x123)))+(((IkReal(-1.00000000000000))*(new_r00)*(x124)))+(((cj3)*(new_r10))));
01131 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x124)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x125))));
01132 evalcond[2]=((((new_r11)*(sj3)))+(x123)+(((cj3)*(new_r01))));
01133 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x125)))+(((cj3)*(new_r00))));
01134 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01135 {
01136 continue;
01137 }
01138 }
01139 
01140 {
01141 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01142 vinfos[0].jointtype = 1;
01143 vinfos[0].foffset = j0;
01144 vinfos[0].indices[0] = _ij0[0];
01145 vinfos[0].indices[1] = _ij0[1];
01146 vinfos[0].maxsolutions = _nj0;
01147 vinfos[1].jointtype = 1;
01148 vinfos[1].foffset = j1;
01149 vinfos[1].indices[0] = _ij1[0];
01150 vinfos[1].indices[1] = _ij1[1];
01151 vinfos[1].maxsolutions = _nj1;
01152 vinfos[2].jointtype = 1;
01153 vinfos[2].foffset = j2;
01154 vinfos[2].indices[0] = _ij2[0];
01155 vinfos[2].indices[1] = _ij2[1];
01156 vinfos[2].maxsolutions = _nj2;
01157 vinfos[3].jointtype = 1;
01158 vinfos[3].foffset = j3;
01159 vinfos[3].indices[0] = _ij3[0];
01160 vinfos[3].indices[1] = _ij3[1];
01161 vinfos[3].maxsolutions = _nj3;
01162 vinfos[4].jointtype = 1;
01163 vinfos[4].foffset = j4;
01164 vinfos[4].indices[0] = _ij4[0];
01165 vinfos[4].indices[1] = _ij4[1];
01166 vinfos[4].maxsolutions = _nj4;
01167 vinfos[5].jointtype = 1;
01168 vinfos[5].foffset = j5;
01169 vinfos[5].indices[0] = _ij5[0];
01170 vinfos[5].indices[1] = _ij5[1];
01171 vinfos[5].maxsolutions = _nj5;
01172 std::vector<int> vfree(0);
01173 solutions.AddSolution(vinfos,vfree);
01174 }
01175 }
01176 }
01177 
01178 } else
01179 {
01180 IkReal x126=((new_r12)*(sj3));
01181 IkReal x127=((IkReal(1.00000000000000))*(new_r02));
01182 IkReal x128=((((IkReal(-1.00000000000000))*(sj3)*(x127)))+(((cj3)*(new_r12))));
01183 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
01184 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
01185 evalcond[2]=new_r20;
01186 evalcond[3]=new_r21;
01187 evalcond[4]=x128;
01188 evalcond[5]=x128;
01189 evalcond[6]=((x126)+(((cj3)*(new_r02))));
01190 evalcond[7]=((IkReal(-1.00000000000000))*(new_r20));
01191 evalcond[8]=((IkReal(-1.00000000000000))*(new_r21));
01192 evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01193 evalcond[10]=((((IkReal(-1.00000000000000))*(cj3)*(x127)))+(((IkReal(-1.00000000000000))*(x126))));
01194 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  )
01195 {
01196 {
01197 IkReal j5array[1], cj5array[1], sj5array[1];
01198 bool j5valid[1]={false};
01199 _nj5 = 1;
01200 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
01201     continue;
01202 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01203 sj5array[0]=IKsin(j5array[0]);
01204 cj5array[0]=IKcos(j5array[0]);
01205 if( j5array[0] > IKPI )
01206 {
01207     j5array[0]-=IK2PI;
01208 }
01209 else if( j5array[0] < -IKPI )
01210 {    j5array[0]+=IK2PI;
01211 }
01212 j5valid[0] = true;
01213 for(int ij5 = 0; ij5 < 1; ++ij5)
01214 {
01215 if( !j5valid[ij5] )
01216 {
01217     continue;
01218 }
01219 _ij5[0] = ij5; _ij5[1] = -1;
01220 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01221 {
01222 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01223 {
01224     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01225 }
01226 }
01227 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01228 {
01229 IkReal evalcond[4];
01230 IkReal x129=IKcos(j5);
01231 IkReal x130=((IkReal(1.00000000000000))*(sj3));
01232 IkReal x131=((IkReal(1.00000000000000))*(IKsin(j5)));
01233 evalcond[0]=((((IkReal(-1.00000000000000))*(x131)))+(((IkReal(-1.00000000000000))*(new_r00)*(x130)))+(((cj3)*(new_r10))));
01234 evalcond[1]=((((IkReal(-1.00000000000000))*(x129)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x130))));
01235 evalcond[2]=((((IkReal(-1.00000000000000))*(x131)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
01236 evalcond[3]=((x129)+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
01237 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01238 {
01239 continue;
01240 }
01241 }
01242 
01243 {
01244 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01245 vinfos[0].jointtype = 1;
01246 vinfos[0].foffset = j0;
01247 vinfos[0].indices[0] = _ij0[0];
01248 vinfos[0].indices[1] = _ij0[1];
01249 vinfos[0].maxsolutions = _nj0;
01250 vinfos[1].jointtype = 1;
01251 vinfos[1].foffset = j1;
01252 vinfos[1].indices[0] = _ij1[0];
01253 vinfos[1].indices[1] = _ij1[1];
01254 vinfos[1].maxsolutions = _nj1;
01255 vinfos[2].jointtype = 1;
01256 vinfos[2].foffset = j2;
01257 vinfos[2].indices[0] = _ij2[0];
01258 vinfos[2].indices[1] = _ij2[1];
01259 vinfos[2].maxsolutions = _nj2;
01260 vinfos[3].jointtype = 1;
01261 vinfos[3].foffset = j3;
01262 vinfos[3].indices[0] = _ij3[0];
01263 vinfos[3].indices[1] = _ij3[1];
01264 vinfos[3].maxsolutions = _nj3;
01265 vinfos[4].jointtype = 1;
01266 vinfos[4].foffset = j4;
01267 vinfos[4].indices[0] = _ij4[0];
01268 vinfos[4].indices[1] = _ij4[1];
01269 vinfos[4].maxsolutions = _nj4;
01270 vinfos[5].jointtype = 1;
01271 vinfos[5].foffset = j5;
01272 vinfos[5].indices[0] = _ij5[0];
01273 vinfos[5].indices[1] = _ij5[1];
01274 vinfos[5].maxsolutions = _nj5;
01275 std::vector<int> vfree(0);
01276 solutions.AddSolution(vinfos,vfree);
01277 }
01278 }
01279 }
01280 
01281 } else
01282 {
01283 if( 1 )
01284 {
01285 continue;
01286 
01287 } else
01288 {
01289 }
01290 }
01291 }
01292 }
01293 
01294 } else
01295 {
01296 {
01297 IkReal j5array[1], cj5array[1], sj5array[1];
01298 bool j5valid[1]={false};
01299 _nj5 = 1;
01300 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 )
01301     continue;
01302 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))));
01303 sj5array[0]=IKsin(j5array[0]);
01304 cj5array[0]=IKcos(j5array[0]);
01305 if( j5array[0] > IKPI )
01306 {
01307     j5array[0]-=IK2PI;
01308 }
01309 else if( j5array[0] < -IKPI )
01310 {    j5array[0]+=IK2PI;
01311 }
01312 j5valid[0] = true;
01313 for(int ij5 = 0; ij5 < 1; ++ij5)
01314 {
01315 if( !j5valid[ij5] )
01316 {
01317     continue;
01318 }
01319 _ij5[0] = ij5; _ij5[1] = -1;
01320 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01321 {
01322 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01323 {
01324     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01325 }
01326 }
01327 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01328 {
01329 IkReal evalcond[8];
01330 IkReal x132=IKsin(j5);
01331 IkReal x133=IKcos(j5);
01332 IkReal x134=((IkReal(1.00000000000000))*(sj3));
01333 IkReal x135=((new_r11)*(sj3));
01334 IkReal x136=((new_r10)*(sj3));
01335 IkReal x137=((cj3)*(cj4));
01336 IkReal x138=((IkReal(1.00000000000000))*(sj4));
01337 IkReal x139=((IkReal(1.00000000000000))*(x133));
01338 IkReal x140=((IkReal(1.00000000000000))*(x132));
01339 evalcond[0]=((new_r20)+(((sj4)*(x133))));
01340 evalcond[1]=((((IkReal(-1.00000000000000))*(x132)*(x138)))+(new_r21));
01341 evalcond[2]=((((IkReal(-1.00000000000000))*(x140)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x134))));
01342 evalcond[3]=((((IkReal(-1.00000000000000))*(x139)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x134))));
01343 evalcond[4]=((((cj4)*(x132)))+(x135)+(((cj3)*(new_r01))));
01344 evalcond[5]=((x136)+(((IkReal(-1.00000000000000))*(cj4)*(x139)))+(((cj3)*(new_r00))));
01345 evalcond[6]=((((cj4)*(x135)))+(((new_r01)*(x137)))+(x132)+(((IkReal(-1.00000000000000))*(new_r21)*(x138))));
01346 evalcond[7]=((((IkReal(-1.00000000000000))*(x139)))+(((IkReal(-1.00000000000000))*(new_r20)*(x138)))+(((cj4)*(x136)))+(((new_r00)*(x137))));
01347 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  )
01348 {
01349 continue;
01350 }
01351 }
01352 
01353 {
01354 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01355 vinfos[0].jointtype = 1;
01356 vinfos[0].foffset = j0;
01357 vinfos[0].indices[0] = _ij0[0];
01358 vinfos[0].indices[1] = _ij0[1];
01359 vinfos[0].maxsolutions = _nj0;
01360 vinfos[1].jointtype = 1;
01361 vinfos[1].foffset = j1;
01362 vinfos[1].indices[0] = _ij1[0];
01363 vinfos[1].indices[1] = _ij1[1];
01364 vinfos[1].maxsolutions = _nj1;
01365 vinfos[2].jointtype = 1;
01366 vinfos[2].foffset = j2;
01367 vinfos[2].indices[0] = _ij2[0];
01368 vinfos[2].indices[1] = _ij2[1];
01369 vinfos[2].maxsolutions = _nj2;
01370 vinfos[3].jointtype = 1;
01371 vinfos[3].foffset = j3;
01372 vinfos[3].indices[0] = _ij3[0];
01373 vinfos[3].indices[1] = _ij3[1];
01374 vinfos[3].maxsolutions = _nj3;
01375 vinfos[4].jointtype = 1;
01376 vinfos[4].foffset = j4;
01377 vinfos[4].indices[0] = _ij4[0];
01378 vinfos[4].indices[1] = _ij4[1];
01379 vinfos[4].maxsolutions = _nj4;
01380 vinfos[5].jointtype = 1;
01381 vinfos[5].foffset = j5;
01382 vinfos[5].indices[0] = _ij5[0];
01383 vinfos[5].indices[1] = _ij5[1];
01384 vinfos[5].maxsolutions = _nj5;
01385 std::vector<int> vfree(0);
01386 solutions.AddSolution(vinfos,vfree);
01387 }
01388 }
01389 }
01390 
01391 }
01392 
01393 }
01394 
01395 } else
01396 {
01397 {
01398 IkReal j5array[1], cj5array[1], sj5array[1];
01399 bool j5valid[1]={false};
01400 _nj5 = 1;
01401 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 )
01402     continue;
01403 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
01404 sj5array[0]=IKsin(j5array[0]);
01405 cj5array[0]=IKcos(j5array[0]);
01406 if( j5array[0] > IKPI )
01407 {
01408     j5array[0]-=IK2PI;
01409 }
01410 else if( j5array[0] < -IKPI )
01411 {    j5array[0]+=IK2PI;
01412 }
01413 j5valid[0] = true;
01414 for(int ij5 = 0; ij5 < 1; ++ij5)
01415 {
01416 if( !j5valid[ij5] )
01417 {
01418     continue;
01419 }
01420 _ij5[0] = ij5; _ij5[1] = -1;
01421 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01422 {
01423 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01424 {
01425     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01426 }
01427 }
01428 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01429 {
01430 IkReal evalcond[8];
01431 IkReal x141=IKsin(j5);
01432 IkReal x142=IKcos(j5);
01433 IkReal x143=((IkReal(1.00000000000000))*(sj3));
01434 IkReal x144=((new_r11)*(sj3));
01435 IkReal x145=((new_r10)*(sj3));
01436 IkReal x146=((cj3)*(cj4));
01437 IkReal x147=((IkReal(1.00000000000000))*(sj4));
01438 IkReal x148=((IkReal(1.00000000000000))*(x142));
01439 IkReal x149=((IkReal(1.00000000000000))*(x141));
01440 evalcond[0]=((((sj4)*(x142)))+(new_r20));
01441 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x141)*(x147))));
01442 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x143)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x149))));
01443 evalcond[3]=((((IkReal(-1.00000000000000))*(x148)))+(((IkReal(-1.00000000000000))*(new_r01)*(x143)))+(((cj3)*(new_r11))));
01444 evalcond[4]=((((cj4)*(x141)))+(x144)+(((cj3)*(new_r01))));
01445 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x148)))+(x145)+(((cj3)*(new_r00))));
01446 evalcond[6]=((((cj4)*(x144)))+(((new_r01)*(x146)))+(x141)+(((IkReal(-1.00000000000000))*(new_r21)*(x147))));
01447 evalcond[7]=((((cj4)*(x145)))+(((IkReal(-1.00000000000000))*(x148)))+(((IkReal(-1.00000000000000))*(new_r20)*(x147)))+(((new_r00)*(x146))));
01448 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  )
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 }
01495 
01496 } else
01497 {
01498 {
01499 IkReal j5array[1], cj5array[1], sj5array[1];
01500 bool j5valid[1]={false};
01501 _nj5 = 1;
01502 if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
01503     continue;
01504 j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(new_r20)));
01505 sj5array[0]=IKsin(j5array[0]);
01506 cj5array[0]=IKcos(j5array[0]);
01507 if( j5array[0] > IKPI )
01508 {
01509     j5array[0]-=IK2PI;
01510 }
01511 else if( j5array[0] < -IKPI )
01512 {    j5array[0]+=IK2PI;
01513 }
01514 j5valid[0] = true;
01515 for(int ij5 = 0; ij5 < 1; ++ij5)
01516 {
01517 if( !j5valid[ij5] )
01518 {
01519     continue;
01520 }
01521 _ij5[0] = ij5; _ij5[1] = -1;
01522 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01523 {
01524 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01525 {
01526     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01527 }
01528 }
01529 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01530 {
01531 IkReal evalcond[8];
01532 IkReal x150=IKsin(j5);
01533 IkReal x151=IKcos(j5);
01534 IkReal x152=((IkReal(1.00000000000000))*(sj3));
01535 IkReal x153=((new_r11)*(sj3));
01536 IkReal x154=((new_r10)*(sj3));
01537 IkReal x155=((cj3)*(cj4));
01538 IkReal x156=((IkReal(1.00000000000000))*(sj4));
01539 IkReal x157=((IkReal(1.00000000000000))*(x151));
01540 IkReal x158=((IkReal(1.00000000000000))*(x150));
01541 evalcond[0]=((((sj4)*(x151)))+(new_r20));
01542 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x150)*(x156))));
01543 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x152)))+(((IkReal(-1.00000000000000))*(x158)))+(((cj3)*(new_r10))));
01544 evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(new_r01)*(x152))));
01545 evalcond[4]=((x153)+(((cj4)*(x150)))+(((cj3)*(new_r01))));
01546 evalcond[5]=((x154)+(((IkReal(-1.00000000000000))*(cj4)*(x157)))+(((cj3)*(new_r00))));
01547 evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x156)))+(x150)+(((new_r01)*(x155)))+(((cj4)*(x153))));
01548 evalcond[7]=((((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(new_r20)*(x156)))+(((cj4)*(x154)))+(((new_r00)*(x155))));
01549 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  )
01550 {
01551 continue;
01552 }
01553 }
01554 
01555 {
01556 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01557 vinfos[0].jointtype = 1;
01558 vinfos[0].foffset = j0;
01559 vinfos[0].indices[0] = _ij0[0];
01560 vinfos[0].indices[1] = _ij0[1];
01561 vinfos[0].maxsolutions = _nj0;
01562 vinfos[1].jointtype = 1;
01563 vinfos[1].foffset = j1;
01564 vinfos[1].indices[0] = _ij1[0];
01565 vinfos[1].indices[1] = _ij1[1];
01566 vinfos[1].maxsolutions = _nj1;
01567 vinfos[2].jointtype = 1;
01568 vinfos[2].foffset = j2;
01569 vinfos[2].indices[0] = _ij2[0];
01570 vinfos[2].indices[1] = _ij2[1];
01571 vinfos[2].maxsolutions = _nj2;
01572 vinfos[3].jointtype = 1;
01573 vinfos[3].foffset = j3;
01574 vinfos[3].indices[0] = _ij3[0];
01575 vinfos[3].indices[1] = _ij3[1];
01576 vinfos[3].maxsolutions = _nj3;
01577 vinfos[4].jointtype = 1;
01578 vinfos[4].foffset = j4;
01579 vinfos[4].indices[0] = _ij4[0];
01580 vinfos[4].indices[1] = _ij4[1];
01581 vinfos[4].maxsolutions = _nj4;
01582 vinfos[5].jointtype = 1;
01583 vinfos[5].foffset = j5;
01584 vinfos[5].indices[0] = _ij5[0];
01585 vinfos[5].indices[1] = _ij5[1];
01586 vinfos[5].maxsolutions = _nj5;
01587 std::vector<int> vfree(0);
01588 solutions.AddSolution(vinfos,vfree);
01589 }
01590 }
01591 }
01592 
01593 }
01594 
01595 }
01596 }
01597 }
01598 
01599 }
01600 
01601 }
01602 
01603 } else
01604 {
01605 {
01606 IkReal j3array[1], cj3array[1], sj3array[1];
01607 bool j3valid[1]={false};
01608 _nj3 = 1;
01609 IkReal x159=((gconst2)*(sj4));
01610 if( IKabs(((new_r12)*(x159))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x159))) < IKFAST_ATAN2_MAGTHRESH )
01611     continue;
01612 j3array[0]=IKatan2(((new_r12)*(x159)), ((new_r02)*(x159)));
01613 sj3array[0]=IKsin(j3array[0]);
01614 cj3array[0]=IKcos(j3array[0]);
01615 if( j3array[0] > IKPI )
01616 {
01617     j3array[0]-=IK2PI;
01618 }
01619 else if( j3array[0] < -IKPI )
01620 {    j3array[0]+=IK2PI;
01621 }
01622 j3valid[0] = true;
01623 for(int ij3 = 0; ij3 < 1; ++ij3)
01624 {
01625 if( !j3valid[ij3] )
01626 {
01627     continue;
01628 }
01629 _ij3[0] = ij3; _ij3[1] = -1;
01630 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01631 {
01632 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01633 {
01634     j3valid[iij3]=false; _ij3[1] = iij3; break; 
01635 }
01636 }
01637 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01638 {
01639 IkReal evalcond[6];
01640 IkReal x160=IKsin(j3);
01641 IkReal x161=IKcos(j3);
01642 IkReal x162=((IkReal(1.00000000000000))*(sj4));
01643 IkReal x163=((sj4)*(x160));
01644 IkReal x164=((sj4)*(x161));
01645 IkReal x165=((new_r02)*(x161));
01646 IkReal x166=((new_r12)*(x160));
01647 evalcond[0]=((((new_r12)*(x161)))+(((IkReal(-1.00000000000000))*(new_r02)*(x160))));
01648 evalcond[1]=((x166)+(x165)+(((IkReal(-1.00000000000000))*(x162))));
01649 evalcond[2]=((((new_r00)*(x164)))+(((new_r10)*(x163)))+(((cj4)*(new_r20))));
01650 evalcond[3]=((((new_r01)*(x164)))+(((new_r11)*(x163)))+(((cj4)*(new_r21))));
01651 evalcond[4]=((IkReal(-1.00000000000000))+(((new_r02)*(x164)))+(((new_r12)*(x163)))+(((cj4)*(new_r22))));
01652 evalcond[5]=((((cj4)*(x165)))+(((IkReal(-1.00000000000000))*(new_r22)*(x162)))+(((cj4)*(x166))));
01653 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  )
01654 {
01655 continue;
01656 }
01657 }
01658 
01659 {
01660 IkReal dummyeval[1];
01661 IkReal gconst5;
01662 gconst5=IKsign(sj4);
01663 dummyeval[0]=sj4;
01664 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01665 {
01666 {
01667 IkReal dummyeval[1];
01668 dummyeval[0]=sj4;
01669 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01670 {
01671 {
01672 IkReal dummyeval[1];
01673 dummyeval[0]=sj4;
01674 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01675 {
01676 {
01677 IkReal evalcond[11];
01678 IkReal x167=((IkReal(-1.00000000000000))+(new_r22));
01679 IkReal x168=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3))));
01680 IkReal x169=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
01681 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
01682 evalcond[1]=x167;
01683 evalcond[2]=new_r20;
01684 evalcond[3]=new_r21;
01685 evalcond[4]=x168;
01686 evalcond[5]=x168;
01687 evalcond[6]=x169;
01688 evalcond[7]=new_r20;
01689 evalcond[8]=new_r21;
01690 evalcond[9]=x167;
01691 evalcond[10]=x169;
01692 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  )
01693 {
01694 {
01695 IkReal j5array[1], cj5array[1], sj5array[1];
01696 bool j5valid[1]={false};
01697 _nj5 = 1;
01698 if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH )
01699     continue;
01700 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
01701 sj5array[0]=IKsin(j5array[0]);
01702 cj5array[0]=IKcos(j5array[0]);
01703 if( j5array[0] > IKPI )
01704 {
01705     j5array[0]-=IK2PI;
01706 }
01707 else if( j5array[0] < -IKPI )
01708 {    j5array[0]+=IK2PI;
01709 }
01710 j5valid[0] = true;
01711 for(int ij5 = 0; ij5 < 1; ++ij5)
01712 {
01713 if( !j5valid[ij5] )
01714 {
01715     continue;
01716 }
01717 _ij5[0] = ij5; _ij5[1] = -1;
01718 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01719 {
01720 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01721 {
01722     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01723 }
01724 }
01725 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01726 {
01727 IkReal evalcond[4];
01728 IkReal x170=IKsin(j5);
01729 IkReal x171=((IkReal(1.00000000000000))*(sj3));
01730 IkReal x172=((IkReal(1.00000000000000))*(IKcos(j5)));
01731 evalcond[0]=((((IkReal(-1.00000000000000))*(x170)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x171))));
01732 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x171)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x172))));
01733 evalcond[2]=((x170)+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
01734 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x172)))+(((cj3)*(new_r00))));
01735 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01736 {
01737 continue;
01738 }
01739 }
01740 
01741 {
01742 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01743 vinfos[0].jointtype = 1;
01744 vinfos[0].foffset = j0;
01745 vinfos[0].indices[0] = _ij0[0];
01746 vinfos[0].indices[1] = _ij0[1];
01747 vinfos[0].maxsolutions = _nj0;
01748 vinfos[1].jointtype = 1;
01749 vinfos[1].foffset = j1;
01750 vinfos[1].indices[0] = _ij1[0];
01751 vinfos[1].indices[1] = _ij1[1];
01752 vinfos[1].maxsolutions = _nj1;
01753 vinfos[2].jointtype = 1;
01754 vinfos[2].foffset = j2;
01755 vinfos[2].indices[0] = _ij2[0];
01756 vinfos[2].indices[1] = _ij2[1];
01757 vinfos[2].maxsolutions = _nj2;
01758 vinfos[3].jointtype = 1;
01759 vinfos[3].foffset = j3;
01760 vinfos[3].indices[0] = _ij3[0];
01761 vinfos[3].indices[1] = _ij3[1];
01762 vinfos[3].maxsolutions = _nj3;
01763 vinfos[4].jointtype = 1;
01764 vinfos[4].foffset = j4;
01765 vinfos[4].indices[0] = _ij4[0];
01766 vinfos[4].indices[1] = _ij4[1];
01767 vinfos[4].maxsolutions = _nj4;
01768 vinfos[5].jointtype = 1;
01769 vinfos[5].foffset = j5;
01770 vinfos[5].indices[0] = _ij5[0];
01771 vinfos[5].indices[1] = _ij5[1];
01772 vinfos[5].maxsolutions = _nj5;
01773 std::vector<int> vfree(0);
01774 solutions.AddSolution(vinfos,vfree);
01775 }
01776 }
01777 }
01778 
01779 } else
01780 {
01781 IkReal x173=((new_r12)*(sj3));
01782 IkReal x174=((IkReal(1.00000000000000))*(new_r02));
01783 IkReal x175=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(sj3)*(x174))));
01784 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
01785 evalcond[1]=((IkReal(1.00000000000000))+(new_r22));
01786 evalcond[2]=new_r20;
01787 evalcond[3]=new_r21;
01788 evalcond[4]=x175;
01789 evalcond[5]=x175;
01790 evalcond[6]=((x173)+(((cj3)*(new_r02))));
01791 evalcond[7]=((IkReal(-1.00000000000000))*(new_r20));
01792 evalcond[8]=((IkReal(-1.00000000000000))*(new_r21));
01793 evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
01794 evalcond[10]=((((IkReal(-1.00000000000000))*(x173)))+(((IkReal(-1.00000000000000))*(cj3)*(x174))));
01795 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  )
01796 {
01797 {
01798 IkReal j5array[1], cj5array[1], sj5array[1];
01799 bool j5valid[1]={false};
01800 _nj5 = 1;
01801 if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH )
01802     continue;
01803 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))));
01804 sj5array[0]=IKsin(j5array[0]);
01805 cj5array[0]=IKcos(j5array[0]);
01806 if( j5array[0] > IKPI )
01807 {
01808     j5array[0]-=IK2PI;
01809 }
01810 else if( j5array[0] < -IKPI )
01811 {    j5array[0]+=IK2PI;
01812 }
01813 j5valid[0] = true;
01814 for(int ij5 = 0; ij5 < 1; ++ij5)
01815 {
01816 if( !j5valid[ij5] )
01817 {
01818     continue;
01819 }
01820 _ij5[0] = ij5; _ij5[1] = -1;
01821 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01822 {
01823 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01824 {
01825     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01826 }
01827 }
01828 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01829 {
01830 IkReal evalcond[4];
01831 IkReal x176=IKcos(j5);
01832 IkReal x177=((IkReal(1.00000000000000))*(sj3));
01833 IkReal x178=((IkReal(1.00000000000000))*(IKsin(j5)));
01834 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x177)))+(((IkReal(-1.00000000000000))*(x178)))+(((cj3)*(new_r10))));
01835 evalcond[1]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x177)))+(((IkReal(-1.00000000000000))*(x176))));
01836 evalcond[2]=((((IkReal(-1.00000000000000))*(x178)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
01837 evalcond[3]=((x176)+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
01838 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
01839 {
01840 continue;
01841 }
01842 }
01843 
01844 {
01845 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01846 vinfos[0].jointtype = 1;
01847 vinfos[0].foffset = j0;
01848 vinfos[0].indices[0] = _ij0[0];
01849 vinfos[0].indices[1] = _ij0[1];
01850 vinfos[0].maxsolutions = _nj0;
01851 vinfos[1].jointtype = 1;
01852 vinfos[1].foffset = j1;
01853 vinfos[1].indices[0] = _ij1[0];
01854 vinfos[1].indices[1] = _ij1[1];
01855 vinfos[1].maxsolutions = _nj1;
01856 vinfos[2].jointtype = 1;
01857 vinfos[2].foffset = j2;
01858 vinfos[2].indices[0] = _ij2[0];
01859 vinfos[2].indices[1] = _ij2[1];
01860 vinfos[2].maxsolutions = _nj2;
01861 vinfos[3].jointtype = 1;
01862 vinfos[3].foffset = j3;
01863 vinfos[3].indices[0] = _ij3[0];
01864 vinfos[3].indices[1] = _ij3[1];
01865 vinfos[3].maxsolutions = _nj3;
01866 vinfos[4].jointtype = 1;
01867 vinfos[4].foffset = j4;
01868 vinfos[4].indices[0] = _ij4[0];
01869 vinfos[4].indices[1] = _ij4[1];
01870 vinfos[4].maxsolutions = _nj4;
01871 vinfos[5].jointtype = 1;
01872 vinfos[5].foffset = j5;
01873 vinfos[5].indices[0] = _ij5[0];
01874 vinfos[5].indices[1] = _ij5[1];
01875 vinfos[5].maxsolutions = _nj5;
01876 std::vector<int> vfree(0);
01877 solutions.AddSolution(vinfos,vfree);
01878 }
01879 }
01880 }
01881 
01882 } else
01883 {
01884 if( 1 )
01885 {
01886 continue;
01887 
01888 } else
01889 {
01890 }
01891 }
01892 }
01893 }
01894 
01895 } else
01896 {
01897 {
01898 IkReal j5array[1], cj5array[1], sj5array[1];
01899 bool j5valid[1]={false};
01900 _nj5 = 1;
01901 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 )
01902     continue;
01903 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))));
01904 sj5array[0]=IKsin(j5array[0]);
01905 cj5array[0]=IKcos(j5array[0]);
01906 if( j5array[0] > IKPI )
01907 {
01908     j5array[0]-=IK2PI;
01909 }
01910 else if( j5array[0] < -IKPI )
01911 {    j5array[0]+=IK2PI;
01912 }
01913 j5valid[0] = true;
01914 for(int ij5 = 0; ij5 < 1; ++ij5)
01915 {
01916 if( !j5valid[ij5] )
01917 {
01918     continue;
01919 }
01920 _ij5[0] = ij5; _ij5[1] = -1;
01921 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01922 {
01923 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01924 {
01925     j5valid[iij5]=false; _ij5[1] = iij5; break; 
01926 }
01927 }
01928 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01929 {
01930 IkReal evalcond[8];
01931 IkReal x179=IKsin(j5);
01932 IkReal x180=IKcos(j5);
01933 IkReal x181=((IkReal(1.00000000000000))*(sj3));
01934 IkReal x182=((new_r11)*(sj3));
01935 IkReal x183=((new_r10)*(sj3));
01936 IkReal x184=((cj3)*(cj4));
01937 IkReal x185=((IkReal(1.00000000000000))*(sj4));
01938 IkReal x186=((IkReal(1.00000000000000))*(x180));
01939 IkReal x187=((IkReal(1.00000000000000))*(x179));
01940 evalcond[0]=((((sj4)*(x180)))+(new_r20));
01941 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x179)*(x185))));
01942 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x181)))+(((IkReal(-1.00000000000000))*(x187)))+(((cj3)*(new_r10))));
01943 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x181)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x186))));
01944 evalcond[4]=((x182)+(((cj3)*(new_r01)))+(((cj4)*(x179))));
01945 evalcond[5]=((x183)+(((IkReal(-1.00000000000000))*(cj4)*(x186)))+(((cj3)*(new_r00))));
01946 evalcond[6]=((x179)+(((cj4)*(x182)))+(((new_r01)*(x184)))+(((IkReal(-1.00000000000000))*(new_r21)*(x185))));
01947 evalcond[7]=((((new_r00)*(x184)))+(((cj4)*(x183)))+(((IkReal(-1.00000000000000))*(new_r20)*(x185)))+(((IkReal(-1.00000000000000))*(x186))));
01948 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  )
01949 {
01950 continue;
01951 }
01952 }
01953 
01954 {
01955 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01956 vinfos[0].jointtype = 1;
01957 vinfos[0].foffset = j0;
01958 vinfos[0].indices[0] = _ij0[0];
01959 vinfos[0].indices[1] = _ij0[1];
01960 vinfos[0].maxsolutions = _nj0;
01961 vinfos[1].jointtype = 1;
01962 vinfos[1].foffset = j1;
01963 vinfos[1].indices[0] = _ij1[0];
01964 vinfos[1].indices[1] = _ij1[1];
01965 vinfos[1].maxsolutions = _nj1;
01966 vinfos[2].jointtype = 1;
01967 vinfos[2].foffset = j2;
01968 vinfos[2].indices[0] = _ij2[0];
01969 vinfos[2].indices[1] = _ij2[1];
01970 vinfos[2].maxsolutions = _nj2;
01971 vinfos[3].jointtype = 1;
01972 vinfos[3].foffset = j3;
01973 vinfos[3].indices[0] = _ij3[0];
01974 vinfos[3].indices[1] = _ij3[1];
01975 vinfos[3].maxsolutions = _nj3;
01976 vinfos[4].jointtype = 1;
01977 vinfos[4].foffset = j4;
01978 vinfos[4].indices[0] = _ij4[0];
01979 vinfos[4].indices[1] = _ij4[1];
01980 vinfos[4].maxsolutions = _nj4;
01981 vinfos[5].jointtype = 1;
01982 vinfos[5].foffset = j5;
01983 vinfos[5].indices[0] = _ij5[0];
01984 vinfos[5].indices[1] = _ij5[1];
01985 vinfos[5].maxsolutions = _nj5;
01986 std::vector<int> vfree(0);
01987 solutions.AddSolution(vinfos,vfree);
01988 }
01989 }
01990 }
01991 
01992 }
01993 
01994 }
01995 
01996 } else
01997 {
01998 {
01999 IkReal j5array[1], cj5array[1], sj5array[1];
02000 bool j5valid[1]={false};
02001 _nj5 = 1;
02002 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 )
02003     continue;
02004 j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
02005 sj5array[0]=IKsin(j5array[0]);
02006 cj5array[0]=IKcos(j5array[0]);
02007 if( j5array[0] > IKPI )
02008 {
02009     j5array[0]-=IK2PI;
02010 }
02011 else if( j5array[0] < -IKPI )
02012 {    j5array[0]+=IK2PI;
02013 }
02014 j5valid[0] = true;
02015 for(int ij5 = 0; ij5 < 1; ++ij5)
02016 {
02017 if( !j5valid[ij5] )
02018 {
02019     continue;
02020 }
02021 _ij5[0] = ij5; _ij5[1] = -1;
02022 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02023 {
02024 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02025 {
02026     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02027 }
02028 }
02029 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02030 {
02031 IkReal evalcond[8];
02032 IkReal x188=IKsin(j5);
02033 IkReal x189=IKcos(j5);
02034 IkReal x190=((IkReal(1.00000000000000))*(sj3));
02035 IkReal x191=((new_r11)*(sj3));
02036 IkReal x192=((new_r10)*(sj3));
02037 IkReal x193=((cj3)*(cj4));
02038 IkReal x194=((IkReal(1.00000000000000))*(sj4));
02039 IkReal x195=((IkReal(1.00000000000000))*(x189));
02040 IkReal x196=((IkReal(1.00000000000000))*(x188));
02041 evalcond[0]=((new_r20)+(((sj4)*(x189))));
02042 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x188)*(x194))));
02043 evalcond[2]=((((IkReal(-1.00000000000000))*(x196)))+(((IkReal(-1.00000000000000))*(new_r00)*(x190)))+(((cj3)*(new_r10))));
02044 evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x190)))+(((IkReal(-1.00000000000000))*(x195))));
02045 evalcond[4]=((((cj4)*(x188)))+(x191)+(((cj3)*(new_r01))));
02046 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x195)))+(x192)+(((cj3)*(new_r00))));
02047 evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x194)))+(((cj4)*(x191)))+(x188)+(((new_r01)*(x193))));
02048 evalcond[7]=((((new_r00)*(x193)))+(((IkReal(-1.00000000000000))*(x195)))+(((cj4)*(x192)))+(((IkReal(-1.00000000000000))*(new_r20)*(x194))));
02049 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  )
02050 {
02051 continue;
02052 }
02053 }
02054 
02055 {
02056 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02057 vinfos[0].jointtype = 1;
02058 vinfos[0].foffset = j0;
02059 vinfos[0].indices[0] = _ij0[0];
02060 vinfos[0].indices[1] = _ij0[1];
02061 vinfos[0].maxsolutions = _nj0;
02062 vinfos[1].jointtype = 1;
02063 vinfos[1].foffset = j1;
02064 vinfos[1].indices[0] = _ij1[0];
02065 vinfos[1].indices[1] = _ij1[1];
02066 vinfos[1].maxsolutions = _nj1;
02067 vinfos[2].jointtype = 1;
02068 vinfos[2].foffset = j2;
02069 vinfos[2].indices[0] = _ij2[0];
02070 vinfos[2].indices[1] = _ij2[1];
02071 vinfos[2].maxsolutions = _nj2;
02072 vinfos[3].jointtype = 1;
02073 vinfos[3].foffset = j3;
02074 vinfos[3].indices[0] = _ij3[0];
02075 vinfos[3].indices[1] = _ij3[1];
02076 vinfos[3].maxsolutions = _nj3;
02077 vinfos[4].jointtype = 1;
02078 vinfos[4].foffset = j4;
02079 vinfos[4].indices[0] = _ij4[0];
02080 vinfos[4].indices[1] = _ij4[1];
02081 vinfos[4].maxsolutions = _nj4;
02082 vinfos[5].jointtype = 1;
02083 vinfos[5].foffset = j5;
02084 vinfos[5].indices[0] = _ij5[0];
02085 vinfos[5].indices[1] = _ij5[1];
02086 vinfos[5].maxsolutions = _nj5;
02087 std::vector<int> vfree(0);
02088 solutions.AddSolution(vinfos,vfree);
02089 }
02090 }
02091 }
02092 
02093 }
02094 
02095 }
02096 
02097 } else
02098 {
02099 {
02100 IkReal j5array[1], cj5array[1], sj5array[1];
02101 bool j5valid[1]={false};
02102 _nj5 = 1;
02103 if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02104     continue;
02105 j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(new_r20)));
02106 sj5array[0]=IKsin(j5array[0]);
02107 cj5array[0]=IKcos(j5array[0]);
02108 if( j5array[0] > IKPI )
02109 {
02110     j5array[0]-=IK2PI;
02111 }
02112 else if( j5array[0] < -IKPI )
02113 {    j5array[0]+=IK2PI;
02114 }
02115 j5valid[0] = true;
02116 for(int ij5 = 0; ij5 < 1; ++ij5)
02117 {
02118 if( !j5valid[ij5] )
02119 {
02120     continue;
02121 }
02122 _ij5[0] = ij5; _ij5[1] = -1;
02123 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02124 {
02125 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02126 {
02127     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02128 }
02129 }
02130 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02131 {
02132 IkReal evalcond[8];
02133 IkReal x197=IKsin(j5);
02134 IkReal x198=IKcos(j5);
02135 IkReal x199=((IkReal(1.00000000000000))*(sj3));
02136 IkReal x200=((new_r11)*(sj3));
02137 IkReal x201=((new_r10)*(sj3));
02138 IkReal x202=((cj3)*(cj4));
02139 IkReal x203=((IkReal(1.00000000000000))*(sj4));
02140 IkReal x204=((IkReal(1.00000000000000))*(x198));
02141 IkReal x205=((IkReal(1.00000000000000))*(x197));
02142 evalcond[0]=((new_r20)+(((sj4)*(x198))));
02143 evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x197)*(x203))));
02144 evalcond[2]=((((IkReal(-1.00000000000000))*(x205)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x199))));
02145 evalcond[3]=((((IkReal(-1.00000000000000))*(x204)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x199))));
02146 evalcond[4]=((((cj4)*(x197)))+(x200)+(((cj3)*(new_r01))));
02147 evalcond[5]=((x201)+(((IkReal(-1.00000000000000))*(cj4)*(x204)))+(((cj3)*(new_r00))));
02148 evalcond[6]=((((new_r01)*(x202)))+(((cj4)*(x200)))+(x197)+(((IkReal(-1.00000000000000))*(new_r21)*(x203))));
02149 evalcond[7]=((((cj4)*(x201)))+(((new_r00)*(x202)))+(((IkReal(-1.00000000000000))*(x204)))+(((IkReal(-1.00000000000000))*(new_r20)*(x203))));
02150 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  )
02151 {
02152 continue;
02153 }
02154 }
02155 
02156 {
02157 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02158 vinfos[0].jointtype = 1;
02159 vinfos[0].foffset = j0;
02160 vinfos[0].indices[0] = _ij0[0];
02161 vinfos[0].indices[1] = _ij0[1];
02162 vinfos[0].maxsolutions = _nj0;
02163 vinfos[1].jointtype = 1;
02164 vinfos[1].foffset = j1;
02165 vinfos[1].indices[0] = _ij1[0];
02166 vinfos[1].indices[1] = _ij1[1];
02167 vinfos[1].maxsolutions = _nj1;
02168 vinfos[2].jointtype = 1;
02169 vinfos[2].foffset = j2;
02170 vinfos[2].indices[0] = _ij2[0];
02171 vinfos[2].indices[1] = _ij2[1];
02172 vinfos[2].maxsolutions = _nj2;
02173 vinfos[3].jointtype = 1;
02174 vinfos[3].foffset = j3;
02175 vinfos[3].indices[0] = _ij3[0];
02176 vinfos[3].indices[1] = _ij3[1];
02177 vinfos[3].maxsolutions = _nj3;
02178 vinfos[4].jointtype = 1;
02179 vinfos[4].foffset = j4;
02180 vinfos[4].indices[0] = _ij4[0];
02181 vinfos[4].indices[1] = _ij4[1];
02182 vinfos[4].maxsolutions = _nj4;
02183 vinfos[5].jointtype = 1;
02184 vinfos[5].foffset = j5;
02185 vinfos[5].indices[0] = _ij5[0];
02186 vinfos[5].indices[1] = _ij5[1];
02187 vinfos[5].maxsolutions = _nj5;
02188 std::vector<int> vfree(0);
02189 solutions.AddSolution(vinfos,vfree);
02190 }
02191 }
02192 }
02193 
02194 }
02195 
02196 }
02197 }
02198 }
02199 
02200 }
02201 
02202 }
02203 
02204 } else
02205 {
02206 {
02207 IkReal j5array[1], cj5array[1], sj5array[1];
02208 bool j5valid[1]={false};
02209 _nj5 = 1;
02210 if( IKabs(((gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
02211     continue;
02212 j5array[0]=IKatan2(((gconst4)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst4)*(new_r20)));
02213 sj5array[0]=IKsin(j5array[0]);
02214 cj5array[0]=IKcos(j5array[0]);
02215 if( j5array[0] > IKPI )
02216 {
02217     j5array[0]-=IK2PI;
02218 }
02219 else if( j5array[0] < -IKPI )
02220 {    j5array[0]+=IK2PI;
02221 }
02222 j5valid[0] = true;
02223 for(int ij5 = 0; ij5 < 1; ++ij5)
02224 {
02225 if( !j5valid[ij5] )
02226 {
02227     continue;
02228 }
02229 _ij5[0] = ij5; _ij5[1] = -1;
02230 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02231 {
02232 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02233 {
02234     j5valid[iij5]=false; _ij5[1] = iij5; break; 
02235 }
02236 }
02237 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02238 {
02239 IkReal evalcond[2];
02240 evalcond[0]=((new_r20)+(((sj4)*(IKcos(j5)))));
02241 evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(IKsin(j5))))+(new_r21));
02242 if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
02243 {
02244 continue;
02245 }
02246 }
02247 
02248 {
02249 IkReal dummyeval[1];
02250 IkReal gconst6;
02251 gconst6=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
02252 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
02253 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02254 {
02255 {
02256 IkReal dummyeval[1];
02257 IkReal gconst7;
02258 gconst7=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12)))));
02259 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12))));
02260 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
02261 {
02262 continue;
02263 
02264 } else
02265 {
02266 {
02267 IkReal j3array[1], cj3array[1], sj3array[1];
02268 bool j3valid[1]={false};
02269 _nj3 = 1;
02270 IkReal x206=((cj4)*(gconst7)*(sj5));
02271 if( IKabs(((new_r12)*(x206))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x206))) < IKFAST_ATAN2_MAGTHRESH )
02272     continue;
02273 j3array[0]=IKatan2(((new_r12)*(x206)), ((new_r02)*(x206)));
02274 sj3array[0]=IKsin(j3array[0]);
02275 cj3array[0]=IKcos(j3array[0]);
02276 if( j3array[0] > IKPI )
02277 {
02278     j3array[0]-=IK2PI;
02279 }
02280 else if( j3array[0] < -IKPI )
02281 {    j3array[0]+=IK2PI;
02282 }
02283 j3valid[0] = true;
02284 for(int ij3 = 0; ij3 < 1; ++ij3)
02285 {
02286 if( !j3valid[ij3] )
02287 {
02288     continue;
02289 }
02290 _ij3[0] = ij3; _ij3[1] = -1;
02291 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02292 {
02293 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02294 {
02295     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02296 }
02297 }
02298 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02299 {
02300 IkReal evalcond[12];
02301 IkReal x207=IKsin(j3);
02302 IkReal x208=IKcos(j3);
02303 IkReal x209=((IkReal(1.00000000000000))*(cj5));
02304 IkReal x210=((IkReal(1.00000000000000))*(sj4));
02305 IkReal x211=((cj4)*(x208));
02306 IkReal x212=((sj4)*(x208));
02307 IkReal x213=((cj4)*(x207));
02308 IkReal x214=((new_r11)*(x207));
02309 IkReal x215=((sj4)*(x207));
02310 IkReal x216=((IkReal(1.00000000000000))*(x207));
02311 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x216)))+(((new_r12)*(x208))));
02312 evalcond[1]=((((IkReal(-1.00000000000000))*(x210)))+(((new_r12)*(x207)))+(((new_r02)*(x208))));
02313 evalcond[2]=((((new_r10)*(x208)))+(((IkReal(-1.00000000000000))*(new_r00)*(x216)))+(((IkReal(-1.00000000000000))*(sj5))));
02314 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x216)))+(((IkReal(-1.00000000000000))*(x209)))+(((new_r11)*(x208))));
02315 evalcond[4]=((((new_r01)*(x208)))+(x214)+(((cj4)*(sj5))));
02316 evalcond[5]=((((new_r10)*(x207)))+(((new_r00)*(x208)))+(((IkReal(-1.00000000000000))*(cj4)*(x209))));
02317 evalcond[6]=((((new_r00)*(x212)))+(((new_r10)*(x215)))+(((cj4)*(new_r20))));
02318 evalcond[7]=((((cj4)*(new_r21)))+(((new_r01)*(x212)))+(((sj4)*(x214))));
02319 evalcond[8]=((IkReal(-1.00000000000000))+(((cj4)*(new_r22)))+(((new_r02)*(x212)))+(((new_r12)*(x215))));
02320 evalcond[9]=((((new_r12)*(x213)))+(((new_r02)*(x211)))+(((IkReal(-1.00000000000000))*(new_r22)*(x210))));
02321 evalcond[10]=((((new_r11)*(x213)))+(((IkReal(-1.00000000000000))*(new_r21)*(x210)))+(sj5)+(((new_r01)*(x211))));
02322 evalcond[11]=((((IkReal(-1.00000000000000))*(x209)))+(((new_r00)*(x211)))+(((IkReal(-1.00000000000000))*(new_r20)*(x210)))+(((new_r10)*(x213))));
02323 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  )
02324 {
02325 continue;
02326 }
02327 }
02328 
02329 {
02330 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02331 vinfos[0].jointtype = 1;
02332 vinfos[0].foffset = j0;
02333 vinfos[0].indices[0] = _ij0[0];
02334 vinfos[0].indices[1] = _ij0[1];
02335 vinfos[0].maxsolutions = _nj0;
02336 vinfos[1].jointtype = 1;
02337 vinfos[1].foffset = j1;
02338 vinfos[1].indices[0] = _ij1[0];
02339 vinfos[1].indices[1] = _ij1[1];
02340 vinfos[1].maxsolutions = _nj1;
02341 vinfos[2].jointtype = 1;
02342 vinfos[2].foffset = j2;
02343 vinfos[2].indices[0] = _ij2[0];
02344 vinfos[2].indices[1] = _ij2[1];
02345 vinfos[2].maxsolutions = _nj2;
02346 vinfos[3].jointtype = 1;
02347 vinfos[3].foffset = j3;
02348 vinfos[3].indices[0] = _ij3[0];
02349 vinfos[3].indices[1] = _ij3[1];
02350 vinfos[3].maxsolutions = _nj3;
02351 vinfos[4].jointtype = 1;
02352 vinfos[4].foffset = j4;
02353 vinfos[4].indices[0] = _ij4[0];
02354 vinfos[4].indices[1] = _ij4[1];
02355 vinfos[4].maxsolutions = _nj4;
02356 vinfos[5].jointtype = 1;
02357 vinfos[5].foffset = j5;
02358 vinfos[5].indices[0] = _ij5[0];
02359 vinfos[5].indices[1] = _ij5[1];
02360 vinfos[5].maxsolutions = _nj5;
02361 std::vector<int> vfree(0);
02362 solutions.AddSolution(vinfos,vfree);
02363 }
02364 }
02365 }
02366 
02367 }
02368 
02369 }
02370 
02371 } else
02372 {
02373 {
02374 IkReal j3array[1], cj3array[1], sj3array[1];
02375 bool j3valid[1]={false};
02376 _nj3 = 1;
02377 IkReal x217=((gconst6)*(sj4));
02378 if( IKabs(((new_r12)*(x217))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x217))) < IKFAST_ATAN2_MAGTHRESH )
02379     continue;
02380 j3array[0]=IKatan2(((new_r12)*(x217)), ((new_r02)*(x217)));
02381 sj3array[0]=IKsin(j3array[0]);
02382 cj3array[0]=IKcos(j3array[0]);
02383 if( j3array[0] > IKPI )
02384 {
02385     j3array[0]-=IK2PI;
02386 }
02387 else if( j3array[0] < -IKPI )
02388 {    j3array[0]+=IK2PI;
02389 }
02390 j3valid[0] = true;
02391 for(int ij3 = 0; ij3 < 1; ++ij3)
02392 {
02393 if( !j3valid[ij3] )
02394 {
02395     continue;
02396 }
02397 _ij3[0] = ij3; _ij3[1] = -1;
02398 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02399 {
02400 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02401 {
02402     j3valid[iij3]=false; _ij3[1] = iij3; break; 
02403 }
02404 }
02405 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02406 {
02407 IkReal evalcond[12];
02408 IkReal x218=IKsin(j3);
02409 IkReal x219=IKcos(j3);
02410 IkReal x220=((IkReal(1.00000000000000))*(cj5));
02411 IkReal x221=((IkReal(1.00000000000000))*(sj4));
02412 IkReal x222=((cj4)*(x219));
02413 IkReal x223=((sj4)*(x219));
02414 IkReal x224=((cj4)*(x218));
02415 IkReal x225=((new_r11)*(x218));
02416 IkReal x226=((sj4)*(x218));
02417 IkReal x227=((IkReal(1.00000000000000))*(x218));
02418 evalcond[0]=((((new_r12)*(x219)))+(((IkReal(-1.00000000000000))*(new_r02)*(x227))));
02419 evalcond[1]=((((IkReal(-1.00000000000000))*(x221)))+(((new_r12)*(x218)))+(((new_r02)*(x219))));
02420 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x227)))+(((new_r10)*(x219)))+(((IkReal(-1.00000000000000))*(sj5))));
02421 evalcond[3]=((((IkReal(-1.00000000000000))*(x220)))+(((new_r11)*(x219)))+(((IkReal(-1.00000000000000))*(new_r01)*(x227))));
02422 evalcond[4]=((((new_r01)*(x219)))+(x225)+(((cj4)*(sj5))));
02423 evalcond[5]=((((new_r00)*(x219)))+(((new_r10)*(x218)))+(((IkReal(-1.00000000000000))*(cj4)*(x220))));
02424 evalcond[6]=((((new_r00)*(x223)))+(((cj4)*(new_r20)))+(((new_r10)*(x226))));
02425 evalcond[7]=((((sj4)*(x225)))+(((cj4)*(new_r21)))+(((new_r01)*(x223))));
02426 evalcond[8]=((IkReal(-1.00000000000000))+(((new_r12)*(x226)))+(((new_r02)*(x223)))+(((cj4)*(new_r22))));
02427 evalcond[9]=((((IkReal(-1.00000000000000))*(new_r22)*(x221)))+(((new_r12)*(x224)))+(((new_r02)*(x222))));
02428 evalcond[10]=((((new_r11)*(x224)))+(((new_r01)*(x222)))+(sj5)+(((IkReal(-1.00000000000000))*(new_r21)*(x221))));
02429 evalcond[11]=((((IkReal(-1.00000000000000))*(x220)))+(((new_r10)*(x224)))+(((IkReal(-1.00000000000000))*(new_r20)*(x221)))+(((new_r00)*(x222))));
02430 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  )
02431 {
02432 continue;
02433 }
02434 }
02435 
02436 {
02437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02438 vinfos[0].jointtype = 1;
02439 vinfos[0].foffset = j0;
02440 vinfos[0].indices[0] = _ij0[0];
02441 vinfos[0].indices[1] = _ij0[1];
02442 vinfos[0].maxsolutions = _nj0;
02443 vinfos[1].jointtype = 1;
02444 vinfos[1].foffset = j1;
02445 vinfos[1].indices[0] = _ij1[0];
02446 vinfos[1].indices[1] = _ij1[1];
02447 vinfos[1].maxsolutions = _nj1;
02448 vinfos[2].jointtype = 1;
02449 vinfos[2].foffset = j2;
02450 vinfos[2].indices[0] = _ij2[0];
02451 vinfos[2].indices[1] = _ij2[1];
02452 vinfos[2].maxsolutions = _nj2;
02453 vinfos[3].jointtype = 1;
02454 vinfos[3].foffset = j3;
02455 vinfos[3].indices[0] = _ij3[0];
02456 vinfos[3].indices[1] = _ij3[1];
02457 vinfos[3].maxsolutions = _nj3;
02458 vinfos[4].jointtype = 1;
02459 vinfos[4].foffset = j4;
02460 vinfos[4].indices[0] = _ij4[0];
02461 vinfos[4].indices[1] = _ij4[1];
02462 vinfos[4].maxsolutions = _nj4;
02463 vinfos[5].jointtype = 1;
02464 vinfos[5].foffset = j5;
02465 vinfos[5].indices[0] = _ij5[0];
02466 vinfos[5].indices[1] = _ij5[1];
02467 vinfos[5].maxsolutions = _nj5;
02468 std::vector<int> vfree(0);
02469 solutions.AddSolution(vinfos,vfree);
02470 }
02471 }
02472 }
02473 
02474 }
02475 
02476 }
02477 }
02478 }
02479 
02480 }
02481 
02482 }
02483 }
02484 }
02485 }
02486 }};
02487 
02488 
02491 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
02492 IKSolver solver;
02493 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
02494 }
02495 
02496 IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - abb_irb2400 (1f04c8a90b29778d31a8f2cb88b4a166)>"; }
02497 
02498 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
02499 
02500 #ifdef IKFAST_NAMESPACE
02501 } // end namespace
02502 #endif
02503 
02504 #ifndef IKFAST_NO_MAIN
02505 #include <stdio.h>
02506 #include <stdlib.h>
02507 #ifdef IKFAST_NAMESPACE
02508 using namespace IKFAST_NAMESPACE;
02509 #endif
02510 int main(int argc, char** argv)
02511 {
02512     if( argc != 12+GetNumFreeParameters()+1 ) {
02513         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
02514                "Returns the ik solutions given the transformation of the end effector specified by\n"
02515                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
02516                "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
02517         return 1;
02518     }
02519 
02520     IkSolutionList<IkReal> solutions;
02521     std::vector<IkReal> vfree(GetNumFreeParameters());
02522     IkReal eerot[9],eetrans[3];
02523     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
02524     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
02525     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
02526     for(std::size_t i = 0; i < vfree.size(); ++i)
02527         vfree[i] = atof(argv[13+i]);
02528     bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
02529 
02530     if( !bSuccess ) {
02531         fprintf(stderr,"Failed to get ik solution\n");
02532         return -1;
02533     }
02534 
02535     printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
02536     std::vector<IkReal> solvalues(GetNumJoints());
02537     for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
02538         const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
02539         printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
02540         std::vector<IkReal> vsolfree(sol.GetFree().size());
02541         sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
02542         for( std::size_t j = 0; j < solvalues.size(); ++j)
02543             printf("%.15f, ", solvalues[j]);
02544         printf("\n");
02545     }
02546     return 0;
02547 }
02548 
02549 #endif