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

m16ib20_arm_navigation
Author(s): Michael O. Blanton Jr
autogenerated on Sun Jul 7 2013 03:08:48