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"
00022 using namespace ikfast;
00023
00024
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
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;
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
00098 #ifndef IKFAST_SINCOS_THRESH
00099 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
00100 #endif
00101
00102
00103 #ifndef IKFAST_ATAN2_MAGTHRESH
00104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
00105 #endif
00106
00107
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 );
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 );
00122 if( f <= -1 ) return -IKPI_2;
00123 else if( f >= 1 ) return IKPI_2;
00124 return asin(f);
00125 }
00126
00127
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
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 );
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 );
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));
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));
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,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65,x66,x67,x68,x69,x70,x71,x72,x73,x74,x75,x76,x77,x78,x79;
00212 x0=IKcos(j[0]);
00213 x1=IKcos(j[1]);
00214 x2=IKsin(j[2]);
00215 x3=IKcos(j[2]);
00216 x4=IKsin(j[1]);
00217 x5=IKsin(j[3]);
00218 x6=IKcos(j[3]);
00219 x7=IKsin(j[0]);
00220 x8=IKcos(j[4]);
00221 x9=IKsin(j[4]);
00222 x10=IKcos(j[5]);
00223 x11=IKsin(j[5]);
00224 x12=((IkReal(0.258820826967441))*(x9));
00225 x13=((IkReal(0.258820826967441))*(x11));
00226 x14=((IkReal(1.00000000000000))*(x8));
00227 x15=((IkReal(0.0300000000000000))*(x0));
00228 x16=((IkReal(1.00000000000000))*(x5));
00229 x17=((IkReal(0.965925348838040))*(x11));
00230 x18=((IkReal(0.258820826967441))*(x8));
00231 x19=((IkReal(0.0470000000000000))*(x0));
00232 x20=((IkReal(0.965925348838040))*(x9));
00233 x21=((IkReal(1.00000000000000))*(x9));
00234 x22=((IkReal(0.00776462480902323))*(x7));
00235 x23=((IkReal(0.965925348838040))*(x8));
00236 x24=((IkReal(0.0289777604651412))*(x7));
00237 x25=((IkReal(1.00000000000000))*(x7));
00238 x26=((IkReal(0.235000000000000))*(x0));
00239 x27=((IkReal(0.0470000000000000))*(x7));
00240 x28=((IkReal(0.0900000000000000))*(x6));
00241 x29=((IkReal(0.965925348838040))*(x10));
00242 x30=((IkReal(1.00000000000000))*(x0));
00243 x31=((IkReal(0.258820826967441))*(x10));
00244 x32=((IkReal(0.0900000000000000))*(x0));
00245 x33=((IkReal(0.0900000000000000))*(x7));
00246 x34=((IkReal(1.00000000000000))*(x6));
00247 x35=((x1)*(x3));
00248 x36=((x6)*(x9));
00249 x37=((x2)*(x4));
00250 x38=((x4)*(x7));
00251 x39=((IkReal(-0.0470000000000000))*(x6));
00252 x40=((x3)*(x4));
00253 x41=((x0)*(x5));
00254 x42=((x1)*(x2));
00255 x43=((x16)*(x7));
00256 x44=((x25)*(x6));
00257 x45=((x30)*(x37));
00258 x46=((x25)*(x37));
00259 x47=((((IkReal(-1.00000000000000))*(x35)))+(x37));
00260 x48=((((IkReal(0.0470000000000000))*(x35)))+(((IkReal(-0.0470000000000000))*(x37))));
00261 x49=((((IkReal(0.0900000000000000))*(x37)))+(((IkReal(-0.0900000000000000))*(x35))));
00262 x50=((((IkReal(1.00000000000000))*(x42)))+(((IkReal(1.00000000000000))*(x40))));
00263 x51=((IkReal(-1.00000000000000))*(x50));
00264 x52=((((IkReal(0.0470000000000000))*(x40)))+(((IkReal(0.0470000000000000))*(x42))));
00265 x53=((((IkReal(0.0900000000000000))*(x42)))+(((IkReal(0.0900000000000000))*(x40))));
00266 x54=((((IkReal(-1.00000000000000))*(x45)))+(((x0)*(x35))));
00267 x55=((((IkReal(-1.00000000000000))*(x46)))+(((x35)*(x7))));
00268 x56=((x52)*(x6));
00269 x57=((x5)*(x51));
00270 x58=((x51)*(x6));
00271 x59=((((IkReal(-1.00000000000000))*(x30)*(x35)))+(x45));
00272 x60=((((IkReal(-1.00000000000000))*(x25)*(x35)))+(x46));
00273 x61=((x30)*(((((IkReal(-1.00000000000000))*(x40)))+(((IkReal(-1.00000000000000))*(x42))))));
00274 x62=((x25)*(((((IkReal(-1.00000000000000))*(x40)))+(((IkReal(-1.00000000000000))*(x42))))));
00275 x63=((x27)*(((x42)+(x40))));
00276 x64=((x33)*(((((IkReal(-1.00000000000000))*(x40)))+(((IkReal(-1.00000000000000))*(x42))))));
00277 x65=((x55)*(x6));
00278 x66=((x54)*(x6));
00279 x67=((x10)*(x5)*(x50));
00280 x68=((x5)*(x59));
00281 x69=((x41)+(x65));
00282 x70=((((x0)*(x6)))+(((x5)*(x60))));
00283 x71=((((IkReal(-1.00000000000000))*(x28)*(x55)))+(((IkReal(-1.00000000000000))*(x32)*(x5))));
00284 x72=((((IkReal(-1.00000000000000))*(x16)*(x60)))+(((IkReal(-1.00000000000000))*(x30)*(x6))));
00285 x73=((((x39)*(x55)))+(((IkReal(-0.0470000000000000))*(x41))));
00286 x74=((x73)*(x8));
00287 x75=((((IkReal(-1.00000000000000))*(x14)*(x47)))+(((IkReal(-1.00000000000000))*(x21)*(x50)*(x6))));
00288 x76=((x11)*(x75));
00289 x77=((((x21)*(((((IkReal(-1.00000000000000))*(x43)))+(((x34)*(x54)))))))+(((IkReal(-1.00000000000000))*(x14)*(x61))));
00290 x78=((((x21)*(((((x0)*(x16)))+(((x34)*(x55)))))))+(((IkReal(-1.00000000000000))*(x14)*(x62))));
00291 x79=((x10)*(x78));
00292 eerot[0]=((((x8)*(((((IkReal(-1.00000000000000))*(x43)))+(x66)))))+(((x61)*(x9))));
00293 eerot[1]=((((x10)*(((((IkReal(-1.00000000000000))*(x44)))+(x68)))))+(((x11)*(x77))));
00294 eerot[2]=((((x10)*(x77)))+(((x11)*(((x44)+(((IkReal(-1.00000000000000))*(x16)*(x59))))))));
00295 IkReal x80=((IkReal(1.00000000000000))*(x42));
00296 IkReal x81=((IkReal(1.00000000000000))*(x40));
00297 eetrans[0]=((((x9)*(((((IkReal(-1.00000000000000))*(x28)*(x54)))+(((x33)*(x5)))))))+(((x15)*(x37)))+(((x8)*(((((IkReal(-1.00000000000000))*(x32)*(x80)))+(((IkReal(-1.00000000000000))*(x32)*(x81)))))))+(((x9)*(((((x19)*(x42)))+(((x19)*(x40)))))))+(((IkReal(-0.250000000000000))*(x0)*(x4)))+(((IkReal(-1.00000000000000))*(x26)*(x80)))+(((IkReal(-1.00000000000000))*(x26)*(x81)))+(((x8)*(((((x39)*(x54)))+(((x27)*(x5)))))))+(((IkReal(-0.0950000000000000))*(x7)))+(((IkReal(-1.00000000000000))*(x15)*(x35))));
00298 eerot[3]=((((x23)*(x69)))+(((x20)*(x62)))+(((x18)*(x58)))+(((x12)*(x47))));
00299 eerot[4]=((((x17)*(x78)))+(((x29)*(x70)))+(((x13)*(x75)))+(((x31)*(x5)*(x50))));
00300 eerot[5]=((((x13)*(x57)))+(((x17)*(x72)))+(((x29)*(x78)))+(((x31)*(x75))));
00301 eetrans[1]=((IkReal(0.145000000000000))+(((IkReal(-0.0608228943373486))*(x35)))+(((IkReal(-0.226992456976939))*(x3)*(x38)))+(((x23)*(x64)))+(((IkReal(0.0917629081396138))*(x0)))+(((x20)*(x63)))+(((x12)*(x53)*(x6)))+(((x23)*(x73)))+(((x18)*(x56)))+(((x18)*(x49)))+(((IkReal(-0.226992456976939))*(x42)*(x7)))+(((IkReal(-0.241481337209510))*(x38)))+(((x20)*(x71)))+(((IkReal(-0.0647052067418602))*(x1)))+(((x12)*(x48)))+(((IkReal(-1.00000000000000))*(x24)*(x35)))+(((IkReal(0.0608228943373486))*(x37)))+(((x24)*(x37)))+(((IkReal(0.00776462480902323))*(x42)))+(((IkReal(0.00776462480902323))*(x40))));
00302 eerot[6]=((((x20)*(x47)))+(((IkReal(-1.00000000000000))*(x18)*(x69)))+(((IkReal(-1.00000000000000))*(x12)*(x62)))+(((x23)*(x58))));
00303 eerot[7]=((((x17)*(x75)))+(((x29)*(x5)*(x50)))+(((IkReal(-1.00000000000000))*(x31)*(x70)))+(((IkReal(-1.00000000000000))*(x13)*(x78))));
00304 eerot[8]=((((x17)*(x57)))+(((x29)*(x75)))+(((IkReal(-1.00000000000000))*(x31)*(x78)))+(((IkReal(-1.00000000000000))*(x13)*(x72))));
00305 IkReal x82=((IkReal(1.00000000000000))*(x12));
00306 IkReal x83=((IkReal(1.00000000000000))*(x18));
00307 eetrans[2]=((IkReal(0.370300000000000))+(((x20)*(x48)))+(((IkReal(-0.226992456976939))*(x35)))+(((IkReal(-1.00000000000000))*(x73)*(x83)))+(((x20)*(x53)*(x6)))+(((IkReal(-1.00000000000000))*(x71)*(x82)))+(((IkReal(-1.00000000000000))*(x63)*(x82)))+(((IkReal(0.0289777604651412))*(x42)))+(((IkReal(0.0289777604651412))*(x40)))+(((x23)*(x49)))+(((IkReal(0.226992456976939))*(x37)))+(((IkReal(0.0647052067418602))*(x38)))+(((x22)*(x35)))+(((IkReal(-1.00000000000000))*(x22)*(x37)))+(((IkReal(-0.0245879785619069))*(x0)))+(((IkReal(-0.241481337209510))*(x1)))+(((IkReal(0.0608228943373486))*(x42)*(x7)))+(((IkReal(0.0608228943373486))*(x3)*(x38)))+(((IkReal(-1.00000000000000))*(x64)*(x83)))+(((x23)*(x56))));
00308 }
00309
00310 IKFAST_API int GetNumFreeParameters() { return 0; }
00311 IKFAST_API int* GetFreeParameters() { return NULL; }
00312 IKFAST_API int GetNumJoints() { return 6; }
00313
00314 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00315
00316 IKFAST_API int GetIkType() { return 0x67000001; }
00317
00318 class IKSolver {
00319 public:
00320 IkReal j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j7,cj7,sj7,htj7,j8,cj8,sj8,htj8,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;
00321 unsigned char _ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij7[2], _nj7,_ij8[2], _nj8;
00322
00323 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00324 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; j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; j7=numeric_limits<IkReal>::quiet_NaN(); _ij7[0] = -1; _ij7[1] = -1; _nj7 = -1; j8=numeric_limits<IkReal>::quiet_NaN(); _ij8[0] = -1; _ij8[1] = -1; _nj8 = -1;
00325 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00326 solutions.Clear();
00327 r00 = eerot[0*3+0];
00328 r01 = eerot[0*3+1];
00329 r02 = eerot[0*3+2];
00330 r10 = eerot[1*3+0];
00331 r11 = eerot[1*3+1];
00332 r12 = eerot[1*3+2];
00333 r20 = eerot[2*3+0];
00334 r21 = eerot[2*3+1];
00335 r22 = eerot[2*3+2];
00336 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00337
00338 new_r00=((IkReal(-1.00000000000000))*(r02));
00339 new_r01=r01;
00340 new_r02=r00;
00341 new_px=((px)+(((IkReal(0.0470000000000000))*(r00))));
00342 new_r10=((((IkReal(-0.965925348838040))*(r12)))+(((IkReal(0.258820826967441))*(r22))));
00343 new_r11=((((IkReal(0.965925348838040))*(r11)))+(((IkReal(-0.258820826967441))*(r21))));
00344 new_r12=((((IkReal(0.965925348838040))*(r10)))+(((IkReal(-0.258820826967441))*(r20))));
00345 new_py=((IkReal(-0.0442178233554725))+(((IkReal(0.965925348838040))*(py)))+(((IkReal(0.0453984913953879))*(r10)))+(((IkReal(-0.0121645788674697))*(r20)))+(((IkReal(-0.258820826967441))*(pz))));
00346 new_r20=((((IkReal(-0.258820826967441))*(r12)))+(((IkReal(-0.965925348838040))*(r22))));
00347 new_r21=((((IkReal(0.965925348838040))*(r21)))+(((IkReal(0.258820826967441))*(r11))));
00348 new_r22=((((IkReal(0.965925348838040))*(r20)))+(((IkReal(0.258820826967441))*(r10))));
00349 new_pz=((IkReal(-0.395211176585005))+(((IkReal(0.965925348838040))*(pz)))+(((IkReal(0.258820826967441))*(py)))+(((IkReal(0.0121645788674697))*(r10)))+(((IkReal(0.0453984913953879))*(r20))));
00350 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;
00351 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00352 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00353 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00354 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00355 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00356 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00357 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00358 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00359 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00360 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00361 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00362 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00363 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00364 IkReal op[72], zeror[48];
00365 int numroots;
00366 IkReal x84=((IkReal(0.650000000000000))*(npx));
00367 IkReal x85=((IkReal(0.0600000000000000))*(npz));
00368 IkReal x86=((IkReal(1.00000000000000))*(pp));
00369 IkReal x87=((IkReal(0.0600000000000000))*(npy));
00370 IkReal x88=((IkReal(0.0950000000000000))*(r22));
00371 IkReal x89=((IkReal(0.120000000000000))*(npx));
00372 IkReal x90=((IkReal(1.00000000000000))*(rxp0_2));
00373 IkReal x91=((IkReal(2.00000000000000))*(rxp1_2));
00374 IkReal x92=((IkReal(0.0950000000000000))*(r21));
00375 IkReal x93=((IkReal(0.940000000000000))*(npz));
00376 IkReal x94=((IkReal(0.190000000000000))*(r20));
00377 IkReal x95=((IkReal(2.00000000000000))*(rxp2_2));
00378 IkReal x96=((IkReal(0.180000000000000))*(r21));
00379 IkReal x97=((IkReal(0.360000000000000))*(r20));
00380 IkReal x98=((IkReal(0.380000000000000))*(r21));
00381 IkReal x99=((IkReal(0.290000000000000))*(npx));
00382 IkReal x100=((IkReal(0.00570000000000000))+(((IkReal(-1.00000000000000))*(x87))));
00383 IkReal x101=((rxp0_2)+(((IkReal(-1.00000000000000))*(x88))));
00384 IkReal x102=((IkReal(0.580000000000000))*(npy));
00385 IkReal x103=((IkReal(-0.00570000000000000))+(((IkReal(-1.00000000000000))*(x87))));
00386 IkReal x104=((IkReal(-1.30000000000000))*(npy));
00387 IkReal x105=((IkReal(-0.0950000000000000))*(r21));
00388 IkReal x106=((IkReal(-0.190000000000000))*(r20));
00389 IkReal x107=((IkReal(0.00570000000000000))+(x87));
00390 IkReal x108=((IkReal(-0.00570000000000000))+(x87));
00391 IkReal x109=((rxp0_2)+(x88));
00392 IkReal x110=((IkReal(-2.00000000000000))*(rxp1_2));
00393 IkReal x111=((x88)+(((IkReal(-1.00000000000000))*(x90))));
00394 IkReal x112=((IkReal(-0.0350000000000000))+(x84));
00395 IkReal x113=((IkReal(0.0496000000000000))+(x85));
00396 IkReal x114=((((IkReal(-1.00000000000000))*(x90)))+(((IkReal(-1.00000000000000))*(x88))));
00397 IkReal x115=((x86)+(x85));
00398 IkReal x116=((x89)+(x93));
00399 IkReal x117=((x95)+(x96));
00400 op[0]=((((IkReal(-1.00000000000000))*(x115)))+(x112));
00401 op[1]=x107;
00402 op[2]=x104;
00403 op[3]=x89;
00404 op[4]=((IkReal(-0.0350000000000000))+(((IkReal(-1.00000000000000))*(x115)))+(((IkReal(-1.00000000000000))*(x84))));
00405 op[5]=x100;
00406 op[6]=x111;
00407 op[7]=x105;
00408 op[8]=x91;
00409 op[9]=x106;
00410 op[10]=x109;
00411 op[11]=x92;
00412 op[12]=x108;
00413 op[13]=((x112)+(x85)+(((IkReal(-1.00000000000000))*(x86))));
00414 op[14]=x89;
00415 op[15]=x104;
00416 op[16]=x103;
00417 op[17]=((IkReal(-0.0350000000000000))+(x85)+(((IkReal(-1.00000000000000))*(x84)))+(((IkReal(-1.00000000000000))*(x86))));
00418 op[18]=x105;
00419 op[19]=x114;
00420 op[20]=x106;
00421 op[21]=x91;
00422 op[22]=x92;
00423 op[23]=x101;
00424 op[24]=((IkReal(-0.0108000000000000))+(x116));
00425 op[25]=IkReal(0);
00426 op[26]=((IkReal(-0.240000000000000))*(npy));
00427 op[27]=IkReal(0);
00428 op[28]=((IkReal(-0.0108000000000000))+(x93)+(((IkReal(-1.00000000000000))*(x89))));
00429 op[29]=IkReal(0);
00430 op[30]=((x96)+(((IkReal(-1.00000000000000))*(x94)))+(((IkReal(-1.00000000000000))*(x95))));
00431 op[31]=IkReal(0);
00432 op[32]=((x98)+(x97));
00433 op[33]=IkReal(0);
00434 op[34]=((((IkReal(-1.00000000000000))*(x117)))+(x94));
00435 op[35]=IkReal(0);
00436 op[36]=IkReal(0);
00437 op[37]=((IkReal(0.0108000000000000))+(x93)+(((IkReal(-1.00000000000000))*(x89))));
00438 op[38]=IkReal(0);
00439 op[39]=((IkReal(0.240000000000000))*(npy));
00440 op[40]=IkReal(0);
00441 op[41]=((IkReal(0.0108000000000000))+(x116));
00442 op[42]=IkReal(0);
00443 op[43]=((x94)+(x96)+(((IkReal(-1.00000000000000))*(x95))));
00444 op[44]=IkReal(0);
00445 op[45]=((x97)+(((IkReal(-1.00000000000000))*(x98))));
00446 op[46]=IkReal(0);
00447 op[47]=((((IkReal(-1.00000000000000))*(x117)))+(((IkReal(-1.00000000000000))*(x94))));
00448 op[48]=((x113)+(((IkReal(-1.00000000000000))*(x99)))+(((IkReal(-1.00000000000000))*(x86))));
00449 op[49]=x107;
00450 op[50]=x102;
00451 op[51]=x89;
00452 op[52]=((x113)+(x99)+(((IkReal(-1.00000000000000))*(x86))));
00453 op[53]=x100;
00454 op[54]=x101;
00455 op[55]=x105;
00456 op[56]=x110;
00457 op[57]=x106;
00458 op[58]=x114;
00459 op[59]=x92;
00460 op[60]=x108;
00461 op[61]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x115)))+(((IkReal(-1.00000000000000))*(x99))));
00462 op[62]=x89;
00463 op[63]=x102;
00464 op[64]=x103;
00465 op[65]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x115)))+(x99));
00466 op[66]=x105;
00467 op[67]=x109;
00468 op[68]=x106;
00469 op[69]=x110;
00470 op[70]=x92;
00471 op[71]=x111;
00472 solvedialyticpoly8qep(op,zeror,numroots);
00473 IkReal j7array[16], cj7array[16], sj7array[16], j8array[16], cj8array[16], sj8array[16], j6array[16], cj6array[16], sj6array[16];
00474 int numsolutions = 0;
00475 for(int ij7 = 0; ij7 < numroots; ij7 += 3)
00476 {
00477 IkReal htj7 = zeror[ij7+0], htj8 = zeror[ij7+1], htj6 = zeror[ij7+2];
00478 j7array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj7)));
00479 j8array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj8)));
00480 j6array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj6)));
00481 IkReal x118=(htj7)*(htj7);
00482 IkReal x119=(htj8)*(htj8);
00483 IkReal x120=(htj6)*(htj6);
00484 cj7array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x118))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x118)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x118))))));
00485 cj8array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x119))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x119)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x119))))));
00486 cj6array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x120))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x120)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x120))))));
00487 sj7array[numsolutions]=((IkReal(2.00000000000000))*(htj7)*(((IKabs(((IkReal(1.00000000000000))+((htj7)*(htj7)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj7)*(htj7))))):(IkReal)1.0e30)));
00488 sj8array[numsolutions]=((IkReal(2.00000000000000))*(htj8)*(((IKabs(((IkReal(1.00000000000000))+((htj8)*(htj8)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj8)*(htj8))))):(IkReal)1.0e30)));
00489 sj6array[numsolutions]=((IkReal(2.00000000000000))*(htj6)*(((IKabs(((IkReal(1.00000000000000))+((htj6)*(htj6)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj6)*(htj6))))):(IkReal)1.0e30)));
00490 if( j7array[numsolutions] > IKPI )
00491 {
00492 j7array[numsolutions]-=IK2PI;
00493 }
00494 else if( j7array[numsolutions] < -IKPI )
00495 {
00496 j7array[numsolutions]+=IK2PI;
00497 }
00498 if( j8array[numsolutions] > IKPI )
00499 {
00500 j8array[numsolutions]-=IK2PI;
00501 }
00502 else if( j8array[numsolutions] < -IKPI )
00503 {
00504 j8array[numsolutions]+=IK2PI;
00505 }
00506 if( j6array[numsolutions] > IKPI )
00507 {
00508 j6array[numsolutions]-=IK2PI;
00509 }
00510 else if( j6array[numsolutions] < -IKPI )
00511 {
00512 j6array[numsolutions]+=IK2PI;
00513 }
00514 numsolutions++;
00515 }
00516 bool j7valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
00517 _nj7 = 16;
00518 _nj8 = 1;
00519 _nj6 = 1;
00520 for(int ij7 = 0; ij7 < numsolutions; ++ij7)
00521 {
00522 if( !j7valid[ij7] )
00523 {
00524 continue;
00525 }
00526 _ij7[0] = ij7; _ij7[1] = -1;
00527 _ij8[0] = 0; _ij8[1] = -1;
00528 _ij6[0] = 0; _ij6[1] = -1;
00529 for(int iij7 = ij7+1; iij7 < numsolutions; ++iij7)
00530 {
00531 if( !j7valid[iij7] ) { continue; }
00532 if( IKabs(cj7array[ij7]-cj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj7array[ij7]-sj7array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(cj8array[ij7]-cj8array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj8array[ij7]-sj8array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(cj6array[ij7]-cj6array[iij7]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij7]-sj6array[iij7]) < IKFAST_SOLUTION_THRESH && 1 )
00533 {
00534 j7valid[iij7]=false; _ij7[1] = iij7; _ij8[1] = 0; _ij6[1] = 0; break;
00535 }
00536 }
00537 j7 = j7array[ij7]; cj7 = cj7array[ij7]; sj7 = sj7array[ij7];
00538
00539 j8 = j8array[ij7]; cj8 = cj8array[ij7]; sj8 = sj8array[ij7];
00540
00541 j6 = j6array[ij7]; cj6 = cj6array[ij7]; sj6 = sj6array[ij7];
00542
00543 {
00544 IkReal dummyeval[1];
00545 IkReal gconst0;
00546 IkReal x121=(sj8)*(sj8);
00547 IkReal x122=(cj8)*(cj8);
00548 IkReal x123=((r00)*(r11));
00549 IkReal x124=((r02)*(sj7));
00550 IkReal x125=((cj7)*(x122));
00551 IkReal x126=((IkReal(1.00000000000000))*(r12)*(sj7));
00552 IkReal x127=((IkReal(1.00000000000000))*(r01)*(r10));
00553 IkReal x128=((cj7)*(x121));
00554 gconst0=IKsign(((((IkReal(-1.00000000000000))*(r00)*(sj8)*(x126)))+(((r10)*(sj8)*(x124)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x126)))+(((x123)*(x125)))+(((x123)*(x128)))+(((IkReal(-1.00000000000000))*(x125)*(x127)))+(((cj8)*(r11)*(x124)))+(((IkReal(-1.00000000000000))*(x127)*(x128)))));
00555 IkReal x129=(sj8)*(sj8);
00556 IkReal x130=(cj8)*(cj8);
00557 IkReal x131=((r00)*(r11));
00558 IkReal x132=((r02)*(sj7));
00559 IkReal x133=((cj7)*(x130));
00560 IkReal x134=((IkReal(1.00000000000000))*(r12)*(sj7));
00561 IkReal x135=((IkReal(1.00000000000000))*(r01)*(r10));
00562 IkReal x136=((cj7)*(x129));
00563 dummyeval[0]=((((IkReal(-1.00000000000000))*(x135)*(x136)))+(((r10)*(sj8)*(x132)))+(((IkReal(-1.00000000000000))*(x133)*(x135)))+(((x131)*(x133)))+(((x131)*(x136)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x134)))+(((cj8)*(r11)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x134))));
00564 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00565 {
00566 {
00567 IkReal dummyeval[1];
00568 IkReal gconst1;
00569 IkReal x137=(sj8)*(sj8);
00570 IkReal x138=(cj8)*(cj8);
00571 IkReal x139=((IkReal(1.00000000000000))*(r10));
00572 IkReal x140=((cj7)*(sj8));
00573 IkReal x141=((r00)*(r11));
00574 IkReal x142=((cj7)*(cj8));
00575 IkReal x143=((sj7)*(x137));
00576 IkReal x144=((sj7)*(x138));
00577 gconst1=IKsign(((((r01)*(r12)*(x142)))+(((IkReal(-1.00000000000000))*(r02)*(x139)*(x140)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x142)))+(((x141)*(x144)))+(((x141)*(x143)))+(((r00)*(r12)*(x140)))+(((IkReal(-1.00000000000000))*(r01)*(x139)*(x143)))+(((IkReal(-1.00000000000000))*(r01)*(x139)*(x144)))));
00578 IkReal x145=(sj8)*(sj8);
00579 IkReal x146=(cj8)*(cj8);
00580 IkReal x147=((IkReal(1.00000000000000))*(r10));
00581 IkReal x148=((cj7)*(sj8));
00582 IkReal x149=((r00)*(r11));
00583 IkReal x150=((cj7)*(cj8));
00584 IkReal x151=((sj7)*(x145));
00585 IkReal x152=((sj7)*(x146));
00586 dummyeval[0]=((((x149)*(x152)))+(((x149)*(x151)))+(((IkReal(-1.00000000000000))*(r02)*(x147)*(x148)))+(((IkReal(-1.00000000000000))*(r01)*(x147)*(x152)))+(((IkReal(-1.00000000000000))*(r01)*(x147)*(x151)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x150)))+(((r00)*(r12)*(x148)))+(((r01)*(r12)*(x150))));
00587 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00588 {
00589 {
00590 IkReal dummyeval[1];
00591 dummyeval[0]=sj6;
00592 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00593 {
00594 {
00595 IkReal evalcond[3];
00596 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j6)), IkReal(6.28318530717959))));
00597 evalcond[1]=((IkReal(-0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
00598 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
00599 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
00600 {
00601 {
00602 IkReal j5array[1], cj5array[1], sj5array[1];
00603 bool j5valid[1]={false};
00604 _nj5 = 1;
00605 IkReal x153=((IkReal(4.00000000000000))*(sj7));
00606 IkReal x154=((npy)*(sj8));
00607 IkReal x155=((IkReal(4.00000000000000))*(cj7));
00608 IkReal x156=((cj8)*(npx));
00609 if( IKabs(((IkReal(0.120000000000000))+(((x153)*(x154)))+(((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x153)*(x156)))+(((npz)*(x155))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x154)*(x155)))+(((x155)*(x156)))+(((npz)*(x153)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((x153)*(x154)))+(((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x153)*(x156)))+(((npz)*(x155)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x154)*(x155)))+(((x155)*(x156)))+(((npz)*(x153)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
00610 continue;
00611 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((x153)*(x154)))+(((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x153)*(x156)))+(((npz)*(x155)))), ((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x154)*(x155)))+(((x155)*(x156)))+(((npz)*(x153)))+(((IkReal(-0.360000000000000))*(cj7)))));
00612 sj5array[0]=IKsin(j5array[0]);
00613 cj5array[0]=IKcos(j5array[0]);
00614 if( j5array[0] > IKPI )
00615 {
00616 j5array[0]-=IK2PI;
00617 }
00618 else if( j5array[0] < -IKPI )
00619 { j5array[0]+=IK2PI;
00620 }
00621 j5valid[0] = true;
00622 for(int ij5 = 0; ij5 < 1; ++ij5)
00623 {
00624 if( !j5valid[ij5] )
00625 {
00626 continue;
00627 }
00628 _ij5[0] = ij5; _ij5[1] = -1;
00629 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
00630 {
00631 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
00632 {
00633 j5valid[iij5]=false; _ij5[1] = iij5; break;
00634 }
00635 }
00636 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00637 {
00638 IkReal evalcond[2];
00639 IkReal x157=((IkReal(1.00000000000000))*(sj7));
00640 IkReal x158=((npy)*(sj8));
00641 IkReal x159=((cj8)*(npx));
00642 evalcond[0]=((IkReal(0.235000000000000))+(((cj7)*(x158)))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(x157)))+(((IkReal(-1.00000000000000))*(cj7)*(x159)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
00643 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x157)*(x159)))+(((sj7)*(x158)))+(((IkReal(-0.250000000000000))*(IKsin(j5))))+(((cj7)*(npz))));
00644 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
00645 {
00646 continue;
00647 }
00648 }
00649
00650 {
00651 IkReal dummyeval[1];
00652 IkReal gconst66;
00653 IkReal x160=(sj8)*(sj8);
00654 IkReal x161=(cj8)*(cj8);
00655 IkReal x162=((IkReal(2.00000000000000))*(cj8)*(sj8));
00656 gconst66=IKsign(((((x160)*((r00)*(r00))))+(((x161)*((r01)*(r01))))+(((x160)*((r10)*(r10))))+(((x161)*((r11)*(r11))))+(((r10)*(r11)*(x162)))+(((r00)*(r01)*(x162)))));
00657 IkReal x163=(sj8)*(sj8);
00658 IkReal x164=(cj8)*(cj8);
00659 IkReal x165=((IkReal(2.00000000000000))*(cj8)*(sj8));
00660 dummyeval[0]=((((x164)*((r01)*(r01))))+(((x163)*((r00)*(r00))))+(((x164)*((r11)*(r11))))+(((x163)*((r10)*(r10))))+(((r10)*(r11)*(x165)))+(((r00)*(r01)*(x165))));
00661 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00662 {
00663 {
00664 IkReal dummyeval[1];
00665 IkReal gconst68;
00666 gconst68=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
00667 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
00668 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00669 {
00670 {
00671 IkReal dummyeval[1];
00672 IkReal gconst67;
00673 IkReal x166=(sj8)*(sj8);
00674 IkReal x167=(cj8)*(cj8);
00675 IkReal x168=((r00)*(r11));
00676 IkReal x169=((r02)*(sj7));
00677 IkReal x170=((cj7)*(x167));
00678 IkReal x171=((IkReal(1.00000000000000))*(r12)*(sj7));
00679 IkReal x172=((IkReal(1.00000000000000))*(r01)*(r10));
00680 IkReal x173=((cj7)*(x166));
00681 gconst67=IKsign(((((cj8)*(r11)*(x169)))+(((x168)*(x173)))+(((x168)*(x170)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x171)))+(((IkReal(-1.00000000000000))*(x170)*(x172)))+(((r10)*(sj8)*(x169)))+(((IkReal(-1.00000000000000))*(x172)*(x173)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x171)))));
00682 IkReal x174=(sj8)*(sj8);
00683 IkReal x175=(cj8)*(cj8);
00684 IkReal x176=((r00)*(r11));
00685 IkReal x177=((r02)*(sj7));
00686 IkReal x178=((cj7)*(x175));
00687 IkReal x179=((IkReal(1.00000000000000))*(r12)*(sj7));
00688 IkReal x180=((IkReal(1.00000000000000))*(r01)*(r10));
00689 IkReal x181=((cj7)*(x174));
00690 dummyeval[0]=((((cj8)*(r11)*(x177)))+(((IkReal(-1.00000000000000))*(x178)*(x180)))+(((x176)*(x181)))+(((x176)*(x178)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x179)))+(((IkReal(-1.00000000000000))*(x180)*(x181)))+(((r10)*(sj8)*(x177)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x179))));
00691 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00692 {
00693 continue;
00694
00695 } else
00696 {
00697 {
00698 IkReal j3array[1], cj3array[1], sj3array[1];
00699 bool j3valid[1]={false};
00700 _nj3 = 1;
00701 IkReal x182=((cj7)*(cj8));
00702 IkReal x183=((IkReal(1.00000000000000))*(cj7)*(sj8));
00703 if( IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x183)))+(((r12)*(sj7)))+(((r10)*(x182))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(r01)*(x183)))+(((r00)*(x182)))+(((r02)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH )
00704 continue;
00705 j3array[0]=IKatan2(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x183)))+(((r12)*(sj7)))+(((r10)*(x182)))))), ((gconst67)*(((((IkReal(-1.00000000000000))*(r01)*(x183)))+(((r00)*(x182)))+(((r02)*(sj7)))))));
00706 sj3array[0]=IKsin(j3array[0]);
00707 cj3array[0]=IKcos(j3array[0]);
00708 if( j3array[0] > IKPI )
00709 {
00710 j3array[0]-=IK2PI;
00711 }
00712 else if( j3array[0] < -IKPI )
00713 { j3array[0]+=IK2PI;
00714 }
00715 j3valid[0] = true;
00716 for(int ij3 = 0; ij3 < 1; ++ij3)
00717 {
00718 if( !j3valid[ij3] )
00719 {
00720 continue;
00721 }
00722 _ij3[0] = ij3; _ij3[1] = -1;
00723 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
00724 {
00725 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
00726 {
00727 j3valid[iij3]=false; _ij3[1] = iij3; break;
00728 }
00729 }
00730 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00731 {
00732 IkReal evalcond[4];
00733 IkReal x184=IKsin(j3);
00734 IkReal x185=IKcos(j3);
00735 IkReal x186=((IkReal(1.00000000000000))*(sj8));
00736 IkReal x187=((IkReal(1.00000000000000))*(cj8));
00737 IkReal x188=((r01)*(sj8));
00738 IkReal x189=((cj8)*(r10));
00739 IkReal x190=((sj7)*(x185));
00740 IkReal x191=((cj7)*(x184));
00741 IkReal x192=((r00)*(x184));
00742 IkReal x193=((sj7)*(x184));
00743 IkReal x194=((cj7)*(x185));
00744 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x185)*(x186)))+(((sj8)*(x192)))+(((cj8)*(r01)*(x184)))+(((IkReal(-1.00000000000000))*(r11)*(x185)*(x187))));
00745 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x185)*(x186)))+(((IkReal(-1.00000000000000))*(r10)*(x184)*(x186)))+(((IkReal(-1.00000000000000))*(r11)*(x184)*(x187)))+(((IkReal(-1.00000000000000))*(r01)*(x185)*(x187))));
00746 evalcond[2]=((((x188)*(x191)))+(((x189)*(x194)))+(((r12)*(x190)))+(((IkReal(-1.00000000000000))*(r11)*(x186)*(x194)))+(((IkReal(-1.00000000000000))*(r00)*(x187)*(x191)))+(((IkReal(-1.00000000000000))*(r02)*(x193))));
00747 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(x194)))+(((x188)*(x193)))+(((x189)*(x190)))+(((IkReal(-1.00000000000000))*(sj7)*(x187)*(x192)))+(((IkReal(-1.00000000000000))*(r11)*(x186)*(x190)))+(((r02)*(x191))));
00748 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00749 {
00750 continue;
00751 }
00752 }
00753
00754 {
00755 IkReal dummyeval[1];
00756 IkReal gconst69;
00757 gconst69=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
00758 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
00759 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00760 {
00761 {
00762 IkReal dummyeval[1];
00763 IkReal gconst70;
00764 gconst70=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
00765 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
00766 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00767 {
00768 continue;
00769
00770 } else
00771 {
00772 {
00773 IkReal j4array[1], cj4array[1], sj4array[1];
00774 bool j4valid[1]={false};
00775 _nj4 = 1;
00776 IkReal x195=((cj3)*(cj5));
00777 IkReal x196=((r02)*(sj7));
00778 IkReal x197=((cj8)*(r00));
00779 IkReal x198=((r22)*(sj7));
00780 IkReal x199=((cj5)*(sj3));
00781 IkReal x200=((cj7)*(cj8));
00782 IkReal x201=((IkReal(1.00000000000000))*(sj5));
00783 IkReal x202=((sj3)*(sj5));
00784 IkReal x203=((r12)*(sj7));
00785 IkReal x204=((cj3)*(cj7)*(sj5));
00786 IkReal x205=((IkReal(1.00000000000000))*(cj7)*(sj8));
00787 if( IKabs(((gconst70)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x200)*(x201)))+(((IkReal(-1.00000000000000))*(r11)*(x199)*(x205)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x198)*(x201)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x205)))+(((cj7)*(x195)*(x197)))+(((r10)*(x199)*(x200)))+(((x199)*(x203))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((cj5)*(x198)))+(((r10)*(x200)*(x202)))+(((cj3)*(sj5)*(x196)))+(((cj5)*(r20)*(x200)))+(((x197)*(x204)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x201)))+(((x202)*(x203)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x201)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x205))))))) < IKFAST_ATAN2_MAGTHRESH )
00788 continue;
00789 j4array[0]=IKatan2(((gconst70)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x200)*(x201)))+(((IkReal(-1.00000000000000))*(r11)*(x199)*(x205)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x198)*(x201)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x205)))+(((cj7)*(x195)*(x197)))+(((r10)*(x199)*(x200)))+(((x199)*(x203)))))), ((gconst70)*(((((cj5)*(x198)))+(((r10)*(x200)*(x202)))+(((cj3)*(sj5)*(x196)))+(((cj5)*(r20)*(x200)))+(((x197)*(x204)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x201)))+(((x202)*(x203)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x201)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x205)))))));
00790 sj4array[0]=IKsin(j4array[0]);
00791 cj4array[0]=IKcos(j4array[0]);
00792 if( j4array[0] > IKPI )
00793 {
00794 j4array[0]-=IK2PI;
00795 }
00796 else if( j4array[0] < -IKPI )
00797 { j4array[0]+=IK2PI;
00798 }
00799 j4valid[0] = true;
00800 for(int ij4 = 0; ij4 < 1; ++ij4)
00801 {
00802 if( !j4valid[ij4] )
00803 {
00804 continue;
00805 }
00806 _ij4[0] = ij4; _ij4[1] = -1;
00807 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
00808 {
00809 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00810 {
00811 j4valid[iij4]=false; _ij4[1] = iij4; break;
00812 }
00813 }
00814 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00815 {
00816 IkReal evalcond[4];
00817 IkReal x206=IKcos(j4);
00818 IkReal x207=IKsin(j4);
00819 IkReal x208=((IkReal(1.00000000000000))*(cj7));
00820 IkReal x209=((cj8)*(r20));
00821 IkReal x210=((cj3)*(r02));
00822 IkReal x211=((IkReal(1.00000000000000))*(sj7));
00823 IkReal x212=((sj3)*(sj7));
00824 IkReal x213=((r21)*(sj8));
00825 IkReal x214=((cj8)*(r10));
00826 IkReal x215=((sj5)*(x206));
00827 IkReal x216=((cj5)*(x207));
00828 IkReal x217=((r11)*(sj3)*(sj8));
00829 IkReal x218=((cj3)*(cj8)*(r00));
00830 IkReal x219=((cj5)*(x206));
00831 IkReal x220=((cj3)*(r01)*(sj8));
00832 IkReal x221=((sj5)*(x207));
00833 IkReal x222=((x215)+(x216));
00834 evalcond[0]=((((cj7)*(x213)))+(((IkReal(-1.00000000000000))*(r22)*(x211)))+(((IkReal(-1.00000000000000))*(x208)*(x209)))+(x221)+(((IkReal(-1.00000000000000))*(x219))));
00835 evalcond[1]=((x222)+(((sj7)*(x213)))+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x209)*(x211))));
00836 evalcond[2]=((((cj7)*(sj3)*(x214)))+(((cj7)*(x218)))+(((r12)*(x212)))+(((IkReal(-1.00000000000000))*(x208)*(x220)))+(x222)+(((sj7)*(x210)))+(((IkReal(-1.00000000000000))*(x208)*(x217))));
00837 evalcond[3]=((x219)+(((sj7)*(x218)))+(((x212)*(x214)))+(((IkReal(-1.00000000000000))*(x211)*(x217)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x208)))+(((IkReal(-1.00000000000000))*(x211)*(x220)))+(((IkReal(-1.00000000000000))*(x208)*(x210)))+(((IkReal(-1.00000000000000))*(x221))));
00838 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00839 {
00840 continue;
00841 }
00842 }
00843
00844 {
00845 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00846 vinfos[0].jointtype = 1;
00847 vinfos[0].foffset = j3;
00848 vinfos[0].indices[0] = _ij3[0];
00849 vinfos[0].indices[1] = _ij3[1];
00850 vinfos[0].maxsolutions = _nj3;
00851 vinfos[1].jointtype = 1;
00852 vinfos[1].foffset = j4;
00853 vinfos[1].indices[0] = _ij4[0];
00854 vinfos[1].indices[1] = _ij4[1];
00855 vinfos[1].maxsolutions = _nj4;
00856 vinfos[2].jointtype = 1;
00857 vinfos[2].foffset = j5;
00858 vinfos[2].indices[0] = _ij5[0];
00859 vinfos[2].indices[1] = _ij5[1];
00860 vinfos[2].maxsolutions = _nj5;
00861 vinfos[3].jointtype = 1;
00862 vinfos[3].foffset = j6;
00863 vinfos[3].indices[0] = _ij6[0];
00864 vinfos[3].indices[1] = _ij6[1];
00865 vinfos[3].maxsolutions = _nj6;
00866 vinfos[4].jointtype = 1;
00867 vinfos[4].foffset = j7;
00868 vinfos[4].indices[0] = _ij7[0];
00869 vinfos[4].indices[1] = _ij7[1];
00870 vinfos[4].maxsolutions = _nj7;
00871 vinfos[5].jointtype = 1;
00872 vinfos[5].foffset = j8;
00873 vinfos[5].indices[0] = _ij8[0];
00874 vinfos[5].indices[1] = _ij8[1];
00875 vinfos[5].maxsolutions = _nj8;
00876 std::vector<int> vfree(0);
00877 solutions.AddSolution(vinfos,vfree);
00878 }
00879 }
00880 }
00881
00882 }
00883
00884 }
00885
00886 } else
00887 {
00888 {
00889 IkReal j4array[1], cj4array[1], sj4array[1];
00890 bool j4valid[1]={false};
00891 _nj4 = 1;
00892 IkReal x223=((r21)*(sj8));
00893 IkReal x224=((sj5)*(sj7));
00894 IkReal x225=((cj5)*(sj7));
00895 IkReal x226=((cj5)*(cj7));
00896 IkReal x227=((cj8)*(r20));
00897 IkReal x228=((cj7)*(sj5));
00898 if( IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(r22)*(x224)))+(((x223)*(x225)))+(((x223)*(x228)))+(((IkReal(-1.00000000000000))*(x225)*(x227)))+(((IkReal(-1.00000000000000))*(x227)*(x228)))+(((r22)*(x226))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((x223)*(x224)))+(((x226)*(x227)))+(((IkReal(-1.00000000000000))*(x224)*(x227)))+(((r22)*(x228)))+(((r22)*(x225)))+(((IkReal(-1.00000000000000))*(x223)*(x226))))))) < IKFAST_ATAN2_MAGTHRESH )
00899 continue;
00900 j4array[0]=IKatan2(((gconst69)*(((((IkReal(-1.00000000000000))*(r22)*(x224)))+(((x223)*(x225)))+(((x223)*(x228)))+(((IkReal(-1.00000000000000))*(x225)*(x227)))+(((IkReal(-1.00000000000000))*(x227)*(x228)))+(((r22)*(x226)))))), ((gconst69)*(((((x223)*(x224)))+(((x226)*(x227)))+(((IkReal(-1.00000000000000))*(x224)*(x227)))+(((r22)*(x228)))+(((r22)*(x225)))+(((IkReal(-1.00000000000000))*(x223)*(x226)))))));
00901 sj4array[0]=IKsin(j4array[0]);
00902 cj4array[0]=IKcos(j4array[0]);
00903 if( j4array[0] > IKPI )
00904 {
00905 j4array[0]-=IK2PI;
00906 }
00907 else if( j4array[0] < -IKPI )
00908 { j4array[0]+=IK2PI;
00909 }
00910 j4valid[0] = true;
00911 for(int ij4 = 0; ij4 < 1; ++ij4)
00912 {
00913 if( !j4valid[ij4] )
00914 {
00915 continue;
00916 }
00917 _ij4[0] = ij4; _ij4[1] = -1;
00918 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
00919 {
00920 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
00921 {
00922 j4valid[iij4]=false; _ij4[1] = iij4; break;
00923 }
00924 }
00925 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00926 {
00927 IkReal evalcond[4];
00928 IkReal x229=IKcos(j4);
00929 IkReal x230=IKsin(j4);
00930 IkReal x231=((IkReal(1.00000000000000))*(cj7));
00931 IkReal x232=((cj8)*(r20));
00932 IkReal x233=((cj3)*(r02));
00933 IkReal x234=((IkReal(1.00000000000000))*(sj7));
00934 IkReal x235=((sj3)*(sj7));
00935 IkReal x236=((r21)*(sj8));
00936 IkReal x237=((cj8)*(r10));
00937 IkReal x238=((sj5)*(x229));
00938 IkReal x239=((cj5)*(x230));
00939 IkReal x240=((r11)*(sj3)*(sj8));
00940 IkReal x241=((cj3)*(cj8)*(r00));
00941 IkReal x242=((cj5)*(x229));
00942 IkReal x243=((cj3)*(r01)*(sj8));
00943 IkReal x244=((sj5)*(x230));
00944 IkReal x245=((x238)+(x239));
00945 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x234)))+(((IkReal(-1.00000000000000))*(x242)))+(x244)+(((IkReal(-1.00000000000000))*(x231)*(x232)))+(((cj7)*(x236))));
00946 evalcond[1]=((x245)+(((IkReal(-1.00000000000000))*(x232)*(x234)))+(((cj7)*(r22)))+(((sj7)*(x236))));
00947 evalcond[2]=((((cj7)*(x241)))+(((IkReal(-1.00000000000000))*(x231)*(x243)))+(((IkReal(-1.00000000000000))*(x231)*(x240)))+(x245)+(((cj7)*(sj3)*(x237)))+(((sj7)*(x233)))+(((r12)*(x235))));
00948 evalcond[3]=((((IkReal(-1.00000000000000))*(x244)))+(((IkReal(-1.00000000000000))*(x234)*(x240)))+(((IkReal(-1.00000000000000))*(x234)*(x243)))+(x242)+(((x235)*(x237)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x231)))+(((IkReal(-1.00000000000000))*(x231)*(x233)))+(((sj7)*(x241))));
00949 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00950 {
00951 continue;
00952 }
00953 }
00954
00955 {
00956 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00957 vinfos[0].jointtype = 1;
00958 vinfos[0].foffset = j3;
00959 vinfos[0].indices[0] = _ij3[0];
00960 vinfos[0].indices[1] = _ij3[1];
00961 vinfos[0].maxsolutions = _nj3;
00962 vinfos[1].jointtype = 1;
00963 vinfos[1].foffset = j4;
00964 vinfos[1].indices[0] = _ij4[0];
00965 vinfos[1].indices[1] = _ij4[1];
00966 vinfos[1].maxsolutions = _nj4;
00967 vinfos[2].jointtype = 1;
00968 vinfos[2].foffset = j5;
00969 vinfos[2].indices[0] = _ij5[0];
00970 vinfos[2].indices[1] = _ij5[1];
00971 vinfos[2].maxsolutions = _nj5;
00972 vinfos[3].jointtype = 1;
00973 vinfos[3].foffset = j6;
00974 vinfos[3].indices[0] = _ij6[0];
00975 vinfos[3].indices[1] = _ij6[1];
00976 vinfos[3].maxsolutions = _nj6;
00977 vinfos[4].jointtype = 1;
00978 vinfos[4].foffset = j7;
00979 vinfos[4].indices[0] = _ij7[0];
00980 vinfos[4].indices[1] = _ij7[1];
00981 vinfos[4].maxsolutions = _nj7;
00982 vinfos[5].jointtype = 1;
00983 vinfos[5].foffset = j8;
00984 vinfos[5].indices[0] = _ij8[0];
00985 vinfos[5].indices[1] = _ij8[1];
00986 vinfos[5].maxsolutions = _nj8;
00987 std::vector<int> vfree(0);
00988 solutions.AddSolution(vinfos,vfree);
00989 }
00990 }
00991 }
00992
00993 }
00994
00995 }
00996 }
00997 }
00998
00999 }
01000
01001 }
01002
01003 } else
01004 {
01005 {
01006 IkReal j4array[1], cj4array[1], sj4array[1];
01007 bool j4valid[1]={false};
01008 _nj4 = 1;
01009 IkReal x246=((sj5)*(sj7));
01010 IkReal x247=((r21)*(sj8));
01011 IkReal x248=((cj5)*(sj7));
01012 IkReal x249=((cj8)*(r20));
01013 IkReal x250=((cj5)*(cj7));
01014 IkReal x251=((cj7)*(sj5));
01015 if( IKabs(((gconst68)*(((((IkReal(-1.00000000000000))*(x249)*(x251)))+(((IkReal(-1.00000000000000))*(x248)*(x249)))+(((r22)*(x250)))+(((x247)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x246)))+(((x247)*(x251))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst68)*(((((x249)*(x250)))+(((r22)*(x248)))+(((IkReal(-1.00000000000000))*(x247)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x249)))+(((r22)*(x251)))+(((x246)*(x247))))))) < IKFAST_ATAN2_MAGTHRESH )
01016 continue;
01017 j4array[0]=IKatan2(((gconst68)*(((((IkReal(-1.00000000000000))*(x249)*(x251)))+(((IkReal(-1.00000000000000))*(x248)*(x249)))+(((r22)*(x250)))+(((x247)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x246)))+(((x247)*(x251)))))), ((gconst68)*(((((x249)*(x250)))+(((r22)*(x248)))+(((IkReal(-1.00000000000000))*(x247)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x249)))+(((r22)*(x251)))+(((x246)*(x247)))))));
01018 sj4array[0]=IKsin(j4array[0]);
01019 cj4array[0]=IKcos(j4array[0]);
01020 if( j4array[0] > IKPI )
01021 {
01022 j4array[0]-=IK2PI;
01023 }
01024 else if( j4array[0] < -IKPI )
01025 { j4array[0]+=IK2PI;
01026 }
01027 j4valid[0] = true;
01028 for(int ij4 = 0; ij4 < 1; ++ij4)
01029 {
01030 if( !j4valid[ij4] )
01031 {
01032 continue;
01033 }
01034 _ij4[0] = ij4; _ij4[1] = -1;
01035 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01036 {
01037 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01038 {
01039 j4valid[iij4]=false; _ij4[1] = iij4; break;
01040 }
01041 }
01042 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01043 {
01044 IkReal evalcond[2];
01045 IkReal x252=IKcos(j4);
01046 IkReal x253=IKsin(j4);
01047 IkReal x254=((r21)*(sj8));
01048 IkReal x255=((IkReal(1.00000000000000))*(cj8)*(r20));
01049 evalcond[0]=((((IkReal(-1.00000000000000))*(cj7)*(x255)))+(((IkReal(-1.00000000000000))*(cj5)*(x252)))+(((cj7)*(x254)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((sj5)*(x253))));
01050 evalcond[1]=((((sj7)*(x254)))+(((IkReal(-1.00000000000000))*(sj7)*(x255)))+(((cj5)*(x253)))+(((sj5)*(x252)))+(((cj7)*(r22))));
01051 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
01052 {
01053 continue;
01054 }
01055 }
01056
01057 {
01058 IkReal dummyeval[1];
01059 IkReal gconst71;
01060 IkReal x256=(sj8)*(sj8);
01061 IkReal x257=(cj8)*(cj8);
01062 IkReal x258=((IkReal(2.00000000000000))*(cj8)*(sj8));
01063 gconst71=IKsign(((((r10)*(r11)*(x258)))+(((x256)*((r10)*(r10))))+(((x257)*((r11)*(r11))))+(((r00)*(r01)*(x258)))+(((x257)*((r01)*(r01))))+(((x256)*((r00)*(r00))))));
01064 IkReal x259=(sj8)*(sj8);
01065 IkReal x260=(cj8)*(cj8);
01066 IkReal x261=((IkReal(2.00000000000000))*(cj8)*(sj8));
01067 dummyeval[0]=((((r00)*(r01)*(x261)))+(((x260)*((r01)*(r01))))+(((x260)*((r11)*(r11))))+(((x259)*((r10)*(r10))))+(((x259)*((r00)*(r00))))+(((r10)*(r11)*(x261))));
01068 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01069 {
01070 {
01071 IkReal dummyeval[1];
01072 IkReal gconst72;
01073 IkReal x262=(sj8)*(sj8);
01074 IkReal x263=(cj8)*(cj8);
01075 IkReal x264=((r00)*(r11));
01076 IkReal x265=((r02)*(sj7));
01077 IkReal x266=((cj7)*(x263));
01078 IkReal x267=((IkReal(1.00000000000000))*(r12)*(sj7));
01079 IkReal x268=((IkReal(1.00000000000000))*(r01)*(r10));
01080 IkReal x269=((cj7)*(x262));
01081 gconst72=IKsign(((((r10)*(sj8)*(x265)))+(((x264)*(x269)))+(((x264)*(x266)))+(((IkReal(-1.00000000000000))*(x268)*(x269)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x267)))+(((cj8)*(r11)*(x265)))+(((IkReal(-1.00000000000000))*(x266)*(x268)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x267)))));
01082 IkReal x270=(sj8)*(sj8);
01083 IkReal x271=(cj8)*(cj8);
01084 IkReal x272=((r00)*(r11));
01085 IkReal x273=((r02)*(sj7));
01086 IkReal x274=((cj7)*(x271));
01087 IkReal x275=((IkReal(1.00000000000000))*(r12)*(sj7));
01088 IkReal x276=((IkReal(1.00000000000000))*(r01)*(r10));
01089 IkReal x277=((cj7)*(x270));
01090 dummyeval[0]=((((x272)*(x274)))+(((x272)*(x277)))+(((IkReal(-1.00000000000000))*(x274)*(x276)))+(((r10)*(sj8)*(x273)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x275)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x275)))+(((cj8)*(r11)*(x273)))+(((IkReal(-1.00000000000000))*(x276)*(x277))));
01091 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01092 {
01093 continue;
01094
01095 } else
01096 {
01097 {
01098 IkReal j3array[1], cj3array[1], sj3array[1];
01099 bool j3valid[1]={false};
01100 _nj3 = 1;
01101 IkReal x278=((cj7)*(cj8));
01102 IkReal x279=((IkReal(1.00000000000000))*(cj7)*(sj8));
01103 if( IKabs(((gconst72)*(((((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r12)*(sj7)))+(((r10)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst72)*(((((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH )
01104 continue;
01105 j3array[0]=IKatan2(((gconst72)*(((((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r12)*(sj7)))+(((r10)*(x278)))))), ((gconst72)*(((((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278)))))));
01106 sj3array[0]=IKsin(j3array[0]);
01107 cj3array[0]=IKcos(j3array[0]);
01108 if( j3array[0] > IKPI )
01109 {
01110 j3array[0]-=IK2PI;
01111 }
01112 else if( j3array[0] < -IKPI )
01113 { j3array[0]+=IK2PI;
01114 }
01115 j3valid[0] = true;
01116 for(int ij3 = 0; ij3 < 1; ++ij3)
01117 {
01118 if( !j3valid[ij3] )
01119 {
01120 continue;
01121 }
01122 _ij3[0] = ij3; _ij3[1] = -1;
01123 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01124 {
01125 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01126 {
01127 j3valid[iij3]=false; _ij3[1] = iij3; break;
01128 }
01129 }
01130 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01131 {
01132 IkReal evalcond[6];
01133 IkReal x280=IKsin(j3);
01134 IkReal x281=IKcos(j3);
01135 IkReal x282=((r01)*(sj8));
01136 IkReal x283=((cj8)*(r00));
01137 IkReal x284=((r00)*(sj8));
01138 IkReal x285=((cj8)*(r11));
01139 IkReal x286=((r11)*(sj8));
01140 IkReal x287=((r10)*(sj8));
01141 IkReal x288=((cj8)*(r10));
01142 IkReal x289=((cj7)*(x280));
01143 IkReal x290=((sj7)*(x281));
01144 IkReal x291=((IkReal(1.00000000000000))*(x280));
01145 IkReal x292=((IkReal(1.00000000000000))*(x281));
01146 IkReal x293=((cj8)*(x280));
01147 IkReal x294=((IkReal(1.00000000000000))*(x286));
01148 IkReal x295=((sj7)*(x280));
01149 IkReal x296=((cj7)*(x281));
01150 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x285)*(x292)))+(((r01)*(x293)))+(((IkReal(-1.00000000000000))*(x287)*(x292)))+(((x280)*(x284))));
01151 evalcond[1]=((((IkReal(-1.00000000000000))*(x284)*(x292)))+(((IkReal(-1.00000000000000))*(x285)*(x291)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x292)))+(((IkReal(-1.00000000000000))*(x287)*(x291))));
01152 evalcond[2]=((((r12)*(x290)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x291)))+(((IkReal(-1.00000000000000))*(x283)*(x289)))+(((x282)*(x289)))+(((x288)*(x296)))+(((IkReal(-1.00000000000000))*(cj7)*(x286)*(x292))));
01153 evalcond[3]=((((IkReal(-1.00000000000000))*(cj7)*(r12)*(x292)))+(((IkReal(-1.00000000000000))*(x290)*(x294)))+(((IkReal(-1.00000000000000))*(sj7)*(x283)*(x291)))+(((x282)*(x295)))+(((r02)*(x289)))+(((x288)*(x290))));
01154 evalcond[4]=((((r12)*(x295)))+(((IkReal(-1.00000000000000))*(cj7)*(x282)*(x292)))+(((IkReal(-1.00000000000000))*(x289)*(x294)))+(((x283)*(x296)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((x288)*(x289)))+(((r02)*(x290))));
01155 evalcond[5]=((((IkReal(-1.00000000000000))*(r12)*(x289)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x292)))+(((x283)*(x290)))+(((IkReal(-1.00000000000000))*(x282)*(x290)))+(((cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(sj7)*(x286)*(x291)))+(((x288)*(x295))));
01156 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 )
01157 {
01158 continue;
01159 }
01160 }
01161
01162 {
01163 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01164 vinfos[0].jointtype = 1;
01165 vinfos[0].foffset = j3;
01166 vinfos[0].indices[0] = _ij3[0];
01167 vinfos[0].indices[1] = _ij3[1];
01168 vinfos[0].maxsolutions = _nj3;
01169 vinfos[1].jointtype = 1;
01170 vinfos[1].foffset = j4;
01171 vinfos[1].indices[0] = _ij4[0];
01172 vinfos[1].indices[1] = _ij4[1];
01173 vinfos[1].maxsolutions = _nj4;
01174 vinfos[2].jointtype = 1;
01175 vinfos[2].foffset = j5;
01176 vinfos[2].indices[0] = _ij5[0];
01177 vinfos[2].indices[1] = _ij5[1];
01178 vinfos[2].maxsolutions = _nj5;
01179 vinfos[3].jointtype = 1;
01180 vinfos[3].foffset = j6;
01181 vinfos[3].indices[0] = _ij6[0];
01182 vinfos[3].indices[1] = _ij6[1];
01183 vinfos[3].maxsolutions = _nj6;
01184 vinfos[4].jointtype = 1;
01185 vinfos[4].foffset = j7;
01186 vinfos[4].indices[0] = _ij7[0];
01187 vinfos[4].indices[1] = _ij7[1];
01188 vinfos[4].maxsolutions = _nj7;
01189 vinfos[5].jointtype = 1;
01190 vinfos[5].foffset = j8;
01191 vinfos[5].indices[0] = _ij8[0];
01192 vinfos[5].indices[1] = _ij8[1];
01193 vinfos[5].maxsolutions = _nj8;
01194 std::vector<int> vfree(0);
01195 solutions.AddSolution(vinfos,vfree);
01196 }
01197 }
01198 }
01199
01200 }
01201
01202 }
01203
01204 } else
01205 {
01206 {
01207 IkReal j3array[1], cj3array[1], sj3array[1];
01208 bool j3valid[1]={false};
01209 _nj3 = 1;
01210 if( IKabs(((gconst71)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst71)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
01211 continue;
01212 j3array[0]=IKatan2(((gconst71)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst71)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
01213 sj3array[0]=IKsin(j3array[0]);
01214 cj3array[0]=IKcos(j3array[0]);
01215 if( j3array[0] > IKPI )
01216 {
01217 j3array[0]-=IK2PI;
01218 }
01219 else if( j3array[0] < -IKPI )
01220 { j3array[0]+=IK2PI;
01221 }
01222 j3valid[0] = true;
01223 for(int ij3 = 0; ij3 < 1; ++ij3)
01224 {
01225 if( !j3valid[ij3] )
01226 {
01227 continue;
01228 }
01229 _ij3[0] = ij3; _ij3[1] = -1;
01230 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01231 {
01232 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01233 {
01234 j3valid[iij3]=false; _ij3[1] = iij3; break;
01235 }
01236 }
01237 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01238 {
01239 IkReal evalcond[6];
01240 IkReal x297=IKsin(j3);
01241 IkReal x298=IKcos(j3);
01242 IkReal x299=((r01)*(sj8));
01243 IkReal x300=((cj8)*(r00));
01244 IkReal x301=((r00)*(sj8));
01245 IkReal x302=((cj8)*(r11));
01246 IkReal x303=((r11)*(sj8));
01247 IkReal x304=((r10)*(sj8));
01248 IkReal x305=((cj8)*(r10));
01249 IkReal x306=((cj7)*(x297));
01250 IkReal x307=((sj7)*(x298));
01251 IkReal x308=((IkReal(1.00000000000000))*(x297));
01252 IkReal x309=((IkReal(1.00000000000000))*(x298));
01253 IkReal x310=((cj8)*(x297));
01254 IkReal x311=((IkReal(1.00000000000000))*(x303));
01255 IkReal x312=((sj7)*(x297));
01256 IkReal x313=((cj7)*(x298));
01257 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x302)*(x309)))+(((r01)*(x310)))+(((x297)*(x301)))+(((IkReal(-1.00000000000000))*(x304)*(x309))));
01258 evalcond[1]=((((IkReal(-1.00000000000000))*(x301)*(x309)))+(((IkReal(-1.00000000000000))*(x302)*(x308)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x309)))+(((IkReal(-1.00000000000000))*(x304)*(x308))));
01259 evalcond[2]=((((x305)*(x313)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x308)))+(((IkReal(-1.00000000000000))*(cj7)*(x303)*(x309)))+(((x299)*(x306)))+(((IkReal(-1.00000000000000))*(x300)*(x306)))+(((r12)*(x307))));
01260 evalcond[3]=((((IkReal(-1.00000000000000))*(sj7)*(x300)*(x308)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x309)))+(((x299)*(x312)))+(((r02)*(x306)))+(((x305)*(x307)))+(((IkReal(-1.00000000000000))*(x307)*(x311))));
01261 evalcond[4]=((((IkReal(-1.00000000000000))*(cj7)*(x299)*(x309)))+(((x300)*(x313)))+(((cj5)*(sj4)))+(((r02)*(x307)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x306)*(x311)))+(((x305)*(x306)))+(((r12)*(x312))));
01262 evalcond[5]=((((x305)*(x312)))+(((IkReal(-1.00000000000000))*(r12)*(x306)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x309)))+(((cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(x299)*(x307)))+(((IkReal(-1.00000000000000))*(sj7)*(x303)*(x308)))+(((x300)*(x307)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5))));
01263 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 )
01264 {
01265 continue;
01266 }
01267 }
01268
01269 {
01270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01271 vinfos[0].jointtype = 1;
01272 vinfos[0].foffset = j3;
01273 vinfos[0].indices[0] = _ij3[0];
01274 vinfos[0].indices[1] = _ij3[1];
01275 vinfos[0].maxsolutions = _nj3;
01276 vinfos[1].jointtype = 1;
01277 vinfos[1].foffset = j4;
01278 vinfos[1].indices[0] = _ij4[0];
01279 vinfos[1].indices[1] = _ij4[1];
01280 vinfos[1].maxsolutions = _nj4;
01281 vinfos[2].jointtype = 1;
01282 vinfos[2].foffset = j5;
01283 vinfos[2].indices[0] = _ij5[0];
01284 vinfos[2].indices[1] = _ij5[1];
01285 vinfos[2].maxsolutions = _nj5;
01286 vinfos[3].jointtype = 1;
01287 vinfos[3].foffset = j6;
01288 vinfos[3].indices[0] = _ij6[0];
01289 vinfos[3].indices[1] = _ij6[1];
01290 vinfos[3].maxsolutions = _nj6;
01291 vinfos[4].jointtype = 1;
01292 vinfos[4].foffset = j7;
01293 vinfos[4].indices[0] = _ij7[0];
01294 vinfos[4].indices[1] = _ij7[1];
01295 vinfos[4].maxsolutions = _nj7;
01296 vinfos[5].jointtype = 1;
01297 vinfos[5].foffset = j8;
01298 vinfos[5].indices[0] = _ij8[0];
01299 vinfos[5].indices[1] = _ij8[1];
01300 vinfos[5].maxsolutions = _nj8;
01301 std::vector<int> vfree(0);
01302 solutions.AddSolution(vinfos,vfree);
01303 }
01304 }
01305 }
01306
01307 }
01308
01309 }
01310 }
01311 }
01312
01313 }
01314
01315 }
01316
01317 } else
01318 {
01319 {
01320 IkReal j3array[1], cj3array[1], sj3array[1];
01321 bool j3valid[1]={false};
01322 _nj3 = 1;
01323 if( IKabs(((gconst66)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst66)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
01324 continue;
01325 j3array[0]=IKatan2(((gconst66)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst66)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
01326 sj3array[0]=IKsin(j3array[0]);
01327 cj3array[0]=IKcos(j3array[0]);
01328 if( j3array[0] > IKPI )
01329 {
01330 j3array[0]-=IK2PI;
01331 }
01332 else if( j3array[0] < -IKPI )
01333 { j3array[0]+=IK2PI;
01334 }
01335 j3valid[0] = true;
01336 for(int ij3 = 0; ij3 < 1; ++ij3)
01337 {
01338 if( !j3valid[ij3] )
01339 {
01340 continue;
01341 }
01342 _ij3[0] = ij3; _ij3[1] = -1;
01343 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01344 {
01345 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01346 {
01347 j3valid[iij3]=false; _ij3[1] = iij3; break;
01348 }
01349 }
01350 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01351 {
01352 IkReal evalcond[4];
01353 IkReal x314=IKsin(j3);
01354 IkReal x315=IKcos(j3);
01355 IkReal x316=((IkReal(1.00000000000000))*(sj8));
01356 IkReal x317=((IkReal(1.00000000000000))*(cj8));
01357 IkReal x318=((r01)*(sj8));
01358 IkReal x319=((cj8)*(r10));
01359 IkReal x320=((sj7)*(x315));
01360 IkReal x321=((cj7)*(x314));
01361 IkReal x322=((r00)*(x314));
01362 IkReal x323=((sj7)*(x314));
01363 IkReal x324=((cj7)*(x315));
01364 evalcond[0]=((IkReal(1.00000000000000))+(((cj8)*(r01)*(x314)))+(((IkReal(-1.00000000000000))*(r11)*(x315)*(x317)))+(((IkReal(-1.00000000000000))*(r10)*(x315)*(x316)))+(((sj8)*(x322))));
01365 evalcond[1]=((((IkReal(-1.00000000000000))*(r10)*(x314)*(x316)))+(((IkReal(-1.00000000000000))*(r01)*(x315)*(x317)))+(((IkReal(-1.00000000000000))*(r11)*(x314)*(x317)))+(((IkReal(-1.00000000000000))*(r00)*(x315)*(x316))));
01366 evalcond[2]=((((x318)*(x321)))+(((r12)*(x320)))+(((IkReal(-1.00000000000000))*(r00)*(x317)*(x321)))+(((IkReal(-1.00000000000000))*(r02)*(x323)))+(((IkReal(-1.00000000000000))*(r11)*(x316)*(x324)))+(((x319)*(x324))));
01367 evalcond[3]=((((x318)*(x323)))+(((r02)*(x321)))+(((IkReal(-1.00000000000000))*(r12)*(x324)))+(((IkReal(-1.00000000000000))*(sj7)*(x317)*(x322)))+(((IkReal(-1.00000000000000))*(r11)*(x316)*(x320)))+(((x319)*(x320))));
01368 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01369 {
01370 continue;
01371 }
01372 }
01373
01374 {
01375 IkReal dummyeval[1];
01376 IkReal gconst69;
01377 gconst69=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
01378 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
01379 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01380 {
01381 {
01382 IkReal dummyeval[1];
01383 IkReal gconst70;
01384 gconst70=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
01385 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
01386 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01387 {
01388 continue;
01389
01390 } else
01391 {
01392 {
01393 IkReal j4array[1], cj4array[1], sj4array[1];
01394 bool j4valid[1]={false};
01395 _nj4 = 1;
01396 IkReal x325=((cj3)*(cj5));
01397 IkReal x326=((r02)*(sj7));
01398 IkReal x327=((cj8)*(r00));
01399 IkReal x328=((r22)*(sj7));
01400 IkReal x329=((cj5)*(sj3));
01401 IkReal x330=((cj7)*(cj8));
01402 IkReal x331=((IkReal(1.00000000000000))*(sj5));
01403 IkReal x332=((sj3)*(sj5));
01404 IkReal x333=((r12)*(sj7));
01405 IkReal x334=((cj3)*(cj7)*(sj5));
01406 IkReal x335=((IkReal(1.00000000000000))*(cj7)*(sj8));
01407 if( IKabs(((gconst70)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((x325)*(x326)))+(((cj7)*(x325)*(x327)))+(((IkReal(-1.00000000000000))*(x328)*(x331)))+(((x329)*(x333)))+(((r10)*(x329)*(x330)))+(((IkReal(-1.00000000000000))*(r01)*(x325)*(x335)))+(((IkReal(-1.00000000000000))*(r11)*(x329)*(x335)))+(((IkReal(-1.00000000000000))*(r20)*(x330)*(x331))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x335)))+(((x332)*(x333)))+(((cj5)*(x328)))+(((x327)*(x334)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x331)))+(((cj3)*(sj5)*(x326)))+(((cj5)*(r20)*(x330)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x331)))+(((r10)*(x330)*(x332))))))) < IKFAST_ATAN2_MAGTHRESH )
01408 continue;
01409 j4array[0]=IKatan2(((gconst70)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((x325)*(x326)))+(((cj7)*(x325)*(x327)))+(((IkReal(-1.00000000000000))*(x328)*(x331)))+(((x329)*(x333)))+(((r10)*(x329)*(x330)))+(((IkReal(-1.00000000000000))*(r01)*(x325)*(x335)))+(((IkReal(-1.00000000000000))*(r11)*(x329)*(x335)))+(((IkReal(-1.00000000000000))*(r20)*(x330)*(x331)))))), ((gconst70)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x335)))+(((x332)*(x333)))+(((cj5)*(x328)))+(((x327)*(x334)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x331)))+(((cj3)*(sj5)*(x326)))+(((cj5)*(r20)*(x330)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x331)))+(((r10)*(x330)*(x332)))))));
01410 sj4array[0]=IKsin(j4array[0]);
01411 cj4array[0]=IKcos(j4array[0]);
01412 if( j4array[0] > IKPI )
01413 {
01414 j4array[0]-=IK2PI;
01415 }
01416 else if( j4array[0] < -IKPI )
01417 { j4array[0]+=IK2PI;
01418 }
01419 j4valid[0] = true;
01420 for(int ij4 = 0; ij4 < 1; ++ij4)
01421 {
01422 if( !j4valid[ij4] )
01423 {
01424 continue;
01425 }
01426 _ij4[0] = ij4; _ij4[1] = -1;
01427 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01428 {
01429 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01430 {
01431 j4valid[iij4]=false; _ij4[1] = iij4; break;
01432 }
01433 }
01434 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01435 {
01436 IkReal evalcond[4];
01437 IkReal x336=IKcos(j4);
01438 IkReal x337=IKsin(j4);
01439 IkReal x338=((IkReal(1.00000000000000))*(cj7));
01440 IkReal x339=((cj8)*(r20));
01441 IkReal x340=((cj3)*(r02));
01442 IkReal x341=((IkReal(1.00000000000000))*(sj7));
01443 IkReal x342=((sj3)*(sj7));
01444 IkReal x343=((r21)*(sj8));
01445 IkReal x344=((cj8)*(r10));
01446 IkReal x345=((sj5)*(x336));
01447 IkReal x346=((cj5)*(x337));
01448 IkReal x347=((r11)*(sj3)*(sj8));
01449 IkReal x348=((cj3)*(cj8)*(r00));
01450 IkReal x349=((cj5)*(x336));
01451 IkReal x350=((cj3)*(r01)*(sj8));
01452 IkReal x351=((sj5)*(x337));
01453 IkReal x352=((x346)+(x345));
01454 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x341)))+(((IkReal(-1.00000000000000))*(x338)*(x339)))+(x351)+(((cj7)*(x343)))+(((IkReal(-1.00000000000000))*(x349))));
01455 evalcond[1]=((x352)+(((sj7)*(x343)))+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x339)*(x341))));
01456 evalcond[2]=((((IkReal(-1.00000000000000))*(x338)*(x350)))+(x352)+(((cj7)*(sj3)*(x344)))+(((cj7)*(x348)))+(((sj7)*(x340)))+(((IkReal(-1.00000000000000))*(x338)*(x347)))+(((r12)*(x342))));
01457 evalcond[3]=((((IkReal(-1.00000000000000))*(x341)*(x350)))+(((IkReal(-1.00000000000000))*(x351)))+(x349)+(((IkReal(-1.00000000000000))*(x341)*(x347)))+(((sj7)*(x348)))+(((IkReal(-1.00000000000000))*(x338)*(x340)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x338)))+(((x342)*(x344))));
01458 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01459 {
01460 continue;
01461 }
01462 }
01463
01464 {
01465 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01466 vinfos[0].jointtype = 1;
01467 vinfos[0].foffset = j3;
01468 vinfos[0].indices[0] = _ij3[0];
01469 vinfos[0].indices[1] = _ij3[1];
01470 vinfos[0].maxsolutions = _nj3;
01471 vinfos[1].jointtype = 1;
01472 vinfos[1].foffset = j4;
01473 vinfos[1].indices[0] = _ij4[0];
01474 vinfos[1].indices[1] = _ij4[1];
01475 vinfos[1].maxsolutions = _nj4;
01476 vinfos[2].jointtype = 1;
01477 vinfos[2].foffset = j5;
01478 vinfos[2].indices[0] = _ij5[0];
01479 vinfos[2].indices[1] = _ij5[1];
01480 vinfos[2].maxsolutions = _nj5;
01481 vinfos[3].jointtype = 1;
01482 vinfos[3].foffset = j6;
01483 vinfos[3].indices[0] = _ij6[0];
01484 vinfos[3].indices[1] = _ij6[1];
01485 vinfos[3].maxsolutions = _nj6;
01486 vinfos[4].jointtype = 1;
01487 vinfos[4].foffset = j7;
01488 vinfos[4].indices[0] = _ij7[0];
01489 vinfos[4].indices[1] = _ij7[1];
01490 vinfos[4].maxsolutions = _nj7;
01491 vinfos[5].jointtype = 1;
01492 vinfos[5].foffset = j8;
01493 vinfos[5].indices[0] = _ij8[0];
01494 vinfos[5].indices[1] = _ij8[1];
01495 vinfos[5].maxsolutions = _nj8;
01496 std::vector<int> vfree(0);
01497 solutions.AddSolution(vinfos,vfree);
01498 }
01499 }
01500 }
01501
01502 }
01503
01504 }
01505
01506 } else
01507 {
01508 {
01509 IkReal j4array[1], cj4array[1], sj4array[1];
01510 bool j4valid[1]={false};
01511 _nj4 = 1;
01512 IkReal x353=((r21)*(sj8));
01513 IkReal x354=((sj5)*(sj7));
01514 IkReal x355=((cj5)*(sj7));
01515 IkReal x356=((cj5)*(cj7));
01516 IkReal x357=((cj8)*(r20));
01517 IkReal x358=((cj7)*(sj5));
01518 if( IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(r22)*(x354)))+(((IkReal(-1.00000000000000))*(x357)*(x358)))+(((x353)*(x358)))+(((x353)*(x355)))+(((r22)*(x356)))+(((IkReal(-1.00000000000000))*(x355)*(x357))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((x356)*(x357)))+(((IkReal(-1.00000000000000))*(x353)*(x356)))+(((x353)*(x354)))+(((r22)*(x355)))+(((r22)*(x358)))+(((IkReal(-1.00000000000000))*(x354)*(x357))))))) < IKFAST_ATAN2_MAGTHRESH )
01519 continue;
01520 j4array[0]=IKatan2(((gconst69)*(((((IkReal(-1.00000000000000))*(r22)*(x354)))+(((IkReal(-1.00000000000000))*(x357)*(x358)))+(((x353)*(x358)))+(((x353)*(x355)))+(((r22)*(x356)))+(((IkReal(-1.00000000000000))*(x355)*(x357)))))), ((gconst69)*(((((x356)*(x357)))+(((IkReal(-1.00000000000000))*(x353)*(x356)))+(((x353)*(x354)))+(((r22)*(x355)))+(((r22)*(x358)))+(((IkReal(-1.00000000000000))*(x354)*(x357)))))));
01521 sj4array[0]=IKsin(j4array[0]);
01522 cj4array[0]=IKcos(j4array[0]);
01523 if( j4array[0] > IKPI )
01524 {
01525 j4array[0]-=IK2PI;
01526 }
01527 else if( j4array[0] < -IKPI )
01528 { j4array[0]+=IK2PI;
01529 }
01530 j4valid[0] = true;
01531 for(int ij4 = 0; ij4 < 1; ++ij4)
01532 {
01533 if( !j4valid[ij4] )
01534 {
01535 continue;
01536 }
01537 _ij4[0] = ij4; _ij4[1] = -1;
01538 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01539 {
01540 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01541 {
01542 j4valid[iij4]=false; _ij4[1] = iij4; break;
01543 }
01544 }
01545 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01546 {
01547 IkReal evalcond[4];
01548 IkReal x359=IKcos(j4);
01549 IkReal x360=IKsin(j4);
01550 IkReal x361=((IkReal(1.00000000000000))*(cj7));
01551 IkReal x362=((cj8)*(r20));
01552 IkReal x363=((cj3)*(r02));
01553 IkReal x364=((IkReal(1.00000000000000))*(sj7));
01554 IkReal x365=((sj3)*(sj7));
01555 IkReal x366=((r21)*(sj8));
01556 IkReal x367=((cj8)*(r10));
01557 IkReal x368=((sj5)*(x359));
01558 IkReal x369=((cj5)*(x360));
01559 IkReal x370=((r11)*(sj3)*(sj8));
01560 IkReal x371=((cj3)*(cj8)*(r00));
01561 IkReal x372=((cj5)*(x359));
01562 IkReal x373=((cj3)*(r01)*(sj8));
01563 IkReal x374=((sj5)*(x360));
01564 IkReal x375=((x368)+(x369));
01565 evalcond[0]=((((cj7)*(x366)))+(((IkReal(-1.00000000000000))*(x372)))+(((IkReal(-1.00000000000000))*(x361)*(x362)))+(x374)+(((IkReal(-1.00000000000000))*(r22)*(x364))));
01566 evalcond[1]=((((IkReal(-1.00000000000000))*(x362)*(x364)))+(x375)+(((sj7)*(x366)))+(((cj7)*(r22))));
01567 evalcond[2]=((((cj7)*(x371)))+(((IkReal(-1.00000000000000))*(x361)*(x373)))+(((IkReal(-1.00000000000000))*(x361)*(x370)))+(x375)+(((r12)*(x365)))+(((sj7)*(x363)))+(((cj7)*(sj3)*(x367))));
01568 evalcond[3]=((((IkReal(-1.00000000000000))*(x374)))+(((IkReal(-1.00000000000000))*(x361)*(x363)))+(((x365)*(x367)))+(x372)+(((sj7)*(x371)))+(((IkReal(-1.00000000000000))*(x364)*(x370)))+(((IkReal(-1.00000000000000))*(x364)*(x373)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x361))));
01569 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01570 {
01571 continue;
01572 }
01573 }
01574
01575 {
01576 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01577 vinfos[0].jointtype = 1;
01578 vinfos[0].foffset = j3;
01579 vinfos[0].indices[0] = _ij3[0];
01580 vinfos[0].indices[1] = _ij3[1];
01581 vinfos[0].maxsolutions = _nj3;
01582 vinfos[1].jointtype = 1;
01583 vinfos[1].foffset = j4;
01584 vinfos[1].indices[0] = _ij4[0];
01585 vinfos[1].indices[1] = _ij4[1];
01586 vinfos[1].maxsolutions = _nj4;
01587 vinfos[2].jointtype = 1;
01588 vinfos[2].foffset = j5;
01589 vinfos[2].indices[0] = _ij5[0];
01590 vinfos[2].indices[1] = _ij5[1];
01591 vinfos[2].maxsolutions = _nj5;
01592 vinfos[3].jointtype = 1;
01593 vinfos[3].foffset = j6;
01594 vinfos[3].indices[0] = _ij6[0];
01595 vinfos[3].indices[1] = _ij6[1];
01596 vinfos[3].maxsolutions = _nj6;
01597 vinfos[4].jointtype = 1;
01598 vinfos[4].foffset = j7;
01599 vinfos[4].indices[0] = _ij7[0];
01600 vinfos[4].indices[1] = _ij7[1];
01601 vinfos[4].maxsolutions = _nj7;
01602 vinfos[5].jointtype = 1;
01603 vinfos[5].foffset = j8;
01604 vinfos[5].indices[0] = _ij8[0];
01605 vinfos[5].indices[1] = _ij8[1];
01606 vinfos[5].maxsolutions = _nj8;
01607 std::vector<int> vfree(0);
01608 solutions.AddSolution(vinfos,vfree);
01609 }
01610 }
01611 }
01612
01613 }
01614
01615 }
01616 }
01617 }
01618
01619 }
01620
01621 }
01622 }
01623 }
01624
01625 } else
01626 {
01627 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j6)), IkReal(6.28318530717959))));
01628 evalcond[1]=((IkReal(0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
01629 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
01630 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
01631 {
01632 {
01633 IkReal j5array[1], cj5array[1], sj5array[1];
01634 bool j5valid[1]={false};
01635 _nj5 = 1;
01636 IkReal x376=((IkReal(4.00000000000000))*(sj7));
01637 IkReal x377=((npy)*(sj8));
01638 IkReal x378=((IkReal(4.00000000000000))*(cj7));
01639 IkReal x379=((cj8)*(npx));
01640 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-0.360000000000000))*(sj7)))+(((x376)*(x379)))+(((IkReal(-1.00000000000000))*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(npz)*(x378))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x376)))+(((IkReal(-1.00000000000000))*(x377)*(x378)))+(((x378)*(x379)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-0.360000000000000))*(sj7)))+(((x376)*(x379)))+(((IkReal(-1.00000000000000))*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(npz)*(x378)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x376)))+(((IkReal(-1.00000000000000))*(x377)*(x378)))+(((x378)*(x379)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
01641 continue;
01642 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-0.360000000000000))*(sj7)))+(((x376)*(x379)))+(((IkReal(-1.00000000000000))*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(npz)*(x378)))), ((IkReal(-0.940000000000000))+(((npz)*(x376)))+(((IkReal(-1.00000000000000))*(x377)*(x378)))+(((x378)*(x379)))+(((IkReal(-0.360000000000000))*(cj7)))));
01643 sj5array[0]=IKsin(j5array[0]);
01644 cj5array[0]=IKcos(j5array[0]);
01645 if( j5array[0] > IKPI )
01646 {
01647 j5array[0]-=IK2PI;
01648 }
01649 else if( j5array[0] < -IKPI )
01650 { j5array[0]+=IK2PI;
01651 }
01652 j5valid[0] = true;
01653 for(int ij5 = 0; ij5 < 1; ++ij5)
01654 {
01655 if( !j5valid[ij5] )
01656 {
01657 continue;
01658 }
01659 _ij5[0] = ij5; _ij5[1] = -1;
01660 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
01661 {
01662 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
01663 {
01664 j5valid[iij5]=false; _ij5[1] = iij5; break;
01665 }
01666 }
01667 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
01668 {
01669 IkReal evalcond[2];
01670 IkReal x380=((IkReal(1.00000000000000))*(sj7));
01671 IkReal x381=((npy)*(sj8));
01672 IkReal x382=((cj8)*(npx));
01673 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x380)))+(((IkReal(0.0900000000000000))*(cj7)))+(((cj7)*(x381)))+(((IkReal(-1.00000000000000))*(cj7)*(x382)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
01674 evalcond[1]=((IkReal(-0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x380)*(x382)))+(((IkReal(0.250000000000000))*(IKsin(j5))))+(((cj7)*(npz)))+(((sj7)*(x381))));
01675 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
01676 {
01677 continue;
01678 }
01679 }
01680
01681 {
01682 IkReal dummyeval[1];
01683 IkReal gconst75;
01684 IkReal x383=(cj8)*(cj8);
01685 IkReal x384=(sj8)*(sj8);
01686 IkReal x385=((IkReal(2.00000000000000))*(cj8)*(sj8));
01687 IkReal x386=((IkReal(1.00000000000000))*(x383));
01688 IkReal x387=((IkReal(1.00000000000000))*(x384));
01689 gconst75=IKsign(((((IkReal(-1.00000000000000))*(x387)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x386)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x385)))+(((IkReal(-1.00000000000000))*(x386)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x385)))+(((IkReal(-1.00000000000000))*(x387)*((r10)*(r10))))));
01690 IkReal x388=(cj8)*(cj8);
01691 IkReal x389=(sj8)*(sj8);
01692 IkReal x390=((IkReal(2.00000000000000))*(cj8)*(sj8));
01693 IkReal x391=((IkReal(1.00000000000000))*(x388));
01694 IkReal x392=((IkReal(1.00000000000000))*(x389));
01695 dummyeval[0]=((((IkReal(-1.00000000000000))*(x392)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x390)))+(((IkReal(-1.00000000000000))*(x391)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x390)))+(((IkReal(-1.00000000000000))*(x391)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x392)*((r10)*(r10)))));
01696 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01697 {
01698 {
01699 IkReal dummyeval[1];
01700 IkReal gconst77;
01701 gconst77=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
01702 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
01703 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01704 {
01705 {
01706 IkReal dummyeval[1];
01707 IkReal gconst76;
01708 IkReal x393=(sj8)*(sj8);
01709 IkReal x394=(cj8)*(cj8);
01710 IkReal x395=((sj7)*(sj8));
01711 IkReal x396=((IkReal(1.00000000000000))*(r02));
01712 IkReal x397=((r01)*(r10));
01713 IkReal x398=((cj8)*(sj7));
01714 IkReal x399=((cj7)*(x393));
01715 IkReal x400=((IkReal(1.00000000000000))*(r00)*(r11));
01716 IkReal x401=((cj7)*(x394));
01717 gconst76=IKsign(((((x397)*(x401)))+(((r01)*(r12)*(x398)))+(((IkReal(-1.00000000000000))*(x400)*(x401)))+(((r00)*(r12)*(x395)))+(((IkReal(-1.00000000000000))*(x399)*(x400)))+(((x397)*(x399)))+(((IkReal(-1.00000000000000))*(r10)*(x395)*(x396)))+(((IkReal(-1.00000000000000))*(r11)*(x396)*(x398)))));
01718 IkReal x402=(sj8)*(sj8);
01719 IkReal x403=(cj8)*(cj8);
01720 IkReal x404=((sj7)*(sj8));
01721 IkReal x405=((IkReal(1.00000000000000))*(r02));
01722 IkReal x406=((r01)*(r10));
01723 IkReal x407=((cj8)*(sj7));
01724 IkReal x408=((cj7)*(x402));
01725 IkReal x409=((IkReal(1.00000000000000))*(r00)*(r11));
01726 IkReal x410=((cj7)*(x403));
01727 dummyeval[0]=((((r01)*(r12)*(x407)))+(((IkReal(-1.00000000000000))*(r10)*(x404)*(x405)))+(((IkReal(-1.00000000000000))*(r11)*(x405)*(x407)))+(((x406)*(x410)))+(((x406)*(x408)))+(((r00)*(r12)*(x404)))+(((IkReal(-1.00000000000000))*(x409)*(x410)))+(((IkReal(-1.00000000000000))*(x408)*(x409))));
01728 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01729 {
01730 continue;
01731
01732 } else
01733 {
01734 {
01735 IkReal j3array[1], cj3array[1], sj3array[1];
01736 bool j3valid[1]={false};
01737 _nj3 = 1;
01738 IkReal x411=((cj7)*(cj8));
01739 IkReal x412=((IkReal(1.00000000000000))*(cj7)*(sj8));
01740 if( IKabs(((gconst76)*(((((r12)*(sj7)))+(((r10)*(x411)))+(((IkReal(-1.00000000000000))*(r11)*(x412))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst76)*(((((r02)*(sj7)))+(((r00)*(x411)))+(((IkReal(-1.00000000000000))*(r01)*(x412))))))) < IKFAST_ATAN2_MAGTHRESH )
01741 continue;
01742 j3array[0]=IKatan2(((gconst76)*(((((r12)*(sj7)))+(((r10)*(x411)))+(((IkReal(-1.00000000000000))*(r11)*(x412)))))), ((gconst76)*(((((r02)*(sj7)))+(((r00)*(x411)))+(((IkReal(-1.00000000000000))*(r01)*(x412)))))));
01743 sj3array[0]=IKsin(j3array[0]);
01744 cj3array[0]=IKcos(j3array[0]);
01745 if( j3array[0] > IKPI )
01746 {
01747 j3array[0]-=IK2PI;
01748 }
01749 else if( j3array[0] < -IKPI )
01750 { j3array[0]+=IK2PI;
01751 }
01752 j3valid[0] = true;
01753 for(int ij3 = 0; ij3 < 1; ++ij3)
01754 {
01755 if( !j3valid[ij3] )
01756 {
01757 continue;
01758 }
01759 _ij3[0] = ij3; _ij3[1] = -1;
01760 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
01761 {
01762 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
01763 {
01764 j3valid[iij3]=false; _ij3[1] = iij3; break;
01765 }
01766 }
01767 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01768 {
01769 IkReal evalcond[4];
01770 IkReal x413=IKsin(j3);
01771 IkReal x414=IKcos(j3);
01772 IkReal x415=((IkReal(1.00000000000000))*(sj8));
01773 IkReal x416=((IkReal(1.00000000000000))*(cj8));
01774 IkReal x417=((r01)*(sj8));
01775 IkReal x418=((cj8)*(r10));
01776 IkReal x419=((sj7)*(x414));
01777 IkReal x420=((cj7)*(x413));
01778 IkReal x421=((r00)*(x413));
01779 IkReal x422=((sj7)*(x413));
01780 IkReal x423=((cj7)*(x414));
01781 evalcond[0]=((IkReal(-1.00000000000000))+(((sj8)*(x421)))+(((IkReal(-1.00000000000000))*(r11)*(x414)*(x416)))+(((IkReal(-1.00000000000000))*(r10)*(x414)*(x415)))+(((cj8)*(r01)*(x413))));
01782 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x413)*(x416)))+(((IkReal(-1.00000000000000))*(r01)*(x414)*(x416)))+(((IkReal(-1.00000000000000))*(r00)*(x414)*(x415)))+(((IkReal(-1.00000000000000))*(r10)*(x413)*(x415))));
01783 evalcond[2]=((((r12)*(x419)))+(((IkReal(-1.00000000000000))*(r02)*(x422)))+(((IkReal(-1.00000000000000))*(r00)*(x416)*(x420)))+(((x418)*(x423)))+(((IkReal(-1.00000000000000))*(r11)*(x415)*(x423)))+(((x417)*(x420))));
01784 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(x423)))+(((IkReal(-1.00000000000000))*(sj7)*(x416)*(x421)))+(((r02)*(x420)))+(((IkReal(-1.00000000000000))*(r11)*(x415)*(x419)))+(((x418)*(x419)))+(((x417)*(x422))));
01785 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01786 {
01787 continue;
01788 }
01789 }
01790
01791 {
01792 IkReal dummyeval[1];
01793 IkReal gconst78;
01794 gconst78=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
01795 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
01796 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01797 {
01798 {
01799 IkReal dummyeval[1];
01800 IkReal gconst79;
01801 gconst79=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
01802 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
01803 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01804 {
01805 continue;
01806
01807 } else
01808 {
01809 {
01810 IkReal j4array[1], cj4array[1], sj4array[1];
01811 bool j4valid[1]={false};
01812 _nj4 = 1;
01813 IkReal x424=((cj3)*(cj5));
01814 IkReal x425=((r02)*(sj7));
01815 IkReal x426=((cj5)*(sj7));
01816 IkReal x427=((cj3)*(sj5));
01817 IkReal x428=((r11)*(sj3));
01818 IkReal x429=((r10)*(sj3));
01819 IkReal x430=((r12)*(sj3));
01820 IkReal x431=((sj5)*(sj7));
01821 IkReal x432=((cj7)*(cj8)*(sj5));
01822 IkReal x433=((IkReal(1.00000000000000))*(cj7)*(sj8));
01823 IkReal x434=((cj5)*(cj7)*(cj8));
01824 if( IKabs(((gconst79)*(((((x429)*(x434)))+(((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x432)))+(((x424)*(x425)))+(((x426)*(x430)))+(((IkReal(-1.00000000000000))*(r01)*(x424)*(x433)))+(((IkReal(-1.00000000000000))*(cj5)*(x428)*(x433)))+(((cj7)*(cj8)*(r00)*(x424)))+(((IkReal(-1.00000000000000))*(r22)*(x431))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((x429)*(x432)))+(((x425)*(x427)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x433)))+(((cj7)*(cj8)*(r00)*(x427)))+(((r22)*(x426)))+(((x430)*(x431)))+(((IkReal(-1.00000000000000))*(sj5)*(x428)*(x433)))+(((r20)*(x434)))+(((IkReal(-1.00000000000000))*(r01)*(x427)*(x433))))))) < IKFAST_ATAN2_MAGTHRESH )
01825 continue;
01826 j4array[0]=IKatan2(((gconst79)*(((((x429)*(x434)))+(((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x432)))+(((x424)*(x425)))+(((x426)*(x430)))+(((IkReal(-1.00000000000000))*(r01)*(x424)*(x433)))+(((IkReal(-1.00000000000000))*(cj5)*(x428)*(x433)))+(((cj7)*(cj8)*(r00)*(x424)))+(((IkReal(-1.00000000000000))*(r22)*(x431)))))), ((gconst79)*(((((x429)*(x432)))+(((x425)*(x427)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x433)))+(((cj7)*(cj8)*(r00)*(x427)))+(((r22)*(x426)))+(((x430)*(x431)))+(((IkReal(-1.00000000000000))*(sj5)*(x428)*(x433)))+(((r20)*(x434)))+(((IkReal(-1.00000000000000))*(r01)*(x427)*(x433)))))));
01827 sj4array[0]=IKsin(j4array[0]);
01828 cj4array[0]=IKcos(j4array[0]);
01829 if( j4array[0] > IKPI )
01830 {
01831 j4array[0]-=IK2PI;
01832 }
01833 else if( j4array[0] < -IKPI )
01834 { j4array[0]+=IK2PI;
01835 }
01836 j4valid[0] = true;
01837 for(int ij4 = 0; ij4 < 1; ++ij4)
01838 {
01839 if( !j4valid[ij4] )
01840 {
01841 continue;
01842 }
01843 _ij4[0] = ij4; _ij4[1] = -1;
01844 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01845 {
01846 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01847 {
01848 j4valid[iij4]=false; _ij4[1] = iij4; break;
01849 }
01850 }
01851 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01852 {
01853 IkReal evalcond[4];
01854 IkReal x435=IKcos(j4);
01855 IkReal x436=IKsin(j4);
01856 IkReal x437=((IkReal(1.00000000000000))*(cj7));
01857 IkReal x438=((cj8)*(r20));
01858 IkReal x439=((cj3)*(r02));
01859 IkReal x440=((IkReal(1.00000000000000))*(sj7));
01860 IkReal x441=((sj3)*(sj7));
01861 IkReal x442=((r21)*(sj8));
01862 IkReal x443=((IkReal(1.00000000000000))*(cj5));
01863 IkReal x444=((cj8)*(r10));
01864 IkReal x445=((sj5)*(x436));
01865 IkReal x446=((sj5)*(x435));
01866 IkReal x447=((r11)*(sj3)*(sj8));
01867 IkReal x448=((cj3)*(cj8)*(r00));
01868 IkReal x449=((cj3)*(r01)*(sj8));
01869 IkReal x450=((x435)*(x443));
01870 evalcond[0]=((((IkReal(-1.00000000000000))*(x437)*(x438)))+(((cj7)*(x442)))+(((IkReal(-1.00000000000000))*(x450)))+(x445)+(((IkReal(-1.00000000000000))*(r22)*(x440))));
01871 evalcond[1]=((((IkReal(-1.00000000000000))*(x438)*(x440)))+(((IkReal(-1.00000000000000))*(x446)))+(((sj7)*(x442)))+(((IkReal(-1.00000000000000))*(x436)*(x443)))+(((cj7)*(r22))));
01872 evalcond[2]=((((IkReal(-1.00000000000000))*(x437)*(x447)))+(((IkReal(-1.00000000000000))*(x437)*(x449)))+(((sj7)*(x439)))+(((cj5)*(x436)))+(((cj7)*(sj3)*(x444)))+(((cj7)*(x448)))+(x446)+(((r12)*(x441))));
01873 evalcond[3]=((((IkReal(-1.00000000000000))*(x437)*(x439)))+(((sj7)*(x448)))+(((IkReal(-1.00000000000000))*(x450)))+(((x441)*(x444)))+(x445)+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x437)))+(((IkReal(-1.00000000000000))*(x440)*(x447)))+(((IkReal(-1.00000000000000))*(x440)*(x449))));
01874 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01875 {
01876 continue;
01877 }
01878 }
01879
01880 {
01881 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01882 vinfos[0].jointtype = 1;
01883 vinfos[0].foffset = j3;
01884 vinfos[0].indices[0] = _ij3[0];
01885 vinfos[0].indices[1] = _ij3[1];
01886 vinfos[0].maxsolutions = _nj3;
01887 vinfos[1].jointtype = 1;
01888 vinfos[1].foffset = j4;
01889 vinfos[1].indices[0] = _ij4[0];
01890 vinfos[1].indices[1] = _ij4[1];
01891 vinfos[1].maxsolutions = _nj4;
01892 vinfos[2].jointtype = 1;
01893 vinfos[2].foffset = j5;
01894 vinfos[2].indices[0] = _ij5[0];
01895 vinfos[2].indices[1] = _ij5[1];
01896 vinfos[2].maxsolutions = _nj5;
01897 vinfos[3].jointtype = 1;
01898 vinfos[3].foffset = j6;
01899 vinfos[3].indices[0] = _ij6[0];
01900 vinfos[3].indices[1] = _ij6[1];
01901 vinfos[3].maxsolutions = _nj6;
01902 vinfos[4].jointtype = 1;
01903 vinfos[4].foffset = j7;
01904 vinfos[4].indices[0] = _ij7[0];
01905 vinfos[4].indices[1] = _ij7[1];
01906 vinfos[4].maxsolutions = _nj7;
01907 vinfos[5].jointtype = 1;
01908 vinfos[5].foffset = j8;
01909 vinfos[5].indices[0] = _ij8[0];
01910 vinfos[5].indices[1] = _ij8[1];
01911 vinfos[5].maxsolutions = _nj8;
01912 std::vector<int> vfree(0);
01913 solutions.AddSolution(vinfos,vfree);
01914 }
01915 }
01916 }
01917
01918 }
01919
01920 }
01921
01922 } else
01923 {
01924 {
01925 IkReal j4array[1], cj4array[1], sj4array[1];
01926 bool j4valid[1]={false};
01927 _nj4 = 1;
01928 IkReal x451=((cj7)*(sj5));
01929 IkReal x452=((r21)*(sj8));
01930 IkReal x453=((cj5)*(cj7));
01931 IkReal x454=((cj8)*(r20));
01932 IkReal x455=((sj5)*(sj7));
01933 IkReal x456=((IkReal(1.00000000000000))*(cj5)*(sj7));
01934 if( IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(x454)*(x456)))+(((x451)*(x454)))+(((r22)*(x455)))+(((r22)*(x453)))+(((IkReal(-1.00000000000000))*(x451)*(x452)))+(((cj5)*(sj7)*(x452))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((x452)*(x453)))+(((x452)*(x455)))+(((IkReal(-1.00000000000000))*(x453)*(x454)))+(((IkReal(-1.00000000000000))*(r22)*(x456)))+(((IkReal(-1.00000000000000))*(x454)*(x455)))+(((r22)*(x451))))))) < IKFAST_ATAN2_MAGTHRESH )
01935 continue;
01936 j4array[0]=IKatan2(((gconst78)*(((((IkReal(-1.00000000000000))*(x454)*(x456)))+(((x451)*(x454)))+(((r22)*(x455)))+(((r22)*(x453)))+(((IkReal(-1.00000000000000))*(x451)*(x452)))+(((cj5)*(sj7)*(x452)))))), ((gconst78)*(((((x452)*(x453)))+(((x452)*(x455)))+(((IkReal(-1.00000000000000))*(x453)*(x454)))+(((IkReal(-1.00000000000000))*(r22)*(x456)))+(((IkReal(-1.00000000000000))*(x454)*(x455)))+(((r22)*(x451)))))));
01937 sj4array[0]=IKsin(j4array[0]);
01938 cj4array[0]=IKcos(j4array[0]);
01939 if( j4array[0] > IKPI )
01940 {
01941 j4array[0]-=IK2PI;
01942 }
01943 else if( j4array[0] < -IKPI )
01944 { j4array[0]+=IK2PI;
01945 }
01946 j4valid[0] = true;
01947 for(int ij4 = 0; ij4 < 1; ++ij4)
01948 {
01949 if( !j4valid[ij4] )
01950 {
01951 continue;
01952 }
01953 _ij4[0] = ij4; _ij4[1] = -1;
01954 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
01955 {
01956 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
01957 {
01958 j4valid[iij4]=false; _ij4[1] = iij4; break;
01959 }
01960 }
01961 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01962 {
01963 IkReal evalcond[4];
01964 IkReal x457=IKcos(j4);
01965 IkReal x458=IKsin(j4);
01966 IkReal x459=((IkReal(1.00000000000000))*(cj7));
01967 IkReal x460=((cj8)*(r20));
01968 IkReal x461=((cj3)*(r02));
01969 IkReal x462=((IkReal(1.00000000000000))*(sj7));
01970 IkReal x463=((sj3)*(sj7));
01971 IkReal x464=((r21)*(sj8));
01972 IkReal x465=((IkReal(1.00000000000000))*(cj5));
01973 IkReal x466=((cj8)*(r10));
01974 IkReal x467=((sj5)*(x458));
01975 IkReal x468=((sj5)*(x457));
01976 IkReal x469=((r11)*(sj3)*(sj8));
01977 IkReal x470=((cj3)*(cj8)*(r00));
01978 IkReal x471=((cj3)*(r01)*(sj8));
01979 IkReal x472=((x457)*(x465));
01980 evalcond[0]=((((IkReal(-1.00000000000000))*(x472)))+(((IkReal(-1.00000000000000))*(x459)*(x460)))+(x467)+(((IkReal(-1.00000000000000))*(r22)*(x462)))+(((cj7)*(x464))));
01981 evalcond[1]=((((sj7)*(x464)))+(((IkReal(-1.00000000000000))*(x460)*(x462)))+(((IkReal(-1.00000000000000))*(x468)))+(((IkReal(-1.00000000000000))*(x458)*(x465)))+(((cj7)*(r22))));
01982 evalcond[2]=((((cj5)*(x458)))+(((IkReal(-1.00000000000000))*(x459)*(x471)))+(((sj7)*(x461)))+(((IkReal(-1.00000000000000))*(x459)*(x469)))+(((cj7)*(sj3)*(x466)))+(((r12)*(x463)))+(x468)+(((cj7)*(x470))));
01983 evalcond[3]=((((IkReal(-1.00000000000000))*(x462)*(x471)))+(((IkReal(-1.00000000000000))*(x462)*(x469)))+(((x463)*(x466)))+(((IkReal(-1.00000000000000))*(x472)))+(((IkReal(-1.00000000000000))*(x459)*(x461)))+(x467)+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x459)))+(((sj7)*(x470))));
01984 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01985 {
01986 continue;
01987 }
01988 }
01989
01990 {
01991 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01992 vinfos[0].jointtype = 1;
01993 vinfos[0].foffset = j3;
01994 vinfos[0].indices[0] = _ij3[0];
01995 vinfos[0].indices[1] = _ij3[1];
01996 vinfos[0].maxsolutions = _nj3;
01997 vinfos[1].jointtype = 1;
01998 vinfos[1].foffset = j4;
01999 vinfos[1].indices[0] = _ij4[0];
02000 vinfos[1].indices[1] = _ij4[1];
02001 vinfos[1].maxsolutions = _nj4;
02002 vinfos[2].jointtype = 1;
02003 vinfos[2].foffset = j5;
02004 vinfos[2].indices[0] = _ij5[0];
02005 vinfos[2].indices[1] = _ij5[1];
02006 vinfos[2].maxsolutions = _nj5;
02007 vinfos[3].jointtype = 1;
02008 vinfos[3].foffset = j6;
02009 vinfos[3].indices[0] = _ij6[0];
02010 vinfos[3].indices[1] = _ij6[1];
02011 vinfos[3].maxsolutions = _nj6;
02012 vinfos[4].jointtype = 1;
02013 vinfos[4].foffset = j7;
02014 vinfos[4].indices[0] = _ij7[0];
02015 vinfos[4].indices[1] = _ij7[1];
02016 vinfos[4].maxsolutions = _nj7;
02017 vinfos[5].jointtype = 1;
02018 vinfos[5].foffset = j8;
02019 vinfos[5].indices[0] = _ij8[0];
02020 vinfos[5].indices[1] = _ij8[1];
02021 vinfos[5].maxsolutions = _nj8;
02022 std::vector<int> vfree(0);
02023 solutions.AddSolution(vinfos,vfree);
02024 }
02025 }
02026 }
02027
02028 }
02029
02030 }
02031 }
02032 }
02033
02034 }
02035
02036 }
02037
02038 } else
02039 {
02040 {
02041 IkReal j4array[1], cj4array[1], sj4array[1];
02042 bool j4valid[1]={false};
02043 _nj4 = 1;
02044 IkReal x473=((cj7)*(sj5));
02045 IkReal x474=((r21)*(sj8));
02046 IkReal x475=((cj5)*(cj7));
02047 IkReal x476=((cj8)*(r20));
02048 IkReal x477=((sj5)*(sj7));
02049 IkReal x478=((IkReal(1.00000000000000))*(cj5)*(sj7));
02050 if( IKabs(((gconst77)*(((((x473)*(x476)))+(((IkReal(-1.00000000000000))*(x476)*(x478)))+(((r22)*(x475)))+(((r22)*(x477)))+(((IkReal(-1.00000000000000))*(x473)*(x474)))+(((cj5)*(sj7)*(x474))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(x475)*(x476)))+(((IkReal(-1.00000000000000))*(x476)*(x477)))+(((IkReal(-1.00000000000000))*(r22)*(x478)))+(((x474)*(x477)))+(((x474)*(x475)))+(((r22)*(x473))))))) < IKFAST_ATAN2_MAGTHRESH )
02051 continue;
02052 j4array[0]=IKatan2(((gconst77)*(((((x473)*(x476)))+(((IkReal(-1.00000000000000))*(x476)*(x478)))+(((r22)*(x475)))+(((r22)*(x477)))+(((IkReal(-1.00000000000000))*(x473)*(x474)))+(((cj5)*(sj7)*(x474)))))), ((gconst77)*(((((IkReal(-1.00000000000000))*(x475)*(x476)))+(((IkReal(-1.00000000000000))*(x476)*(x477)))+(((IkReal(-1.00000000000000))*(r22)*(x478)))+(((x474)*(x477)))+(((x474)*(x475)))+(((r22)*(x473)))))));
02053 sj4array[0]=IKsin(j4array[0]);
02054 cj4array[0]=IKcos(j4array[0]);
02055 if( j4array[0] > IKPI )
02056 {
02057 j4array[0]-=IK2PI;
02058 }
02059 else if( j4array[0] < -IKPI )
02060 { j4array[0]+=IK2PI;
02061 }
02062 j4valid[0] = true;
02063 for(int ij4 = 0; ij4 < 1; ++ij4)
02064 {
02065 if( !j4valid[ij4] )
02066 {
02067 continue;
02068 }
02069 _ij4[0] = ij4; _ij4[1] = -1;
02070 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02071 {
02072 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02073 {
02074 j4valid[iij4]=false; _ij4[1] = iij4; break;
02075 }
02076 }
02077 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02078 {
02079 IkReal evalcond[2];
02080 IkReal x479=IKcos(j4);
02081 IkReal x480=IKsin(j4);
02082 IkReal x481=((IkReal(1.00000000000000))*(cj5));
02083 IkReal x482=((r21)*(sj8));
02084 IkReal x483=((IkReal(1.00000000000000))*(cj8)*(r20));
02085 evalcond[0]=((((cj7)*(x482)))+(((IkReal(-1.00000000000000))*(x479)*(x481)))+(((sj5)*(x480)))+(((IkReal(-1.00000000000000))*(cj7)*(x483)))+(((IkReal(-1.00000000000000))*(r22)*(sj7))));
02086 evalcond[1]=((((sj7)*(x482)))+(((IkReal(-1.00000000000000))*(sj5)*(x479)))+(((IkReal(-1.00000000000000))*(sj7)*(x483)))+(((IkReal(-1.00000000000000))*(x480)*(x481)))+(((cj7)*(r22))));
02087 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
02088 {
02089 continue;
02090 }
02091 }
02092
02093 {
02094 IkReal dummyeval[1];
02095 IkReal gconst80;
02096 IkReal x484=(cj8)*(cj8);
02097 IkReal x485=(sj8)*(sj8);
02098 IkReal x486=((IkReal(2.00000000000000))*(cj8)*(sj8));
02099 IkReal x487=((IkReal(1.00000000000000))*(x484));
02100 IkReal x488=((IkReal(1.00000000000000))*(x485));
02101 gconst80=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x486)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x486)))+(((IkReal(-1.00000000000000))*(x487)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x487)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x488)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x488)*((r10)*(r10))))));
02102 IkReal x489=(cj8)*(cj8);
02103 IkReal x490=(sj8)*(sj8);
02104 IkReal x491=((IkReal(2.00000000000000))*(cj8)*(sj8));
02105 IkReal x492=((IkReal(1.00000000000000))*(x489));
02106 IkReal x493=((IkReal(1.00000000000000))*(x490));
02107 dummyeval[0]=((((IkReal(-1.00000000000000))*(x493)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x492)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x491)))+(((IkReal(-1.00000000000000))*(x493)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x491)))+(((IkReal(-1.00000000000000))*(x492)*((r11)*(r11)))));
02108 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02109 {
02110 {
02111 IkReal dummyeval[1];
02112 IkReal gconst81;
02113 IkReal x494=(sj8)*(sj8);
02114 IkReal x495=(cj8)*(cj8);
02115 IkReal x496=((sj7)*(sj8));
02116 IkReal x497=((IkReal(1.00000000000000))*(r02));
02117 IkReal x498=((r01)*(r10));
02118 IkReal x499=((cj8)*(sj7));
02119 IkReal x500=((cj7)*(x494));
02120 IkReal x501=((IkReal(1.00000000000000))*(r00)*(r11));
02121 IkReal x502=((cj7)*(x495));
02122 gconst81=IKsign(((((IkReal(-1.00000000000000))*(x501)*(x502)))+(((r00)*(r12)*(x496)))+(((x498)*(x502)))+(((x498)*(x500)))+(((IkReal(-1.00000000000000))*(r11)*(x497)*(x499)))+(((IkReal(-1.00000000000000))*(r10)*(x496)*(x497)))+(((IkReal(-1.00000000000000))*(x500)*(x501)))+(((r01)*(r12)*(x499)))));
02123 IkReal x503=(sj8)*(sj8);
02124 IkReal x504=(cj8)*(cj8);
02125 IkReal x505=((sj7)*(sj8));
02126 IkReal x506=((IkReal(1.00000000000000))*(r02));
02127 IkReal x507=((r01)*(r10));
02128 IkReal x508=((cj8)*(sj7));
02129 IkReal x509=((cj7)*(x503));
02130 IkReal x510=((IkReal(1.00000000000000))*(r00)*(r11));
02131 IkReal x511=((cj7)*(x504));
02132 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x506)*(x508)))+(((x507)*(x511)))+(((x507)*(x509)))+(((r01)*(r12)*(x508)))+(((IkReal(-1.00000000000000))*(x509)*(x510)))+(((IkReal(-1.00000000000000))*(x510)*(x511)))+(((r00)*(r12)*(x505)))+(((IkReal(-1.00000000000000))*(r10)*(x505)*(x506))));
02133 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02134 {
02135 continue;
02136
02137 } else
02138 {
02139 {
02140 IkReal j3array[1], cj3array[1], sj3array[1];
02141 bool j3valid[1]={false};
02142 _nj3 = 1;
02143 IkReal x512=((cj7)*(cj8));
02144 IkReal x513=((IkReal(1.00000000000000))*(cj7)*(sj8));
02145 if( IKabs(((gconst81)*(((((IkReal(-1.00000000000000))*(r11)*(x513)))+(((r10)*(x512)))+(((r12)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst81)*(((((IkReal(-1.00000000000000))*(r01)*(x513)))+(((r02)*(sj7)))+(((r00)*(x512))))))) < IKFAST_ATAN2_MAGTHRESH )
02146 continue;
02147 j3array[0]=IKatan2(((gconst81)*(((((IkReal(-1.00000000000000))*(r11)*(x513)))+(((r10)*(x512)))+(((r12)*(sj7)))))), ((gconst81)*(((((IkReal(-1.00000000000000))*(r01)*(x513)))+(((r02)*(sj7)))+(((r00)*(x512)))))));
02148 sj3array[0]=IKsin(j3array[0]);
02149 cj3array[0]=IKcos(j3array[0]);
02150 if( j3array[0] > IKPI )
02151 {
02152 j3array[0]-=IK2PI;
02153 }
02154 else if( j3array[0] < -IKPI )
02155 { j3array[0]+=IK2PI;
02156 }
02157 j3valid[0] = true;
02158 for(int ij3 = 0; ij3 < 1; ++ij3)
02159 {
02160 if( !j3valid[ij3] )
02161 {
02162 continue;
02163 }
02164 _ij3[0] = ij3; _ij3[1] = -1;
02165 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02166 {
02167 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02168 {
02169 j3valid[iij3]=false; _ij3[1] = iij3; break;
02170 }
02171 }
02172 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02173 {
02174 IkReal evalcond[6];
02175 IkReal x514=IKsin(j3);
02176 IkReal x515=IKcos(j3);
02177 IkReal x516=((r01)*(sj8));
02178 IkReal x517=((cj8)*(r00));
02179 IkReal x518=((r00)*(sj8));
02180 IkReal x519=((cj8)*(r11));
02181 IkReal x520=((r11)*(sj8));
02182 IkReal x521=((r10)*(sj8));
02183 IkReal x522=((cj8)*(r10));
02184 IkReal x523=((cj7)*(x514));
02185 IkReal x524=((sj7)*(x515));
02186 IkReal x525=((IkReal(1.00000000000000))*(x514));
02187 IkReal x526=((IkReal(1.00000000000000))*(x515));
02188 IkReal x527=((cj8)*(x514));
02189 IkReal x528=((IkReal(1.00000000000000))*(x520));
02190 IkReal x529=((sj7)*(x514));
02191 IkReal x530=((cj7)*(x515));
02192 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x521)*(x526)))+(((x514)*(x518)))+(((r01)*(x527)))+(((IkReal(-1.00000000000000))*(x519)*(x526))));
02193 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x526)))+(((IkReal(-1.00000000000000))*(x521)*(x525)))+(((IkReal(-1.00000000000000))*(x518)*(x526)))+(((IkReal(-1.00000000000000))*(x519)*(x525))));
02194 evalcond[2]=((((x522)*(x530)))+(((IkReal(-1.00000000000000))*(cj7)*(x520)*(x526)))+(((r12)*(x524)))+(((x516)*(x523)))+(((IkReal(-1.00000000000000))*(x517)*(x523)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x525))));
02195 evalcond[3]=((((x522)*(x524)))+(((IkReal(-1.00000000000000))*(x524)*(x528)))+(((r02)*(x523)))+(((IkReal(-1.00000000000000))*(sj7)*(x517)*(x525)))+(((x516)*(x529)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x526))));
02196 evalcond[4]=((((x522)*(x523)))+(((r02)*(x524)))+(((IkReal(-1.00000000000000))*(x523)*(x528)))+(((r12)*(x529)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((x517)*(x530)))+(((IkReal(-1.00000000000000))*(cj7)*(x516)*(x526))));
02197 evalcond[5]=((((x522)*(x529)))+(((x517)*(x524)))+(((IkReal(-1.00000000000000))*(x516)*(x524)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x526)))+(((IkReal(-1.00000000000000))*(r12)*(x523)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(sj7)*(x520)*(x525))));
02198 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 )
02199 {
02200 continue;
02201 }
02202 }
02203
02204 {
02205 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02206 vinfos[0].jointtype = 1;
02207 vinfos[0].foffset = j3;
02208 vinfos[0].indices[0] = _ij3[0];
02209 vinfos[0].indices[1] = _ij3[1];
02210 vinfos[0].maxsolutions = _nj3;
02211 vinfos[1].jointtype = 1;
02212 vinfos[1].foffset = j4;
02213 vinfos[1].indices[0] = _ij4[0];
02214 vinfos[1].indices[1] = _ij4[1];
02215 vinfos[1].maxsolutions = _nj4;
02216 vinfos[2].jointtype = 1;
02217 vinfos[2].foffset = j5;
02218 vinfos[2].indices[0] = _ij5[0];
02219 vinfos[2].indices[1] = _ij5[1];
02220 vinfos[2].maxsolutions = _nj5;
02221 vinfos[3].jointtype = 1;
02222 vinfos[3].foffset = j6;
02223 vinfos[3].indices[0] = _ij6[0];
02224 vinfos[3].indices[1] = _ij6[1];
02225 vinfos[3].maxsolutions = _nj6;
02226 vinfos[4].jointtype = 1;
02227 vinfos[4].foffset = j7;
02228 vinfos[4].indices[0] = _ij7[0];
02229 vinfos[4].indices[1] = _ij7[1];
02230 vinfos[4].maxsolutions = _nj7;
02231 vinfos[5].jointtype = 1;
02232 vinfos[5].foffset = j8;
02233 vinfos[5].indices[0] = _ij8[0];
02234 vinfos[5].indices[1] = _ij8[1];
02235 vinfos[5].maxsolutions = _nj8;
02236 std::vector<int> vfree(0);
02237 solutions.AddSolution(vinfos,vfree);
02238 }
02239 }
02240 }
02241
02242 }
02243
02244 }
02245
02246 } else
02247 {
02248 {
02249 IkReal j3array[1], cj3array[1], sj3array[1];
02250 bool j3valid[1]={false};
02251 _nj3 = 1;
02252 if( IKabs(((gconst80)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst80)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
02253 continue;
02254 j3array[0]=IKatan2(((gconst80)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst80)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
02255 sj3array[0]=IKsin(j3array[0]);
02256 cj3array[0]=IKcos(j3array[0]);
02257 if( j3array[0] > IKPI )
02258 {
02259 j3array[0]-=IK2PI;
02260 }
02261 else if( j3array[0] < -IKPI )
02262 { j3array[0]+=IK2PI;
02263 }
02264 j3valid[0] = true;
02265 for(int ij3 = 0; ij3 < 1; ++ij3)
02266 {
02267 if( !j3valid[ij3] )
02268 {
02269 continue;
02270 }
02271 _ij3[0] = ij3; _ij3[1] = -1;
02272 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02273 {
02274 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02275 {
02276 j3valid[iij3]=false; _ij3[1] = iij3; break;
02277 }
02278 }
02279 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02280 {
02281 IkReal evalcond[6];
02282 IkReal x531=IKsin(j3);
02283 IkReal x532=IKcos(j3);
02284 IkReal x533=((r01)*(sj8));
02285 IkReal x534=((cj8)*(r00));
02286 IkReal x535=((r00)*(sj8));
02287 IkReal x536=((cj8)*(r11));
02288 IkReal x537=((r11)*(sj8));
02289 IkReal x538=((r10)*(sj8));
02290 IkReal x539=((cj8)*(r10));
02291 IkReal x540=((cj7)*(x531));
02292 IkReal x541=((sj7)*(x532));
02293 IkReal x542=((IkReal(1.00000000000000))*(x531));
02294 IkReal x543=((IkReal(1.00000000000000))*(x532));
02295 IkReal x544=((cj8)*(x531));
02296 IkReal x545=((IkReal(1.00000000000000))*(x537));
02297 IkReal x546=((sj7)*(x531));
02298 IkReal x547=((cj7)*(x532));
02299 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x538)*(x543)))+(((x531)*(x535)))+(((IkReal(-1.00000000000000))*(x536)*(x543)))+(((r01)*(x544))));
02300 evalcond[1]=((((IkReal(-1.00000000000000))*(x538)*(x542)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x543)))+(((IkReal(-1.00000000000000))*(x535)*(x543)))+(((IkReal(-1.00000000000000))*(x536)*(x542))));
02301 evalcond[2]=((((x539)*(x547)))+(((IkReal(-1.00000000000000))*(cj7)*(x537)*(x543)))+(((r12)*(x541)))+(((IkReal(-1.00000000000000))*(x534)*(x540)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x542)))+(((x533)*(x540))));
02302 evalcond[3]=((((IkReal(-1.00000000000000))*(x541)*(x545)))+(((x539)*(x541)))+(((r02)*(x540)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x543)))+(((x533)*(x546)))+(((IkReal(-1.00000000000000))*(sj7)*(x534)*(x542))));
02303 evalcond[4]=((((x539)*(x540)))+(((r02)*(x541)))+(((IkReal(-1.00000000000000))*(x540)*(x545)))+(((cj5)*(sj4)))+(((r12)*(x546)))+(((IkReal(-1.00000000000000))*(cj7)*(x533)*(x543)))+(((cj4)*(sj5)))+(((x534)*(x547))));
02304 evalcond[5]=((((x539)*(x546)))+(((IkReal(-1.00000000000000))*(sj7)*(x537)*(x542)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x543)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x533)*(x541)))+(((IkReal(-1.00000000000000))*(r12)*(x540)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)))+(((x534)*(x541))));
02305 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 )
02306 {
02307 continue;
02308 }
02309 }
02310
02311 {
02312 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02313 vinfos[0].jointtype = 1;
02314 vinfos[0].foffset = j3;
02315 vinfos[0].indices[0] = _ij3[0];
02316 vinfos[0].indices[1] = _ij3[1];
02317 vinfos[0].maxsolutions = _nj3;
02318 vinfos[1].jointtype = 1;
02319 vinfos[1].foffset = j4;
02320 vinfos[1].indices[0] = _ij4[0];
02321 vinfos[1].indices[1] = _ij4[1];
02322 vinfos[1].maxsolutions = _nj4;
02323 vinfos[2].jointtype = 1;
02324 vinfos[2].foffset = j5;
02325 vinfos[2].indices[0] = _ij5[0];
02326 vinfos[2].indices[1] = _ij5[1];
02327 vinfos[2].maxsolutions = _nj5;
02328 vinfos[3].jointtype = 1;
02329 vinfos[3].foffset = j6;
02330 vinfos[3].indices[0] = _ij6[0];
02331 vinfos[3].indices[1] = _ij6[1];
02332 vinfos[3].maxsolutions = _nj6;
02333 vinfos[4].jointtype = 1;
02334 vinfos[4].foffset = j7;
02335 vinfos[4].indices[0] = _ij7[0];
02336 vinfos[4].indices[1] = _ij7[1];
02337 vinfos[4].maxsolutions = _nj7;
02338 vinfos[5].jointtype = 1;
02339 vinfos[5].foffset = j8;
02340 vinfos[5].indices[0] = _ij8[0];
02341 vinfos[5].indices[1] = _ij8[1];
02342 vinfos[5].maxsolutions = _nj8;
02343 std::vector<int> vfree(0);
02344 solutions.AddSolution(vinfos,vfree);
02345 }
02346 }
02347 }
02348
02349 }
02350
02351 }
02352 }
02353 }
02354
02355 }
02356
02357 }
02358
02359 } else
02360 {
02361 {
02362 IkReal j3array[1], cj3array[1], sj3array[1];
02363 bool j3valid[1]={false};
02364 _nj3 = 1;
02365 if( IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst75)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
02366 continue;
02367 j3array[0]=IKatan2(((gconst75)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst75)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
02368 sj3array[0]=IKsin(j3array[0]);
02369 cj3array[0]=IKcos(j3array[0]);
02370 if( j3array[0] > IKPI )
02371 {
02372 j3array[0]-=IK2PI;
02373 }
02374 else if( j3array[0] < -IKPI )
02375 { j3array[0]+=IK2PI;
02376 }
02377 j3valid[0] = true;
02378 for(int ij3 = 0; ij3 < 1; ++ij3)
02379 {
02380 if( !j3valid[ij3] )
02381 {
02382 continue;
02383 }
02384 _ij3[0] = ij3; _ij3[1] = -1;
02385 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02386 {
02387 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02388 {
02389 j3valid[iij3]=false; _ij3[1] = iij3; break;
02390 }
02391 }
02392 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02393 {
02394 IkReal evalcond[4];
02395 IkReal x548=IKsin(j3);
02396 IkReal x549=IKcos(j3);
02397 IkReal x550=((IkReal(1.00000000000000))*(sj8));
02398 IkReal x551=((IkReal(1.00000000000000))*(cj8));
02399 IkReal x552=((r01)*(sj8));
02400 IkReal x553=((cj8)*(r10));
02401 IkReal x554=((sj7)*(x549));
02402 IkReal x555=((cj7)*(x548));
02403 IkReal x556=((r00)*(x548));
02404 IkReal x557=((sj7)*(x548));
02405 IkReal x558=((cj7)*(x549));
02406 evalcond[0]=((IkReal(-1.00000000000000))+(((sj8)*(x556)))+(((IkReal(-1.00000000000000))*(r11)*(x549)*(x551)))+(((cj8)*(r01)*(x548)))+(((IkReal(-1.00000000000000))*(r10)*(x549)*(x550))));
02407 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x549)*(x551)))+(((IkReal(-1.00000000000000))*(r10)*(x548)*(x550)))+(((IkReal(-1.00000000000000))*(r11)*(x548)*(x551)))+(((IkReal(-1.00000000000000))*(r00)*(x549)*(x550))));
02408 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x557)))+(((r12)*(x554)))+(((IkReal(-1.00000000000000))*(r00)*(x551)*(x555)))+(((IkReal(-1.00000000000000))*(r11)*(x550)*(x558)))+(((x552)*(x555)))+(((x553)*(x558))));
02409 evalcond[3]=((((IkReal(-1.00000000000000))*(sj7)*(x551)*(x556)))+(((r02)*(x555)))+(((IkReal(-1.00000000000000))*(r12)*(x558)))+(((IkReal(-1.00000000000000))*(r11)*(x550)*(x554)))+(((x552)*(x557)))+(((x553)*(x554))));
02410 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02411 {
02412 continue;
02413 }
02414 }
02415
02416 {
02417 IkReal dummyeval[1];
02418 IkReal gconst78;
02419 gconst78=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
02420 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
02421 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02422 {
02423 {
02424 IkReal dummyeval[1];
02425 IkReal gconst79;
02426 gconst79=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
02427 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
02428 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02429 {
02430 continue;
02431
02432 } else
02433 {
02434 {
02435 IkReal j4array[1], cj4array[1], sj4array[1];
02436 bool j4valid[1]={false};
02437 _nj4 = 1;
02438 IkReal x559=((cj3)*(cj5));
02439 IkReal x560=((r02)*(sj7));
02440 IkReal x561=((cj5)*(sj7));
02441 IkReal x562=((cj3)*(sj5));
02442 IkReal x563=((r11)*(sj3));
02443 IkReal x564=((r10)*(sj3));
02444 IkReal x565=((r12)*(sj3));
02445 IkReal x566=((sj5)*(sj7));
02446 IkReal x567=((cj7)*(cj8)*(sj5));
02447 IkReal x568=((IkReal(1.00000000000000))*(cj7)*(sj8));
02448 IkReal x569=((cj5)*(cj7)*(cj8));
02449 if( IKabs(((gconst79)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(cj5)*(x563)*(x568)))+(((x564)*(x569)))+(((cj7)*(cj8)*(r00)*(x559)))+(((IkReal(-1.00000000000000))*(r22)*(x566)))+(((x561)*(x565)))+(((IkReal(-1.00000000000000))*(r20)*(x567)))+(((x559)*(x560)))+(((IkReal(-1.00000000000000))*(r01)*(x559)*(x568))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((x565)*(x566)))+(((x564)*(x567)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x568)))+(((r22)*(x561)))+(((IkReal(-1.00000000000000))*(sj5)*(x563)*(x568)))+(((IkReal(-1.00000000000000))*(r01)*(x562)*(x568)))+(((x560)*(x562)))+(((r20)*(x569)))+(((cj7)*(cj8)*(r00)*(x562))))))) < IKFAST_ATAN2_MAGTHRESH )
02450 continue;
02451 j4array[0]=IKatan2(((gconst79)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(cj5)*(x563)*(x568)))+(((x564)*(x569)))+(((cj7)*(cj8)*(r00)*(x559)))+(((IkReal(-1.00000000000000))*(r22)*(x566)))+(((x561)*(x565)))+(((IkReal(-1.00000000000000))*(r20)*(x567)))+(((x559)*(x560)))+(((IkReal(-1.00000000000000))*(r01)*(x559)*(x568)))))), ((gconst79)*(((((x565)*(x566)))+(((x564)*(x567)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x568)))+(((r22)*(x561)))+(((IkReal(-1.00000000000000))*(sj5)*(x563)*(x568)))+(((IkReal(-1.00000000000000))*(r01)*(x562)*(x568)))+(((x560)*(x562)))+(((r20)*(x569)))+(((cj7)*(cj8)*(r00)*(x562)))))));
02452 sj4array[0]=IKsin(j4array[0]);
02453 cj4array[0]=IKcos(j4array[0]);
02454 if( j4array[0] > IKPI )
02455 {
02456 j4array[0]-=IK2PI;
02457 }
02458 else if( j4array[0] < -IKPI )
02459 { j4array[0]+=IK2PI;
02460 }
02461 j4valid[0] = true;
02462 for(int ij4 = 0; ij4 < 1; ++ij4)
02463 {
02464 if( !j4valid[ij4] )
02465 {
02466 continue;
02467 }
02468 _ij4[0] = ij4; _ij4[1] = -1;
02469 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02470 {
02471 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02472 {
02473 j4valid[iij4]=false; _ij4[1] = iij4; break;
02474 }
02475 }
02476 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02477 {
02478 IkReal evalcond[4];
02479 IkReal x570=IKcos(j4);
02480 IkReal x571=IKsin(j4);
02481 IkReal x572=((IkReal(1.00000000000000))*(cj7));
02482 IkReal x573=((cj8)*(r20));
02483 IkReal x574=((cj3)*(r02));
02484 IkReal x575=((IkReal(1.00000000000000))*(sj7));
02485 IkReal x576=((sj3)*(sj7));
02486 IkReal x577=((r21)*(sj8));
02487 IkReal x578=((IkReal(1.00000000000000))*(cj5));
02488 IkReal x579=((cj8)*(r10));
02489 IkReal x580=((sj5)*(x571));
02490 IkReal x581=((sj5)*(x570));
02491 IkReal x582=((r11)*(sj3)*(sj8));
02492 IkReal x583=((cj3)*(cj8)*(r00));
02493 IkReal x584=((cj3)*(r01)*(sj8));
02494 IkReal x585=((x570)*(x578));
02495 evalcond[0]=((((cj7)*(x577)))+(x580)+(((IkReal(-1.00000000000000))*(r22)*(x575)))+(((IkReal(-1.00000000000000))*(x585)))+(((IkReal(-1.00000000000000))*(x572)*(x573))));
02496 evalcond[1]=((((IkReal(-1.00000000000000))*(x581)))+(((IkReal(-1.00000000000000))*(x573)*(x575)))+(((IkReal(-1.00000000000000))*(x571)*(x578)))+(((sj7)*(x577)))+(((cj7)*(r22))));
02497 evalcond[2]=((((cj5)*(x571)))+(((r12)*(x576)))+(((IkReal(-1.00000000000000))*(x572)*(x582)))+(((IkReal(-1.00000000000000))*(x572)*(x584)))+(((cj7)*(x583)))+(((cj7)*(sj3)*(x579)))+(((sj7)*(x574)))+(x581));
02498 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj3)*(x572)))+(((x576)*(x579)))+(x580)+(((IkReal(-1.00000000000000))*(x575)*(x582)))+(((IkReal(-1.00000000000000))*(x575)*(x584)))+(((IkReal(-1.00000000000000))*(x585)))+(((IkReal(-1.00000000000000))*(x572)*(x574)))+(((sj7)*(x583))));
02499 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02500 {
02501 continue;
02502 }
02503 }
02504
02505 {
02506 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02507 vinfos[0].jointtype = 1;
02508 vinfos[0].foffset = j3;
02509 vinfos[0].indices[0] = _ij3[0];
02510 vinfos[0].indices[1] = _ij3[1];
02511 vinfos[0].maxsolutions = _nj3;
02512 vinfos[1].jointtype = 1;
02513 vinfos[1].foffset = j4;
02514 vinfos[1].indices[0] = _ij4[0];
02515 vinfos[1].indices[1] = _ij4[1];
02516 vinfos[1].maxsolutions = _nj4;
02517 vinfos[2].jointtype = 1;
02518 vinfos[2].foffset = j5;
02519 vinfos[2].indices[0] = _ij5[0];
02520 vinfos[2].indices[1] = _ij5[1];
02521 vinfos[2].maxsolutions = _nj5;
02522 vinfos[3].jointtype = 1;
02523 vinfos[3].foffset = j6;
02524 vinfos[3].indices[0] = _ij6[0];
02525 vinfos[3].indices[1] = _ij6[1];
02526 vinfos[3].maxsolutions = _nj6;
02527 vinfos[4].jointtype = 1;
02528 vinfos[4].foffset = j7;
02529 vinfos[4].indices[0] = _ij7[0];
02530 vinfos[4].indices[1] = _ij7[1];
02531 vinfos[4].maxsolutions = _nj7;
02532 vinfos[5].jointtype = 1;
02533 vinfos[5].foffset = j8;
02534 vinfos[5].indices[0] = _ij8[0];
02535 vinfos[5].indices[1] = _ij8[1];
02536 vinfos[5].maxsolutions = _nj8;
02537 std::vector<int> vfree(0);
02538 solutions.AddSolution(vinfos,vfree);
02539 }
02540 }
02541 }
02542
02543 }
02544
02545 }
02546
02547 } else
02548 {
02549 {
02550 IkReal j4array[1], cj4array[1], sj4array[1];
02551 bool j4valid[1]={false};
02552 _nj4 = 1;
02553 IkReal x586=((cj7)*(sj5));
02554 IkReal x587=((r21)*(sj8));
02555 IkReal x588=((cj5)*(cj7));
02556 IkReal x589=((cj8)*(r20));
02557 IkReal x590=((sj5)*(sj7));
02558 IkReal x591=((IkReal(1.00000000000000))*(cj5)*(sj7));
02559 if( IKabs(((gconst78)*(((((r22)*(x590)))+(((IkReal(-1.00000000000000))*(x589)*(x591)))+(((x586)*(x589)))+(((r22)*(x588)))+(((IkReal(-1.00000000000000))*(x586)*(x587)))+(((cj5)*(sj7)*(x587))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(x588)*(x589)))+(((IkReal(-1.00000000000000))*(x589)*(x590)))+(((x587)*(x588)))+(((IkReal(-1.00000000000000))*(r22)*(x591)))+(((r22)*(x586)))+(((x587)*(x590))))))) < IKFAST_ATAN2_MAGTHRESH )
02560 continue;
02561 j4array[0]=IKatan2(((gconst78)*(((((r22)*(x590)))+(((IkReal(-1.00000000000000))*(x589)*(x591)))+(((x586)*(x589)))+(((r22)*(x588)))+(((IkReal(-1.00000000000000))*(x586)*(x587)))+(((cj5)*(sj7)*(x587)))))), ((gconst78)*(((((IkReal(-1.00000000000000))*(x588)*(x589)))+(((IkReal(-1.00000000000000))*(x589)*(x590)))+(((x587)*(x588)))+(((IkReal(-1.00000000000000))*(r22)*(x591)))+(((r22)*(x586)))+(((x587)*(x590)))))));
02562 sj4array[0]=IKsin(j4array[0]);
02563 cj4array[0]=IKcos(j4array[0]);
02564 if( j4array[0] > IKPI )
02565 {
02566 j4array[0]-=IK2PI;
02567 }
02568 else if( j4array[0] < -IKPI )
02569 { j4array[0]+=IK2PI;
02570 }
02571 j4valid[0] = true;
02572 for(int ij4 = 0; ij4 < 1; ++ij4)
02573 {
02574 if( !j4valid[ij4] )
02575 {
02576 continue;
02577 }
02578 _ij4[0] = ij4; _ij4[1] = -1;
02579 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02580 {
02581 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02582 {
02583 j4valid[iij4]=false; _ij4[1] = iij4; break;
02584 }
02585 }
02586 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02587 {
02588 IkReal evalcond[4];
02589 IkReal x592=IKcos(j4);
02590 IkReal x593=IKsin(j4);
02591 IkReal x594=((IkReal(1.00000000000000))*(cj7));
02592 IkReal x595=((cj8)*(r20));
02593 IkReal x596=((cj3)*(r02));
02594 IkReal x597=((IkReal(1.00000000000000))*(sj7));
02595 IkReal x598=((sj3)*(sj7));
02596 IkReal x599=((r21)*(sj8));
02597 IkReal x600=((IkReal(1.00000000000000))*(cj5));
02598 IkReal x601=((cj8)*(r10));
02599 IkReal x602=((sj5)*(x593));
02600 IkReal x603=((sj5)*(x592));
02601 IkReal x604=((r11)*(sj3)*(sj8));
02602 IkReal x605=((cj3)*(cj8)*(r00));
02603 IkReal x606=((cj3)*(r01)*(sj8));
02604 IkReal x607=((x592)*(x600));
02605 evalcond[0]=((((IkReal(-1.00000000000000))*(x594)*(x595)))+(((IkReal(-1.00000000000000))*(r22)*(x597)))+(x602)+(((IkReal(-1.00000000000000))*(x607)))+(((cj7)*(x599))));
02606 evalcond[1]=((((IkReal(-1.00000000000000))*(x593)*(x600)))+(((sj7)*(x599)))+(((IkReal(-1.00000000000000))*(x603)))+(((IkReal(-1.00000000000000))*(x595)*(x597)))+(((cj7)*(r22))));
02607 evalcond[2]=((((cj7)*(x605)))+(((cj7)*(sj3)*(x601)))+(((sj7)*(x596)))+(((r12)*(x598)))+(x603)+(((IkReal(-1.00000000000000))*(x594)*(x606)))+(((IkReal(-1.00000000000000))*(x594)*(x604)))+(((cj5)*(x593))));
02608 evalcond[3]=((((sj7)*(x605)))+(((IkReal(-1.00000000000000))*(x597)*(x606)))+(((IkReal(-1.00000000000000))*(x597)*(x604)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x594)))+(((IkReal(-1.00000000000000))*(x594)*(x596)))+(x602)+(((IkReal(-1.00000000000000))*(x607)))+(((x598)*(x601))));
02609 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02610 {
02611 continue;
02612 }
02613 }
02614
02615 {
02616 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02617 vinfos[0].jointtype = 1;
02618 vinfos[0].foffset = j3;
02619 vinfos[0].indices[0] = _ij3[0];
02620 vinfos[0].indices[1] = _ij3[1];
02621 vinfos[0].maxsolutions = _nj3;
02622 vinfos[1].jointtype = 1;
02623 vinfos[1].foffset = j4;
02624 vinfos[1].indices[0] = _ij4[0];
02625 vinfos[1].indices[1] = _ij4[1];
02626 vinfos[1].maxsolutions = _nj4;
02627 vinfos[2].jointtype = 1;
02628 vinfos[2].foffset = j5;
02629 vinfos[2].indices[0] = _ij5[0];
02630 vinfos[2].indices[1] = _ij5[1];
02631 vinfos[2].maxsolutions = _nj5;
02632 vinfos[3].jointtype = 1;
02633 vinfos[3].foffset = j6;
02634 vinfos[3].indices[0] = _ij6[0];
02635 vinfos[3].indices[1] = _ij6[1];
02636 vinfos[3].maxsolutions = _nj6;
02637 vinfos[4].jointtype = 1;
02638 vinfos[4].foffset = j7;
02639 vinfos[4].indices[0] = _ij7[0];
02640 vinfos[4].indices[1] = _ij7[1];
02641 vinfos[4].maxsolutions = _nj7;
02642 vinfos[5].jointtype = 1;
02643 vinfos[5].foffset = j8;
02644 vinfos[5].indices[0] = _ij8[0];
02645 vinfos[5].indices[1] = _ij8[1];
02646 vinfos[5].maxsolutions = _nj8;
02647 std::vector<int> vfree(0);
02648 solutions.AddSolution(vinfos,vfree);
02649 }
02650 }
02651 }
02652
02653 }
02654
02655 }
02656 }
02657 }
02658
02659 }
02660
02661 }
02662 }
02663 }
02664
02665 } else
02666 {
02667 if( 1 )
02668 {
02669 continue;
02670
02671 } else
02672 {
02673 }
02674 }
02675 }
02676 }
02677
02678 } else
02679 {
02680 {
02681 IkReal j5array[1], cj5array[1], sj5array[1];
02682 bool j5valid[1]={false};
02683 _nj5 = 1;
02684 IkReal x608=((IkReal(4.00000000000000))*(npx));
02685 IkReal x609=((IkReal(4.00000000000000))*(npy));
02686 if( IKabs(((((IKabs(sj6) != 0)?((IkReal)1/(sj6)):(IkReal)1.0e30))*(((((IkReal(0.380000000000000))*(cj6)))+(((IkReal(-1.00000000000000))*(cj8)*(x609)))+(((IkReal(0.120000000000000))*(sj6)))+(((IkReal(-1.00000000000000))*(sj8)*(x608))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x609)))+(((cj7)*(cj8)*(x608)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj6) != 0)?((IkReal)1/(sj6)):(IkReal)1.0e30))*(((((IkReal(0.380000000000000))*(cj6)))+(((IkReal(-1.00000000000000))*(cj8)*(x609)))+(((IkReal(0.120000000000000))*(sj6)))+(((IkReal(-1.00000000000000))*(sj8)*(x608)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x609)))+(((cj7)*(cj8)*(x608)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
02687 continue;
02688 j5array[0]=IKatan2(((((IKabs(sj6) != 0)?((IkReal)1/(sj6)):(IkReal)1.0e30))*(((((IkReal(0.380000000000000))*(cj6)))+(((IkReal(-1.00000000000000))*(cj8)*(x609)))+(((IkReal(0.120000000000000))*(sj6)))+(((IkReal(-1.00000000000000))*(sj8)*(x608)))))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x609)))+(((cj7)*(cj8)*(x608)))+(((IkReal(-0.360000000000000))*(cj7)))));
02689 sj5array[0]=IKsin(j5array[0]);
02690 cj5array[0]=IKcos(j5array[0]);
02691 if( j5array[0] > IKPI )
02692 {
02693 j5array[0]-=IK2PI;
02694 }
02695 else if( j5array[0] < -IKPI )
02696 { j5array[0]+=IK2PI;
02697 }
02698 j5valid[0] = true;
02699 for(int ij5 = 0; ij5 < 1; ++ij5)
02700 {
02701 if( !j5valid[ij5] )
02702 {
02703 continue;
02704 }
02705 _ij5[0] = ij5; _ij5[1] = -1;
02706 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
02707 {
02708 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
02709 {
02710 j5valid[iij5]=false; _ij5[1] = iij5; break;
02711 }
02712 }
02713 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
02714 {
02715 IkReal evalcond[3];
02716 IkReal x610=IKsin(j5);
02717 IkReal x611=((IkReal(1.00000000000000))*(sj7));
02718 IkReal x612=((npy)*(sj8));
02719 IkReal x613=((cj8)*(npx));
02720 IkReal x614=((IkReal(0.250000000000000))*(x610));
02721 evalcond[0]=((((IkReal(-0.0300000000000000))*(sj6)))+(((IkReal(-0.0950000000000000))*(cj6)))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((sj6)*(x614))));
02722 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x613)))+(((IkReal(-1.00000000000000))*(npz)*(x611)))+(((cj7)*(x612)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
02723 evalcond[2]=((((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x611)*(x613)))+(((IkReal(-1.00000000000000))*(cj6)*(x614)))+(((IkReal(0.0300000000000000))*(cj6)))+(((IkReal(-0.0950000000000000))*(sj6)))+(((cj7)*(npz)))+(((sj7)*(x612))));
02724 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
02725 {
02726 continue;
02727 }
02728 }
02729
02730 {
02731 IkReal dummyeval[1];
02732 IkReal gconst4;
02733 gconst4=IKsign(((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5))))));
02734 dummyeval[0]=((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5)))));
02735 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02736 {
02737 {
02738 IkReal dummyeval[1];
02739 IkReal gconst5;
02740 gconst5=IKsign(((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5))))));
02741 dummyeval[0]=((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5)))));
02742 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02743 {
02744 {
02745 IkReal dummyeval[1];
02746 IkReal gconst2;
02747 IkReal x615=(sj8)*(sj8);
02748 IkReal x616=(cj8)*(cj8);
02749 IkReal x617=((r00)*(r11));
02750 IkReal x618=((r02)*(sj7));
02751 IkReal x619=((cj7)*(x616));
02752 IkReal x620=((IkReal(1.00000000000000))*(r12)*(sj7));
02753 IkReal x621=((IkReal(1.00000000000000))*(r01)*(r10));
02754 IkReal x622=((cj7)*(x615));
02755 gconst2=IKsign(((((cj8)*(r11)*(x618)))+(((IkReal(-1.00000000000000))*(x621)*(x622)))+(((x617)*(x622)))+(((r10)*(sj8)*(x618)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x620)))+(((x617)*(x619)))+(((IkReal(-1.00000000000000))*(x619)*(x621)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x620)))));
02756 IkReal x623=(sj8)*(sj8);
02757 IkReal x624=(cj8)*(cj8);
02758 IkReal x625=((r00)*(r11));
02759 IkReal x626=((r02)*(sj7));
02760 IkReal x627=((cj7)*(x624));
02761 IkReal x628=((IkReal(1.00000000000000))*(r12)*(sj7));
02762 IkReal x629=((IkReal(1.00000000000000))*(r01)*(r10));
02763 IkReal x630=((cj7)*(x623));
02764 dummyeval[0]=((((IkReal(-1.00000000000000))*(x629)*(x630)))+(((x625)*(x630)))+(((cj8)*(r11)*(x626)))+(((x625)*(x627)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x628)))+(((IkReal(-1.00000000000000))*(x627)*(x629)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x628)))+(((r10)*(sj8)*(x626))));
02765 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02766 {
02767 {
02768 IkReal evalcond[5];
02769 IkReal x631=((IkReal(1.00000000000000))*(sj7));
02770 IkReal x632=((npy)*(sj8));
02771 IkReal x633=((cj8)*(npx));
02772 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j6)), IkReal(6.28318530717959))));
02773 evalcond[1]=((IkReal(-0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(0.250000000000000))*(sj5))));
02774 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x631)))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x633)))+(((IkReal(0.250000000000000))*(cj5)))+(((cj7)*(x632))));
02775 evalcond[3]=((((cj7)*(r22)))+(((r21)*(sj7)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r20)*(x631))));
02776 evalcond[4]=((IkReal(-0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x631)*(x633)))+(((sj7)*(x632)))+(((cj7)*(npz))));
02777 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 )
02778 {
02779 {
02780 IkReal dummyeval[1];
02781 IkReal gconst20;
02782 gconst20=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
02783 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
02784 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02785 {
02786 {
02787 IkReal dummyeval[1];
02788 IkReal gconst18;
02789 IkReal x634=(cj8)*(cj8);
02790 IkReal x635=(sj8)*(sj8);
02791 IkReal x636=((cj7)*(r02));
02792 IkReal x637=((IkReal(1.00000000000000))*(r00));
02793 IkReal x638=((cj7)*(r12));
02794 IkReal x639=((r01)*(r10));
02795 IkReal x640=((sj7)*(x635));
02796 IkReal x641=((sj7)*(x634));
02797 gconst18=IKsign(((((IkReal(-1.00000000000000))*(sj8)*(x637)*(x638)))+(((x639)*(x641)))+(((x639)*(x640)))+(((IkReal(-1.00000000000000))*(r11)*(x637)*(x640)))+(((IkReal(-1.00000000000000))*(r11)*(x637)*(x641)))+(((cj8)*(r11)*(x636)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x638)))+(((r10)*(sj8)*(x636)))));
02798 IkReal x642=(cj8)*(cj8);
02799 IkReal x643=(sj8)*(sj8);
02800 IkReal x644=((cj7)*(r02));
02801 IkReal x645=((IkReal(1.00000000000000))*(r00));
02802 IkReal x646=((cj7)*(r12));
02803 IkReal x647=((r01)*(r10));
02804 IkReal x648=((sj7)*(x643));
02805 IkReal x649=((sj7)*(x642));
02806 dummyeval[0]=((((IkReal(-1.00000000000000))*(sj8)*(x645)*(x646)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x646)))+(((cj8)*(r11)*(x644)))+(((r10)*(sj8)*(x644)))+(((IkReal(-1.00000000000000))*(r11)*(x645)*(x648)))+(((IkReal(-1.00000000000000))*(r11)*(x645)*(x649)))+(((x647)*(x648)))+(((x647)*(x649))));
02807 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02808 {
02809 {
02810 IkReal dummyeval[1];
02811 IkReal gconst19;
02812 IkReal x650=(sj7)*(sj7);
02813 IkReal x651=(cj7)*(cj7);
02814 IkReal x652=((r02)*(r11));
02815 IkReal x653=((sj8)*(x650));
02816 IkReal x654=((IkReal(1.00000000000000))*(r01)*(r12));
02817 IkReal x655=((cj8)*(r00)*(r12));
02818 IkReal x656=((sj8)*(x651));
02819 IkReal x657=((IkReal(1.00000000000000))*(cj8)*(r02)*(r10));
02820 gconst19=IKsign(((((IkReal(-1.00000000000000))*(x651)*(x657)))+(((x652)*(x653)))+(((x652)*(x656)))+(((x651)*(x655)))+(((IkReal(-1.00000000000000))*(x650)*(x657)))+(((x650)*(x655)))+(((IkReal(-1.00000000000000))*(x653)*(x654)))+(((IkReal(-1.00000000000000))*(x654)*(x656)))));
02821 IkReal x658=(sj7)*(sj7);
02822 IkReal x659=(cj7)*(cj7);
02823 IkReal x660=((r02)*(r11));
02824 IkReal x661=((sj8)*(x658));
02825 IkReal x662=((IkReal(1.00000000000000))*(r01)*(r12));
02826 IkReal x663=((cj8)*(r00)*(r12));
02827 IkReal x664=((sj8)*(x659));
02828 IkReal x665=x657;
02829 dummyeval[0]=((((IkReal(-1.00000000000000))*(x662)*(x664)))+(((IkReal(-1.00000000000000))*(x659)*(x665)))+(((IkReal(-1.00000000000000))*(x661)*(x662)))+(((IkReal(-1.00000000000000))*(x658)*(x665)))+(((x660)*(x661)))+(((x660)*(x664)))+(((x659)*(x663)))+(((x658)*(x663))));
02830 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02831 {
02832 continue;
02833
02834 } else
02835 {
02836 {
02837 IkReal j3array[1], cj3array[1], sj3array[1];
02838 bool j3valid[1]={false};
02839 _nj3 = 1;
02840 IkReal x666=((cj7)*(cj8));
02841 IkReal x667=((IkReal(1.00000000000000))*(cj7)*(sj8));
02842 if( IKabs(((gconst19)*(((((IkReal(-1.00000000000000))*(r11)*(x667)))+(((r12)*(sj7)))+(((r10)*(x666))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((r00)*(x666)))+(((IkReal(-1.00000000000000))*(r01)*(x667)))+(((r02)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH )
02843 continue;
02844 j3array[0]=IKatan2(((gconst19)*(((((IkReal(-1.00000000000000))*(r11)*(x667)))+(((r12)*(sj7)))+(((r10)*(x666)))))), ((gconst19)*(((((r00)*(x666)))+(((IkReal(-1.00000000000000))*(r01)*(x667)))+(((r02)*(sj7)))))));
02845 sj3array[0]=IKsin(j3array[0]);
02846 cj3array[0]=IKcos(j3array[0]);
02847 if( j3array[0] > IKPI )
02848 {
02849 j3array[0]-=IK2PI;
02850 }
02851 else if( j3array[0] < -IKPI )
02852 { j3array[0]+=IK2PI;
02853 }
02854 j3valid[0] = true;
02855 for(int ij3 = 0; ij3 < 1; ++ij3)
02856 {
02857 if( !j3valid[ij3] )
02858 {
02859 continue;
02860 }
02861 _ij3[0] = ij3; _ij3[1] = -1;
02862 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
02863 {
02864 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
02865 {
02866 j3valid[iij3]=false; _ij3[1] = iij3; break;
02867 }
02868 }
02869 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
02870 {
02871 IkReal evalcond[4];
02872 IkReal x668=IKsin(j3);
02873 IkReal x669=IKcos(j3);
02874 IkReal x670=((r01)*(sj8));
02875 IkReal x671=((cj8)*(sj7));
02876 IkReal x672=((IkReal(1.00000000000000))*(sj7));
02877 IkReal x673=((IkReal(1.00000000000000))*(r12));
02878 IkReal x674=((r11)*(sj8));
02879 IkReal x675=((IkReal(1.00000000000000))*(r00));
02880 IkReal x676=((cj7)*(x668));
02881 IkReal x677=((r10)*(x669));
02882 IkReal x678=((IkReal(1.00000000000000))*(cj7)*(x669));
02883 evalcond[0]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x669)))+(((r00)*(sj8)*(x668)))+(((cj8)*(r01)*(x668)))+(((IkReal(-1.00000000000000))*(sj8)*(x677))));
02884 evalcond[1]=((((cj7)*(cj8)*(x677)))+(((IkReal(-1.00000000000000))*(cj8)*(x675)*(x676)))+(((IkReal(-1.00000000000000))*(r02)*(x668)*(x672)))+(((x670)*(x676)))+(((r12)*(sj7)*(x669)))+(((IkReal(-1.00000000000000))*(x674)*(x678))));
02885 evalcond[2]=((IkReal(1.00000000000000))+(((r02)*(x676)))+(((x671)*(x677)))+(((IkReal(-1.00000000000000))*(cj7)*(x669)*(x673)))+(((sj7)*(x668)*(x670)))+(((IkReal(-1.00000000000000))*(x669)*(x672)*(x674)))+(((IkReal(-1.00000000000000))*(x668)*(x671)*(x675))));
02886 evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x678)))+(((IkReal(-1.00000000000000))*(x669)*(x670)*(x672)))+(((r00)*(x669)*(x671)))+(((r10)*(x668)*(x671)))+(((IkReal(-1.00000000000000))*(x673)*(x676)))+(((IkReal(-1.00000000000000))*(x668)*(x672)*(x674))));
02887 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02888 {
02889 continue;
02890 }
02891 }
02892
02893 {
02894 IkReal dummyeval[1];
02895 IkReal gconst21;
02896 gconst21=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
02897 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
02898 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02899 {
02900 {
02901 IkReal dummyeval[1];
02902 IkReal gconst22;
02903 gconst22=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
02904 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
02905 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02906 {
02907 continue;
02908
02909 } else
02910 {
02911 {
02912 IkReal j4array[1], cj4array[1], sj4array[1];
02913 bool j4valid[1]={false};
02914 _nj4 = 1;
02915 IkReal x679=((cj8)*(sj5));
02916 IkReal x680=((cj3)*(r01));
02917 IkReal x681=((IkReal(1.00000000000000))*(cj5));
02918 IkReal x682=((r11)*(sj3));
02919 IkReal x683=((sj5)*(sj8));
02920 IkReal x684=((r10)*(sj3));
02921 IkReal x685=((cj3)*(r00)*(sj8));
02922 if( IKabs(((gconst22)*(((((cj3)*(r00)*(x683)))+(((cj5)*(r20)*(sj8)))+(((x683)*(x684)))+(((x679)*(x682)))+(((x679)*(x680)))+(((cj5)*(cj8)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((r21)*(x679)))+(((IkReal(-1.00000000000000))*(x681)*(x685)))+(((IkReal(-1.00000000000000))*(cj8)*(x680)*(x681)))+(((IkReal(-1.00000000000000))*(cj8)*(x681)*(x682)))+(((r20)*(x683)))+(((IkReal(-1.00000000000000))*(sj8)*(x681)*(x684))))))) < IKFAST_ATAN2_MAGTHRESH )
02923 continue;
02924 j4array[0]=IKatan2(((gconst22)*(((((cj3)*(r00)*(x683)))+(((cj5)*(r20)*(sj8)))+(((x683)*(x684)))+(((x679)*(x682)))+(((x679)*(x680)))+(((cj5)*(cj8)*(r21)))))), ((gconst22)*(((((r21)*(x679)))+(((IkReal(-1.00000000000000))*(x681)*(x685)))+(((IkReal(-1.00000000000000))*(cj8)*(x680)*(x681)))+(((IkReal(-1.00000000000000))*(cj8)*(x681)*(x682)))+(((r20)*(x683)))+(((IkReal(-1.00000000000000))*(sj8)*(x681)*(x684)))))));
02925 sj4array[0]=IKsin(j4array[0]);
02926 cj4array[0]=IKcos(j4array[0]);
02927 if( j4array[0] > IKPI )
02928 {
02929 j4array[0]-=IK2PI;
02930 }
02931 else if( j4array[0] < -IKPI )
02932 { j4array[0]+=IK2PI;
02933 }
02934 j4valid[0] = true;
02935 for(int ij4 = 0; ij4 < 1; ++ij4)
02936 {
02937 if( !j4valid[ij4] )
02938 {
02939 continue;
02940 }
02941 _ij4[0] = ij4; _ij4[1] = -1;
02942 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
02943 {
02944 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
02945 {
02946 j4valid[iij4]=false; _ij4[1] = iij4; break;
02947 }
02948 }
02949 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
02950 {
02951 IkReal evalcond[4];
02952 IkReal x686=IKsin(j4);
02953 IkReal x687=IKcos(j4);
02954 IkReal x688=((IkReal(1.00000000000000))*(cj8));
02955 IkReal x689=((cj3)*(r01));
02956 IkReal x690=((cj3)*(r00));
02957 IkReal x691=((cj7)*(cj8));
02958 IkReal x692=((IkReal(1.00000000000000))*(cj5));
02959 IkReal x693=((IkReal(1.00000000000000))*(sj8));
02960 IkReal x694=((r11)*(sj3));
02961 IkReal x695=((r10)*(sj3));
02962 IkReal x696=((sj5)*(x686));
02963 IkReal x697=((sj5)*(x687));
02964 IkReal x698=((x687)*(x692));
02965 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x686)*(x692)))+(((r20)*(sj8)))+(((IkReal(-1.00000000000000))*(x697))));
02966 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x688)))+(x696)+(((IkReal(-1.00000000000000))*(x698)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
02967 evalcond[2]=((((IkReal(-1.00000000000000))*(x688)*(x689)))+(((IkReal(-1.00000000000000))*(x693)*(x695)))+(((IkReal(-1.00000000000000))*(x688)*(x694)))+(x696)+(((IkReal(-1.00000000000000))*(x698)))+(((IkReal(-1.00000000000000))*(x690)*(x693))));
02968 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((cj5)*(x686)))+(((x690)*(x691)))+(((x691)*(x695)))+(((cj3)*(r02)*(sj7)))+(x697)+(((IkReal(-1.00000000000000))*(cj7)*(x689)*(x693)))+(((IkReal(-1.00000000000000))*(cj7)*(x693)*(x694))));
02969 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02970 {
02971 continue;
02972 }
02973 }
02974
02975 {
02976 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02977 vinfos[0].jointtype = 1;
02978 vinfos[0].foffset = j3;
02979 vinfos[0].indices[0] = _ij3[0];
02980 vinfos[0].indices[1] = _ij3[1];
02981 vinfos[0].maxsolutions = _nj3;
02982 vinfos[1].jointtype = 1;
02983 vinfos[1].foffset = j4;
02984 vinfos[1].indices[0] = _ij4[0];
02985 vinfos[1].indices[1] = _ij4[1];
02986 vinfos[1].maxsolutions = _nj4;
02987 vinfos[2].jointtype = 1;
02988 vinfos[2].foffset = j5;
02989 vinfos[2].indices[0] = _ij5[0];
02990 vinfos[2].indices[1] = _ij5[1];
02991 vinfos[2].maxsolutions = _nj5;
02992 vinfos[3].jointtype = 1;
02993 vinfos[3].foffset = j6;
02994 vinfos[3].indices[0] = _ij6[0];
02995 vinfos[3].indices[1] = _ij6[1];
02996 vinfos[3].maxsolutions = _nj6;
02997 vinfos[4].jointtype = 1;
02998 vinfos[4].foffset = j7;
02999 vinfos[4].indices[0] = _ij7[0];
03000 vinfos[4].indices[1] = _ij7[1];
03001 vinfos[4].maxsolutions = _nj7;
03002 vinfos[5].jointtype = 1;
03003 vinfos[5].foffset = j8;
03004 vinfos[5].indices[0] = _ij8[0];
03005 vinfos[5].indices[1] = _ij8[1];
03006 vinfos[5].maxsolutions = _nj8;
03007 std::vector<int> vfree(0);
03008 solutions.AddSolution(vinfos,vfree);
03009 }
03010 }
03011 }
03012
03013 }
03014
03015 }
03016
03017 } else
03018 {
03019 {
03020 IkReal j4array[1], cj4array[1], sj4array[1];
03021 bool j4valid[1]={false};
03022 _nj4 = 1;
03023 IkReal x699=((sj5)*(sj8));
03024 IkReal x700=((IkReal(1.00000000000000))*(cj7));
03025 IkReal x701=((cj8)*(sj5));
03026 IkReal x702=((r22)*(sj7));
03027 IkReal x703=((cj5)*(cj8));
03028 IkReal x704=((cj5)*(sj8));
03029 if( IKabs(((gconst21)*(((((cj7)*(r20)*(x701)))+(((sj5)*(x702)))+(((r20)*(x704)))+(((IkReal(-1.00000000000000))*(r21)*(x699)*(x700)))+(((r21)*(x703))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj5)*(x702)))+(((r20)*(x699)))+(((IkReal(-1.00000000000000))*(r20)*(x700)*(x703)))+(((r21)*(x701)))+(((cj7)*(r21)*(x704))))))) < IKFAST_ATAN2_MAGTHRESH )
03030 continue;
03031 j4array[0]=IKatan2(((gconst21)*(((((cj7)*(r20)*(x701)))+(((sj5)*(x702)))+(((r20)*(x704)))+(((IkReal(-1.00000000000000))*(r21)*(x699)*(x700)))+(((r21)*(x703)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj5)*(x702)))+(((r20)*(x699)))+(((IkReal(-1.00000000000000))*(r20)*(x700)*(x703)))+(((r21)*(x701)))+(((cj7)*(r21)*(x704)))))));
03032 sj4array[0]=IKsin(j4array[0]);
03033 cj4array[0]=IKcos(j4array[0]);
03034 if( j4array[0] > IKPI )
03035 {
03036 j4array[0]-=IK2PI;
03037 }
03038 else if( j4array[0] < -IKPI )
03039 { j4array[0]+=IK2PI;
03040 }
03041 j4valid[0] = true;
03042 for(int ij4 = 0; ij4 < 1; ++ij4)
03043 {
03044 if( !j4valid[ij4] )
03045 {
03046 continue;
03047 }
03048 _ij4[0] = ij4; _ij4[1] = -1;
03049 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03050 {
03051 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03052 {
03053 j4valid[iij4]=false; _ij4[1] = iij4; break;
03054 }
03055 }
03056 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03057 {
03058 IkReal evalcond[4];
03059 IkReal x705=IKsin(j4);
03060 IkReal x706=IKcos(j4);
03061 IkReal x707=((IkReal(1.00000000000000))*(cj8));
03062 IkReal x708=((cj3)*(r01));
03063 IkReal x709=((cj3)*(r00));
03064 IkReal x710=((cj7)*(cj8));
03065 IkReal x711=((IkReal(1.00000000000000))*(cj5));
03066 IkReal x712=((IkReal(1.00000000000000))*(sj8));
03067 IkReal x713=((r11)*(sj3));
03068 IkReal x714=((r10)*(sj3));
03069 IkReal x715=((sj5)*(x705));
03070 IkReal x716=((sj5)*(x706));
03071 IkReal x717=((x706)*(x711));
03072 evalcond[0]=((((IkReal(-1.00000000000000))*(x716)))+(((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x705)*(x711)))+(((r20)*(sj8))));
03073 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x707)))+(((IkReal(-1.00000000000000))*(x717)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x715)+(((cj7)*(r21)*(sj8))));
03074 evalcond[2]=((((IkReal(-1.00000000000000))*(x707)*(x708)))+(((IkReal(-1.00000000000000))*(x707)*(x713)))+(((IkReal(-1.00000000000000))*(x709)*(x712)))+(((IkReal(-1.00000000000000))*(x717)))+(((IkReal(-1.00000000000000))*(x712)*(x714)))+(x715));
03075 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x708)*(x712)))+(((cj5)*(x705)))+(((x709)*(x710)))+(((x710)*(x714)))+(((IkReal(-1.00000000000000))*(cj7)*(x712)*(x713)))+(((cj3)*(r02)*(sj7)))+(x716));
03076 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03077 {
03078 continue;
03079 }
03080 }
03081
03082 {
03083 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03084 vinfos[0].jointtype = 1;
03085 vinfos[0].foffset = j3;
03086 vinfos[0].indices[0] = _ij3[0];
03087 vinfos[0].indices[1] = _ij3[1];
03088 vinfos[0].maxsolutions = _nj3;
03089 vinfos[1].jointtype = 1;
03090 vinfos[1].foffset = j4;
03091 vinfos[1].indices[0] = _ij4[0];
03092 vinfos[1].indices[1] = _ij4[1];
03093 vinfos[1].maxsolutions = _nj4;
03094 vinfos[2].jointtype = 1;
03095 vinfos[2].foffset = j5;
03096 vinfos[2].indices[0] = _ij5[0];
03097 vinfos[2].indices[1] = _ij5[1];
03098 vinfos[2].maxsolutions = _nj5;
03099 vinfos[3].jointtype = 1;
03100 vinfos[3].foffset = j6;
03101 vinfos[3].indices[0] = _ij6[0];
03102 vinfos[3].indices[1] = _ij6[1];
03103 vinfos[3].maxsolutions = _nj6;
03104 vinfos[4].jointtype = 1;
03105 vinfos[4].foffset = j7;
03106 vinfos[4].indices[0] = _ij7[0];
03107 vinfos[4].indices[1] = _ij7[1];
03108 vinfos[4].maxsolutions = _nj7;
03109 vinfos[5].jointtype = 1;
03110 vinfos[5].foffset = j8;
03111 vinfos[5].indices[0] = _ij8[0];
03112 vinfos[5].indices[1] = _ij8[1];
03113 vinfos[5].maxsolutions = _nj8;
03114 std::vector<int> vfree(0);
03115 solutions.AddSolution(vinfos,vfree);
03116 }
03117 }
03118 }
03119
03120 }
03121
03122 }
03123 }
03124 }
03125
03126 }
03127
03128 }
03129
03130 } else
03131 {
03132 {
03133 IkReal j3array[1], cj3array[1], sj3array[1];
03134 bool j3valid[1]={false};
03135 _nj3 = 1;
03136 IkReal x718=((IkReal(1.00000000000000))*(cj8));
03137 IkReal x719=((IkReal(1.00000000000000))*(sj8));
03138 if( IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r11)*(x718)))+(((IkReal(-1.00000000000000))*(r10)*(x719))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x718)))+(((IkReal(-1.00000000000000))*(r00)*(x719))))))) < IKFAST_ATAN2_MAGTHRESH )
03139 continue;
03140 j3array[0]=IKatan2(((gconst18)*(((((IkReal(-1.00000000000000))*(r11)*(x718)))+(((IkReal(-1.00000000000000))*(r10)*(x719)))))), ((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x718)))+(((IkReal(-1.00000000000000))*(r00)*(x719)))))));
03141 sj3array[0]=IKsin(j3array[0]);
03142 cj3array[0]=IKcos(j3array[0]);
03143 if( j3array[0] > IKPI )
03144 {
03145 j3array[0]-=IK2PI;
03146 }
03147 else if( j3array[0] < -IKPI )
03148 { j3array[0]+=IK2PI;
03149 }
03150 j3valid[0] = true;
03151 for(int ij3 = 0; ij3 < 1; ++ij3)
03152 {
03153 if( !j3valid[ij3] )
03154 {
03155 continue;
03156 }
03157 _ij3[0] = ij3; _ij3[1] = -1;
03158 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03159 {
03160 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03161 {
03162 j3valid[iij3]=false; _ij3[1] = iij3; break;
03163 }
03164 }
03165 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03166 {
03167 IkReal evalcond[4];
03168 IkReal x720=IKsin(j3);
03169 IkReal x721=IKcos(j3);
03170 IkReal x722=((r01)*(sj8));
03171 IkReal x723=((cj8)*(sj7));
03172 IkReal x724=((IkReal(1.00000000000000))*(sj7));
03173 IkReal x725=((IkReal(1.00000000000000))*(r12));
03174 IkReal x726=((r11)*(sj8));
03175 IkReal x727=((IkReal(1.00000000000000))*(r00));
03176 IkReal x728=((cj7)*(x720));
03177 IkReal x729=((r10)*(x721));
03178 IkReal x730=((IkReal(1.00000000000000))*(cj7)*(x721));
03179 evalcond[0]=((((r00)*(sj8)*(x720)))+(((IkReal(-1.00000000000000))*(sj8)*(x729)))+(((cj8)*(r01)*(x720)))+(((IkReal(-1.00000000000000))*(cj8)*(r11)*(x721))));
03180 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(x727)*(x728)))+(((x722)*(x728)))+(((IkReal(-1.00000000000000))*(r02)*(x720)*(x724)))+(((cj7)*(cj8)*(x729)))+(((r12)*(sj7)*(x721)))+(((IkReal(-1.00000000000000))*(x726)*(x730))));
03181 evalcond[2]=((IkReal(1.00000000000000))+(((x723)*(x729)))+(((IkReal(-1.00000000000000))*(cj7)*(x721)*(x725)))+(((IkReal(-1.00000000000000))*(x720)*(x723)*(x727)))+(((IkReal(-1.00000000000000))*(x721)*(x724)*(x726)))+(((sj7)*(x720)*(x722)))+(((r02)*(x728))));
03182 evalcond[3]=((((r10)*(x720)*(x723)))+(((IkReal(-1.00000000000000))*(r02)*(x730)))+(((IkReal(-1.00000000000000))*(x721)*(x722)*(x724)))+(((IkReal(-1.00000000000000))*(x725)*(x728)))+(((r00)*(x721)*(x723)))+(((IkReal(-1.00000000000000))*(x720)*(x724)*(x726))));
03183 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03184 {
03185 continue;
03186 }
03187 }
03188
03189 {
03190 IkReal dummyeval[1];
03191 IkReal gconst21;
03192 gconst21=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
03193 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
03194 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03195 {
03196 {
03197 IkReal dummyeval[1];
03198 IkReal gconst22;
03199 gconst22=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
03200 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
03201 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03202 {
03203 continue;
03204
03205 } else
03206 {
03207 {
03208 IkReal j4array[1], cj4array[1], sj4array[1];
03209 bool j4valid[1]={false};
03210 _nj4 = 1;
03211 IkReal x731=((cj8)*(sj5));
03212 IkReal x732=((cj3)*(r01));
03213 IkReal x733=((IkReal(1.00000000000000))*(cj5));
03214 IkReal x734=((r11)*(sj3));
03215 IkReal x735=((sj5)*(sj8));
03216 IkReal x736=((r10)*(sj3));
03217 IkReal x737=((cj3)*(r00)*(sj8));
03218 if( IKabs(((gconst22)*(((((x731)*(x734)))+(((x731)*(x732)))+(((x735)*(x736)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x735)))+(((cj5)*(cj8)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(sj8)*(x733)*(x736)))+(((IkReal(-1.00000000000000))*(x733)*(x737)))+(((r20)*(x735)))+(((IkReal(-1.00000000000000))*(cj8)*(x732)*(x733)))+(((r21)*(x731)))+(((IkReal(-1.00000000000000))*(cj8)*(x733)*(x734))))))) < IKFAST_ATAN2_MAGTHRESH )
03219 continue;
03220 j4array[0]=IKatan2(((gconst22)*(((((x731)*(x734)))+(((x731)*(x732)))+(((x735)*(x736)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x735)))+(((cj5)*(cj8)*(r21)))))), ((gconst22)*(((((IkReal(-1.00000000000000))*(sj8)*(x733)*(x736)))+(((IkReal(-1.00000000000000))*(x733)*(x737)))+(((r20)*(x735)))+(((IkReal(-1.00000000000000))*(cj8)*(x732)*(x733)))+(((r21)*(x731)))+(((IkReal(-1.00000000000000))*(cj8)*(x733)*(x734)))))));
03221 sj4array[0]=IKsin(j4array[0]);
03222 cj4array[0]=IKcos(j4array[0]);
03223 if( j4array[0] > IKPI )
03224 {
03225 j4array[0]-=IK2PI;
03226 }
03227 else if( j4array[0] < -IKPI )
03228 { j4array[0]+=IK2PI;
03229 }
03230 j4valid[0] = true;
03231 for(int ij4 = 0; ij4 < 1; ++ij4)
03232 {
03233 if( !j4valid[ij4] )
03234 {
03235 continue;
03236 }
03237 _ij4[0] = ij4; _ij4[1] = -1;
03238 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03239 {
03240 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03241 {
03242 j4valid[iij4]=false; _ij4[1] = iij4; break;
03243 }
03244 }
03245 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03246 {
03247 IkReal evalcond[4];
03248 IkReal x738=IKsin(j4);
03249 IkReal x739=IKcos(j4);
03250 IkReal x740=((IkReal(1.00000000000000))*(cj8));
03251 IkReal x741=((cj3)*(r01));
03252 IkReal x742=((cj3)*(r00));
03253 IkReal x743=((cj7)*(cj8));
03254 IkReal x744=((IkReal(1.00000000000000))*(cj5));
03255 IkReal x745=((IkReal(1.00000000000000))*(sj8));
03256 IkReal x746=((r11)*(sj3));
03257 IkReal x747=((r10)*(sj3));
03258 IkReal x748=((sj5)*(x738));
03259 IkReal x749=((sj5)*(x739));
03260 IkReal x750=((x739)*(x744));
03261 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x738)*(x744)))+(((IkReal(-1.00000000000000))*(x749)))+(((r20)*(sj8))));
03262 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x748)+(((cj7)*(r21)*(sj8)))+(((IkReal(-1.00000000000000))*(x750)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x740))));
03263 evalcond[2]=((((IkReal(-1.00000000000000))*(x745)*(x747)))+(((IkReal(-1.00000000000000))*(x742)*(x745)))+(((IkReal(-1.00000000000000))*(x740)*(x741)))+(((IkReal(-1.00000000000000))*(x740)*(x746)))+(x748)+(((IkReal(-1.00000000000000))*(x750))));
03264 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((cj5)*(x738)))+(((x742)*(x743)))+(((IkReal(-1.00000000000000))*(cj7)*(x741)*(x745)))+(((IkReal(-1.00000000000000))*(cj7)*(x745)*(x746)))+(((cj3)*(r02)*(sj7)))+(((x743)*(x747)))+(x749));
03265 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03266 {
03267 continue;
03268 }
03269 }
03270
03271 {
03272 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03273 vinfos[0].jointtype = 1;
03274 vinfos[0].foffset = j3;
03275 vinfos[0].indices[0] = _ij3[0];
03276 vinfos[0].indices[1] = _ij3[1];
03277 vinfos[0].maxsolutions = _nj3;
03278 vinfos[1].jointtype = 1;
03279 vinfos[1].foffset = j4;
03280 vinfos[1].indices[0] = _ij4[0];
03281 vinfos[1].indices[1] = _ij4[1];
03282 vinfos[1].maxsolutions = _nj4;
03283 vinfos[2].jointtype = 1;
03284 vinfos[2].foffset = j5;
03285 vinfos[2].indices[0] = _ij5[0];
03286 vinfos[2].indices[1] = _ij5[1];
03287 vinfos[2].maxsolutions = _nj5;
03288 vinfos[3].jointtype = 1;
03289 vinfos[3].foffset = j6;
03290 vinfos[3].indices[0] = _ij6[0];
03291 vinfos[3].indices[1] = _ij6[1];
03292 vinfos[3].maxsolutions = _nj6;
03293 vinfos[4].jointtype = 1;
03294 vinfos[4].foffset = j7;
03295 vinfos[4].indices[0] = _ij7[0];
03296 vinfos[4].indices[1] = _ij7[1];
03297 vinfos[4].maxsolutions = _nj7;
03298 vinfos[5].jointtype = 1;
03299 vinfos[5].foffset = j8;
03300 vinfos[5].indices[0] = _ij8[0];
03301 vinfos[5].indices[1] = _ij8[1];
03302 vinfos[5].maxsolutions = _nj8;
03303 std::vector<int> vfree(0);
03304 solutions.AddSolution(vinfos,vfree);
03305 }
03306 }
03307 }
03308
03309 }
03310
03311 }
03312
03313 } else
03314 {
03315 {
03316 IkReal j4array[1], cj4array[1], sj4array[1];
03317 bool j4valid[1]={false};
03318 _nj4 = 1;
03319 IkReal x751=((sj5)*(sj8));
03320 IkReal x752=((IkReal(1.00000000000000))*(cj7));
03321 IkReal x753=((cj8)*(sj5));
03322 IkReal x754=((r22)*(sj7));
03323 IkReal x755=((cj5)*(cj8));
03324 IkReal x756=((cj5)*(sj8));
03325 if( IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x751)*(x752)))+(((cj7)*(r20)*(x753)))+(((r21)*(x755)))+(((sj5)*(x754)))+(((r20)*(x756))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((r21)*(x753)))+(((cj7)*(r21)*(x756)))+(((IkReal(-1.00000000000000))*(cj5)*(x754)))+(((r20)*(x751)))+(((IkReal(-1.00000000000000))*(r20)*(x752)*(x755))))))) < IKFAST_ATAN2_MAGTHRESH )
03326 continue;
03327 j4array[0]=IKatan2(((gconst21)*(((((IkReal(-1.00000000000000))*(r21)*(x751)*(x752)))+(((cj7)*(r20)*(x753)))+(((r21)*(x755)))+(((sj5)*(x754)))+(((r20)*(x756)))))), ((gconst21)*(((((r21)*(x753)))+(((cj7)*(r21)*(x756)))+(((IkReal(-1.00000000000000))*(cj5)*(x754)))+(((r20)*(x751)))+(((IkReal(-1.00000000000000))*(r20)*(x752)*(x755)))))));
03328 sj4array[0]=IKsin(j4array[0]);
03329 cj4array[0]=IKcos(j4array[0]);
03330 if( j4array[0] > IKPI )
03331 {
03332 j4array[0]-=IK2PI;
03333 }
03334 else if( j4array[0] < -IKPI )
03335 { j4array[0]+=IK2PI;
03336 }
03337 j4valid[0] = true;
03338 for(int ij4 = 0; ij4 < 1; ++ij4)
03339 {
03340 if( !j4valid[ij4] )
03341 {
03342 continue;
03343 }
03344 _ij4[0] = ij4; _ij4[1] = -1;
03345 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03346 {
03347 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03348 {
03349 j4valid[iij4]=false; _ij4[1] = iij4; break;
03350 }
03351 }
03352 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03353 {
03354 IkReal evalcond[4];
03355 IkReal x757=IKsin(j4);
03356 IkReal x758=IKcos(j4);
03357 IkReal x759=((IkReal(1.00000000000000))*(cj8));
03358 IkReal x760=((cj3)*(r01));
03359 IkReal x761=((cj3)*(r00));
03360 IkReal x762=((cj7)*(cj8));
03361 IkReal x763=((IkReal(1.00000000000000))*(cj5));
03362 IkReal x764=((IkReal(1.00000000000000))*(sj8));
03363 IkReal x765=((r11)*(sj3));
03364 IkReal x766=((r10)*(sj3));
03365 IkReal x767=((sj5)*(x757));
03366 IkReal x768=((sj5)*(x758));
03367 IkReal x769=((x758)*(x763));
03368 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x757)*(x763)))+(((IkReal(-1.00000000000000))*(x768)))+(((r20)*(sj8))));
03369 evalcond[1]=((((IkReal(-1.00000000000000))*(x769)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x767)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x759)))+(((cj7)*(r21)*(sj8))));
03370 evalcond[2]=((((IkReal(-1.00000000000000))*(x769)))+(((IkReal(-1.00000000000000))*(x764)*(x766)))+(x767)+(((IkReal(-1.00000000000000))*(x761)*(x764)))+(((IkReal(-1.00000000000000))*(x759)*(x765)))+(((IkReal(-1.00000000000000))*(x759)*(x760))));
03371 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((cj5)*(x757)))+(((x761)*(x762)))+(((x762)*(x766)))+(((IkReal(-1.00000000000000))*(cj7)*(x764)*(x765)))+(((cj3)*(r02)*(sj7)))+(x768)+(((IkReal(-1.00000000000000))*(cj7)*(x760)*(x764))));
03372 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03373 {
03374 continue;
03375 }
03376 }
03377
03378 {
03379 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03380 vinfos[0].jointtype = 1;
03381 vinfos[0].foffset = j3;
03382 vinfos[0].indices[0] = _ij3[0];
03383 vinfos[0].indices[1] = _ij3[1];
03384 vinfos[0].maxsolutions = _nj3;
03385 vinfos[1].jointtype = 1;
03386 vinfos[1].foffset = j4;
03387 vinfos[1].indices[0] = _ij4[0];
03388 vinfos[1].indices[1] = _ij4[1];
03389 vinfos[1].maxsolutions = _nj4;
03390 vinfos[2].jointtype = 1;
03391 vinfos[2].foffset = j5;
03392 vinfos[2].indices[0] = _ij5[0];
03393 vinfos[2].indices[1] = _ij5[1];
03394 vinfos[2].maxsolutions = _nj5;
03395 vinfos[3].jointtype = 1;
03396 vinfos[3].foffset = j6;
03397 vinfos[3].indices[0] = _ij6[0];
03398 vinfos[3].indices[1] = _ij6[1];
03399 vinfos[3].maxsolutions = _nj6;
03400 vinfos[4].jointtype = 1;
03401 vinfos[4].foffset = j7;
03402 vinfos[4].indices[0] = _ij7[0];
03403 vinfos[4].indices[1] = _ij7[1];
03404 vinfos[4].maxsolutions = _nj7;
03405 vinfos[5].jointtype = 1;
03406 vinfos[5].foffset = j8;
03407 vinfos[5].indices[0] = _ij8[0];
03408 vinfos[5].indices[1] = _ij8[1];
03409 vinfos[5].maxsolutions = _nj8;
03410 std::vector<int> vfree(0);
03411 solutions.AddSolution(vinfos,vfree);
03412 }
03413 }
03414 }
03415
03416 }
03417
03418 }
03419 }
03420 }
03421
03422 }
03423
03424 }
03425
03426 } else
03427 {
03428 {
03429 IkReal j4array[1], cj4array[1], sj4array[1];
03430 bool j4valid[1]={false};
03431 _nj4 = 1;
03432 IkReal x770=((sj5)*(sj8));
03433 IkReal x771=((IkReal(1.00000000000000))*(cj7));
03434 IkReal x772=((cj8)*(sj5));
03435 IkReal x773=((r22)*(sj7));
03436 IkReal x774=((cj5)*(cj8));
03437 IkReal x775=((cj5)*(sj8));
03438 if( IKabs(((gconst20)*(((((cj7)*(r20)*(x772)))+(((IkReal(-1.00000000000000))*(r21)*(x770)*(x771)))+(((r21)*(x774)))+(((r20)*(x775)))+(((sj5)*(x773))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((cj7)*(r21)*(x775)))+(((r21)*(x772)))+(((IkReal(-1.00000000000000))*(cj5)*(x773)))+(((r20)*(x770)))+(((IkReal(-1.00000000000000))*(r20)*(x771)*(x774))))))) < IKFAST_ATAN2_MAGTHRESH )
03439 continue;
03440 j4array[0]=IKatan2(((gconst20)*(((((cj7)*(r20)*(x772)))+(((IkReal(-1.00000000000000))*(r21)*(x770)*(x771)))+(((r21)*(x774)))+(((r20)*(x775)))+(((sj5)*(x773)))))), ((gconst20)*(((((cj7)*(r21)*(x775)))+(((r21)*(x772)))+(((IkReal(-1.00000000000000))*(cj5)*(x773)))+(((r20)*(x770)))+(((IkReal(-1.00000000000000))*(r20)*(x771)*(x774)))))));
03441 sj4array[0]=IKsin(j4array[0]);
03442 cj4array[0]=IKcos(j4array[0]);
03443 if( j4array[0] > IKPI )
03444 {
03445 j4array[0]-=IK2PI;
03446 }
03447 else if( j4array[0] < -IKPI )
03448 { j4array[0]+=IK2PI;
03449 }
03450 j4valid[0] = true;
03451 for(int ij4 = 0; ij4 < 1; ++ij4)
03452 {
03453 if( !j4valid[ij4] )
03454 {
03455 continue;
03456 }
03457 _ij4[0] = ij4; _ij4[1] = -1;
03458 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03459 {
03460 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03461 {
03462 j4valid[iij4]=false; _ij4[1] = iij4; break;
03463 }
03464 }
03465 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03466 {
03467 IkReal evalcond[2];
03468 IkReal x776=IKsin(j4);
03469 IkReal x777=IKcos(j4);
03470 IkReal x778=((IkReal(1.00000000000000))*(cj5));
03471 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x776)*(x778)))+(((IkReal(-1.00000000000000))*(sj5)*(x777)))+(((r20)*(sj8))));
03472 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(cj8)*(r20)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((sj5)*(x776)))+(((IkReal(-1.00000000000000))*(x777)*(x778)))+(((cj7)*(r21)*(sj8))));
03473 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
03474 {
03475 continue;
03476 }
03477 }
03478
03479 {
03480 IkReal dummyeval[1];
03481 IkReal gconst24;
03482 IkReal x779=(cj8)*(cj8);
03483 IkReal x780=(sj8)*(sj8);
03484 IkReal x781=((cj7)*(r02));
03485 IkReal x782=((IkReal(1.00000000000000))*(r00));
03486 IkReal x783=((cj7)*(r12));
03487 IkReal x784=((r01)*(r10));
03488 IkReal x785=((sj7)*(x780));
03489 IkReal x786=((sj7)*(x779));
03490 gconst24=IKsign(((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x783)))+(((x784)*(x786)))+(((x784)*(x785)))+(((IkReal(-1.00000000000000))*(r11)*(x782)*(x786)))+(((IkReal(-1.00000000000000))*(r11)*(x782)*(x785)))+(((r10)*(sj8)*(x781)))+(((IkReal(-1.00000000000000))*(sj8)*(x782)*(x783)))+(((cj8)*(r11)*(x781)))));
03491 IkReal x787=(cj8)*(cj8);
03492 IkReal x788=(sj8)*(sj8);
03493 IkReal x789=((cj7)*(r02));
03494 IkReal x790=((IkReal(1.00000000000000))*(r00));
03495 IkReal x791=((cj7)*(r12));
03496 IkReal x792=((r01)*(r10));
03497 IkReal x793=((sj7)*(x788));
03498 IkReal x794=((sj7)*(x787));
03499 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x790)*(x794)))+(((IkReal(-1.00000000000000))*(r11)*(x790)*(x793)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x791)))+(((x792)*(x794)))+(((x792)*(x793)))+(((r10)*(sj8)*(x789)))+(((IkReal(-1.00000000000000))*(sj8)*(x790)*(x791)))+(((cj8)*(r11)*(x789))));
03500 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03501 {
03502 {
03503 IkReal dummyeval[1];
03504 IkReal gconst23;
03505 IkReal x795=(cj8)*(cj8);
03506 IkReal x796=(sj8)*(sj8);
03507 IkReal x797=((IkReal(2.00000000000000))*(cj8)*(sj8));
03508 IkReal x798=((IkReal(1.00000000000000))*(x795));
03509 IkReal x799=((IkReal(1.00000000000000))*(x796));
03510 gconst23=IKsign(((((IkReal(-1.00000000000000))*(x799)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x798)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x799)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x797)))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x797)))+(((IkReal(-1.00000000000000))*(x798)*((r01)*(r01))))));
03511 IkReal x800=(cj8)*(cj8);
03512 IkReal x801=(sj8)*(sj8);
03513 IkReal x802=((IkReal(2.00000000000000))*(cj8)*(sj8));
03514 IkReal x803=((IkReal(1.00000000000000))*(x800));
03515 IkReal x804=((IkReal(1.00000000000000))*(x801));
03516 dummyeval[0]=((((IkReal(-1.00000000000000))*(x803)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x802)))+(((IkReal(-1.00000000000000))*(x804)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x803)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x804)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x802))));
03517 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03518 {
03519 continue;
03520
03521 } else
03522 {
03523 {
03524 IkReal j3array[1], cj3array[1], sj3array[1];
03525 bool j3valid[1]={false};
03526 _nj3 = 1;
03527 IkReal x805=((cj8)*(r01));
03528 IkReal x806=((cj4)*(cj5));
03529 IkReal x807=((r10)*(sj8));
03530 IkReal x808=((cj8)*(r11));
03531 IkReal x809=((r00)*(sj8));
03532 IkReal x810=((IkReal(1.00000000000000))*(sj4)*(sj5));
03533 if( IKabs(((gconst23)*(((((x806)*(x808)))+(((x806)*(x807)))+(((IkReal(-1.00000000000000))*(x808)*(x810)))+(((IkReal(-1.00000000000000))*(x807)*(x810))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst23)*(((((x806)*(x809)))+(((IkReal(-1.00000000000000))*(x809)*(x810)))+(((x805)*(x806)))+(((IkReal(-1.00000000000000))*(x805)*(x810))))))) < IKFAST_ATAN2_MAGTHRESH )
03534 continue;
03535 j3array[0]=IKatan2(((gconst23)*(((((x806)*(x808)))+(((x806)*(x807)))+(((IkReal(-1.00000000000000))*(x808)*(x810)))+(((IkReal(-1.00000000000000))*(x807)*(x810)))))), ((gconst23)*(((((x806)*(x809)))+(((IkReal(-1.00000000000000))*(x809)*(x810)))+(((x805)*(x806)))+(((IkReal(-1.00000000000000))*(x805)*(x810)))))));
03536 sj3array[0]=IKsin(j3array[0]);
03537 cj3array[0]=IKcos(j3array[0]);
03538 if( j3array[0] > IKPI )
03539 {
03540 j3array[0]-=IK2PI;
03541 }
03542 else if( j3array[0] < -IKPI )
03543 { j3array[0]+=IK2PI;
03544 }
03545 j3valid[0] = true;
03546 for(int ij3 = 0; ij3 < 1; ++ij3)
03547 {
03548 if( !j3valid[ij3] )
03549 {
03550 continue;
03551 }
03552 _ij3[0] = ij3; _ij3[1] = -1;
03553 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03554 {
03555 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03556 {
03557 j3valid[iij3]=false; _ij3[1] = iij3; break;
03558 }
03559 }
03560 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03561 {
03562 IkReal evalcond[6];
03563 IkReal x811=IKsin(j3);
03564 IkReal x812=IKcos(j3);
03565 IkReal x813=((cj7)*(sj8));
03566 IkReal x814=((r00)*(sj8));
03567 IkReal x815=((cj8)*(r11));
03568 IkReal x816=((cj7)*(r02));
03569 IkReal x817=((cj7)*(r12));
03570 IkReal x818=((cj8)*(r10));
03571 IkReal x819=((sj7)*(sj8));
03572 IkReal x820=((cj8)*(r00));
03573 IkReal x821=((r10)*(sj8));
03574 IkReal x822=((r01)*(x811));
03575 IkReal x823=((IkReal(1.00000000000000))*(x812));
03576 IkReal x824=((IkReal(1.00000000000000))*(x811));
03577 IkReal x825=((sj7)*(x811));
03578 IkReal x826=((sj7)*(x812));
03579 IkReal x827=((cj7)*(x812));
03580 IkReal x828=((cj8)*(x826));
03581 evalcond[0]=((((IkReal(-1.00000000000000))*(x815)*(x823)))+(((x811)*(x814)))+(((IkReal(-1.00000000000000))*(x821)*(x823)))+(((cj8)*(x822))));
03582 evalcond[1]=((((IkReal(-1.00000000000000))*(x814)*(x823)))+(((IkReal(-1.00000000000000))*(x815)*(x824)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x821)*(x824)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x823))));
03583 evalcond[2]=((((r12)*(x826)))+(((IkReal(-1.00000000000000))*(cj7)*(x820)*(x824)))+(((IkReal(-1.00000000000000))*(r11)*(x813)*(x823)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x824)))+(((x813)*(x822)))+(((x818)*(x827))));
03584 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(sj7)*(x820)*(x824)))+(((IkReal(-1.00000000000000))*(r11)*(x819)*(x823)))+(((IkReal(-1.00000000000000))*(x817)*(x823)))+(((x819)*(x822)))+(((x811)*(x816)))+(((x818)*(x826))));
03585 evalcond[4]=((((cj7)*(x811)*(x818)))+(((r12)*(x825)))+(((x820)*(x827)))+(((IkReal(-1.00000000000000))*(r11)*(x813)*(x824)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((r02)*(x826)))+(((IkReal(-1.00000000000000))*(r01)*(x813)*(x823))));
03586 evalcond[5]=((((IkReal(-1.00000000000000))*(x816)*(x823)))+(((x820)*(x826)))+(((IkReal(-1.00000000000000))*(r11)*(x819)*(x824)))+(((IkReal(-1.00000000000000))*(x817)*(x824)))+(((x818)*(x825)))+(((IkReal(-1.00000000000000))*(r01)*(x819)*(x823))));
03587 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 )
03588 {
03589 continue;
03590 }
03591 }
03592
03593 {
03594 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03595 vinfos[0].jointtype = 1;
03596 vinfos[0].foffset = j3;
03597 vinfos[0].indices[0] = _ij3[0];
03598 vinfos[0].indices[1] = _ij3[1];
03599 vinfos[0].maxsolutions = _nj3;
03600 vinfos[1].jointtype = 1;
03601 vinfos[1].foffset = j4;
03602 vinfos[1].indices[0] = _ij4[0];
03603 vinfos[1].indices[1] = _ij4[1];
03604 vinfos[1].maxsolutions = _nj4;
03605 vinfos[2].jointtype = 1;
03606 vinfos[2].foffset = j5;
03607 vinfos[2].indices[0] = _ij5[0];
03608 vinfos[2].indices[1] = _ij5[1];
03609 vinfos[2].maxsolutions = _nj5;
03610 vinfos[3].jointtype = 1;
03611 vinfos[3].foffset = j6;
03612 vinfos[3].indices[0] = _ij6[0];
03613 vinfos[3].indices[1] = _ij6[1];
03614 vinfos[3].maxsolutions = _nj6;
03615 vinfos[4].jointtype = 1;
03616 vinfos[4].foffset = j7;
03617 vinfos[4].indices[0] = _ij7[0];
03618 vinfos[4].indices[1] = _ij7[1];
03619 vinfos[4].maxsolutions = _nj7;
03620 vinfos[5].jointtype = 1;
03621 vinfos[5].foffset = j8;
03622 vinfos[5].indices[0] = _ij8[0];
03623 vinfos[5].indices[1] = _ij8[1];
03624 vinfos[5].maxsolutions = _nj8;
03625 std::vector<int> vfree(0);
03626 solutions.AddSolution(vinfos,vfree);
03627 }
03628 }
03629 }
03630
03631 }
03632
03633 }
03634
03635 } else
03636 {
03637 {
03638 IkReal j3array[1], cj3array[1], sj3array[1];
03639 bool j3valid[1]={false};
03640 _nj3 = 1;
03641 IkReal x829=((IkReal(1.00000000000000))*(cj8));
03642 IkReal x830=((IkReal(1.00000000000000))*(sj8));
03643 if( IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r11)*(x829)))+(((IkReal(-1.00000000000000))*(r10)*(x830))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x829)))+(((IkReal(-1.00000000000000))*(r00)*(x830))))))) < IKFAST_ATAN2_MAGTHRESH )
03644 continue;
03645 j3array[0]=IKatan2(((gconst24)*(((((IkReal(-1.00000000000000))*(r11)*(x829)))+(((IkReal(-1.00000000000000))*(r10)*(x830)))))), ((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x829)))+(((IkReal(-1.00000000000000))*(r00)*(x830)))))));
03646 sj3array[0]=IKsin(j3array[0]);
03647 cj3array[0]=IKcos(j3array[0]);
03648 if( j3array[0] > IKPI )
03649 {
03650 j3array[0]-=IK2PI;
03651 }
03652 else if( j3array[0] < -IKPI )
03653 { j3array[0]+=IK2PI;
03654 }
03655 j3valid[0] = true;
03656 for(int ij3 = 0; ij3 < 1; ++ij3)
03657 {
03658 if( !j3valid[ij3] )
03659 {
03660 continue;
03661 }
03662 _ij3[0] = ij3; _ij3[1] = -1;
03663 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03664 {
03665 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03666 {
03667 j3valid[iij3]=false; _ij3[1] = iij3; break;
03668 }
03669 }
03670 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03671 {
03672 IkReal evalcond[6];
03673 IkReal x831=IKsin(j3);
03674 IkReal x832=IKcos(j3);
03675 IkReal x833=((cj7)*(sj8));
03676 IkReal x834=((r00)*(sj8));
03677 IkReal x835=((cj8)*(r11));
03678 IkReal x836=((cj7)*(r02));
03679 IkReal x837=((cj7)*(r12));
03680 IkReal x838=((cj8)*(r10));
03681 IkReal x839=((sj7)*(sj8));
03682 IkReal x840=((cj8)*(r00));
03683 IkReal x841=((r10)*(sj8));
03684 IkReal x842=((r01)*(x831));
03685 IkReal x843=((IkReal(1.00000000000000))*(x832));
03686 IkReal x844=((IkReal(1.00000000000000))*(x831));
03687 IkReal x845=((sj7)*(x831));
03688 IkReal x846=((sj7)*(x832));
03689 IkReal x847=((cj7)*(x832));
03690 IkReal x848=((cj8)*(x846));
03691 evalcond[0]=((((x831)*(x834)))+(((IkReal(-1.00000000000000))*(x835)*(x843)))+(((IkReal(-1.00000000000000))*(x841)*(x843)))+(((cj8)*(x842))));
03692 evalcond[1]=((((IkReal(-1.00000000000000))*(x835)*(x844)))+(((IkReal(-1.00000000000000))*(x841)*(x844)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x843)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x834)*(x843)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5))));
03693 evalcond[2]=((((x838)*(x847)))+(((IkReal(-1.00000000000000))*(cj7)*(x840)*(x844)))+(((IkReal(-1.00000000000000))*(r11)*(x833)*(x843)))+(((x833)*(x842)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x844)))+(((r12)*(x846))));
03694 evalcond[3]=((IkReal(1.00000000000000))+(((x831)*(x836)))+(((x838)*(x846)))+(((x839)*(x842)))+(((IkReal(-1.00000000000000))*(x837)*(x843)))+(((IkReal(-1.00000000000000))*(sj7)*(x840)*(x844)))+(((IkReal(-1.00000000000000))*(r11)*(x839)*(x843))));
03695 evalcond[4]=((((x840)*(x847)))+(((IkReal(-1.00000000000000))*(r11)*(x833)*(x844)))+(((r02)*(x846)))+(((cj7)*(x831)*(x838)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((r12)*(x845)))+(((IkReal(-1.00000000000000))*(r01)*(x833)*(x843))));
03696 evalcond[5]=((((x838)*(x845)))+(((x840)*(x846)))+(((IkReal(-1.00000000000000))*(x836)*(x843)))+(((IkReal(-1.00000000000000))*(x837)*(x844)))+(((IkReal(-1.00000000000000))*(r01)*(x839)*(x843)))+(((IkReal(-1.00000000000000))*(r11)*(x839)*(x844))));
03697 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 )
03698 {
03699 continue;
03700 }
03701 }
03702
03703 {
03704 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03705 vinfos[0].jointtype = 1;
03706 vinfos[0].foffset = j3;
03707 vinfos[0].indices[0] = _ij3[0];
03708 vinfos[0].indices[1] = _ij3[1];
03709 vinfos[0].maxsolutions = _nj3;
03710 vinfos[1].jointtype = 1;
03711 vinfos[1].foffset = j4;
03712 vinfos[1].indices[0] = _ij4[0];
03713 vinfos[1].indices[1] = _ij4[1];
03714 vinfos[1].maxsolutions = _nj4;
03715 vinfos[2].jointtype = 1;
03716 vinfos[2].foffset = j5;
03717 vinfos[2].indices[0] = _ij5[0];
03718 vinfos[2].indices[1] = _ij5[1];
03719 vinfos[2].maxsolutions = _nj5;
03720 vinfos[3].jointtype = 1;
03721 vinfos[3].foffset = j6;
03722 vinfos[3].indices[0] = _ij6[0];
03723 vinfos[3].indices[1] = _ij6[1];
03724 vinfos[3].maxsolutions = _nj6;
03725 vinfos[4].jointtype = 1;
03726 vinfos[4].foffset = j7;
03727 vinfos[4].indices[0] = _ij7[0];
03728 vinfos[4].indices[1] = _ij7[1];
03729 vinfos[4].maxsolutions = _nj7;
03730 vinfos[5].jointtype = 1;
03731 vinfos[5].foffset = j8;
03732 vinfos[5].indices[0] = _ij8[0];
03733 vinfos[5].indices[1] = _ij8[1];
03734 vinfos[5].maxsolutions = _nj8;
03735 std::vector<int> vfree(0);
03736 solutions.AddSolution(vinfos,vfree);
03737 }
03738 }
03739 }
03740
03741 }
03742
03743 }
03744 }
03745 }
03746
03747 }
03748
03749 }
03750
03751 } else
03752 {
03753 IkReal x849=((IkReal(1.00000000000000))*(sj7));
03754 IkReal x850=((npy)*(sj8));
03755 IkReal x851=((cj8)*(npx));
03756 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j6)), IkReal(6.28318530717959))));
03757 evalcond[1]=((IkReal(0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(-0.250000000000000))*(sj5))));
03758 evalcond[2]=((IkReal(0.235000000000000))+(((cj7)*(x850)))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x851)))+(((IkReal(-1.00000000000000))*(npz)*(x849)))+(((IkReal(0.250000000000000))*(cj5))));
03759 evalcond[3]=((((IkReal(-1.00000000000000))*(cj8)*(r20)*(x849)))+(((cj7)*(r22)))+(((r21)*(sj7)*(sj8))));
03760 evalcond[4]=((IkReal(0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x849)*(x851)))+(((cj7)*(npz)))+(((sj7)*(x850))));
03761 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 )
03762 {
03763 {
03764 IkReal dummyeval[1];
03765 IkReal gconst27;
03766 gconst27=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
03767 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
03768 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03769 {
03770 {
03771 IkReal dummyeval[1];
03772 IkReal gconst25;
03773 IkReal x852=(sj8)*(sj8);
03774 IkReal x853=(cj8)*(cj8);
03775 IkReal x854=((IkReal(1.00000000000000))*(r10));
03776 IkReal x855=((cj7)*(sj8));
03777 IkReal x856=((r00)*(r11));
03778 IkReal x857=((cj7)*(cj8));
03779 IkReal x858=((sj7)*(x852));
03780 IkReal x859=((sj7)*(x853));
03781 gconst25=IKsign(((((IkReal(-1.00000000000000))*(r01)*(x854)*(x858)))+(((IkReal(-1.00000000000000))*(r01)*(x854)*(x859)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x857)))+(((x856)*(x859)))+(((x856)*(x858)))+(((r01)*(r12)*(x857)))+(((r00)*(r12)*(x855)))+(((IkReal(-1.00000000000000))*(r02)*(x854)*(x855)))));
03782 IkReal x860=(sj8)*(sj8);
03783 IkReal x861=(cj8)*(cj8);
03784 IkReal x862=((IkReal(1.00000000000000))*(r10));
03785 IkReal x863=((cj7)*(sj8));
03786 IkReal x864=((r00)*(r11));
03787 IkReal x865=((cj7)*(cj8));
03788 IkReal x866=((sj7)*(x860));
03789 IkReal x867=((sj7)*(x861));
03790 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(x862)*(x863)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x865)))+(((r01)*(r12)*(x865)))+(((IkReal(-1.00000000000000))*(r01)*(x862)*(x866)))+(((IkReal(-1.00000000000000))*(r01)*(x862)*(x867)))+(((r00)*(r12)*(x863)))+(((x864)*(x866)))+(((x864)*(x867))));
03791 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03792 {
03793 {
03794 IkReal dummyeval[1];
03795 IkReal gconst26;
03796 IkReal x868=(sj7)*(sj7);
03797 IkReal x869=(cj7)*(cj7);
03798 IkReal x870=((r02)*(r10));
03799 IkReal x871=((r01)*(r12));
03800 IkReal x872=((sj8)*(x868));
03801 IkReal x873=((IkReal(1.00000000000000))*(r02)*(r11));
03802 IkReal x874=((cj8)*(x869));
03803 IkReal x875=((IkReal(1.00000000000000))*(r00)*(r12));
03804 IkReal x876=((sj8)*(x869));
03805 IkReal x877=((cj8)*(x868));
03806 gconst26=IKsign(((((IkReal(-1.00000000000000))*(x872)*(x873)))+(((x871)*(x872)))+(((x871)*(x876)))+(((x870)*(x874)))+(((x870)*(x877)))+(((IkReal(-1.00000000000000))*(x873)*(x876)))+(((IkReal(-1.00000000000000))*(x875)*(x877)))+(((IkReal(-1.00000000000000))*(x874)*(x875)))));
03807 IkReal x878=(sj7)*(sj7);
03808 IkReal x879=(cj7)*(cj7);
03809 IkReal x880=((r02)*(r10));
03810 IkReal x881=((r01)*(r12));
03811 IkReal x882=((sj8)*(x878));
03812 IkReal x883=((IkReal(1.00000000000000))*(r02)*(r11));
03813 IkReal x884=((cj8)*(x879));
03814 IkReal x885=((IkReal(1.00000000000000))*(r00)*(r12));
03815 IkReal x886=((sj8)*(x879));
03816 IkReal x887=((cj8)*(x878));
03817 dummyeval[0]=((((IkReal(-1.00000000000000))*(x883)*(x886)))+(((x881)*(x886)))+(((x881)*(x882)))+(((IkReal(-1.00000000000000))*(x882)*(x883)))+(((x880)*(x887)))+(((x880)*(x884)))+(((IkReal(-1.00000000000000))*(x885)*(x887)))+(((IkReal(-1.00000000000000))*(x884)*(x885))));
03818 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03819 {
03820 continue;
03821
03822 } else
03823 {
03824 {
03825 IkReal j3array[1], cj3array[1], sj3array[1];
03826 bool j3valid[1]={false};
03827 _nj3 = 1;
03828 IkReal x888=((cj7)*(cj8));
03829 IkReal x889=((IkReal(1.00000000000000))*(cj7)*(sj8));
03830 if( IKabs(((gconst26)*(((((r10)*(x888)))+(((IkReal(-1.00000000000000))*(r11)*(x889)))+(((r12)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst26)*(((((r00)*(x888)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x889))))))) < IKFAST_ATAN2_MAGTHRESH )
03831 continue;
03832 j3array[0]=IKatan2(((gconst26)*(((((r10)*(x888)))+(((IkReal(-1.00000000000000))*(r11)*(x889)))+(((r12)*(sj7)))))), ((gconst26)*(((((r00)*(x888)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x889)))))));
03833 sj3array[0]=IKsin(j3array[0]);
03834 cj3array[0]=IKcos(j3array[0]);
03835 if( j3array[0] > IKPI )
03836 {
03837 j3array[0]-=IK2PI;
03838 }
03839 else if( j3array[0] < -IKPI )
03840 { j3array[0]+=IK2PI;
03841 }
03842 j3valid[0] = true;
03843 for(int ij3 = 0; ij3 < 1; ++ij3)
03844 {
03845 if( !j3valid[ij3] )
03846 {
03847 continue;
03848 }
03849 _ij3[0] = ij3; _ij3[1] = -1;
03850 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
03851 {
03852 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
03853 {
03854 j3valid[iij3]=false; _ij3[1] = iij3; break;
03855 }
03856 }
03857 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
03858 {
03859 IkReal evalcond[4];
03860 IkReal x890=IKsin(j3);
03861 IkReal x891=IKcos(j3);
03862 IkReal x892=((r01)*(sj8));
03863 IkReal x893=((cj8)*(sj7));
03864 IkReal x894=((IkReal(1.00000000000000))*(sj7));
03865 IkReal x895=((IkReal(1.00000000000000))*(r12));
03866 IkReal x896=((r11)*(sj8));
03867 IkReal x897=((IkReal(1.00000000000000))*(r00));
03868 IkReal x898=((cj7)*(x890));
03869 IkReal x899=((r10)*(x891));
03870 IkReal x900=((IkReal(1.00000000000000))*(cj7)*(x891));
03871 evalcond[0]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x891)))+(((IkReal(-1.00000000000000))*(sj8)*(x899)))+(((r00)*(sj8)*(x890)))+(((cj8)*(r01)*(x890))));
03872 evalcond[1]=((((IkReal(-1.00000000000000))*(x896)*(x900)))+(((IkReal(-1.00000000000000))*(r02)*(x890)*(x894)))+(((r12)*(sj7)*(x891)))+(((cj7)*(cj8)*(x899)))+(((x892)*(x898)))+(((IkReal(-1.00000000000000))*(cj8)*(x897)*(x898))));
03873 evalcond[2]=((IkReal(-1.00000000000000))+(((r02)*(x898)))+(((sj7)*(x890)*(x892)))+(((IkReal(-1.00000000000000))*(x891)*(x894)*(x896)))+(((IkReal(-1.00000000000000))*(x890)*(x893)*(x897)))+(((x893)*(x899)))+(((IkReal(-1.00000000000000))*(cj7)*(x891)*(x895))));
03874 evalcond[3]=((((r00)*(x891)*(x893)))+(((IkReal(-1.00000000000000))*(x895)*(x898)))+(((r10)*(x890)*(x893)))+(((IkReal(-1.00000000000000))*(x890)*(x894)*(x896)))+(((IkReal(-1.00000000000000))*(x891)*(x892)*(x894)))+(((IkReal(-1.00000000000000))*(r02)*(x900))));
03875 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03876 {
03877 continue;
03878 }
03879 }
03880
03881 {
03882 IkReal dummyeval[1];
03883 IkReal gconst28;
03884 gconst28=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
03885 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
03886 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03887 {
03888 {
03889 IkReal dummyeval[1];
03890 IkReal gconst29;
03891 gconst29=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
03892 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
03893 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03894 {
03895 continue;
03896
03897 } else
03898 {
03899 {
03900 IkReal j4array[1], cj4array[1], sj4array[1];
03901 bool j4valid[1]={false};
03902 _nj4 = 1;
03903 IkReal x901=((IkReal(1.00000000000000))*(sj5));
03904 IkReal x902=((r20)*(sj8));
03905 IkReal x903=((cj5)*(cj8));
03906 IkReal x904=((r11)*(sj3));
03907 IkReal x905=((cj3)*(r01));
03908 IkReal x906=((cj3)*(r00)*(sj8));
03909 IkReal x907=((r10)*(sj3)*(sj8));
03910 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(r21)*(x903)))+(((IkReal(-1.00000000000000))*(x901)*(x906)))+(((IkReal(-1.00000000000000))*(x901)*(x907)))+(((IkReal(-1.00000000000000))*(cj5)*(x902)))+(((IkReal(-1.00000000000000))*(cj8)*(x901)*(x905)))+(((IkReal(-1.00000000000000))*(cj8)*(x901)*(x904))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((x903)*(x905)))+(((x903)*(x904)))+(((cj5)*(x907)))+(((cj5)*(x906)))+(((IkReal(-1.00000000000000))*(x901)*(x902)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x901))))))) < IKFAST_ATAN2_MAGTHRESH )
03911 continue;
03912 j4array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(r21)*(x903)))+(((IkReal(-1.00000000000000))*(x901)*(x906)))+(((IkReal(-1.00000000000000))*(x901)*(x907)))+(((IkReal(-1.00000000000000))*(cj5)*(x902)))+(((IkReal(-1.00000000000000))*(cj8)*(x901)*(x905)))+(((IkReal(-1.00000000000000))*(cj8)*(x901)*(x904)))))), ((gconst29)*(((((x903)*(x905)))+(((x903)*(x904)))+(((cj5)*(x907)))+(((cj5)*(x906)))+(((IkReal(-1.00000000000000))*(x901)*(x902)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x901)))))));
03913 sj4array[0]=IKsin(j4array[0]);
03914 cj4array[0]=IKcos(j4array[0]);
03915 if( j4array[0] > IKPI )
03916 {
03917 j4array[0]-=IK2PI;
03918 }
03919 else if( j4array[0] < -IKPI )
03920 { j4array[0]+=IK2PI;
03921 }
03922 j4valid[0] = true;
03923 for(int ij4 = 0; ij4 < 1; ++ij4)
03924 {
03925 if( !j4valid[ij4] )
03926 {
03927 continue;
03928 }
03929 _ij4[0] = ij4; _ij4[1] = -1;
03930 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
03931 {
03932 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
03933 {
03934 j4valid[iij4]=false; _ij4[1] = iij4; break;
03935 }
03936 }
03937 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
03938 {
03939 IkReal evalcond[4];
03940 IkReal x908=IKsin(j4);
03941 IkReal x909=IKcos(j4);
03942 IkReal x910=((IkReal(1.00000000000000))*(cj8));
03943 IkReal x911=((cj3)*(r01));
03944 IkReal x912=((cj3)*(r00));
03945 IkReal x913=((cj7)*(cj8));
03946 IkReal x914=((r11)*(sj3));
03947 IkReal x915=((IkReal(1.00000000000000))*(sj8));
03948 IkReal x916=((r10)*(sj3));
03949 IkReal x917=((sj5)*(x909));
03950 IkReal x918=((cj5)*(x908));
03951 IkReal x919=((cj5)*(x909));
03952 IkReal x920=((sj5)*(x908));
03953 IkReal x921=((x918)+(x917));
03954 evalcond[0]=((((cj8)*(r21)))+(x921)+(((r20)*(sj8))));
03955 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x910)))+(((IkReal(-1.00000000000000))*(x919)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x920)+(((cj7)*(r21)*(sj8))));
03956 evalcond[2]=((((IkReal(-1.00000000000000))*(x920)))+(((IkReal(-1.00000000000000))*(x910)*(x911)))+(((IkReal(-1.00000000000000))*(x910)*(x914)))+(((IkReal(-1.00000000000000))*(x912)*(x915)))+(x919)+(((IkReal(-1.00000000000000))*(x915)*(x916))));
03957 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x911)*(x915)))+(((cj3)*(r02)*(sj7)))+(((x912)*(x913)))+(((x913)*(x916)))+(x921)+(((IkReal(-1.00000000000000))*(cj7)*(x914)*(x915))));
03958 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03959 {
03960 continue;
03961 }
03962 }
03963
03964 {
03965 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03966 vinfos[0].jointtype = 1;
03967 vinfos[0].foffset = j3;
03968 vinfos[0].indices[0] = _ij3[0];
03969 vinfos[0].indices[1] = _ij3[1];
03970 vinfos[0].maxsolutions = _nj3;
03971 vinfos[1].jointtype = 1;
03972 vinfos[1].foffset = j4;
03973 vinfos[1].indices[0] = _ij4[0];
03974 vinfos[1].indices[1] = _ij4[1];
03975 vinfos[1].maxsolutions = _nj4;
03976 vinfos[2].jointtype = 1;
03977 vinfos[2].foffset = j5;
03978 vinfos[2].indices[0] = _ij5[0];
03979 vinfos[2].indices[1] = _ij5[1];
03980 vinfos[2].maxsolutions = _nj5;
03981 vinfos[3].jointtype = 1;
03982 vinfos[3].foffset = j6;
03983 vinfos[3].indices[0] = _ij6[0];
03984 vinfos[3].indices[1] = _ij6[1];
03985 vinfos[3].maxsolutions = _nj6;
03986 vinfos[4].jointtype = 1;
03987 vinfos[4].foffset = j7;
03988 vinfos[4].indices[0] = _ij7[0];
03989 vinfos[4].indices[1] = _ij7[1];
03990 vinfos[4].maxsolutions = _nj7;
03991 vinfos[5].jointtype = 1;
03992 vinfos[5].foffset = j8;
03993 vinfos[5].indices[0] = _ij8[0];
03994 vinfos[5].indices[1] = _ij8[1];
03995 vinfos[5].maxsolutions = _nj8;
03996 std::vector<int> vfree(0);
03997 solutions.AddSolution(vinfos,vfree);
03998 }
03999 }
04000 }
04001
04002 }
04003
04004 }
04005
04006 } else
04007 {
04008 {
04009 IkReal j4array[1], cj4array[1], sj4array[1];
04010 bool j4valid[1]={false};
04011 _nj4 = 1;
04012 IkReal x922=((r22)*(sj7));
04013 IkReal x923=((cj8)*(sj5));
04014 IkReal x924=((sj5)*(sj8));
04015 IkReal x925=((cj7)*(r20));
04016 IkReal x926=((cj5)*(cj8));
04017 IkReal x927=((cj7)*(r21));
04018 IkReal x928=((cj5)*(sj8));
04019 if( IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(sj5)*(x922)))+(((x924)*(x927)))+(((r21)*(x926)))+(((IkReal(-1.00000000000000))*(x923)*(x925)))+(((r20)*(x928))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(x927)*(x928)))+(((r21)*(x923)))+(((r20)*(x924)))+(((x925)*(x926)))+(((cj5)*(x922))))))) < IKFAST_ATAN2_MAGTHRESH )
04020 continue;
04021 j4array[0]=IKatan2(((gconst28)*(((((IkReal(-1.00000000000000))*(sj5)*(x922)))+(((x924)*(x927)))+(((r21)*(x926)))+(((IkReal(-1.00000000000000))*(x923)*(x925)))+(((r20)*(x928)))))), ((gconst28)*(((((IkReal(-1.00000000000000))*(x927)*(x928)))+(((r21)*(x923)))+(((r20)*(x924)))+(((x925)*(x926)))+(((cj5)*(x922)))))));
04022 sj4array[0]=IKsin(j4array[0]);
04023 cj4array[0]=IKcos(j4array[0]);
04024 if( j4array[0] > IKPI )
04025 {
04026 j4array[0]-=IK2PI;
04027 }
04028 else if( j4array[0] < -IKPI )
04029 { j4array[0]+=IK2PI;
04030 }
04031 j4valid[0] = true;
04032 for(int ij4 = 0; ij4 < 1; ++ij4)
04033 {
04034 if( !j4valid[ij4] )
04035 {
04036 continue;
04037 }
04038 _ij4[0] = ij4; _ij4[1] = -1;
04039 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04040 {
04041 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04042 {
04043 j4valid[iij4]=false; _ij4[1] = iij4; break;
04044 }
04045 }
04046 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04047 {
04048 IkReal evalcond[4];
04049 IkReal x929=IKsin(j4);
04050 IkReal x930=IKcos(j4);
04051 IkReal x931=((IkReal(1.00000000000000))*(cj8));
04052 IkReal x932=((cj3)*(r01));
04053 IkReal x933=((cj3)*(r00));
04054 IkReal x934=((cj7)*(cj8));
04055 IkReal x935=((r11)*(sj3));
04056 IkReal x936=((IkReal(1.00000000000000))*(sj8));
04057 IkReal x937=((r10)*(sj3));
04058 IkReal x938=((sj5)*(x930));
04059 IkReal x939=((cj5)*(x929));
04060 IkReal x940=((cj5)*(x930));
04061 IkReal x941=((sj5)*(x929));
04062 IkReal x942=((x939)+(x938));
04063 evalcond[0]=((((cj8)*(r21)))+(x942)+(((r20)*(sj8))));
04064 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x931)))+(((IkReal(-1.00000000000000))*(x940)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x941)+(((cj7)*(r21)*(sj8))));
04065 evalcond[2]=((((IkReal(-1.00000000000000))*(x931)*(x932)))+(((IkReal(-1.00000000000000))*(x931)*(x935)))+(((IkReal(-1.00000000000000))*(x941)))+(((IkReal(-1.00000000000000))*(x936)*(x937)))+(((IkReal(-1.00000000000000))*(x933)*(x936)))+(x940));
04066 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x932)*(x936)))+(((IkReal(-1.00000000000000))*(cj7)*(x935)*(x936)))+(((cj3)*(r02)*(sj7)))+(((x934)*(x937)))+(x942)+(((x933)*(x934))));
04067 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04068 {
04069 continue;
04070 }
04071 }
04072
04073 {
04074 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04075 vinfos[0].jointtype = 1;
04076 vinfos[0].foffset = j3;
04077 vinfos[0].indices[0] = _ij3[0];
04078 vinfos[0].indices[1] = _ij3[1];
04079 vinfos[0].maxsolutions = _nj3;
04080 vinfos[1].jointtype = 1;
04081 vinfos[1].foffset = j4;
04082 vinfos[1].indices[0] = _ij4[0];
04083 vinfos[1].indices[1] = _ij4[1];
04084 vinfos[1].maxsolutions = _nj4;
04085 vinfos[2].jointtype = 1;
04086 vinfos[2].foffset = j5;
04087 vinfos[2].indices[0] = _ij5[0];
04088 vinfos[2].indices[1] = _ij5[1];
04089 vinfos[2].maxsolutions = _nj5;
04090 vinfos[3].jointtype = 1;
04091 vinfos[3].foffset = j6;
04092 vinfos[3].indices[0] = _ij6[0];
04093 vinfos[3].indices[1] = _ij6[1];
04094 vinfos[3].maxsolutions = _nj6;
04095 vinfos[4].jointtype = 1;
04096 vinfos[4].foffset = j7;
04097 vinfos[4].indices[0] = _ij7[0];
04098 vinfos[4].indices[1] = _ij7[1];
04099 vinfos[4].maxsolutions = _nj7;
04100 vinfos[5].jointtype = 1;
04101 vinfos[5].foffset = j8;
04102 vinfos[5].indices[0] = _ij8[0];
04103 vinfos[5].indices[1] = _ij8[1];
04104 vinfos[5].maxsolutions = _nj8;
04105 std::vector<int> vfree(0);
04106 solutions.AddSolution(vinfos,vfree);
04107 }
04108 }
04109 }
04110
04111 }
04112
04113 }
04114 }
04115 }
04116
04117 }
04118
04119 }
04120
04121 } else
04122 {
04123 {
04124 IkReal j3array[1], cj3array[1], sj3array[1];
04125 bool j3valid[1]={false};
04126 _nj3 = 1;
04127 IkReal x943=((IkReal(1.00000000000000))*(cj8));
04128 IkReal x944=((IkReal(1.00000000000000))*(sj8));
04129 if( IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r11)*(x943)))+(((IkReal(-1.00000000000000))*(r10)*(x944))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x943)))+(((IkReal(-1.00000000000000))*(r00)*(x944))))))) < IKFAST_ATAN2_MAGTHRESH )
04130 continue;
04131 j3array[0]=IKatan2(((gconst25)*(((((IkReal(-1.00000000000000))*(r11)*(x943)))+(((IkReal(-1.00000000000000))*(r10)*(x944)))))), ((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x943)))+(((IkReal(-1.00000000000000))*(r00)*(x944)))))));
04132 sj3array[0]=IKsin(j3array[0]);
04133 cj3array[0]=IKcos(j3array[0]);
04134 if( j3array[0] > IKPI )
04135 {
04136 j3array[0]-=IK2PI;
04137 }
04138 else if( j3array[0] < -IKPI )
04139 { j3array[0]+=IK2PI;
04140 }
04141 j3valid[0] = true;
04142 for(int ij3 = 0; ij3 < 1; ++ij3)
04143 {
04144 if( !j3valid[ij3] )
04145 {
04146 continue;
04147 }
04148 _ij3[0] = ij3; _ij3[1] = -1;
04149 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04150 {
04151 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04152 {
04153 j3valid[iij3]=false; _ij3[1] = iij3; break;
04154 }
04155 }
04156 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04157 {
04158 IkReal evalcond[4];
04159 IkReal x945=IKsin(j3);
04160 IkReal x946=IKcos(j3);
04161 IkReal x947=((r01)*(sj8));
04162 IkReal x948=((cj8)*(sj7));
04163 IkReal x949=((IkReal(1.00000000000000))*(sj7));
04164 IkReal x950=((IkReal(1.00000000000000))*(r12));
04165 IkReal x951=((r11)*(sj8));
04166 IkReal x952=((IkReal(1.00000000000000))*(r00));
04167 IkReal x953=((cj7)*(x945));
04168 IkReal x954=((r10)*(x946));
04169 IkReal x955=((IkReal(1.00000000000000))*(cj7)*(x946));
04170 evalcond[0]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x946)))+(((cj8)*(r01)*(x945)))+(((r00)*(sj8)*(x945)))+(((IkReal(-1.00000000000000))*(sj8)*(x954))));
04171 evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(x945)*(x949)))+(((cj7)*(cj8)*(x954)))+(((IkReal(-1.00000000000000))*(cj8)*(x952)*(x953)))+(((x947)*(x953)))+(((r12)*(sj7)*(x946)))+(((IkReal(-1.00000000000000))*(x951)*(x955))));
04172 evalcond[2]=((IkReal(-1.00000000000000))+(((sj7)*(x945)*(x947)))+(((x948)*(x954)))+(((IkReal(-1.00000000000000))*(cj7)*(x946)*(x950)))+(((IkReal(-1.00000000000000))*(x946)*(x949)*(x951)))+(((r02)*(x953)))+(((IkReal(-1.00000000000000))*(x945)*(x948)*(x952))));
04173 evalcond[3]=((((r00)*(x946)*(x948)))+(((IkReal(-1.00000000000000))*(x946)*(x947)*(x949)))+(((IkReal(-1.00000000000000))*(x950)*(x953)))+(((r10)*(x945)*(x948)))+(((IkReal(-1.00000000000000))*(r02)*(x955)))+(((IkReal(-1.00000000000000))*(x945)*(x949)*(x951))));
04174 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04175 {
04176 continue;
04177 }
04178 }
04179
04180 {
04181 IkReal dummyeval[1];
04182 IkReal gconst28;
04183 gconst28=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
04184 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
04185 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04186 {
04187 {
04188 IkReal dummyeval[1];
04189 IkReal gconst29;
04190 gconst29=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
04191 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
04192 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04193 {
04194 continue;
04195
04196 } else
04197 {
04198 {
04199 IkReal j4array[1], cj4array[1], sj4array[1];
04200 bool j4valid[1]={false};
04201 _nj4 = 1;
04202 IkReal x956=((IkReal(1.00000000000000))*(sj5));
04203 IkReal x957=((r20)*(sj8));
04204 IkReal x958=((cj5)*(cj8));
04205 IkReal x959=((r11)*(sj3));
04206 IkReal x960=((cj3)*(r01));
04207 IkReal x961=((cj3)*(r00)*(sj8));
04208 IkReal x962=((r10)*(sj3)*(sj8));
04209 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(cj8)*(x956)*(x959)))+(((IkReal(-1.00000000000000))*(cj8)*(x956)*(x960)))+(((IkReal(-1.00000000000000))*(x956)*(x962)))+(((IkReal(-1.00000000000000))*(x956)*(x961)))+(((IkReal(-1.00000000000000))*(cj5)*(x957)))+(((IkReal(-1.00000000000000))*(r21)*(x958))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(x956)*(x957)))+(((x958)*(x959)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x956)))+(((x958)*(x960)))+(((cj5)*(x961)))+(((cj5)*(x962))))))) < IKFAST_ATAN2_MAGTHRESH )
04210 continue;
04211 j4array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(cj8)*(x956)*(x959)))+(((IkReal(-1.00000000000000))*(cj8)*(x956)*(x960)))+(((IkReal(-1.00000000000000))*(x956)*(x962)))+(((IkReal(-1.00000000000000))*(x956)*(x961)))+(((IkReal(-1.00000000000000))*(cj5)*(x957)))+(((IkReal(-1.00000000000000))*(r21)*(x958)))))), ((gconst29)*(((((IkReal(-1.00000000000000))*(x956)*(x957)))+(((x958)*(x959)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x956)))+(((x958)*(x960)))+(((cj5)*(x961)))+(((cj5)*(x962)))))));
04212 sj4array[0]=IKsin(j4array[0]);
04213 cj4array[0]=IKcos(j4array[0]);
04214 if( j4array[0] > IKPI )
04215 {
04216 j4array[0]-=IK2PI;
04217 }
04218 else if( j4array[0] < -IKPI )
04219 { j4array[0]+=IK2PI;
04220 }
04221 j4valid[0] = true;
04222 for(int ij4 = 0; ij4 < 1; ++ij4)
04223 {
04224 if( !j4valid[ij4] )
04225 {
04226 continue;
04227 }
04228 _ij4[0] = ij4; _ij4[1] = -1;
04229 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04230 {
04231 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04232 {
04233 j4valid[iij4]=false; _ij4[1] = iij4; break;
04234 }
04235 }
04236 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04237 {
04238 IkReal evalcond[4];
04239 IkReal x963=IKsin(j4);
04240 IkReal x964=IKcos(j4);
04241 IkReal x965=((IkReal(1.00000000000000))*(cj8));
04242 IkReal x966=((cj3)*(r01));
04243 IkReal x967=((cj3)*(r00));
04244 IkReal x968=((cj7)*(cj8));
04245 IkReal x969=((r11)*(sj3));
04246 IkReal x970=((IkReal(1.00000000000000))*(sj8));
04247 IkReal x971=((r10)*(sj3));
04248 IkReal x972=((sj5)*(x964));
04249 IkReal x973=((cj5)*(x963));
04250 IkReal x974=((cj5)*(x964));
04251 IkReal x975=((sj5)*(x963));
04252 IkReal x976=((x973)+(x972));
04253 evalcond[0]=((((cj8)*(r21)))+(x976)+(((r20)*(sj8))));
04254 evalcond[1]=((((IkReal(-1.00000000000000))*(x974)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(x975)+(((cj7)*(r21)*(sj8)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x965))));
04255 evalcond[2]=((((IkReal(-1.00000000000000))*(x975)))+(((IkReal(-1.00000000000000))*(x965)*(x969)))+(((IkReal(-1.00000000000000))*(x965)*(x966)))+(((IkReal(-1.00000000000000))*(x967)*(x970)))+(((IkReal(-1.00000000000000))*(x970)*(x971)))+(x974));
04256 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((x968)*(x971)))+(((IkReal(-1.00000000000000))*(cj7)*(x969)*(x970)))+(((x967)*(x968)))+(((cj3)*(r02)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x966)*(x970)))+(x976));
04257 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04258 {
04259 continue;
04260 }
04261 }
04262
04263 {
04264 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04265 vinfos[0].jointtype = 1;
04266 vinfos[0].foffset = j3;
04267 vinfos[0].indices[0] = _ij3[0];
04268 vinfos[0].indices[1] = _ij3[1];
04269 vinfos[0].maxsolutions = _nj3;
04270 vinfos[1].jointtype = 1;
04271 vinfos[1].foffset = j4;
04272 vinfos[1].indices[0] = _ij4[0];
04273 vinfos[1].indices[1] = _ij4[1];
04274 vinfos[1].maxsolutions = _nj4;
04275 vinfos[2].jointtype = 1;
04276 vinfos[2].foffset = j5;
04277 vinfos[2].indices[0] = _ij5[0];
04278 vinfos[2].indices[1] = _ij5[1];
04279 vinfos[2].maxsolutions = _nj5;
04280 vinfos[3].jointtype = 1;
04281 vinfos[3].foffset = j6;
04282 vinfos[3].indices[0] = _ij6[0];
04283 vinfos[3].indices[1] = _ij6[1];
04284 vinfos[3].maxsolutions = _nj6;
04285 vinfos[4].jointtype = 1;
04286 vinfos[4].foffset = j7;
04287 vinfos[4].indices[0] = _ij7[0];
04288 vinfos[4].indices[1] = _ij7[1];
04289 vinfos[4].maxsolutions = _nj7;
04290 vinfos[5].jointtype = 1;
04291 vinfos[5].foffset = j8;
04292 vinfos[5].indices[0] = _ij8[0];
04293 vinfos[5].indices[1] = _ij8[1];
04294 vinfos[5].maxsolutions = _nj8;
04295 std::vector<int> vfree(0);
04296 solutions.AddSolution(vinfos,vfree);
04297 }
04298 }
04299 }
04300
04301 }
04302
04303 }
04304
04305 } else
04306 {
04307 {
04308 IkReal j4array[1], cj4array[1], sj4array[1];
04309 bool j4valid[1]={false};
04310 _nj4 = 1;
04311 IkReal x977=((r22)*(sj7));
04312 IkReal x978=((cj8)*(sj5));
04313 IkReal x979=((sj5)*(sj8));
04314 IkReal x980=((cj7)*(r20));
04315 IkReal x981=((cj5)*(cj8));
04316 IkReal x982=((cj7)*(r21));
04317 IkReal x983=((cj5)*(sj8));
04318 if( IKabs(((gconst28)*(((((r21)*(x981)))+(((IkReal(-1.00000000000000))*(x978)*(x980)))+(((x979)*(x982)))+(((IkReal(-1.00000000000000))*(sj5)*(x977)))+(((r20)*(x983))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(x982)*(x983)))+(((cj5)*(x977)))+(((r21)*(x978)))+(((r20)*(x979)))+(((x980)*(x981))))))) < IKFAST_ATAN2_MAGTHRESH )
04319 continue;
04320 j4array[0]=IKatan2(((gconst28)*(((((r21)*(x981)))+(((IkReal(-1.00000000000000))*(x978)*(x980)))+(((x979)*(x982)))+(((IkReal(-1.00000000000000))*(sj5)*(x977)))+(((r20)*(x983)))))), ((gconst28)*(((((IkReal(-1.00000000000000))*(x982)*(x983)))+(((cj5)*(x977)))+(((r21)*(x978)))+(((r20)*(x979)))+(((x980)*(x981)))))));
04321 sj4array[0]=IKsin(j4array[0]);
04322 cj4array[0]=IKcos(j4array[0]);
04323 if( j4array[0] > IKPI )
04324 {
04325 j4array[0]-=IK2PI;
04326 }
04327 else if( j4array[0] < -IKPI )
04328 { j4array[0]+=IK2PI;
04329 }
04330 j4valid[0] = true;
04331 for(int ij4 = 0; ij4 < 1; ++ij4)
04332 {
04333 if( !j4valid[ij4] )
04334 {
04335 continue;
04336 }
04337 _ij4[0] = ij4; _ij4[1] = -1;
04338 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04339 {
04340 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04341 {
04342 j4valid[iij4]=false; _ij4[1] = iij4; break;
04343 }
04344 }
04345 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04346 {
04347 IkReal evalcond[4];
04348 IkReal x984=IKsin(j4);
04349 IkReal x985=IKcos(j4);
04350 IkReal x986=((IkReal(1.00000000000000))*(cj8));
04351 IkReal x987=((cj3)*(r01));
04352 IkReal x988=((cj3)*(r00));
04353 IkReal x989=((cj7)*(cj8));
04354 IkReal x990=((r11)*(sj3));
04355 IkReal x991=((IkReal(1.00000000000000))*(sj8));
04356 IkReal x992=((r10)*(sj3));
04357 IkReal x993=((sj5)*(x985));
04358 IkReal x994=((cj5)*(x984));
04359 IkReal x995=((cj5)*(x985));
04360 IkReal x996=((sj5)*(x984));
04361 IkReal x997=((x993)+(x994));
04362 evalcond[0]=((((cj8)*(r21)))+(x997)+(((r20)*(sj8))));
04363 evalcond[1]=((((IkReal(-1.00000000000000))*(x995)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x986)))+(x996)+(((cj7)*(r21)*(sj8))));
04364 evalcond[2]=((((IkReal(-1.00000000000000))*(x996)))+(((IkReal(-1.00000000000000))*(x991)*(x992)))+(((IkReal(-1.00000000000000))*(x986)*(x987)))+(x995)+(((IkReal(-1.00000000000000))*(x986)*(x990)))+(((IkReal(-1.00000000000000))*(x988)*(x991))));
04365 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((x989)*(x992)))+(((x988)*(x989)))+(((IkReal(-1.00000000000000))*(cj7)*(x987)*(x991)))+(((IkReal(-1.00000000000000))*(cj7)*(x990)*(x991)))+(((cj3)*(r02)*(sj7)))+(x997));
04366 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04367 {
04368 continue;
04369 }
04370 }
04371
04372 {
04373 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04374 vinfos[0].jointtype = 1;
04375 vinfos[0].foffset = j3;
04376 vinfos[0].indices[0] = _ij3[0];
04377 vinfos[0].indices[1] = _ij3[1];
04378 vinfos[0].maxsolutions = _nj3;
04379 vinfos[1].jointtype = 1;
04380 vinfos[1].foffset = j4;
04381 vinfos[1].indices[0] = _ij4[0];
04382 vinfos[1].indices[1] = _ij4[1];
04383 vinfos[1].maxsolutions = _nj4;
04384 vinfos[2].jointtype = 1;
04385 vinfos[2].foffset = j5;
04386 vinfos[2].indices[0] = _ij5[0];
04387 vinfos[2].indices[1] = _ij5[1];
04388 vinfos[2].maxsolutions = _nj5;
04389 vinfos[3].jointtype = 1;
04390 vinfos[3].foffset = j6;
04391 vinfos[3].indices[0] = _ij6[0];
04392 vinfos[3].indices[1] = _ij6[1];
04393 vinfos[3].maxsolutions = _nj6;
04394 vinfos[4].jointtype = 1;
04395 vinfos[4].foffset = j7;
04396 vinfos[4].indices[0] = _ij7[0];
04397 vinfos[4].indices[1] = _ij7[1];
04398 vinfos[4].maxsolutions = _nj7;
04399 vinfos[5].jointtype = 1;
04400 vinfos[5].foffset = j8;
04401 vinfos[5].indices[0] = _ij8[0];
04402 vinfos[5].indices[1] = _ij8[1];
04403 vinfos[5].maxsolutions = _nj8;
04404 std::vector<int> vfree(0);
04405 solutions.AddSolution(vinfos,vfree);
04406 }
04407 }
04408 }
04409
04410 }
04411
04412 }
04413 }
04414 }
04415
04416 }
04417
04418 }
04419
04420 } else
04421 {
04422 {
04423 IkReal j4array[1], cj4array[1], sj4array[1];
04424 bool j4valid[1]={false};
04425 _nj4 = 1;
04426 IkReal x998=((cj8)*(r21));
04427 IkReal x999=((r22)*(sj7));
04428 IkReal x1000=((sj5)*(sj8));
04429 IkReal x1001=((IkReal(1.00000000000000))*(sj5));
04430 IkReal x1002=((cj5)*(cj7));
04431 IkReal x1003=((cj8)*(r20));
04432 if( IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(cj7)*(x1001)*(x1003)))+(((cj5)*(x998)))+(((cj5)*(r20)*(sj8)))+(((IkReal(-1.00000000000000))*(x1001)*(x999)))+(((cj7)*(r21)*(x1000))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst27)*(((((cj5)*(x999)))+(((sj5)*(x998)))+(((r20)*(x1000)))+(((x1002)*(x1003)))+(((IkReal(-1.00000000000000))*(r21)*(sj8)*(x1002))))))) < IKFAST_ATAN2_MAGTHRESH )
04433 continue;
04434 j4array[0]=IKatan2(((gconst27)*(((((IkReal(-1.00000000000000))*(cj7)*(x1001)*(x1003)))+(((cj5)*(x998)))+(((cj5)*(r20)*(sj8)))+(((IkReal(-1.00000000000000))*(x1001)*(x999)))+(((cj7)*(r21)*(x1000)))))), ((gconst27)*(((((cj5)*(x999)))+(((sj5)*(x998)))+(((r20)*(x1000)))+(((x1002)*(x1003)))+(((IkReal(-1.00000000000000))*(r21)*(sj8)*(x1002)))))));
04435 sj4array[0]=IKsin(j4array[0]);
04436 cj4array[0]=IKcos(j4array[0]);
04437 if( j4array[0] > IKPI )
04438 {
04439 j4array[0]-=IK2PI;
04440 }
04441 else if( j4array[0] < -IKPI )
04442 { j4array[0]+=IK2PI;
04443 }
04444 j4valid[0] = true;
04445 for(int ij4 = 0; ij4 < 1; ++ij4)
04446 {
04447 if( !j4valid[ij4] )
04448 {
04449 continue;
04450 }
04451 _ij4[0] = ij4; _ij4[1] = -1;
04452 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04453 {
04454 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04455 {
04456 j4valid[iij4]=false; _ij4[1] = iij4; break;
04457 }
04458 }
04459 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04460 {
04461 IkReal evalcond[2];
04462 IkReal x1004=IKsin(j4);
04463 IkReal x1005=IKcos(j4);
04464 evalcond[0]=((((cj8)*(r21)))+(((cj5)*(x1004)))+(((sj5)*(x1005)))+(((r20)*(sj8))));
04465 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(cj8)*(r20)))+(((sj5)*(x1004)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj5)*(x1005)))+(((cj7)*(r21)*(sj8))));
04466 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
04467 {
04468 continue;
04469 }
04470 }
04471
04472 {
04473 IkReal dummyeval[1];
04474 IkReal gconst31;
04475 IkReal x1006=(sj8)*(sj8);
04476 IkReal x1007=(cj8)*(cj8);
04477 IkReal x1008=((IkReal(1.00000000000000))*(r10));
04478 IkReal x1009=((cj7)*(sj8));
04479 IkReal x1010=((r00)*(r11));
04480 IkReal x1011=((cj7)*(cj8));
04481 IkReal x1012=((sj7)*(x1006));
04482 IkReal x1013=((sj7)*(x1007));
04483 gconst31=IKsign(((((r01)*(r12)*(x1011)))+(((r00)*(r12)*(x1009)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1011)))+(((IkReal(-1.00000000000000))*(r01)*(x1008)*(x1013)))+(((IkReal(-1.00000000000000))*(r01)*(x1008)*(x1012)))+(((x1010)*(x1013)))+(((x1010)*(x1012)))+(((IkReal(-1.00000000000000))*(r02)*(x1008)*(x1009)))));
04484 IkReal x1014=(sj8)*(sj8);
04485 IkReal x1015=(cj8)*(cj8);
04486 IkReal x1016=((IkReal(1.00000000000000))*(r10));
04487 IkReal x1017=((cj7)*(sj8));
04488 IkReal x1018=((r00)*(r11));
04489 IkReal x1019=((cj7)*(cj8));
04490 IkReal x1020=((sj7)*(x1014));
04491 IkReal x1021=((sj7)*(x1015));
04492 dummyeval[0]=((((IkReal(-1.00000000000000))*(r01)*(x1016)*(x1021)))+(((IkReal(-1.00000000000000))*(r01)*(x1016)*(x1020)))+(((r01)*(r12)*(x1019)))+(((r00)*(r12)*(x1017)))+(((x1018)*(x1021)))+(((x1018)*(x1020)))+(((IkReal(-1.00000000000000))*(r02)*(x1016)*(x1017)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1019))));
04493 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04494 {
04495 {
04496 IkReal dummyeval[1];
04497 IkReal gconst30;
04498 IkReal x1022=(cj8)*(cj8);
04499 IkReal x1023=(sj8)*(sj8);
04500 IkReal x1024=((IkReal(2.00000000000000))*(cj8)*(sj8));
04501 IkReal x1025=((IkReal(1.00000000000000))*(x1022));
04502 IkReal x1026=((IkReal(1.00000000000000))*(x1023));
04503 gconst30=IKsign(((((IkReal(-1.00000000000000))*(x1025)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1026)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1024)))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1024)))+(((IkReal(-1.00000000000000))*(x1026)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1025)*((r11)*(r11))))));
04504 IkReal x1027=(cj8)*(cj8);
04505 IkReal x1028=(sj8)*(sj8);
04506 IkReal x1029=((IkReal(2.00000000000000))*(cj8)*(sj8));
04507 IkReal x1030=((IkReal(1.00000000000000))*(x1027));
04508 IkReal x1031=((IkReal(1.00000000000000))*(x1028));
04509 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1030)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1030)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1029)))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1029)))+(((IkReal(-1.00000000000000))*(x1031)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1031)*((r10)*(r10)))));
04510 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04511 {
04512 continue;
04513
04514 } else
04515 {
04516 {
04517 IkReal j3array[1], cj3array[1], sj3array[1];
04518 bool j3valid[1]={false};
04519 _nj3 = 1;
04520 IkReal x1032=((sj4)*(sj5));
04521 IkReal x1033=((r10)*(sj8));
04522 IkReal x1034=((cj8)*(r11));
04523 IkReal x1035=((cj8)*(r01));
04524 IkReal x1036=((r00)*(sj8));
04525 IkReal x1037=((IkReal(1.00000000000000))*(cj4)*(cj5));
04526 if( IKabs(((gconst30)*(((((IkReal(-1.00000000000000))*(x1033)*(x1037)))+(((IkReal(-1.00000000000000))*(x1034)*(x1037)))+(((x1032)*(x1034)))+(((x1032)*(x1033))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst30)*(((((IkReal(-1.00000000000000))*(x1035)*(x1037)))+(((IkReal(-1.00000000000000))*(x1036)*(x1037)))+(((x1032)*(x1035)))+(((x1032)*(x1036))))))) < IKFAST_ATAN2_MAGTHRESH )
04527 continue;
04528 j3array[0]=IKatan2(((gconst30)*(((((IkReal(-1.00000000000000))*(x1033)*(x1037)))+(((IkReal(-1.00000000000000))*(x1034)*(x1037)))+(((x1032)*(x1034)))+(((x1032)*(x1033)))))), ((gconst30)*(((((IkReal(-1.00000000000000))*(x1035)*(x1037)))+(((IkReal(-1.00000000000000))*(x1036)*(x1037)))+(((x1032)*(x1035)))+(((x1032)*(x1036)))))));
04529 sj3array[0]=IKsin(j3array[0]);
04530 cj3array[0]=IKcos(j3array[0]);
04531 if( j3array[0] > IKPI )
04532 {
04533 j3array[0]-=IK2PI;
04534 }
04535 else if( j3array[0] < -IKPI )
04536 { j3array[0]+=IK2PI;
04537 }
04538 j3valid[0] = true;
04539 for(int ij3 = 0; ij3 < 1; ++ij3)
04540 {
04541 if( !j3valid[ij3] )
04542 {
04543 continue;
04544 }
04545 _ij3[0] = ij3; _ij3[1] = -1;
04546 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04547 {
04548 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04549 {
04550 j3valid[iij3]=false; _ij3[1] = iij3; break;
04551 }
04552 }
04553 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04554 {
04555 IkReal evalcond[6];
04556 IkReal x1038=IKsin(j3);
04557 IkReal x1039=IKcos(j3);
04558 IkReal x1040=((cj7)*(sj8));
04559 IkReal x1041=((r00)*(sj8));
04560 IkReal x1042=((cj8)*(r11));
04561 IkReal x1043=((cj7)*(r02));
04562 IkReal x1044=((cj7)*(r12));
04563 IkReal x1045=((r12)*(sj7));
04564 IkReal x1046=((r02)*(sj7));
04565 IkReal x1047=((sj7)*(sj8));
04566 IkReal x1048=((r10)*(sj8));
04567 IkReal x1049=((cj8)*(r00));
04568 IkReal x1050=((r01)*(x1038));
04569 IkReal x1051=((cj8)*(r10)*(sj7));
04570 IkReal x1052=((IkReal(1.00000000000000))*(x1039));
04571 IkReal x1053=((IkReal(1.00000000000000))*(x1038));
04572 IkReal x1054=((cj7)*(cj8)*(r10));
04573 IkReal x1055=((x1039)*(x1049));
04574 evalcond[0]=((((x1038)*(x1041)))+(((IkReal(-1.00000000000000))*(x1042)*(x1052)))+(((cj8)*(x1050)))+(((IkReal(-1.00000000000000))*(x1048)*(x1052))));
04575 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1052)))+(((IkReal(-1.00000000000000))*(x1042)*(x1053)))+(((IkReal(-1.00000000000000))*(x1041)*(x1052)))+(((cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(x1048)*(x1053)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5))));
04576 evalcond[2]=((((IkReal(-1.00000000000000))*(x1046)*(x1053)))+(((x1039)*(x1054)))+(((x1039)*(x1045)))+(((x1040)*(x1050)))+(((IkReal(-1.00000000000000))*(cj7)*(x1049)*(x1053)))+(((IkReal(-1.00000000000000))*(r11)*(x1040)*(x1052))));
04577 evalcond[3]=((IkReal(-1.00000000000000))+(((x1038)*(x1043)))+(((x1039)*(x1051)))+(((IkReal(-1.00000000000000))*(x1044)*(x1052)))+(((IkReal(-1.00000000000000))*(r11)*(x1047)*(x1052)))+(((IkReal(-1.00000000000000))*(sj7)*(x1049)*(x1053)))+(((x1047)*(x1050))));
04578 evalcond[4]=((((x1038)*(x1045)))+(((x1038)*(x1054)))+(((cj7)*(x1055)))+(((IkReal(-1.00000000000000))*(r01)*(x1040)*(x1052)))+(((x1039)*(x1046)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(r11)*(x1040)*(x1053))));
04579 evalcond[5]=((((x1038)*(x1051)))+(((IkReal(-1.00000000000000))*(r01)*(x1047)*(x1052)))+(((IkReal(-1.00000000000000))*(x1044)*(x1053)))+(((IkReal(-1.00000000000000))*(x1043)*(x1052)))+(((sj7)*(x1055)))+(((IkReal(-1.00000000000000))*(r11)*(x1047)*(x1053))));
04580 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 )
04581 {
04582 continue;
04583 }
04584 }
04585
04586 {
04587 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04588 vinfos[0].jointtype = 1;
04589 vinfos[0].foffset = j3;
04590 vinfos[0].indices[0] = _ij3[0];
04591 vinfos[0].indices[1] = _ij3[1];
04592 vinfos[0].maxsolutions = _nj3;
04593 vinfos[1].jointtype = 1;
04594 vinfos[1].foffset = j4;
04595 vinfos[1].indices[0] = _ij4[0];
04596 vinfos[1].indices[1] = _ij4[1];
04597 vinfos[1].maxsolutions = _nj4;
04598 vinfos[2].jointtype = 1;
04599 vinfos[2].foffset = j5;
04600 vinfos[2].indices[0] = _ij5[0];
04601 vinfos[2].indices[1] = _ij5[1];
04602 vinfos[2].maxsolutions = _nj5;
04603 vinfos[3].jointtype = 1;
04604 vinfos[3].foffset = j6;
04605 vinfos[3].indices[0] = _ij6[0];
04606 vinfos[3].indices[1] = _ij6[1];
04607 vinfos[3].maxsolutions = _nj6;
04608 vinfos[4].jointtype = 1;
04609 vinfos[4].foffset = j7;
04610 vinfos[4].indices[0] = _ij7[0];
04611 vinfos[4].indices[1] = _ij7[1];
04612 vinfos[4].maxsolutions = _nj7;
04613 vinfos[5].jointtype = 1;
04614 vinfos[5].foffset = j8;
04615 vinfos[5].indices[0] = _ij8[0];
04616 vinfos[5].indices[1] = _ij8[1];
04617 vinfos[5].maxsolutions = _nj8;
04618 std::vector<int> vfree(0);
04619 solutions.AddSolution(vinfos,vfree);
04620 }
04621 }
04622 }
04623
04624 }
04625
04626 }
04627
04628 } else
04629 {
04630 {
04631 IkReal j3array[1], cj3array[1], sj3array[1];
04632 bool j3valid[1]={false};
04633 _nj3 = 1;
04634 IkReal x1056=((IkReal(1.00000000000000))*(cj8));
04635 IkReal x1057=((IkReal(1.00000000000000))*(sj8));
04636 if( IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r10)*(x1057)))+(((IkReal(-1.00000000000000))*(r11)*(x1056))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r00)*(x1057)))+(((IkReal(-1.00000000000000))*(r01)*(x1056))))))) < IKFAST_ATAN2_MAGTHRESH )
04637 continue;
04638 j3array[0]=IKatan2(((gconst31)*(((((IkReal(-1.00000000000000))*(r10)*(x1057)))+(((IkReal(-1.00000000000000))*(r11)*(x1056)))))), ((gconst31)*(((((IkReal(-1.00000000000000))*(r00)*(x1057)))+(((IkReal(-1.00000000000000))*(r01)*(x1056)))))));
04639 sj3array[0]=IKsin(j3array[0]);
04640 cj3array[0]=IKcos(j3array[0]);
04641 if( j3array[0] > IKPI )
04642 {
04643 j3array[0]-=IK2PI;
04644 }
04645 else if( j3array[0] < -IKPI )
04646 { j3array[0]+=IK2PI;
04647 }
04648 j3valid[0] = true;
04649 for(int ij3 = 0; ij3 < 1; ++ij3)
04650 {
04651 if( !j3valid[ij3] )
04652 {
04653 continue;
04654 }
04655 _ij3[0] = ij3; _ij3[1] = -1;
04656 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04657 {
04658 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04659 {
04660 j3valid[iij3]=false; _ij3[1] = iij3; break;
04661 }
04662 }
04663 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04664 {
04665 IkReal evalcond[6];
04666 IkReal x1058=IKsin(j3);
04667 IkReal x1059=IKcos(j3);
04668 IkReal x1060=((cj7)*(sj8));
04669 IkReal x1061=((r00)*(sj8));
04670 IkReal x1062=((cj8)*(r11));
04671 IkReal x1063=((cj7)*(r02));
04672 IkReal x1064=((cj7)*(r12));
04673 IkReal x1065=((r12)*(sj7));
04674 IkReal x1066=((r02)*(sj7));
04675 IkReal x1067=((sj7)*(sj8));
04676 IkReal x1068=((r10)*(sj8));
04677 IkReal x1069=((cj8)*(r00));
04678 IkReal x1070=((r01)*(x1058));
04679 IkReal x1071=((cj8)*(r10)*(sj7));
04680 IkReal x1072=((IkReal(1.00000000000000))*(x1059));
04681 IkReal x1073=((IkReal(1.00000000000000))*(x1058));
04682 IkReal x1074=((cj7)*(cj8)*(r10));
04683 IkReal x1075=((x1059)*(x1069));
04684 evalcond[0]=((((cj8)*(x1070)))+(((IkReal(-1.00000000000000))*(x1062)*(x1072)))+(((IkReal(-1.00000000000000))*(x1068)*(x1072)))+(((x1058)*(x1061))));
04685 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1072)))+(((IkReal(-1.00000000000000))*(x1062)*(x1073)))+(((IkReal(-1.00000000000000))*(x1061)*(x1072)))+(((IkReal(-1.00000000000000))*(x1068)*(x1073)))+(((cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5))));
04686 evalcond[2]=((((x1060)*(x1070)))+(((IkReal(-1.00000000000000))*(r11)*(x1060)*(x1072)))+(((IkReal(-1.00000000000000))*(cj7)*(x1069)*(x1073)))+(((IkReal(-1.00000000000000))*(x1066)*(x1073)))+(((x1059)*(x1065)))+(((x1059)*(x1074))));
04687 evalcond[3]=((IkReal(-1.00000000000000))+(((x1067)*(x1070)))+(((IkReal(-1.00000000000000))*(sj7)*(x1069)*(x1073)))+(((IkReal(-1.00000000000000))*(r11)*(x1067)*(x1072)))+(((IkReal(-1.00000000000000))*(x1064)*(x1072)))+(((x1059)*(x1071)))+(((x1058)*(x1063))));
04688 evalcond[4]=((((x1058)*(x1074)))+(((cj7)*(x1075)))+(((cj5)*(sj4)))+(((IkReal(-1.00000000000000))*(r11)*(x1060)*(x1073)))+(((cj4)*(sj5)))+(((x1059)*(x1066)))+(((IkReal(-1.00000000000000))*(r01)*(x1060)*(x1072)))+(((x1058)*(x1065))));
04689 evalcond[5]=((((IkReal(-1.00000000000000))*(x1063)*(x1072)))+(((x1058)*(x1071)))+(((sj7)*(x1075)))+(((IkReal(-1.00000000000000))*(r11)*(x1067)*(x1073)))+(((IkReal(-1.00000000000000))*(r01)*(x1067)*(x1072)))+(((IkReal(-1.00000000000000))*(x1064)*(x1073))));
04690 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 )
04691 {
04692 continue;
04693 }
04694 }
04695
04696 {
04697 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04698 vinfos[0].jointtype = 1;
04699 vinfos[0].foffset = j3;
04700 vinfos[0].indices[0] = _ij3[0];
04701 vinfos[0].indices[1] = _ij3[1];
04702 vinfos[0].maxsolutions = _nj3;
04703 vinfos[1].jointtype = 1;
04704 vinfos[1].foffset = j4;
04705 vinfos[1].indices[0] = _ij4[0];
04706 vinfos[1].indices[1] = _ij4[1];
04707 vinfos[1].maxsolutions = _nj4;
04708 vinfos[2].jointtype = 1;
04709 vinfos[2].foffset = j5;
04710 vinfos[2].indices[0] = _ij5[0];
04711 vinfos[2].indices[1] = _ij5[1];
04712 vinfos[2].maxsolutions = _nj5;
04713 vinfos[3].jointtype = 1;
04714 vinfos[3].foffset = j6;
04715 vinfos[3].indices[0] = _ij6[0];
04716 vinfos[3].indices[1] = _ij6[1];
04717 vinfos[3].maxsolutions = _nj6;
04718 vinfos[4].jointtype = 1;
04719 vinfos[4].foffset = j7;
04720 vinfos[4].indices[0] = _ij7[0];
04721 vinfos[4].indices[1] = _ij7[1];
04722 vinfos[4].maxsolutions = _nj7;
04723 vinfos[5].jointtype = 1;
04724 vinfos[5].foffset = j8;
04725 vinfos[5].indices[0] = _ij8[0];
04726 vinfos[5].indices[1] = _ij8[1];
04727 vinfos[5].maxsolutions = _nj8;
04728 std::vector<int> vfree(0);
04729 solutions.AddSolution(vinfos,vfree);
04730 }
04731 }
04732 }
04733
04734 }
04735
04736 }
04737 }
04738 }
04739
04740 }
04741
04742 }
04743
04744 } else
04745 {
04746 IkReal x1076=((IkReal(1.00000000000000))*(sj7));
04747 IkReal x1077=((npy)*(sj8));
04748 IkReal x1078=((cj8)*(npx));
04749 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j6)), IkReal(6.28318530717959))));
04750 evalcond[1]=((IkReal(-0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
04751 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
04752 evalcond[3]=((IkReal(0.235000000000000))+(((cj7)*(x1077)))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(cj7)*(x1078)))+(((IkReal(-1.00000000000000))*(npz)*(x1076))));
04753 evalcond[4]=((IkReal(0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((sj7)*(x1077)))+(((IkReal(-1.00000000000000))*(x1076)*(x1078)))+(((cj7)*(npz)))+(((IkReal(-0.250000000000000))*(sj5))));
04754 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 )
04755 {
04756 {
04757 IkReal dummyeval[1];
04758 IkReal gconst32;
04759 IkReal x1079=(sj8)*(sj8);
04760 IkReal x1080=(cj8)*(cj8);
04761 IkReal x1081=((IkReal(2.00000000000000))*(cj8)*(sj8));
04762 gconst32=IKsign(((((x1079)*((r10)*(r10))))+(((x1079)*((r00)*(r00))))+(((r10)*(r11)*(x1081)))+(((x1080)*((r11)*(r11))))+(((x1080)*((r01)*(r01))))+(((r00)*(r01)*(x1081)))));
04763 IkReal x1082=(sj8)*(sj8);
04764 IkReal x1083=(cj8)*(cj8);
04765 IkReal x1084=((IkReal(2.00000000000000))*(cj8)*(sj8));
04766 dummyeval[0]=((((x1082)*((r10)*(r10))))+(((x1082)*((r00)*(r00))))+(((x1083)*((r11)*(r11))))+(((r10)*(r11)*(x1084)))+(((r00)*(r01)*(x1084)))+(((x1083)*((r01)*(r01)))));
04767 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04768 {
04769 {
04770 IkReal dummyeval[1];
04771 IkReal gconst34;
04772 gconst34=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
04773 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
04774 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04775 {
04776 {
04777 IkReal dummyeval[1];
04778 IkReal gconst33;
04779 IkReal x1085=(sj8)*(sj8);
04780 IkReal x1086=(cj8)*(cj8);
04781 IkReal x1087=((r00)*(r11));
04782 IkReal x1088=((r02)*(sj7));
04783 IkReal x1089=((cj7)*(x1086));
04784 IkReal x1090=((IkReal(1.00000000000000))*(r12)*(sj7));
04785 IkReal x1091=((IkReal(1.00000000000000))*(r01)*(r10));
04786 IkReal x1092=((cj7)*(x1085));
04787 gconst33=IKsign(((((x1087)*(x1092)))+(((IkReal(-1.00000000000000))*(x1091)*(x1092)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1090)))+(((r10)*(sj8)*(x1088)))+(((IkReal(-1.00000000000000))*(x1089)*(x1091)))+(((x1087)*(x1089)))+(((cj8)*(r11)*(x1088)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1090)))));
04788 IkReal x1093=(sj8)*(sj8);
04789 IkReal x1094=(cj8)*(cj8);
04790 IkReal x1095=((r00)*(r11));
04791 IkReal x1096=((r02)*(sj7));
04792 IkReal x1097=((cj7)*(x1094));
04793 IkReal x1098=((IkReal(1.00000000000000))*(r12)*(sj7));
04794 IkReal x1099=((IkReal(1.00000000000000))*(r01)*(r10));
04795 IkReal x1100=((cj7)*(x1093));
04796 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1098)))+(((cj8)*(r11)*(x1096)))+(((IkReal(-1.00000000000000))*(x1097)*(x1099)))+(((x1095)*(x1097)))+(((IkReal(-1.00000000000000))*(x1099)*(x1100)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1098)))+(((r10)*(sj8)*(x1096)))+(((x1095)*(x1100))));
04797 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04798 {
04799 continue;
04800
04801 } else
04802 {
04803 {
04804 IkReal j3array[1], cj3array[1], sj3array[1];
04805 bool j3valid[1]={false};
04806 _nj3 = 1;
04807 IkReal x1101=((cj7)*(cj8));
04808 IkReal x1102=((IkReal(1.00000000000000))*(cj7)*(sj8));
04809 if( IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1102)))+(((r12)*(sj7)))+(((r10)*(x1101))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1102)))+(((r00)*(x1101)))+(((r02)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH )
04810 continue;
04811 j3array[0]=IKatan2(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1102)))+(((r12)*(sj7)))+(((r10)*(x1101)))))), ((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1102)))+(((r00)*(x1101)))+(((r02)*(sj7)))))));
04812 sj3array[0]=IKsin(j3array[0]);
04813 cj3array[0]=IKcos(j3array[0]);
04814 if( j3array[0] > IKPI )
04815 {
04816 j3array[0]-=IK2PI;
04817 }
04818 else if( j3array[0] < -IKPI )
04819 { j3array[0]+=IK2PI;
04820 }
04821 j3valid[0] = true;
04822 for(int ij3 = 0; ij3 < 1; ++ij3)
04823 {
04824 if( !j3valid[ij3] )
04825 {
04826 continue;
04827 }
04828 _ij3[0] = ij3; _ij3[1] = -1;
04829 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
04830 {
04831 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
04832 {
04833 j3valid[iij3]=false; _ij3[1] = iij3; break;
04834 }
04835 }
04836 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
04837 {
04838 IkReal evalcond[4];
04839 IkReal x1103=IKsin(j3);
04840 IkReal x1104=IKcos(j3);
04841 IkReal x1105=((IkReal(1.00000000000000))*(sj8));
04842 IkReal x1106=((IkReal(1.00000000000000))*(cj8));
04843 IkReal x1107=((r01)*(sj8));
04844 IkReal x1108=((cj8)*(r10));
04845 IkReal x1109=((sj7)*(x1104));
04846 IkReal x1110=((cj7)*(x1103));
04847 IkReal x1111=((r00)*(x1103));
04848 IkReal x1112=((sj7)*(x1103));
04849 IkReal x1113=((cj7)*(x1104));
04850 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x1104)*(x1106)))+(((cj8)*(r01)*(x1103)))+(((IkReal(-1.00000000000000))*(r10)*(x1104)*(x1105)))+(((sj8)*(x1111))));
04851 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x1103)*(x1106)))+(((IkReal(-1.00000000000000))*(r10)*(x1103)*(x1105)))+(((IkReal(-1.00000000000000))*(r01)*(x1104)*(x1106)))+(((IkReal(-1.00000000000000))*(r00)*(x1104)*(x1105))));
04852 evalcond[2]=((((x1108)*(x1113)))+(((IkReal(-1.00000000000000))*(r02)*(x1112)))+(((IkReal(-1.00000000000000))*(r11)*(x1105)*(x1113)))+(((IkReal(-1.00000000000000))*(r00)*(x1106)*(x1110)))+(((r12)*(x1109)))+(((x1107)*(x1110))));
04853 evalcond[3]=((((x1108)*(x1109)))+(((IkReal(-1.00000000000000))*(r11)*(x1105)*(x1109)))+(((IkReal(-1.00000000000000))*(r12)*(x1113)))+(((IkReal(-1.00000000000000))*(sj7)*(x1106)*(x1111)))+(((x1107)*(x1112)))+(((r02)*(x1110))));
04854 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04855 {
04856 continue;
04857 }
04858 }
04859
04860 {
04861 IkReal dummyeval[1];
04862 IkReal gconst35;
04863 gconst35=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
04864 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
04865 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04866 {
04867 {
04868 IkReal dummyeval[1];
04869 IkReal gconst36;
04870 gconst36=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
04871 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
04872 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04873 {
04874 continue;
04875
04876 } else
04877 {
04878 {
04879 IkReal j4array[1], cj4array[1], sj4array[1];
04880 bool j4valid[1]={false};
04881 _nj4 = 1;
04882 IkReal x1114=((cj3)*(cj5));
04883 IkReal x1115=((r02)*(sj7));
04884 IkReal x1116=((cj5)*(sj7));
04885 IkReal x1117=((cj3)*(sj5));
04886 IkReal x1118=((cj5)*(sj3));
04887 IkReal x1119=((cj7)*(cj8));
04888 IkReal x1120=((IkReal(1.00000000000000))*(sj5));
04889 IkReal x1121=((r12)*(sj3));
04890 IkReal x1122=((sj3)*(sj5));
04891 IkReal x1123=((IkReal(1.00000000000000))*(cj7)*(sj8));
04892 if( IKabs(((gconst36)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((r00)*(x1114)*(x1119)))+(((x1114)*(x1115)))+(((r10)*(x1118)*(x1119)))+(((IkReal(-1.00000000000000))*(r11)*(x1118)*(x1123)))+(((IkReal(-1.00000000000000))*(r20)*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1120)))+(((x1116)*(x1121)))+(((IkReal(-1.00000000000000))*(r01)*(x1114)*(x1123))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1123)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x1120)))+(((sj5)*(sj7)*(x1121)))+(((r22)*(x1116)))+(((r10)*(x1119)*(x1122)))+(((cj5)*(r20)*(x1119)))+(((x1115)*(x1117)))+(((IkReal(-1.00000000000000))*(r01)*(x1117)*(x1123)))+(((r00)*(x1117)*(x1119))))))) < IKFAST_ATAN2_MAGTHRESH )
04893 continue;
04894 j4array[0]=IKatan2(((gconst36)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((r00)*(x1114)*(x1119)))+(((x1114)*(x1115)))+(((r10)*(x1118)*(x1119)))+(((IkReal(-1.00000000000000))*(r11)*(x1118)*(x1123)))+(((IkReal(-1.00000000000000))*(r20)*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1120)))+(((x1116)*(x1121)))+(((IkReal(-1.00000000000000))*(r01)*(x1114)*(x1123)))))), ((gconst36)*(((((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1123)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x1120)))+(((sj5)*(sj7)*(x1121)))+(((r22)*(x1116)))+(((r10)*(x1119)*(x1122)))+(((cj5)*(r20)*(x1119)))+(((x1115)*(x1117)))+(((IkReal(-1.00000000000000))*(r01)*(x1117)*(x1123)))+(((r00)*(x1117)*(x1119)))))));
04895 sj4array[0]=IKsin(j4array[0]);
04896 cj4array[0]=IKcos(j4array[0]);
04897 if( j4array[0] > IKPI )
04898 {
04899 j4array[0]-=IK2PI;
04900 }
04901 else if( j4array[0] < -IKPI )
04902 { j4array[0]+=IK2PI;
04903 }
04904 j4valid[0] = true;
04905 for(int ij4 = 0; ij4 < 1; ++ij4)
04906 {
04907 if( !j4valid[ij4] )
04908 {
04909 continue;
04910 }
04911 _ij4[0] = ij4; _ij4[1] = -1;
04912 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
04913 {
04914 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
04915 {
04916 j4valid[iij4]=false; _ij4[1] = iij4; break;
04917 }
04918 }
04919 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
04920 {
04921 IkReal evalcond[4];
04922 IkReal x1124=IKcos(j4);
04923 IkReal x1125=IKsin(j4);
04924 IkReal x1126=((IkReal(1.00000000000000))*(cj7));
04925 IkReal x1127=((cj8)*(r20));
04926 IkReal x1128=((cj3)*(r02));
04927 IkReal x1129=((IkReal(1.00000000000000))*(sj7));
04928 IkReal x1130=((sj3)*(sj7));
04929 IkReal x1131=((r21)*(sj8));
04930 IkReal x1132=((cj8)*(r10));
04931 IkReal x1133=((sj5)*(x1124));
04932 IkReal x1134=((cj5)*(x1125));
04933 IkReal x1135=((r11)*(sj3)*(sj8));
04934 IkReal x1136=((cj3)*(cj8)*(r00));
04935 IkReal x1137=((cj5)*(x1124));
04936 IkReal x1138=((cj3)*(r01)*(sj8));
04937 IkReal x1139=((sj5)*(x1125));
04938 IkReal x1140=((x1133)+(x1134));
04939 evalcond[0]=((x1139)+(((cj7)*(x1131)))+(((IkReal(-1.00000000000000))*(x1126)*(x1127)))+(((IkReal(-1.00000000000000))*(r22)*(x1129)))+(((IkReal(-1.00000000000000))*(x1137))));
04940 evalcond[1]=((x1140)+(((IkReal(-1.00000000000000))*(x1127)*(x1129)))+(((sj7)*(x1131)))+(((cj7)*(r22))));
04941 evalcond[2]=((x1140)+(((cj7)*(x1136)))+(((IkReal(-1.00000000000000))*(x1126)*(x1135)))+(((IkReal(-1.00000000000000))*(x1126)*(x1138)))+(((r12)*(x1130)))+(((sj7)*(x1128)))+(((cj7)*(sj3)*(x1132))));
04942 evalcond[3]=((x1137)+(((x1130)*(x1132)))+(((IkReal(-1.00000000000000))*(x1126)*(x1128)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1126)))+(((IkReal(-1.00000000000000))*(x1139)))+(((sj7)*(x1136)))+(((IkReal(-1.00000000000000))*(x1129)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1138))));
04943 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04944 {
04945 continue;
04946 }
04947 }
04948
04949 {
04950 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04951 vinfos[0].jointtype = 1;
04952 vinfos[0].foffset = j3;
04953 vinfos[0].indices[0] = _ij3[0];
04954 vinfos[0].indices[1] = _ij3[1];
04955 vinfos[0].maxsolutions = _nj3;
04956 vinfos[1].jointtype = 1;
04957 vinfos[1].foffset = j4;
04958 vinfos[1].indices[0] = _ij4[0];
04959 vinfos[1].indices[1] = _ij4[1];
04960 vinfos[1].maxsolutions = _nj4;
04961 vinfos[2].jointtype = 1;
04962 vinfos[2].foffset = j5;
04963 vinfos[2].indices[0] = _ij5[0];
04964 vinfos[2].indices[1] = _ij5[1];
04965 vinfos[2].maxsolutions = _nj5;
04966 vinfos[3].jointtype = 1;
04967 vinfos[3].foffset = j6;
04968 vinfos[3].indices[0] = _ij6[0];
04969 vinfos[3].indices[1] = _ij6[1];
04970 vinfos[3].maxsolutions = _nj6;
04971 vinfos[4].jointtype = 1;
04972 vinfos[4].foffset = j7;
04973 vinfos[4].indices[0] = _ij7[0];
04974 vinfos[4].indices[1] = _ij7[1];
04975 vinfos[4].maxsolutions = _nj7;
04976 vinfos[5].jointtype = 1;
04977 vinfos[5].foffset = j8;
04978 vinfos[5].indices[0] = _ij8[0];
04979 vinfos[5].indices[1] = _ij8[1];
04980 vinfos[5].maxsolutions = _nj8;
04981 std::vector<int> vfree(0);
04982 solutions.AddSolution(vinfos,vfree);
04983 }
04984 }
04985 }
04986
04987 }
04988
04989 }
04990
04991 } else
04992 {
04993 {
04994 IkReal j4array[1], cj4array[1], sj4array[1];
04995 bool j4valid[1]={false};
04996 _nj4 = 1;
04997 IkReal x1141=((sj5)*(sj7));
04998 IkReal x1142=((r21)*(sj8));
04999 IkReal x1143=((cj5)*(sj7));
05000 IkReal x1144=((cj8)*(r20));
05001 IkReal x1145=((cj5)*(cj7));
05002 IkReal x1146=((cj7)*(sj5));
05003 if( IKabs(((gconst35)*(((((IkReal(-1.00000000000000))*(x1144)*(x1146)))+(((IkReal(-1.00000000000000))*(r22)*(x1141)))+(((r22)*(x1145)))+(((x1142)*(x1146)))+(((x1142)*(x1143)))+(((IkReal(-1.00000000000000))*(x1143)*(x1144))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((r22)*(x1143)))+(((r22)*(x1146)))+(((x1141)*(x1142)))+(((IkReal(-1.00000000000000))*(x1141)*(x1144)))+(((IkReal(-1.00000000000000))*(x1142)*(x1145)))+(((x1144)*(x1145))))))) < IKFAST_ATAN2_MAGTHRESH )
05004 continue;
05005 j4array[0]=IKatan2(((gconst35)*(((((IkReal(-1.00000000000000))*(x1144)*(x1146)))+(((IkReal(-1.00000000000000))*(r22)*(x1141)))+(((r22)*(x1145)))+(((x1142)*(x1146)))+(((x1142)*(x1143)))+(((IkReal(-1.00000000000000))*(x1143)*(x1144)))))), ((gconst35)*(((((r22)*(x1143)))+(((r22)*(x1146)))+(((x1141)*(x1142)))+(((IkReal(-1.00000000000000))*(x1141)*(x1144)))+(((IkReal(-1.00000000000000))*(x1142)*(x1145)))+(((x1144)*(x1145)))))));
05006 sj4array[0]=IKsin(j4array[0]);
05007 cj4array[0]=IKcos(j4array[0]);
05008 if( j4array[0] > IKPI )
05009 {
05010 j4array[0]-=IK2PI;
05011 }
05012 else if( j4array[0] < -IKPI )
05013 { j4array[0]+=IK2PI;
05014 }
05015 j4valid[0] = true;
05016 for(int ij4 = 0; ij4 < 1; ++ij4)
05017 {
05018 if( !j4valid[ij4] )
05019 {
05020 continue;
05021 }
05022 _ij4[0] = ij4; _ij4[1] = -1;
05023 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05024 {
05025 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05026 {
05027 j4valid[iij4]=false; _ij4[1] = iij4; break;
05028 }
05029 }
05030 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05031 {
05032 IkReal evalcond[4];
05033 IkReal x1147=IKcos(j4);
05034 IkReal x1148=IKsin(j4);
05035 IkReal x1149=((IkReal(1.00000000000000))*(cj7));
05036 IkReal x1150=((cj8)*(r20));
05037 IkReal x1151=((cj3)*(r02));
05038 IkReal x1152=((IkReal(1.00000000000000))*(sj7));
05039 IkReal x1153=((sj3)*(sj7));
05040 IkReal x1154=((r21)*(sj8));
05041 IkReal x1155=((cj8)*(r10));
05042 IkReal x1156=((sj5)*(x1147));
05043 IkReal x1157=((cj5)*(x1148));
05044 IkReal x1158=((r11)*(sj3)*(sj8));
05045 IkReal x1159=((cj3)*(cj8)*(r00));
05046 IkReal x1160=((cj5)*(x1147));
05047 IkReal x1161=((cj3)*(r01)*(sj8));
05048 IkReal x1162=((sj5)*(x1148));
05049 IkReal x1163=((x1157)+(x1156));
05050 evalcond[0]=((x1162)+(((IkReal(-1.00000000000000))*(x1149)*(x1150)))+(((IkReal(-1.00000000000000))*(r22)*(x1152)))+(((cj7)*(x1154)))+(((IkReal(-1.00000000000000))*(x1160))));
05051 evalcond[1]=((x1163)+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x1150)*(x1152)))+(((sj7)*(x1154))));
05052 evalcond[2]=((x1163)+(((IkReal(-1.00000000000000))*(x1149)*(x1158)))+(((r12)*(x1153)))+(((IkReal(-1.00000000000000))*(x1149)*(x1161)))+(((cj7)*(sj3)*(x1155)))+(((cj7)*(x1159)))+(((sj7)*(x1151))));
05053 evalcond[3]=((x1160)+(((IkReal(-1.00000000000000))*(x1149)*(x1151)))+(((IkReal(-1.00000000000000))*(x1152)*(x1161)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1149)))+(((x1153)*(x1155)))+(((IkReal(-1.00000000000000))*(x1152)*(x1158)))+(((IkReal(-1.00000000000000))*(x1162)))+(((sj7)*(x1159))));
05054 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05055 {
05056 continue;
05057 }
05058 }
05059
05060 {
05061 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05062 vinfos[0].jointtype = 1;
05063 vinfos[0].foffset = j3;
05064 vinfos[0].indices[0] = _ij3[0];
05065 vinfos[0].indices[1] = _ij3[1];
05066 vinfos[0].maxsolutions = _nj3;
05067 vinfos[1].jointtype = 1;
05068 vinfos[1].foffset = j4;
05069 vinfos[1].indices[0] = _ij4[0];
05070 vinfos[1].indices[1] = _ij4[1];
05071 vinfos[1].maxsolutions = _nj4;
05072 vinfos[2].jointtype = 1;
05073 vinfos[2].foffset = j5;
05074 vinfos[2].indices[0] = _ij5[0];
05075 vinfos[2].indices[1] = _ij5[1];
05076 vinfos[2].maxsolutions = _nj5;
05077 vinfos[3].jointtype = 1;
05078 vinfos[3].foffset = j6;
05079 vinfos[3].indices[0] = _ij6[0];
05080 vinfos[3].indices[1] = _ij6[1];
05081 vinfos[3].maxsolutions = _nj6;
05082 vinfos[4].jointtype = 1;
05083 vinfos[4].foffset = j7;
05084 vinfos[4].indices[0] = _ij7[0];
05085 vinfos[4].indices[1] = _ij7[1];
05086 vinfos[4].maxsolutions = _nj7;
05087 vinfos[5].jointtype = 1;
05088 vinfos[5].foffset = j8;
05089 vinfos[5].indices[0] = _ij8[0];
05090 vinfos[5].indices[1] = _ij8[1];
05091 vinfos[5].maxsolutions = _nj8;
05092 std::vector<int> vfree(0);
05093 solutions.AddSolution(vinfos,vfree);
05094 }
05095 }
05096 }
05097
05098 }
05099
05100 }
05101 }
05102 }
05103
05104 }
05105
05106 }
05107
05108 } else
05109 {
05110 {
05111 IkReal j4array[1], cj4array[1], sj4array[1];
05112 bool j4valid[1]={false};
05113 _nj4 = 1;
05114 IkReal x1164=((sj5)*(sj7));
05115 IkReal x1165=((r21)*(sj8));
05116 IkReal x1166=((cj5)*(sj7));
05117 IkReal x1167=((cj7)*(sj5));
05118 IkReal x1168=((cj5)*(cj7));
05119 IkReal x1169=((IkReal(1.00000000000000))*(cj8)*(r20));
05120 if( IKabs(((gconst34)*(((((IkReal(-1.00000000000000))*(x1166)*(x1169)))+(((x1165)*(x1166)))+(((x1165)*(x1167)))+(((IkReal(-1.00000000000000))*(x1167)*(x1169)))+(((IkReal(-1.00000000000000))*(r22)*(x1164)))+(((r22)*(x1168))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst34)*(((((x1164)*(x1165)))+(((IkReal(-1.00000000000000))*(x1165)*(x1168)))+(((cj8)*(r20)*(x1168)))+(((r22)*(x1166)))+(((r22)*(x1167)))+(((IkReal(-1.00000000000000))*(x1164)*(x1169))))))) < IKFAST_ATAN2_MAGTHRESH )
05121 continue;
05122 j4array[0]=IKatan2(((gconst34)*(((((IkReal(-1.00000000000000))*(x1166)*(x1169)))+(((x1165)*(x1166)))+(((x1165)*(x1167)))+(((IkReal(-1.00000000000000))*(x1167)*(x1169)))+(((IkReal(-1.00000000000000))*(r22)*(x1164)))+(((r22)*(x1168)))))), ((gconst34)*(((((x1164)*(x1165)))+(((IkReal(-1.00000000000000))*(x1165)*(x1168)))+(((cj8)*(r20)*(x1168)))+(((r22)*(x1166)))+(((r22)*(x1167)))+(((IkReal(-1.00000000000000))*(x1164)*(x1169)))))));
05123 sj4array[0]=IKsin(j4array[0]);
05124 cj4array[0]=IKcos(j4array[0]);
05125 if( j4array[0] > IKPI )
05126 {
05127 j4array[0]-=IK2PI;
05128 }
05129 else if( j4array[0] < -IKPI )
05130 { j4array[0]+=IK2PI;
05131 }
05132 j4valid[0] = true;
05133 for(int ij4 = 0; ij4 < 1; ++ij4)
05134 {
05135 if( !j4valid[ij4] )
05136 {
05137 continue;
05138 }
05139 _ij4[0] = ij4; _ij4[1] = -1;
05140 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05141 {
05142 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05143 {
05144 j4valid[iij4]=false; _ij4[1] = iij4; break;
05145 }
05146 }
05147 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05148 {
05149 IkReal evalcond[2];
05150 IkReal x1170=IKcos(j4);
05151 IkReal x1171=IKsin(j4);
05152 IkReal x1172=((r21)*(sj8));
05153 IkReal x1173=((IkReal(1.00000000000000))*(cj8)*(r20));
05154 evalcond[0]=((((IkReal(-1.00000000000000))*(cj7)*(x1173)))+(((cj7)*(x1172)))+(((IkReal(-1.00000000000000))*(cj5)*(x1170)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((sj5)*(x1171))));
05155 evalcond[1]=((((cj5)*(x1171)))+(((sj7)*(x1172)))+(((sj5)*(x1170)))+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(sj7)*(x1173))));
05156 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
05157 {
05158 continue;
05159 }
05160 }
05161
05162 {
05163 IkReal dummyeval[1];
05164 IkReal gconst37;
05165 IkReal x1174=(sj8)*(sj8);
05166 IkReal x1175=(cj8)*(cj8);
05167 IkReal x1176=((IkReal(2.00000000000000))*(cj8)*(sj8));
05168 gconst37=IKsign(((((x1175)*((r11)*(r11))))+(((r10)*(r11)*(x1176)))+(((x1174)*((r00)*(r00))))+(((r00)*(r01)*(x1176)))+(((x1174)*((r10)*(r10))))+(((x1175)*((r01)*(r01))))));
05169 IkReal x1177=(sj8)*(sj8);
05170 IkReal x1178=(cj8)*(cj8);
05171 IkReal x1179=((IkReal(2.00000000000000))*(cj8)*(sj8));
05172 dummyeval[0]=((((x1178)*((r11)*(r11))))+(((r10)*(r11)*(x1179)))+(((r00)*(r01)*(x1179)))+(((x1177)*((r10)*(r10))))+(((x1178)*((r01)*(r01))))+(((x1177)*((r00)*(r00)))));
05173 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05174 {
05175 {
05176 IkReal dummyeval[1];
05177 IkReal gconst38;
05178 IkReal x1180=(sj8)*(sj8);
05179 IkReal x1181=(cj8)*(cj8);
05180 IkReal x1182=((r00)*(r11));
05181 IkReal x1183=((r02)*(sj7));
05182 IkReal x1184=((cj7)*(x1181));
05183 IkReal x1185=((IkReal(1.00000000000000))*(r12)*(sj7));
05184 IkReal x1186=((IkReal(1.00000000000000))*(r01)*(r10));
05185 IkReal x1187=((cj7)*(x1180));
05186 gconst38=IKsign(((((r10)*(sj8)*(x1183)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1185)))+(((cj8)*(r11)*(x1183)))+(((x1182)*(x1184)))+(((x1182)*(x1187)))+(((IkReal(-1.00000000000000))*(x1186)*(x1187)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1185)))+(((IkReal(-1.00000000000000))*(x1184)*(x1186)))));
05187 IkReal x1188=(sj8)*(sj8);
05188 IkReal x1189=(cj8)*(cj8);
05189 IkReal x1190=((r00)*(r11));
05190 IkReal x1191=((r02)*(sj7));
05191 IkReal x1192=((cj7)*(x1189));
05192 IkReal x1193=((IkReal(1.00000000000000))*(r12)*(sj7));
05193 IkReal x1194=((IkReal(1.00000000000000))*(r01)*(r10));
05194 IkReal x1195=((cj7)*(x1188));
05195 dummyeval[0]=((((r10)*(sj8)*(x1191)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1193)))+(((cj8)*(r11)*(x1191)))+(((IkReal(-1.00000000000000))*(x1194)*(x1195)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1193)))+(((IkReal(-1.00000000000000))*(x1192)*(x1194)))+(((x1190)*(x1195)))+(((x1190)*(x1192))));
05196 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05197 {
05198 continue;
05199
05200 } else
05201 {
05202 {
05203 IkReal j3array[1], cj3array[1], sj3array[1];
05204 bool j3valid[1]={false};
05205 _nj3 = 1;
05206 IkReal x1196=((cj7)*(cj8));
05207 IkReal x1197=((IkReal(1.00000000000000))*(cj7)*(sj8));
05208 if( IKabs(((gconst38)*(((((r10)*(x1196)))+(((r12)*(sj7)))+(((IkReal(-1.00000000000000))*(r11)*(x1197))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst38)*(((((r00)*(x1196)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x1197))))))) < IKFAST_ATAN2_MAGTHRESH )
05209 continue;
05210 j3array[0]=IKatan2(((gconst38)*(((((r10)*(x1196)))+(((r12)*(sj7)))+(((IkReal(-1.00000000000000))*(r11)*(x1197)))))), ((gconst38)*(((((r00)*(x1196)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x1197)))))));
05211 sj3array[0]=IKsin(j3array[0]);
05212 cj3array[0]=IKcos(j3array[0]);
05213 if( j3array[0] > IKPI )
05214 {
05215 j3array[0]-=IK2PI;
05216 }
05217 else if( j3array[0] < -IKPI )
05218 { j3array[0]+=IK2PI;
05219 }
05220 j3valid[0] = true;
05221 for(int ij3 = 0; ij3 < 1; ++ij3)
05222 {
05223 if( !j3valid[ij3] )
05224 {
05225 continue;
05226 }
05227 _ij3[0] = ij3; _ij3[1] = -1;
05228 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05229 {
05230 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05231 {
05232 j3valid[iij3]=false; _ij3[1] = iij3; break;
05233 }
05234 }
05235 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05236 {
05237 IkReal evalcond[6];
05238 IkReal x1198=IKsin(j3);
05239 IkReal x1199=IKcos(j3);
05240 IkReal x1200=((r01)*(sj8));
05241 IkReal x1201=((cj8)*(r00));
05242 IkReal x1202=((r00)*(sj8));
05243 IkReal x1203=((cj8)*(r11));
05244 IkReal x1204=((r11)*(sj8));
05245 IkReal x1205=((r10)*(sj8));
05246 IkReal x1206=((cj8)*(r10));
05247 IkReal x1207=((cj7)*(x1198));
05248 IkReal x1208=((sj7)*(x1199));
05249 IkReal x1209=((IkReal(1.00000000000000))*(x1198));
05250 IkReal x1210=((IkReal(1.00000000000000))*(x1199));
05251 IkReal x1211=((cj8)*(x1198));
05252 IkReal x1212=((IkReal(1.00000000000000))*(x1204));
05253 IkReal x1213=((sj7)*(x1198));
05254 IkReal x1214=((cj7)*(x1199));
05255 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1203)*(x1210)))+(((r01)*(x1211)))+(((IkReal(-1.00000000000000))*(x1205)*(x1210)))+(((x1198)*(x1202))));
05256 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1210)))+(((IkReal(-1.00000000000000))*(x1202)*(x1210)))+(((IkReal(-1.00000000000000))*(x1205)*(x1209)))+(((IkReal(-1.00000000000000))*(x1203)*(x1209))));
05257 evalcond[2]=((((IkReal(-1.00000000000000))*(cj7)*(x1204)*(x1210)))+(((r12)*(x1208)))+(((x1206)*(x1214)))+(((x1200)*(x1207)))+(((IkReal(-1.00000000000000))*(x1201)*(x1207)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1209))));
05258 evalcond[3]=((((IkReal(-1.00000000000000))*(x1208)*(x1212)))+(((x1200)*(x1213)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1210)))+(((x1206)*(x1208)))+(((r02)*(x1207)))+(((IkReal(-1.00000000000000))*(sj7)*(x1201)*(x1209))));
05259 evalcond[4]=((((r12)*(x1213)))+(((IkReal(-1.00000000000000))*(x1207)*(x1212)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(cj7)*(x1200)*(x1210)))+(((x1201)*(x1214)))+(((x1206)*(x1207)))+(((r02)*(x1208))));
05260 evalcond[5]=((((IkReal(-1.00000000000000))*(x1200)*(x1208)))+(((x1206)*(x1213)))+(((cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(sj7)*(x1204)*(x1209)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1210)))+(((x1201)*(x1208)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(r12)*(x1207))));
05261 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 )
05262 {
05263 continue;
05264 }
05265 }
05266
05267 {
05268 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05269 vinfos[0].jointtype = 1;
05270 vinfos[0].foffset = j3;
05271 vinfos[0].indices[0] = _ij3[0];
05272 vinfos[0].indices[1] = _ij3[1];
05273 vinfos[0].maxsolutions = _nj3;
05274 vinfos[1].jointtype = 1;
05275 vinfos[1].foffset = j4;
05276 vinfos[1].indices[0] = _ij4[0];
05277 vinfos[1].indices[1] = _ij4[1];
05278 vinfos[1].maxsolutions = _nj4;
05279 vinfos[2].jointtype = 1;
05280 vinfos[2].foffset = j5;
05281 vinfos[2].indices[0] = _ij5[0];
05282 vinfos[2].indices[1] = _ij5[1];
05283 vinfos[2].maxsolutions = _nj5;
05284 vinfos[3].jointtype = 1;
05285 vinfos[3].foffset = j6;
05286 vinfos[3].indices[0] = _ij6[0];
05287 vinfos[3].indices[1] = _ij6[1];
05288 vinfos[3].maxsolutions = _nj6;
05289 vinfos[4].jointtype = 1;
05290 vinfos[4].foffset = j7;
05291 vinfos[4].indices[0] = _ij7[0];
05292 vinfos[4].indices[1] = _ij7[1];
05293 vinfos[4].maxsolutions = _nj7;
05294 vinfos[5].jointtype = 1;
05295 vinfos[5].foffset = j8;
05296 vinfos[5].indices[0] = _ij8[0];
05297 vinfos[5].indices[1] = _ij8[1];
05298 vinfos[5].maxsolutions = _nj8;
05299 std::vector<int> vfree(0);
05300 solutions.AddSolution(vinfos,vfree);
05301 }
05302 }
05303 }
05304
05305 }
05306
05307 }
05308
05309 } else
05310 {
05311 {
05312 IkReal j3array[1], cj3array[1], sj3array[1];
05313 bool j3valid[1]={false};
05314 _nj3 = 1;
05315 if( IKabs(((gconst37)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst37)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
05316 continue;
05317 j3array[0]=IKatan2(((gconst37)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst37)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
05318 sj3array[0]=IKsin(j3array[0]);
05319 cj3array[0]=IKcos(j3array[0]);
05320 if( j3array[0] > IKPI )
05321 {
05322 j3array[0]-=IK2PI;
05323 }
05324 else if( j3array[0] < -IKPI )
05325 { j3array[0]+=IK2PI;
05326 }
05327 j3valid[0] = true;
05328 for(int ij3 = 0; ij3 < 1; ++ij3)
05329 {
05330 if( !j3valid[ij3] )
05331 {
05332 continue;
05333 }
05334 _ij3[0] = ij3; _ij3[1] = -1;
05335 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05336 {
05337 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05338 {
05339 j3valid[iij3]=false; _ij3[1] = iij3; break;
05340 }
05341 }
05342 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05343 {
05344 IkReal evalcond[6];
05345 IkReal x1215=IKsin(j3);
05346 IkReal x1216=IKcos(j3);
05347 IkReal x1217=((r01)*(sj8));
05348 IkReal x1218=((cj8)*(r00));
05349 IkReal x1219=((r00)*(sj8));
05350 IkReal x1220=((cj8)*(r11));
05351 IkReal x1221=((r11)*(sj8));
05352 IkReal x1222=((r10)*(sj8));
05353 IkReal x1223=((cj8)*(r10));
05354 IkReal x1224=((cj7)*(x1215));
05355 IkReal x1225=((sj7)*(x1216));
05356 IkReal x1226=((IkReal(1.00000000000000))*(x1215));
05357 IkReal x1227=((IkReal(1.00000000000000))*(x1216));
05358 IkReal x1228=((cj8)*(x1215));
05359 IkReal x1229=((IkReal(1.00000000000000))*(x1221));
05360 IkReal x1230=((sj7)*(x1215));
05361 IkReal x1231=((cj7)*(x1216));
05362 evalcond[0]=((IkReal(1.00000000000000))+(((r01)*(x1228)))+(((IkReal(-1.00000000000000))*(x1220)*(x1227)))+(((IkReal(-1.00000000000000))*(x1222)*(x1227)))+(((x1215)*(x1219))));
05363 evalcond[1]=((((IkReal(-1.00000000000000))*(x1220)*(x1226)))+(((IkReal(-1.00000000000000))*(x1222)*(x1226)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1227)))+(((IkReal(-1.00000000000000))*(x1219)*(x1227))));
05364 evalcond[2]=((((IkReal(-1.00000000000000))*(x1218)*(x1224)))+(((x1217)*(x1224)))+(((x1223)*(x1231)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1226)))+(((IkReal(-1.00000000000000))*(cj7)*(x1221)*(x1227)))+(((r12)*(x1225))));
05365 evalcond[3]=((((x1217)*(x1230)))+(((IkReal(-1.00000000000000))*(x1225)*(x1229)))+(((x1223)*(x1225)))+(((r02)*(x1224)))+(((IkReal(-1.00000000000000))*(sj7)*(x1218)*(x1226)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1227))));
05366 evalcond[4]=((((IkReal(-1.00000000000000))*(cj7)*(x1217)*(x1227)))+(((IkReal(-1.00000000000000))*(x1224)*(x1229)))+(((x1223)*(x1224)))+(((cj5)*(sj4)))+(((r02)*(x1225)))+(((cj4)*(sj5)))+(((x1218)*(x1231)))+(((r12)*(x1230))));
05367 evalcond[5]=((((IkReal(-1.00000000000000))*(sj7)*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(x1217)*(x1225)))+(((x1223)*(x1230)))+(((IkReal(-1.00000000000000))*(r12)*(x1224)))+(((cj4)*(cj5)))+(((x1218)*(x1225)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1227)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5))));
05368 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 )
05369 {
05370 continue;
05371 }
05372 }
05373
05374 {
05375 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05376 vinfos[0].jointtype = 1;
05377 vinfos[0].foffset = j3;
05378 vinfos[0].indices[0] = _ij3[0];
05379 vinfos[0].indices[1] = _ij3[1];
05380 vinfos[0].maxsolutions = _nj3;
05381 vinfos[1].jointtype = 1;
05382 vinfos[1].foffset = j4;
05383 vinfos[1].indices[0] = _ij4[0];
05384 vinfos[1].indices[1] = _ij4[1];
05385 vinfos[1].maxsolutions = _nj4;
05386 vinfos[2].jointtype = 1;
05387 vinfos[2].foffset = j5;
05388 vinfos[2].indices[0] = _ij5[0];
05389 vinfos[2].indices[1] = _ij5[1];
05390 vinfos[2].maxsolutions = _nj5;
05391 vinfos[3].jointtype = 1;
05392 vinfos[3].foffset = j6;
05393 vinfos[3].indices[0] = _ij6[0];
05394 vinfos[3].indices[1] = _ij6[1];
05395 vinfos[3].maxsolutions = _nj6;
05396 vinfos[4].jointtype = 1;
05397 vinfos[4].foffset = j7;
05398 vinfos[4].indices[0] = _ij7[0];
05399 vinfos[4].indices[1] = _ij7[1];
05400 vinfos[4].maxsolutions = _nj7;
05401 vinfos[5].jointtype = 1;
05402 vinfos[5].foffset = j8;
05403 vinfos[5].indices[0] = _ij8[0];
05404 vinfos[5].indices[1] = _ij8[1];
05405 vinfos[5].maxsolutions = _nj8;
05406 std::vector<int> vfree(0);
05407 solutions.AddSolution(vinfos,vfree);
05408 }
05409 }
05410 }
05411
05412 }
05413
05414 }
05415 }
05416 }
05417
05418 }
05419
05420 }
05421
05422 } else
05423 {
05424 {
05425 IkReal j3array[1], cj3array[1], sj3array[1];
05426 bool j3valid[1]={false};
05427 _nj3 = 1;
05428 if( IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst32)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
05429 continue;
05430 j3array[0]=IKatan2(((gconst32)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst32)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
05431 sj3array[0]=IKsin(j3array[0]);
05432 cj3array[0]=IKcos(j3array[0]);
05433 if( j3array[0] > IKPI )
05434 {
05435 j3array[0]-=IK2PI;
05436 }
05437 else if( j3array[0] < -IKPI )
05438 { j3array[0]+=IK2PI;
05439 }
05440 j3valid[0] = true;
05441 for(int ij3 = 0; ij3 < 1; ++ij3)
05442 {
05443 if( !j3valid[ij3] )
05444 {
05445 continue;
05446 }
05447 _ij3[0] = ij3; _ij3[1] = -1;
05448 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05449 {
05450 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05451 {
05452 j3valid[iij3]=false; _ij3[1] = iij3; break;
05453 }
05454 }
05455 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05456 {
05457 IkReal evalcond[4];
05458 IkReal x1232=IKsin(j3);
05459 IkReal x1233=IKcos(j3);
05460 IkReal x1234=((IkReal(1.00000000000000))*(sj8));
05461 IkReal x1235=((IkReal(1.00000000000000))*(cj8));
05462 IkReal x1236=((r01)*(sj8));
05463 IkReal x1237=((cj8)*(r10));
05464 IkReal x1238=((sj7)*(x1233));
05465 IkReal x1239=((cj7)*(x1232));
05466 IkReal x1240=((r00)*(x1232));
05467 IkReal x1241=((sj7)*(x1232));
05468 IkReal x1242=((cj7)*(x1233));
05469 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x1233)*(x1235)))+(((cj8)*(r01)*(x1232)))+(((IkReal(-1.00000000000000))*(r10)*(x1233)*(x1234)))+(((sj8)*(x1240))));
05470 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x1232)*(x1235)))+(((IkReal(-1.00000000000000))*(r00)*(x1233)*(x1234)))+(((IkReal(-1.00000000000000))*(r01)*(x1233)*(x1235)))+(((IkReal(-1.00000000000000))*(r10)*(x1232)*(x1234))));
05471 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1235)*(x1239)))+(((IkReal(-1.00000000000000))*(r11)*(x1234)*(x1242)))+(((IkReal(-1.00000000000000))*(r02)*(x1241)))+(((x1236)*(x1239)))+(((x1237)*(x1242)))+(((r12)*(x1238))));
05472 evalcond[3]=((((x1236)*(x1241)))+(((IkReal(-1.00000000000000))*(r12)*(x1242)))+(((IkReal(-1.00000000000000))*(r11)*(x1234)*(x1238)))+(((r02)*(x1239)))+(((IkReal(-1.00000000000000))*(sj7)*(x1235)*(x1240)))+(((x1237)*(x1238))));
05473 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05474 {
05475 continue;
05476 }
05477 }
05478
05479 {
05480 IkReal dummyeval[1];
05481 IkReal gconst35;
05482 gconst35=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
05483 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
05484 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05485 {
05486 {
05487 IkReal dummyeval[1];
05488 IkReal gconst36;
05489 gconst36=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
05490 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
05491 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05492 {
05493 continue;
05494
05495 } else
05496 {
05497 {
05498 IkReal j4array[1], cj4array[1], sj4array[1];
05499 bool j4valid[1]={false};
05500 _nj4 = 1;
05501 IkReal x1243=((cj3)*(cj5));
05502 IkReal x1244=((r02)*(sj7));
05503 IkReal x1245=((cj5)*(sj7));
05504 IkReal x1246=((cj3)*(sj5));
05505 IkReal x1247=((cj5)*(sj3));
05506 IkReal x1248=((cj7)*(cj8));
05507 IkReal x1249=((IkReal(1.00000000000000))*(sj5));
05508 IkReal x1250=((r12)*(sj3));
05509 IkReal x1251=((sj3)*(sj5));
05510 IkReal x1252=((IkReal(1.00000000000000))*(cj7)*(sj8));
05511 if( IKabs(((gconst36)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x1248)*(x1249)))+(((x1245)*(x1250)))+(((r00)*(x1243)*(x1248)))+(((IkReal(-1.00000000000000))*(r11)*(x1247)*(x1252)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1249)))+(((x1243)*(x1244)))+(((IkReal(-1.00000000000000))*(r01)*(x1243)*(x1252)))+(((r10)*(x1247)*(x1248))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x1249)))+(((cj5)*(r20)*(x1248)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1252)))+(((r10)*(x1248)*(x1251)))+(((IkReal(-1.00000000000000))*(r01)*(x1246)*(x1252)))+(((r00)*(x1246)*(x1248)))+(((sj5)*(sj7)*(x1250)))+(((x1244)*(x1246)))+(((r22)*(x1245))))))) < IKFAST_ATAN2_MAGTHRESH )
05512 continue;
05513 j4array[0]=IKatan2(((gconst36)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x1248)*(x1249)))+(((x1245)*(x1250)))+(((r00)*(x1243)*(x1248)))+(((IkReal(-1.00000000000000))*(r11)*(x1247)*(x1252)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1249)))+(((x1243)*(x1244)))+(((IkReal(-1.00000000000000))*(r01)*(x1243)*(x1252)))+(((r10)*(x1247)*(x1248)))))), ((gconst36)*(((((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x1249)))+(((cj5)*(r20)*(x1248)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1252)))+(((r10)*(x1248)*(x1251)))+(((IkReal(-1.00000000000000))*(r01)*(x1246)*(x1252)))+(((r00)*(x1246)*(x1248)))+(((sj5)*(sj7)*(x1250)))+(((x1244)*(x1246)))+(((r22)*(x1245)))))));
05514 sj4array[0]=IKsin(j4array[0]);
05515 cj4array[0]=IKcos(j4array[0]);
05516 if( j4array[0] > IKPI )
05517 {
05518 j4array[0]-=IK2PI;
05519 }
05520 else if( j4array[0] < -IKPI )
05521 { j4array[0]+=IK2PI;
05522 }
05523 j4valid[0] = true;
05524 for(int ij4 = 0; ij4 < 1; ++ij4)
05525 {
05526 if( !j4valid[ij4] )
05527 {
05528 continue;
05529 }
05530 _ij4[0] = ij4; _ij4[1] = -1;
05531 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05532 {
05533 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05534 {
05535 j4valid[iij4]=false; _ij4[1] = iij4; break;
05536 }
05537 }
05538 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05539 {
05540 IkReal evalcond[4];
05541 IkReal x1253=IKcos(j4);
05542 IkReal x1254=IKsin(j4);
05543 IkReal x1255=((IkReal(1.00000000000000))*(cj7));
05544 IkReal x1256=((cj8)*(r20));
05545 IkReal x1257=((cj3)*(r02));
05546 IkReal x1258=((IkReal(1.00000000000000))*(sj7));
05547 IkReal x1259=((sj3)*(sj7));
05548 IkReal x1260=((r21)*(sj8));
05549 IkReal x1261=((cj8)*(r10));
05550 IkReal x1262=((sj5)*(x1253));
05551 IkReal x1263=((cj5)*(x1254));
05552 IkReal x1264=((r11)*(sj3)*(sj8));
05553 IkReal x1265=((cj3)*(cj8)*(r00));
05554 IkReal x1266=((cj5)*(x1253));
05555 IkReal x1267=((cj3)*(r01)*(sj8));
05556 IkReal x1268=((sj5)*(x1254));
05557 IkReal x1269=((x1263)+(x1262));
05558 evalcond[0]=((x1268)+(((cj7)*(x1260)))+(((IkReal(-1.00000000000000))*(r22)*(x1258)))+(((IkReal(-1.00000000000000))*(x1255)*(x1256)))+(((IkReal(-1.00000000000000))*(x1266))));
05559 evalcond[1]=((x1269)+(((sj7)*(x1260)))+(((IkReal(-1.00000000000000))*(x1256)*(x1258)))+(((cj7)*(r22))));
05560 evalcond[2]=((x1269)+(((cj7)*(x1265)))+(((r12)*(x1259)))+(((sj7)*(x1257)))+(((IkReal(-1.00000000000000))*(x1255)*(x1267)))+(((IkReal(-1.00000000000000))*(x1255)*(x1264)))+(((cj7)*(sj3)*(x1261))));
05561 evalcond[3]=((((IkReal(-1.00000000000000))*(x1258)*(x1267)))+(((IkReal(-1.00000000000000))*(x1258)*(x1264)))+(x1266)+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1255)))+(((sj7)*(x1265)))+(((IkReal(-1.00000000000000))*(x1255)*(x1257)))+(((x1259)*(x1261)))+(((IkReal(-1.00000000000000))*(x1268))));
05562 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05563 {
05564 continue;
05565 }
05566 }
05567
05568 {
05569 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05570 vinfos[0].jointtype = 1;
05571 vinfos[0].foffset = j3;
05572 vinfos[0].indices[0] = _ij3[0];
05573 vinfos[0].indices[1] = _ij3[1];
05574 vinfos[0].maxsolutions = _nj3;
05575 vinfos[1].jointtype = 1;
05576 vinfos[1].foffset = j4;
05577 vinfos[1].indices[0] = _ij4[0];
05578 vinfos[1].indices[1] = _ij4[1];
05579 vinfos[1].maxsolutions = _nj4;
05580 vinfos[2].jointtype = 1;
05581 vinfos[2].foffset = j5;
05582 vinfos[2].indices[0] = _ij5[0];
05583 vinfos[2].indices[1] = _ij5[1];
05584 vinfos[2].maxsolutions = _nj5;
05585 vinfos[3].jointtype = 1;
05586 vinfos[3].foffset = j6;
05587 vinfos[3].indices[0] = _ij6[0];
05588 vinfos[3].indices[1] = _ij6[1];
05589 vinfos[3].maxsolutions = _nj6;
05590 vinfos[4].jointtype = 1;
05591 vinfos[4].foffset = j7;
05592 vinfos[4].indices[0] = _ij7[0];
05593 vinfos[4].indices[1] = _ij7[1];
05594 vinfos[4].maxsolutions = _nj7;
05595 vinfos[5].jointtype = 1;
05596 vinfos[5].foffset = j8;
05597 vinfos[5].indices[0] = _ij8[0];
05598 vinfos[5].indices[1] = _ij8[1];
05599 vinfos[5].maxsolutions = _nj8;
05600 std::vector<int> vfree(0);
05601 solutions.AddSolution(vinfos,vfree);
05602 }
05603 }
05604 }
05605
05606 }
05607
05608 }
05609
05610 } else
05611 {
05612 {
05613 IkReal j4array[1], cj4array[1], sj4array[1];
05614 bool j4valid[1]={false};
05615 _nj4 = 1;
05616 IkReal x1270=((sj5)*(sj7));
05617 IkReal x1271=((r21)*(sj8));
05618 IkReal x1272=((cj5)*(sj7));
05619 IkReal x1273=((cj8)*(r20));
05620 IkReal x1274=((cj5)*(cj7));
05621 IkReal x1275=((cj7)*(sj5));
05622 if( IKabs(((gconst35)*(((((IkReal(-1.00000000000000))*(x1273)*(x1275)))+(((r22)*(x1274)))+(((IkReal(-1.00000000000000))*(r22)*(x1270)))+(((IkReal(-1.00000000000000))*(x1272)*(x1273)))+(((x1271)*(x1275)))+(((x1271)*(x1272))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((IkReal(-1.00000000000000))*(x1271)*(x1274)))+(((r22)*(x1272)))+(((r22)*(x1275)))+(((x1273)*(x1274)))+(((x1270)*(x1271)))+(((IkReal(-1.00000000000000))*(x1270)*(x1273))))))) < IKFAST_ATAN2_MAGTHRESH )
05623 continue;
05624 j4array[0]=IKatan2(((gconst35)*(((((IkReal(-1.00000000000000))*(x1273)*(x1275)))+(((r22)*(x1274)))+(((IkReal(-1.00000000000000))*(r22)*(x1270)))+(((IkReal(-1.00000000000000))*(x1272)*(x1273)))+(((x1271)*(x1275)))+(((x1271)*(x1272)))))), ((gconst35)*(((((IkReal(-1.00000000000000))*(x1271)*(x1274)))+(((r22)*(x1272)))+(((r22)*(x1275)))+(((x1273)*(x1274)))+(((x1270)*(x1271)))+(((IkReal(-1.00000000000000))*(x1270)*(x1273)))))));
05625 sj4array[0]=IKsin(j4array[0]);
05626 cj4array[0]=IKcos(j4array[0]);
05627 if( j4array[0] > IKPI )
05628 {
05629 j4array[0]-=IK2PI;
05630 }
05631 else if( j4array[0] < -IKPI )
05632 { j4array[0]+=IK2PI;
05633 }
05634 j4valid[0] = true;
05635 for(int ij4 = 0; ij4 < 1; ++ij4)
05636 {
05637 if( !j4valid[ij4] )
05638 {
05639 continue;
05640 }
05641 _ij4[0] = ij4; _ij4[1] = -1;
05642 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05643 {
05644 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05645 {
05646 j4valid[iij4]=false; _ij4[1] = iij4; break;
05647 }
05648 }
05649 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05650 {
05651 IkReal evalcond[4];
05652 IkReal x1276=IKcos(j4);
05653 IkReal x1277=IKsin(j4);
05654 IkReal x1278=((IkReal(1.00000000000000))*(cj7));
05655 IkReal x1279=((cj8)*(r20));
05656 IkReal x1280=((cj3)*(r02));
05657 IkReal x1281=((IkReal(1.00000000000000))*(sj7));
05658 IkReal x1282=((sj3)*(sj7));
05659 IkReal x1283=((r21)*(sj8));
05660 IkReal x1284=((cj8)*(r10));
05661 IkReal x1285=((sj5)*(x1276));
05662 IkReal x1286=((cj5)*(x1277));
05663 IkReal x1287=((r11)*(sj3)*(sj8));
05664 IkReal x1288=((cj3)*(cj8)*(r00));
05665 IkReal x1289=((cj5)*(x1276));
05666 IkReal x1290=((cj3)*(r01)*(sj8));
05667 IkReal x1291=((sj5)*(x1277));
05668 IkReal x1292=((x1285)+(x1286));
05669 evalcond[0]=((((IkReal(-1.00000000000000))*(x1289)))+(x1291)+(((IkReal(-1.00000000000000))*(r22)*(x1281)))+(((cj7)*(x1283)))+(((IkReal(-1.00000000000000))*(x1278)*(x1279))));
05670 evalcond[1]=((x1292)+(((IkReal(-1.00000000000000))*(x1279)*(x1281)))+(((cj7)*(r22)))+(((sj7)*(x1283))));
05671 evalcond[2]=((x1292)+(((IkReal(-1.00000000000000))*(x1278)*(x1290)))+(((cj7)*(x1288)))+(((cj7)*(sj3)*(x1284)))+(((IkReal(-1.00000000000000))*(x1278)*(x1287)))+(((sj7)*(x1280)))+(((r12)*(x1282))));
05672 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1278)))+(x1289)+(((IkReal(-1.00000000000000))*(x1281)*(x1290)))+(((IkReal(-1.00000000000000))*(x1278)*(x1280)))+(((IkReal(-1.00000000000000))*(x1291)))+(((x1282)*(x1284)))+(((sj7)*(x1288)))+(((IkReal(-1.00000000000000))*(x1281)*(x1287))));
05673 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05674 {
05675 continue;
05676 }
05677 }
05678
05679 {
05680 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05681 vinfos[0].jointtype = 1;
05682 vinfos[0].foffset = j3;
05683 vinfos[0].indices[0] = _ij3[0];
05684 vinfos[0].indices[1] = _ij3[1];
05685 vinfos[0].maxsolutions = _nj3;
05686 vinfos[1].jointtype = 1;
05687 vinfos[1].foffset = j4;
05688 vinfos[1].indices[0] = _ij4[0];
05689 vinfos[1].indices[1] = _ij4[1];
05690 vinfos[1].maxsolutions = _nj4;
05691 vinfos[2].jointtype = 1;
05692 vinfos[2].foffset = j5;
05693 vinfos[2].indices[0] = _ij5[0];
05694 vinfos[2].indices[1] = _ij5[1];
05695 vinfos[2].maxsolutions = _nj5;
05696 vinfos[3].jointtype = 1;
05697 vinfos[3].foffset = j6;
05698 vinfos[3].indices[0] = _ij6[0];
05699 vinfos[3].indices[1] = _ij6[1];
05700 vinfos[3].maxsolutions = _nj6;
05701 vinfos[4].jointtype = 1;
05702 vinfos[4].foffset = j7;
05703 vinfos[4].indices[0] = _ij7[0];
05704 vinfos[4].indices[1] = _ij7[1];
05705 vinfos[4].maxsolutions = _nj7;
05706 vinfos[5].jointtype = 1;
05707 vinfos[5].foffset = j8;
05708 vinfos[5].indices[0] = _ij8[0];
05709 vinfos[5].indices[1] = _ij8[1];
05710 vinfos[5].maxsolutions = _nj8;
05711 std::vector<int> vfree(0);
05712 solutions.AddSolution(vinfos,vfree);
05713 }
05714 }
05715 }
05716
05717 }
05718
05719 }
05720 }
05721 }
05722
05723 }
05724
05725 }
05726
05727 } else
05728 {
05729 IkReal x1293=((IkReal(1.00000000000000))*(sj7));
05730 IkReal x1294=((npy)*(sj8));
05731 IkReal x1295=((cj8)*(npx));
05732 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j6)), IkReal(6.28318530717959))));
05733 evalcond[1]=((IkReal(0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
05734 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
05735 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj7)*(x1295)))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(x1293)))+(((IkReal(0.250000000000000))*(cj5)))+(((cj7)*(x1294))));
05736 evalcond[4]=((IkReal(-0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(0.250000000000000))*(sj5)))+(((sj7)*(x1294)))+(((IkReal(-1.00000000000000))*(x1293)*(x1295)))+(((cj7)*(npz))));
05737 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 )
05738 {
05739 {
05740 IkReal dummyeval[1];
05741 IkReal gconst39;
05742 IkReal x1296=(cj8)*(cj8);
05743 IkReal x1297=(sj8)*(sj8);
05744 IkReal x1298=((IkReal(2.00000000000000))*(cj8)*(sj8));
05745 IkReal x1299=((IkReal(1.00000000000000))*(x1296));
05746 IkReal x1300=((IkReal(1.00000000000000))*(x1297));
05747 gconst39=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1298)))+(((IkReal(-1.00000000000000))*(x1299)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1298)))+(((IkReal(-1.00000000000000))*(x1299)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1300)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1300)*((r00)*(r00))))));
05748 IkReal x1301=(cj8)*(cj8);
05749 IkReal x1302=(sj8)*(sj8);
05750 IkReal x1303=((IkReal(2.00000000000000))*(cj8)*(sj8));
05751 IkReal x1304=((IkReal(1.00000000000000))*(x1301));
05752 IkReal x1305=((IkReal(1.00000000000000))*(x1302));
05753 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1305)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1305)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1303)))+(((IkReal(-1.00000000000000))*(x1304)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1304)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1303))));
05754 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05755 {
05756 {
05757 IkReal dummyeval[1];
05758 IkReal gconst41;
05759 gconst41=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
05760 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
05761 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05762 {
05763 {
05764 IkReal dummyeval[1];
05765 IkReal gconst40;
05766 IkReal x1306=(sj8)*(sj8);
05767 IkReal x1307=(cj8)*(cj8);
05768 IkReal x1308=((sj7)*(sj8));
05769 IkReal x1309=((IkReal(1.00000000000000))*(r02));
05770 IkReal x1310=((r01)*(r10));
05771 IkReal x1311=((cj8)*(sj7));
05772 IkReal x1312=((cj7)*(x1306));
05773 IkReal x1313=((IkReal(1.00000000000000))*(r00)*(r11));
05774 IkReal x1314=((cj7)*(x1307));
05775 gconst40=IKsign(((((x1310)*(x1312)))+(((x1310)*(x1314)))+(((IkReal(-1.00000000000000))*(x1313)*(x1314)))+(((r01)*(r12)*(x1311)))+(((IkReal(-1.00000000000000))*(r10)*(x1308)*(x1309)))+(((IkReal(-1.00000000000000))*(r11)*(x1309)*(x1311)))+(((r00)*(r12)*(x1308)))+(((IkReal(-1.00000000000000))*(x1312)*(x1313)))));
05776 IkReal x1315=(sj8)*(sj8);
05777 IkReal x1316=(cj8)*(cj8);
05778 IkReal x1317=((sj7)*(sj8));
05779 IkReal x1318=((IkReal(1.00000000000000))*(r02));
05780 IkReal x1319=((r01)*(r10));
05781 IkReal x1320=((cj8)*(sj7));
05782 IkReal x1321=((cj7)*(x1315));
05783 IkReal x1322=((IkReal(1.00000000000000))*(r00)*(r11));
05784 IkReal x1323=((cj7)*(x1316));
05785 dummyeval[0]=((((IkReal(-1.00000000000000))*(r11)*(x1318)*(x1320)))+(((IkReal(-1.00000000000000))*(x1322)*(x1323)))+(((r01)*(r12)*(x1320)))+(((x1319)*(x1321)))+(((x1319)*(x1323)))+(((IkReal(-1.00000000000000))*(x1321)*(x1322)))+(((r00)*(r12)*(x1317)))+(((IkReal(-1.00000000000000))*(r10)*(x1317)*(x1318))));
05786 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05787 {
05788 continue;
05789
05790 } else
05791 {
05792 {
05793 IkReal j3array[1], cj3array[1], sj3array[1];
05794 bool j3valid[1]={false};
05795 _nj3 = 1;
05796 IkReal x1324=((cj7)*(cj8));
05797 IkReal x1325=((IkReal(1.00000000000000))*(cj7)*(sj8));
05798 if( IKabs(((gconst40)*(((((r10)*(x1324)))+(((IkReal(-1.00000000000000))*(r11)*(x1325)))+(((r12)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst40)*(((((r00)*(x1324)))+(((IkReal(-1.00000000000000))*(r01)*(x1325)))+(((r02)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH )
05799 continue;
05800 j3array[0]=IKatan2(((gconst40)*(((((r10)*(x1324)))+(((IkReal(-1.00000000000000))*(r11)*(x1325)))+(((r12)*(sj7)))))), ((gconst40)*(((((r00)*(x1324)))+(((IkReal(-1.00000000000000))*(r01)*(x1325)))+(((r02)*(sj7)))))));
05801 sj3array[0]=IKsin(j3array[0]);
05802 cj3array[0]=IKcos(j3array[0]);
05803 if( j3array[0] > IKPI )
05804 {
05805 j3array[0]-=IK2PI;
05806 }
05807 else if( j3array[0] < -IKPI )
05808 { j3array[0]+=IK2PI;
05809 }
05810 j3valid[0] = true;
05811 for(int ij3 = 0; ij3 < 1; ++ij3)
05812 {
05813 if( !j3valid[ij3] )
05814 {
05815 continue;
05816 }
05817 _ij3[0] = ij3; _ij3[1] = -1;
05818 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
05819 {
05820 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
05821 {
05822 j3valid[iij3]=false; _ij3[1] = iij3; break;
05823 }
05824 }
05825 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
05826 {
05827 IkReal evalcond[4];
05828 IkReal x1326=IKsin(j3);
05829 IkReal x1327=IKcos(j3);
05830 IkReal x1328=((IkReal(1.00000000000000))*(sj8));
05831 IkReal x1329=((IkReal(1.00000000000000))*(cj8));
05832 IkReal x1330=((r01)*(sj8));
05833 IkReal x1331=((cj8)*(r10));
05834 IkReal x1332=((sj7)*(x1327));
05835 IkReal x1333=((cj7)*(x1326));
05836 IkReal x1334=((r00)*(x1326));
05837 IkReal x1335=((sj7)*(x1326));
05838 IkReal x1336=((cj7)*(x1327));
05839 evalcond[0]=((IkReal(-1.00000000000000))+(((cj8)*(r01)*(x1326)))+(((IkReal(-1.00000000000000))*(r11)*(x1327)*(x1329)))+(((sj8)*(x1334)))+(((IkReal(-1.00000000000000))*(r10)*(x1327)*(x1328))));
05840 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1327)*(x1329)))+(((IkReal(-1.00000000000000))*(r00)*(x1327)*(x1328)))+(((IkReal(-1.00000000000000))*(r11)*(x1326)*(x1329)))+(((IkReal(-1.00000000000000))*(r10)*(x1326)*(x1328))));
05841 evalcond[2]=((((r12)*(x1332)))+(((x1331)*(x1336)))+(((IkReal(-1.00000000000000))*(r11)*(x1328)*(x1336)))+(((IkReal(-1.00000000000000))*(r00)*(x1329)*(x1333)))+(((x1330)*(x1333)))+(((IkReal(-1.00000000000000))*(r02)*(x1335))));
05842 evalcond[3]=((((x1331)*(x1332)))+(((IkReal(-1.00000000000000))*(r11)*(x1328)*(x1332)))+(((IkReal(-1.00000000000000))*(sj7)*(x1329)*(x1334)))+(((IkReal(-1.00000000000000))*(r12)*(x1336)))+(((x1330)*(x1335)))+(((r02)*(x1333))));
05843 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05844 {
05845 continue;
05846 }
05847 }
05848
05849 {
05850 IkReal dummyeval[1];
05851 IkReal gconst42;
05852 gconst42=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
05853 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
05854 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05855 {
05856 {
05857 IkReal dummyeval[1];
05858 IkReal gconst43;
05859 gconst43=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
05860 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
05861 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05862 {
05863 continue;
05864
05865 } else
05866 {
05867 {
05868 IkReal j4array[1], cj4array[1], sj4array[1];
05869 bool j4valid[1]={false};
05870 _nj4 = 1;
05871 IkReal x1337=((cj3)*(r01));
05872 IkReal x1338=((cj5)*(sj7));
05873 IkReal x1339=((r11)*(sj3));
05874 IkReal x1340=((r10)*(sj3));
05875 IkReal x1341=((cj3)*(r00));
05876 IkReal x1342=((r12)*(sj3));
05877 IkReal x1343=((sj5)*(sj7));
05878 IkReal x1344=((cj7)*(sj5));
05879 IkReal x1345=((cj3)*(r02)*(sj7));
05880 IkReal x1346=((IkReal(1.00000000000000))*(x1344));
05881 IkReal x1347=((cj5)*(cj7)*(cj8));
05882 IkReal x1348=((IkReal(1.00000000000000))*(cj5)*(cj7)*(sj8));
05883 if( IKabs(((gconst43)*(((((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1346)))+(((x1340)*(x1347)))+(((x1341)*(x1347)))+(((x1338)*(x1342)))+(((IkReal(-1.00000000000000))*(x1339)*(x1348)))+(((r21)*(sj8)*(x1344)))+(((IkReal(-1.00000000000000))*(r22)*(x1343)))+(((IkReal(-1.00000000000000))*(x1337)*(x1348)))+(((cj3)*(r02)*(x1338))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((cj8)*(x1340)*(x1344)))+(((IkReal(-1.00000000000000))*(r21)*(x1348)))+(((cj8)*(x1341)*(x1344)))+(((IkReal(-1.00000000000000))*(sj8)*(x1337)*(x1346)))+(((IkReal(-1.00000000000000))*(sj8)*(x1339)*(x1346)))+(((cj3)*(r02)*(x1343)))+(((x1342)*(x1343)))+(((r22)*(x1338)))+(((r20)*(x1347))))))) < IKFAST_ATAN2_MAGTHRESH )
05884 continue;
05885 j4array[0]=IKatan2(((gconst43)*(((((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1346)))+(((x1340)*(x1347)))+(((x1341)*(x1347)))+(((x1338)*(x1342)))+(((IkReal(-1.00000000000000))*(x1339)*(x1348)))+(((r21)*(sj8)*(x1344)))+(((IkReal(-1.00000000000000))*(r22)*(x1343)))+(((IkReal(-1.00000000000000))*(x1337)*(x1348)))+(((cj3)*(r02)*(x1338)))))), ((gconst43)*(((((cj8)*(x1340)*(x1344)))+(((IkReal(-1.00000000000000))*(r21)*(x1348)))+(((cj8)*(x1341)*(x1344)))+(((IkReal(-1.00000000000000))*(sj8)*(x1337)*(x1346)))+(((IkReal(-1.00000000000000))*(sj8)*(x1339)*(x1346)))+(((cj3)*(r02)*(x1343)))+(((x1342)*(x1343)))+(((r22)*(x1338)))+(((r20)*(x1347)))))));
05886 sj4array[0]=IKsin(j4array[0]);
05887 cj4array[0]=IKcos(j4array[0]);
05888 if( j4array[0] > IKPI )
05889 {
05890 j4array[0]-=IK2PI;
05891 }
05892 else if( j4array[0] < -IKPI )
05893 { j4array[0]+=IK2PI;
05894 }
05895 j4valid[0] = true;
05896 for(int ij4 = 0; ij4 < 1; ++ij4)
05897 {
05898 if( !j4valid[ij4] )
05899 {
05900 continue;
05901 }
05902 _ij4[0] = ij4; _ij4[1] = -1;
05903 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
05904 {
05905 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
05906 {
05907 j4valid[iij4]=false; _ij4[1] = iij4; break;
05908 }
05909 }
05910 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
05911 {
05912 IkReal evalcond[4];
05913 IkReal x1349=IKcos(j4);
05914 IkReal x1350=IKsin(j4);
05915 IkReal x1351=((IkReal(1.00000000000000))*(cj7));
05916 IkReal x1352=((cj8)*(r20));
05917 IkReal x1353=((cj3)*(r02));
05918 IkReal x1354=((IkReal(1.00000000000000))*(sj7));
05919 IkReal x1355=((sj3)*(sj7));
05920 IkReal x1356=((r21)*(sj8));
05921 IkReal x1357=((IkReal(1.00000000000000))*(cj5));
05922 IkReal x1358=((cj8)*(r10));
05923 IkReal x1359=((sj5)*(x1350));
05924 IkReal x1360=((sj5)*(x1349));
05925 IkReal x1361=((r11)*(sj3)*(sj8));
05926 IkReal x1362=((cj3)*(cj8)*(r00));
05927 IkReal x1363=((cj3)*(r01)*(sj8));
05928 IkReal x1364=((x1349)*(x1357));
05929 evalcond[0]=((x1359)+(((IkReal(-1.00000000000000))*(r22)*(x1354)))+(((cj7)*(x1356)))+(((IkReal(-1.00000000000000))*(x1351)*(x1352)))+(((IkReal(-1.00000000000000))*(x1364))));
05930 evalcond[1]=((((IkReal(-1.00000000000000))*(x1352)*(x1354)))+(((IkReal(-1.00000000000000))*(x1360)))+(((IkReal(-1.00000000000000))*(x1350)*(x1357)))+(((cj7)*(r22)))+(((sj7)*(x1356))));
05931 evalcond[2]=((x1360)+(((cj5)*(x1350)))+(((cj7)*(x1362)))+(((cj7)*(sj3)*(x1358)))+(((r12)*(x1355)))+(((IkReal(-1.00000000000000))*(x1351)*(x1363)))+(((IkReal(-1.00000000000000))*(x1351)*(x1361)))+(((sj7)*(x1353))));
05932 evalcond[3]=((x1359)+(((x1355)*(x1358)))+(((IkReal(-1.00000000000000))*(x1354)*(x1361)))+(((IkReal(-1.00000000000000))*(x1354)*(x1363)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1351)))+(((IkReal(-1.00000000000000))*(x1351)*(x1353)))+(((IkReal(-1.00000000000000))*(x1364)))+(((sj7)*(x1362))));
05933 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05934 {
05935 continue;
05936 }
05937 }
05938
05939 {
05940 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05941 vinfos[0].jointtype = 1;
05942 vinfos[0].foffset = j3;
05943 vinfos[0].indices[0] = _ij3[0];
05944 vinfos[0].indices[1] = _ij3[1];
05945 vinfos[0].maxsolutions = _nj3;
05946 vinfos[1].jointtype = 1;
05947 vinfos[1].foffset = j4;
05948 vinfos[1].indices[0] = _ij4[0];
05949 vinfos[1].indices[1] = _ij4[1];
05950 vinfos[1].maxsolutions = _nj4;
05951 vinfos[2].jointtype = 1;
05952 vinfos[2].foffset = j5;
05953 vinfos[2].indices[0] = _ij5[0];
05954 vinfos[2].indices[1] = _ij5[1];
05955 vinfos[2].maxsolutions = _nj5;
05956 vinfos[3].jointtype = 1;
05957 vinfos[3].foffset = j6;
05958 vinfos[3].indices[0] = _ij6[0];
05959 vinfos[3].indices[1] = _ij6[1];
05960 vinfos[3].maxsolutions = _nj6;
05961 vinfos[4].jointtype = 1;
05962 vinfos[4].foffset = j7;
05963 vinfos[4].indices[0] = _ij7[0];
05964 vinfos[4].indices[1] = _ij7[1];
05965 vinfos[4].maxsolutions = _nj7;
05966 vinfos[5].jointtype = 1;
05967 vinfos[5].foffset = j8;
05968 vinfos[5].indices[0] = _ij8[0];
05969 vinfos[5].indices[1] = _ij8[1];
05970 vinfos[5].maxsolutions = _nj8;
05971 std::vector<int> vfree(0);
05972 solutions.AddSolution(vinfos,vfree);
05973 }
05974 }
05975 }
05976
05977 }
05978
05979 }
05980
05981 } else
05982 {
05983 {
05984 IkReal j4array[1], cj4array[1], sj4array[1];
05985 bool j4valid[1]={false};
05986 _nj4 = 1;
05987 IkReal x1365=((cj7)*(sj5));
05988 IkReal x1366=((r21)*(sj8));
05989 IkReal x1367=((cj5)*(cj7));
05990 IkReal x1368=((cj8)*(r20));
05991 IkReal x1369=((sj5)*(sj7));
05992 IkReal x1370=((IkReal(1.00000000000000))*(cj5)*(sj7));
05993 if( IKabs(((gconst42)*(((((r22)*(x1367)))+(((r22)*(x1369)))+(((cj5)*(sj7)*(x1366)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)))+(((x1365)*(x1368)))+(((IkReal(-1.00000000000000))*(x1368)*(x1370))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1368)*(x1369)))+(((x1366)*(x1367)))+(((x1366)*(x1369)))+(((r22)*(x1365)))+(((IkReal(-1.00000000000000))*(r22)*(x1370)))+(((IkReal(-1.00000000000000))*(x1367)*(x1368))))))) < IKFAST_ATAN2_MAGTHRESH )
05994 continue;
05995 j4array[0]=IKatan2(((gconst42)*(((((r22)*(x1367)))+(((r22)*(x1369)))+(((cj5)*(sj7)*(x1366)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)))+(((x1365)*(x1368)))+(((IkReal(-1.00000000000000))*(x1368)*(x1370)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1368)*(x1369)))+(((x1366)*(x1367)))+(((x1366)*(x1369)))+(((r22)*(x1365)))+(((IkReal(-1.00000000000000))*(r22)*(x1370)))+(((IkReal(-1.00000000000000))*(x1367)*(x1368)))))));
05996 sj4array[0]=IKsin(j4array[0]);
05997 cj4array[0]=IKcos(j4array[0]);
05998 if( j4array[0] > IKPI )
05999 {
06000 j4array[0]-=IK2PI;
06001 }
06002 else if( j4array[0] < -IKPI )
06003 { j4array[0]+=IK2PI;
06004 }
06005 j4valid[0] = true;
06006 for(int ij4 = 0; ij4 < 1; ++ij4)
06007 {
06008 if( !j4valid[ij4] )
06009 {
06010 continue;
06011 }
06012 _ij4[0] = ij4; _ij4[1] = -1;
06013 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06014 {
06015 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06016 {
06017 j4valid[iij4]=false; _ij4[1] = iij4; break;
06018 }
06019 }
06020 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06021 {
06022 IkReal evalcond[4];
06023 IkReal x1371=IKcos(j4);
06024 IkReal x1372=IKsin(j4);
06025 IkReal x1373=((IkReal(1.00000000000000))*(cj7));
06026 IkReal x1374=((cj8)*(r20));
06027 IkReal x1375=((cj3)*(r02));
06028 IkReal x1376=((IkReal(1.00000000000000))*(sj7));
06029 IkReal x1377=((sj3)*(sj7));
06030 IkReal x1378=((r21)*(sj8));
06031 IkReal x1379=((IkReal(1.00000000000000))*(cj5));
06032 IkReal x1380=((cj8)*(r10));
06033 IkReal x1381=((sj5)*(x1372));
06034 IkReal x1382=((sj5)*(x1371));
06035 IkReal x1383=((r11)*(sj3)*(sj8));
06036 IkReal x1384=((cj3)*(cj8)*(r00));
06037 IkReal x1385=((cj3)*(r01)*(sj8));
06038 IkReal x1386=((x1371)*(x1379));
06039 evalcond[0]=((((IkReal(-1.00000000000000))*(x1386)))+(x1381)+(((IkReal(-1.00000000000000))*(r22)*(x1376)))+(((IkReal(-1.00000000000000))*(x1373)*(x1374)))+(((cj7)*(x1378))));
06040 evalcond[1]=((((IkReal(-1.00000000000000))*(x1372)*(x1379)))+(((IkReal(-1.00000000000000))*(x1374)*(x1376)))+(((IkReal(-1.00000000000000))*(x1382)))+(((sj7)*(x1378)))+(((cj7)*(r22))));
06041 evalcond[2]=((x1382)+(((IkReal(-1.00000000000000))*(x1373)*(x1385)))+(((IkReal(-1.00000000000000))*(x1373)*(x1383)))+(((cj5)*(x1372)))+(((r12)*(x1377)))+(((cj7)*(x1384)))+(((cj7)*(sj3)*(x1380)))+(((sj7)*(x1375))));
06042 evalcond[3]=((((IkReal(-1.00000000000000))*(x1386)))+(x1381)+(((sj7)*(x1384)))+(((IkReal(-1.00000000000000))*(x1376)*(x1383)))+(((IkReal(-1.00000000000000))*(x1376)*(x1385)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1373)))+(((x1377)*(x1380)))+(((IkReal(-1.00000000000000))*(x1373)*(x1375))));
06043 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06044 {
06045 continue;
06046 }
06047 }
06048
06049 {
06050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06051 vinfos[0].jointtype = 1;
06052 vinfos[0].foffset = j3;
06053 vinfos[0].indices[0] = _ij3[0];
06054 vinfos[0].indices[1] = _ij3[1];
06055 vinfos[0].maxsolutions = _nj3;
06056 vinfos[1].jointtype = 1;
06057 vinfos[1].foffset = j4;
06058 vinfos[1].indices[0] = _ij4[0];
06059 vinfos[1].indices[1] = _ij4[1];
06060 vinfos[1].maxsolutions = _nj4;
06061 vinfos[2].jointtype = 1;
06062 vinfos[2].foffset = j5;
06063 vinfos[2].indices[0] = _ij5[0];
06064 vinfos[2].indices[1] = _ij5[1];
06065 vinfos[2].maxsolutions = _nj5;
06066 vinfos[3].jointtype = 1;
06067 vinfos[3].foffset = j6;
06068 vinfos[3].indices[0] = _ij6[0];
06069 vinfos[3].indices[1] = _ij6[1];
06070 vinfos[3].maxsolutions = _nj6;
06071 vinfos[4].jointtype = 1;
06072 vinfos[4].foffset = j7;
06073 vinfos[4].indices[0] = _ij7[0];
06074 vinfos[4].indices[1] = _ij7[1];
06075 vinfos[4].maxsolutions = _nj7;
06076 vinfos[5].jointtype = 1;
06077 vinfos[5].foffset = j8;
06078 vinfos[5].indices[0] = _ij8[0];
06079 vinfos[5].indices[1] = _ij8[1];
06080 vinfos[5].maxsolutions = _nj8;
06081 std::vector<int> vfree(0);
06082 solutions.AddSolution(vinfos,vfree);
06083 }
06084 }
06085 }
06086
06087 }
06088
06089 }
06090 }
06091 }
06092
06093 }
06094
06095 }
06096
06097 } else
06098 {
06099 {
06100 IkReal j4array[1], cj4array[1], sj4array[1];
06101 bool j4valid[1]={false};
06102 _nj4 = 1;
06103 IkReal x1387=((cj7)*(sj5));
06104 IkReal x1388=((r21)*(sj8));
06105 IkReal x1389=((cj5)*(cj7));
06106 IkReal x1390=((cj8)*(r20));
06107 IkReal x1391=((sj5)*(sj7));
06108 IkReal x1392=((IkReal(1.00000000000000))*(cj5)*(sj7));
06109 if( IKabs(((gconst41)*(((((IkReal(-1.00000000000000))*(x1387)*(x1388)))+(((IkReal(-1.00000000000000))*(x1390)*(x1392)))+(((x1387)*(x1390)))+(((cj5)*(sj7)*(x1388)))+(((r22)*(x1389)))+(((r22)*(x1391))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst41)*(((((x1388)*(x1391)))+(((IkReal(-1.00000000000000))*(x1390)*(x1391)))+(((x1388)*(x1389)))+(((IkReal(-1.00000000000000))*(x1389)*(x1390)))+(((IkReal(-1.00000000000000))*(r22)*(x1392)))+(((r22)*(x1387))))))) < IKFAST_ATAN2_MAGTHRESH )
06110 continue;
06111 j4array[0]=IKatan2(((gconst41)*(((((IkReal(-1.00000000000000))*(x1387)*(x1388)))+(((IkReal(-1.00000000000000))*(x1390)*(x1392)))+(((x1387)*(x1390)))+(((cj5)*(sj7)*(x1388)))+(((r22)*(x1389)))+(((r22)*(x1391)))))), ((gconst41)*(((((x1388)*(x1391)))+(((IkReal(-1.00000000000000))*(x1390)*(x1391)))+(((x1388)*(x1389)))+(((IkReal(-1.00000000000000))*(x1389)*(x1390)))+(((IkReal(-1.00000000000000))*(r22)*(x1392)))+(((r22)*(x1387)))))));
06112 sj4array[0]=IKsin(j4array[0]);
06113 cj4array[0]=IKcos(j4array[0]);
06114 if( j4array[0] > IKPI )
06115 {
06116 j4array[0]-=IK2PI;
06117 }
06118 else if( j4array[0] < -IKPI )
06119 { j4array[0]+=IK2PI;
06120 }
06121 j4valid[0] = true;
06122 for(int ij4 = 0; ij4 < 1; ++ij4)
06123 {
06124 if( !j4valid[ij4] )
06125 {
06126 continue;
06127 }
06128 _ij4[0] = ij4; _ij4[1] = -1;
06129 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06130 {
06131 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06132 {
06133 j4valid[iij4]=false; _ij4[1] = iij4; break;
06134 }
06135 }
06136 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06137 {
06138 IkReal evalcond[2];
06139 IkReal x1393=IKcos(j4);
06140 IkReal x1394=IKsin(j4);
06141 IkReal x1395=((IkReal(1.00000000000000))*(cj5));
06142 IkReal x1396=((r21)*(sj8));
06143 IkReal x1397=((IkReal(1.00000000000000))*(cj8)*(r20));
06144 evalcond[0]=((((IkReal(-1.00000000000000))*(x1393)*(x1395)))+(((cj7)*(x1396)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((sj5)*(x1394)))+(((IkReal(-1.00000000000000))*(cj7)*(x1397))));
06145 evalcond[1]=((((IkReal(-1.00000000000000))*(sj7)*(x1397)))+(((IkReal(-1.00000000000000))*(sj5)*(x1393)))+(((sj7)*(x1396)))+(((IkReal(-1.00000000000000))*(x1394)*(x1395)))+(((cj7)*(r22))));
06146 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
06147 {
06148 continue;
06149 }
06150 }
06151
06152 {
06153 IkReal dummyeval[1];
06154 IkReal gconst44;
06155 IkReal x1398=(cj8)*(cj8);
06156 IkReal x1399=(sj8)*(sj8);
06157 IkReal x1400=((IkReal(2.00000000000000))*(cj8)*(sj8));
06158 IkReal x1401=((IkReal(1.00000000000000))*(x1398));
06159 IkReal x1402=((IkReal(1.00000000000000))*(x1399));
06160 gconst44=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1400)))+(((IkReal(-1.00000000000000))*(x1401)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1402)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1401)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1400)))+(((IkReal(-1.00000000000000))*(x1402)*((r10)*(r10))))));
06161 IkReal x1403=(cj8)*(cj8);
06162 IkReal x1404=(sj8)*(sj8);
06163 IkReal x1405=((IkReal(2.00000000000000))*(cj8)*(sj8));
06164 IkReal x1406=((IkReal(1.00000000000000))*(x1403));
06165 IkReal x1407=((IkReal(1.00000000000000))*(x1404));
06166 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1405)))+(((IkReal(-1.00000000000000))*(x1406)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1407)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1407)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1406)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1405))));
06167 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06168 {
06169 {
06170 IkReal dummyeval[1];
06171 IkReal gconst45;
06172 IkReal x1408=(sj8)*(sj8);
06173 IkReal x1409=(cj8)*(cj8);
06174 IkReal x1410=((sj7)*(sj8));
06175 IkReal x1411=((IkReal(1.00000000000000))*(r02));
06176 IkReal x1412=((r01)*(r10));
06177 IkReal x1413=((cj8)*(sj7));
06178 IkReal x1414=((cj7)*(x1408));
06179 IkReal x1415=((IkReal(1.00000000000000))*(r00)*(r11));
06180 IkReal x1416=((cj7)*(x1409));
06181 gconst45=IKsign(((((IkReal(-1.00000000000000))*(x1415)*(x1416)))+(((x1412)*(x1414)))+(((x1412)*(x1416)))+(((IkReal(-1.00000000000000))*(r11)*(x1411)*(x1413)))+(((IkReal(-1.00000000000000))*(x1414)*(x1415)))+(((r01)*(r12)*(x1413)))+(((IkReal(-1.00000000000000))*(r10)*(x1410)*(x1411)))+(((r00)*(r12)*(x1410)))));
06182 IkReal x1417=(sj8)*(sj8);
06183 IkReal x1418=(cj8)*(cj8);
06184 IkReal x1419=((sj7)*(sj8));
06185 IkReal x1420=((IkReal(1.00000000000000))*(r02));
06186 IkReal x1421=((r01)*(r10));
06187 IkReal x1422=((cj8)*(sj7));
06188 IkReal x1423=((cj7)*(x1417));
06189 IkReal x1424=((IkReal(1.00000000000000))*(r00)*(r11));
06190 IkReal x1425=((cj7)*(x1418));
06191 dummyeval[0]=((((r01)*(r12)*(x1422)))+(((IkReal(-1.00000000000000))*(x1424)*(x1425)))+(((IkReal(-1.00000000000000))*(x1423)*(x1424)))+(((IkReal(-1.00000000000000))*(r11)*(x1420)*(x1422)))+(((r00)*(r12)*(x1419)))+(((x1421)*(x1423)))+(((x1421)*(x1425)))+(((IkReal(-1.00000000000000))*(r10)*(x1419)*(x1420))));
06192 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06193 {
06194 continue;
06195
06196 } else
06197 {
06198 {
06199 IkReal j3array[1], cj3array[1], sj3array[1];
06200 bool j3valid[1]={false};
06201 _nj3 = 1;
06202 IkReal x1426=((cj7)*(cj8));
06203 IkReal x1427=((IkReal(1.00000000000000))*(cj7)*(sj8));
06204 if( IKabs(((gconst45)*(((((IkReal(-1.00000000000000))*(r11)*(x1427)))+(((r10)*(x1426)))+(((r12)*(sj7))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst45)*(((((r00)*(x1426)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x1427))))))) < IKFAST_ATAN2_MAGTHRESH )
06205 continue;
06206 j3array[0]=IKatan2(((gconst45)*(((((IkReal(-1.00000000000000))*(r11)*(x1427)))+(((r10)*(x1426)))+(((r12)*(sj7)))))), ((gconst45)*(((((r00)*(x1426)))+(((r02)*(sj7)))+(((IkReal(-1.00000000000000))*(r01)*(x1427)))))));
06207 sj3array[0]=IKsin(j3array[0]);
06208 cj3array[0]=IKcos(j3array[0]);
06209 if( j3array[0] > IKPI )
06210 {
06211 j3array[0]-=IK2PI;
06212 }
06213 else if( j3array[0] < -IKPI )
06214 { j3array[0]+=IK2PI;
06215 }
06216 j3valid[0] = true;
06217 for(int ij3 = 0; ij3 < 1; ++ij3)
06218 {
06219 if( !j3valid[ij3] )
06220 {
06221 continue;
06222 }
06223 _ij3[0] = ij3; _ij3[1] = -1;
06224 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06225 {
06226 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06227 {
06228 j3valid[iij3]=false; _ij3[1] = iij3; break;
06229 }
06230 }
06231 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06232 {
06233 IkReal evalcond[6];
06234 IkReal x1428=IKsin(j3);
06235 IkReal x1429=IKcos(j3);
06236 IkReal x1430=((r01)*(sj8));
06237 IkReal x1431=((cj8)*(r00));
06238 IkReal x1432=((r00)*(sj8));
06239 IkReal x1433=((cj8)*(r11));
06240 IkReal x1434=((r11)*(sj8));
06241 IkReal x1435=((r10)*(sj8));
06242 IkReal x1436=((cj8)*(r10));
06243 IkReal x1437=((cj7)*(x1428));
06244 IkReal x1438=((sj7)*(x1429));
06245 IkReal x1439=((IkReal(1.00000000000000))*(x1428));
06246 IkReal x1440=((IkReal(1.00000000000000))*(x1429));
06247 IkReal x1441=((cj8)*(x1428));
06248 IkReal x1442=((IkReal(1.00000000000000))*(x1434));
06249 IkReal x1443=((sj7)*(x1428));
06250 IkReal x1444=((cj7)*(x1429));
06251 evalcond[0]=((IkReal(-1.00000000000000))+(((x1428)*(x1432)))+(((IkReal(-1.00000000000000))*(x1433)*(x1440)))+(((IkReal(-1.00000000000000))*(x1435)*(x1440)))+(((r01)*(x1441))));
06252 evalcond[1]=((((IkReal(-1.00000000000000))*(x1435)*(x1439)))+(((IkReal(-1.00000000000000))*(x1432)*(x1440)))+(((IkReal(-1.00000000000000))*(x1433)*(x1439)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1440))));
06253 evalcond[2]=((((x1430)*(x1437)))+(((r12)*(x1438)))+(((IkReal(-1.00000000000000))*(x1431)*(x1437)))+(((x1436)*(x1444)))+(((IkReal(-1.00000000000000))*(cj7)*(x1434)*(x1440)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1439))));
06254 evalcond[3]=((((r02)*(x1437)))+(((x1436)*(x1438)))+(((IkReal(-1.00000000000000))*(x1438)*(x1442)))+(((x1430)*(x1443)))+(((IkReal(-1.00000000000000))*(sj7)*(x1431)*(x1439)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1440))));
06255 evalcond[4]=((((r12)*(x1443)))+(((r02)*(x1438)))+(((x1436)*(x1437)))+(((IkReal(-1.00000000000000))*(x1437)*(x1442)))+(((cj5)*(sj4)))+(((IkReal(-1.00000000000000))*(cj7)*(x1430)*(x1440)))+(((cj4)*(sj5)))+(((x1431)*(x1444))));
06256 evalcond[5]=((((IkReal(-1.00000000000000))*(sj7)*(x1434)*(x1439)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1440)))+(((x1436)*(x1443)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x1430)*(x1438)))+(((IkReal(-1.00000000000000))*(r12)*(x1437)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)))+(((x1431)*(x1438))));
06257 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 )
06258 {
06259 continue;
06260 }
06261 }
06262
06263 {
06264 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06265 vinfos[0].jointtype = 1;
06266 vinfos[0].foffset = j3;
06267 vinfos[0].indices[0] = _ij3[0];
06268 vinfos[0].indices[1] = _ij3[1];
06269 vinfos[0].maxsolutions = _nj3;
06270 vinfos[1].jointtype = 1;
06271 vinfos[1].foffset = j4;
06272 vinfos[1].indices[0] = _ij4[0];
06273 vinfos[1].indices[1] = _ij4[1];
06274 vinfos[1].maxsolutions = _nj4;
06275 vinfos[2].jointtype = 1;
06276 vinfos[2].foffset = j5;
06277 vinfos[2].indices[0] = _ij5[0];
06278 vinfos[2].indices[1] = _ij5[1];
06279 vinfos[2].maxsolutions = _nj5;
06280 vinfos[3].jointtype = 1;
06281 vinfos[3].foffset = j6;
06282 vinfos[3].indices[0] = _ij6[0];
06283 vinfos[3].indices[1] = _ij6[1];
06284 vinfos[3].maxsolutions = _nj6;
06285 vinfos[4].jointtype = 1;
06286 vinfos[4].foffset = j7;
06287 vinfos[4].indices[0] = _ij7[0];
06288 vinfos[4].indices[1] = _ij7[1];
06289 vinfos[4].maxsolutions = _nj7;
06290 vinfos[5].jointtype = 1;
06291 vinfos[5].foffset = j8;
06292 vinfos[5].indices[0] = _ij8[0];
06293 vinfos[5].indices[1] = _ij8[1];
06294 vinfos[5].maxsolutions = _nj8;
06295 std::vector<int> vfree(0);
06296 solutions.AddSolution(vinfos,vfree);
06297 }
06298 }
06299 }
06300
06301 }
06302
06303 }
06304
06305 } else
06306 {
06307 {
06308 IkReal j3array[1], cj3array[1], sj3array[1];
06309 bool j3valid[1]={false};
06310 _nj3 = 1;
06311 if( IKabs(((gconst44)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
06312 continue;
06313 j3array[0]=IKatan2(((gconst44)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst44)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
06314 sj3array[0]=IKsin(j3array[0]);
06315 cj3array[0]=IKcos(j3array[0]);
06316 if( j3array[0] > IKPI )
06317 {
06318 j3array[0]-=IK2PI;
06319 }
06320 else if( j3array[0] < -IKPI )
06321 { j3array[0]+=IK2PI;
06322 }
06323 j3valid[0] = true;
06324 for(int ij3 = 0; ij3 < 1; ++ij3)
06325 {
06326 if( !j3valid[ij3] )
06327 {
06328 continue;
06329 }
06330 _ij3[0] = ij3; _ij3[1] = -1;
06331 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06332 {
06333 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06334 {
06335 j3valid[iij3]=false; _ij3[1] = iij3; break;
06336 }
06337 }
06338 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06339 {
06340 IkReal evalcond[6];
06341 IkReal x1445=IKsin(j3);
06342 IkReal x1446=IKcos(j3);
06343 IkReal x1447=((r01)*(sj8));
06344 IkReal x1448=((cj8)*(r00));
06345 IkReal x1449=((r00)*(sj8));
06346 IkReal x1450=((cj8)*(r11));
06347 IkReal x1451=((r11)*(sj8));
06348 IkReal x1452=((r10)*(sj8));
06349 IkReal x1453=((cj8)*(r10));
06350 IkReal x1454=((cj7)*(x1445));
06351 IkReal x1455=((sj7)*(x1446));
06352 IkReal x1456=((IkReal(1.00000000000000))*(x1445));
06353 IkReal x1457=((IkReal(1.00000000000000))*(x1446));
06354 IkReal x1458=((cj8)*(x1445));
06355 IkReal x1459=((IkReal(1.00000000000000))*(x1451));
06356 IkReal x1460=((sj7)*(x1445));
06357 IkReal x1461=((cj7)*(x1446));
06358 evalcond[0]=((IkReal(-1.00000000000000))+(((x1445)*(x1449)))+(((r01)*(x1458)))+(((IkReal(-1.00000000000000))*(x1452)*(x1457)))+(((IkReal(-1.00000000000000))*(x1450)*(x1457))));
06359 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1457)))+(((IkReal(-1.00000000000000))*(x1452)*(x1456)))+(((IkReal(-1.00000000000000))*(x1449)*(x1457)))+(((IkReal(-1.00000000000000))*(x1450)*(x1456))));
06360 evalcond[2]=((((x1453)*(x1461)))+(((x1447)*(x1454)))+(((r12)*(x1455)))+(((IkReal(-1.00000000000000))*(cj7)*(x1451)*(x1457)))+(((IkReal(-1.00000000000000))*(x1448)*(x1454)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1456))));
06361 evalcond[3]=((((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1457)))+(((IkReal(-1.00000000000000))*(x1455)*(x1459)))+(((x1447)*(x1460)))+(((IkReal(-1.00000000000000))*(sj7)*(x1448)*(x1456)))+(((x1453)*(x1455)))+(((r02)*(x1454))));
06362 evalcond[4]=((((IkReal(-1.00000000000000))*(x1454)*(x1459)))+(((IkReal(-1.00000000000000))*(cj7)*(x1447)*(x1457)))+(((cj5)*(sj4)))+(((x1448)*(x1461)))+(((cj4)*(sj5)))+(((r12)*(x1460)))+(((x1453)*(x1454)))+(((r02)*(x1455))));
06363 evalcond[5]=((((x1453)*(x1460)))+(((IkReal(-1.00000000000000))*(x1447)*(x1455)))+(((IkReal(-1.00000000000000))*(sj7)*(x1451)*(x1456)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1457)))+(((x1448)*(x1455)))+(((sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)))+(((IkReal(-1.00000000000000))*(r12)*(x1454))));
06364 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 )
06365 {
06366 continue;
06367 }
06368 }
06369
06370 {
06371 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06372 vinfos[0].jointtype = 1;
06373 vinfos[0].foffset = j3;
06374 vinfos[0].indices[0] = _ij3[0];
06375 vinfos[0].indices[1] = _ij3[1];
06376 vinfos[0].maxsolutions = _nj3;
06377 vinfos[1].jointtype = 1;
06378 vinfos[1].foffset = j4;
06379 vinfos[1].indices[0] = _ij4[0];
06380 vinfos[1].indices[1] = _ij4[1];
06381 vinfos[1].maxsolutions = _nj4;
06382 vinfos[2].jointtype = 1;
06383 vinfos[2].foffset = j5;
06384 vinfos[2].indices[0] = _ij5[0];
06385 vinfos[2].indices[1] = _ij5[1];
06386 vinfos[2].maxsolutions = _nj5;
06387 vinfos[3].jointtype = 1;
06388 vinfos[3].foffset = j6;
06389 vinfos[3].indices[0] = _ij6[0];
06390 vinfos[3].indices[1] = _ij6[1];
06391 vinfos[3].maxsolutions = _nj6;
06392 vinfos[4].jointtype = 1;
06393 vinfos[4].foffset = j7;
06394 vinfos[4].indices[0] = _ij7[0];
06395 vinfos[4].indices[1] = _ij7[1];
06396 vinfos[4].maxsolutions = _nj7;
06397 vinfos[5].jointtype = 1;
06398 vinfos[5].foffset = j8;
06399 vinfos[5].indices[0] = _ij8[0];
06400 vinfos[5].indices[1] = _ij8[1];
06401 vinfos[5].maxsolutions = _nj8;
06402 std::vector<int> vfree(0);
06403 solutions.AddSolution(vinfos,vfree);
06404 }
06405 }
06406 }
06407
06408 }
06409
06410 }
06411 }
06412 }
06413
06414 }
06415
06416 }
06417
06418 } else
06419 {
06420 {
06421 IkReal j3array[1], cj3array[1], sj3array[1];
06422 bool j3valid[1]={false};
06423 _nj3 = 1;
06424 if( IKabs(((gconst39)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst39)*(((((r10)*(sj8)))+(((cj8)*(r11))))))) < IKFAST_ATAN2_MAGTHRESH )
06425 continue;
06426 j3array[0]=IKatan2(((gconst39)*(((((IkReal(-1.00000000000000))*(r00)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)))))), ((gconst39)*(((((r10)*(sj8)))+(((cj8)*(r11)))))));
06427 sj3array[0]=IKsin(j3array[0]);
06428 cj3array[0]=IKcos(j3array[0]);
06429 if( j3array[0] > IKPI )
06430 {
06431 j3array[0]-=IK2PI;
06432 }
06433 else if( j3array[0] < -IKPI )
06434 { j3array[0]+=IK2PI;
06435 }
06436 j3valid[0] = true;
06437 for(int ij3 = 0; ij3 < 1; ++ij3)
06438 {
06439 if( !j3valid[ij3] )
06440 {
06441 continue;
06442 }
06443 _ij3[0] = ij3; _ij3[1] = -1;
06444 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06445 {
06446 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06447 {
06448 j3valid[iij3]=false; _ij3[1] = iij3; break;
06449 }
06450 }
06451 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06452 {
06453 IkReal evalcond[4];
06454 IkReal x1462=IKsin(j3);
06455 IkReal x1463=IKcos(j3);
06456 IkReal x1464=((IkReal(1.00000000000000))*(sj8));
06457 IkReal x1465=((IkReal(1.00000000000000))*(cj8));
06458 IkReal x1466=((r01)*(sj8));
06459 IkReal x1467=((cj8)*(r10));
06460 IkReal x1468=((sj7)*(x1463));
06461 IkReal x1469=((cj7)*(x1462));
06462 IkReal x1470=((r00)*(x1462));
06463 IkReal x1471=((sj7)*(x1462));
06464 IkReal x1472=((cj7)*(x1463));
06465 evalcond[0]=((IkReal(-1.00000000000000))+(((sj8)*(x1470)))+(((IkReal(-1.00000000000000))*(r10)*(x1463)*(x1464)))+(((IkReal(-1.00000000000000))*(r11)*(x1463)*(x1465)))+(((cj8)*(r01)*(x1462))));
06466 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x1463)*(x1465)))+(((IkReal(-1.00000000000000))*(r10)*(x1462)*(x1464)))+(((IkReal(-1.00000000000000))*(r00)*(x1463)*(x1464)))+(((IkReal(-1.00000000000000))*(r11)*(x1462)*(x1465))));
06467 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1465)*(x1469)))+(((IkReal(-1.00000000000000))*(r11)*(x1464)*(x1472)))+(((x1467)*(x1472)))+(((x1466)*(x1469)))+(((IkReal(-1.00000000000000))*(r02)*(x1471)))+(((r12)*(x1468))));
06468 evalcond[3]=((((x1466)*(x1471)))+(((IkReal(-1.00000000000000))*(r12)*(x1472)))+(((IkReal(-1.00000000000000))*(sj7)*(x1465)*(x1470)))+(((x1467)*(x1468)))+(((IkReal(-1.00000000000000))*(r11)*(x1464)*(x1468)))+(((r02)*(x1469))));
06469 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06470 {
06471 continue;
06472 }
06473 }
06474
06475 {
06476 IkReal dummyeval[1];
06477 IkReal gconst42;
06478 gconst42=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
06479 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
06480 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06481 {
06482 {
06483 IkReal dummyeval[1];
06484 IkReal gconst43;
06485 gconst43=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
06486 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
06487 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06488 {
06489 continue;
06490
06491 } else
06492 {
06493 {
06494 IkReal j4array[1], cj4array[1], sj4array[1];
06495 bool j4valid[1]={false};
06496 _nj4 = 1;
06497 IkReal x1473=((cj3)*(r01));
06498 IkReal x1474=((cj5)*(sj7));
06499 IkReal x1475=((r11)*(sj3));
06500 IkReal x1476=((r10)*(sj3));
06501 IkReal x1477=((cj3)*(r00));
06502 IkReal x1478=((r12)*(sj3));
06503 IkReal x1479=((sj5)*(sj7));
06504 IkReal x1480=((cj7)*(sj5));
06505 IkReal x1481=((cj3)*(r02)*(sj7));
06506 IkReal x1482=((IkReal(1.00000000000000))*(x1480));
06507 IkReal x1483=((cj5)*(cj7)*(cj8));
06508 IkReal x1484=((IkReal(1.00000000000000))*(cj5)*(cj7)*(sj8));
06509 if( IKabs(((gconst43)*(((((x1476)*(x1483)))+(((x1474)*(x1478)))+(((x1477)*(x1483)))+(((r21)*(sj8)*(x1480)))+(((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1482)))+(((IkReal(-1.00000000000000))*(x1475)*(x1484)))+(((IkReal(-1.00000000000000))*(r22)*(x1479)))+(((IkReal(-1.00000000000000))*(x1473)*(x1484)))+(((cj3)*(r02)*(x1474))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((cj8)*(x1476)*(x1480)))+(((cj8)*(x1477)*(x1480)))+(((x1478)*(x1479)))+(((IkReal(-1.00000000000000))*(r21)*(x1484)))+(((r20)*(x1483)))+(((r22)*(x1474)))+(((cj3)*(r02)*(x1479)))+(((IkReal(-1.00000000000000))*(sj8)*(x1473)*(x1482)))+(((IkReal(-1.00000000000000))*(sj8)*(x1475)*(x1482))))))) < IKFAST_ATAN2_MAGTHRESH )
06510 continue;
06511 j4array[0]=IKatan2(((gconst43)*(((((x1476)*(x1483)))+(((x1474)*(x1478)))+(((x1477)*(x1483)))+(((r21)*(sj8)*(x1480)))+(((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1482)))+(((IkReal(-1.00000000000000))*(x1475)*(x1484)))+(((IkReal(-1.00000000000000))*(r22)*(x1479)))+(((IkReal(-1.00000000000000))*(x1473)*(x1484)))+(((cj3)*(r02)*(x1474)))))), ((gconst43)*(((((cj8)*(x1476)*(x1480)))+(((cj8)*(x1477)*(x1480)))+(((x1478)*(x1479)))+(((IkReal(-1.00000000000000))*(r21)*(x1484)))+(((r20)*(x1483)))+(((r22)*(x1474)))+(((cj3)*(r02)*(x1479)))+(((IkReal(-1.00000000000000))*(sj8)*(x1473)*(x1482)))+(((IkReal(-1.00000000000000))*(sj8)*(x1475)*(x1482)))))));
06512 sj4array[0]=IKsin(j4array[0]);
06513 cj4array[0]=IKcos(j4array[0]);
06514 if( j4array[0] > IKPI )
06515 {
06516 j4array[0]-=IK2PI;
06517 }
06518 else if( j4array[0] < -IKPI )
06519 { j4array[0]+=IK2PI;
06520 }
06521 j4valid[0] = true;
06522 for(int ij4 = 0; ij4 < 1; ++ij4)
06523 {
06524 if( !j4valid[ij4] )
06525 {
06526 continue;
06527 }
06528 _ij4[0] = ij4; _ij4[1] = -1;
06529 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06530 {
06531 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06532 {
06533 j4valid[iij4]=false; _ij4[1] = iij4; break;
06534 }
06535 }
06536 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06537 {
06538 IkReal evalcond[4];
06539 IkReal x1485=IKcos(j4);
06540 IkReal x1486=IKsin(j4);
06541 IkReal x1487=((IkReal(1.00000000000000))*(cj7));
06542 IkReal x1488=((cj8)*(r20));
06543 IkReal x1489=((cj3)*(r02));
06544 IkReal x1490=((IkReal(1.00000000000000))*(sj7));
06545 IkReal x1491=((sj3)*(sj7));
06546 IkReal x1492=((r21)*(sj8));
06547 IkReal x1493=((IkReal(1.00000000000000))*(cj5));
06548 IkReal x1494=((cj8)*(r10));
06549 IkReal x1495=((sj5)*(x1486));
06550 IkReal x1496=((sj5)*(x1485));
06551 IkReal x1497=((r11)*(sj3)*(sj8));
06552 IkReal x1498=((cj3)*(cj8)*(r00));
06553 IkReal x1499=((cj3)*(r01)*(sj8));
06554 IkReal x1500=((x1485)*(x1493));
06555 evalcond[0]=((((IkReal(-1.00000000000000))*(x1500)))+(((IkReal(-1.00000000000000))*(x1487)*(x1488)))+(x1495)+(((IkReal(-1.00000000000000))*(r22)*(x1490)))+(((cj7)*(x1492))));
06556 evalcond[1]=((((IkReal(-1.00000000000000))*(x1486)*(x1493)))+(((IkReal(-1.00000000000000))*(x1496)))+(((sj7)*(x1492)))+(((IkReal(-1.00000000000000))*(x1488)*(x1490)))+(((cj7)*(r22))));
06557 evalcond[2]=((((IkReal(-1.00000000000000))*(x1487)*(x1497)))+(((IkReal(-1.00000000000000))*(x1487)*(x1499)))+(((cj7)*(sj3)*(x1494)))+(((r12)*(x1491)))+(((cj5)*(x1486)))+(x1496)+(((sj7)*(x1489)))+(((cj7)*(x1498))));
06558 evalcond[3]=((((IkReal(-1.00000000000000))*(x1500)))+(((IkReal(-1.00000000000000))*(x1487)*(x1489)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1487)))+(((x1491)*(x1494)))+(((sj7)*(x1498)))+(x1495)+(((IkReal(-1.00000000000000))*(x1490)*(x1497)))+(((IkReal(-1.00000000000000))*(x1490)*(x1499))));
06559 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06560 {
06561 continue;
06562 }
06563 }
06564
06565 {
06566 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06567 vinfos[0].jointtype = 1;
06568 vinfos[0].foffset = j3;
06569 vinfos[0].indices[0] = _ij3[0];
06570 vinfos[0].indices[1] = _ij3[1];
06571 vinfos[0].maxsolutions = _nj3;
06572 vinfos[1].jointtype = 1;
06573 vinfos[1].foffset = j4;
06574 vinfos[1].indices[0] = _ij4[0];
06575 vinfos[1].indices[1] = _ij4[1];
06576 vinfos[1].maxsolutions = _nj4;
06577 vinfos[2].jointtype = 1;
06578 vinfos[2].foffset = j5;
06579 vinfos[2].indices[0] = _ij5[0];
06580 vinfos[2].indices[1] = _ij5[1];
06581 vinfos[2].maxsolutions = _nj5;
06582 vinfos[3].jointtype = 1;
06583 vinfos[3].foffset = j6;
06584 vinfos[3].indices[0] = _ij6[0];
06585 vinfos[3].indices[1] = _ij6[1];
06586 vinfos[3].maxsolutions = _nj6;
06587 vinfos[4].jointtype = 1;
06588 vinfos[4].foffset = j7;
06589 vinfos[4].indices[0] = _ij7[0];
06590 vinfos[4].indices[1] = _ij7[1];
06591 vinfos[4].maxsolutions = _nj7;
06592 vinfos[5].jointtype = 1;
06593 vinfos[5].foffset = j8;
06594 vinfos[5].indices[0] = _ij8[0];
06595 vinfos[5].indices[1] = _ij8[1];
06596 vinfos[5].maxsolutions = _nj8;
06597 std::vector<int> vfree(0);
06598 solutions.AddSolution(vinfos,vfree);
06599 }
06600 }
06601 }
06602
06603 }
06604
06605 }
06606
06607 } else
06608 {
06609 {
06610 IkReal j4array[1], cj4array[1], sj4array[1];
06611 bool j4valid[1]={false};
06612 _nj4 = 1;
06613 IkReal x1501=((cj7)*(sj5));
06614 IkReal x1502=((r21)*(sj8));
06615 IkReal x1503=((cj5)*(cj7));
06616 IkReal x1504=((cj8)*(r20));
06617 IkReal x1505=((sj5)*(sj7));
06618 IkReal x1506=((IkReal(1.00000000000000))*(cj5)*(sj7));
06619 if( IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1504)*(x1506)))+(((x1501)*(x1504)))+(((cj5)*(sj7)*(x1502)))+(((r22)*(x1503)))+(((r22)*(x1505)))+(((IkReal(-1.00000000000000))*(x1501)*(x1502))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1504)*(x1505)))+(((IkReal(-1.00000000000000))*(r22)*(x1506)))+(((IkReal(-1.00000000000000))*(x1503)*(x1504)))+(((r22)*(x1501)))+(((x1502)*(x1503)))+(((x1502)*(x1505))))))) < IKFAST_ATAN2_MAGTHRESH )
06620 continue;
06621 j4array[0]=IKatan2(((gconst42)*(((((IkReal(-1.00000000000000))*(x1504)*(x1506)))+(((x1501)*(x1504)))+(((cj5)*(sj7)*(x1502)))+(((r22)*(x1503)))+(((r22)*(x1505)))+(((IkReal(-1.00000000000000))*(x1501)*(x1502)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1504)*(x1505)))+(((IkReal(-1.00000000000000))*(r22)*(x1506)))+(((IkReal(-1.00000000000000))*(x1503)*(x1504)))+(((r22)*(x1501)))+(((x1502)*(x1503)))+(((x1502)*(x1505)))))));
06622 sj4array[0]=IKsin(j4array[0]);
06623 cj4array[0]=IKcos(j4array[0]);
06624 if( j4array[0] > IKPI )
06625 {
06626 j4array[0]-=IK2PI;
06627 }
06628 else if( j4array[0] < -IKPI )
06629 { j4array[0]+=IK2PI;
06630 }
06631 j4valid[0] = true;
06632 for(int ij4 = 0; ij4 < 1; ++ij4)
06633 {
06634 if( !j4valid[ij4] )
06635 {
06636 continue;
06637 }
06638 _ij4[0] = ij4; _ij4[1] = -1;
06639 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06640 {
06641 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06642 {
06643 j4valid[iij4]=false; _ij4[1] = iij4; break;
06644 }
06645 }
06646 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06647 {
06648 IkReal evalcond[4];
06649 IkReal x1507=IKcos(j4);
06650 IkReal x1508=IKsin(j4);
06651 IkReal x1509=((IkReal(1.00000000000000))*(cj7));
06652 IkReal x1510=((cj8)*(r20));
06653 IkReal x1511=((cj3)*(r02));
06654 IkReal x1512=((IkReal(1.00000000000000))*(sj7));
06655 IkReal x1513=((sj3)*(sj7));
06656 IkReal x1514=((r21)*(sj8));
06657 IkReal x1515=((IkReal(1.00000000000000))*(cj5));
06658 IkReal x1516=((cj8)*(r10));
06659 IkReal x1517=((sj5)*(x1508));
06660 IkReal x1518=((sj5)*(x1507));
06661 IkReal x1519=((r11)*(sj3)*(sj8));
06662 IkReal x1520=((cj3)*(cj8)*(r00));
06663 IkReal x1521=((cj3)*(r01)*(sj8));
06664 IkReal x1522=((x1507)*(x1515));
06665 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1512)))+(((IkReal(-1.00000000000000))*(x1522)))+(x1517)+(((IkReal(-1.00000000000000))*(x1509)*(x1510)))+(((cj7)*(x1514))));
06666 evalcond[1]=((((IkReal(-1.00000000000000))*(x1518)))+(((IkReal(-1.00000000000000))*(x1510)*(x1512)))+(((sj7)*(x1514)))+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x1508)*(x1515))));
06667 evalcond[2]=((((cj7)*(x1520)))+(((r12)*(x1513)))+(((cj7)*(sj3)*(x1516)))+(x1518)+(((cj5)*(x1508)))+(((sj7)*(x1511)))+(((IkReal(-1.00000000000000))*(x1509)*(x1519)))+(((IkReal(-1.00000000000000))*(x1509)*(x1521))));
06668 evalcond[3]=((((x1513)*(x1516)))+(((IkReal(-1.00000000000000))*(x1522)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1509)))+(x1517)+(((IkReal(-1.00000000000000))*(x1512)*(x1521)))+(((sj7)*(x1520)))+(((IkReal(-1.00000000000000))*(x1512)*(x1519)))+(((IkReal(-1.00000000000000))*(x1509)*(x1511))));
06669 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06670 {
06671 continue;
06672 }
06673 }
06674
06675 {
06676 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06677 vinfos[0].jointtype = 1;
06678 vinfos[0].foffset = j3;
06679 vinfos[0].indices[0] = _ij3[0];
06680 vinfos[0].indices[1] = _ij3[1];
06681 vinfos[0].maxsolutions = _nj3;
06682 vinfos[1].jointtype = 1;
06683 vinfos[1].foffset = j4;
06684 vinfos[1].indices[0] = _ij4[0];
06685 vinfos[1].indices[1] = _ij4[1];
06686 vinfos[1].maxsolutions = _nj4;
06687 vinfos[2].jointtype = 1;
06688 vinfos[2].foffset = j5;
06689 vinfos[2].indices[0] = _ij5[0];
06690 vinfos[2].indices[1] = _ij5[1];
06691 vinfos[2].maxsolutions = _nj5;
06692 vinfos[3].jointtype = 1;
06693 vinfos[3].foffset = j6;
06694 vinfos[3].indices[0] = _ij6[0];
06695 vinfos[3].indices[1] = _ij6[1];
06696 vinfos[3].maxsolutions = _nj6;
06697 vinfos[4].jointtype = 1;
06698 vinfos[4].foffset = j7;
06699 vinfos[4].indices[0] = _ij7[0];
06700 vinfos[4].indices[1] = _ij7[1];
06701 vinfos[4].maxsolutions = _nj7;
06702 vinfos[5].jointtype = 1;
06703 vinfos[5].foffset = j8;
06704 vinfos[5].indices[0] = _ij8[0];
06705 vinfos[5].indices[1] = _ij8[1];
06706 vinfos[5].maxsolutions = _nj8;
06707 std::vector<int> vfree(0);
06708 solutions.AddSolution(vinfos,vfree);
06709 }
06710 }
06711 }
06712
06713 }
06714
06715 }
06716 }
06717 }
06718
06719 }
06720
06721 }
06722
06723 } else
06724 {
06725 if( 1 )
06726 {
06727 continue;
06728
06729 } else
06730 {
06731 }
06732 }
06733 }
06734 }
06735 }
06736 }
06737
06738 } else
06739 {
06740 {
06741 IkReal j3array[1], cj3array[1], sj3array[1];
06742 bool j3valid[1]={false};
06743 _nj3 = 1;
06744 IkReal x1523=((cj6)*(cj7));
06745 IkReal x1524=((IkReal(1.00000000000000))*(sj8));
06746 IkReal x1525=((cj6)*(sj7));
06747 if( IKabs(((gconst2)*(((((r12)*(x1525)))+(((cj8)*(r10)*(x1523)))+(((IkReal(-1.00000000000000))*(r11)*(x1523)*(x1524))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj8)*(r00)*(x1523)))+(((IkReal(-1.00000000000000))*(r01)*(x1523)*(x1524)))+(((r02)*(x1525))))))) < IKFAST_ATAN2_MAGTHRESH )
06748 continue;
06749 j3array[0]=IKatan2(((gconst2)*(((((r12)*(x1525)))+(((cj8)*(r10)*(x1523)))+(((IkReal(-1.00000000000000))*(r11)*(x1523)*(x1524)))))), ((gconst2)*(((((cj8)*(r00)*(x1523)))+(((IkReal(-1.00000000000000))*(r01)*(x1523)*(x1524)))+(((r02)*(x1525)))))));
06750 sj3array[0]=IKsin(j3array[0]);
06751 cj3array[0]=IKcos(j3array[0]);
06752 if( j3array[0] > IKPI )
06753 {
06754 j3array[0]-=IK2PI;
06755 }
06756 else if( j3array[0] < -IKPI )
06757 { j3array[0]+=IK2PI;
06758 }
06759 j3valid[0] = true;
06760 for(int ij3 = 0; ij3 < 1; ++ij3)
06761 {
06762 if( !j3valid[ij3] )
06763 {
06764 continue;
06765 }
06766 _ij3[0] = ij3; _ij3[1] = -1;
06767 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
06768 {
06769 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
06770 {
06771 j3valid[iij3]=false; _ij3[1] = iij3; break;
06772 }
06773 }
06774 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
06775 {
06776 IkReal evalcond[3];
06777 IkReal x1526=IKsin(j3);
06778 IkReal x1527=IKcos(j3);
06779 IkReal x1528=((IkReal(1.00000000000000))*(r11));
06780 IkReal x1529=((r01)*(sj8));
06781 IkReal x1530=((cj8)*(r10));
06782 IkReal x1531=((IkReal(1.00000000000000))*(sj7));
06783 IkReal x1532=((sj7)*(x1527));
06784 IkReal x1533=((cj7)*(x1526));
06785 IkReal x1534=((r00)*(x1526));
06786 IkReal x1535=((cj7)*(x1527));
06787 evalcond[0]=((cj6)+(((cj8)*(r01)*(x1526)))+(((sj8)*(x1534)))+(((IkReal(-1.00000000000000))*(cj8)*(x1527)*(x1528)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x1527))));
06788 evalcond[1]=((((r12)*(x1532)))+(((x1530)*(x1535)))+(((IkReal(-1.00000000000000))*(sj8)*(x1528)*(x1535)))+(((x1529)*(x1533)))+(((IkReal(-1.00000000000000))*(r02)*(x1526)*(x1531)))+(((IkReal(-1.00000000000000))*(cj8)*(r00)*(x1533))));
06789 evalcond[2]=((((sj7)*(x1526)*(x1529)))+(sj6)+(((IkReal(-1.00000000000000))*(cj8)*(x1531)*(x1534)))+(((x1530)*(x1532)))+(((IkReal(-1.00000000000000))*(sj8)*(x1528)*(x1532)))+(((r02)*(x1533)))+(((IkReal(-1.00000000000000))*(r12)*(x1535))));
06790 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
06791 {
06792 continue;
06793 }
06794 }
06795
06796 {
06797 IkReal dummyeval[1];
06798 IkReal gconst6;
06799 gconst6=IKsign(((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5))))));
06800 dummyeval[0]=((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5)))));
06801 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06802 {
06803 {
06804 IkReal dummyeval[1];
06805 IkReal gconst7;
06806 gconst7=IKsign(((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5))))));
06807 dummyeval[0]=((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5)))));
06808 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06809 {
06810 {
06811 IkReal evalcond[9];
06812 IkReal x1536=((IkReal(1.00000000000000))*(sj7));
06813 IkReal x1537=((IkReal(1.00000000000000))*(cj3));
06814 IkReal x1538=((cj7)*(r02));
06815 IkReal x1539=((cj8)*(r00));
06816 IkReal x1540=((cj3)*(sj7));
06817 IkReal x1541=((cj8)*(npx));
06818 IkReal x1542=((r01)*(sj3));
06819 IkReal x1543=((cj7)*(sj8));
06820 IkReal x1544=((r11)*(sj8));
06821 IkReal x1545=((IkReal(1.00000000000000))*(cj7));
06822 IkReal x1546=((sj7)*(sj8));
06823 IkReal x1547=((cj8)*(r10));
06824 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j6)), IkReal(6.28318530717959))));
06825 evalcond[1]=((IkReal(-0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(0.250000000000000))*(sj5))));
06826 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(x1536)))+(((npy)*(x1543)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(x1541)*(x1545))));
06827 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(sj8)*(x1537)))+(((cj8)*(x1542)))+(((r00)*(sj3)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r11)*(x1537))));
06828 evalcond[4]=((((r21)*(x1546)))+(((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1536)))+(((cj7)*(r22))));
06829 evalcond[5]=((IkReal(-0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((npy)*(x1546)))+(((IkReal(-1.00000000000000))*(x1536)*(x1541)))+(((cj7)*(npz))));
06830 evalcond[6]=((((r12)*(x1540)))+(((IkReal(-1.00000000000000))*(r11)*(x1537)*(x1543)))+(((cj3)*(cj7)*(x1547)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1536)))+(((x1542)*(x1543)))+(((IkReal(-1.00000000000000))*(sj3)*(x1539)*(x1545))));
06831 evalcond[7]=((IkReal(1.00000000000000))+(((sj3)*(x1538)))+(((IkReal(-1.00000000000000))*(cj3)*(x1536)*(x1544)))+(((IkReal(-1.00000000000000))*(sj3)*(x1536)*(x1539)))+(((x1540)*(x1547)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1537)))+(((x1542)*(x1546))));
06832 evalcond[8]=((((x1539)*(x1540)))+(((IkReal(-1.00000000000000))*(x1537)*(x1538)))+(((IkReal(-1.00000000000000))*(sj3)*(x1536)*(x1544)))+(((sj3)*(sj7)*(x1547)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1545)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(sj8)*(x1536))));
06833 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 )
06834 {
06835 {
06836 IkReal dummyeval[1];
06837 IkReal gconst8;
06838 gconst8=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
06839 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
06840 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06841 {
06842 {
06843 IkReal dummyeval[1];
06844 IkReal gconst9;
06845 gconst9=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
06846 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
06847 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06848 {
06849 continue;
06850
06851 } else
06852 {
06853 {
06854 IkReal j4array[1], cj4array[1], sj4array[1];
06855 bool j4valid[1]={false};
06856 _nj4 = 1;
06857 IkReal x1548=((cj8)*(sj5));
06858 IkReal x1549=((cj3)*(r01));
06859 IkReal x1550=((IkReal(1.00000000000000))*(cj5));
06860 IkReal x1551=((sj5)*(sj8));
06861 IkReal x1552=((r10)*(sj3));
06862 IkReal x1553=((r11)*(sj3));
06863 IkReal x1554=((cj3)*(r00)*(sj8));
06864 if( IKabs(((gconst9)*(((((x1551)*(x1552)))+(((cj3)*(r00)*(x1551)))+(((cj5)*(r20)*(sj8)))+(((x1548)*(x1553)))+(((x1548)*(x1549)))+(((cj5)*(cj8)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(cj8)*(x1550)*(x1553)))+(((r21)*(x1548)))+(((IkReal(-1.00000000000000))*(x1550)*(x1554)))+(((IkReal(-1.00000000000000))*(cj8)*(x1549)*(x1550)))+(((r20)*(x1551)))+(((IkReal(-1.00000000000000))*(sj8)*(x1550)*(x1552))))))) < IKFAST_ATAN2_MAGTHRESH )
06865 continue;
06866 j4array[0]=IKatan2(((gconst9)*(((((x1551)*(x1552)))+(((cj3)*(r00)*(x1551)))+(((cj5)*(r20)*(sj8)))+(((x1548)*(x1553)))+(((x1548)*(x1549)))+(((cj5)*(cj8)*(r21)))))), ((gconst9)*(((((IkReal(-1.00000000000000))*(cj8)*(x1550)*(x1553)))+(((r21)*(x1548)))+(((IkReal(-1.00000000000000))*(x1550)*(x1554)))+(((IkReal(-1.00000000000000))*(cj8)*(x1549)*(x1550)))+(((r20)*(x1551)))+(((IkReal(-1.00000000000000))*(sj8)*(x1550)*(x1552)))))));
06867 sj4array[0]=IKsin(j4array[0]);
06868 cj4array[0]=IKcos(j4array[0]);
06869 if( j4array[0] > IKPI )
06870 {
06871 j4array[0]-=IK2PI;
06872 }
06873 else if( j4array[0] < -IKPI )
06874 { j4array[0]+=IK2PI;
06875 }
06876 j4valid[0] = true;
06877 for(int ij4 = 0; ij4 < 1; ++ij4)
06878 {
06879 if( !j4valid[ij4] )
06880 {
06881 continue;
06882 }
06883 _ij4[0] = ij4; _ij4[1] = -1;
06884 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06885 {
06886 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06887 {
06888 j4valid[iij4]=false; _ij4[1] = iij4; break;
06889 }
06890 }
06891 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06892 {
06893 IkReal evalcond[4];
06894 IkReal x1555=IKsin(j4);
06895 IkReal x1556=IKcos(j4);
06896 IkReal x1557=((IkReal(1.00000000000000))*(cj8));
06897 IkReal x1558=((cj3)*(r01));
06898 IkReal x1559=((cj3)*(r00));
06899 IkReal x1560=((cj7)*(cj8));
06900 IkReal x1561=((IkReal(1.00000000000000))*(cj5));
06901 IkReal x1562=((IkReal(1.00000000000000))*(sj8));
06902 IkReal x1563=((r11)*(sj3));
06903 IkReal x1564=((r10)*(sj3));
06904 IkReal x1565=((sj5)*(x1555));
06905 IkReal x1566=((sj5)*(x1556));
06906 IkReal x1567=((x1556)*(x1561));
06907 evalcond[0]=((((IkReal(-1.00000000000000))*(x1555)*(x1561)))+(((cj8)*(r21)))+(((r20)*(sj8)))+(((IkReal(-1.00000000000000))*(x1566))));
06908 evalcond[1]=((x1565)+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1567)))+(((cj7)*(r21)*(sj8)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1557))));
06909 evalcond[2]=((((IkReal(-1.00000000000000))*(x1559)*(x1562)))+(((IkReal(-1.00000000000000))*(x1557)*(x1563)))+(((IkReal(-1.00000000000000))*(x1562)*(x1564)))+(x1565)+(((IkReal(-1.00000000000000))*(x1557)*(x1558)))+(((IkReal(-1.00000000000000))*(x1567))));
06910 evalcond[3]=((((x1560)*(x1564)))+(((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x1558)*(x1562)))+(x1566)+(((cj3)*(r02)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x1562)*(x1563)))+(((x1559)*(x1560)))+(((cj5)*(x1555))));
06911 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06912 {
06913 continue;
06914 }
06915 }
06916
06917 {
06918 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06919 vinfos[0].jointtype = 1;
06920 vinfos[0].foffset = j3;
06921 vinfos[0].indices[0] = _ij3[0];
06922 vinfos[0].indices[1] = _ij3[1];
06923 vinfos[0].maxsolutions = _nj3;
06924 vinfos[1].jointtype = 1;
06925 vinfos[1].foffset = j4;
06926 vinfos[1].indices[0] = _ij4[0];
06927 vinfos[1].indices[1] = _ij4[1];
06928 vinfos[1].maxsolutions = _nj4;
06929 vinfos[2].jointtype = 1;
06930 vinfos[2].foffset = j5;
06931 vinfos[2].indices[0] = _ij5[0];
06932 vinfos[2].indices[1] = _ij5[1];
06933 vinfos[2].maxsolutions = _nj5;
06934 vinfos[3].jointtype = 1;
06935 vinfos[3].foffset = j6;
06936 vinfos[3].indices[0] = _ij6[0];
06937 vinfos[3].indices[1] = _ij6[1];
06938 vinfos[3].maxsolutions = _nj6;
06939 vinfos[4].jointtype = 1;
06940 vinfos[4].foffset = j7;
06941 vinfos[4].indices[0] = _ij7[0];
06942 vinfos[4].indices[1] = _ij7[1];
06943 vinfos[4].maxsolutions = _nj7;
06944 vinfos[5].jointtype = 1;
06945 vinfos[5].foffset = j8;
06946 vinfos[5].indices[0] = _ij8[0];
06947 vinfos[5].indices[1] = _ij8[1];
06948 vinfos[5].maxsolutions = _nj8;
06949 std::vector<int> vfree(0);
06950 solutions.AddSolution(vinfos,vfree);
06951 }
06952 }
06953 }
06954
06955 }
06956
06957 }
06958
06959 } else
06960 {
06961 {
06962 IkReal j4array[1], cj4array[1], sj4array[1];
06963 bool j4valid[1]={false};
06964 _nj4 = 1;
06965 IkReal x1568=((sj5)*(sj8));
06966 IkReal x1569=((IkReal(1.00000000000000))*(cj7));
06967 IkReal x1570=((cj8)*(sj5));
06968 IkReal x1571=((r22)*(sj7));
06969 IkReal x1572=((cj5)*(cj8));
06970 IkReal x1573=((cj5)*(sj8));
06971 if( IKabs(((gconst8)*(((((r20)*(x1573)))+(((cj7)*(r20)*(x1570)))+(((IkReal(-1.00000000000000))*(r21)*(x1568)*(x1569)))+(((r21)*(x1572)))+(((sj5)*(x1571))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((r20)*(x1568)))+(((cj7)*(r21)*(x1573)))+(((IkReal(-1.00000000000000))*(r20)*(x1569)*(x1572)))+(((r21)*(x1570)))+(((IkReal(-1.00000000000000))*(cj5)*(x1571))))))) < IKFAST_ATAN2_MAGTHRESH )
06972 continue;
06973 j4array[0]=IKatan2(((gconst8)*(((((r20)*(x1573)))+(((cj7)*(r20)*(x1570)))+(((IkReal(-1.00000000000000))*(r21)*(x1568)*(x1569)))+(((r21)*(x1572)))+(((sj5)*(x1571)))))), ((gconst8)*(((((r20)*(x1568)))+(((cj7)*(r21)*(x1573)))+(((IkReal(-1.00000000000000))*(r20)*(x1569)*(x1572)))+(((r21)*(x1570)))+(((IkReal(-1.00000000000000))*(cj5)*(x1571)))))));
06974 sj4array[0]=IKsin(j4array[0]);
06975 cj4array[0]=IKcos(j4array[0]);
06976 if( j4array[0] > IKPI )
06977 {
06978 j4array[0]-=IK2PI;
06979 }
06980 else if( j4array[0] < -IKPI )
06981 { j4array[0]+=IK2PI;
06982 }
06983 j4valid[0] = true;
06984 for(int ij4 = 0; ij4 < 1; ++ij4)
06985 {
06986 if( !j4valid[ij4] )
06987 {
06988 continue;
06989 }
06990 _ij4[0] = ij4; _ij4[1] = -1;
06991 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
06992 {
06993 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
06994 {
06995 j4valid[iij4]=false; _ij4[1] = iij4; break;
06996 }
06997 }
06998 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
06999 {
07000 IkReal evalcond[4];
07001 IkReal x1574=IKsin(j4);
07002 IkReal x1575=IKcos(j4);
07003 IkReal x1576=((IkReal(1.00000000000000))*(cj8));
07004 IkReal x1577=((cj3)*(r01));
07005 IkReal x1578=((cj3)*(r00));
07006 IkReal x1579=((cj7)*(cj8));
07007 IkReal x1580=((IkReal(1.00000000000000))*(cj5));
07008 IkReal x1581=((IkReal(1.00000000000000))*(sj8));
07009 IkReal x1582=((r11)*(sj3));
07010 IkReal x1583=((r10)*(sj3));
07011 IkReal x1584=((sj5)*(x1574));
07012 IkReal x1585=((sj5)*(x1575));
07013 IkReal x1586=((x1575)*(x1580));
07014 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x1574)*(x1580)))+(((IkReal(-1.00000000000000))*(x1585)))+(((r20)*(sj8))));
07015 evalcond[1]=((((IkReal(-1.00000000000000))*(x1586)))+(x1584)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1576)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
07016 evalcond[2]=((((IkReal(-1.00000000000000))*(x1586)))+(((IkReal(-1.00000000000000))*(x1576)*(x1577)))+(((IkReal(-1.00000000000000))*(x1578)*(x1581)))+(x1584)+(((IkReal(-1.00000000000000))*(x1581)*(x1583)))+(((IkReal(-1.00000000000000))*(x1576)*(x1582))));
07017 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((cj5)*(x1574)))+(x1585)+(((IkReal(-1.00000000000000))*(cj7)*(x1577)*(x1581)))+(((cj3)*(r02)*(sj7)))+(((x1579)*(x1583)))+(((x1578)*(x1579)))+(((IkReal(-1.00000000000000))*(cj7)*(x1581)*(x1582))));
07018 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07019 {
07020 continue;
07021 }
07022 }
07023
07024 {
07025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07026 vinfos[0].jointtype = 1;
07027 vinfos[0].foffset = j3;
07028 vinfos[0].indices[0] = _ij3[0];
07029 vinfos[0].indices[1] = _ij3[1];
07030 vinfos[0].maxsolutions = _nj3;
07031 vinfos[1].jointtype = 1;
07032 vinfos[1].foffset = j4;
07033 vinfos[1].indices[0] = _ij4[0];
07034 vinfos[1].indices[1] = _ij4[1];
07035 vinfos[1].maxsolutions = _nj4;
07036 vinfos[2].jointtype = 1;
07037 vinfos[2].foffset = j5;
07038 vinfos[2].indices[0] = _ij5[0];
07039 vinfos[2].indices[1] = _ij5[1];
07040 vinfos[2].maxsolutions = _nj5;
07041 vinfos[3].jointtype = 1;
07042 vinfos[3].foffset = j6;
07043 vinfos[3].indices[0] = _ij6[0];
07044 vinfos[3].indices[1] = _ij6[1];
07045 vinfos[3].maxsolutions = _nj6;
07046 vinfos[4].jointtype = 1;
07047 vinfos[4].foffset = j7;
07048 vinfos[4].indices[0] = _ij7[0];
07049 vinfos[4].indices[1] = _ij7[1];
07050 vinfos[4].maxsolutions = _nj7;
07051 vinfos[5].jointtype = 1;
07052 vinfos[5].foffset = j8;
07053 vinfos[5].indices[0] = _ij8[0];
07054 vinfos[5].indices[1] = _ij8[1];
07055 vinfos[5].maxsolutions = _nj8;
07056 std::vector<int> vfree(0);
07057 solutions.AddSolution(vinfos,vfree);
07058 }
07059 }
07060 }
07061
07062 }
07063
07064 }
07065
07066 } else
07067 {
07068 IkReal x1587=((IkReal(1.00000000000000))*(sj7));
07069 IkReal x1588=((IkReal(1.00000000000000))*(cj3));
07070 IkReal x1589=((cj7)*(r02));
07071 IkReal x1590=((cj8)*(r00));
07072 IkReal x1591=((cj3)*(sj7));
07073 IkReal x1592=((cj8)*(npx));
07074 IkReal x1593=((r01)*(sj3));
07075 IkReal x1594=((cj7)*(sj8));
07076 IkReal x1595=((r11)*(sj8));
07077 IkReal x1596=((IkReal(1.00000000000000))*(cj7));
07078 IkReal x1597=((sj7)*(sj8));
07079 IkReal x1598=((cj8)*(r10));
07080 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j6)), IkReal(6.28318530717959))));
07081 evalcond[1]=((IkReal(0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(-0.250000000000000))*(sj5))));
07082 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(x1592)*(x1596)))+(((IkReal(0.0900000000000000))*(cj7)))+(((npy)*(x1594)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(npz)*(x1587))));
07083 evalcond[3]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x1588)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x1588)))+(((cj8)*(x1593)))+(((r00)*(sj3)*(sj8))));
07084 evalcond[4]=((((IkReal(-1.00000000000000))*(cj8)*(r20)*(x1587)))+(((r21)*(x1597)))+(((cj7)*(r22))));
07085 evalcond[5]=((IkReal(0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((npy)*(x1597)))+(((cj7)*(npz)))+(((IkReal(-1.00000000000000))*(x1587)*(x1592))));
07086 evalcond[6]=((((x1593)*(x1594)))+(((r12)*(x1591)))+(((IkReal(-1.00000000000000))*(r11)*(x1588)*(x1594)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1587)))+(((cj3)*(cj7)*(x1598)))+(((IkReal(-1.00000000000000))*(sj3)*(x1590)*(x1596))));
07087 evalcond[7]=((IkReal(-1.00000000000000))+(((sj3)*(x1589)))+(((x1593)*(x1597)))+(((IkReal(-1.00000000000000))*(cj3)*(x1587)*(x1595)))+(((x1591)*(x1598)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1588)))+(((IkReal(-1.00000000000000))*(sj3)*(x1587)*(x1590))));
07088 evalcond[8]=((((x1590)*(x1591)))+(((sj3)*(sj7)*(x1598)))+(((IkReal(-1.00000000000000))*(x1588)*(x1589)))+(((IkReal(-1.00000000000000))*(sj3)*(x1587)*(x1595)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1596)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(sj8)*(x1587))));
07089 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 )
07090 {
07091 {
07092 IkReal dummyeval[1];
07093 IkReal gconst10;
07094 gconst10=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
07095 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
07096 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07097 {
07098 {
07099 IkReal dummyeval[1];
07100 IkReal gconst11;
07101 gconst11=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
07102 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
07103 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07104 {
07105 continue;
07106
07107 } else
07108 {
07109 {
07110 IkReal j4array[1], cj4array[1], sj4array[1];
07111 bool j4valid[1]={false};
07112 _nj4 = 1;
07113 IkReal x1599=((IkReal(1.00000000000000))*(sj5));
07114 IkReal x1600=((r20)*(sj8));
07115 IkReal x1601=((IkReal(1.00000000000000))*(cj5));
07116 IkReal x1602=((cj8)*(r21));
07117 IkReal x1603=((cj8)*(r11)*(sj3));
07118 IkReal x1604=((cj3)*(cj8)*(r01));
07119 IkReal x1605=((cj3)*(r00)*(sj8));
07120 IkReal x1606=((r10)*(sj3)*(sj8));
07121 if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(x1601)*(x1602)))+(((IkReal(-1.00000000000000))*(x1600)*(x1601)))+(((IkReal(-1.00000000000000))*(x1599)*(x1603)))+(((IkReal(-1.00000000000000))*(x1599)*(x1605)))+(((IkReal(-1.00000000000000))*(x1599)*(x1604)))+(((IkReal(-1.00000000000000))*(x1599)*(x1606))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj5)*(x1603)))+(((cj5)*(x1604)))+(((cj5)*(x1605)))+(((cj5)*(x1606)))+(((IkReal(-1.00000000000000))*(x1599)*(x1600)))+(((IkReal(-1.00000000000000))*(x1599)*(x1602))))))) < IKFAST_ATAN2_MAGTHRESH )
07122 continue;
07123 j4array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(x1601)*(x1602)))+(((IkReal(-1.00000000000000))*(x1600)*(x1601)))+(((IkReal(-1.00000000000000))*(x1599)*(x1603)))+(((IkReal(-1.00000000000000))*(x1599)*(x1605)))+(((IkReal(-1.00000000000000))*(x1599)*(x1604)))+(((IkReal(-1.00000000000000))*(x1599)*(x1606)))))), ((gconst11)*(((((cj5)*(x1603)))+(((cj5)*(x1604)))+(((cj5)*(x1605)))+(((cj5)*(x1606)))+(((IkReal(-1.00000000000000))*(x1599)*(x1600)))+(((IkReal(-1.00000000000000))*(x1599)*(x1602)))))));
07124 sj4array[0]=IKsin(j4array[0]);
07125 cj4array[0]=IKcos(j4array[0]);
07126 if( j4array[0] > IKPI )
07127 {
07128 j4array[0]-=IK2PI;
07129 }
07130 else if( j4array[0] < -IKPI )
07131 { j4array[0]+=IK2PI;
07132 }
07133 j4valid[0] = true;
07134 for(int ij4 = 0; ij4 < 1; ++ij4)
07135 {
07136 if( !j4valid[ij4] )
07137 {
07138 continue;
07139 }
07140 _ij4[0] = ij4; _ij4[1] = -1;
07141 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07142 {
07143 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07144 {
07145 j4valid[iij4]=false; _ij4[1] = iij4; break;
07146 }
07147 }
07148 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07149 {
07150 IkReal evalcond[4];
07151 IkReal x1607=IKsin(j4);
07152 IkReal x1608=IKcos(j4);
07153 IkReal x1609=((IkReal(1.00000000000000))*(cj8));
07154 IkReal x1610=((cj3)*(r01));
07155 IkReal x1611=((cj3)*(r00));
07156 IkReal x1612=((cj7)*(cj8));
07157 IkReal x1613=((r11)*(sj3));
07158 IkReal x1614=((IkReal(1.00000000000000))*(sj8));
07159 IkReal x1615=((r10)*(sj3));
07160 IkReal x1616=((sj5)*(x1608));
07161 IkReal x1617=((cj5)*(x1607));
07162 IkReal x1618=((cj5)*(x1608));
07163 IkReal x1619=((sj5)*(x1607));
07164 IkReal x1620=((x1616)+(x1617));
07165 evalcond[0]=((x1620)+(((cj8)*(r21)))+(((r20)*(sj8))));
07166 evalcond[1]=((((IkReal(-1.00000000000000))*(x1618)))+(x1619)+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1609)))+(((cj7)*(r21)*(sj8))));
07167 evalcond[2]=((((IkReal(-1.00000000000000))*(x1619)))+(((IkReal(-1.00000000000000))*(x1611)*(x1614)))+(x1618)+(((IkReal(-1.00000000000000))*(x1609)*(x1610)))+(((IkReal(-1.00000000000000))*(x1609)*(x1613)))+(((IkReal(-1.00000000000000))*(x1614)*(x1615))));
07168 evalcond[3]=((((r12)*(sj3)*(sj7)))+(x1620)+(((IkReal(-1.00000000000000))*(cj7)*(x1610)*(x1614)))+(((IkReal(-1.00000000000000))*(cj7)*(x1613)*(x1614)))+(((cj3)*(r02)*(sj7)))+(((x1612)*(x1615)))+(((x1611)*(x1612))));
07169 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07170 {
07171 continue;
07172 }
07173 }
07174
07175 {
07176 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07177 vinfos[0].jointtype = 1;
07178 vinfos[0].foffset = j3;
07179 vinfos[0].indices[0] = _ij3[0];
07180 vinfos[0].indices[1] = _ij3[1];
07181 vinfos[0].maxsolutions = _nj3;
07182 vinfos[1].jointtype = 1;
07183 vinfos[1].foffset = j4;
07184 vinfos[1].indices[0] = _ij4[0];
07185 vinfos[1].indices[1] = _ij4[1];
07186 vinfos[1].maxsolutions = _nj4;
07187 vinfos[2].jointtype = 1;
07188 vinfos[2].foffset = j5;
07189 vinfos[2].indices[0] = _ij5[0];
07190 vinfos[2].indices[1] = _ij5[1];
07191 vinfos[2].maxsolutions = _nj5;
07192 vinfos[3].jointtype = 1;
07193 vinfos[3].foffset = j6;
07194 vinfos[3].indices[0] = _ij6[0];
07195 vinfos[3].indices[1] = _ij6[1];
07196 vinfos[3].maxsolutions = _nj6;
07197 vinfos[4].jointtype = 1;
07198 vinfos[4].foffset = j7;
07199 vinfos[4].indices[0] = _ij7[0];
07200 vinfos[4].indices[1] = _ij7[1];
07201 vinfos[4].maxsolutions = _nj7;
07202 vinfos[5].jointtype = 1;
07203 vinfos[5].foffset = j8;
07204 vinfos[5].indices[0] = _ij8[0];
07205 vinfos[5].indices[1] = _ij8[1];
07206 vinfos[5].maxsolutions = _nj8;
07207 std::vector<int> vfree(0);
07208 solutions.AddSolution(vinfos,vfree);
07209 }
07210 }
07211 }
07212
07213 }
07214
07215 }
07216
07217 } else
07218 {
07219 {
07220 IkReal j4array[1], cj4array[1], sj4array[1];
07221 bool j4valid[1]={false};
07222 _nj4 = 1;
07223 IkReal x1621=((r20)*(sj5));
07224 IkReal x1622=((r22)*(sj7));
07225 IkReal x1623=((cj8)*(r21));
07226 IkReal x1624=((cj7)*(cj8));
07227 IkReal x1625=((cj5)*(r20));
07228 IkReal x1626=((cj7)*(r21)*(sj8));
07229 if( IKabs(((gconst10)*(((((sj5)*(x1626)))+(((cj5)*(x1623)))+(((IkReal(-1.00000000000000))*(x1621)*(x1624)))+(((sj8)*(x1625)))+(((IkReal(-1.00000000000000))*(sj5)*(x1622))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((sj5)*(x1623)))+(((IkReal(-1.00000000000000))*(cj5)*(x1626)))+(((cj5)*(x1622)))+(((sj8)*(x1621)))+(((x1624)*(x1625))))))) < IKFAST_ATAN2_MAGTHRESH )
07230 continue;
07231 j4array[0]=IKatan2(((gconst10)*(((((sj5)*(x1626)))+(((cj5)*(x1623)))+(((IkReal(-1.00000000000000))*(x1621)*(x1624)))+(((sj8)*(x1625)))+(((IkReal(-1.00000000000000))*(sj5)*(x1622)))))), ((gconst10)*(((((sj5)*(x1623)))+(((IkReal(-1.00000000000000))*(cj5)*(x1626)))+(((cj5)*(x1622)))+(((sj8)*(x1621)))+(((x1624)*(x1625)))))));
07232 sj4array[0]=IKsin(j4array[0]);
07233 cj4array[0]=IKcos(j4array[0]);
07234 if( j4array[0] > IKPI )
07235 {
07236 j4array[0]-=IK2PI;
07237 }
07238 else if( j4array[0] < -IKPI )
07239 { j4array[0]+=IK2PI;
07240 }
07241 j4valid[0] = true;
07242 for(int ij4 = 0; ij4 < 1; ++ij4)
07243 {
07244 if( !j4valid[ij4] )
07245 {
07246 continue;
07247 }
07248 _ij4[0] = ij4; _ij4[1] = -1;
07249 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07250 {
07251 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07252 {
07253 j4valid[iij4]=false; _ij4[1] = iij4; break;
07254 }
07255 }
07256 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07257 {
07258 IkReal evalcond[4];
07259 IkReal x1627=IKsin(j4);
07260 IkReal x1628=IKcos(j4);
07261 IkReal x1629=((IkReal(1.00000000000000))*(cj8));
07262 IkReal x1630=((cj3)*(r01));
07263 IkReal x1631=((cj3)*(r00));
07264 IkReal x1632=((cj7)*(cj8));
07265 IkReal x1633=((r11)*(sj3));
07266 IkReal x1634=((IkReal(1.00000000000000))*(sj8));
07267 IkReal x1635=((r10)*(sj3));
07268 IkReal x1636=((sj5)*(x1628));
07269 IkReal x1637=((cj5)*(x1627));
07270 IkReal x1638=((cj5)*(x1628));
07271 IkReal x1639=((sj5)*(x1627));
07272 IkReal x1640=((x1636)+(x1637));
07273 evalcond[0]=((x1640)+(((cj8)*(r21)))+(((r20)*(sj8))));
07274 evalcond[1]=((x1639)+(((IkReal(-1.00000000000000))*(x1638)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1629)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
07275 evalcond[2]=((((IkReal(-1.00000000000000))*(x1634)*(x1635)))+(x1638)+(((IkReal(-1.00000000000000))*(x1639)))+(((IkReal(-1.00000000000000))*(x1629)*(x1633)))+(((IkReal(-1.00000000000000))*(x1629)*(x1630)))+(((IkReal(-1.00000000000000))*(x1631)*(x1634))));
07276 evalcond[3]=((((r12)*(sj3)*(sj7)))+(x1640)+(((IkReal(-1.00000000000000))*(cj7)*(x1633)*(x1634)))+(((cj3)*(r02)*(sj7)))+(((x1632)*(x1635)))+(((x1631)*(x1632)))+(((IkReal(-1.00000000000000))*(cj7)*(x1630)*(x1634))));
07277 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07278 {
07279 continue;
07280 }
07281 }
07282
07283 {
07284 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07285 vinfos[0].jointtype = 1;
07286 vinfos[0].foffset = j3;
07287 vinfos[0].indices[0] = _ij3[0];
07288 vinfos[0].indices[1] = _ij3[1];
07289 vinfos[0].maxsolutions = _nj3;
07290 vinfos[1].jointtype = 1;
07291 vinfos[1].foffset = j4;
07292 vinfos[1].indices[0] = _ij4[0];
07293 vinfos[1].indices[1] = _ij4[1];
07294 vinfos[1].maxsolutions = _nj4;
07295 vinfos[2].jointtype = 1;
07296 vinfos[2].foffset = j5;
07297 vinfos[2].indices[0] = _ij5[0];
07298 vinfos[2].indices[1] = _ij5[1];
07299 vinfos[2].maxsolutions = _nj5;
07300 vinfos[3].jointtype = 1;
07301 vinfos[3].foffset = j6;
07302 vinfos[3].indices[0] = _ij6[0];
07303 vinfos[3].indices[1] = _ij6[1];
07304 vinfos[3].maxsolutions = _nj6;
07305 vinfos[4].jointtype = 1;
07306 vinfos[4].foffset = j7;
07307 vinfos[4].indices[0] = _ij7[0];
07308 vinfos[4].indices[1] = _ij7[1];
07309 vinfos[4].maxsolutions = _nj7;
07310 vinfos[5].jointtype = 1;
07311 vinfos[5].foffset = j8;
07312 vinfos[5].indices[0] = _ij8[0];
07313 vinfos[5].indices[1] = _ij8[1];
07314 vinfos[5].maxsolutions = _nj8;
07315 std::vector<int> vfree(0);
07316 solutions.AddSolution(vinfos,vfree);
07317 }
07318 }
07319 }
07320
07321 }
07322
07323 }
07324
07325 } else
07326 {
07327 IkReal x1641=((cj3)*(cj8));
07328 IkReal x1642=((IkReal(1.00000000000000))*(sj7));
07329 IkReal x1643=((cj8)*(npx));
07330 IkReal x1644=((cj8)*(sj3));
07331 IkReal x1645=((npy)*(sj8));
07332 IkReal x1646=((r02)*(sj3));
07333 IkReal x1647=((IkReal(1.00000000000000))*(r11));
07334 IkReal x1648=((cj3)*(r12));
07335 IkReal x1649=((IkReal(1.00000000000000))*(cj7));
07336 IkReal x1650=((sj3)*(sj8));
07337 IkReal x1651=((IkReal(1.00000000000000))*(cj3)*(sj8));
07338 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j6)), IkReal(6.28318530717959))));
07339 evalcond[1]=((IkReal(-0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
07340 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
07341 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(x1642)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(x1643)*(x1649)))+(((cj7)*(x1645))));
07342 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1641)*(x1647)))+(((IkReal(-1.00000000000000))*(r10)*(x1651)))+(((r00)*(x1650)))+(((r01)*(x1644))));
07343 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x1642)*(x1643)))+(((cj7)*(npz)))+(((sj7)*(x1645)))+(((IkReal(-0.250000000000000))*(sj5))));
07344 evalcond[6]=((((IkReal(-1.00000000000000))*(r00)*(x1651)))+(((IkReal(-1.00000000000000))*(x1644)*(x1647)))+(((IkReal(-1.00000000000000))*(r01)*(x1641)))+(((IkReal(-1.00000000000000))*(r10)*(x1650))));
07345 evalcond[7]=((((cj7)*(r01)*(x1650)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(sj8)*(x1647)))+(((cj7)*(r10)*(x1641)))+(((IkReal(-1.00000000000000))*(x1642)*(x1646)))+(((IkReal(-1.00000000000000))*(r00)*(x1644)*(x1649)))+(((sj7)*(x1648))));
07346 evalcond[8]=((((cj7)*(x1646)))+(((IkReal(-1.00000000000000))*(x1648)*(x1649)))+(((r10)*(sj7)*(x1641)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(sj8)*(x1642)))+(((r01)*(sj7)*(x1650)))+(((IkReal(-1.00000000000000))*(r00)*(x1642)*(x1644))));
07347 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 )
07348 {
07349 {
07350 IkReal dummyeval[1];
07351 IkReal gconst12;
07352 gconst12=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
07353 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
07354 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07355 {
07356 {
07357 IkReal dummyeval[1];
07358 IkReal gconst13;
07359 gconst13=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
07360 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
07361 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07362 {
07363 continue;
07364
07365 } else
07366 {
07367 {
07368 IkReal j4array[1], cj4array[1], sj4array[1];
07369 bool j4valid[1]={false};
07370 _nj4 = 1;
07371 IkReal x1652=((cj3)*(cj5));
07372 IkReal x1653=((r02)*(sj7));
07373 IkReal x1654=((cj5)*(sj7));
07374 IkReal x1655=((cj3)*(sj5));
07375 IkReal x1656=((IkReal(1.00000000000000))*(sj8));
07376 IkReal x1657=((cj8)*(r10));
07377 IkReal x1658=((cj7)*(cj8));
07378 IkReal x1659=((IkReal(1.00000000000000))*(sj5));
07379 IkReal x1660=((r12)*(sj3));
07380 IkReal x1661=((cj7)*(sj5));
07381 IkReal x1662=((cj5)*(cj7)*(sj3));
07382 IkReal x1663=((cj7)*(r01)*(x1656));
07383 if( IKabs(((gconst13)*(((((r21)*(sj8)*(x1661)))+(((r00)*(x1652)*(x1658)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1659)))+(((IkReal(-1.00000000000000))*(r11)*(x1656)*(x1662)))+(((x1654)*(x1660)))+(((IkReal(-1.00000000000000))*(x1652)*(x1663)))+(((x1657)*(x1662)))+(((IkReal(-1.00000000000000))*(r20)*(x1658)*(x1659)))+(((x1652)*(x1653))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst13)*(((((cj5)*(r20)*(x1658)))+(((r22)*(x1654)))+(((sj5)*(sj7)*(x1660)))+(((sj3)*(x1657)*(x1661)))+(((IkReal(-1.00000000000000))*(x1655)*(x1663)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(r21)*(x1656)))+(((r00)*(x1655)*(x1658)))+(((x1653)*(x1655)))+(((IkReal(-1.00000000000000))*(r11)*(sj3)*(x1656)*(x1661))))))) < IKFAST_ATAN2_MAGTHRESH )
07384 continue;
07385 j4array[0]=IKatan2(((gconst13)*(((((r21)*(sj8)*(x1661)))+(((r00)*(x1652)*(x1658)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1659)))+(((IkReal(-1.00000000000000))*(r11)*(x1656)*(x1662)))+(((x1654)*(x1660)))+(((IkReal(-1.00000000000000))*(x1652)*(x1663)))+(((x1657)*(x1662)))+(((IkReal(-1.00000000000000))*(r20)*(x1658)*(x1659)))+(((x1652)*(x1653)))))), ((gconst13)*(((((cj5)*(r20)*(x1658)))+(((r22)*(x1654)))+(((sj5)*(sj7)*(x1660)))+(((sj3)*(x1657)*(x1661)))+(((IkReal(-1.00000000000000))*(x1655)*(x1663)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(r21)*(x1656)))+(((r00)*(x1655)*(x1658)))+(((x1653)*(x1655)))+(((IkReal(-1.00000000000000))*(r11)*(sj3)*(x1656)*(x1661)))))));
07386 sj4array[0]=IKsin(j4array[0]);
07387 cj4array[0]=IKcos(j4array[0]);
07388 if( j4array[0] > IKPI )
07389 {
07390 j4array[0]-=IK2PI;
07391 }
07392 else if( j4array[0] < -IKPI )
07393 { j4array[0]+=IK2PI;
07394 }
07395 j4valid[0] = true;
07396 for(int ij4 = 0; ij4 < 1; ++ij4)
07397 {
07398 if( !j4valid[ij4] )
07399 {
07400 continue;
07401 }
07402 _ij4[0] = ij4; _ij4[1] = -1;
07403 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07404 {
07405 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07406 {
07407 j4valid[iij4]=false; _ij4[1] = iij4; break;
07408 }
07409 }
07410 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07411 {
07412 IkReal evalcond[4];
07413 IkReal x1664=IKcos(j4);
07414 IkReal x1665=IKsin(j4);
07415 IkReal x1666=((IkReal(1.00000000000000))*(cj7));
07416 IkReal x1667=((cj8)*(r20));
07417 IkReal x1668=((cj3)*(r02));
07418 IkReal x1669=((IkReal(1.00000000000000))*(sj7));
07419 IkReal x1670=((sj3)*(sj7));
07420 IkReal x1671=((r21)*(sj8));
07421 IkReal x1672=((cj8)*(r10));
07422 IkReal x1673=((sj5)*(x1664));
07423 IkReal x1674=((cj5)*(x1665));
07424 IkReal x1675=((r11)*(sj3)*(sj8));
07425 IkReal x1676=((cj3)*(cj8)*(r00));
07426 IkReal x1677=((cj5)*(x1664));
07427 IkReal x1678=((cj3)*(r01)*(sj8));
07428 IkReal x1679=((sj5)*(x1665));
07429 IkReal x1680=((x1674)+(x1673));
07430 evalcond[0]=((x1679)+(((IkReal(-1.00000000000000))*(r22)*(x1669)))+(((IkReal(-1.00000000000000))*(x1677)))+(((cj7)*(x1671)))+(((IkReal(-1.00000000000000))*(x1666)*(x1667))));
07431 evalcond[1]=((x1680)+(((IkReal(-1.00000000000000))*(x1667)*(x1669)))+(((sj7)*(x1671)))+(((cj7)*(r22))));
07432 evalcond[2]=((x1680)+(((cj7)*(sj3)*(x1672)))+(((IkReal(-1.00000000000000))*(x1666)*(x1675)))+(((IkReal(-1.00000000000000))*(x1666)*(x1678)))+(((cj7)*(x1676)))+(((sj7)*(x1668)))+(((r12)*(x1670))));
07433 evalcond[3]=((x1677)+(((IkReal(-1.00000000000000))*(x1669)*(x1678)))+(((IkReal(-1.00000000000000))*(x1669)*(x1675)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1666)))+(((sj7)*(x1676)))+(((IkReal(-1.00000000000000))*(x1679)))+(((IkReal(-1.00000000000000))*(x1666)*(x1668)))+(((x1670)*(x1672))));
07434 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07435 {
07436 continue;
07437 }
07438 }
07439
07440 {
07441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07442 vinfos[0].jointtype = 1;
07443 vinfos[0].foffset = j3;
07444 vinfos[0].indices[0] = _ij3[0];
07445 vinfos[0].indices[1] = _ij3[1];
07446 vinfos[0].maxsolutions = _nj3;
07447 vinfos[1].jointtype = 1;
07448 vinfos[1].foffset = j4;
07449 vinfos[1].indices[0] = _ij4[0];
07450 vinfos[1].indices[1] = _ij4[1];
07451 vinfos[1].maxsolutions = _nj4;
07452 vinfos[2].jointtype = 1;
07453 vinfos[2].foffset = j5;
07454 vinfos[2].indices[0] = _ij5[0];
07455 vinfos[2].indices[1] = _ij5[1];
07456 vinfos[2].maxsolutions = _nj5;
07457 vinfos[3].jointtype = 1;
07458 vinfos[3].foffset = j6;
07459 vinfos[3].indices[0] = _ij6[0];
07460 vinfos[3].indices[1] = _ij6[1];
07461 vinfos[3].maxsolutions = _nj6;
07462 vinfos[4].jointtype = 1;
07463 vinfos[4].foffset = j7;
07464 vinfos[4].indices[0] = _ij7[0];
07465 vinfos[4].indices[1] = _ij7[1];
07466 vinfos[4].maxsolutions = _nj7;
07467 vinfos[5].jointtype = 1;
07468 vinfos[5].foffset = j8;
07469 vinfos[5].indices[0] = _ij8[0];
07470 vinfos[5].indices[1] = _ij8[1];
07471 vinfos[5].maxsolutions = _nj8;
07472 std::vector<int> vfree(0);
07473 solutions.AddSolution(vinfos,vfree);
07474 }
07475 }
07476 }
07477
07478 }
07479
07480 }
07481
07482 } else
07483 {
07484 {
07485 IkReal j4array[1], cj4array[1], sj4array[1];
07486 bool j4valid[1]={false};
07487 _nj4 = 1;
07488 IkReal x1681=((sj5)*(sj7));
07489 IkReal x1682=((r21)*(sj8));
07490 IkReal x1683=((cj5)*(sj7));
07491 IkReal x1684=((cj8)*(r20));
07492 IkReal x1685=((cj5)*(cj7));
07493 IkReal x1686=((cj7)*(sj5));
07494 if( IKabs(((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(x1681)))+(((r22)*(x1685)))+(((IkReal(-1.00000000000000))*(x1684)*(x1686)))+(((IkReal(-1.00000000000000))*(x1683)*(x1684)))+(((x1682)*(x1683)))+(((x1682)*(x1686))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((x1681)*(x1682)))+(((r22)*(x1683)))+(((r22)*(x1686)))+(((IkReal(-1.00000000000000))*(x1681)*(x1684)))+(((x1684)*(x1685)))+(((IkReal(-1.00000000000000))*(x1682)*(x1685))))))) < IKFAST_ATAN2_MAGTHRESH )
07495 continue;
07496 j4array[0]=IKatan2(((gconst12)*(((((IkReal(-1.00000000000000))*(r22)*(x1681)))+(((r22)*(x1685)))+(((IkReal(-1.00000000000000))*(x1684)*(x1686)))+(((IkReal(-1.00000000000000))*(x1683)*(x1684)))+(((x1682)*(x1683)))+(((x1682)*(x1686)))))), ((gconst12)*(((((x1681)*(x1682)))+(((r22)*(x1683)))+(((r22)*(x1686)))+(((IkReal(-1.00000000000000))*(x1681)*(x1684)))+(((x1684)*(x1685)))+(((IkReal(-1.00000000000000))*(x1682)*(x1685)))))));
07497 sj4array[0]=IKsin(j4array[0]);
07498 cj4array[0]=IKcos(j4array[0]);
07499 if( j4array[0] > IKPI )
07500 {
07501 j4array[0]-=IK2PI;
07502 }
07503 else if( j4array[0] < -IKPI )
07504 { j4array[0]+=IK2PI;
07505 }
07506 j4valid[0] = true;
07507 for(int ij4 = 0; ij4 < 1; ++ij4)
07508 {
07509 if( !j4valid[ij4] )
07510 {
07511 continue;
07512 }
07513 _ij4[0] = ij4; _ij4[1] = -1;
07514 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07515 {
07516 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07517 {
07518 j4valid[iij4]=false; _ij4[1] = iij4; break;
07519 }
07520 }
07521 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07522 {
07523 IkReal evalcond[4];
07524 IkReal x1687=IKcos(j4);
07525 IkReal x1688=IKsin(j4);
07526 IkReal x1689=((IkReal(1.00000000000000))*(cj7));
07527 IkReal x1690=((cj8)*(r20));
07528 IkReal x1691=((cj3)*(r02));
07529 IkReal x1692=((IkReal(1.00000000000000))*(sj7));
07530 IkReal x1693=((sj3)*(sj7));
07531 IkReal x1694=((r21)*(sj8));
07532 IkReal x1695=((cj8)*(r10));
07533 IkReal x1696=((sj5)*(x1687));
07534 IkReal x1697=((cj5)*(x1688));
07535 IkReal x1698=((r11)*(sj3)*(sj8));
07536 IkReal x1699=((cj3)*(cj8)*(r00));
07537 IkReal x1700=((cj5)*(x1687));
07538 IkReal x1701=((cj3)*(r01)*(sj8));
07539 IkReal x1702=((sj5)*(x1688));
07540 IkReal x1703=((x1696)+(x1697));
07541 evalcond[0]=((x1702)+(((IkReal(-1.00000000000000))*(x1700)))+(((IkReal(-1.00000000000000))*(r22)*(x1692)))+(((cj7)*(x1694)))+(((IkReal(-1.00000000000000))*(x1689)*(x1690))));
07542 evalcond[1]=((x1703)+(((IkReal(-1.00000000000000))*(x1690)*(x1692)))+(((sj7)*(x1694)))+(((cj7)*(r22))));
07543 evalcond[2]=((((r12)*(x1693)))+(x1703)+(((sj7)*(x1691)))+(((IkReal(-1.00000000000000))*(x1689)*(x1701)))+(((cj7)*(sj3)*(x1695)))+(((cj7)*(x1699)))+(((IkReal(-1.00000000000000))*(x1689)*(x1698))));
07544 evalcond[3]=((((IkReal(-1.00000000000000))*(x1692)*(x1698)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1689)))+(x1700)+(((IkReal(-1.00000000000000))*(x1702)))+(((sj7)*(x1699)))+(((x1693)*(x1695)))+(((IkReal(-1.00000000000000))*(x1692)*(x1701)))+(((IkReal(-1.00000000000000))*(x1689)*(x1691))));
07545 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07546 {
07547 continue;
07548 }
07549 }
07550
07551 {
07552 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07553 vinfos[0].jointtype = 1;
07554 vinfos[0].foffset = j3;
07555 vinfos[0].indices[0] = _ij3[0];
07556 vinfos[0].indices[1] = _ij3[1];
07557 vinfos[0].maxsolutions = _nj3;
07558 vinfos[1].jointtype = 1;
07559 vinfos[1].foffset = j4;
07560 vinfos[1].indices[0] = _ij4[0];
07561 vinfos[1].indices[1] = _ij4[1];
07562 vinfos[1].maxsolutions = _nj4;
07563 vinfos[2].jointtype = 1;
07564 vinfos[2].foffset = j5;
07565 vinfos[2].indices[0] = _ij5[0];
07566 vinfos[2].indices[1] = _ij5[1];
07567 vinfos[2].maxsolutions = _nj5;
07568 vinfos[3].jointtype = 1;
07569 vinfos[3].foffset = j6;
07570 vinfos[3].indices[0] = _ij6[0];
07571 vinfos[3].indices[1] = _ij6[1];
07572 vinfos[3].maxsolutions = _nj6;
07573 vinfos[4].jointtype = 1;
07574 vinfos[4].foffset = j7;
07575 vinfos[4].indices[0] = _ij7[0];
07576 vinfos[4].indices[1] = _ij7[1];
07577 vinfos[4].maxsolutions = _nj7;
07578 vinfos[5].jointtype = 1;
07579 vinfos[5].foffset = j8;
07580 vinfos[5].indices[0] = _ij8[0];
07581 vinfos[5].indices[1] = _ij8[1];
07582 vinfos[5].maxsolutions = _nj8;
07583 std::vector<int> vfree(0);
07584 solutions.AddSolution(vinfos,vfree);
07585 }
07586 }
07587 }
07588
07589 }
07590
07591 }
07592
07593 } else
07594 {
07595 IkReal x1704=((cj3)*(cj8));
07596 IkReal x1705=((IkReal(1.00000000000000))*(sj7));
07597 IkReal x1706=((cj8)*(npx));
07598 IkReal x1707=((cj8)*(sj3));
07599 IkReal x1708=((npy)*(sj8));
07600 IkReal x1709=((r02)*(sj3));
07601 IkReal x1710=((IkReal(1.00000000000000))*(r11));
07602 IkReal x1711=((cj3)*(r12));
07603 IkReal x1712=((IkReal(1.00000000000000))*(cj7));
07604 IkReal x1713=((sj3)*(sj8));
07605 IkReal x1714=((IkReal(1.00000000000000))*(cj3)*(sj8));
07606 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j6)), IkReal(6.28318530717959))));
07607 evalcond[1]=((IkReal(0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
07608 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
07609 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((cj7)*(x1708)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(x1706)*(x1712)))+(((IkReal(-1.00000000000000))*(npz)*(x1705))));
07610 evalcond[4]=((IkReal(-1.00000000000000))+(((r01)*(x1707)))+(((r00)*(x1713)))+(((IkReal(-1.00000000000000))*(r10)*(x1714)))+(((IkReal(-1.00000000000000))*(x1704)*(x1710))));
07611 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(0.250000000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(x1705)*(x1706)))+(((cj7)*(npz)))+(((sj7)*(x1708))));
07612 evalcond[6]=((((IkReal(-1.00000000000000))*(r00)*(x1714)))+(((IkReal(-1.00000000000000))*(r01)*(x1704)))+(((IkReal(-1.00000000000000))*(x1707)*(x1710)))+(((IkReal(-1.00000000000000))*(r10)*(x1713))));
07613 evalcond[7]=((((cj7)*(r10)*(x1704)))+(((IkReal(-1.00000000000000))*(r00)*(x1707)*(x1712)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(sj8)*(x1710)))+(((IkReal(-1.00000000000000))*(x1705)*(x1709)))+(((sj7)*(x1711)))+(((cj7)*(r01)*(x1713))));
07614 evalcond[8]=((((IkReal(-1.00000000000000))*(cj3)*(r11)*(sj8)*(x1705)))+(((r01)*(sj7)*(x1713)))+(((cj7)*(x1709)))+(((IkReal(-1.00000000000000))*(x1711)*(x1712)))+(((IkReal(-1.00000000000000))*(r00)*(x1705)*(x1707)))+(((r10)*(sj7)*(x1704))));
07615 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 )
07616 {
07617 {
07618 IkReal dummyeval[1];
07619 IkReal gconst14;
07620 gconst14=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
07621 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
07622 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07623 {
07624 {
07625 IkReal dummyeval[1];
07626 IkReal gconst15;
07627 gconst15=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
07628 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
07629 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07630 {
07631 continue;
07632
07633 } else
07634 {
07635 {
07636 IkReal j4array[1], cj4array[1], sj4array[1];
07637 bool j4valid[1]={false};
07638 _nj4 = 1;
07639 IkReal x1715=((cj3)*(cj5));
07640 IkReal x1716=((r02)*(sj7));
07641 IkReal x1717=((cj8)*(r00));
07642 IkReal x1718=((cj5)*(sj7));
07643 IkReal x1719=((cj5)*(sj3));
07644 IkReal x1720=((sj3)*(sj5));
07645 IkReal x1721=((IkReal(1.00000000000000))*(sj5));
07646 IkReal x1722=((cj3)*(cj7)*(sj5));
07647 IkReal x1723=((IkReal(1.00000000000000))*(cj7)*(sj8));
07648 IkReal x1724=((cj7)*(cj8)*(r10));
07649 IkReal x1725=((cj7)*(cj8)*(r20));
07650 if( IKabs(((gconst15)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r01)*(x1715)*(x1723)))+(((r12)*(sj3)*(x1718)))+(((IkReal(-1.00000000000000))*(r11)*(x1719)*(x1723)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1721)))+(((x1715)*(x1716)))+(((x1719)*(x1724)))+(((cj7)*(x1715)*(x1717)))+(((IkReal(-1.00000000000000))*(x1721)*(x1725))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((r22)*(x1718)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x1721)))+(((IkReal(-1.00000000000000))*(r11)*(x1720)*(x1723)))+(((cj3)*(sj5)*(x1716)))+(((x1717)*(x1722)))+(((x1720)*(x1724)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1723)))+(((cj5)*(x1725)))+(((r12)*(sj7)*(x1720))))))) < IKFAST_ATAN2_MAGTHRESH )
07651 continue;
07652 j4array[0]=IKatan2(((gconst15)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r01)*(x1715)*(x1723)))+(((r12)*(sj3)*(x1718)))+(((IkReal(-1.00000000000000))*(r11)*(x1719)*(x1723)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x1721)))+(((x1715)*(x1716)))+(((x1719)*(x1724)))+(((cj7)*(x1715)*(x1717)))+(((IkReal(-1.00000000000000))*(x1721)*(x1725)))))), ((gconst15)*(((((r22)*(x1718)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x1721)))+(((IkReal(-1.00000000000000))*(r11)*(x1720)*(x1723)))+(((cj3)*(sj5)*(x1716)))+(((x1717)*(x1722)))+(((x1720)*(x1724)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x1723)))+(((cj5)*(x1725)))+(((r12)*(sj7)*(x1720)))))));
07653 sj4array[0]=IKsin(j4array[0]);
07654 cj4array[0]=IKcos(j4array[0]);
07655 if( j4array[0] > IKPI )
07656 {
07657 j4array[0]-=IK2PI;
07658 }
07659 else if( j4array[0] < -IKPI )
07660 { j4array[0]+=IK2PI;
07661 }
07662 j4valid[0] = true;
07663 for(int ij4 = 0; ij4 < 1; ++ij4)
07664 {
07665 if( !j4valid[ij4] )
07666 {
07667 continue;
07668 }
07669 _ij4[0] = ij4; _ij4[1] = -1;
07670 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07671 {
07672 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07673 {
07674 j4valid[iij4]=false; _ij4[1] = iij4; break;
07675 }
07676 }
07677 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07678 {
07679 IkReal evalcond[4];
07680 IkReal x1726=IKcos(j4);
07681 IkReal x1727=IKsin(j4);
07682 IkReal x1728=((IkReal(1.00000000000000))*(cj7));
07683 IkReal x1729=((cj8)*(r20));
07684 IkReal x1730=((cj3)*(r02));
07685 IkReal x1731=((IkReal(1.00000000000000))*(sj7));
07686 IkReal x1732=((sj3)*(sj7));
07687 IkReal x1733=((r21)*(sj8));
07688 IkReal x1734=((IkReal(1.00000000000000))*(cj5));
07689 IkReal x1735=((cj8)*(r10));
07690 IkReal x1736=((sj5)*(x1727));
07691 IkReal x1737=((sj5)*(x1726));
07692 IkReal x1738=((r11)*(sj3)*(sj8));
07693 IkReal x1739=((cj3)*(cj8)*(r00));
07694 IkReal x1740=((cj3)*(r01)*(sj8));
07695 IkReal x1741=((x1726)*(x1734));
07696 evalcond[0]=((x1736)+(((IkReal(-1.00000000000000))*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(x1741)))+(((cj7)*(x1733)))+(((IkReal(-1.00000000000000))*(r22)*(x1731))));
07697 evalcond[1]=((((sj7)*(x1733)))+(((IkReal(-1.00000000000000))*(x1729)*(x1731)))+(((IkReal(-1.00000000000000))*(x1737)))+(((IkReal(-1.00000000000000))*(x1727)*(x1734)))+(((cj7)*(r22))));
07698 evalcond[2]=((((IkReal(-1.00000000000000))*(x1728)*(x1738)))+(x1737)+(((sj7)*(x1730)))+(((IkReal(-1.00000000000000))*(x1728)*(x1740)))+(((cj7)*(sj3)*(x1735)))+(((r12)*(x1732)))+(((cj5)*(x1727)))+(((cj7)*(x1739))));
07699 evalcond[3]=((((IkReal(-1.00000000000000))*(x1731)*(x1738)))+(((IkReal(-1.00000000000000))*(x1728)*(x1730)))+(x1736)+(((sj7)*(x1739)))+(((IkReal(-1.00000000000000))*(x1741)))+(((x1732)*(x1735)))+(((IkReal(-1.00000000000000))*(x1731)*(x1740)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1728))));
07700 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07701 {
07702 continue;
07703 }
07704 }
07705
07706 {
07707 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07708 vinfos[0].jointtype = 1;
07709 vinfos[0].foffset = j3;
07710 vinfos[0].indices[0] = _ij3[0];
07711 vinfos[0].indices[1] = _ij3[1];
07712 vinfos[0].maxsolutions = _nj3;
07713 vinfos[1].jointtype = 1;
07714 vinfos[1].foffset = j4;
07715 vinfos[1].indices[0] = _ij4[0];
07716 vinfos[1].indices[1] = _ij4[1];
07717 vinfos[1].maxsolutions = _nj4;
07718 vinfos[2].jointtype = 1;
07719 vinfos[2].foffset = j5;
07720 vinfos[2].indices[0] = _ij5[0];
07721 vinfos[2].indices[1] = _ij5[1];
07722 vinfos[2].maxsolutions = _nj5;
07723 vinfos[3].jointtype = 1;
07724 vinfos[3].foffset = j6;
07725 vinfos[3].indices[0] = _ij6[0];
07726 vinfos[3].indices[1] = _ij6[1];
07727 vinfos[3].maxsolutions = _nj6;
07728 vinfos[4].jointtype = 1;
07729 vinfos[4].foffset = j7;
07730 vinfos[4].indices[0] = _ij7[0];
07731 vinfos[4].indices[1] = _ij7[1];
07732 vinfos[4].maxsolutions = _nj7;
07733 vinfos[5].jointtype = 1;
07734 vinfos[5].foffset = j8;
07735 vinfos[5].indices[0] = _ij8[0];
07736 vinfos[5].indices[1] = _ij8[1];
07737 vinfos[5].maxsolutions = _nj8;
07738 std::vector<int> vfree(0);
07739 solutions.AddSolution(vinfos,vfree);
07740 }
07741 }
07742 }
07743
07744 }
07745
07746 }
07747
07748 } else
07749 {
07750 {
07751 IkReal j4array[1], cj4array[1], sj4array[1];
07752 bool j4valid[1]={false};
07753 _nj4 = 1;
07754 IkReal x1742=((IkReal(1.00000000000000))*(sj5));
07755 IkReal x1743=((cj7)*(sj5));
07756 IkReal x1744=((cj8)*(r20));
07757 IkReal x1745=((cj5)*(cj7));
07758 IkReal x1746=((sj5)*(sj7));
07759 IkReal x1747=((r21)*(sj8));
07760 IkReal x1748=((cj7)*(x1747));
07761 IkReal x1749=((IkReal(1.00000000000000))*(cj5)*(sj7));
07762 if( IKabs(((gconst14)*(((((r22)*(x1745)))+(((r22)*(x1746)))+(((cj5)*(sj7)*(x1747)))+(((IkReal(-1.00000000000000))*(x1744)*(x1749)))+(((x1743)*(x1744)))+(((IkReal(-1.00000000000000))*(x1742)*(x1748))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst14)*(((((r22)*(x1743)))+(((x1745)*(x1747)))+(((x1746)*(x1747)))+(((IkReal(-1.00000000000000))*(r22)*(x1749)))+(((IkReal(-1.00000000000000))*(sj7)*(x1742)*(x1744)))+(((IkReal(-1.00000000000000))*(x1744)*(x1745))))))) < IKFAST_ATAN2_MAGTHRESH )
07763 continue;
07764 j4array[0]=IKatan2(((gconst14)*(((((r22)*(x1745)))+(((r22)*(x1746)))+(((cj5)*(sj7)*(x1747)))+(((IkReal(-1.00000000000000))*(x1744)*(x1749)))+(((x1743)*(x1744)))+(((IkReal(-1.00000000000000))*(x1742)*(x1748)))))), ((gconst14)*(((((r22)*(x1743)))+(((x1745)*(x1747)))+(((x1746)*(x1747)))+(((IkReal(-1.00000000000000))*(r22)*(x1749)))+(((IkReal(-1.00000000000000))*(sj7)*(x1742)*(x1744)))+(((IkReal(-1.00000000000000))*(x1744)*(x1745)))))));
07765 sj4array[0]=IKsin(j4array[0]);
07766 cj4array[0]=IKcos(j4array[0]);
07767 if( j4array[0] > IKPI )
07768 {
07769 j4array[0]-=IK2PI;
07770 }
07771 else if( j4array[0] < -IKPI )
07772 { j4array[0]+=IK2PI;
07773 }
07774 j4valid[0] = true;
07775 for(int ij4 = 0; ij4 < 1; ++ij4)
07776 {
07777 if( !j4valid[ij4] )
07778 {
07779 continue;
07780 }
07781 _ij4[0] = ij4; _ij4[1] = -1;
07782 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07783 {
07784 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07785 {
07786 j4valid[iij4]=false; _ij4[1] = iij4; break;
07787 }
07788 }
07789 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07790 {
07791 IkReal evalcond[4];
07792 IkReal x1750=IKcos(j4);
07793 IkReal x1751=IKsin(j4);
07794 IkReal x1752=((IkReal(1.00000000000000))*(cj7));
07795 IkReal x1753=((cj8)*(r20));
07796 IkReal x1754=((cj3)*(r02));
07797 IkReal x1755=((IkReal(1.00000000000000))*(sj7));
07798 IkReal x1756=((sj3)*(sj7));
07799 IkReal x1757=((r21)*(sj8));
07800 IkReal x1758=((IkReal(1.00000000000000))*(cj5));
07801 IkReal x1759=((cj8)*(r10));
07802 IkReal x1760=((sj5)*(x1751));
07803 IkReal x1761=((sj5)*(x1750));
07804 IkReal x1762=((r11)*(sj3)*(sj8));
07805 IkReal x1763=((cj3)*(cj8)*(r00));
07806 IkReal x1764=((cj3)*(r01)*(sj8));
07807 IkReal x1765=((x1750)*(x1758));
07808 evalcond[0]=((((IkReal(-1.00000000000000))*(x1765)))+(x1760)+(((IkReal(-1.00000000000000))*(r22)*(x1755)))+(((cj7)*(x1757)))+(((IkReal(-1.00000000000000))*(x1752)*(x1753))));
07809 evalcond[1]=((((IkReal(-1.00000000000000))*(x1761)))+(((sj7)*(x1757)))+(((IkReal(-1.00000000000000))*(x1751)*(x1758)))+(((IkReal(-1.00000000000000))*(x1753)*(x1755)))+(((cj7)*(r22))));
07810 evalcond[2]=((((IkReal(-1.00000000000000))*(x1752)*(x1764)))+(((IkReal(-1.00000000000000))*(x1752)*(x1762)))+(((cj5)*(x1751)))+(((cj7)*(x1763)))+(x1761)+(((cj7)*(sj3)*(x1759)))+(((sj7)*(x1754)))+(((r12)*(x1756))));
07811 evalcond[3]=((((sj7)*(x1763)))+(((IkReal(-1.00000000000000))*(x1765)))+(x1760)+(((IkReal(-1.00000000000000))*(x1755)*(x1762)))+(((IkReal(-1.00000000000000))*(x1755)*(x1764)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1752)))+(((IkReal(-1.00000000000000))*(x1752)*(x1754)))+(((x1756)*(x1759))));
07812 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07813 {
07814 continue;
07815 }
07816 }
07817
07818 {
07819 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07820 vinfos[0].jointtype = 1;
07821 vinfos[0].foffset = j3;
07822 vinfos[0].indices[0] = _ij3[0];
07823 vinfos[0].indices[1] = _ij3[1];
07824 vinfos[0].maxsolutions = _nj3;
07825 vinfos[1].jointtype = 1;
07826 vinfos[1].foffset = j4;
07827 vinfos[1].indices[0] = _ij4[0];
07828 vinfos[1].indices[1] = _ij4[1];
07829 vinfos[1].maxsolutions = _nj4;
07830 vinfos[2].jointtype = 1;
07831 vinfos[2].foffset = j5;
07832 vinfos[2].indices[0] = _ij5[0];
07833 vinfos[2].indices[1] = _ij5[1];
07834 vinfos[2].maxsolutions = _nj5;
07835 vinfos[3].jointtype = 1;
07836 vinfos[3].foffset = j6;
07837 vinfos[3].indices[0] = _ij6[0];
07838 vinfos[3].indices[1] = _ij6[1];
07839 vinfos[3].maxsolutions = _nj6;
07840 vinfos[4].jointtype = 1;
07841 vinfos[4].foffset = j7;
07842 vinfos[4].indices[0] = _ij7[0];
07843 vinfos[4].indices[1] = _ij7[1];
07844 vinfos[4].maxsolutions = _nj7;
07845 vinfos[5].jointtype = 1;
07846 vinfos[5].foffset = j8;
07847 vinfos[5].indices[0] = _ij8[0];
07848 vinfos[5].indices[1] = _ij8[1];
07849 vinfos[5].maxsolutions = _nj8;
07850 std::vector<int> vfree(0);
07851 solutions.AddSolution(vinfos,vfree);
07852 }
07853 }
07854 }
07855
07856 }
07857
07858 }
07859
07860 } else
07861 {
07862 if( 1 )
07863 {
07864 continue;
07865
07866 } else
07867 {
07868 }
07869 }
07870 }
07871 }
07872 }
07873 }
07874
07875 } else
07876 {
07877 {
07878 IkReal j4array[1], cj4array[1], sj4array[1];
07879 bool j4valid[1]={false};
07880 _nj4 = 1;
07881 IkReal x1766=((r21)*(sj8));
07882 IkReal x1767=((IkReal(1.00000000000000))*(r22));
07883 IkReal x1768=((cj5)*(sj7));
07884 IkReal x1769=((cj6)*(cj7));
07885 IkReal x1770=((sj5)*(sj7));
07886 IkReal x1771=((cj5)*(cj8)*(r20));
07887 IkReal x1772=((IkReal(1.00000000000000))*(x1769));
07888 IkReal x1773=((cj8)*(r20)*(sj5));
07889 if( IKabs(((gconst7)*(((((cj6)*(r22)*(x1770)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(x1767)))+(((cj8)*(r20)*(x1768)))+(((IkReal(-1.00000000000000))*(sj5)*(x1766)*(x1772)))+(((x1769)*(x1773)))+(((IkReal(-1.00000000000000))*(x1766)*(x1768))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x1766)*(x1770)))+(((cj8)*(r20)*(x1770)))+(((IkReal(-1.00000000000000))*(x1771)*(x1772)))+(((IkReal(-1.00000000000000))*(cj6)*(x1767)*(x1768)))+(((IkReal(-1.00000000000000))*(cj7)*(sj5)*(x1767)))+(((cj5)*(x1766)*(x1769))))))) < IKFAST_ATAN2_MAGTHRESH )
07890 continue;
07891 j4array[0]=IKatan2(((gconst7)*(((((cj6)*(r22)*(x1770)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(x1767)))+(((cj8)*(r20)*(x1768)))+(((IkReal(-1.00000000000000))*(sj5)*(x1766)*(x1772)))+(((x1769)*(x1773)))+(((IkReal(-1.00000000000000))*(x1766)*(x1768)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(x1766)*(x1770)))+(((cj8)*(r20)*(x1770)))+(((IkReal(-1.00000000000000))*(x1771)*(x1772)))+(((IkReal(-1.00000000000000))*(cj6)*(x1767)*(x1768)))+(((IkReal(-1.00000000000000))*(cj7)*(sj5)*(x1767)))+(((cj5)*(x1766)*(x1769)))))));
07892 sj4array[0]=IKsin(j4array[0]);
07893 cj4array[0]=IKcos(j4array[0]);
07894 if( j4array[0] > IKPI )
07895 {
07896 j4array[0]-=IK2PI;
07897 }
07898 else if( j4array[0] < -IKPI )
07899 { j4array[0]+=IK2PI;
07900 }
07901 j4valid[0] = true;
07902 for(int ij4 = 0; ij4 < 1; ++ij4)
07903 {
07904 if( !j4valid[ij4] )
07905 {
07906 continue;
07907 }
07908 _ij4[0] = ij4; _ij4[1] = -1;
07909 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
07910 {
07911 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
07912 {
07913 j4valid[iij4]=false; _ij4[1] = iij4; break;
07914 }
07915 }
07916 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
07917 {
07918 IkReal evalcond[6];
07919 IkReal x1774=IKsin(j4);
07920 IkReal x1775=IKcos(j4);
07921 IkReal x1776=((IkReal(1.00000000000000))*(cj8));
07922 IkReal x1777=((cj3)*(r01));
07923 IkReal x1778=((IkReal(1.00000000000000))*(sj8));
07924 IkReal x1779=((r11)*(sj3));
07925 IkReal x1780=((cj3)*(r00));
07926 IkReal x1781=((IkReal(1.00000000000000))*(sj6));
07927 IkReal x1782=((IkReal(1.00000000000000))*(cj7));
07928 IkReal x1783=((cj3)*(r02));
07929 IkReal x1784=((sj3)*(sj7));
07930 IkReal x1785=((cj7)*(cj8));
07931 IkReal x1786=((r21)*(sj8));
07932 IkReal x1787=((r10)*(sj3));
07933 IkReal x1788=((sj5)*(x1775));
07934 IkReal x1789=((cj5)*(x1775));
07935 IkReal x1790=((cj6)*(x1774));
07936 IkReal x1791=((cj5)*(x1774));
07937 IkReal x1792=((sj5)*(x1774));
07938 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x1781)*(x1788)))+(((IkReal(-1.00000000000000))*(x1781)*(x1791)))+(((r20)*(sj8))));
07939 evalcond[1]=((((cj7)*(x1786)))+(x1792)+(((IkReal(-1.00000000000000))*(x1789)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1776))));
07940 evalcond[2]=((((cj6)*(x1788)))+(((sj7)*(x1786)))+(((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1776)))+(((cj7)*(r22)))+(((cj5)*(x1790))));
07941 evalcond[3]=((((sj6)*(x1792)))+(((IkReal(-1.00000000000000))*(x1776)*(x1779)))+(((IkReal(-1.00000000000000))*(x1776)*(x1777)))+(((IkReal(-1.00000000000000))*(x1778)*(x1780)))+(((IkReal(-1.00000000000000))*(x1778)*(x1787)))+(((IkReal(-1.00000000000000))*(x1781)*(x1789))));
07942 evalcond[4]=((x1788)+(x1791)+(((IkReal(-1.00000000000000))*(cj7)*(x1777)*(x1778)))+(((IkReal(-1.00000000000000))*(cj7)*(x1778)*(x1779)))+(((sj7)*(x1783)))+(((r12)*(x1784)))+(((x1785)*(x1787)))+(((x1780)*(x1785))));
07943 evalcond[5]=((((cj6)*(x1789)))+(((IkReal(-1.00000000000000))*(sj5)*(x1790)))+(((cj8)*(r10)*(x1784)))+(((cj8)*(sj7)*(x1780)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1782)))+(((IkReal(-1.00000000000000))*(sj7)*(x1777)*(x1778)))+(((IkReal(-1.00000000000000))*(x1782)*(x1783)))+(((IkReal(-1.00000000000000))*(sj7)*(x1778)*(x1779))));
07944 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 )
07945 {
07946 continue;
07947 }
07948 }
07949
07950 {
07951 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07952 vinfos[0].jointtype = 1;
07953 vinfos[0].foffset = j3;
07954 vinfos[0].indices[0] = _ij3[0];
07955 vinfos[0].indices[1] = _ij3[1];
07956 vinfos[0].maxsolutions = _nj3;
07957 vinfos[1].jointtype = 1;
07958 vinfos[1].foffset = j4;
07959 vinfos[1].indices[0] = _ij4[0];
07960 vinfos[1].indices[1] = _ij4[1];
07961 vinfos[1].maxsolutions = _nj4;
07962 vinfos[2].jointtype = 1;
07963 vinfos[2].foffset = j5;
07964 vinfos[2].indices[0] = _ij5[0];
07965 vinfos[2].indices[1] = _ij5[1];
07966 vinfos[2].maxsolutions = _nj5;
07967 vinfos[3].jointtype = 1;
07968 vinfos[3].foffset = j6;
07969 vinfos[3].indices[0] = _ij6[0];
07970 vinfos[3].indices[1] = _ij6[1];
07971 vinfos[3].maxsolutions = _nj6;
07972 vinfos[4].jointtype = 1;
07973 vinfos[4].foffset = j7;
07974 vinfos[4].indices[0] = _ij7[0];
07975 vinfos[4].indices[1] = _ij7[1];
07976 vinfos[4].maxsolutions = _nj7;
07977 vinfos[5].jointtype = 1;
07978 vinfos[5].foffset = j8;
07979 vinfos[5].indices[0] = _ij8[0];
07980 vinfos[5].indices[1] = _ij8[1];
07981 vinfos[5].maxsolutions = _nj8;
07982 std::vector<int> vfree(0);
07983 solutions.AddSolution(vinfos,vfree);
07984 }
07985 }
07986 }
07987
07988 }
07989
07990 }
07991
07992 } else
07993 {
07994 {
07995 IkReal j4array[1], cj4array[1], sj4array[1];
07996 bool j4valid[1]={false};
07997 _nj4 = 1;
07998 IkReal x1793=((sj5)*(sj6));
07999 IkReal x1794=((r22)*(sj7));
08000 IkReal x1795=((r20)*(sj8));
08001 IkReal x1796=((cj8)*(r21));
08002 IkReal x1797=((cj5)*(sj6));
08003 IkReal x1798=((cj7)*(cj8)*(r20));
08004 IkReal x1799=((cj7)*(r21)*(sj8));
08005 if( IKabs(((gconst6)*(((((x1793)*(x1798)))+(((x1793)*(x1794)))+(((IkReal(-1.00000000000000))*(x1793)*(x1799)))+(((cj5)*(x1796)))+(((cj5)*(x1795))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((x1797)*(x1799)))+(((IkReal(-1.00000000000000))*(x1797)*(x1798)))+(((IkReal(-1.00000000000000))*(x1794)*(x1797)))+(((sj5)*(x1796)))+(((sj5)*(x1795))))))) < IKFAST_ATAN2_MAGTHRESH )
08006 continue;
08007 j4array[0]=IKatan2(((gconst6)*(((((x1793)*(x1798)))+(((x1793)*(x1794)))+(((IkReal(-1.00000000000000))*(x1793)*(x1799)))+(((cj5)*(x1796)))+(((cj5)*(x1795)))))), ((gconst6)*(((((x1797)*(x1799)))+(((IkReal(-1.00000000000000))*(x1797)*(x1798)))+(((IkReal(-1.00000000000000))*(x1794)*(x1797)))+(((sj5)*(x1796)))+(((sj5)*(x1795)))))));
08008 sj4array[0]=IKsin(j4array[0]);
08009 cj4array[0]=IKcos(j4array[0]);
08010 if( j4array[0] > IKPI )
08011 {
08012 j4array[0]-=IK2PI;
08013 }
08014 else if( j4array[0] < -IKPI )
08015 { j4array[0]+=IK2PI;
08016 }
08017 j4valid[0] = true;
08018 for(int ij4 = 0; ij4 < 1; ++ij4)
08019 {
08020 if( !j4valid[ij4] )
08021 {
08022 continue;
08023 }
08024 _ij4[0] = ij4; _ij4[1] = -1;
08025 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08026 {
08027 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08028 {
08029 j4valid[iij4]=false; _ij4[1] = iij4; break;
08030 }
08031 }
08032 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08033 {
08034 IkReal evalcond[6];
08035 IkReal x1800=IKsin(j4);
08036 IkReal x1801=IKcos(j4);
08037 IkReal x1802=((IkReal(1.00000000000000))*(cj8));
08038 IkReal x1803=((cj3)*(r01));
08039 IkReal x1804=((IkReal(1.00000000000000))*(sj8));
08040 IkReal x1805=((r11)*(sj3));
08041 IkReal x1806=((cj3)*(r00));
08042 IkReal x1807=((IkReal(1.00000000000000))*(sj6));
08043 IkReal x1808=((IkReal(1.00000000000000))*(cj7));
08044 IkReal x1809=((cj3)*(r02));
08045 IkReal x1810=((sj3)*(sj7));
08046 IkReal x1811=((cj7)*(cj8));
08047 IkReal x1812=((r21)*(sj8));
08048 IkReal x1813=((r10)*(sj3));
08049 IkReal x1814=((sj5)*(x1801));
08050 IkReal x1815=((cj5)*(x1801));
08051 IkReal x1816=((cj6)*(x1800));
08052 IkReal x1817=((cj5)*(x1800));
08053 IkReal x1818=((sj5)*(x1800));
08054 evalcond[0]=((((IkReal(-1.00000000000000))*(x1807)*(x1817)))+(((IkReal(-1.00000000000000))*(x1807)*(x1814)))+(((cj8)*(r21)))+(((r20)*(sj8))));
08055 evalcond[1]=((x1818)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x1802)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x1815)))+(((cj7)*(x1812))));
08056 evalcond[2]=((((IkReal(-1.00000000000000))*(r20)*(sj7)*(x1802)))+(((cj6)*(x1814)))+(((cj7)*(r22)))+(((cj5)*(x1816)))+(((sj7)*(x1812))));
08057 evalcond[3]=((((IkReal(-1.00000000000000))*(x1804)*(x1813)))+(((IkReal(-1.00000000000000))*(x1807)*(x1815)))+(((IkReal(-1.00000000000000))*(x1804)*(x1806)))+(((IkReal(-1.00000000000000))*(x1802)*(x1803)))+(((IkReal(-1.00000000000000))*(x1802)*(x1805)))+(((sj6)*(x1818))));
08058 evalcond[4]=((x1814)+(x1817)+(((IkReal(-1.00000000000000))*(cj7)*(x1804)*(x1805)))+(((sj7)*(x1809)))+(((x1811)*(x1813)))+(((x1806)*(x1811)))+(((IkReal(-1.00000000000000))*(cj7)*(x1803)*(x1804)))+(((r12)*(x1810))));
08059 evalcond[5]=((((IkReal(-1.00000000000000))*(sj5)*(x1816)))+(((cj8)*(r10)*(x1810)))+(((cj6)*(x1815)))+(((IkReal(-1.00000000000000))*(x1808)*(x1809)))+(((IkReal(-1.00000000000000))*(sj7)*(x1803)*(x1804)))+(((cj8)*(sj7)*(x1806)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x1808)))+(((IkReal(-1.00000000000000))*(sj7)*(x1804)*(x1805))));
08060 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 )
08061 {
08062 continue;
08063 }
08064 }
08065
08066 {
08067 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08068 vinfos[0].jointtype = 1;
08069 vinfos[0].foffset = j3;
08070 vinfos[0].indices[0] = _ij3[0];
08071 vinfos[0].indices[1] = _ij3[1];
08072 vinfos[0].maxsolutions = _nj3;
08073 vinfos[1].jointtype = 1;
08074 vinfos[1].foffset = j4;
08075 vinfos[1].indices[0] = _ij4[0];
08076 vinfos[1].indices[1] = _ij4[1];
08077 vinfos[1].maxsolutions = _nj4;
08078 vinfos[2].jointtype = 1;
08079 vinfos[2].foffset = j5;
08080 vinfos[2].indices[0] = _ij5[0];
08081 vinfos[2].indices[1] = _ij5[1];
08082 vinfos[2].maxsolutions = _nj5;
08083 vinfos[3].jointtype = 1;
08084 vinfos[3].foffset = j6;
08085 vinfos[3].indices[0] = _ij6[0];
08086 vinfos[3].indices[1] = _ij6[1];
08087 vinfos[3].maxsolutions = _nj6;
08088 vinfos[4].jointtype = 1;
08089 vinfos[4].foffset = j7;
08090 vinfos[4].indices[0] = _ij7[0];
08091 vinfos[4].indices[1] = _ij7[1];
08092 vinfos[4].maxsolutions = _nj7;
08093 vinfos[5].jointtype = 1;
08094 vinfos[5].foffset = j8;
08095 vinfos[5].indices[0] = _ij8[0];
08096 vinfos[5].indices[1] = _ij8[1];
08097 vinfos[5].maxsolutions = _nj8;
08098 std::vector<int> vfree(0);
08099 solutions.AddSolution(vinfos,vfree);
08100 }
08101 }
08102 }
08103
08104 }
08105
08106 }
08107 }
08108 }
08109
08110 }
08111
08112 }
08113
08114 } else
08115 {
08116 {
08117 IkReal j4array[1], cj4array[1], sj4array[1];
08118 bool j4valid[1]={false};
08119 _nj4 = 1;
08120 IkReal x1819=((r21)*(sj8));
08121 IkReal x1820=((IkReal(1.00000000000000))*(r22));
08122 IkReal x1821=((cj5)*(sj7));
08123 IkReal x1822=((cj6)*(cj7));
08124 IkReal x1823=((sj5)*(sj7));
08125 IkReal x1824=((cj5)*(cj8)*(r20));
08126 IkReal x1825=((IkReal(1.00000000000000))*(x1822));
08127 IkReal x1826=((cj8)*(r20)*(sj5));
08128 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x1819)*(x1821)))+(((x1822)*(x1826)))+(((cj8)*(r20)*(x1821)))+(((IkReal(-1.00000000000000))*(sj5)*(x1819)*(x1825)))+(((cj6)*(r22)*(x1823)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(x1820))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x1819)*(x1823)))+(((cj5)*(x1819)*(x1822)))+(((IkReal(-1.00000000000000))*(cj6)*(x1820)*(x1821)))+(((cj8)*(r20)*(x1823)))+(((IkReal(-1.00000000000000))*(x1824)*(x1825)))+(((IkReal(-1.00000000000000))*(cj7)*(sj5)*(x1820))))))) < IKFAST_ATAN2_MAGTHRESH )
08129 continue;
08130 j4array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x1819)*(x1821)))+(((x1822)*(x1826)))+(((cj8)*(r20)*(x1821)))+(((IkReal(-1.00000000000000))*(sj5)*(x1819)*(x1825)))+(((cj6)*(r22)*(x1823)))+(((IkReal(-1.00000000000000))*(cj5)*(cj7)*(x1820)))))), ((gconst5)*(((((IkReal(-1.00000000000000))*(x1819)*(x1823)))+(((cj5)*(x1819)*(x1822)))+(((IkReal(-1.00000000000000))*(cj6)*(x1820)*(x1821)))+(((cj8)*(r20)*(x1823)))+(((IkReal(-1.00000000000000))*(x1824)*(x1825)))+(((IkReal(-1.00000000000000))*(cj7)*(sj5)*(x1820)))))));
08131 sj4array[0]=IKsin(j4array[0]);
08132 cj4array[0]=IKcos(j4array[0]);
08133 if( j4array[0] > IKPI )
08134 {
08135 j4array[0]-=IK2PI;
08136 }
08137 else if( j4array[0] < -IKPI )
08138 { j4array[0]+=IK2PI;
08139 }
08140 j4valid[0] = true;
08141 for(int ij4 = 0; ij4 < 1; ++ij4)
08142 {
08143 if( !j4valid[ij4] )
08144 {
08145 continue;
08146 }
08147 _ij4[0] = ij4; _ij4[1] = -1;
08148 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08149 {
08150 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08151 {
08152 j4valid[iij4]=false; _ij4[1] = iij4; break;
08153 }
08154 }
08155 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08156 {
08157 IkReal evalcond[3];
08158 IkReal x1827=IKsin(j4);
08159 IkReal x1828=IKcos(j4);
08160 IkReal x1829=((IkReal(1.00000000000000))*(cj5));
08161 IkReal x1830=((r21)*(sj8));
08162 IkReal x1831=((IkReal(1.00000000000000))*(cj8)*(r20));
08163 IkReal x1832=((sj5)*(x1828));
08164 evalcond[0]=((((IkReal(-1.00000000000000))*(sj6)*(x1832)))+(((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(sj6)*(x1827)*(x1829)))+(((r20)*(sj8))));
08165 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(x1831)))+(((cj7)*(x1830)))+(((IkReal(-1.00000000000000))*(x1828)*(x1829)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((sj5)*(x1827))));
08166 evalcond[2]=((((cj6)*(x1832)))+(((sj7)*(x1830)))+(((cj5)*(cj6)*(x1827)))+(((IkReal(-1.00000000000000))*(sj7)*(x1831)))+(((cj7)*(r22))));
08167 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08168 {
08169 continue;
08170 }
08171 }
08172
08173 {
08174 IkReal dummyeval[1];
08175 IkReal gconst16;
08176 IkReal x1833=(sj8)*(sj8);
08177 IkReal x1834=(cj8)*(cj8);
08178 IkReal x1835=((r00)*(r11));
08179 IkReal x1836=((r02)*(sj7));
08180 IkReal x1837=((cj7)*(x1834));
08181 IkReal x1838=((IkReal(1.00000000000000))*(r12)*(sj7));
08182 IkReal x1839=((IkReal(1.00000000000000))*(r01)*(r10));
08183 IkReal x1840=((cj7)*(x1833));
08184 gconst16=IKsign(((((IkReal(-1.00000000000000))*(x1837)*(x1839)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1835)*(x1837)))+(((cj8)*(r11)*(x1836)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1838)))+(((r10)*(sj8)*(x1836)))+(((x1835)*(x1840)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1838)))));
08185 IkReal x1841=(sj8)*(sj8);
08186 IkReal x1842=(cj8)*(cj8);
08187 IkReal x1843=((r00)*(r11));
08188 IkReal x1844=((r02)*(sj7));
08189 IkReal x1845=((cj7)*(x1842));
08190 IkReal x1846=((IkReal(1.00000000000000))*(r12)*(sj7));
08191 IkReal x1847=((IkReal(1.00000000000000))*(r01)*(r10));
08192 IkReal x1848=((cj7)*(x1841));
08193 dummyeval[0]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1846)))+(((IkReal(-1.00000000000000))*(x1845)*(x1847)))+(((cj8)*(r11)*(x1844)))+(((IkReal(-1.00000000000000))*(x1847)*(x1848)))+(((x1843)*(x1848)))+(((x1843)*(x1845)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1846)))+(((r10)*(sj8)*(x1844))));
08194 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08195 {
08196 {
08197 IkReal dummyeval[1];
08198 IkReal gconst17;
08199 IkReal x1849=(sj8)*(sj8);
08200 IkReal x1850=(cj8)*(cj8);
08201 IkReal x1851=((IkReal(1.00000000000000))*(r10));
08202 IkReal x1852=((cj7)*(sj8));
08203 IkReal x1853=((r00)*(r11));
08204 IkReal x1854=((cj7)*(cj8));
08205 IkReal x1855=((sj7)*(x1849));
08206 IkReal x1856=((sj7)*(x1850));
08207 gconst17=IKsign(((((IkReal(-1.00000000000000))*(r01)*(x1851)*(x1856)))+(((IkReal(-1.00000000000000))*(r01)*(x1851)*(x1855)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1854)))+(((IkReal(-1.00000000000000))*(r02)*(x1851)*(x1852)))+(((x1853)*(x1855)))+(((x1853)*(x1856)))+(((r01)*(r12)*(x1854)))+(((r00)*(r12)*(x1852)))));
08208 IkReal x1857=(sj8)*(sj8);
08209 IkReal x1858=(cj8)*(cj8);
08210 IkReal x1859=((IkReal(1.00000000000000))*(r10));
08211 IkReal x1860=((cj7)*(sj8));
08212 IkReal x1861=((r00)*(r11));
08213 IkReal x1862=((cj7)*(cj8));
08214 IkReal x1863=((sj7)*(x1857));
08215 IkReal x1864=((sj7)*(x1858));
08216 dummyeval[0]=((((r01)*(r12)*(x1862)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1862)))+(((r00)*(r12)*(x1860)))+(((IkReal(-1.00000000000000))*(r01)*(x1859)*(x1863)))+(((IkReal(-1.00000000000000))*(r01)*(x1859)*(x1864)))+(((x1861)*(x1863)))+(((x1861)*(x1864)))+(((IkReal(-1.00000000000000))*(r02)*(x1859)*(x1860))));
08217 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08218 {
08219 continue;
08220
08221 } else
08222 {
08223 {
08224 IkReal j3array[1], cj3array[1], sj3array[1];
08225 bool j3valid[1]={false};
08226 _nj3 = 1;
08227 IkReal x1865=((cj6)*(sj7));
08228 IkReal x1866=((IkReal(1.00000000000000))*(sj8));
08229 IkReal x1867=((cj8)*(sj6));
08230 IkReal x1868=((sj6)*(sj8));
08231 IkReal x1869=((IkReal(1.00000000000000))*(cj6)*(cj7));
08232 if( IKabs(((gconst17)*(((((IkReal(-1.00000000000000))*(r11)*(x1865)*(x1866)))+(((r11)*(x1867)))+(((IkReal(-1.00000000000000))*(r12)*(x1869)))+(((r10)*(x1868)))+(((cj8)*(r10)*(x1865))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1865)*(x1866)))+(((IkReal(-1.00000000000000))*(r02)*(x1869)))+(((r01)*(x1867)))+(((cj8)*(r00)*(x1865)))+(((r00)*(x1868))))))) < IKFAST_ATAN2_MAGTHRESH )
08233 continue;
08234 j3array[0]=IKatan2(((gconst17)*(((((IkReal(-1.00000000000000))*(r11)*(x1865)*(x1866)))+(((r11)*(x1867)))+(((IkReal(-1.00000000000000))*(r12)*(x1869)))+(((r10)*(x1868)))+(((cj8)*(r10)*(x1865)))))), ((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1865)*(x1866)))+(((IkReal(-1.00000000000000))*(r02)*(x1869)))+(((r01)*(x1867)))+(((cj8)*(r00)*(x1865)))+(((r00)*(x1868)))))));
08235 sj3array[0]=IKsin(j3array[0]);
08236 cj3array[0]=IKcos(j3array[0]);
08237 if( j3array[0] > IKPI )
08238 {
08239 j3array[0]-=IK2PI;
08240 }
08241 else if( j3array[0] < -IKPI )
08242 { j3array[0]+=IK2PI;
08243 }
08244 j3valid[0] = true;
08245 for(int ij3 = 0; ij3 < 1; ++ij3)
08246 {
08247 if( !j3valid[ij3] )
08248 {
08249 continue;
08250 }
08251 _ij3[0] = ij3; _ij3[1] = -1;
08252 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08253 {
08254 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08255 {
08256 j3valid[iij3]=false; _ij3[1] = iij3; break;
08257 }
08258 }
08259 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08260 {
08261 IkReal evalcond[6];
08262 IkReal x1870=IKsin(j3);
08263 IkReal x1871=IKcos(j3);
08264 IkReal x1872=((cj4)*(cj5));
08265 IkReal x1873=((r01)*(sj8));
08266 IkReal x1874=((cj8)*(r00));
08267 IkReal x1875=((r00)*(sj8));
08268 IkReal x1876=((cj8)*(r11));
08269 IkReal x1877=((r11)*(sj8));
08270 IkReal x1878=((sj4)*(sj5));
08271 IkReal x1879=((r10)*(sj8));
08272 IkReal x1880=((cj8)*(r10));
08273 IkReal x1881=((cj7)*(x1870));
08274 IkReal x1882=((sj7)*(x1871));
08275 IkReal x1883=((IkReal(1.00000000000000))*(x1870));
08276 IkReal x1884=((IkReal(1.00000000000000))*(x1871));
08277 IkReal x1885=((cj8)*(x1870));
08278 IkReal x1886=((IkReal(1.00000000000000))*(x1877));
08279 IkReal x1887=((sj7)*(x1870));
08280 IkReal x1888=((cj7)*(x1871));
08281 evalcond[0]=((((IkReal(-1.00000000000000))*(x1876)*(x1884)))+(((IkReal(-1.00000000000000))*(x1879)*(x1884)))+(cj6)+(((r01)*(x1885)))+(((x1870)*(x1875))));
08282 evalcond[1]=((((sj6)*(x1878)))+(((IkReal(-1.00000000000000))*(x1876)*(x1883)))+(((IkReal(-1.00000000000000))*(x1879)*(x1883)))+(((IkReal(-1.00000000000000))*(sj6)*(x1872)))+(((IkReal(-1.00000000000000))*(x1875)*(x1884)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1884))));
08283 evalcond[2]=((((r12)*(x1882)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1883)))+(((x1880)*(x1888)))+(((IkReal(-1.00000000000000))*(x1874)*(x1881)))+(((IkReal(-1.00000000000000))*(cj7)*(x1877)*(x1884)))+(((x1873)*(x1881))));
08284 evalcond[3]=((((IkReal(-1.00000000000000))*(x1882)*(x1886)))+(sj6)+(((x1880)*(x1882)))+(((x1873)*(x1887)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1884)))+(((IkReal(-1.00000000000000))*(sj7)*(x1874)*(x1883)))+(((r02)*(x1881))));
08285 evalcond[4]=((((x1874)*(x1888)))+(((r12)*(x1887)))+(((x1880)*(x1881)))+(((cj5)*(sj4)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(x1881)*(x1886)))+(((IkReal(-1.00000000000000))*(cj7)*(x1873)*(x1884)))+(((r02)*(x1882))));
08286 evalcond[5]=((((IkReal(-1.00000000000000))*(x1873)*(x1882)))+(((IkReal(-1.00000000000000))*(sj7)*(x1877)*(x1883)))+(((x1874)*(x1882)))+(((IkReal(-1.00000000000000))*(cj6)*(x1878)))+(((x1880)*(x1887)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1884)))+(((cj6)*(x1872)))+(((IkReal(-1.00000000000000))*(r12)*(x1881))));
08287 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 )
08288 {
08289 continue;
08290 }
08291 }
08292
08293 {
08294 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08295 vinfos[0].jointtype = 1;
08296 vinfos[0].foffset = j3;
08297 vinfos[0].indices[0] = _ij3[0];
08298 vinfos[0].indices[1] = _ij3[1];
08299 vinfos[0].maxsolutions = _nj3;
08300 vinfos[1].jointtype = 1;
08301 vinfos[1].foffset = j4;
08302 vinfos[1].indices[0] = _ij4[0];
08303 vinfos[1].indices[1] = _ij4[1];
08304 vinfos[1].maxsolutions = _nj4;
08305 vinfos[2].jointtype = 1;
08306 vinfos[2].foffset = j5;
08307 vinfos[2].indices[0] = _ij5[0];
08308 vinfos[2].indices[1] = _ij5[1];
08309 vinfos[2].maxsolutions = _nj5;
08310 vinfos[3].jointtype = 1;
08311 vinfos[3].foffset = j6;
08312 vinfos[3].indices[0] = _ij6[0];
08313 vinfos[3].indices[1] = _ij6[1];
08314 vinfos[3].maxsolutions = _nj6;
08315 vinfos[4].jointtype = 1;
08316 vinfos[4].foffset = j7;
08317 vinfos[4].indices[0] = _ij7[0];
08318 vinfos[4].indices[1] = _ij7[1];
08319 vinfos[4].maxsolutions = _nj7;
08320 vinfos[5].jointtype = 1;
08321 vinfos[5].foffset = j8;
08322 vinfos[5].indices[0] = _ij8[0];
08323 vinfos[5].indices[1] = _ij8[1];
08324 vinfos[5].maxsolutions = _nj8;
08325 std::vector<int> vfree(0);
08326 solutions.AddSolution(vinfos,vfree);
08327 }
08328 }
08329 }
08330
08331 }
08332
08333 }
08334
08335 } else
08336 {
08337 {
08338 IkReal j3array[1], cj3array[1], sj3array[1];
08339 bool j3valid[1]={false};
08340 _nj3 = 1;
08341 IkReal x1889=((cj6)*(cj7));
08342 IkReal x1890=((IkReal(1.00000000000000))*(sj8));
08343 IkReal x1891=((cj6)*(sj7));
08344 if( IKabs(((gconst16)*(((((cj8)*(r10)*(x1889)))+(((IkReal(-1.00000000000000))*(r11)*(x1889)*(x1890)))+(((r12)*(x1891))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((r02)*(x1891)))+(((cj8)*(r00)*(x1889)))+(((IkReal(-1.00000000000000))*(r01)*(x1889)*(x1890))))))) < IKFAST_ATAN2_MAGTHRESH )
08345 continue;
08346 j3array[0]=IKatan2(((gconst16)*(((((cj8)*(r10)*(x1889)))+(((IkReal(-1.00000000000000))*(r11)*(x1889)*(x1890)))+(((r12)*(x1891)))))), ((gconst16)*(((((r02)*(x1891)))+(((cj8)*(r00)*(x1889)))+(((IkReal(-1.00000000000000))*(r01)*(x1889)*(x1890)))))));
08347 sj3array[0]=IKsin(j3array[0]);
08348 cj3array[0]=IKcos(j3array[0]);
08349 if( j3array[0] > IKPI )
08350 {
08351 j3array[0]-=IK2PI;
08352 }
08353 else if( j3array[0] < -IKPI )
08354 { j3array[0]+=IK2PI;
08355 }
08356 j3valid[0] = true;
08357 for(int ij3 = 0; ij3 < 1; ++ij3)
08358 {
08359 if( !j3valid[ij3] )
08360 {
08361 continue;
08362 }
08363 _ij3[0] = ij3; _ij3[1] = -1;
08364 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08365 {
08366 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08367 {
08368 j3valid[iij3]=false; _ij3[1] = iij3; break;
08369 }
08370 }
08371 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08372 {
08373 IkReal evalcond[6];
08374 IkReal x1892=IKsin(j3);
08375 IkReal x1893=IKcos(j3);
08376 IkReal x1894=((cj4)*(cj5));
08377 IkReal x1895=((r01)*(sj8));
08378 IkReal x1896=((cj8)*(r00));
08379 IkReal x1897=((r00)*(sj8));
08380 IkReal x1898=((cj8)*(r11));
08381 IkReal x1899=((r11)*(sj8));
08382 IkReal x1900=((sj4)*(sj5));
08383 IkReal x1901=((r10)*(sj8));
08384 IkReal x1902=((cj8)*(r10));
08385 IkReal x1903=((cj7)*(x1892));
08386 IkReal x1904=((sj7)*(x1893));
08387 IkReal x1905=((IkReal(1.00000000000000))*(x1892));
08388 IkReal x1906=((IkReal(1.00000000000000))*(x1893));
08389 IkReal x1907=((cj8)*(x1892));
08390 IkReal x1908=((IkReal(1.00000000000000))*(x1899));
08391 IkReal x1909=((sj7)*(x1892));
08392 IkReal x1910=((cj7)*(x1893));
08393 evalcond[0]=((((r01)*(x1907)))+(cj6)+(((x1892)*(x1897)))+(((IkReal(-1.00000000000000))*(x1901)*(x1906)))+(((IkReal(-1.00000000000000))*(x1898)*(x1906))));
08394 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1906)))+(((IkReal(-1.00000000000000))*(x1901)*(x1905)))+(((IkReal(-1.00000000000000))*(x1898)*(x1905)))+(((IkReal(-1.00000000000000))*(x1897)*(x1906)))+(((sj6)*(x1900)))+(((IkReal(-1.00000000000000))*(sj6)*(x1894))));
08395 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1905)))+(((IkReal(-1.00000000000000))*(cj7)*(x1899)*(x1906)))+(((r12)*(x1904)))+(((IkReal(-1.00000000000000))*(x1896)*(x1903)))+(((x1895)*(x1903)))+(((x1902)*(x1910))));
08396 evalcond[3]=((sj6)+(((IkReal(-1.00000000000000))*(x1904)*(x1908)))+(((x1895)*(x1909)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1906)))+(((x1902)*(x1904)))+(((r02)*(x1903)))+(((IkReal(-1.00000000000000))*(sj7)*(x1896)*(x1905))));
08397 evalcond[4]=((((IkReal(-1.00000000000000))*(x1903)*(x1908)))+(((cj5)*(sj4)))+(((r12)*(x1909)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(cj7)*(x1895)*(x1906)))+(((x1896)*(x1910)))+(((x1902)*(x1903)))+(((r02)*(x1904))));
08398 evalcond[5]=((((IkReal(-1.00000000000000))*(x1895)*(x1904)))+(((IkReal(-1.00000000000000))*(sj7)*(x1899)*(x1905)))+(((IkReal(-1.00000000000000))*(r12)*(x1903)))+(((cj6)*(x1894)))+(((IkReal(-1.00000000000000))*(cj6)*(x1900)))+(((x1902)*(x1909)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1906)))+(((x1896)*(x1904))));
08399 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 )
08400 {
08401 continue;
08402 }
08403 }
08404
08405 {
08406 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08407 vinfos[0].jointtype = 1;
08408 vinfos[0].foffset = j3;
08409 vinfos[0].indices[0] = _ij3[0];
08410 vinfos[0].indices[1] = _ij3[1];
08411 vinfos[0].maxsolutions = _nj3;
08412 vinfos[1].jointtype = 1;
08413 vinfos[1].foffset = j4;
08414 vinfos[1].indices[0] = _ij4[0];
08415 vinfos[1].indices[1] = _ij4[1];
08416 vinfos[1].maxsolutions = _nj4;
08417 vinfos[2].jointtype = 1;
08418 vinfos[2].foffset = j5;
08419 vinfos[2].indices[0] = _ij5[0];
08420 vinfos[2].indices[1] = _ij5[1];
08421 vinfos[2].maxsolutions = _nj5;
08422 vinfos[3].jointtype = 1;
08423 vinfos[3].foffset = j6;
08424 vinfos[3].indices[0] = _ij6[0];
08425 vinfos[3].indices[1] = _ij6[1];
08426 vinfos[3].maxsolutions = _nj6;
08427 vinfos[4].jointtype = 1;
08428 vinfos[4].foffset = j7;
08429 vinfos[4].indices[0] = _ij7[0];
08430 vinfos[4].indices[1] = _ij7[1];
08431 vinfos[4].maxsolutions = _nj7;
08432 vinfos[5].jointtype = 1;
08433 vinfos[5].foffset = j8;
08434 vinfos[5].indices[0] = _ij8[0];
08435 vinfos[5].indices[1] = _ij8[1];
08436 vinfos[5].maxsolutions = _nj8;
08437 std::vector<int> vfree(0);
08438 solutions.AddSolution(vinfos,vfree);
08439 }
08440 }
08441 }
08442
08443 }
08444
08445 }
08446 }
08447 }
08448
08449 }
08450
08451 }
08452
08453 } else
08454 {
08455 {
08456 IkReal j4array[1], cj4array[1], sj4array[1];
08457 bool j4valid[1]={false};
08458 _nj4 = 1;
08459 IkReal x1911=((sj5)*(sj6));
08460 IkReal x1912=((r22)*(sj7));
08461 IkReal x1913=((cj8)*(r21));
08462 IkReal x1914=((r20)*(sj8));
08463 IkReal x1915=((cj5)*(sj6));
08464 IkReal x1916=((cj7)*(cj8)*(r20));
08465 IkReal x1917=((cj7)*(r21)*(sj8));
08466 if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(x1911)*(x1917)))+(((x1911)*(x1916)))+(((x1911)*(x1912)))+(((cj5)*(x1913)))+(((cj5)*(x1914))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((sj5)*(x1913)))+(((sj5)*(x1914)))+(((IkReal(-1.00000000000000))*(x1912)*(x1915)))+(((IkReal(-1.00000000000000))*(x1915)*(x1916)))+(((x1915)*(x1917))))))) < IKFAST_ATAN2_MAGTHRESH )
08467 continue;
08468 j4array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(x1911)*(x1917)))+(((x1911)*(x1916)))+(((x1911)*(x1912)))+(((cj5)*(x1913)))+(((cj5)*(x1914)))))), ((gconst4)*(((((sj5)*(x1913)))+(((sj5)*(x1914)))+(((IkReal(-1.00000000000000))*(x1912)*(x1915)))+(((IkReal(-1.00000000000000))*(x1915)*(x1916)))+(((x1915)*(x1917)))))));
08469 sj4array[0]=IKsin(j4array[0]);
08470 cj4array[0]=IKcos(j4array[0]);
08471 if( j4array[0] > IKPI )
08472 {
08473 j4array[0]-=IK2PI;
08474 }
08475 else if( j4array[0] < -IKPI )
08476 { j4array[0]+=IK2PI;
08477 }
08478 j4valid[0] = true;
08479 for(int ij4 = 0; ij4 < 1; ++ij4)
08480 {
08481 if( !j4valid[ij4] )
08482 {
08483 continue;
08484 }
08485 _ij4[0] = ij4; _ij4[1] = -1;
08486 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08487 {
08488 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08489 {
08490 j4valid[iij4]=false; _ij4[1] = iij4; break;
08491 }
08492 }
08493 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08494 {
08495 IkReal evalcond[3];
08496 IkReal x1918=IKsin(j4);
08497 IkReal x1919=IKcos(j4);
08498 IkReal x1920=((IkReal(1.00000000000000))*(cj5));
08499 IkReal x1921=((r21)*(sj8));
08500 IkReal x1922=((IkReal(1.00000000000000))*(cj8)*(r20));
08501 IkReal x1923=((sj5)*(x1919));
08502 evalcond[0]=((((cj8)*(r21)))+(((r20)*(sj8)))+(((IkReal(-1.00000000000000))*(sj6)*(x1923)))+(((IkReal(-1.00000000000000))*(sj6)*(x1918)*(x1920))));
08503 evalcond[1]=((((sj5)*(x1918)))+(((cj7)*(x1921)))+(((IkReal(-1.00000000000000))*(cj7)*(x1922)))+(((IkReal(-1.00000000000000))*(x1919)*(x1920)))+(((IkReal(-1.00000000000000))*(r22)*(sj7))));
08504 evalcond[2]=((((sj7)*(x1921)))+(((cj5)*(cj6)*(x1918)))+(((IkReal(-1.00000000000000))*(sj7)*(x1922)))+(((cj6)*(x1923)))+(((cj7)*(r22))));
08505 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08506 {
08507 continue;
08508 }
08509 }
08510
08511 {
08512 IkReal dummyeval[1];
08513 IkReal gconst16;
08514 IkReal x1924=(sj8)*(sj8);
08515 IkReal x1925=(cj8)*(cj8);
08516 IkReal x1926=((r00)*(r11));
08517 IkReal x1927=((r02)*(sj7));
08518 IkReal x1928=((cj7)*(x1925));
08519 IkReal x1929=((IkReal(1.00000000000000))*(r12)*(sj7));
08520 IkReal x1930=((IkReal(1.00000000000000))*(r01)*(r10));
08521 IkReal x1931=((cj7)*(x1924));
08522 gconst16=IKsign(((((IkReal(-1.00000000000000))*(x1928)*(x1930)))+(((IkReal(-1.00000000000000))*(x1930)*(x1931)))+(((cj8)*(r11)*(x1927)))+(((r10)*(sj8)*(x1927)))+(((x1926)*(x1931)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1929)))+(((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1929)))+(((x1926)*(x1928)))));
08523 IkReal x1932=(sj8)*(sj8);
08524 IkReal x1933=(cj8)*(cj8);
08525 IkReal x1934=((r00)*(r11));
08526 IkReal x1935=((r02)*(sj7));
08527 IkReal x1936=((cj7)*(x1933));
08528 IkReal x1937=((IkReal(1.00000000000000))*(r12)*(sj7));
08529 IkReal x1938=((IkReal(1.00000000000000))*(r01)*(r10));
08530 IkReal x1939=((cj7)*(x1932));
08531 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(sj8)*(x1937)))+(((x1934)*(x1936)))+(((x1934)*(x1939)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1937)))+(((IkReal(-1.00000000000000))*(x1938)*(x1939)))+(((cj8)*(r11)*(x1935)))+(((IkReal(-1.00000000000000))*(x1936)*(x1938)))+(((r10)*(sj8)*(x1935))));
08532 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08533 {
08534 {
08535 IkReal dummyeval[1];
08536 IkReal gconst17;
08537 IkReal x1940=(sj8)*(sj8);
08538 IkReal x1941=(cj8)*(cj8);
08539 IkReal x1942=((IkReal(1.00000000000000))*(r10));
08540 IkReal x1943=((cj7)*(sj8));
08541 IkReal x1944=((r00)*(r11));
08542 IkReal x1945=((cj7)*(cj8));
08543 IkReal x1946=((sj7)*(x1940));
08544 IkReal x1947=((sj7)*(x1941));
08545 gconst17=IKsign(((((IkReal(-1.00000000000000))*(r02)*(r11)*(x1945)))+(((x1944)*(x1947)))+(((x1944)*(x1946)))+(((IkReal(-1.00000000000000))*(r01)*(x1942)*(x1946)))+(((IkReal(-1.00000000000000))*(r01)*(x1942)*(x1947)))+(((r00)*(r12)*(x1943)))+(((IkReal(-1.00000000000000))*(r02)*(x1942)*(x1943)))+(((r01)*(r12)*(x1945)))));
08546 IkReal x1948=(sj8)*(sj8);
08547 IkReal x1949=(cj8)*(cj8);
08548 IkReal x1950=((IkReal(1.00000000000000))*(r10));
08549 IkReal x1951=((cj7)*(sj8));
08550 IkReal x1952=((r00)*(r11));
08551 IkReal x1953=((cj7)*(cj8));
08552 IkReal x1954=((sj7)*(x1948));
08553 IkReal x1955=((sj7)*(x1949));
08554 dummyeval[0]=((((r01)*(r12)*(x1953)))+(((x1952)*(x1955)))+(((x1952)*(x1954)))+(((IkReal(-1.00000000000000))*(r01)*(x1950)*(x1954)))+(((IkReal(-1.00000000000000))*(r01)*(x1950)*(x1955)))+(((r00)*(r12)*(x1951)))+(((IkReal(-1.00000000000000))*(r02)*(x1950)*(x1951)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1953))));
08555 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08556 {
08557 continue;
08558
08559 } else
08560 {
08561 {
08562 IkReal j3array[1], cj3array[1], sj3array[1];
08563 bool j3valid[1]={false};
08564 _nj3 = 1;
08565 IkReal x1956=((cj6)*(sj7));
08566 IkReal x1957=((IkReal(1.00000000000000))*(sj8));
08567 IkReal x1958=((cj8)*(sj6));
08568 IkReal x1959=((sj6)*(sj8));
08569 IkReal x1960=((IkReal(1.00000000000000))*(cj6)*(cj7));
08570 if( IKabs(((gconst17)*(((((r11)*(x1958)))+(((r10)*(x1959)))+(((IkReal(-1.00000000000000))*(r11)*(x1956)*(x1957)))+(((IkReal(-1.00000000000000))*(r12)*(x1960)))+(((cj8)*(r10)*(x1956))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((cj8)*(r00)*(x1956)))+(((IkReal(-1.00000000000000))*(r02)*(x1960)))+(((IkReal(-1.00000000000000))*(r01)*(x1956)*(x1957)))+(((r01)*(x1958)))+(((r00)*(x1959))))))) < IKFAST_ATAN2_MAGTHRESH )
08571 continue;
08572 j3array[0]=IKatan2(((gconst17)*(((((r11)*(x1958)))+(((r10)*(x1959)))+(((IkReal(-1.00000000000000))*(r11)*(x1956)*(x1957)))+(((IkReal(-1.00000000000000))*(r12)*(x1960)))+(((cj8)*(r10)*(x1956)))))), ((gconst17)*(((((cj8)*(r00)*(x1956)))+(((IkReal(-1.00000000000000))*(r02)*(x1960)))+(((IkReal(-1.00000000000000))*(r01)*(x1956)*(x1957)))+(((r01)*(x1958)))+(((r00)*(x1959)))))));
08573 sj3array[0]=IKsin(j3array[0]);
08574 cj3array[0]=IKcos(j3array[0]);
08575 if( j3array[0] > IKPI )
08576 {
08577 j3array[0]-=IK2PI;
08578 }
08579 else if( j3array[0] < -IKPI )
08580 { j3array[0]+=IK2PI;
08581 }
08582 j3valid[0] = true;
08583 for(int ij3 = 0; ij3 < 1; ++ij3)
08584 {
08585 if( !j3valid[ij3] )
08586 {
08587 continue;
08588 }
08589 _ij3[0] = ij3; _ij3[1] = -1;
08590 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08591 {
08592 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08593 {
08594 j3valid[iij3]=false; _ij3[1] = iij3; break;
08595 }
08596 }
08597 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08598 {
08599 IkReal evalcond[6];
08600 IkReal x1961=IKsin(j3);
08601 IkReal x1962=IKcos(j3);
08602 IkReal x1963=((cj4)*(cj5));
08603 IkReal x1964=((r01)*(sj8));
08604 IkReal x1965=((cj8)*(r00));
08605 IkReal x1966=((r00)*(sj8));
08606 IkReal x1967=((cj8)*(r11));
08607 IkReal x1968=((r11)*(sj8));
08608 IkReal x1969=((sj4)*(sj5));
08609 IkReal x1970=((r10)*(sj8));
08610 IkReal x1971=((cj8)*(r10));
08611 IkReal x1972=((cj7)*(x1961));
08612 IkReal x1973=((sj7)*(x1962));
08613 IkReal x1974=((IkReal(1.00000000000000))*(x1961));
08614 IkReal x1975=((IkReal(1.00000000000000))*(x1962));
08615 IkReal x1976=((cj8)*(x1961));
08616 IkReal x1977=((IkReal(1.00000000000000))*(x1968));
08617 IkReal x1978=((sj7)*(x1961));
08618 IkReal x1979=((cj7)*(x1962));
08619 evalcond[0]=((cj6)+(((IkReal(-1.00000000000000))*(x1970)*(x1975)))+(((r01)*(x1976)))+(((x1961)*(x1966)))+(((IkReal(-1.00000000000000))*(x1967)*(x1975))));
08620 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1975)))+(((IkReal(-1.00000000000000))*(sj6)*(x1963)))+(((IkReal(-1.00000000000000))*(x1966)*(x1975)))+(((IkReal(-1.00000000000000))*(x1970)*(x1974)))+(((sj6)*(x1969)))+(((IkReal(-1.00000000000000))*(x1967)*(x1974))));
08621 evalcond[2]=((((r12)*(x1973)))+(((x1964)*(x1972)))+(((x1971)*(x1979)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1974)))+(((IkReal(-1.00000000000000))*(cj7)*(x1968)*(x1975)))+(((IkReal(-1.00000000000000))*(x1965)*(x1972))));
08622 evalcond[3]=((((r02)*(x1972)))+(sj6)+(((IkReal(-1.00000000000000))*(sj7)*(x1965)*(x1974)))+(((x1964)*(x1978)))+(((x1971)*(x1973)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1975)))+(((IkReal(-1.00000000000000))*(x1973)*(x1977))));
08623 evalcond[4]=((((r02)*(x1973)))+(((IkReal(-1.00000000000000))*(cj7)*(x1964)*(x1975)))+(((r12)*(x1978)))+(((x1971)*(x1972)))+(((cj5)*(sj4)))+(((IkReal(-1.00000000000000))*(x1972)*(x1977)))+(((cj4)*(sj5)))+(((x1965)*(x1979))));
08624 evalcond[5]=((((IkReal(-1.00000000000000))*(cj6)*(x1969)))+(((cj6)*(x1963)))+(((x1971)*(x1978)))+(((IkReal(-1.00000000000000))*(x1964)*(x1973)))+(((IkReal(-1.00000000000000))*(r12)*(x1972)))+(((x1965)*(x1973)))+(((IkReal(-1.00000000000000))*(sj7)*(x1968)*(x1974)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1975))));
08625 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 )
08626 {
08627 continue;
08628 }
08629 }
08630
08631 {
08632 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08633 vinfos[0].jointtype = 1;
08634 vinfos[0].foffset = j3;
08635 vinfos[0].indices[0] = _ij3[0];
08636 vinfos[0].indices[1] = _ij3[1];
08637 vinfos[0].maxsolutions = _nj3;
08638 vinfos[1].jointtype = 1;
08639 vinfos[1].foffset = j4;
08640 vinfos[1].indices[0] = _ij4[0];
08641 vinfos[1].indices[1] = _ij4[1];
08642 vinfos[1].maxsolutions = _nj4;
08643 vinfos[2].jointtype = 1;
08644 vinfos[2].foffset = j5;
08645 vinfos[2].indices[0] = _ij5[0];
08646 vinfos[2].indices[1] = _ij5[1];
08647 vinfos[2].maxsolutions = _nj5;
08648 vinfos[3].jointtype = 1;
08649 vinfos[3].foffset = j6;
08650 vinfos[3].indices[0] = _ij6[0];
08651 vinfos[3].indices[1] = _ij6[1];
08652 vinfos[3].maxsolutions = _nj6;
08653 vinfos[4].jointtype = 1;
08654 vinfos[4].foffset = j7;
08655 vinfos[4].indices[0] = _ij7[0];
08656 vinfos[4].indices[1] = _ij7[1];
08657 vinfos[4].maxsolutions = _nj7;
08658 vinfos[5].jointtype = 1;
08659 vinfos[5].foffset = j8;
08660 vinfos[5].indices[0] = _ij8[0];
08661 vinfos[5].indices[1] = _ij8[1];
08662 vinfos[5].maxsolutions = _nj8;
08663 std::vector<int> vfree(0);
08664 solutions.AddSolution(vinfos,vfree);
08665 }
08666 }
08667 }
08668
08669 }
08670
08671 }
08672
08673 } else
08674 {
08675 {
08676 IkReal j3array[1], cj3array[1], sj3array[1];
08677 bool j3valid[1]={false};
08678 _nj3 = 1;
08679 IkReal x1980=((cj6)*(cj7));
08680 IkReal x1981=((IkReal(1.00000000000000))*(sj8));
08681 IkReal x1982=((cj6)*(sj7));
08682 if( IKabs(((gconst16)*(((((cj8)*(r10)*(x1980)))+(((r12)*(x1982)))+(((IkReal(-1.00000000000000))*(r11)*(x1980)*(x1981))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((cj8)*(r00)*(x1980)))+(((IkReal(-1.00000000000000))*(r01)*(x1980)*(x1981)))+(((r02)*(x1982))))))) < IKFAST_ATAN2_MAGTHRESH )
08683 continue;
08684 j3array[0]=IKatan2(((gconst16)*(((((cj8)*(r10)*(x1980)))+(((r12)*(x1982)))+(((IkReal(-1.00000000000000))*(r11)*(x1980)*(x1981)))))), ((gconst16)*(((((cj8)*(r00)*(x1980)))+(((IkReal(-1.00000000000000))*(r01)*(x1980)*(x1981)))+(((r02)*(x1982)))))));
08685 sj3array[0]=IKsin(j3array[0]);
08686 cj3array[0]=IKcos(j3array[0]);
08687 if( j3array[0] > IKPI )
08688 {
08689 j3array[0]-=IK2PI;
08690 }
08691 else if( j3array[0] < -IKPI )
08692 { j3array[0]+=IK2PI;
08693 }
08694 j3valid[0] = true;
08695 for(int ij3 = 0; ij3 < 1; ++ij3)
08696 {
08697 if( !j3valid[ij3] )
08698 {
08699 continue;
08700 }
08701 _ij3[0] = ij3; _ij3[1] = -1;
08702 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08703 {
08704 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08705 {
08706 j3valid[iij3]=false; _ij3[1] = iij3; break;
08707 }
08708 }
08709 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08710 {
08711 IkReal evalcond[6];
08712 IkReal x1983=IKsin(j3);
08713 IkReal x1984=IKcos(j3);
08714 IkReal x1985=((cj4)*(cj5));
08715 IkReal x1986=((r01)*(sj8));
08716 IkReal x1987=((cj8)*(r00));
08717 IkReal x1988=((r00)*(sj8));
08718 IkReal x1989=((cj8)*(r11));
08719 IkReal x1990=((r11)*(sj8));
08720 IkReal x1991=((sj4)*(sj5));
08721 IkReal x1992=((r10)*(sj8));
08722 IkReal x1993=((cj8)*(r10));
08723 IkReal x1994=((cj7)*(x1983));
08724 IkReal x1995=((sj7)*(x1984));
08725 IkReal x1996=((IkReal(1.00000000000000))*(x1983));
08726 IkReal x1997=((IkReal(1.00000000000000))*(x1984));
08727 IkReal x1998=((cj8)*(x1983));
08728 IkReal x1999=((IkReal(1.00000000000000))*(x1990));
08729 IkReal x2000=((sj7)*(x1983));
08730 IkReal x2001=((cj7)*(x1984));
08731 evalcond[0]=((cj6)+(((IkReal(-1.00000000000000))*(x1989)*(x1997)))+(((IkReal(-1.00000000000000))*(x1992)*(x1997)))+(((r01)*(x1998)))+(((x1983)*(x1988))));
08732 evalcond[1]=((((IkReal(-1.00000000000000))*(sj6)*(x1985)))+(((IkReal(-1.00000000000000))*(x1989)*(x1996)))+(((IkReal(-1.00000000000000))*(cj8)*(r01)*(x1997)))+(((sj6)*(x1991)))+(((IkReal(-1.00000000000000))*(x1992)*(x1996)))+(((IkReal(-1.00000000000000))*(x1988)*(x1997))));
08733 evalcond[2]=((((IkReal(-1.00000000000000))*(x1987)*(x1994)))+(((r12)*(x1995)))+(((x1993)*(x2001)))+(((x1986)*(x1994)))+(((IkReal(-1.00000000000000))*(cj7)*(x1990)*(x1997)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x1996))));
08734 evalcond[3]=((sj6)+(((x1986)*(x2000)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x1997)))+(((IkReal(-1.00000000000000))*(sj7)*(x1987)*(x1996)))+(((r02)*(x1994)))+(((IkReal(-1.00000000000000))*(x1995)*(x1999)))+(((x1993)*(x1995))));
08735 evalcond[4]=((((r12)*(x2000)))+(((x1987)*(x2001)))+(((cj5)*(sj4)))+(((r02)*(x1995)))+(((cj4)*(sj5)))+(((IkReal(-1.00000000000000))*(cj7)*(x1986)*(x1997)))+(((IkReal(-1.00000000000000))*(x1994)*(x1999)))+(((x1993)*(x1994))));
08736 evalcond[5]=((((IkReal(-1.00000000000000))*(r12)*(x1994)))+(((IkReal(-1.00000000000000))*(cj6)*(x1991)))+(((x1993)*(x2000)))+(((cj6)*(x1985)))+(((IkReal(-1.00000000000000))*(cj7)*(r02)*(x1997)))+(((IkReal(-1.00000000000000))*(x1986)*(x1995)))+(((x1987)*(x1995)))+(((IkReal(-1.00000000000000))*(sj7)*(x1990)*(x1996))));
08737 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 )
08738 {
08739 continue;
08740 }
08741 }
08742
08743 {
08744 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08745 vinfos[0].jointtype = 1;
08746 vinfos[0].foffset = j3;
08747 vinfos[0].indices[0] = _ij3[0];
08748 vinfos[0].indices[1] = _ij3[1];
08749 vinfos[0].maxsolutions = _nj3;
08750 vinfos[1].jointtype = 1;
08751 vinfos[1].foffset = j4;
08752 vinfos[1].indices[0] = _ij4[0];
08753 vinfos[1].indices[1] = _ij4[1];
08754 vinfos[1].maxsolutions = _nj4;
08755 vinfos[2].jointtype = 1;
08756 vinfos[2].foffset = j5;
08757 vinfos[2].indices[0] = _ij5[0];
08758 vinfos[2].indices[1] = _ij5[1];
08759 vinfos[2].maxsolutions = _nj5;
08760 vinfos[3].jointtype = 1;
08761 vinfos[3].foffset = j6;
08762 vinfos[3].indices[0] = _ij6[0];
08763 vinfos[3].indices[1] = _ij6[1];
08764 vinfos[3].maxsolutions = _nj6;
08765 vinfos[4].jointtype = 1;
08766 vinfos[4].foffset = j7;
08767 vinfos[4].indices[0] = _ij7[0];
08768 vinfos[4].indices[1] = _ij7[1];
08769 vinfos[4].maxsolutions = _nj7;
08770 vinfos[5].jointtype = 1;
08771 vinfos[5].foffset = j8;
08772 vinfos[5].indices[0] = _ij8[0];
08773 vinfos[5].indices[1] = _ij8[1];
08774 vinfos[5].maxsolutions = _nj8;
08775 std::vector<int> vfree(0);
08776 solutions.AddSolution(vinfos,vfree);
08777 }
08778 }
08779 }
08780
08781 }
08782
08783 }
08784 }
08785 }
08786
08787 }
08788
08789 }
08790 }
08791 }
08792
08793 }
08794
08795 }
08796
08797 } else
08798 {
08799 {
08800 IkReal j3array[1], cj3array[1], sj3array[1];
08801 bool j3valid[1]={false};
08802 _nj3 = 1;
08803 IkReal x2002=((cj6)*(sj7));
08804 IkReal x2003=((IkReal(1.00000000000000))*(sj8));
08805 IkReal x2004=((cj8)*(sj6));
08806 IkReal x2005=((sj6)*(sj8));
08807 IkReal x2006=((IkReal(1.00000000000000))*(cj6)*(cj7));
08808 if( IKabs(((gconst1)*(((((r11)*(x2004)))+(((r10)*(x2005)))+(((IkReal(-1.00000000000000))*(r12)*(x2006)))+(((cj8)*(r10)*(x2002)))+(((IkReal(-1.00000000000000))*(r11)*(x2002)*(x2003))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((r01)*(x2004)))+(((r00)*(x2005)))+(((IkReal(-1.00000000000000))*(r02)*(x2006)))+(((cj8)*(r00)*(x2002)))+(((IkReal(-1.00000000000000))*(r01)*(x2002)*(x2003))))))) < IKFAST_ATAN2_MAGTHRESH )
08809 continue;
08810 j3array[0]=IKatan2(((gconst1)*(((((r11)*(x2004)))+(((r10)*(x2005)))+(((IkReal(-1.00000000000000))*(r12)*(x2006)))+(((cj8)*(r10)*(x2002)))+(((IkReal(-1.00000000000000))*(r11)*(x2002)*(x2003)))))), ((gconst1)*(((((r01)*(x2004)))+(((r00)*(x2005)))+(((IkReal(-1.00000000000000))*(r02)*(x2006)))+(((cj8)*(r00)*(x2002)))+(((IkReal(-1.00000000000000))*(r01)*(x2002)*(x2003)))))));
08811 sj3array[0]=IKsin(j3array[0]);
08812 cj3array[0]=IKcos(j3array[0]);
08813 if( j3array[0] > IKPI )
08814 {
08815 j3array[0]-=IK2PI;
08816 }
08817 else if( j3array[0] < -IKPI )
08818 { j3array[0]+=IK2PI;
08819 }
08820 j3valid[0] = true;
08821 for(int ij3 = 0; ij3 < 1; ++ij3)
08822 {
08823 if( !j3valid[ij3] )
08824 {
08825 continue;
08826 }
08827 _ij3[0] = ij3; _ij3[1] = -1;
08828 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
08829 {
08830 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
08831 {
08832 j3valid[iij3]=false; _ij3[1] = iij3; break;
08833 }
08834 }
08835 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
08836 {
08837 IkReal evalcond[3];
08838 IkReal x2007=IKsin(j3);
08839 IkReal x2008=IKcos(j3);
08840 IkReal x2009=((IkReal(1.00000000000000))*(r11));
08841 IkReal x2010=((r01)*(sj8));
08842 IkReal x2011=((cj8)*(r10));
08843 IkReal x2012=((IkReal(1.00000000000000))*(sj7));
08844 IkReal x2013=((sj7)*(x2008));
08845 IkReal x2014=((cj7)*(x2007));
08846 IkReal x2015=((r00)*(x2007));
08847 IkReal x2016=((cj7)*(x2008));
08848 evalcond[0]=((cj6)+(((IkReal(-1.00000000000000))*(cj8)*(x2008)*(x2009)))+(((sj8)*(x2015)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x2008)))+(((cj8)*(r01)*(x2007))));
08849 evalcond[1]=((((IkReal(-1.00000000000000))*(r02)*(x2007)*(x2012)))+(((IkReal(-1.00000000000000))*(sj8)*(x2009)*(x2016)))+(((x2011)*(x2016)))+(((x2010)*(x2014)))+(((r12)*(x2013)))+(((IkReal(-1.00000000000000))*(cj8)*(r00)*(x2014))));
08850 evalcond[2]=((sj6)+(((IkReal(-1.00000000000000))*(sj8)*(x2009)*(x2013)))+(((x2011)*(x2013)))+(((IkReal(-1.00000000000000))*(cj8)*(x2012)*(x2015)))+(((IkReal(-1.00000000000000))*(r12)*(x2016)))+(((sj7)*(x2007)*(x2010)))+(((r02)*(x2014))));
08851 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08852 {
08853 continue;
08854 }
08855 }
08856
08857 {
08858 IkReal dummyeval[1];
08859 dummyeval[0]=sj6;
08860 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08861 {
08862 {
08863 IkReal dummyeval[1];
08864 dummyeval[0]=cj6;
08865 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08866 {
08867 {
08868 IkReal evalcond[7];
08869 IkReal x2017=((IkReal(1.00000000000000))*(cj3));
08870 IkReal x2018=((cj7)*(r02));
08871 IkReal x2019=((cj8)*(sj7));
08872 IkReal x2020=((r01)*(sj3));
08873 IkReal x2021=((IkReal(1.00000000000000))*(sj3));
08874 IkReal x2022=((sj7)*(sj8));
08875 IkReal x2023=((cj7)*(r12));
08876 IkReal x2024=((cj7)*(cj8));
08877 IkReal x2025=((cj3)*(r10));
08878 IkReal x2026=((cj7)*(sj8));
08879 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j6)), IkReal(6.28318530717959))));
08880 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x2017)))+(((r00)*(sj3)*(sj8)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x2017)))+(((cj8)*(x2020))));
08881 evalcond[2]=((((IkReal(-1.00000000000000))*(r20)*(x2019)))+(((cj7)*(r22)))+(((r21)*(x2022))));
08882 evalcond[3]=((IkReal(-0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((npy)*(x2022)))+(((IkReal(-1.00000000000000))*(npx)*(x2019)))+(((cj7)*(npz))));
08883 evalcond[4]=((((x2020)*(x2026)))+(((x2024)*(x2025)))+(((IkReal(-1.00000000000000))*(r11)*(x2017)*(x2026)))+(((cj3)*(r12)*(sj7)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x2021)))+(((IkReal(-1.00000000000000))*(r00)*(x2021)*(x2024))));
08884 evalcond[5]=((IkReal(1.00000000000000))+(((x2020)*(x2022)))+(((IkReal(-1.00000000000000))*(r11)*(x2017)*(x2022)))+(((x2019)*(x2025)))+(((IkReal(-1.00000000000000))*(r00)*(x2019)*(x2021)))+(((IkReal(-1.00000000000000))*(x2017)*(x2023)))+(((sj3)*(x2018))));
08885 evalcond[6]=((((r10)*(sj3)*(x2019)))+(((IkReal(-1.00000000000000))*(x2021)*(x2023)))+(((cj3)*(r00)*(x2019)))+(((IkReal(-1.00000000000000))*(r11)*(x2021)*(x2022)))+(((IkReal(-1.00000000000000))*(r01)*(x2017)*(x2022)))+(((IkReal(-1.00000000000000))*(x2017)*(x2018))));
08886 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 )
08887 {
08888 {
08889 IkReal j5array[1], cj5array[1], sj5array[1];
08890 bool j5valid[1]={false};
08891 _nj5 = 1;
08892 IkReal x2027=((IkReal(4.00000000000000))*(npx));
08893 IkReal x2028=((IkReal(4.00000000000000))*(npy));
08894 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(cj8)*(x2028)))+(((IkReal(-1.00000000000000))*(sj8)*(x2027))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((cj7)*(cj8)*(x2027)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2028)))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(cj8)*(x2028)))+(((IkReal(-1.00000000000000))*(sj8)*(x2027)))))+IKsqr(((IkReal(-0.940000000000000))+(((cj7)*(cj8)*(x2027)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2028)))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
08895 continue;
08896 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(cj8)*(x2028)))+(((IkReal(-1.00000000000000))*(sj8)*(x2027)))), ((IkReal(-0.940000000000000))+(((cj7)*(cj8)*(x2027)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2028)))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-0.360000000000000))*(cj7)))));
08897 sj5array[0]=IKsin(j5array[0]);
08898 cj5array[0]=IKcos(j5array[0]);
08899 if( j5array[0] > IKPI )
08900 {
08901 j5array[0]-=IK2PI;
08902 }
08903 else if( j5array[0] < -IKPI )
08904 { j5array[0]+=IK2PI;
08905 }
08906 j5valid[0] = true;
08907 for(int ij5 = 0; ij5 < 1; ++ij5)
08908 {
08909 if( !j5valid[ij5] )
08910 {
08911 continue;
08912 }
08913 _ij5[0] = ij5; _ij5[1] = -1;
08914 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
08915 {
08916 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
08917 {
08918 j5valid[iij5]=false; _ij5[1] = iij5; break;
08919 }
08920 }
08921 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
08922 {
08923 IkReal evalcond[2];
08924 evalcond[0]=((IkReal(-0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(0.250000000000000))*(IKsin(j5)))));
08925 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(cj8)*(npx)))+(((cj7)*(npy)*(sj8)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
08926 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
08927 {
08928 continue;
08929 }
08930 }
08931
08932 {
08933 IkReal dummyeval[1];
08934 IkReal gconst56;
08935 gconst56=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
08936 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
08937 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08938 {
08939 {
08940 IkReal dummyeval[1];
08941 IkReal gconst57;
08942 gconst57=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
08943 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
08944 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08945 {
08946 continue;
08947
08948 } else
08949 {
08950 {
08951 IkReal j4array[1], cj4array[1], sj4array[1];
08952 bool j4valid[1]={false};
08953 _nj4 = 1;
08954 IkReal x2029=((cj8)*(sj5));
08955 IkReal x2030=((cj3)*(r01));
08956 IkReal x2031=((IkReal(1.00000000000000))*(cj5));
08957 IkReal x2032=((r11)*(sj3));
08958 IkReal x2033=((sj5)*(sj8));
08959 IkReal x2034=((r10)*(sj3));
08960 IkReal x2035=((cj3)*(r00)*(sj8));
08961 if( IKabs(((gconst57)*(((((x2033)*(x2034)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x2033)))+(((x2029)*(x2032)))+(((x2029)*(x2030)))+(((cj5)*(cj8)*(r21))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst57)*(((((r20)*(x2033)))+(((IkReal(-1.00000000000000))*(cj8)*(x2030)*(x2031)))+(((IkReal(-1.00000000000000))*(sj8)*(x2031)*(x2034)))+(((IkReal(-1.00000000000000))*(x2031)*(x2035)))+(((IkReal(-1.00000000000000))*(cj8)*(x2031)*(x2032)))+(((r21)*(x2029))))))) < IKFAST_ATAN2_MAGTHRESH )
08962 continue;
08963 j4array[0]=IKatan2(((gconst57)*(((((x2033)*(x2034)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x2033)))+(((x2029)*(x2032)))+(((x2029)*(x2030)))+(((cj5)*(cj8)*(r21)))))), ((gconst57)*(((((r20)*(x2033)))+(((IkReal(-1.00000000000000))*(cj8)*(x2030)*(x2031)))+(((IkReal(-1.00000000000000))*(sj8)*(x2031)*(x2034)))+(((IkReal(-1.00000000000000))*(x2031)*(x2035)))+(((IkReal(-1.00000000000000))*(cj8)*(x2031)*(x2032)))+(((r21)*(x2029)))))));
08964 sj4array[0]=IKsin(j4array[0]);
08965 cj4array[0]=IKcos(j4array[0]);
08966 if( j4array[0] > IKPI )
08967 {
08968 j4array[0]-=IK2PI;
08969 }
08970 else if( j4array[0] < -IKPI )
08971 { j4array[0]+=IK2PI;
08972 }
08973 j4valid[0] = true;
08974 for(int ij4 = 0; ij4 < 1; ++ij4)
08975 {
08976 if( !j4valid[ij4] )
08977 {
08978 continue;
08979 }
08980 _ij4[0] = ij4; _ij4[1] = -1;
08981 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
08982 {
08983 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
08984 {
08985 j4valid[iij4]=false; _ij4[1] = iij4; break;
08986 }
08987 }
08988 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
08989 {
08990 IkReal evalcond[4];
08991 IkReal x2036=IKsin(j4);
08992 IkReal x2037=IKcos(j4);
08993 IkReal x2038=((IkReal(1.00000000000000))*(cj8));
08994 IkReal x2039=((cj3)*(r01));
08995 IkReal x2040=((cj3)*(r00));
08996 IkReal x2041=((cj7)*(cj8));
08997 IkReal x2042=((IkReal(1.00000000000000))*(cj5));
08998 IkReal x2043=((IkReal(1.00000000000000))*(sj8));
08999 IkReal x2044=((r11)*(sj3));
09000 IkReal x2045=((r10)*(sj3));
09001 IkReal x2046=((sj5)*(x2036));
09002 IkReal x2047=((sj5)*(x2037));
09003 IkReal x2048=((x2037)*(x2042));
09004 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x2047)))+(((IkReal(-1.00000000000000))*(x2036)*(x2042)))+(((r20)*(sj8))));
09005 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2038)))+(((IkReal(-1.00000000000000))*(x2048)))+(x2046)+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
09006 evalcond[2]=((((IkReal(-1.00000000000000))*(x2048)))+(x2046)+(((IkReal(-1.00000000000000))*(x2040)*(x2043)))+(((IkReal(-1.00000000000000))*(x2043)*(x2045)))+(((IkReal(-1.00000000000000))*(x2038)*(x2039)))+(((IkReal(-1.00000000000000))*(x2038)*(x2044))));
09007 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((x2041)*(x2045)))+(((x2040)*(x2041)))+(x2047)+(((cj3)*(r02)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2043)*(x2044)))+(((IkReal(-1.00000000000000))*(cj7)*(x2039)*(x2043)))+(((cj5)*(x2036))));
09008 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09009 {
09010 continue;
09011 }
09012 }
09013
09014 {
09015 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09016 vinfos[0].jointtype = 1;
09017 vinfos[0].foffset = j3;
09018 vinfos[0].indices[0] = _ij3[0];
09019 vinfos[0].indices[1] = _ij3[1];
09020 vinfos[0].maxsolutions = _nj3;
09021 vinfos[1].jointtype = 1;
09022 vinfos[1].foffset = j4;
09023 vinfos[1].indices[0] = _ij4[0];
09024 vinfos[1].indices[1] = _ij4[1];
09025 vinfos[1].maxsolutions = _nj4;
09026 vinfos[2].jointtype = 1;
09027 vinfos[2].foffset = j5;
09028 vinfos[2].indices[0] = _ij5[0];
09029 vinfos[2].indices[1] = _ij5[1];
09030 vinfos[2].maxsolutions = _nj5;
09031 vinfos[3].jointtype = 1;
09032 vinfos[3].foffset = j6;
09033 vinfos[3].indices[0] = _ij6[0];
09034 vinfos[3].indices[1] = _ij6[1];
09035 vinfos[3].maxsolutions = _nj6;
09036 vinfos[4].jointtype = 1;
09037 vinfos[4].foffset = j7;
09038 vinfos[4].indices[0] = _ij7[0];
09039 vinfos[4].indices[1] = _ij7[1];
09040 vinfos[4].maxsolutions = _nj7;
09041 vinfos[5].jointtype = 1;
09042 vinfos[5].foffset = j8;
09043 vinfos[5].indices[0] = _ij8[0];
09044 vinfos[5].indices[1] = _ij8[1];
09045 vinfos[5].maxsolutions = _nj8;
09046 std::vector<int> vfree(0);
09047 solutions.AddSolution(vinfos,vfree);
09048 }
09049 }
09050 }
09051
09052 }
09053
09054 }
09055
09056 } else
09057 {
09058 {
09059 IkReal j4array[1], cj4array[1], sj4array[1];
09060 bool j4valid[1]={false};
09061 _nj4 = 1;
09062 IkReal x2049=((cj7)*(sj5));
09063 IkReal x2050=((r21)*(sj8));
09064 IkReal x2051=((cj8)*(r20));
09065 IkReal x2052=((cj5)*(cj7));
09066 IkReal x2053=((r22)*(sj7));
09067 IkReal x2054=((cj8)*(r21));
09068 IkReal x2055=((r20)*(sj8));
09069 if( IKabs(((gconst56)*(((((cj5)*(x2054)))+(((cj5)*(x2055)))+(((x2049)*(x2051)))+(((sj5)*(x2053)))+(((IkReal(-1.00000000000000))*(x2049)*(x2050))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst56)*(((((sj5)*(x2054)))+(((sj5)*(x2055)))+(((IkReal(-1.00000000000000))*(x2051)*(x2052)))+(((x2050)*(x2052)))+(((IkReal(-1.00000000000000))*(cj5)*(x2053))))))) < IKFAST_ATAN2_MAGTHRESH )
09070 continue;
09071 j4array[0]=IKatan2(((gconst56)*(((((cj5)*(x2054)))+(((cj5)*(x2055)))+(((x2049)*(x2051)))+(((sj5)*(x2053)))+(((IkReal(-1.00000000000000))*(x2049)*(x2050)))))), ((gconst56)*(((((sj5)*(x2054)))+(((sj5)*(x2055)))+(((IkReal(-1.00000000000000))*(x2051)*(x2052)))+(((x2050)*(x2052)))+(((IkReal(-1.00000000000000))*(cj5)*(x2053)))))));
09072 sj4array[0]=IKsin(j4array[0]);
09073 cj4array[0]=IKcos(j4array[0]);
09074 if( j4array[0] > IKPI )
09075 {
09076 j4array[0]-=IK2PI;
09077 }
09078 else if( j4array[0] < -IKPI )
09079 { j4array[0]+=IK2PI;
09080 }
09081 j4valid[0] = true;
09082 for(int ij4 = 0; ij4 < 1; ++ij4)
09083 {
09084 if( !j4valid[ij4] )
09085 {
09086 continue;
09087 }
09088 _ij4[0] = ij4; _ij4[1] = -1;
09089 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09090 {
09091 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09092 {
09093 j4valid[iij4]=false; _ij4[1] = iij4; break;
09094 }
09095 }
09096 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09097 {
09098 IkReal evalcond[4];
09099 IkReal x2056=IKsin(j4);
09100 IkReal x2057=IKcos(j4);
09101 IkReal x2058=((IkReal(1.00000000000000))*(cj8));
09102 IkReal x2059=((cj3)*(r01));
09103 IkReal x2060=((cj3)*(r00));
09104 IkReal x2061=((cj7)*(cj8));
09105 IkReal x2062=((IkReal(1.00000000000000))*(cj5));
09106 IkReal x2063=((IkReal(1.00000000000000))*(sj8));
09107 IkReal x2064=((r11)*(sj3));
09108 IkReal x2065=((r10)*(sj3));
09109 IkReal x2066=((sj5)*(x2056));
09110 IkReal x2067=((sj5)*(x2057));
09111 IkReal x2068=((x2057)*(x2062));
09112 evalcond[0]=((((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x2067)))+(((IkReal(-1.00000000000000))*(x2056)*(x2062)))+(((r20)*(sj8))));
09113 evalcond[1]=((x2066)+(((IkReal(-1.00000000000000))*(x2068)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2058))));
09114 evalcond[2]=((((IkReal(-1.00000000000000))*(x2058)*(x2059)))+(((IkReal(-1.00000000000000))*(x2063)*(x2065)))+(((IkReal(-1.00000000000000))*(x2058)*(x2064)))+(x2066)+(((IkReal(-1.00000000000000))*(x2068)))+(((IkReal(-1.00000000000000))*(x2060)*(x2063))));
09115 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((cj5)*(x2056)))+(x2067)+(((IkReal(-1.00000000000000))*(cj7)*(x2059)*(x2063)))+(((x2060)*(x2061)))+(((cj3)*(r02)*(sj7)))+(((x2061)*(x2065)))+(((IkReal(-1.00000000000000))*(cj7)*(x2063)*(x2064))));
09116 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09117 {
09118 continue;
09119 }
09120 }
09121
09122 {
09123 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09124 vinfos[0].jointtype = 1;
09125 vinfos[0].foffset = j3;
09126 vinfos[0].indices[0] = _ij3[0];
09127 vinfos[0].indices[1] = _ij3[1];
09128 vinfos[0].maxsolutions = _nj3;
09129 vinfos[1].jointtype = 1;
09130 vinfos[1].foffset = j4;
09131 vinfos[1].indices[0] = _ij4[0];
09132 vinfos[1].indices[1] = _ij4[1];
09133 vinfos[1].maxsolutions = _nj4;
09134 vinfos[2].jointtype = 1;
09135 vinfos[2].foffset = j5;
09136 vinfos[2].indices[0] = _ij5[0];
09137 vinfos[2].indices[1] = _ij5[1];
09138 vinfos[2].maxsolutions = _nj5;
09139 vinfos[3].jointtype = 1;
09140 vinfos[3].foffset = j6;
09141 vinfos[3].indices[0] = _ij6[0];
09142 vinfos[3].indices[1] = _ij6[1];
09143 vinfos[3].maxsolutions = _nj6;
09144 vinfos[4].jointtype = 1;
09145 vinfos[4].foffset = j7;
09146 vinfos[4].indices[0] = _ij7[0];
09147 vinfos[4].indices[1] = _ij7[1];
09148 vinfos[4].maxsolutions = _nj7;
09149 vinfos[5].jointtype = 1;
09150 vinfos[5].foffset = j8;
09151 vinfos[5].indices[0] = _ij8[0];
09152 vinfos[5].indices[1] = _ij8[1];
09153 vinfos[5].maxsolutions = _nj8;
09154 std::vector<int> vfree(0);
09155 solutions.AddSolution(vinfos,vfree);
09156 }
09157 }
09158 }
09159
09160 }
09161
09162 }
09163 }
09164 }
09165
09166 } else
09167 {
09168 IkReal x2069=((IkReal(1.00000000000000))*(cj3));
09169 IkReal x2070=((cj7)*(r02));
09170 IkReal x2071=((cj8)*(sj7));
09171 IkReal x2072=((r01)*(sj3));
09172 IkReal x2073=((IkReal(1.00000000000000))*(sj3));
09173 IkReal x2074=((sj7)*(sj8));
09174 IkReal x2075=((cj7)*(r12));
09175 IkReal x2076=((cj7)*(cj8));
09176 IkReal x2077=((cj3)*(r10));
09177 IkReal x2078=((cj7)*(sj8));
09178 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j6)), IkReal(6.28318530717959))));
09179 evalcond[1]=((((IkReal(-1.00000000000000))*(cj8)*(r11)*(x2069)))+(((r00)*(sj3)*(sj8)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x2069)))+(((cj8)*(x2072))));
09180 evalcond[2]=((((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(x2071)))+(((r21)*(x2074))));
09181 evalcond[3]=((IkReal(0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((npy)*(x2074)))+(((cj7)*(npz)))+(((IkReal(-1.00000000000000))*(npx)*(x2071))));
09182 evalcond[4]=((((IkReal(-1.00000000000000))*(r11)*(x2069)*(x2078)))+(((cj3)*(r12)*(sj7)))+(((IkReal(-1.00000000000000))*(r00)*(x2073)*(x2076)))+(((x2076)*(x2077)))+(((x2072)*(x2078)))+(((IkReal(-1.00000000000000))*(r02)*(sj7)*(x2073))));
09183 evalcond[5]=((IkReal(-1.00000000000000))+(((sj3)*(x2070)))+(((IkReal(-1.00000000000000))*(r00)*(x2071)*(x2073)))+(((IkReal(-1.00000000000000))*(r11)*(x2069)*(x2074)))+(((x2071)*(x2077)))+(((IkReal(-1.00000000000000))*(x2069)*(x2075)))+(((x2072)*(x2074))));
09184 evalcond[6]=((((cj3)*(r00)*(x2071)))+(((IkReal(-1.00000000000000))*(x2069)*(x2070)))+(((IkReal(-1.00000000000000))*(x2073)*(x2075)))+(((IkReal(-1.00000000000000))*(r01)*(x2069)*(x2074)))+(((IkReal(-1.00000000000000))*(r11)*(x2073)*(x2074)))+(((r10)*(sj3)*(x2071))));
09185 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 )
09186 {
09187 {
09188 IkReal j5array[1], cj5array[1], sj5array[1];
09189 bool j5valid[1]={false};
09190 _nj5 = 1;
09191 IkReal x2079=((IkReal(4.00000000000000))*(npx));
09192 IkReal x2080=((IkReal(4.00000000000000))*(npy));
09193 if( IKabs(((IkReal(0.120000000000000))+(((cj8)*(x2080)))+(((sj8)*(x2079))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2080)))+(((cj7)*(cj8)*(x2079)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((cj8)*(x2080)))+(((sj8)*(x2079)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2080)))+(((cj7)*(cj8)*(x2079)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
09194 continue;
09195 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((cj8)*(x2080)))+(((sj8)*(x2079)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(sj8)*(x2080)))+(((cj7)*(cj8)*(x2079)))+(((IkReal(-0.360000000000000))*(cj7)))));
09196 sj5array[0]=IKsin(j5array[0]);
09197 cj5array[0]=IKcos(j5array[0]);
09198 if( j5array[0] > IKPI )
09199 {
09200 j5array[0]-=IK2PI;
09201 }
09202 else if( j5array[0] < -IKPI )
09203 { j5array[0]+=IK2PI;
09204 }
09205 j5valid[0] = true;
09206 for(int ij5 = 0; ij5 < 1; ++ij5)
09207 {
09208 if( !j5valid[ij5] )
09209 {
09210 continue;
09211 }
09212 _ij5[0] = ij5; _ij5[1] = -1;
09213 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
09214 {
09215 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
09216 {
09217 j5valid[iij5]=false; _ij5[1] = iij5; break;
09218 }
09219 }
09220 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
09221 {
09222 IkReal evalcond[2];
09223 evalcond[0]=((IkReal(0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(-0.250000000000000))*(IKsin(j5)))));
09224 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(npz)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(cj8)*(npx)))+(((cj7)*(npy)*(sj8)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
09225 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09226 {
09227 continue;
09228 }
09229 }
09230
09231 {
09232 IkReal dummyeval[1];
09233 IkReal gconst58;
09234 gconst58=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
09235 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
09236 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09237 {
09238 {
09239 IkReal dummyeval[1];
09240 IkReal gconst59;
09241 gconst59=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
09242 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
09243 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09244 {
09245 continue;
09246
09247 } else
09248 {
09249 {
09250 IkReal j4array[1], cj4array[1], sj4array[1];
09251 bool j4valid[1]={false};
09252 _nj4 = 1;
09253 IkReal x2081=((IkReal(1.00000000000000))*(sj5));
09254 IkReal x2082=((r20)*(sj8));
09255 IkReal x2083=((cj5)*(cj8));
09256 IkReal x2084=((r11)*(sj3));
09257 IkReal x2085=((cj3)*(r01));
09258 IkReal x2086=((cj3)*(r00)*(sj8));
09259 IkReal x2087=((r10)*(sj3)*(sj8));
09260 if( IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(r21)*(x2083)))+(((IkReal(-1.00000000000000))*(x2081)*(x2086)))+(((IkReal(-1.00000000000000))*(x2081)*(x2087)))+(((IkReal(-1.00000000000000))*(cj8)*(x2081)*(x2085)))+(((IkReal(-1.00000000000000))*(cj8)*(x2081)*(x2084)))+(((IkReal(-1.00000000000000))*(cj5)*(x2082))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(x2081)*(x2082)))+(((cj5)*(x2087)))+(((cj5)*(x2086)))+(((x2083)*(x2084)))+(((x2083)*(x2085)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x2081))))))) < IKFAST_ATAN2_MAGTHRESH )
09261 continue;
09262 j4array[0]=IKatan2(((gconst59)*(((((IkReal(-1.00000000000000))*(r21)*(x2083)))+(((IkReal(-1.00000000000000))*(x2081)*(x2086)))+(((IkReal(-1.00000000000000))*(x2081)*(x2087)))+(((IkReal(-1.00000000000000))*(cj8)*(x2081)*(x2085)))+(((IkReal(-1.00000000000000))*(cj8)*(x2081)*(x2084)))+(((IkReal(-1.00000000000000))*(cj5)*(x2082)))))), ((gconst59)*(((((IkReal(-1.00000000000000))*(x2081)*(x2082)))+(((cj5)*(x2087)))+(((cj5)*(x2086)))+(((x2083)*(x2084)))+(((x2083)*(x2085)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x2081)))))));
09263 sj4array[0]=IKsin(j4array[0]);
09264 cj4array[0]=IKcos(j4array[0]);
09265 if( j4array[0] > IKPI )
09266 {
09267 j4array[0]-=IK2PI;
09268 }
09269 else if( j4array[0] < -IKPI )
09270 { j4array[0]+=IK2PI;
09271 }
09272 j4valid[0] = true;
09273 for(int ij4 = 0; ij4 < 1; ++ij4)
09274 {
09275 if( !j4valid[ij4] )
09276 {
09277 continue;
09278 }
09279 _ij4[0] = ij4; _ij4[1] = -1;
09280 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09281 {
09282 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09283 {
09284 j4valid[iij4]=false; _ij4[1] = iij4; break;
09285 }
09286 }
09287 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09288 {
09289 IkReal evalcond[4];
09290 IkReal x2088=IKsin(j4);
09291 IkReal x2089=IKcos(j4);
09292 IkReal x2090=((IkReal(1.00000000000000))*(cj8));
09293 IkReal x2091=((cj3)*(r01));
09294 IkReal x2092=((cj3)*(r00));
09295 IkReal x2093=((cj7)*(cj8));
09296 IkReal x2094=((r11)*(sj3));
09297 IkReal x2095=((IkReal(1.00000000000000))*(sj8));
09298 IkReal x2096=((r10)*(sj3));
09299 IkReal x2097=((sj5)*(x2089));
09300 IkReal x2098=((cj5)*(x2088));
09301 IkReal x2099=((cj5)*(x2089));
09302 IkReal x2100=((sj5)*(x2088));
09303 IkReal x2101=((x2097)+(x2098));
09304 evalcond[0]=((x2101)+(((cj8)*(r21)))+(((r20)*(sj8))));
09305 evalcond[1]=((x2100)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2090)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x2099)))+(((cj7)*(r21)*(sj8))));
09306 evalcond[2]=((((IkReal(-1.00000000000000))*(x2090)*(x2094)))+(((IkReal(-1.00000000000000))*(x2090)*(x2091)))+(x2099)+(((IkReal(-1.00000000000000))*(x2095)*(x2096)))+(((IkReal(-1.00000000000000))*(x2092)*(x2095)))+(((IkReal(-1.00000000000000))*(x2100))));
09307 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2094)*(x2095)))+(((IkReal(-1.00000000000000))*(cj7)*(x2091)*(x2095)))+(x2101)+(((x2092)*(x2093)))+(((cj3)*(r02)*(sj7)))+(((x2093)*(x2096))));
09308 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09309 {
09310 continue;
09311 }
09312 }
09313
09314 {
09315 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09316 vinfos[0].jointtype = 1;
09317 vinfos[0].foffset = j3;
09318 vinfos[0].indices[0] = _ij3[0];
09319 vinfos[0].indices[1] = _ij3[1];
09320 vinfos[0].maxsolutions = _nj3;
09321 vinfos[1].jointtype = 1;
09322 vinfos[1].foffset = j4;
09323 vinfos[1].indices[0] = _ij4[0];
09324 vinfos[1].indices[1] = _ij4[1];
09325 vinfos[1].maxsolutions = _nj4;
09326 vinfos[2].jointtype = 1;
09327 vinfos[2].foffset = j5;
09328 vinfos[2].indices[0] = _ij5[0];
09329 vinfos[2].indices[1] = _ij5[1];
09330 vinfos[2].maxsolutions = _nj5;
09331 vinfos[3].jointtype = 1;
09332 vinfos[3].foffset = j6;
09333 vinfos[3].indices[0] = _ij6[0];
09334 vinfos[3].indices[1] = _ij6[1];
09335 vinfos[3].maxsolutions = _nj6;
09336 vinfos[4].jointtype = 1;
09337 vinfos[4].foffset = j7;
09338 vinfos[4].indices[0] = _ij7[0];
09339 vinfos[4].indices[1] = _ij7[1];
09340 vinfos[4].maxsolutions = _nj7;
09341 vinfos[5].jointtype = 1;
09342 vinfos[5].foffset = j8;
09343 vinfos[5].indices[0] = _ij8[0];
09344 vinfos[5].indices[1] = _ij8[1];
09345 vinfos[5].maxsolutions = _nj8;
09346 std::vector<int> vfree(0);
09347 solutions.AddSolution(vinfos,vfree);
09348 }
09349 }
09350 }
09351
09352 }
09353
09354 }
09355
09356 } else
09357 {
09358 {
09359 IkReal j4array[1], cj4array[1], sj4array[1];
09360 bool j4valid[1]={false};
09361 _nj4 = 1;
09362 IkReal x2102=((r22)*(sj7));
09363 IkReal x2103=((cj8)*(sj5));
09364 IkReal x2104=((sj5)*(sj8));
09365 IkReal x2105=((cj7)*(r20));
09366 IkReal x2106=((cj5)*(cj8));
09367 IkReal x2107=((cj7)*(r21));
09368 IkReal x2108=((cj5)*(sj8));
09369 if( IKabs(((gconst58)*(((((r20)*(x2108)))+(((IkReal(-1.00000000000000))*(x2103)*(x2105)))+(((r21)*(x2106)))+(((x2104)*(x2107)))+(((IkReal(-1.00000000000000))*(sj5)*(x2102))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst58)*(((((r20)*(x2104)))+(((cj5)*(x2102)))+(((r21)*(x2103)))+(((IkReal(-1.00000000000000))*(x2107)*(x2108)))+(((x2105)*(x2106))))))) < IKFAST_ATAN2_MAGTHRESH )
09370 continue;
09371 j4array[0]=IKatan2(((gconst58)*(((((r20)*(x2108)))+(((IkReal(-1.00000000000000))*(x2103)*(x2105)))+(((r21)*(x2106)))+(((x2104)*(x2107)))+(((IkReal(-1.00000000000000))*(sj5)*(x2102)))))), ((gconst58)*(((((r20)*(x2104)))+(((cj5)*(x2102)))+(((r21)*(x2103)))+(((IkReal(-1.00000000000000))*(x2107)*(x2108)))+(((x2105)*(x2106)))))));
09372 sj4array[0]=IKsin(j4array[0]);
09373 cj4array[0]=IKcos(j4array[0]);
09374 if( j4array[0] > IKPI )
09375 {
09376 j4array[0]-=IK2PI;
09377 }
09378 else if( j4array[0] < -IKPI )
09379 { j4array[0]+=IK2PI;
09380 }
09381 j4valid[0] = true;
09382 for(int ij4 = 0; ij4 < 1; ++ij4)
09383 {
09384 if( !j4valid[ij4] )
09385 {
09386 continue;
09387 }
09388 _ij4[0] = ij4; _ij4[1] = -1;
09389 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09390 {
09391 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09392 {
09393 j4valid[iij4]=false; _ij4[1] = iij4; break;
09394 }
09395 }
09396 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09397 {
09398 IkReal evalcond[4];
09399 IkReal x2109=IKsin(j4);
09400 IkReal x2110=IKcos(j4);
09401 IkReal x2111=((IkReal(1.00000000000000))*(cj8));
09402 IkReal x2112=((cj3)*(r01));
09403 IkReal x2113=((cj3)*(r00));
09404 IkReal x2114=((cj7)*(cj8));
09405 IkReal x2115=((r11)*(sj3));
09406 IkReal x2116=((IkReal(1.00000000000000))*(sj8));
09407 IkReal x2117=((r10)*(sj3));
09408 IkReal x2118=((sj5)*(x2110));
09409 IkReal x2119=((cj5)*(x2109));
09410 IkReal x2120=((cj5)*(x2110));
09411 IkReal x2121=((sj5)*(x2109));
09412 IkReal x2122=((x2118)+(x2119));
09413 evalcond[0]=((x2122)+(((cj8)*(r21)))+(((r20)*(sj8))));
09414 evalcond[1]=((((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2111)))+(x2121)+(((IkReal(-1.00000000000000))*(x2120)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
09415 evalcond[2]=((((IkReal(-1.00000000000000))*(x2116)*(x2117)))+(x2120)+(((IkReal(-1.00000000000000))*(x2121)))+(((IkReal(-1.00000000000000))*(x2111)*(x2112)))+(((IkReal(-1.00000000000000))*(x2111)*(x2115)))+(((IkReal(-1.00000000000000))*(x2113)*(x2116))));
09416 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2115)*(x2116)))+(((IkReal(-1.00000000000000))*(cj7)*(x2112)*(x2116)))+(x2122)+(((cj3)*(r02)*(sj7)))+(((x2113)*(x2114)))+(((x2114)*(x2117))));
09417 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09418 {
09419 continue;
09420 }
09421 }
09422
09423 {
09424 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09425 vinfos[0].jointtype = 1;
09426 vinfos[0].foffset = j3;
09427 vinfos[0].indices[0] = _ij3[0];
09428 vinfos[0].indices[1] = _ij3[1];
09429 vinfos[0].maxsolutions = _nj3;
09430 vinfos[1].jointtype = 1;
09431 vinfos[1].foffset = j4;
09432 vinfos[1].indices[0] = _ij4[0];
09433 vinfos[1].indices[1] = _ij4[1];
09434 vinfos[1].maxsolutions = _nj4;
09435 vinfos[2].jointtype = 1;
09436 vinfos[2].foffset = j5;
09437 vinfos[2].indices[0] = _ij5[0];
09438 vinfos[2].indices[1] = _ij5[1];
09439 vinfos[2].maxsolutions = _nj5;
09440 vinfos[3].jointtype = 1;
09441 vinfos[3].foffset = j6;
09442 vinfos[3].indices[0] = _ij6[0];
09443 vinfos[3].indices[1] = _ij6[1];
09444 vinfos[3].maxsolutions = _nj6;
09445 vinfos[4].jointtype = 1;
09446 vinfos[4].foffset = j7;
09447 vinfos[4].indices[0] = _ij7[0];
09448 vinfos[4].indices[1] = _ij7[1];
09449 vinfos[4].maxsolutions = _nj7;
09450 vinfos[5].jointtype = 1;
09451 vinfos[5].foffset = j8;
09452 vinfos[5].indices[0] = _ij8[0];
09453 vinfos[5].indices[1] = _ij8[1];
09454 vinfos[5].maxsolutions = _nj8;
09455 std::vector<int> vfree(0);
09456 solutions.AddSolution(vinfos,vfree);
09457 }
09458 }
09459 }
09460
09461 }
09462
09463 }
09464 }
09465 }
09466
09467 } else
09468 {
09469 IkReal x2123=((cj3)*(cj8));
09470 IkReal x2124=((r01)*(sj3));
09471 IkReal x2125=((r02)*(sj3));
09472 IkReal x2126=((IkReal(1.00000000000000))*(sj7));
09473 IkReal x2127=((IkReal(1.00000000000000))*(r11));
09474 IkReal x2128=((cj3)*(r12));
09475 IkReal x2129=((IkReal(1.00000000000000))*(cj7));
09476 IkReal x2130=((cj8)*(sj3));
09477 IkReal x2131=((sj3)*(sj8));
09478 IkReal x2132=((IkReal(1.00000000000000))*(cj3)*(sj8));
09479 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j6)), IkReal(6.28318530717959))));
09480 evalcond[1]=((IkReal(-0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
09481 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
09482 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(x2131)))+(((cj8)*(x2124)))+(((IkReal(-1.00000000000000))*(r10)*(x2132)))+(((IkReal(-1.00000000000000))*(x2123)*(x2127))));
09483 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x2132)))+(((IkReal(-1.00000000000000))*(x2127)*(x2130)))+(((IkReal(-1.00000000000000))*(r10)*(x2131)))+(((IkReal(-1.00000000000000))*(r01)*(x2123))));
09484 evalcond[5]=((((IkReal(-1.00000000000000))*(r00)*(x2129)*(x2130)))+(((cj7)*(sj8)*(x2124)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(sj8)*(x2127)))+(((IkReal(-1.00000000000000))*(x2125)*(x2126)))+(((sj7)*(x2128)))+(((cj7)*(r10)*(x2123))));
09485 evalcond[6]=((((cj7)*(x2125)))+(((IkReal(-1.00000000000000))*(r00)*(x2126)*(x2130)))+(((sj7)*(sj8)*(x2124)))+(((IkReal(-1.00000000000000))*(x2128)*(x2129)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(sj8)*(x2126)))+(((r10)*(sj7)*(x2123))));
09486 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 )
09487 {
09488 {
09489 IkReal j5array[1], cj5array[1], sj5array[1];
09490 bool j5valid[1]={false};
09491 _nj5 = 1;
09492 IkReal x2133=((IkReal(4.00000000000000))*(sj7));
09493 IkReal x2134=((npy)*(sj8));
09494 IkReal x2135=((IkReal(4.00000000000000))*(cj7));
09495 IkReal x2136=((cj8)*(npx));
09496 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj7)))+(((npz)*(x2135)))+(((IkReal(-1.00000000000000))*(x2133)*(x2136)))+(((x2133)*(x2134))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x2134)*(x2135)))+(((x2135)*(x2136)))+(((npz)*(x2133)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj7)))+(((npz)*(x2135)))+(((IkReal(-1.00000000000000))*(x2133)*(x2136)))+(((x2133)*(x2134)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x2134)*(x2135)))+(((x2135)*(x2136)))+(((npz)*(x2133)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
09497 continue;
09498 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj7)))+(((npz)*(x2135)))+(((IkReal(-1.00000000000000))*(x2133)*(x2136)))+(((x2133)*(x2134)))), ((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(x2134)*(x2135)))+(((x2135)*(x2136)))+(((npz)*(x2133)))+(((IkReal(-0.360000000000000))*(cj7)))));
09499 sj5array[0]=IKsin(j5array[0]);
09500 cj5array[0]=IKcos(j5array[0]);
09501 if( j5array[0] > IKPI )
09502 {
09503 j5array[0]-=IK2PI;
09504 }
09505 else if( j5array[0] < -IKPI )
09506 { j5array[0]+=IK2PI;
09507 }
09508 j5valid[0] = true;
09509 for(int ij5 = 0; ij5 < 1; ++ij5)
09510 {
09511 if( !j5valid[ij5] )
09512 {
09513 continue;
09514 }
09515 _ij5[0] = ij5; _ij5[1] = -1;
09516 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
09517 {
09518 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
09519 {
09520 j5valid[iij5]=false; _ij5[1] = iij5; break;
09521 }
09522 }
09523 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
09524 {
09525 IkReal evalcond[2];
09526 IkReal x2137=((IkReal(1.00000000000000))*(sj7));
09527 IkReal x2138=((npy)*(sj8));
09528 IkReal x2139=((cj8)*(npx));
09529 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj7)*(x2139)))+(((IkReal(0.0900000000000000))*(cj7)))+(((cj7)*(x2138)))+(((IkReal(-1.00000000000000))*(npz)*(x2137)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
09530 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((sj7)*(x2138)))+(((IkReal(-0.250000000000000))*(IKsin(j5))))+(((IkReal(-1.00000000000000))*(x2137)*(x2139)))+(((cj7)*(npz))));
09531 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09532 {
09533 continue;
09534 }
09535 }
09536
09537 {
09538 IkReal dummyeval[1];
09539 IkReal gconst60;
09540 gconst60=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
09541 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
09542 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09543 {
09544 {
09545 IkReal dummyeval[1];
09546 IkReal gconst61;
09547 gconst61=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
09548 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
09549 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09550 {
09551 continue;
09552
09553 } else
09554 {
09555 {
09556 IkReal j4array[1], cj4array[1], sj4array[1];
09557 bool j4valid[1]={false};
09558 _nj4 = 1;
09559 IkReal x2140=((cj3)*(cj5));
09560 IkReal x2141=((r02)*(sj7));
09561 IkReal x2142=((cj8)*(r00));
09562 IkReal x2143=((cj5)*(sj7));
09563 IkReal x2144=((cj5)*(sj3));
09564 IkReal x2145=((cj7)*(cj8));
09565 IkReal x2146=((IkReal(1.00000000000000))*(sj5));
09566 IkReal x2147=((r12)*(sj3));
09567 IkReal x2148=((sj3)*(sj5));
09568 IkReal x2149=((cj3)*(cj7)*(sj5));
09569 IkReal x2150=((IkReal(1.00000000000000))*(cj7)*(sj8));
09570 if( IKabs(((gconst61)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r11)*(x2144)*(x2150)))+(((cj7)*(x2140)*(x2142)))+(((x2143)*(x2147)))+(((IkReal(-1.00000000000000))*(r01)*(x2140)*(x2150)))+(((x2140)*(x2141)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x2146)))+(((r10)*(x2144)*(x2145)))+(((IkReal(-1.00000000000000))*(r20)*(x2145)*(x2146))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst61)*(((((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x2146)))+(((sj5)*(sj7)*(x2147)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x2146)))+(((r10)*(x2145)*(x2148)))+(((cj5)*(r20)*(x2145)))+(((x2142)*(x2149)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2150)))+(((r22)*(x2143)))+(((cj3)*(sj5)*(x2141))))))) < IKFAST_ATAN2_MAGTHRESH )
09571 continue;
09572 j4array[0]=IKatan2(((gconst61)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r11)*(x2144)*(x2150)))+(((cj7)*(x2140)*(x2142)))+(((x2143)*(x2147)))+(((IkReal(-1.00000000000000))*(r01)*(x2140)*(x2150)))+(((x2140)*(x2141)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x2146)))+(((r10)*(x2144)*(x2145)))+(((IkReal(-1.00000000000000))*(r20)*(x2145)*(x2146)))))), ((gconst61)*(((((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x2146)))+(((sj5)*(sj7)*(x2147)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x2146)))+(((r10)*(x2145)*(x2148)))+(((cj5)*(r20)*(x2145)))+(((x2142)*(x2149)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2150)))+(((r22)*(x2143)))+(((cj3)*(sj5)*(x2141)))))));
09573 sj4array[0]=IKsin(j4array[0]);
09574 cj4array[0]=IKcos(j4array[0]);
09575 if( j4array[0] > IKPI )
09576 {
09577 j4array[0]-=IK2PI;
09578 }
09579 else if( j4array[0] < -IKPI )
09580 { j4array[0]+=IK2PI;
09581 }
09582 j4valid[0] = true;
09583 for(int ij4 = 0; ij4 < 1; ++ij4)
09584 {
09585 if( !j4valid[ij4] )
09586 {
09587 continue;
09588 }
09589 _ij4[0] = ij4; _ij4[1] = -1;
09590 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09591 {
09592 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09593 {
09594 j4valid[iij4]=false; _ij4[1] = iij4; break;
09595 }
09596 }
09597 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09598 {
09599 IkReal evalcond[4];
09600 IkReal x2151=IKcos(j4);
09601 IkReal x2152=IKsin(j4);
09602 IkReal x2153=((IkReal(1.00000000000000))*(cj7));
09603 IkReal x2154=((cj8)*(r20));
09604 IkReal x2155=((cj3)*(r02));
09605 IkReal x2156=((IkReal(1.00000000000000))*(sj7));
09606 IkReal x2157=((sj3)*(sj7));
09607 IkReal x2158=((r21)*(sj8));
09608 IkReal x2159=((cj8)*(r10));
09609 IkReal x2160=((sj5)*(x2151));
09610 IkReal x2161=((cj5)*(x2152));
09611 IkReal x2162=((r11)*(sj3)*(sj8));
09612 IkReal x2163=((cj3)*(cj8)*(r00));
09613 IkReal x2164=((cj5)*(x2151));
09614 IkReal x2165=((cj3)*(r01)*(sj8));
09615 IkReal x2166=((sj5)*(x2152));
09616 IkReal x2167=((x2161)+(x2160));
09617 evalcond[0]=((x2166)+(((IkReal(-1.00000000000000))*(x2153)*(x2154)))+(((cj7)*(x2158)))+(((IkReal(-1.00000000000000))*(r22)*(x2156)))+(((IkReal(-1.00000000000000))*(x2164))));
09618 evalcond[1]=((x2167)+(((sj7)*(x2158)))+(((IkReal(-1.00000000000000))*(x2154)*(x2156)))+(((cj7)*(r22))));
09619 evalcond[2]=((((IkReal(-1.00000000000000))*(x2153)*(x2162)))+(((IkReal(-1.00000000000000))*(x2153)*(x2165)))+(((cj7)*(x2163)))+(x2167)+(((sj7)*(x2155)))+(((r12)*(x2157)))+(((cj7)*(sj3)*(x2159))));
09620 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2153)))+(((sj7)*(x2163)))+(x2164)+(((IkReal(-1.00000000000000))*(x2153)*(x2155)))+(((IkReal(-1.00000000000000))*(x2156)*(x2162)))+(((IkReal(-1.00000000000000))*(x2156)*(x2165)))+(((IkReal(-1.00000000000000))*(x2166)))+(((x2157)*(x2159))));
09621 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09622 {
09623 continue;
09624 }
09625 }
09626
09627 {
09628 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09629 vinfos[0].jointtype = 1;
09630 vinfos[0].foffset = j3;
09631 vinfos[0].indices[0] = _ij3[0];
09632 vinfos[0].indices[1] = _ij3[1];
09633 vinfos[0].maxsolutions = _nj3;
09634 vinfos[1].jointtype = 1;
09635 vinfos[1].foffset = j4;
09636 vinfos[1].indices[0] = _ij4[0];
09637 vinfos[1].indices[1] = _ij4[1];
09638 vinfos[1].maxsolutions = _nj4;
09639 vinfos[2].jointtype = 1;
09640 vinfos[2].foffset = j5;
09641 vinfos[2].indices[0] = _ij5[0];
09642 vinfos[2].indices[1] = _ij5[1];
09643 vinfos[2].maxsolutions = _nj5;
09644 vinfos[3].jointtype = 1;
09645 vinfos[3].foffset = j6;
09646 vinfos[3].indices[0] = _ij6[0];
09647 vinfos[3].indices[1] = _ij6[1];
09648 vinfos[3].maxsolutions = _nj6;
09649 vinfos[4].jointtype = 1;
09650 vinfos[4].foffset = j7;
09651 vinfos[4].indices[0] = _ij7[0];
09652 vinfos[4].indices[1] = _ij7[1];
09653 vinfos[4].maxsolutions = _nj7;
09654 vinfos[5].jointtype = 1;
09655 vinfos[5].foffset = j8;
09656 vinfos[5].indices[0] = _ij8[0];
09657 vinfos[5].indices[1] = _ij8[1];
09658 vinfos[5].maxsolutions = _nj8;
09659 std::vector<int> vfree(0);
09660 solutions.AddSolution(vinfos,vfree);
09661 }
09662 }
09663 }
09664
09665 }
09666
09667 }
09668
09669 } else
09670 {
09671 {
09672 IkReal j4array[1], cj4array[1], sj4array[1];
09673 bool j4valid[1]={false};
09674 _nj4 = 1;
09675 IkReal x2168=((sj5)*(sj7));
09676 IkReal x2169=((r21)*(sj8));
09677 IkReal x2170=((cj5)*(sj7));
09678 IkReal x2171=((cj8)*(r20));
09679 IkReal x2172=((cj5)*(cj7));
09680 IkReal x2173=((cj7)*(sj5));
09681 if( IKabs(((gconst60)*(((((r22)*(x2172)))+(((IkReal(-1.00000000000000))*(x2171)*(x2173)))+(((IkReal(-1.00000000000000))*(r22)*(x2168)))+(((IkReal(-1.00000000000000))*(x2170)*(x2171)))+(((x2169)*(x2170)))+(((x2169)*(x2173))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst60)*(((((x2168)*(x2169)))+(((r22)*(x2173)))+(((r22)*(x2170)))+(((IkReal(-1.00000000000000))*(x2168)*(x2171)))+(((IkReal(-1.00000000000000))*(x2169)*(x2172)))+(((x2171)*(x2172))))))) < IKFAST_ATAN2_MAGTHRESH )
09682 continue;
09683 j4array[0]=IKatan2(((gconst60)*(((((r22)*(x2172)))+(((IkReal(-1.00000000000000))*(x2171)*(x2173)))+(((IkReal(-1.00000000000000))*(r22)*(x2168)))+(((IkReal(-1.00000000000000))*(x2170)*(x2171)))+(((x2169)*(x2170)))+(((x2169)*(x2173)))))), ((gconst60)*(((((x2168)*(x2169)))+(((r22)*(x2173)))+(((r22)*(x2170)))+(((IkReal(-1.00000000000000))*(x2168)*(x2171)))+(((IkReal(-1.00000000000000))*(x2169)*(x2172)))+(((x2171)*(x2172)))))));
09684 sj4array[0]=IKsin(j4array[0]);
09685 cj4array[0]=IKcos(j4array[0]);
09686 if( j4array[0] > IKPI )
09687 {
09688 j4array[0]-=IK2PI;
09689 }
09690 else if( j4array[0] < -IKPI )
09691 { j4array[0]+=IK2PI;
09692 }
09693 j4valid[0] = true;
09694 for(int ij4 = 0; ij4 < 1; ++ij4)
09695 {
09696 if( !j4valid[ij4] )
09697 {
09698 continue;
09699 }
09700 _ij4[0] = ij4; _ij4[1] = -1;
09701 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09702 {
09703 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09704 {
09705 j4valid[iij4]=false; _ij4[1] = iij4; break;
09706 }
09707 }
09708 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09709 {
09710 IkReal evalcond[4];
09711 IkReal x2174=IKcos(j4);
09712 IkReal x2175=IKsin(j4);
09713 IkReal x2176=((IkReal(1.00000000000000))*(cj7));
09714 IkReal x2177=((cj8)*(r20));
09715 IkReal x2178=((cj3)*(r02));
09716 IkReal x2179=((IkReal(1.00000000000000))*(sj7));
09717 IkReal x2180=((sj3)*(sj7));
09718 IkReal x2181=((r21)*(sj8));
09719 IkReal x2182=((cj8)*(r10));
09720 IkReal x2183=((sj5)*(x2174));
09721 IkReal x2184=((cj5)*(x2175));
09722 IkReal x2185=((r11)*(sj3)*(sj8));
09723 IkReal x2186=((cj3)*(cj8)*(r00));
09724 IkReal x2187=((cj5)*(x2174));
09725 IkReal x2188=((cj3)*(r01)*(sj8));
09726 IkReal x2189=((sj5)*(x2175));
09727 IkReal x2190=((x2183)+(x2184));
09728 evalcond[0]=((((IkReal(-1.00000000000000))*(x2176)*(x2177)))+(x2189)+(((IkReal(-1.00000000000000))*(x2187)))+(((cj7)*(x2181)))+(((IkReal(-1.00000000000000))*(r22)*(x2179))));
09729 evalcond[1]=((x2190)+(((sj7)*(x2181)))+(((cj7)*(r22)))+(((IkReal(-1.00000000000000))*(x2177)*(x2179))));
09730 evalcond[2]=((((sj7)*(x2178)))+(x2190)+(((cj7)*(sj3)*(x2182)))+(((r12)*(x2180)))+(((IkReal(-1.00000000000000))*(x2176)*(x2188)))+(((IkReal(-1.00000000000000))*(x2176)*(x2185)))+(((cj7)*(x2186))));
09731 evalcond[3]=((((IkReal(-1.00000000000000))*(x2176)*(x2178)))+(((x2180)*(x2182)))+(x2187)+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2176)))+(((IkReal(-1.00000000000000))*(x2189)))+(((sj7)*(x2186)))+(((IkReal(-1.00000000000000))*(x2179)*(x2185)))+(((IkReal(-1.00000000000000))*(x2179)*(x2188))));
09732 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09733 {
09734 continue;
09735 }
09736 }
09737
09738 {
09739 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09740 vinfos[0].jointtype = 1;
09741 vinfos[0].foffset = j3;
09742 vinfos[0].indices[0] = _ij3[0];
09743 vinfos[0].indices[1] = _ij3[1];
09744 vinfos[0].maxsolutions = _nj3;
09745 vinfos[1].jointtype = 1;
09746 vinfos[1].foffset = j4;
09747 vinfos[1].indices[0] = _ij4[0];
09748 vinfos[1].indices[1] = _ij4[1];
09749 vinfos[1].maxsolutions = _nj4;
09750 vinfos[2].jointtype = 1;
09751 vinfos[2].foffset = j5;
09752 vinfos[2].indices[0] = _ij5[0];
09753 vinfos[2].indices[1] = _ij5[1];
09754 vinfos[2].maxsolutions = _nj5;
09755 vinfos[3].jointtype = 1;
09756 vinfos[3].foffset = j6;
09757 vinfos[3].indices[0] = _ij6[0];
09758 vinfos[3].indices[1] = _ij6[1];
09759 vinfos[3].maxsolutions = _nj6;
09760 vinfos[4].jointtype = 1;
09761 vinfos[4].foffset = j7;
09762 vinfos[4].indices[0] = _ij7[0];
09763 vinfos[4].indices[1] = _ij7[1];
09764 vinfos[4].maxsolutions = _nj7;
09765 vinfos[5].jointtype = 1;
09766 vinfos[5].foffset = j8;
09767 vinfos[5].indices[0] = _ij8[0];
09768 vinfos[5].indices[1] = _ij8[1];
09769 vinfos[5].maxsolutions = _nj8;
09770 std::vector<int> vfree(0);
09771 solutions.AddSolution(vinfos,vfree);
09772 }
09773 }
09774 }
09775
09776 }
09777
09778 }
09779 }
09780 }
09781
09782 } else
09783 {
09784 IkReal x2191=((cj3)*(cj8));
09785 IkReal x2192=((r01)*(sj3));
09786 IkReal x2193=((r02)*(sj3));
09787 IkReal x2194=((IkReal(1.00000000000000))*(sj7));
09788 IkReal x2195=((IkReal(1.00000000000000))*(r11));
09789 IkReal x2196=((cj3)*(r12));
09790 IkReal x2197=((IkReal(1.00000000000000))*(cj7));
09791 IkReal x2198=((cj8)*(sj3));
09792 IkReal x2199=((sj3)*(sj8));
09793 IkReal x2200=((IkReal(1.00000000000000))*(cj3)*(sj8));
09794 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j6)), IkReal(6.28318530717959))));
09795 evalcond[1]=((IkReal(0.0950000000000000))+(((npx)*(sj8)))+(((cj8)*(npy))));
09796 evalcond[2]=((((cj8)*(r21)))+(((r20)*(sj8))));
09797 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x2200)))+(((cj8)*(x2192)))+(((IkReal(-1.00000000000000))*(x2191)*(x2195)))+(((r00)*(x2199))));
09798 evalcond[4]=((((IkReal(-1.00000000000000))*(x2195)*(x2198)))+(((IkReal(-1.00000000000000))*(r10)*(x2199)))+(((IkReal(-1.00000000000000))*(r00)*(x2200)))+(((IkReal(-1.00000000000000))*(r01)*(x2191))));
09799 evalcond[5]=((((IkReal(-1.00000000000000))*(x2193)*(x2194)))+(((IkReal(-1.00000000000000))*(r00)*(x2197)*(x2198)))+(((sj7)*(x2196)))+(((cj7)*(sj8)*(x2192)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(sj8)*(x2195)))+(((cj7)*(r10)*(x2191))));
09800 evalcond[6]=((((sj7)*(sj8)*(x2192)))+(((IkReal(-1.00000000000000))*(cj3)*(r11)*(sj8)*(x2194)))+(((IkReal(-1.00000000000000))*(x2196)*(x2197)))+(((IkReal(-1.00000000000000))*(r00)*(x2194)*(x2198)))+(((r10)*(sj7)*(x2191)))+(((cj7)*(x2193))));
09801 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 )
09802 {
09803 {
09804 IkReal j5array[1], cj5array[1], sj5array[1];
09805 bool j5valid[1]={false};
09806 _nj5 = 1;
09807 IkReal x2201=((IkReal(4.00000000000000))*(sj7));
09808 IkReal x2202=((npy)*(sj8));
09809 IkReal x2203=((IkReal(4.00000000000000))*(cj7));
09810 IkReal x2204=((cj8)*(npx));
09811 if( IKabs(((IkReal(0.120000000000000))+(((x2201)*(x2204)))+(((IkReal(-0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2201)*(x2202)))+(((IkReal(-1.00000000000000))*(npz)*(x2203))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x2201)))+(((x2203)*(x2204)))+(((IkReal(-1.00000000000000))*(x2202)*(x2203)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((x2201)*(x2204)))+(((IkReal(-0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2201)*(x2202)))+(((IkReal(-1.00000000000000))*(npz)*(x2203)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x2201)))+(((x2203)*(x2204)))+(((IkReal(-1.00000000000000))*(x2202)*(x2203)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
09812 continue;
09813 j5array[0]=IKatan2(((IkReal(0.120000000000000))+(((x2201)*(x2204)))+(((IkReal(-0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2201)*(x2202)))+(((IkReal(-1.00000000000000))*(npz)*(x2203)))), ((IkReal(-0.940000000000000))+(((npz)*(x2201)))+(((x2203)*(x2204)))+(((IkReal(-1.00000000000000))*(x2202)*(x2203)))+(((IkReal(-0.360000000000000))*(cj7)))));
09814 sj5array[0]=IKsin(j5array[0]);
09815 cj5array[0]=IKcos(j5array[0]);
09816 if( j5array[0] > IKPI )
09817 {
09818 j5array[0]-=IK2PI;
09819 }
09820 else if( j5array[0] < -IKPI )
09821 { j5array[0]+=IK2PI;
09822 }
09823 j5valid[0] = true;
09824 for(int ij5 = 0; ij5 < 1; ++ij5)
09825 {
09826 if( !j5valid[ij5] )
09827 {
09828 continue;
09829 }
09830 _ij5[0] = ij5; _ij5[1] = -1;
09831 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
09832 {
09833 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
09834 {
09835 j5valid[iij5]=false; _ij5[1] = iij5; break;
09836 }
09837 }
09838 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
09839 {
09840 IkReal evalcond[2];
09841 IkReal x2205=((IkReal(1.00000000000000))*(sj7));
09842 IkReal x2206=((npy)*(sj8));
09843 IkReal x2207=((cj8)*(npx));
09844 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2207)))+(((cj7)*(x2206)))+(((IkReal(-1.00000000000000))*(npz)*(x2205)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
09845 evalcond[1]=((IkReal(-0.0300000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((sj7)*(x2206)))+(((IkReal(-1.00000000000000))*(x2205)*(x2207)))+(((IkReal(0.250000000000000))*(IKsin(j5))))+(((cj7)*(npz))));
09846 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09847 {
09848 continue;
09849 }
09850 }
09851
09852 {
09853 IkReal dummyeval[1];
09854 IkReal gconst62;
09855 gconst62=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
09856 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
09857 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09858 {
09859 {
09860 IkReal dummyeval[1];
09861 IkReal gconst63;
09862 gconst63=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
09863 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
09864 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09865 {
09866 continue;
09867
09868 } else
09869 {
09870 {
09871 IkReal j4array[1], cj4array[1], sj4array[1];
09872 bool j4valid[1]={false};
09873 _nj4 = 1;
09874 IkReal x2208=((cj3)*(cj5));
09875 IkReal x2209=((r02)*(sj7));
09876 IkReal x2210=((cj8)*(r00));
09877 IkReal x2211=((cj5)*(sj7));
09878 IkReal x2212=((cj5)*(sj3));
09879 IkReal x2213=((cj7)*(cj8));
09880 IkReal x2214=((IkReal(1.00000000000000))*(sj5));
09881 IkReal x2215=((r12)*(sj3));
09882 IkReal x2216=((sj3)*(sj5));
09883 IkReal x2217=((cj3)*(cj7)*(sj5));
09884 IkReal x2218=((IkReal(1.00000000000000))*(cj7)*(sj8));
09885 if( IKabs(((gconst63)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x2213)*(x2214)))+(((r10)*(x2212)*(x2213)))+(((IkReal(-1.00000000000000))*(r01)*(x2208)*(x2218)))+(((x2211)*(x2215)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x2214)))+(((cj7)*(x2208)*(x2210)))+(((IkReal(-1.00000000000000))*(r11)*(x2212)*(x2218)))+(((x2208)*(x2209))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst63)*(((((x2210)*(x2217)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2218)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x2214)))+(((sj5)*(sj7)*(x2215)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x2214)))+(((r22)*(x2211)))+(((cj5)*(r20)*(x2213)))+(((r10)*(x2213)*(x2216)))+(((cj3)*(sj5)*(x2209))))))) < IKFAST_ATAN2_MAGTHRESH )
09886 continue;
09887 j4array[0]=IKatan2(((gconst63)*(((((cj7)*(r21)*(sj5)*(sj8)))+(((IkReal(-1.00000000000000))*(r20)*(x2213)*(x2214)))+(((r10)*(x2212)*(x2213)))+(((IkReal(-1.00000000000000))*(r01)*(x2208)*(x2218)))+(((x2211)*(x2215)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)*(x2214)))+(((cj7)*(x2208)*(x2210)))+(((IkReal(-1.00000000000000))*(r11)*(x2212)*(x2218)))+(((x2208)*(x2209)))))), ((gconst63)*(((((x2210)*(x2217)))+(((IkReal(-1.00000000000000))*(cj5)*(r21)*(x2218)))+(((IkReal(-1.00000000000000))*(cj7)*(r11)*(sj3)*(sj8)*(x2214)))+(((sj5)*(sj7)*(x2215)))+(((IkReal(-1.00000000000000))*(cj3)*(cj7)*(r01)*(sj8)*(x2214)))+(((r22)*(x2211)))+(((cj5)*(r20)*(x2213)))+(((r10)*(x2213)*(x2216)))+(((cj3)*(sj5)*(x2209)))))));
09888 sj4array[0]=IKsin(j4array[0]);
09889 cj4array[0]=IKcos(j4array[0]);
09890 if( j4array[0] > IKPI )
09891 {
09892 j4array[0]-=IK2PI;
09893 }
09894 else if( j4array[0] < -IKPI )
09895 { j4array[0]+=IK2PI;
09896 }
09897 j4valid[0] = true;
09898 for(int ij4 = 0; ij4 < 1; ++ij4)
09899 {
09900 if( !j4valid[ij4] )
09901 {
09902 continue;
09903 }
09904 _ij4[0] = ij4; _ij4[1] = -1;
09905 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
09906 {
09907 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
09908 {
09909 j4valid[iij4]=false; _ij4[1] = iij4; break;
09910 }
09911 }
09912 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
09913 {
09914 IkReal evalcond[4];
09915 IkReal x2219=IKcos(j4);
09916 IkReal x2220=IKsin(j4);
09917 IkReal x2221=((IkReal(1.00000000000000))*(cj7));
09918 IkReal x2222=((cj8)*(r20));
09919 IkReal x2223=((cj3)*(r02));
09920 IkReal x2224=((IkReal(1.00000000000000))*(sj7));
09921 IkReal x2225=((sj3)*(sj7));
09922 IkReal x2226=((r21)*(sj8));
09923 IkReal x2227=((IkReal(1.00000000000000))*(cj5));
09924 IkReal x2228=((cj8)*(r10));
09925 IkReal x2229=((sj5)*(x2220));
09926 IkReal x2230=((sj5)*(x2219));
09927 IkReal x2231=((r11)*(sj3)*(sj8));
09928 IkReal x2232=((cj3)*(cj8)*(r00));
09929 IkReal x2233=((cj3)*(r01)*(sj8));
09930 IkReal x2234=((x2219)*(x2227));
09931 evalcond[0]=((x2229)+(((cj7)*(x2226)))+(((IkReal(-1.00000000000000))*(x2221)*(x2222)))+(((IkReal(-1.00000000000000))*(x2234)))+(((IkReal(-1.00000000000000))*(r22)*(x2224))));
09932 evalcond[1]=((((IkReal(-1.00000000000000))*(x2222)*(x2224)))+(((IkReal(-1.00000000000000))*(x2220)*(x2227)))+(((sj7)*(x2226)))+(((IkReal(-1.00000000000000))*(x2230)))+(((cj7)*(r22))));
09933 evalcond[2]=((((r12)*(x2225)))+(x2230)+(((cj7)*(x2232)))+(((cj7)*(sj3)*(x2228)))+(((IkReal(-1.00000000000000))*(x2221)*(x2233)))+(((IkReal(-1.00000000000000))*(x2221)*(x2231)))+(((cj5)*(x2220)))+(((sj7)*(x2223))));
09934 evalcond[3]=((x2229)+(((IkReal(-1.00000000000000))*(x2224)*(x2233)))+(((IkReal(-1.00000000000000))*(x2224)*(x2231)))+(((x2225)*(x2228)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2221)))+(((IkReal(-1.00000000000000))*(x2221)*(x2223)))+(((sj7)*(x2232)))+(((IkReal(-1.00000000000000))*(x2234))));
09935 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09936 {
09937 continue;
09938 }
09939 }
09940
09941 {
09942 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09943 vinfos[0].jointtype = 1;
09944 vinfos[0].foffset = j3;
09945 vinfos[0].indices[0] = _ij3[0];
09946 vinfos[0].indices[1] = _ij3[1];
09947 vinfos[0].maxsolutions = _nj3;
09948 vinfos[1].jointtype = 1;
09949 vinfos[1].foffset = j4;
09950 vinfos[1].indices[0] = _ij4[0];
09951 vinfos[1].indices[1] = _ij4[1];
09952 vinfos[1].maxsolutions = _nj4;
09953 vinfos[2].jointtype = 1;
09954 vinfos[2].foffset = j5;
09955 vinfos[2].indices[0] = _ij5[0];
09956 vinfos[2].indices[1] = _ij5[1];
09957 vinfos[2].maxsolutions = _nj5;
09958 vinfos[3].jointtype = 1;
09959 vinfos[3].foffset = j6;
09960 vinfos[3].indices[0] = _ij6[0];
09961 vinfos[3].indices[1] = _ij6[1];
09962 vinfos[3].maxsolutions = _nj6;
09963 vinfos[4].jointtype = 1;
09964 vinfos[4].foffset = j7;
09965 vinfos[4].indices[0] = _ij7[0];
09966 vinfos[4].indices[1] = _ij7[1];
09967 vinfos[4].maxsolutions = _nj7;
09968 vinfos[5].jointtype = 1;
09969 vinfos[5].foffset = j8;
09970 vinfos[5].indices[0] = _ij8[0];
09971 vinfos[5].indices[1] = _ij8[1];
09972 vinfos[5].maxsolutions = _nj8;
09973 std::vector<int> vfree(0);
09974 solutions.AddSolution(vinfos,vfree);
09975 }
09976 }
09977 }
09978
09979 }
09980
09981 }
09982
09983 } else
09984 {
09985 {
09986 IkReal j4array[1], cj4array[1], sj4array[1];
09987 bool j4valid[1]={false};
09988 _nj4 = 1;
09989 IkReal x2235=((cj7)*(sj5));
09990 IkReal x2236=((r21)*(sj8));
09991 IkReal x2237=((cj5)*(cj7));
09992 IkReal x2238=((cj8)*(r20));
09993 IkReal x2239=((sj5)*(sj7));
09994 IkReal x2240=((IkReal(1.00000000000000))*(cj5)*(sj7));
09995 if( IKabs(((gconst62)*(((((x2235)*(x2238)))+(((r22)*(x2239)))+(((r22)*(x2237)))+(((cj5)*(sj7)*(x2236)))+(((IkReal(-1.00000000000000))*(x2238)*(x2240)))+(((IkReal(-1.00000000000000))*(x2235)*(x2236))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst62)*(((((IkReal(-1.00000000000000))*(x2237)*(x2238)))+(((r22)*(x2235)))+(((IkReal(-1.00000000000000))*(r22)*(x2240)))+(((x2236)*(x2239)))+(((x2236)*(x2237)))+(((IkReal(-1.00000000000000))*(x2238)*(x2239))))))) < IKFAST_ATAN2_MAGTHRESH )
09996 continue;
09997 j4array[0]=IKatan2(((gconst62)*(((((x2235)*(x2238)))+(((r22)*(x2239)))+(((r22)*(x2237)))+(((cj5)*(sj7)*(x2236)))+(((IkReal(-1.00000000000000))*(x2238)*(x2240)))+(((IkReal(-1.00000000000000))*(x2235)*(x2236)))))), ((gconst62)*(((((IkReal(-1.00000000000000))*(x2237)*(x2238)))+(((r22)*(x2235)))+(((IkReal(-1.00000000000000))*(r22)*(x2240)))+(((x2236)*(x2239)))+(((x2236)*(x2237)))+(((IkReal(-1.00000000000000))*(x2238)*(x2239)))))));
09998 sj4array[0]=IKsin(j4array[0]);
09999 cj4array[0]=IKcos(j4array[0]);
10000 if( j4array[0] > IKPI )
10001 {
10002 j4array[0]-=IK2PI;
10003 }
10004 else if( j4array[0] < -IKPI )
10005 { j4array[0]+=IK2PI;
10006 }
10007 j4valid[0] = true;
10008 for(int ij4 = 0; ij4 < 1; ++ij4)
10009 {
10010 if( !j4valid[ij4] )
10011 {
10012 continue;
10013 }
10014 _ij4[0] = ij4; _ij4[1] = -1;
10015 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10016 {
10017 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10018 {
10019 j4valid[iij4]=false; _ij4[1] = iij4; break;
10020 }
10021 }
10022 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10023 {
10024 IkReal evalcond[4];
10025 IkReal x2241=IKcos(j4);
10026 IkReal x2242=IKsin(j4);
10027 IkReal x2243=((IkReal(1.00000000000000))*(cj7));
10028 IkReal x2244=((cj8)*(r20));
10029 IkReal x2245=((cj3)*(r02));
10030 IkReal x2246=((IkReal(1.00000000000000))*(sj7));
10031 IkReal x2247=((sj3)*(sj7));
10032 IkReal x2248=((r21)*(sj8));
10033 IkReal x2249=((IkReal(1.00000000000000))*(cj5));
10034 IkReal x2250=((cj8)*(r10));
10035 IkReal x2251=((sj5)*(x2242));
10036 IkReal x2252=((sj5)*(x2241));
10037 IkReal x2253=((r11)*(sj3)*(sj8));
10038 IkReal x2254=((cj3)*(cj8)*(r00));
10039 IkReal x2255=((cj3)*(r01)*(sj8));
10040 IkReal x2256=((x2241)*(x2249));
10041 evalcond[0]=((((cj7)*(x2248)))+(x2251)+(((IkReal(-1.00000000000000))*(r22)*(x2246)))+(((IkReal(-1.00000000000000))*(x2256)))+(((IkReal(-1.00000000000000))*(x2243)*(x2244))));
10042 evalcond[1]=((((IkReal(-1.00000000000000))*(x2252)))+(((IkReal(-1.00000000000000))*(x2244)*(x2246)))+(((IkReal(-1.00000000000000))*(x2242)*(x2249)))+(((sj7)*(x2248)))+(((cj7)*(r22))));
10043 evalcond[2]=((((cj7)*(x2254)))+(x2252)+(((cj7)*(sj3)*(x2250)))+(((IkReal(-1.00000000000000))*(x2243)*(x2255)))+(((IkReal(-1.00000000000000))*(x2243)*(x2253)))+(((cj5)*(x2242)))+(((sj7)*(x2245)))+(((r12)*(x2247))));
10044 evalcond[3]=((x2251)+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2243)))+(((IkReal(-1.00000000000000))*(x2256)))+(((x2247)*(x2250)))+(((sj7)*(x2254)))+(((IkReal(-1.00000000000000))*(x2243)*(x2245)))+(((IkReal(-1.00000000000000))*(x2246)*(x2253)))+(((IkReal(-1.00000000000000))*(x2246)*(x2255))));
10045 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10046 {
10047 continue;
10048 }
10049 }
10050
10051 {
10052 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10053 vinfos[0].jointtype = 1;
10054 vinfos[0].foffset = j3;
10055 vinfos[0].indices[0] = _ij3[0];
10056 vinfos[0].indices[1] = _ij3[1];
10057 vinfos[0].maxsolutions = _nj3;
10058 vinfos[1].jointtype = 1;
10059 vinfos[1].foffset = j4;
10060 vinfos[1].indices[0] = _ij4[0];
10061 vinfos[1].indices[1] = _ij4[1];
10062 vinfos[1].maxsolutions = _nj4;
10063 vinfos[2].jointtype = 1;
10064 vinfos[2].foffset = j5;
10065 vinfos[2].indices[0] = _ij5[0];
10066 vinfos[2].indices[1] = _ij5[1];
10067 vinfos[2].maxsolutions = _nj5;
10068 vinfos[3].jointtype = 1;
10069 vinfos[3].foffset = j6;
10070 vinfos[3].indices[0] = _ij6[0];
10071 vinfos[3].indices[1] = _ij6[1];
10072 vinfos[3].maxsolutions = _nj6;
10073 vinfos[4].jointtype = 1;
10074 vinfos[4].foffset = j7;
10075 vinfos[4].indices[0] = _ij7[0];
10076 vinfos[4].indices[1] = _ij7[1];
10077 vinfos[4].maxsolutions = _nj7;
10078 vinfos[5].jointtype = 1;
10079 vinfos[5].foffset = j8;
10080 vinfos[5].indices[0] = _ij8[0];
10081 vinfos[5].indices[1] = _ij8[1];
10082 vinfos[5].maxsolutions = _nj8;
10083 std::vector<int> vfree(0);
10084 solutions.AddSolution(vinfos,vfree);
10085 }
10086 }
10087 }
10088
10089 }
10090
10091 }
10092 }
10093 }
10094
10095 } else
10096 {
10097 if( 1 )
10098 {
10099 continue;
10100
10101 } else
10102 {
10103 }
10104 }
10105 }
10106 }
10107 }
10108 }
10109
10110 } else
10111 {
10112 {
10113 IkReal j5array[1], cj5array[1], sj5array[1];
10114 bool j5valid[1]={false};
10115 _nj5 = 1;
10116 IkReal x2257=((IkReal(4.00000000000000))*(cj7));
10117 IkReal x2258=((cj8)*(npx));
10118 IkReal x2259=((IkReal(4.00000000000000))*(sj7));
10119 IkReal x2260=((IkReal(4.00000000000000))*(npy)*(sj8));
10120 if( IKabs(((((IKabs(cj6) != 0)?((IkReal)1/(cj6)):(IkReal)1.0e30))*(((((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2258)*(x2259)))+(((npy)*(sj8)*(x2259)))+(((IkReal(-0.380000000000000))*(sj6)))+(((npz)*(x2257)))+(((IkReal(0.120000000000000))*(cj6))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(npy)*(sj8)*(x2257)))+(((npz)*(x2259)))+(((x2257)*(x2258)))+(((IkReal(-0.360000000000000))*(cj7))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj6) != 0)?((IkReal)1/(cj6)):(IkReal)1.0e30))*(((((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2258)*(x2259)))+(((npy)*(sj8)*(x2259)))+(((IkReal(-0.380000000000000))*(sj6)))+(((npz)*(x2257)))+(((IkReal(0.120000000000000))*(cj6)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(npy)*(sj8)*(x2257)))+(((npz)*(x2259)))+(((x2257)*(x2258)))+(((IkReal(-0.360000000000000))*(cj7)))))-1) <= IKFAST_SINCOS_THRESH )
10121 continue;
10122 j5array[0]=IKatan2(((((IKabs(cj6) != 0)?((IkReal)1/(cj6)):(IkReal)1.0e30))*(((((IkReal(0.360000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2258)*(x2259)))+(((npy)*(sj8)*(x2259)))+(((IkReal(-0.380000000000000))*(sj6)))+(((npz)*(x2257)))+(((IkReal(0.120000000000000))*(cj6)))))), ((IkReal(-0.940000000000000))+(((IkReal(-1.00000000000000))*(npy)*(sj8)*(x2257)))+(((npz)*(x2259)))+(((x2257)*(x2258)))+(((IkReal(-0.360000000000000))*(cj7)))));
10123 sj5array[0]=IKsin(j5array[0]);
10124 cj5array[0]=IKcos(j5array[0]);
10125 if( j5array[0] > IKPI )
10126 {
10127 j5array[0]-=IK2PI;
10128 }
10129 else if( j5array[0] < -IKPI )
10130 { j5array[0]+=IK2PI;
10131 }
10132 j5valid[0] = true;
10133 for(int ij5 = 0; ij5 < 1; ++ij5)
10134 {
10135 if( !j5valid[ij5] )
10136 {
10137 continue;
10138 }
10139 _ij5[0] = ij5; _ij5[1] = -1;
10140 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
10141 {
10142 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
10143 {
10144 j5valid[iij5]=false; _ij5[1] = iij5; break;
10145 }
10146 }
10147 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
10148 {
10149 IkReal evalcond[3];
10150 IkReal x2261=IKsin(j5);
10151 IkReal x2262=((IkReal(1.00000000000000))*(sj7));
10152 IkReal x2263=((npy)*(sj8));
10153 IkReal x2264=((cj8)*(npx));
10154 IkReal x2265=((IkReal(0.250000000000000))*(x2261));
10155 evalcond[0]=((((IkReal(-0.0300000000000000))*(sj6)))+(((IkReal(-0.0950000000000000))*(cj6)))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((sj6)*(x2265))));
10156 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x2262)))+(((IkReal(0.0900000000000000))*(cj7)))+(((cj7)*(x2263)))+(((IkReal(-1.00000000000000))*(cj7)*(x2264)))+(((IkReal(0.250000000000000))*(IKcos(j5)))));
10157 evalcond[2]=((((IkReal(0.0900000000000000))*(sj7)))+(((sj7)*(x2263)))+(((IkReal(0.0300000000000000))*(cj6)))+(((IkReal(-0.0950000000000000))*(sj6)))+(((cj7)*(npz)))+(((IkReal(-1.00000000000000))*(x2262)*(x2264)))+(((IkReal(-1.00000000000000))*(cj6)*(x2265))));
10158 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
10159 {
10160 continue;
10161 }
10162 }
10163
10164 {
10165 IkReal dummyeval[1];
10166 IkReal gconst46;
10167 gconst46=IKsign(((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5))))));
10168 dummyeval[0]=((((sj6)*((cj5)*(cj5))))+(((sj6)*((sj5)*(sj5)))));
10169 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10170 {
10171 {
10172 IkReal dummyeval[1];
10173 IkReal gconst47;
10174 gconst47=IKsign(((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5))))));
10175 dummyeval[0]=((((cj6)*((cj5)*(cj5))))+(((cj6)*((sj5)*(sj5)))));
10176 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10177 {
10178 {
10179 IkReal evalcond[9];
10180 IkReal x2266=((IkReal(1.00000000000000))*(sj7));
10181 IkReal x2267=((IkReal(1.00000000000000))*(cj3));
10182 IkReal x2268=((cj7)*(r02));
10183 IkReal x2269=((cj8)*(r00));
10184 IkReal x2270=((cj3)*(sj7));
10185 IkReal x2271=((cj8)*(npx));
10186 IkReal x2272=((r01)*(sj3));
10187 IkReal x2273=((cj7)*(sj8));
10188 IkReal x2274=((r11)*(sj8));
10189 IkReal x2275=((IkReal(1.00000000000000))*(cj7));
10190 IkReal x2276=((sj7)*(sj8));
10191 IkReal x2277=((cj8)*(r10));
10192 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j6)), IkReal(6.28318530717959))));
10193 evalcond[1]=((IkReal(-0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(0.250000000000000))*(sj5))));
10194 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x2266)))+(((IkReal(0.0900000000000000))*(cj7)))+(((npy)*(x2273)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(x2271)*(x2275))));
10195 evalcond[3]=((((cj8)*(x2272)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x2267)))+(((r00)*(sj3)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r11)*(x2267))));
10196 evalcond[4]=((((r21)*(x2276)))+(((IkReal(-1.00000000000000))*(cj8)*(r20)*(x2266)))+(((cj7)*(r22))));
10197 evalcond[5]=((IkReal(-0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((IkReal(-1.00000000000000))*(x2266)*(x2271)))+(((npy)*(x2276)))+(((cj7)*(npz))));
10198 evalcond[6]=((((IkReal(-1.00000000000000))*(r02)*(sj3)*(x2266)))+(((cj3)*(cj7)*(x2277)))+(((IkReal(-1.00000000000000))*(r11)*(x2267)*(x2273)))+(((IkReal(-1.00000000000000))*(sj3)*(x2269)*(x2275)))+(((x2272)*(x2273)))+(((r12)*(x2270))));
10199 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x2267)))+(((x2270)*(x2277)))+(((IkReal(-1.00000000000000))*(sj3)*(x2266)*(x2269)))+(((x2272)*(x2276)))+(((sj3)*(x2268)))+(((IkReal(-1.00000000000000))*(cj3)*(x2266)*(x2274))));
10200 evalcond[8]=((((IkReal(-1.00000000000000))*(cj3)*(r01)*(sj8)*(x2266)))+(((x2269)*(x2270)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2275)))+(((IkReal(-1.00000000000000))*(sj3)*(x2266)*(x2274)))+(((sj3)*(sj7)*(x2277)))+(((IkReal(-1.00000000000000))*(x2267)*(x2268))));
10201 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 )
10202 {
10203 {
10204 IkReal dummyeval[1];
10205 IkReal gconst48;
10206 gconst48=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
10207 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
10208 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10209 {
10210 {
10211 IkReal dummyeval[1];
10212 IkReal gconst49;
10213 gconst49=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
10214 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
10215 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10216 {
10217 continue;
10218
10219 } else
10220 {
10221 {
10222 IkReal j4array[1], cj4array[1], sj4array[1];
10223 bool j4valid[1]={false};
10224 _nj4 = 1;
10225 IkReal x2278=((cj8)*(sj5));
10226 IkReal x2279=((cj3)*(r01));
10227 IkReal x2280=((IkReal(1.00000000000000))*(cj5));
10228 IkReal x2281=((r11)*(sj3));
10229 IkReal x2282=((sj5)*(sj8));
10230 IkReal x2283=((r10)*(sj3));
10231 IkReal x2284=((cj3)*(r00)*(sj8));
10232 if( IKabs(((gconst49)*(((((x2282)*(x2283)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x2282)))+(((x2278)*(x2281)))+(((cj5)*(cj8)*(r21)))+(((x2278)*(x2279))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((IkReal(-1.00000000000000))*(x2280)*(x2284)))+(((r21)*(x2278)))+(((r20)*(x2282)))+(((IkReal(-1.00000000000000))*(sj8)*(x2280)*(x2283)))+(((IkReal(-1.00000000000000))*(cj8)*(x2280)*(x2281)))+(((IkReal(-1.00000000000000))*(cj8)*(x2279)*(x2280))))))) < IKFAST_ATAN2_MAGTHRESH )
10233 continue;
10234 j4array[0]=IKatan2(((gconst49)*(((((x2282)*(x2283)))+(((cj5)*(r20)*(sj8)))+(((cj3)*(r00)*(x2282)))+(((x2278)*(x2281)))+(((cj5)*(cj8)*(r21)))+(((x2278)*(x2279)))))), ((gconst49)*(((((IkReal(-1.00000000000000))*(x2280)*(x2284)))+(((r21)*(x2278)))+(((r20)*(x2282)))+(((IkReal(-1.00000000000000))*(sj8)*(x2280)*(x2283)))+(((IkReal(-1.00000000000000))*(cj8)*(x2280)*(x2281)))+(((IkReal(-1.00000000000000))*(cj8)*(x2279)*(x2280)))))));
10235 sj4array[0]=IKsin(j4array[0]);
10236 cj4array[0]=IKcos(j4array[0]);
10237 if( j4array[0] > IKPI )
10238 {
10239 j4array[0]-=IK2PI;
10240 }
10241 else if( j4array[0] < -IKPI )
10242 { j4array[0]+=IK2PI;
10243 }
10244 j4valid[0] = true;
10245 for(int ij4 = 0; ij4 < 1; ++ij4)
10246 {
10247 if( !j4valid[ij4] )
10248 {
10249 continue;
10250 }
10251 _ij4[0] = ij4; _ij4[1] = -1;
10252 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10253 {
10254 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10255 {
10256 j4valid[iij4]=false; _ij4[1] = iij4; break;
10257 }
10258 }
10259 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10260 {
10261 IkReal evalcond[4];
10262 IkReal x2285=IKsin(j4);
10263 IkReal x2286=IKcos(j4);
10264 IkReal x2287=((IkReal(1.00000000000000))*(cj8));
10265 IkReal x2288=((cj3)*(r01));
10266 IkReal x2289=((cj3)*(r00));
10267 IkReal x2290=((cj7)*(cj8));
10268 IkReal x2291=((IkReal(1.00000000000000))*(cj5));
10269 IkReal x2292=((IkReal(1.00000000000000))*(sj8));
10270 IkReal x2293=((r11)*(sj3));
10271 IkReal x2294=((r10)*(sj3));
10272 IkReal x2295=((sj5)*(x2285));
10273 IkReal x2296=((sj5)*(x2286));
10274 IkReal x2297=((x2286)*(x2291));
10275 evalcond[0]=((((IkReal(-1.00000000000000))*(x2285)*(x2291)))+(((cj8)*(r21)))+(((IkReal(-1.00000000000000))*(x2296)))+(((r20)*(sj8))));
10276 evalcond[1]=((x2295)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2287)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((IkReal(-1.00000000000000))*(x2297)))+(((cj7)*(r21)*(sj8))));
10277 evalcond[2]=((((IkReal(-1.00000000000000))*(x2289)*(x2292)))+(x2295)+(((IkReal(-1.00000000000000))*(x2292)*(x2294)))+(((IkReal(-1.00000000000000))*(x2287)*(x2293)))+(((IkReal(-1.00000000000000))*(x2287)*(x2288)))+(((IkReal(-1.00000000000000))*(x2297))));
10278 evalcond[3]=((((r12)*(sj3)*(sj7)))+(x2296)+(((IkReal(-1.00000000000000))*(cj7)*(x2288)*(x2292)))+(((IkReal(-1.00000000000000))*(cj7)*(x2292)*(x2293)))+(((x2289)*(x2290)))+(((cj3)*(r02)*(sj7)))+(((cj5)*(x2285)))+(((x2290)*(x2294))));
10279 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10280 {
10281 continue;
10282 }
10283 }
10284
10285 {
10286 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10287 vinfos[0].jointtype = 1;
10288 vinfos[0].foffset = j3;
10289 vinfos[0].indices[0] = _ij3[0];
10290 vinfos[0].indices[1] = _ij3[1];
10291 vinfos[0].maxsolutions = _nj3;
10292 vinfos[1].jointtype = 1;
10293 vinfos[1].foffset = j4;
10294 vinfos[1].indices[0] = _ij4[0];
10295 vinfos[1].indices[1] = _ij4[1];
10296 vinfos[1].maxsolutions = _nj4;
10297 vinfos[2].jointtype = 1;
10298 vinfos[2].foffset = j5;
10299 vinfos[2].indices[0] = _ij5[0];
10300 vinfos[2].indices[1] = _ij5[1];
10301 vinfos[2].maxsolutions = _nj5;
10302 vinfos[3].jointtype = 1;
10303 vinfos[3].foffset = j6;
10304 vinfos[3].indices[0] = _ij6[0];
10305 vinfos[3].indices[1] = _ij6[1];
10306 vinfos[3].maxsolutions = _nj6;
10307 vinfos[4].jointtype = 1;
10308 vinfos[4].foffset = j7;
10309 vinfos[4].indices[0] = _ij7[0];
10310 vinfos[4].indices[1] = _ij7[1];
10311 vinfos[4].maxsolutions = _nj7;
10312 vinfos[5].jointtype = 1;
10313 vinfos[5].foffset = j8;
10314 vinfos[5].indices[0] = _ij8[0];
10315 vinfos[5].indices[1] = _ij8[1];
10316 vinfos[5].maxsolutions = _nj8;
10317 std::vector<int> vfree(0);
10318 solutions.AddSolution(vinfos,vfree);
10319 }
10320 }
10321 }
10322
10323 }
10324
10325 }
10326
10327 } else
10328 {
10329 {
10330 IkReal j4array[1], cj4array[1], sj4array[1];
10331 bool j4valid[1]={false};
10332 _nj4 = 1;
10333 IkReal x2298=((sj5)*(sj8));
10334 IkReal x2299=((IkReal(1.00000000000000))*(cj7));
10335 IkReal x2300=((r22)*(sj7));
10336 IkReal x2301=((cj8)*(sj5));
10337 IkReal x2302=((cj5)*(cj8));
10338 IkReal x2303=((cj5)*(sj8));
10339 if( IKabs(((gconst48)*(((((cj7)*(r20)*(x2301)))+(((r20)*(x2303)))+(((sj5)*(x2300)))+(((IkReal(-1.00000000000000))*(r21)*(x2298)*(x2299)))+(((r21)*(x2302))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((IkReal(-1.00000000000000))*(cj5)*(x2300)))+(((cj7)*(r21)*(x2303)))+(((r20)*(x2298)))+(((r21)*(x2301)))+(((IkReal(-1.00000000000000))*(r20)*(x2299)*(x2302))))))) < IKFAST_ATAN2_MAGTHRESH )
10340 continue;
10341 j4array[0]=IKatan2(((gconst48)*(((((cj7)*(r20)*(x2301)))+(((r20)*(x2303)))+(((sj5)*(x2300)))+(((IkReal(-1.00000000000000))*(r21)*(x2298)*(x2299)))+(((r21)*(x2302)))))), ((gconst48)*(((((IkReal(-1.00000000000000))*(cj5)*(x2300)))+(((cj7)*(r21)*(x2303)))+(((r20)*(x2298)))+(((r21)*(x2301)))+(((IkReal(-1.00000000000000))*(r20)*(x2299)*(x2302)))))));
10342 sj4array[0]=IKsin(j4array[0]);
10343 cj4array[0]=IKcos(j4array[0]);
10344 if( j4array[0] > IKPI )
10345 {
10346 j4array[0]-=IK2PI;
10347 }
10348 else if( j4array[0] < -IKPI )
10349 { j4array[0]+=IK2PI;
10350 }
10351 j4valid[0] = true;
10352 for(int ij4 = 0; ij4 < 1; ++ij4)
10353 {
10354 if( !j4valid[ij4] )
10355 {
10356 continue;
10357 }
10358 _ij4[0] = ij4; _ij4[1] = -1;
10359 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10360 {
10361 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10362 {
10363 j4valid[iij4]=false; _ij4[1] = iij4; break;
10364 }
10365 }
10366 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10367 {
10368 IkReal evalcond[4];
10369 IkReal x2304=IKsin(j4);
10370 IkReal x2305=IKcos(j4);
10371 IkReal x2306=((IkReal(1.00000000000000))*(cj8));
10372 IkReal x2307=((cj3)*(r01));
10373 IkReal x2308=((cj3)*(r00));
10374 IkReal x2309=((cj7)*(cj8));
10375 IkReal x2310=((IkReal(1.00000000000000))*(cj5));
10376 IkReal x2311=((IkReal(1.00000000000000))*(sj8));
10377 IkReal x2312=((r11)*(sj3));
10378 IkReal x2313=((r10)*(sj3));
10379 IkReal x2314=((sj5)*(x2304));
10380 IkReal x2315=((sj5)*(x2305));
10381 IkReal x2316=((x2305)*(x2310));
10382 evalcond[0]=((((IkReal(-1.00000000000000))*(x2315)))+(((IkReal(-1.00000000000000))*(x2304)*(x2310)))+(((cj8)*(r21)))+(((r20)*(sj8))));
10383 evalcond[1]=((x2314)+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2306)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8)))+(((IkReal(-1.00000000000000))*(x2316))));
10384 evalcond[2]=((((IkReal(-1.00000000000000))*(x2306)*(x2307)))+(((IkReal(-1.00000000000000))*(x2311)*(x2313)))+(x2314)+(((IkReal(-1.00000000000000))*(x2308)*(x2311)))+(((IkReal(-1.00000000000000))*(x2306)*(x2312)))+(((IkReal(-1.00000000000000))*(x2316))));
10385 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2311)*(x2312)))+(((x2308)*(x2309)))+(x2315)+(((cj5)*(x2304)))+(((x2309)*(x2313)))+(((IkReal(-1.00000000000000))*(cj7)*(x2307)*(x2311)))+(((cj3)*(r02)*(sj7))));
10386 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10387 {
10388 continue;
10389 }
10390 }
10391
10392 {
10393 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10394 vinfos[0].jointtype = 1;
10395 vinfos[0].foffset = j3;
10396 vinfos[0].indices[0] = _ij3[0];
10397 vinfos[0].indices[1] = _ij3[1];
10398 vinfos[0].maxsolutions = _nj3;
10399 vinfos[1].jointtype = 1;
10400 vinfos[1].foffset = j4;
10401 vinfos[1].indices[0] = _ij4[0];
10402 vinfos[1].indices[1] = _ij4[1];
10403 vinfos[1].maxsolutions = _nj4;
10404 vinfos[2].jointtype = 1;
10405 vinfos[2].foffset = j5;
10406 vinfos[2].indices[0] = _ij5[0];
10407 vinfos[2].indices[1] = _ij5[1];
10408 vinfos[2].maxsolutions = _nj5;
10409 vinfos[3].jointtype = 1;
10410 vinfos[3].foffset = j6;
10411 vinfos[3].indices[0] = _ij6[0];
10412 vinfos[3].indices[1] = _ij6[1];
10413 vinfos[3].maxsolutions = _nj6;
10414 vinfos[4].jointtype = 1;
10415 vinfos[4].foffset = j7;
10416 vinfos[4].indices[0] = _ij7[0];
10417 vinfos[4].indices[1] = _ij7[1];
10418 vinfos[4].maxsolutions = _nj7;
10419 vinfos[5].jointtype = 1;
10420 vinfos[5].foffset = j8;
10421 vinfos[5].indices[0] = _ij8[0];
10422 vinfos[5].indices[1] = _ij8[1];
10423 vinfos[5].maxsolutions = _nj8;
10424 std::vector<int> vfree(0);
10425 solutions.AddSolution(vinfos,vfree);
10426 }
10427 }
10428 }
10429
10430 }
10431
10432 }
10433
10434 } else
10435 {
10436 IkReal x2317=((IkReal(1.00000000000000))*(sj7));
10437 IkReal x2318=((IkReal(1.00000000000000))*(cj3));
10438 IkReal x2319=((cj7)*(r02));
10439 IkReal x2320=((cj8)*(r00));
10440 IkReal x2321=((cj3)*(sj7));
10441 IkReal x2322=((cj8)*(npx));
10442 IkReal x2323=((r01)*(sj3));
10443 IkReal x2324=((cj7)*(sj8));
10444 IkReal x2325=((r11)*(sj8));
10445 IkReal x2326=((IkReal(1.00000000000000))*(cj7));
10446 IkReal x2327=((sj7)*(sj8));
10447 IkReal x2328=((cj8)*(r10));
10448 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j6)), IkReal(6.28318530717959))));
10449 evalcond[1]=((IkReal(0.0300000000000000))+(((npx)*(sj8)))+(((cj8)*(npy)))+(((IkReal(-0.250000000000000))*(sj5))));
10450 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj7)))+(((npy)*(x2324)))+(((IkReal(-1.00000000000000))*(x2322)*(x2326)))+(((IkReal(0.250000000000000))*(cj5)))+(((IkReal(-1.00000000000000))*(npz)*(x2317))));
10451 evalcond[3]=((((cj8)*(x2323)))+(((IkReal(-1.00000000000000))*(r10)*(sj8)*(x2318)))+(((r00)*(sj3)*(sj8)))+(((IkReal(-1.00000000000000))*(cj8)*(r11)*(x2318))));
10452 evalcond[4]=((((IkReal(-1.00000000000000))*(cj8)*(r20)*(x2317)))+(((r21)*(x2327)))+(((cj7)*(r22))));
10453 evalcond[5]=((IkReal(0.0950000000000000))+(((IkReal(0.0900000000000000))*(sj7)))+(((npy)*(x2327)))+(((IkReal(-1.00000000000000))*(x2317)*(x2322)))+(((cj7)*(npz))));
10454 evalcond[6]=((((cj3)*(cj7)*(x2328)))+(((IkReal(-1.00000000000000))*(r11)*(x2318)*(x2324)))+(((r12)*(x2321)))+(((x2323)*(x2324)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x2317)))+(((IkReal(-1.00000000000000))*(sj3)*(x2320)*(x2326))));
10455 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x2317)*(x2325)))+(((sj3)*(x2319)))+(((x2321)*(x2328)))+(((IkReal(-1.00000000000000))*(cj7)*(r12)*(x2318)))+(((IkReal(-1.00000000000000))*(sj3)*(x2317)*(x2320)))+(((x2323)*(x2327))));
10456 evalcond[8]=((((IkReal(-1.00000000000000))*(x2318)*(x2319)))+(((x2320)*(x2321)))+(((IkReal(-1.00000000000000))*(sj3)*(x2317)*(x2325)))+(((sj3)*(sj7)*(x2328)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(sj8)*(x2317)))+(((IkReal(-1.00000000000000))*(r12)*(sj3)*(x2326))));
10457 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 )
10458 {
10459 {
10460 IkReal dummyeval[1];
10461 IkReal gconst50;
10462 gconst50=IKsign(((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5))))));
10463 dummyeval[0]=((((IkReal(-1.00000000000000))*((cj5)*(cj5))))+(((IkReal(-1.00000000000000))*((sj5)*(sj5)))));
10464 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10465 {
10466 {
10467 IkReal dummyeval[1];
10468 IkReal gconst51;
10469 gconst51=IKsign((((cj5)*(cj5))+((sj5)*(sj5))));
10470 dummyeval[0]=(((cj5)*(cj5))+((sj5)*(sj5)));
10471 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10472 {
10473 continue;
10474
10475 } else
10476 {
10477 {
10478 IkReal j4array[1], cj4array[1], sj4array[1];
10479 bool j4valid[1]={false};
10480 _nj4 = 1;
10481 IkReal x2329=((IkReal(1.00000000000000))*(sj5));
10482 IkReal x2330=((r20)*(sj8));
10483 IkReal x2331=((cj5)*(cj8));
10484 IkReal x2332=((r11)*(sj3));
10485 IkReal x2333=((cj3)*(r01));
10486 IkReal x2334=((cj3)*(r00)*(sj8));
10487 IkReal x2335=((r10)*(sj3)*(sj8));
10488 if( IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(r21)*(x2331)))+(((IkReal(-1.00000000000000))*(cj5)*(x2330)))+(((IkReal(-1.00000000000000))*(cj8)*(x2329)*(x2333)))+(((IkReal(-1.00000000000000))*(cj8)*(x2329)*(x2332)))+(((IkReal(-1.00000000000000))*(x2329)*(x2334)))+(((IkReal(-1.00000000000000))*(x2329)*(x2335))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((x2331)*(x2332)))+(((x2331)*(x2333)))+(((IkReal(-1.00000000000000))*(x2329)*(x2330)))+(((cj5)*(x2334)))+(((cj5)*(x2335)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x2329))))))) < IKFAST_ATAN2_MAGTHRESH )
10489 continue;
10490 j4array[0]=IKatan2(((gconst51)*(((((IkReal(-1.00000000000000))*(r21)*(x2331)))+(((IkReal(-1.00000000000000))*(cj5)*(x2330)))+(((IkReal(-1.00000000000000))*(cj8)*(x2329)*(x2333)))+(((IkReal(-1.00000000000000))*(cj8)*(x2329)*(x2332)))+(((IkReal(-1.00000000000000))*(x2329)*(x2334)))+(((IkReal(-1.00000000000000))*(x2329)*(x2335)))))), ((gconst51)*(((((x2331)*(x2332)))+(((x2331)*(x2333)))+(((IkReal(-1.00000000000000))*(x2329)*(x2330)))+(((cj5)*(x2334)))+(((cj5)*(x2335)))+(((IkReal(-1.00000000000000))*(cj8)*(r21)*(x2329)))))));
10491 sj4array[0]=IKsin(j4array[0]);
10492 cj4array[0]=IKcos(j4array[0]);
10493 if( j4array[0] > IKPI )
10494 {
10495 j4array[0]-=IK2PI;
10496 }
10497 else if( j4array[0] < -IKPI )
10498 { j4array[0]+=IK2PI;
10499 }
10500 j4valid[0] = true;
10501 for(int ij4 = 0; ij4 < 1; ++ij4)
10502 {
10503 if( !j4valid[ij4] )
10504 {
10505 continue;
10506 }
10507 _ij4[0] = ij4; _ij4[1] = -1;
10508 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10509 {
10510 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10511 {
10512 j4valid[iij4]=false; _ij4[1] = iij4; break;
10513 }
10514 }
10515 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10516 {
10517 IkReal evalcond[4];
10518 IkReal x2336=IKsin(j4);
10519 IkReal x2337=IKcos(j4);
10520 IkReal x2338=((IkReal(1.00000000000000))*(cj8));
10521 IkReal x2339=((cj3)*(r01));
10522 IkReal x2340=((cj3)*(r00));
10523 IkReal x2341=((cj7)*(cj8));
10524 IkReal x2342=((r11)*(sj3));
10525 IkReal x2343=((IkReal(1.00000000000000))*(sj8));
10526 IkReal x2344=((r10)*(sj3));
10527 IkReal x2345=((sj5)*(x2337));
10528 IkReal x2346=((cj5)*(x2336));
10529 IkReal x2347=((cj5)*(x2337));
10530 IkReal x2348=((sj5)*(x2336));
10531 IkReal x2349=((x2345)+(x2346));
10532 evalcond[0]=((x2349)+(((cj8)*(r21)))+(((r20)*(sj8))));
10533 evalcond[1]=((x2348)+(((IkReal(-1.00000000000000))*(x2347)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2338)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
10534 evalcond[2]=((((IkReal(-1.00000000000000))*(x2340)*(x2343)))+(x2347)+(((IkReal(-1.00000000000000))*(x2348)))+(((IkReal(-1.00000000000000))*(x2338)*(x2339)))+(((IkReal(-1.00000000000000))*(x2343)*(x2344)))+(((IkReal(-1.00000000000000))*(x2338)*(x2342))));
10535 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((x2340)*(x2341)))+(x2349)+(((x2341)*(x2344)))+(((IkReal(-1.00000000000000))*(cj7)*(x2339)*(x2343)))+(((IkReal(-1.00000000000000))*(cj7)*(x2342)*(x2343)))+(((cj3)*(r02)*(sj7))));
10536 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10537 {
10538 continue;
10539 }
10540 }
10541
10542 {
10543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10544 vinfos[0].jointtype = 1;
10545 vinfos[0].foffset = j3;
10546 vinfos[0].indices[0] = _ij3[0];
10547 vinfos[0].indices[1] = _ij3[1];
10548 vinfos[0].maxsolutions = _nj3;
10549 vinfos[1].jointtype = 1;
10550 vinfos[1].foffset = j4;
10551 vinfos[1].indices[0] = _ij4[0];
10552 vinfos[1].indices[1] = _ij4[1];
10553 vinfos[1].maxsolutions = _nj4;
10554 vinfos[2].jointtype = 1;
10555 vinfos[2].foffset = j5;
10556 vinfos[2].indices[0] = _ij5[0];
10557 vinfos[2].indices[1] = _ij5[1];
10558 vinfos[2].maxsolutions = _nj5;
10559 vinfos[3].jointtype = 1;
10560 vinfos[3].foffset = j6;
10561 vinfos[3].indices[0] = _ij6[0];
10562 vinfos[3].indices[1] = _ij6[1];
10563 vinfos[3].maxsolutions = _nj6;
10564 vinfos[4].jointtype = 1;
10565 vinfos[4].foffset = j7;
10566 vinfos[4].indices[0] = _ij7[0];
10567 vinfos[4].indices[1] = _ij7[1];
10568 vinfos[4].maxsolutions = _nj7;
10569 vinfos[5].jointtype = 1;
10570 vinfos[5].foffset = j8;
10571 vinfos[5].indices[0] = _ij8[0];
10572 vinfos[5].indices[1] = _ij8[1];
10573 vinfos[5].maxsolutions = _nj8;
10574 std::vector<int> vfree(0);
10575 solutions.AddSolution(vinfos,vfree);
10576 }
10577 }
10578 }
10579
10580 }
10581
10582 }
10583
10584 } else
10585 {
10586 {
10587 IkReal j4array[1], cj4array[1], sj4array[1];
10588 bool j4valid[1]={false};
10589 _nj4 = 1;
10590 IkReal x2350=((cj8)*(sj5));
10591 IkReal x2351=((sj5)*(sj8));
10592 IkReal x2352=((r22)*(sj7));
10593 IkReal x2353=((cj7)*(r20));
10594 IkReal x2354=((cj5)*(cj8));
10595 IkReal x2355=((cj7)*(r21));
10596 IkReal x2356=((cj5)*(sj8));
10597 if( IKabs(((gconst50)*(((((x2351)*(x2355)))+(((r20)*(x2356)))+(((IkReal(-1.00000000000000))*(sj5)*(x2352)))+(((r21)*(x2354)))+(((IkReal(-1.00000000000000))*(x2350)*(x2353))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst50)*(((((IkReal(-1.00000000000000))*(x2355)*(x2356)))+(((x2353)*(x2354)))+(((r20)*(x2351)))+(((r21)*(x2350)))+(((cj5)*(x2352))))))) < IKFAST_ATAN2_MAGTHRESH )
10598 continue;
10599 j4array[0]=IKatan2(((gconst50)*(((((x2351)*(x2355)))+(((r20)*(x2356)))+(((IkReal(-1.00000000000000))*(sj5)*(x2352)))+(((r21)*(x2354)))+(((IkReal(-1.00000000000000))*(x2350)*(x2353)))))), ((gconst50)*(((((IkReal(-1.00000000000000))*(x2355)*(x2356)))+(((x2353)*(x2354)))+(((r20)*(x2351)))+(((r21)*(x2350)))+(((cj5)*(x2352)))))));
10600 sj4array[0]=IKsin(j4array[0]);
10601 cj4array[0]=IKcos(j4array[0]);
10602 if( j4array[0] > IKPI )
10603 {
10604 j4array[0]-=IK2PI;
10605 }
10606 else if( j4array[0] < -IKPI )
10607 { j4array[0]+=IK2PI;
10608 }
10609 j4valid[0] = true;
10610 for(int ij4 = 0; ij4 < 1; ++ij4)
10611 {
10612 if( !j4valid[ij4] )
10613 {
10614 continue;
10615 }
10616 _ij4[0] = ij4; _ij4[1] = -1;
10617 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10618 {
10619 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10620 {
10621 j4valid[iij4]=false; _ij4[1] = iij4; break;
10622 }
10623 }
10624 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10625 {
10626 IkReal evalcond[4];
10627 IkReal x2357=IKsin(j4);
10628 IkReal x2358=IKcos(j4);
10629 IkReal x2359=((IkReal(1.00000000000000))*(cj8));
10630 IkReal x2360=((cj3)*(r01));
10631 IkReal x2361=((cj3)*(r00));
10632 IkReal x2362=((cj7)*(cj8));
10633 IkReal x2363=((r11)*(sj3));
10634 IkReal x2364=((IkReal(1.00000000000000))*(sj8));
10635 IkReal x2365=((r10)*(sj3));
10636 IkReal x2366=((sj5)*(x2358));
10637 IkReal x2367=((cj5)*(x2357));
10638 IkReal x2368=((cj5)*(x2358));
10639 IkReal x2369=((sj5)*(x2357));
10640 IkReal x2370=((x2367)+(x2366));
10641 evalcond[0]=((x2370)+(((cj8)*(r21)))+(((r20)*(sj8))));
10642 evalcond[1]=((x2369)+(((IkReal(-1.00000000000000))*(x2368)))+(((IkReal(-1.00000000000000))*(cj7)*(r20)*(x2359)))+(((IkReal(-1.00000000000000))*(r22)*(sj7)))+(((cj7)*(r21)*(sj8))));
10643 evalcond[2]=((x2368)+(((IkReal(-1.00000000000000))*(x2369)))+(((IkReal(-1.00000000000000))*(x2359)*(x2363)))+(((IkReal(-1.00000000000000))*(x2359)*(x2360)))+(((IkReal(-1.00000000000000))*(x2364)*(x2365)))+(((IkReal(-1.00000000000000))*(x2361)*(x2364))));
10644 evalcond[3]=((((r12)*(sj3)*(sj7)))+(((IkReal(-1.00000000000000))*(cj7)*(x2360)*(x2364)))+(x2370)+(((x2361)*(x2362)))+(((cj3)*(r02)*(sj7)))+(((x2362)*(x2365)))+(((IkReal(-1.00000000000000))*(cj7)*(x2363)*(x2364))));
10645 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10646 {
10647 continue;
10648 }
10649 }
10650
10651 {
10652 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10653 vinfos[0].jointtype = 1;
10654 vinfos[0].foffset = j3;
10655 vinfos[0].indices[0] = _ij3[0];
10656 vinfos[0].indices[1] = _ij3[1];
10657 vinfos[0].maxsolutions = _nj3;
10658 vinfos[1].jointtype = 1;
10659 vinfos[1].foffset = j4;
10660 vinfos[1].indices[0] = _ij4[0];
10661 vinfos[1].indices[1] = _ij4[1];
10662 vinfos[1].maxsolutions = _nj4;
10663 vinfos[2].jointtype = 1;
10664 vinfos[2].foffset = j5;
10665 vinfos[2].indices[0] = _ij5[0];
10666 vinfos[2].indices[1] = _ij5[1];
10667 vinfos[2].maxsolutions = _nj5;
10668 vinfos[3].jointtype = 1;
10669 vinfos[3].foffset = j6;
10670 vinfos[3].indices[0] = _ij6[0];
10671 vinfos[3].indices[1] = _ij6[1];
10672 vinfos[3].maxsolutions = _nj6;
10673 vinfos[4].jointtype = 1;
10674 vinfos[4].foffset = j7;
10675 vinfos[4].indices[0] = _ij7[0];
10676 vinfos[4].indices[1] = _ij7[1];
10677 vinfos[4].maxsolutions = _nj7;
10678 vinfos[5].jointtype = 1;
10679 vinfos[5].foffset = j8;
10680 vinfos[5].indices[0] = _ij8[0];
10681 vinfos[5].indices[1] = _ij8[1];
10682 vinfos[5].maxsolutions = _nj8;
10683 std::vector<int> vfree(0);
10684 solutions.AddSolution(vinfos,vfree);
10685 }
10686 }
10687 }
10688
10689 }
10690
10691 }
10692
10693 } else
10694 {
<