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