fanuc_m20ia_moveit_plugins: fanuc_m20ia_manipulator_ikfast

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