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


fanuc_lrmate200i_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sun Feb 10 2019 03:59:01