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 "nextage_ik_plugin/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;
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.0900000000000000))*(x4));
00226 x14=((IkReal(0.258820826967441))*(x11));
00227 x15=((IkReal(1.00000000000000))*(x8));
00228 x16=((IkReal(1.00000000000000))*(x5));
00229 x17=((IkReal(0.965925348838040))*(x11));
00230 x18=((IkReal(0.0470000000000000))*(x3));
00231 x19=((IkReal(0.965925348838040))*(x9));
00232 x20=((IkReal(1.00000000000000))*(x9));
00233 x21=((IkReal(0.00776462480902323))*(x2));
00234 x22=((IkReal(0.965925348838040))*(x8));
00235 x23=((IkReal(1.00000000000000))*(x2));
00236 x24=((IkReal(0.258820826967441))*(x8));
00237 x25=((IkReal(1.00000000000000))*(x7));
00238 x26=((IkReal(1.00000000000000))*(x3));
00239 x27=((IkReal(0.0900000000000000))*(x6));
00240 x28=((IkReal(0.965925348838040))*(x10));
00241 x29=((IkReal(0.258820826967441))*(x10));
00242 x30=((IkReal(1.00000000000000))*(x6));
00243 x31=((x0)*(x5));
00244 x32=((x1)*(x3));
00245 x33=((x6)*(x9));
00246 x34=((x4)*(x7));
00247 x35=((x0)*(x4));
00248 x36=((IkReal(-0.0470000000000000))*(x6));
00249 x37=((x3)*(x4));
00250 x38=((x2)*(x4));
00251 x39=((x1)*(x7));
00252 x40=((x1)*(x2));
00253 x41=((x5)*(x7));
00254 x42=((x16)*(x7));
00255 x43=((x25)*(x6));
00256 x44=((IkReal(0.0470000000000000))*(x40));
00257 x45=((x23)*(x35));
00258 x46=((x23)*(x34));
00259 x47=((((IkReal(-1.00000000000000))*(x1)*(x26)))+(x38));
00260 x48=((((x1)*(x18)))+(((IkReal(-0.0470000000000000))*(x38))));
00261 x49=((((IkReal(-0.0900000000000000))*(x32)))+(((x13)*(x2))));
00262 x50=((((x1)*(x23)))+(((x26)*(x4))));
00263 x51=((IkReal(-1.00000000000000))*(x50));
00264 x52=((((x18)*(x4)))+(x44));
00265 x53=((((IkReal(0.0900000000000000))*(x40)))+(((x13)*(x3))));
00266 x54=((((IkReal(-1.00000000000000))*(x45)))+(((x0)*(x32))));
00267 x55=((((IkReal(-1.00000000000000))*(x46)))+(((x32)*(x7))));
00268 x56=((x52)*(x6));
00269 x57=((x5)*(x51));
00270 x58=((x51)*(x6));
00271 x59=((((IkReal(-1.00000000000000))*(x0)*(x1)*(x26)))+(x45));
00272 x60=((((IkReal(-1.00000000000000))*(x25)*(x32)))+(x46));
00273 x61=((((IkReal(-1.00000000000000))*(x0)*(x1)*(x23)))+(((IkReal(-1.00000000000000))*(x26)*(x35))));
00274 x62=((((IkReal(-1.00000000000000))*(x25)*(x37)))+(((IkReal(-1.00000000000000))*(x23)*(x39))));
00275 x63=((((IkReal(0.0470000000000000))*(x2)*(x39)))+(((x18)*(x34))));
00276 x64=((((IkReal(-0.0900000000000000))*(x2)*(x39)))+(((IkReal(-1.00000000000000))*(x13)*(x3)*(x7))));
00277 x65=((x55)*(x6));
00278 x66=((x54)*(x6));
00279 x67=((x10)*(x5)*(x50));
00280 x68=((x5)*(x59));
00281 x69=((x31)+(x65));
00282 x70=((((x0)*(x6)))+(((x5)*(x60))));
00283 x71=((((IkReal(-1.00000000000000))*(x0)*(x30)))+(((IkReal(-1.00000000000000))*(x16)*(x60))));
00284 x72=((((IkReal(-1.00000000000000))*(x15)*(x47)))+(((IkReal(-1.00000000000000))*(x20)*(x50)*(x6))));
00285 x73=((x11)*(x72));
00286 x74=((((x20)*(((((IkReal(-1.00000000000000))*(x42)))+(((x30)*(x54)))))))+(((IkReal(-1.00000000000000))*(x15)*(x61))));
00287 x75=((((x20)*(((((x0)*(x16)))+(((x30)*(x55)))))))+(((IkReal(-1.00000000000000))*(x15)*(x62))));
00288 x76=((x10)*(x75));
00289 eerot[0]=((((x8)*(((((IkReal(-1.00000000000000))*(x42)))+(x66)))))+(((x61)*(x9))));
00290 eerot[1]=((((x11)*(x74)))+(((x10)*(((((IkReal(-1.00000000000000))*(x43)))+(x68))))));
00291 eerot[2]=((((x10)*(x74)))+(((x11)*(((x43)+(((IkReal(-1.00000000000000))*(x16)*(x59))))))));
00292 IkReal x77=((x0)*(x40));
00293 eetrans[0]=((((IkReal(-0.235000000000000))*(x77)))+(((IkReal(-0.235000000000000))*(x3)*(x35)))+(((IkReal(-0.250000000000000))*(x35)))+(((x8)*(((((x36)*(x54)))+(((IkReal(0.0470000000000000))*(x41)))))))+(((IkReal(0.0950000000000000))*(x7)))+(((x9)*(((((IkReal(0.0900000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(x27)*(x54)))))))+(((IkReal(0.0300000000000000))*(x2)*(x35)))+(((x8)*(((((IkReal(-1.00000000000000))*(x0)*(x13)*(x3)))+(((IkReal(-0.0900000000000000))*(x77)))))))+(((IkReal(-0.0300000000000000))*(x0)*(x32)))+(((x9)*(((((x18)*(x35)))+(((x0)*(x44))))))));
00294 eerot[3]=((((IkReal(-1.00000000000000))*(x12)*(x47)))+(((x19)*(x62)))+(((x22)*(x69)))+(((IkReal(-1.00000000000000))*(x24)*(x58))));
00295 eerot[4]=((((x17)*(x75)))+(((x28)*(x70)))+(((IkReal(-1.00000000000000))*(x29)*(x5)*(x50)))+(((IkReal(-1.00000000000000))*(x14)*(x72))));
00296 eerot[5]=((((x17)*(x71)))+(((x28)*(x75)))+(((IkReal(-1.00000000000000))*(x14)*(x57)))+(((IkReal(-1.00000000000000))*(x29)*(x72))));
00297 IkReal x78=((IkReal(1.00000000000000))*(x24));
00298 IkReal x79=((IkReal(1.00000000000000))*(x12));
00299 eetrans[1]=((IkReal(-0.145000000000000))+(((IkReal(-0.0608228943373486))*(x38)))+(((IkReal(-0.226992456976939))*(x3)*(x34)))+(((IkReal(0.0647052067418602))*(x1)))+(((IkReal(-0.226992456976939))*(x2)*(x39)))+(((IkReal(-1.00000000000000))*(x53)*(x6)*(x79)))+(((x22)*(((((x36)*(x55)))+(((IkReal(-0.0470000000000000))*(x31)))))))+(((IkReal(-1.00000000000000))*(x1)*(x21)))+(((IkReal(-0.0289777604651412))*(x32)*(x7)))+(((x19)*(x63)))+(((IkReal(-0.241481337209510))*(x34)))+(((x19)*(((((IkReal(-1.00000000000000))*(x27)*(x55)))+(((IkReal(-0.0900000000000000))*(x31)))))))+(((IkReal(-1.00000000000000))*(x49)*(x78)))+(((IkReal(-1.00000000000000))*(x48)*(x79)))+(((IkReal(0.0608228943373486))*(x32)))+(((IkReal(0.0289777604651412))*(x2)*(x34)))+(((IkReal(-0.0917629081396138))*(x0)))+(((IkReal(-1.00000000000000))*(x56)*(x78)))+(((IkReal(-0.00776462480902323))*(x37)))+(((x22)*(x64))));
00300 eerot[6]=((((x12)*(x62)))+(((x19)*(x47)))+(((x24)*(x69)))+(((x22)*(x58))));
00301 eerot[7]=((((x14)*(x75)))+(((x17)*(x72)))+(((x28)*(x5)*(x50)))+(((x29)*(x70))));
00302 eerot[8]=((((x14)*(x71)))+(((x17)*(x57)))+(((x28)*(x72)))+(((x29)*(x75))));
00303 eetrans[2]=((IkReal(0.370300000000000))+(((x12)*(x63)))+(((IkReal(-0.226992456976939))*(x32)))+(((IkReal(0.0289777604651412))*(x37)))+(((x19)*(x48)))+(((x24)*(((((IkReal(-0.0470000000000000))*(x65)))+(((IkReal(-0.0470000000000000))*(x31)))))))+(((IkReal(-0.00776462480902323))*(x32)*(x7)))+(((IkReal(0.0289777604651412))*(x40)))+(((IkReal(0.226992456976939))*(x38)))+(((x19)*(x53)*(x6)))+(((x22)*(x49)))+(((x21)*(x34)))+(((x24)*(x64)))+(((IkReal(-0.0245879785619069))*(x0)))+(((IkReal(-0.241481337209510))*(x1)))+(((IkReal(-0.0608228943373486))*(x2)*(x39)))+(((x12)*(((((IkReal(-0.0900000000000000))*(x65)))+(((IkReal(-0.0900000000000000))*(x31)))))))+(((IkReal(-0.0608228943373486))*(x3)*(x34)))+(((x22)*(x56)))+(((IkReal(-0.0647052067418602))*(x34))));
00304 }
00305
00306 IKFAST_API int GetNumFreeParameters() { return 0; }
00307 IKFAST_API int* GetFreeParameters() { return NULL; }
00308 IKFAST_API int GetNumJoints() { return 6; }
00309
00310 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00311
00312 IKFAST_API int GetIkType() { return 0x67000001; }
00313
00314 class IKSolver {
00315 public:
00316 IkReal j9,cj9,sj9,htj9,j10,cj10,sj10,htj10,j11,cj11,sj11,htj11,j12,cj12,sj12,htj12,j13,cj13,sj13,htj13,j14,cj14,sj14,htj14,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;
00317 unsigned char _ij9[2], _nj9,_ij10[2], _nj10,_ij11[2], _nj11,_ij12[2], _nj12,_ij13[2], _nj13,_ij14[2], _nj14;
00318
00319 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00320 j9=numeric_limits<IkReal>::quiet_NaN(); _ij9[0] = -1; _ij9[1] = -1; _nj9 = -1; j10=numeric_limits<IkReal>::quiet_NaN(); _ij10[0] = -1; _ij10[1] = -1; _nj10 = -1; j11=numeric_limits<IkReal>::quiet_NaN(); _ij11[0] = -1; _ij11[1] = -1; _nj11 = -1; j12=numeric_limits<IkReal>::quiet_NaN(); _ij12[0] = -1; _ij12[1] = -1; _nj12 = -1; j13=numeric_limits<IkReal>::quiet_NaN(); _ij13[0] = -1; _ij13[1] = -1; _nj13 = -1; j14=numeric_limits<IkReal>::quiet_NaN(); _ij14[0] = -1; _ij14[1] = -1; _nj14 = -1;
00321 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00322 solutions.Clear();
00323 r00 = eerot[0*3+0];
00324 r01 = eerot[0*3+1];
00325 r02 = eerot[0*3+2];
00326 r10 = eerot[1*3+0];
00327 r11 = eerot[1*3+1];
00328 r12 = eerot[1*3+2];
00329 r20 = eerot[2*3+0];
00330 r21 = eerot[2*3+1];
00331 r22 = eerot[2*3+2];
00332 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00333
00334 new_r00=((IkReal(-1.00000000000000))*(r02));
00335 new_r01=r01;
00336 new_r02=r00;
00337 new_px=((px)+(((IkReal(0.0470000000000000))*(r00))));
00338 new_r10=((((IkReal(-0.965925348838040))*(r12)))+(((IkReal(-0.258820826967441))*(r22))));
00339 new_r11=((((IkReal(0.258820826967441))*(r21)))+(((IkReal(0.965925348838040))*(r11))));
00340 new_r12=((((IkReal(0.258820826967441))*(r20)))+(((IkReal(0.965925348838040))*(r10))));
00341 new_py=((IkReal(0.0442178233554725))+(((IkReal(0.965925348838040))*(py)))+(((IkReal(0.0453984913953879))*(r10)))+(((IkReal(0.258820826967441))*(pz)))+(((IkReal(0.0121645788674697))*(r20))));
00342 new_r20=((((IkReal(0.258820826967441))*(r12)))+(((IkReal(-0.965925348838040))*(r22))));
00343 new_r21=((((IkReal(0.965925348838040))*(r21)))+(((IkReal(-0.258820826967441))*(r11))));
00344 new_r22=((((IkReal(0.965925348838040))*(r20)))+(((IkReal(-0.258820826967441))*(r10))));
00345 new_pz=((IkReal(-0.395211176585005))+(((IkReal(0.965925348838040))*(pz)))+(((IkReal(-0.0121645788674697))*(r10)))+(((IkReal(0.0453984913953879))*(r20)))+(((IkReal(-0.258820826967441))*(py))));
00346 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;
00347 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00348 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00349 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00350 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00351 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
00352 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
00353 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
00354 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
00355 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
00356 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
00357 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
00358 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
00359 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
00360 IkReal op[72], zeror[48];
00361 int numroots;
00362 IkReal x80=((IkReal(0.650000000000000))*(npx));
00363 IkReal x81=((IkReal(0.0600000000000000))*(npz));
00364 IkReal x82=((IkReal(1.00000000000000))*(pp));
00365 IkReal x83=((IkReal(0.0600000000000000))*(npy));
00366 IkReal x84=((IkReal(0.0950000000000000))*(r22));
00367 IkReal x85=((IkReal(0.120000000000000))*(npx));
00368 IkReal x86=((IkReal(0.0950000000000000))*(r21));
00369 IkReal x87=((IkReal(1.00000000000000))*(rxp0_2));
00370 IkReal x88=((IkReal(0.190000000000000))*(r20));
00371 IkReal x89=((IkReal(2.00000000000000))*(rxp1_2));
00372 IkReal x90=((IkReal(0.940000000000000))*(npz));
00373 IkReal x91=((IkReal(2.00000000000000))*(rxp2_2));
00374 IkReal x92=((IkReal(0.180000000000000))*(r21));
00375 IkReal x93=((IkReal(0.360000000000000))*(r20));
00376 IkReal x94=((IkReal(0.380000000000000))*(r21));
00377 IkReal x95=((IkReal(0.290000000000000))*(npx));
00378 IkReal x96=((IkReal(-0.00570000000000000))+(((IkReal(-1.00000000000000))*(x83))));
00379 IkReal x97=((IkReal(0.580000000000000))*(npy));
00380 IkReal x98=((IkReal(0.00570000000000000))+(((IkReal(-1.00000000000000))*(x83))));
00381 IkReal x99=((rxp0_2)+(((IkReal(-1.00000000000000))*(x84))));
00382 IkReal x100=((IkReal(-1.30000000000000))*(npy));
00383 IkReal x101=((IkReal(-0.0950000000000000))*(r21));
00384 IkReal x102=((IkReal(-0.00570000000000000))+(x83));
00385 IkReal x103=((rxp0_2)+(x84));
00386 IkReal x104=((x84)+(((IkReal(-1.00000000000000))*(x87))));
00387 IkReal x105=((IkReal(0.00570000000000000))+(x83));
00388 IkReal x106=((IkReal(-2.00000000000000))*(rxp1_2));
00389 IkReal x107=((IkReal(-0.0350000000000000))+(x80));
00390 IkReal x108=((IkReal(0.0496000000000000))+(x81));
00391 IkReal x109=((x82)+(x81));
00392 IkReal x110=((((IkReal(-1.00000000000000))*(x84)))+(((IkReal(-1.00000000000000))*(x87))));
00393 IkReal x111=((x85)+(x90));
00394 IkReal x112=((x91)+(x92));
00395 op[0]=((((IkReal(-1.00000000000000))*(x109)))+(x107));
00396 op[1]=x102;
00397 op[2]=x100;
00398 op[3]=x85;
00399 op[4]=((IkReal(-0.0350000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(x80))));
00400 op[5]=x96;
00401 op[6]=x110;
00402 op[7]=x86;
00403 op[8]=x89;
00404 op[9]=x88;
00405 op[10]=x99;
00406 op[11]=x101;
00407 op[12]=x105;
00408 op[13]=((x107)+(x81)+(((IkReal(-1.00000000000000))*(x82))));
00409 op[14]=x85;
00410 op[15]=x100;
00411 op[16]=x98;
00412 op[17]=((IkReal(-0.0350000000000000))+(x81)+(((IkReal(-1.00000000000000))*(x80)))+(((IkReal(-1.00000000000000))*(x82))));
00413 op[18]=x86;
00414 op[19]=x104;
00415 op[20]=x88;
00416 op[21]=x89;
00417 op[22]=x101;
00418 op[23]=x103;
00419 op[24]=((IkReal(-0.0108000000000000))+(x111));
00420 op[25]=IkReal(0);
00421 op[26]=((IkReal(-0.240000000000000))*(npy));
00422 op[27]=IkReal(0);
00423 op[28]=((IkReal(-0.0108000000000000))+(x90)+(((IkReal(-1.00000000000000))*(x85))));
00424 op[29]=IkReal(0);
00425 op[30]=((x88)+(x92)+(((IkReal(-1.00000000000000))*(x91))));
00426 op[31]=IkReal(0);
00427 op[32]=((x93)+(((IkReal(-1.00000000000000))*(x94))));
00428 op[33]=IkReal(0);
00429 op[34]=((((IkReal(-1.00000000000000))*(x112)))+(((IkReal(-1.00000000000000))*(x88))));
00430 op[35]=IkReal(0);
00431 op[36]=IkReal(0);
00432 op[37]=((IkReal(0.0108000000000000))+(x90)+(((IkReal(-1.00000000000000))*(x85))));
00433 op[38]=IkReal(0);
00434 op[39]=((IkReal(0.240000000000000))*(npy));
00435 op[40]=IkReal(0);
00436 op[41]=((IkReal(0.0108000000000000))+(x111));
00437 op[42]=IkReal(0);
00438 op[43]=((x92)+(((IkReal(-1.00000000000000))*(x91)))+(((IkReal(-1.00000000000000))*(x88))));
00439 op[44]=IkReal(0);
00440 op[45]=((x93)+(x94));
00441 op[46]=IkReal(0);
00442 op[47]=((((IkReal(-1.00000000000000))*(x112)))+(x88));
00443 op[48]=((x108)+(((IkReal(-1.00000000000000))*(x95)))+(((IkReal(-1.00000000000000))*(x82))));
00444 op[49]=x102;
00445 op[50]=x97;
00446 op[51]=x85;
00447 op[52]=((x108)+(x95)+(((IkReal(-1.00000000000000))*(x82))));
00448 op[53]=x96;
00449 op[54]=x103;
00450 op[55]=x86;
00451 op[56]=x106;
00452 op[57]=x88;
00453 op[58]=x104;
00454 op[59]=x101;
00455 op[60]=x105;
00456 op[61]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(x95))));
00457 op[62]=x85;
00458 op[63]=x97;
00459 op[64]=x98;
00460 op[65]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(x95));
00461 op[66]=x86;
00462 op[67]=x99;
00463 op[68]=x88;
00464 op[69]=x106;
00465 op[70]=x101;
00466 op[71]=x110;
00467 solvedialyticpoly8qep(op,zeror,numroots);
00468 IkReal j13array[16], cj13array[16], sj13array[16], j14array[16], cj14array[16], sj14array[16], j12array[16], cj12array[16], sj12array[16];
00469 int numsolutions = 0;
00470 for(int ij13 = 0; ij13 < numroots; ij13 += 3)
00471 {
00472 IkReal htj13 = zeror[ij13+0], htj14 = zeror[ij13+1], htj12 = zeror[ij13+2];
00473 j13array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj13)));
00474 j14array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj14)));
00475 j12array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj12)));
00476 IkReal x113=(htj13)*(htj13);
00477 IkReal x114=(htj14)*(htj14);
00478 IkReal x115=(htj12)*(htj12);
00479 cj13array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x113))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x113)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x113))))));
00480 cj14array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x114))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x114)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x114))))));
00481 cj12array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x115))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x115)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x115))))));
00482 sj13array[numsolutions]=((IkReal(2.00000000000000))*(htj13)*(((IKabs(((IkReal(1.00000000000000))+((htj13)*(htj13)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj13)*(htj13))))):(IkReal)1.0e30)));
00483 sj14array[numsolutions]=((IkReal(2.00000000000000))*(htj14)*(((IKabs(((IkReal(1.00000000000000))+((htj14)*(htj14)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj14)*(htj14))))):(IkReal)1.0e30)));
00484 sj12array[numsolutions]=((IkReal(2.00000000000000))*(htj12)*(((IKabs(((IkReal(1.00000000000000))+((htj12)*(htj12)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj12)*(htj12))))):(IkReal)1.0e30)));
00485 if( j13array[numsolutions] > IKPI )
00486 {
00487 j13array[numsolutions]-=IK2PI;
00488 }
00489 else if( j13array[numsolutions] < -IKPI )
00490 {
00491 j13array[numsolutions]+=IK2PI;
00492 }
00493 if( j14array[numsolutions] > IKPI )
00494 {
00495 j14array[numsolutions]-=IK2PI;
00496 }
00497 else if( j14array[numsolutions] < -IKPI )
00498 {
00499 j14array[numsolutions]+=IK2PI;
00500 }
00501 if( j12array[numsolutions] > IKPI )
00502 {
00503 j12array[numsolutions]-=IK2PI;
00504 }
00505 else if( j12array[numsolutions] < -IKPI )
00506 {
00507 j12array[numsolutions]+=IK2PI;
00508 }
00509 numsolutions++;
00510 }
00511 bool j13valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
00512 _nj13 = 16;
00513 _nj14 = 1;
00514 _nj12 = 1;
00515 for(int ij13 = 0; ij13 < numsolutions; ++ij13)
00516 {
00517 if( !j13valid[ij13] )
00518 {
00519 continue;
00520 }
00521 _ij13[0] = ij13; _ij13[1] = -1;
00522 _ij14[0] = 0; _ij14[1] = -1;
00523 _ij12[0] = 0; _ij12[1] = -1;
00524 for(int iij13 = ij13+1; iij13 < numsolutions; ++iij13)
00525 {
00526 if( !j13valid[iij13] ) { continue; }
00527 if( IKabs(cj13array[ij13]-cj13array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj13array[ij13]-sj13array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(cj14array[ij13]-cj14array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj14array[ij13]-sj14array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(cj12array[ij13]-cj12array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij13]-sj12array[iij13]) < IKFAST_SOLUTION_THRESH && 1 )
00528 {
00529 j13valid[iij13]=false; _ij13[1] = iij13; _ij14[1] = 0; _ij12[1] = 0; break;
00530 }
00531 }
00532 j13 = j13array[ij13]; cj13 = cj13array[ij13]; sj13 = sj13array[ij13];
00533
00534 j14 = j14array[ij13]; cj14 = cj14array[ij13]; sj14 = sj14array[ij13];
00535
00536 j12 = j12array[ij13]; cj12 = cj12array[ij13]; sj12 = sj12array[ij13];
00537
00538 {
00539 IkReal dummyeval[1];
00540 IkReal gconst0;
00541 IkReal x116=(cj14)*(cj14);
00542 IkReal x117=(sj14)*(sj14);
00543 IkReal x118=((IkReal(1.00000000000000))*(r01));
00544 IkReal x119=((sj13)*(sj14));
00545 IkReal x120=((cj14)*(sj13));
00546 IkReal x121=((r00)*(r11));
00547 IkReal x122=((cj13)*(x116));
00548 IkReal x123=((cj13)*(x117));
00549 gconst0=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x118)*(x120)))+(((x121)*(x122)))+(((x121)*(x123)))+(((r02)*(r10)*(x119)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x119)))+(((IkReal(-1.00000000000000))*(r10)*(x118)*(x123)))+(((IkReal(-1.00000000000000))*(r10)*(x118)*(x122)))+(((r02)*(r11)*(x120)))));
00550 IkReal x124=(cj14)*(cj14);
00551 IkReal x125=(sj14)*(sj14);
00552 IkReal x126=((IkReal(1.00000000000000))*(r01));
00553 IkReal x127=((sj13)*(sj14));
00554 IkReal x128=((cj14)*(sj13));
00555 IkReal x129=((r00)*(r11));
00556 IkReal x130=((cj13)*(x124));
00557 IkReal x131=((cj13)*(x125));
00558 dummyeval[0]=((((IkReal(-1.00000000000000))*(r12)*(x126)*(x128)))+(((x129)*(x130)))+(((x129)*(x131)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x127)))+(((r02)*(r11)*(x128)))+(((r02)*(r10)*(x127)))+(((IkReal(-1.00000000000000))*(r10)*(x126)*(x130)))+(((IkReal(-1.00000000000000))*(r10)*(x126)*(x131))));
00559 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00560 {
00561 {
00562 IkReal dummyeval[1];
00563 IkReal gconst1;
00564 IkReal x132=(sj14)*(sj14);
00565 IkReal x133=(cj14)*(cj14);
00566 IkReal x134=((cj13)*(r12));
00567 IkReal x135=((IkReal(1.00000000000000))*(r10));
00568 IkReal x136=((cj13)*(r02));
00569 IkReal x137=((r01)*(sj13));
00570 IkReal x138=((r00)*(r11)*(sj13));
00571 gconst1=IKsign(((((r00)*(sj14)*(x134)))+(((IkReal(-1.00000000000000))*(x132)*(x135)*(x137)))+(((x133)*(x138)))+(((x132)*(x138)))+(((cj14)*(r01)*(x134)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x136)))+(((IkReal(-1.00000000000000))*(sj14)*(x135)*(x136)))+(((IkReal(-1.00000000000000))*(x133)*(x135)*(x137)))));
00572 IkReal x139=(sj14)*(sj14);
00573 IkReal x140=(cj14)*(cj14);
00574 IkReal x141=((cj13)*(r12));
00575 IkReal x142=((IkReal(1.00000000000000))*(r10));
00576 IkReal x143=((cj13)*(r02));
00577 IkReal x144=((r01)*(sj13));
00578 IkReal x145=((r00)*(r11)*(sj13));
00579 dummyeval[0]=((((x140)*(x145)))+(((cj14)*(r01)*(x141)))+(((IkReal(-1.00000000000000))*(x140)*(x142)*(x144)))+(((IkReal(-1.00000000000000))*(sj14)*(x142)*(x143)))+(((r00)*(sj14)*(x141)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x143)))+(((IkReal(-1.00000000000000))*(x139)*(x142)*(x144)))+(((x139)*(x145))));
00580 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00581 {
00582 {
00583 IkReal dummyeval[1];
00584 dummyeval[0]=sj12;
00585 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00586 {
00587 {
00588 IkReal evalcond[3];
00589 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
00590 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
00591 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
00592 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
00593 {
00594 {
00595 IkReal j11array[1], cj11array[1], sj11array[1];
00596 bool j11valid[1]={false};
00597 _nj11 = 1;
00598 IkReal x146=((IkReal(4.00000000000000))*(cj13));
00599 IkReal x147=((npy)*(sj14));
00600 IkReal x148=((cj14)*(npx));
00601 IkReal x149=((IkReal(4.00000000000000))*(sj13));
00602 if( IKabs(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148)))))-1) <= IKFAST_SINCOS_THRESH )
00603 continue;
00604 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149)))), ((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148)))));
00605 sj11array[0]=IKsin(j11array[0]);
00606 cj11array[0]=IKcos(j11array[0]);
00607 if( j11array[0] > IKPI )
00608 {
00609 j11array[0]-=IK2PI;
00610 }
00611 else if( j11array[0] < -IKPI )
00612 { j11array[0]+=IK2PI;
00613 }
00614 j11valid[0] = true;
00615 for(int ij11 = 0; ij11 < 1; ++ij11)
00616 {
00617 if( !j11valid[ij11] )
00618 {
00619 continue;
00620 }
00621 _ij11[0] = ij11; _ij11[1] = -1;
00622 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
00623 {
00624 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
00625 {
00626 j11valid[iij11]=false; _ij11[1] = iij11; break;
00627 }
00628 }
00629 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
00630 {
00631 IkReal evalcond[2];
00632 IkReal x150=((IkReal(1.00000000000000))*(sj13));
00633 IkReal x151=((cj14)*(npx));
00634 IkReal x152=((npy)*(sj14));
00635 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x152)))+(((IkReal(-1.00000000000000))*(cj13)*(x151)))+(((IkReal(-1.00000000000000))*(npz)*(x150))));
00636 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((IkReal(-1.00000000000000))*(x150)*(x151)))+(((sj13)*(x152)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
00637 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
00638 {
00639 continue;
00640 }
00641 }
00642
00643 {
00644 IkReal dummyeval[1];
00645 IkReal gconst66;
00646 IkReal x153=(sj14)*(sj14);
00647 IkReal x154=(cj14)*(cj14);
00648 IkReal x155=((IkReal(2.00000000000000))*(cj14)*(sj14));
00649 gconst66=IKsign(((((x153)*((r00)*(r00))))+(((x154)*((r01)*(r01))))+(((x154)*((r11)*(r11))))+(((r10)*(r11)*(x155)))+(((x153)*((r10)*(r10))))+(((r00)*(r01)*(x155)))));
00650 IkReal x156=(sj14)*(sj14);
00651 IkReal x157=(cj14)*(cj14);
00652 IkReal x158=((IkReal(2.00000000000000))*(cj14)*(sj14));
00653 dummyeval[0]=((((x156)*((r00)*(r00))))+(((x157)*((r01)*(r01))))+(((x156)*((r10)*(r10))))+(((r10)*(r11)*(x158)))+(((x157)*((r11)*(r11))))+(((r00)*(r01)*(x158))));
00654 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00655 {
00656 {
00657 IkReal dummyeval[1];
00658 IkReal gconst68;
00659 gconst68=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
00660 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
00661 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00662 {
00663 {
00664 IkReal dummyeval[1];
00665 IkReal gconst67;
00666 IkReal x159=(cj14)*(cj14);
00667 IkReal x160=(sj14)*(sj14);
00668 IkReal x161=((IkReal(1.00000000000000))*(r01));
00669 IkReal x162=((sj13)*(sj14));
00670 IkReal x163=((cj14)*(sj13));
00671 IkReal x164=((r00)*(r11));
00672 IkReal x165=((cj13)*(x159));
00673 IkReal x166=((cj13)*(x160));
00674 gconst67=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x161)*(x163)))+(((IkReal(-1.00000000000000))*(r10)*(x161)*(x165)))+(((IkReal(-1.00000000000000))*(r10)*(x161)*(x166)))+(((x164)*(x166)))+(((x164)*(x165)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x162)))+(((r02)*(r11)*(x163)))+(((r02)*(r10)*(x162)))));
00675 IkReal x167=(cj14)*(cj14);
00676 IkReal x168=(sj14)*(sj14);
00677 IkReal x169=((IkReal(1.00000000000000))*(r01));
00678 IkReal x170=((sj13)*(sj14));
00679 IkReal x171=((cj14)*(sj13));
00680 IkReal x172=((r00)*(r11));
00681 IkReal x173=((cj13)*(x167));
00682 IkReal x174=((cj13)*(x168));
00683 dummyeval[0]=((((x172)*(x174)))+(((x172)*(x173)))+(((IkReal(-1.00000000000000))*(r10)*(x169)*(x174)))+(((IkReal(-1.00000000000000))*(r10)*(x169)*(x173)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x170)))+(((r02)*(r10)*(x170)))+(((r02)*(r11)*(x171)))+(((IkReal(-1.00000000000000))*(r12)*(x169)*(x171))));
00684 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00685 {
00686 continue;
00687
00688 } else
00689 {
00690 {
00691 IkReal j9array[1], cj9array[1], sj9array[1];
00692 bool j9valid[1]={false};
00693 _nj9 = 1;
00694 IkReal x175=((cj13)*(cj14));
00695 IkReal x176=((IkReal(1.00000000000000))*(cj13)*(sj14));
00696 if( IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x176)))+(((r12)*(sj13)))+(((r10)*(x175))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst67)*(((((r02)*(sj13)))+(((r00)*(x175)))+(((IkReal(-1.00000000000000))*(r01)*(x176))))))) < IKFAST_ATAN2_MAGTHRESH )
00697 continue;
00698 j9array[0]=IKatan2(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x176)))+(((r12)*(sj13)))+(((r10)*(x175)))))), ((gconst67)*(((((r02)*(sj13)))+(((r00)*(x175)))+(((IkReal(-1.00000000000000))*(r01)*(x176)))))));
00699 sj9array[0]=IKsin(j9array[0]);
00700 cj9array[0]=IKcos(j9array[0]);
00701 if( j9array[0] > IKPI )
00702 {
00703 j9array[0]-=IK2PI;
00704 }
00705 else if( j9array[0] < -IKPI )
00706 { j9array[0]+=IK2PI;
00707 }
00708 j9valid[0] = true;
00709 for(int ij9 = 0; ij9 < 1; ++ij9)
00710 {
00711 if( !j9valid[ij9] )
00712 {
00713 continue;
00714 }
00715 _ij9[0] = ij9; _ij9[1] = -1;
00716 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
00717 {
00718 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
00719 {
00720 j9valid[iij9]=false; _ij9[1] = iij9; break;
00721 }
00722 }
00723 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
00724 {
00725 IkReal evalcond[4];
00726 IkReal x177=IKsin(j9);
00727 IkReal x178=IKcos(j9);
00728 IkReal x179=((r10)*(sj14));
00729 IkReal x180=((cj14)*(r11));
00730 IkReal x181=((cj14)*(r10));
00731 IkReal x182=((cj14)*(r00));
00732 IkReal x183=((r11)*(sj14));
00733 IkReal x184=((r00)*(sj14));
00734 IkReal x185=((IkReal(1.00000000000000))*(x177));
00735 IkReal x186=((cj13)*(x177));
00736 IkReal x187=((sj13)*(x178));
00737 IkReal x188=((r01)*(x177));
00738 IkReal x189=((IkReal(1.00000000000000))*(x178));
00739 evalcond[0]=((IkReal(1.00000000000000))+(((x177)*(x184)))+(((cj14)*(x188)))+(((IkReal(-1.00000000000000))*(x180)*(x189)))+(((IkReal(-1.00000000000000))*(x179)*(x189))));
00740 evalcond[1]=((((IkReal(-1.00000000000000))*(x180)*(x185)))+(((IkReal(-1.00000000000000))*(x184)*(x189)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x189)))+(((IkReal(-1.00000000000000))*(x179)*(x185))));
00741 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x183)*(x189)))+(((IkReal(-1.00000000000000))*(cj13)*(x182)*(x185)))+(((r12)*(x187)))+(((r01)*(sj14)*(x186)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x185)))+(((cj13)*(x178)*(x181))));
00742 evalcond[3]=((((IkReal(-1.00000000000000))*(sj13)*(x182)*(x185)))+(((IkReal(-1.00000000000000))*(x183)*(x187)))+(((r02)*(x186)))+(((sj13)*(sj14)*(x188)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x189)))+(((x181)*(x187))));
00743 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00744 {
00745 continue;
00746 }
00747 }
00748
00749 {
00750 IkReal dummyeval[1];
00751 IkReal gconst69;
00752 gconst69=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
00753 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
00754 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00755 {
00756 {
00757 IkReal dummyeval[1];
00758 IkReal gconst70;
00759 gconst70=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
00760 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
00761 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00762 {
00763 continue;
00764
00765 } else
00766 {
00767 {
00768 IkReal j10array[1], cj10array[1], sj10array[1];
00769 bool j10valid[1]={false};
00770 _nj10 = 1;
00771 IkReal x190=((cj13)*(sj14));
00772 IkReal x191=((IkReal(1.00000000000000))*(cj11));
00773 IkReal x192=((r11)*(sj9));
00774 IkReal x193=((IkReal(1.00000000000000))*(sj11));
00775 IkReal x194=((cj13)*(cj14));
00776 IkReal x195=((cj11)*(sj13));
00777 IkReal x196=((r12)*(sj9));
00778 IkReal x197=((cj9)*(r01));
00779 IkReal x198=((sj11)*(sj13));
00780 IkReal x199=((r10)*(sj9));
00781 IkReal x200=((cj9)*(r00));
00782 IkReal x201=((cj9)*(r02));
00783 if( IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(r20)*(x193)*(x194)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x193)))+(((r21)*(sj11)*(x190)))+(((x195)*(x201)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x197)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x192)))+(((cj11)*(x194)*(x199)))+(((cj11)*(x194)*(x200))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(x190)*(x193)*(x197)))+(((x198)*(x201)))+(((sj11)*(x194)*(x200)))+(((cj11)*(r20)*(x194)))+(((x196)*(x198)))+(((IkReal(-1.00000000000000))*(r21)*(x190)*(x191)))+(((IkReal(-1.00000000000000))*(x190)*(x192)*(x193)))+(((sj11)*(x194)*(x199)))+(((r22)*(x195))))))) < IKFAST_ATAN2_MAGTHRESH )
00784 continue;
00785 j10array[0]=IKatan2(((gconst70)*(((((IkReal(-1.00000000000000))*(r20)*(x193)*(x194)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x193)))+(((r21)*(sj11)*(x190)))+(((x195)*(x201)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x197)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x192)))+(((cj11)*(x194)*(x199)))+(((cj11)*(x194)*(x200)))))), ((gconst70)*(((((IkReal(-1.00000000000000))*(x190)*(x193)*(x197)))+(((x198)*(x201)))+(((sj11)*(x194)*(x200)))+(((cj11)*(r20)*(x194)))+(((x196)*(x198)))+(((IkReal(-1.00000000000000))*(r21)*(x190)*(x191)))+(((IkReal(-1.00000000000000))*(x190)*(x192)*(x193)))+(((sj11)*(x194)*(x199)))+(((r22)*(x195)))))));
00786 sj10array[0]=IKsin(j10array[0]);
00787 cj10array[0]=IKcos(j10array[0]);
00788 if( j10array[0] > IKPI )
00789 {
00790 j10array[0]-=IK2PI;
00791 }
00792 else if( j10array[0] < -IKPI )
00793 { j10array[0]+=IK2PI;
00794 }
00795 j10valid[0] = true;
00796 for(int ij10 = 0; ij10 < 1; ++ij10)
00797 {
00798 if( !j10valid[ij10] )
00799 {
00800 continue;
00801 }
00802 _ij10[0] = ij10; _ij10[1] = -1;
00803 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
00804 {
00805 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
00806 {
00807 j10valid[iij10]=false; _ij10[1] = iij10; break;
00808 }
00809 }
00810 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
00811 {
00812 IkReal evalcond[4];
00813 IkReal x202=IKsin(j10);
00814 IkReal x203=IKcos(j10);
00815 IkReal x204=((IkReal(1.00000000000000))*(sj13));
00816 IkReal x205=((r11)*(sj9));
00817 IkReal x206=((cj9)*(r01));
00818 IkReal x207=((r21)*(sj14));
00819 IkReal x208=((cj9)*(r02));
00820 IkReal x209=((sj13)*(sj9));
00821 IkReal x210=((cj14)*(r10));
00822 IkReal x211=((IkReal(1.00000000000000))*(cj13));
00823 IkReal x212=((cj14)*(r20));
00824 IkReal x213=((cj11)*(x202));
00825 IkReal x214=((sj11)*(x203));
00826 IkReal x215=((sj14)*(x211));
00827 IkReal x216=((sj11)*(x202));
00828 IkReal x217=((cj11)*(x203));
00829 IkReal x218=((cj14)*(cj9)*(r00));
00830 IkReal x219=((x213)+(x214));
00831 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x204)))+(((cj13)*(x207)))+(x216)+(((IkReal(-1.00000000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x211)*(x212))));
00832 evalcond[1]=((((IkReal(-1.00000000000000))*(x204)*(x212)))+(((sj13)*(x207)))+(x219)+(((cj13)*(r22))));
00833 evalcond[2]=((((cj13)*(x218)))+(((IkReal(-1.00000000000000))*(x206)*(x215)))+(((sj13)*(x208)))+(((IkReal(-1.00000000000000))*(x205)*(x215)))+(x219)+(((cj13)*(sj9)*(x210)))+(((r12)*(x209))));
00834 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x211)))+(((x209)*(x210)))+(x217)+(((IkReal(-1.00000000000000))*(sj14)*(x204)*(x206)))+(((IkReal(-1.00000000000000))*(sj14)*(x204)*(x205)))+(((IkReal(-1.00000000000000))*(x216)))+(((IkReal(-1.00000000000000))*(x208)*(x211)))+(((sj13)*(x218))));
00835 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00836 {
00837 continue;
00838 }
00839 }
00840
00841 {
00842 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00843 vinfos[0].jointtype = 1;
00844 vinfos[0].foffset = j9;
00845 vinfos[0].indices[0] = _ij9[0];
00846 vinfos[0].indices[1] = _ij9[1];
00847 vinfos[0].maxsolutions = _nj9;
00848 vinfos[1].jointtype = 1;
00849 vinfos[1].foffset = j10;
00850 vinfos[1].indices[0] = _ij10[0];
00851 vinfos[1].indices[1] = _ij10[1];
00852 vinfos[1].maxsolutions = _nj10;
00853 vinfos[2].jointtype = 1;
00854 vinfos[2].foffset = j11;
00855 vinfos[2].indices[0] = _ij11[0];
00856 vinfos[2].indices[1] = _ij11[1];
00857 vinfos[2].maxsolutions = _nj11;
00858 vinfos[3].jointtype = 1;
00859 vinfos[3].foffset = j12;
00860 vinfos[3].indices[0] = _ij12[0];
00861 vinfos[3].indices[1] = _ij12[1];
00862 vinfos[3].maxsolutions = _nj12;
00863 vinfos[4].jointtype = 1;
00864 vinfos[4].foffset = j13;
00865 vinfos[4].indices[0] = _ij13[0];
00866 vinfos[4].indices[1] = _ij13[1];
00867 vinfos[4].maxsolutions = _nj13;
00868 vinfos[5].jointtype = 1;
00869 vinfos[5].foffset = j14;
00870 vinfos[5].indices[0] = _ij14[0];
00871 vinfos[5].indices[1] = _ij14[1];
00872 vinfos[5].maxsolutions = _nj14;
00873 std::vector<int> vfree(0);
00874 solutions.AddSolution(vinfos,vfree);
00875 }
00876 }
00877 }
00878
00879 }
00880
00881 }
00882
00883 } else
00884 {
00885 {
00886 IkReal j10array[1], cj10array[1], sj10array[1];
00887 bool j10valid[1]={false};
00888 _nj10 = 1;
00889 IkReal x220=((cj13)*(sj11));
00890 IkReal x221=((r21)*(sj14));
00891 IkReal x222=((cj11)*(cj13));
00892 IkReal x223=((r22)*(sj13));
00893 IkReal x224=((sj11)*(sj13));
00894 IkReal x225=((cj11)*(sj13));
00895 IkReal x226=((IkReal(1.00000000000000))*(cj14)*(r20));
00896 if( IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(x225)*(x226)))+(((x220)*(x221)))+(((IkReal(-1.00000000000000))*(sj11)*(x223)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((x221)*(x225)))+(((r22)*(x222))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((cj11)*(x223)))+(((IkReal(-1.00000000000000))*(x224)*(x226)))+(((cj14)*(r20)*(x222)))+(((x221)*(x224)))+(((r22)*(x220)))+(((IkReal(-1.00000000000000))*(x221)*(x222))))))) < IKFAST_ATAN2_MAGTHRESH )
00897 continue;
00898 j10array[0]=IKatan2(((gconst69)*(((((IkReal(-1.00000000000000))*(x225)*(x226)))+(((x220)*(x221)))+(((IkReal(-1.00000000000000))*(sj11)*(x223)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((x221)*(x225)))+(((r22)*(x222)))))), ((gconst69)*(((((cj11)*(x223)))+(((IkReal(-1.00000000000000))*(x224)*(x226)))+(((cj14)*(r20)*(x222)))+(((x221)*(x224)))+(((r22)*(x220)))+(((IkReal(-1.00000000000000))*(x221)*(x222)))))));
00899 sj10array[0]=IKsin(j10array[0]);
00900 cj10array[0]=IKcos(j10array[0]);
00901 if( j10array[0] > IKPI )
00902 {
00903 j10array[0]-=IK2PI;
00904 }
00905 else if( j10array[0] < -IKPI )
00906 { j10array[0]+=IK2PI;
00907 }
00908 j10valid[0] = true;
00909 for(int ij10 = 0; ij10 < 1; ++ij10)
00910 {
00911 if( !j10valid[ij10] )
00912 {
00913 continue;
00914 }
00915 _ij10[0] = ij10; _ij10[1] = -1;
00916 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
00917 {
00918 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
00919 {
00920 j10valid[iij10]=false; _ij10[1] = iij10; break;
00921 }
00922 }
00923 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
00924 {
00925 IkReal evalcond[4];
00926 IkReal x227=IKsin(j10);
00927 IkReal x228=IKcos(j10);
00928 IkReal x229=((IkReal(1.00000000000000))*(sj13));
00929 IkReal x230=((r11)*(sj9));
00930 IkReal x231=((cj9)*(r01));
00931 IkReal x232=((r21)*(sj14));
00932 IkReal x233=((cj9)*(r02));
00933 IkReal x234=((sj13)*(sj9));
00934 IkReal x235=((cj14)*(r10));
00935 IkReal x236=((IkReal(1.00000000000000))*(cj13));
00936 IkReal x237=((cj14)*(r20));
00937 IkReal x238=((cj11)*(x227));
00938 IkReal x239=((sj11)*(x228));
00939 IkReal x240=((sj14)*(x236));
00940 IkReal x241=((sj11)*(x227));
00941 IkReal x242=((cj11)*(x228));
00942 IkReal x243=((cj14)*(cj9)*(r00));
00943 IkReal x244=((x238)+(x239));
00944 evalcond[0]=((((IkReal(-1.00000000000000))*(x242)))+(x241)+(((cj13)*(x232)))+(((IkReal(-1.00000000000000))*(x236)*(x237)))+(((IkReal(-1.00000000000000))*(r22)*(x229))));
00945 evalcond[1]=((((sj13)*(x232)))+(x244)+(((IkReal(-1.00000000000000))*(x229)*(x237)))+(((cj13)*(r22))));
00946 evalcond[2]=((((IkReal(-1.00000000000000))*(x231)*(x240)))+(((sj13)*(x233)))+(((IkReal(-1.00000000000000))*(x230)*(x240)))+(x244)+(((cj13)*(x243)))+(((cj13)*(sj9)*(x235)))+(((r12)*(x234))));
00947 evalcond[3]=((((x234)*(x235)))+(((IkReal(-1.00000000000000))*(x233)*(x236)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x236)))+(((IkReal(-1.00000000000000))*(x241)))+(((IkReal(-1.00000000000000))*(sj14)*(x229)*(x231)))+(((IkReal(-1.00000000000000))*(sj14)*(x229)*(x230)))+(x242)+(((sj13)*(x243))));
00948 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
00949 {
00950 continue;
00951 }
00952 }
00953
00954 {
00955 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
00956 vinfos[0].jointtype = 1;
00957 vinfos[0].foffset = j9;
00958 vinfos[0].indices[0] = _ij9[0];
00959 vinfos[0].indices[1] = _ij9[1];
00960 vinfos[0].maxsolutions = _nj9;
00961 vinfos[1].jointtype = 1;
00962 vinfos[1].foffset = j10;
00963 vinfos[1].indices[0] = _ij10[0];
00964 vinfos[1].indices[1] = _ij10[1];
00965 vinfos[1].maxsolutions = _nj10;
00966 vinfos[2].jointtype = 1;
00967 vinfos[2].foffset = j11;
00968 vinfos[2].indices[0] = _ij11[0];
00969 vinfos[2].indices[1] = _ij11[1];
00970 vinfos[2].maxsolutions = _nj11;
00971 vinfos[3].jointtype = 1;
00972 vinfos[3].foffset = j12;
00973 vinfos[3].indices[0] = _ij12[0];
00974 vinfos[3].indices[1] = _ij12[1];
00975 vinfos[3].maxsolutions = _nj12;
00976 vinfos[4].jointtype = 1;
00977 vinfos[4].foffset = j13;
00978 vinfos[4].indices[0] = _ij13[0];
00979 vinfos[4].indices[1] = _ij13[1];
00980 vinfos[4].maxsolutions = _nj13;
00981 vinfos[5].jointtype = 1;
00982 vinfos[5].foffset = j14;
00983 vinfos[5].indices[0] = _ij14[0];
00984 vinfos[5].indices[1] = _ij14[1];
00985 vinfos[5].maxsolutions = _nj14;
00986 std::vector<int> vfree(0);
00987 solutions.AddSolution(vinfos,vfree);
00988 }
00989 }
00990 }
00991
00992 }
00993
00994 }
00995 }
00996 }
00997
00998 }
00999
01000 }
01001
01002 } else
01003 {
01004 {
01005 IkReal j10array[1], cj10array[1], sj10array[1];
01006 bool j10valid[1]={false};
01007 _nj10 = 1;
01008 IkReal x245=((cj13)*(sj11));
01009 IkReal x246=((r21)*(sj14));
01010 IkReal x247=((cj11)*(cj13));
01011 IkReal x248=((cj11)*(sj13));
01012 IkReal x249=((sj11)*(sj13));
01013 IkReal x250=((IkReal(1.00000000000000))*(cj14)*(r20));
01014 if( IKabs(((gconst68)*(((((r22)*(x247)))+(((IkReal(-1.00000000000000))*(x248)*(x250)))+(((IkReal(-1.00000000000000))*(x245)*(x250)))+(((x245)*(x246)))+(((x246)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x249))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst68)*(((((r22)*(x248)))+(((r22)*(x245)))+(((IkReal(-1.00000000000000))*(x249)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))+(((x246)*(x249)))+(((cj14)*(r20)*(x247))))))) < IKFAST_ATAN2_MAGTHRESH )
01015 continue;
01016 j10array[0]=IKatan2(((gconst68)*(((((r22)*(x247)))+(((IkReal(-1.00000000000000))*(x248)*(x250)))+(((IkReal(-1.00000000000000))*(x245)*(x250)))+(((x245)*(x246)))+(((x246)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x249)))))), ((gconst68)*(((((r22)*(x248)))+(((r22)*(x245)))+(((IkReal(-1.00000000000000))*(x249)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))+(((x246)*(x249)))+(((cj14)*(r20)*(x247)))))));
01017 sj10array[0]=IKsin(j10array[0]);
01018 cj10array[0]=IKcos(j10array[0]);
01019 if( j10array[0] > IKPI )
01020 {
01021 j10array[0]-=IK2PI;
01022 }
01023 else if( j10array[0] < -IKPI )
01024 { j10array[0]+=IK2PI;
01025 }
01026 j10valid[0] = true;
01027 for(int ij10 = 0; ij10 < 1; ++ij10)
01028 {
01029 if( !j10valid[ij10] )
01030 {
01031 continue;
01032 }
01033 _ij10[0] = ij10; _ij10[1] = -1;
01034 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
01035 {
01036 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
01037 {
01038 j10valid[iij10]=false; _ij10[1] = iij10; break;
01039 }
01040 }
01041 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
01042 {
01043 IkReal evalcond[2];
01044 IkReal x251=IKsin(j10);
01045 IkReal x252=IKcos(j10);
01046 IkReal x253=((IkReal(1.00000000000000))*(sj13));
01047 IkReal x254=((cj14)*(r20));
01048 IkReal x255=((r21)*(sj14));
01049 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x253)))+(((cj13)*(x255)))+(((sj11)*(x251)))+(((IkReal(-1.00000000000000))*(cj13)*(x254)))+(((IkReal(-1.00000000000000))*(cj11)*(x252))));
01050 evalcond[1]=((((sj13)*(x255)))+(((cj11)*(x251)))+(((sj11)*(x252)))+(((IkReal(-1.00000000000000))*(x253)*(x254)))+(((cj13)*(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=(sj14)*(sj14);
01061 IkReal x257=(cj14)*(cj14);
01062 IkReal x258=((IkReal(2.00000000000000))*(cj14)*(sj14));
01063 gconst71=IKsign(((((r10)*(r11)*(x258)))+(((x256)*((r10)*(r10))))+(((x257)*((r11)*(r11))))+(((r00)*(r01)*(x258)))+(((x257)*((r01)*(r01))))+(((x256)*((r00)*(r00))))));
01064 IkReal x259=(sj14)*(sj14);
01065 IkReal x260=(cj14)*(cj14);
01066 IkReal x261=((IkReal(2.00000000000000))*(cj14)*(sj14));
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=(cj14)*(cj14);
01074 IkReal x263=(sj14)*(sj14);
01075 IkReal x264=((IkReal(1.00000000000000))*(r01));
01076 IkReal x265=((sj13)*(sj14));
01077 IkReal x266=((cj14)*(sj13));
01078 IkReal x267=((r00)*(r11));
01079 IkReal x268=((cj13)*(x262));
01080 IkReal x269=((cj13)*(x263));
01081 gconst72=IKsign(((((x267)*(x268)))+(((x267)*(x269)))+(((r02)*(r10)*(x265)))+(((r02)*(r11)*(x266)))+(((IkReal(-1.00000000000000))*(r10)*(x264)*(x269)))+(((IkReal(-1.00000000000000))*(r10)*(x264)*(x268)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x265)))+(((IkReal(-1.00000000000000))*(r12)*(x264)*(x266)))));
01082 IkReal x270=(cj14)*(cj14);
01083 IkReal x271=(sj14)*(sj14);
01084 IkReal x272=((IkReal(1.00000000000000))*(r01));
01085 IkReal x273=((sj13)*(sj14));
01086 IkReal x274=((cj14)*(sj13));
01087 IkReal x275=((r00)*(r11));
01088 IkReal x276=((cj13)*(x270));
01089 IkReal x277=((cj13)*(x271));
01090 dummyeval[0]=((((x275)*(x277)))+(((x275)*(x276)))+(((IkReal(-1.00000000000000))*(r12)*(x272)*(x274)))+(((r02)*(r11)*(x274)))+(((r02)*(r10)*(x273)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x273)))+(((IkReal(-1.00000000000000))*(r10)*(x272)*(x277)))+(((IkReal(-1.00000000000000))*(r10)*(x272)*(x276))));
01091 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01092 {
01093 continue;
01094
01095 } else
01096 {
01097 {
01098 IkReal j9array[1], cj9array[1], sj9array[1];
01099 bool j9valid[1]={false};
01100 _nj9 = 1;
01101 IkReal x278=((cj13)*(cj14));
01102 IkReal x279=((IkReal(1.00000000000000))*(cj13)*(sj14));
01103 if( IKabs(((gconst72)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r10)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst72)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH )
01104 continue;
01105 j9array[0]=IKatan2(((gconst72)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r10)*(x278)))))), ((gconst72)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278)))))));
01106 sj9array[0]=IKsin(j9array[0]);
01107 cj9array[0]=IKcos(j9array[0]);
01108 if( j9array[0] > IKPI )
01109 {
01110 j9array[0]-=IK2PI;
01111 }
01112 else if( j9array[0] < -IKPI )
01113 { j9array[0]+=IK2PI;
01114 }
01115 j9valid[0] = true;
01116 for(int ij9 = 0; ij9 < 1; ++ij9)
01117 {
01118 if( !j9valid[ij9] )
01119 {
01120 continue;
01121 }
01122 _ij9[0] = ij9; _ij9[1] = -1;
01123 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
01124 {
01125 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
01126 {
01127 j9valid[iij9]=false; _ij9[1] = iij9; break;
01128 }
01129 }
01130 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
01131 {
01132 IkReal evalcond[6];
01133 IkReal x280=IKsin(j9);
01134 IkReal x281=IKcos(j9);
01135 IkReal x282=((IkReal(1.00000000000000))*(cj14));
01136 IkReal x283=((IkReal(1.00000000000000))*(sj14));
01137 IkReal x284=((cj13)*(cj14));
01138 IkReal x285=((cj14)*(r10));
01139 IkReal x286=((r01)*(sj14));
01140 IkReal x287=((IkReal(1.00000000000000))*(r12));
01141 IkReal x288=((sj13)*(x281));
01142 IkReal x289=((r02)*(x280));
01143 IkReal x290=((r11)*(x280));
01144 IkReal x291=((r10)*(x281));
01145 IkReal x292=((r01)*(x281));
01146 IkReal x293=((sj13)*(x280));
01147 IkReal x294=((r11)*(x281));
01148 IkReal x295=((cj13)*(x280));
01149 IkReal x296=((r10)*(x280));
01150 IkReal x297=((r00)*(x281));
01151 IkReal x298=((cj13)*(x281));
01152 evalcond[0]=((IkReal(1.00000000000000))+(((r00)*(sj14)*(x280)))+(((IkReal(-1.00000000000000))*(x283)*(x291)))+(((cj14)*(r01)*(x280)))+(((IkReal(-1.00000000000000))*(x282)*(x294))));
01153 evalcond[1]=((((IkReal(-1.00000000000000))*(x283)*(x297)))+(((IkReal(-1.00000000000000))*(x283)*(x296)))+(((IkReal(-1.00000000000000))*(x282)*(x290)))+(((IkReal(-1.00000000000000))*(x282)*(x292))));
01154 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x283)*(x294)))+(((IkReal(-1.00000000000000))*(sj13)*(x289)))+(((x284)*(x291)))+(((r12)*(x288)))+(((x286)*(x295)))+(((IkReal(-1.00000000000000))*(r00)*(x282)*(x295))));
01155 evalcond[3]=((((x285)*(x288)))+(((cj13)*(x289)))+(((IkReal(-1.00000000000000))*(r11)*(x283)*(x288)))+(((x286)*(x293)))+(((IkReal(-1.00000000000000))*(x287)*(x298)))+(((IkReal(-1.00000000000000))*(r00)*(x282)*(x293))));
01156 evalcond[4]=((((r12)*(x293)))+(((IkReal(-1.00000000000000))*(cj13)*(x283)*(x290)))+(((IkReal(-1.00000000000000))*(cj13)*(x283)*(x292)))+(((r02)*(x288)))+(((cj10)*(sj11)))+(((x284)*(x296)))+(((x284)*(x297)))+(((cj11)*(sj10))));
01157 evalcond[5]=((((cj14)*(r00)*(x288)))+(((IkReal(-1.00000000000000))*(r01)*(x283)*(x288)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(r02)*(x298)))+(((x285)*(x293)))+(((IkReal(-1.00000000000000))*(x287)*(x295)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(sj13)*(x283)*(x290))));
01158 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 )
01159 {
01160 continue;
01161 }
01162 }
01163
01164 {
01165 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01166 vinfos[0].jointtype = 1;
01167 vinfos[0].foffset = j9;
01168 vinfos[0].indices[0] = _ij9[0];
01169 vinfos[0].indices[1] = _ij9[1];
01170 vinfos[0].maxsolutions = _nj9;
01171 vinfos[1].jointtype = 1;
01172 vinfos[1].foffset = j10;
01173 vinfos[1].indices[0] = _ij10[0];
01174 vinfos[1].indices[1] = _ij10[1];
01175 vinfos[1].maxsolutions = _nj10;
01176 vinfos[2].jointtype = 1;
01177 vinfos[2].foffset = j11;
01178 vinfos[2].indices[0] = _ij11[0];
01179 vinfos[2].indices[1] = _ij11[1];
01180 vinfos[2].maxsolutions = _nj11;
01181 vinfos[3].jointtype = 1;
01182 vinfos[3].foffset = j12;
01183 vinfos[3].indices[0] = _ij12[0];
01184 vinfos[3].indices[1] = _ij12[1];
01185 vinfos[3].maxsolutions = _nj12;
01186 vinfos[4].jointtype = 1;
01187 vinfos[4].foffset = j13;
01188 vinfos[4].indices[0] = _ij13[0];
01189 vinfos[4].indices[1] = _ij13[1];
01190 vinfos[4].maxsolutions = _nj13;
01191 vinfos[5].jointtype = 1;
01192 vinfos[5].foffset = j14;
01193 vinfos[5].indices[0] = _ij14[0];
01194 vinfos[5].indices[1] = _ij14[1];
01195 vinfos[5].maxsolutions = _nj14;
01196 std::vector<int> vfree(0);
01197 solutions.AddSolution(vinfos,vfree);
01198 }
01199 }
01200 }
01201
01202 }
01203
01204 }
01205
01206 } else
01207 {
01208 {
01209 IkReal j9array[1], cj9array[1], sj9array[1];
01210 bool j9valid[1]={false};
01211 _nj9 = 1;
01212 if( IKabs(((gconst71)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst71)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
01213 continue;
01214 j9array[0]=IKatan2(((gconst71)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst71)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
01215 sj9array[0]=IKsin(j9array[0]);
01216 cj9array[0]=IKcos(j9array[0]);
01217 if( j9array[0] > IKPI )
01218 {
01219 j9array[0]-=IK2PI;
01220 }
01221 else if( j9array[0] < -IKPI )
01222 { j9array[0]+=IK2PI;
01223 }
01224 j9valid[0] = true;
01225 for(int ij9 = 0; ij9 < 1; ++ij9)
01226 {
01227 if( !j9valid[ij9] )
01228 {
01229 continue;
01230 }
01231 _ij9[0] = ij9; _ij9[1] = -1;
01232 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
01233 {
01234 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
01235 {
01236 j9valid[iij9]=false; _ij9[1] = iij9; break;
01237 }
01238 }
01239 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
01240 {
01241 IkReal evalcond[6];
01242 IkReal x299=IKsin(j9);
01243 IkReal x300=IKcos(j9);
01244 IkReal x301=((IkReal(1.00000000000000))*(cj14));
01245 IkReal x302=((IkReal(1.00000000000000))*(sj14));
01246 IkReal x303=((cj13)*(cj14));
01247 IkReal x304=((cj14)*(r10));
01248 IkReal x305=((r01)*(sj14));
01249 IkReal x306=((IkReal(1.00000000000000))*(r12));
01250 IkReal x307=((sj13)*(x300));
01251 IkReal x308=((r02)*(x299));
01252 IkReal x309=((r11)*(x299));
01253 IkReal x310=((r10)*(x300));
01254 IkReal x311=((r01)*(x300));
01255 IkReal x312=((sj13)*(x299));
01256 IkReal x313=((r11)*(x300));
01257 IkReal x314=((cj13)*(x299));
01258 IkReal x315=((r10)*(x299));
01259 IkReal x316=((r00)*(x300));
01260 IkReal x317=((cj13)*(x300));
01261 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(r01)*(x299)))+(((IkReal(-1.00000000000000))*(x302)*(x310)))+(((r00)*(sj14)*(x299)))+(((IkReal(-1.00000000000000))*(x301)*(x313))));
01262 evalcond[1]=((((IkReal(-1.00000000000000))*(x301)*(x309)))+(((IkReal(-1.00000000000000))*(x302)*(x316)))+(((IkReal(-1.00000000000000))*(x302)*(x315)))+(((IkReal(-1.00000000000000))*(x301)*(x311))));
01263 evalcond[2]=((((x305)*(x314)))+(((IkReal(-1.00000000000000))*(r00)*(x301)*(x314)))+(((IkReal(-1.00000000000000))*(cj13)*(x302)*(x313)))+(((IkReal(-1.00000000000000))*(sj13)*(x308)))+(((x303)*(x310)))+(((r12)*(x307))));
01264 evalcond[3]=((((x305)*(x312)))+(((IkReal(-1.00000000000000))*(r00)*(x301)*(x312)))+(((IkReal(-1.00000000000000))*(r11)*(x302)*(x307)))+(((IkReal(-1.00000000000000))*(x306)*(x317)))+(((x304)*(x307)))+(((cj13)*(x308))));
01265 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x302)*(x309)))+(((r02)*(x307)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x302)*(x311)))+(((x303)*(x315)))+(((x303)*(x316)))+(((r12)*(x312)))+(((cj11)*(sj10))));
01266 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x317)))+(((x304)*(x312)))+(((IkReal(-1.00000000000000))*(r01)*(x302)*(x307)))+(((IkReal(-1.00000000000000))*(sj13)*(x302)*(x309)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x306)*(x314)))+(((cj10)*(cj11)))+(((cj14)*(r00)*(x307))));
01267 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 )
01268 {
01269 continue;
01270 }
01271 }
01272
01273 {
01274 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01275 vinfos[0].jointtype = 1;
01276 vinfos[0].foffset = j9;
01277 vinfos[0].indices[0] = _ij9[0];
01278 vinfos[0].indices[1] = _ij9[1];
01279 vinfos[0].maxsolutions = _nj9;
01280 vinfos[1].jointtype = 1;
01281 vinfos[1].foffset = j10;
01282 vinfos[1].indices[0] = _ij10[0];
01283 vinfos[1].indices[1] = _ij10[1];
01284 vinfos[1].maxsolutions = _nj10;
01285 vinfos[2].jointtype = 1;
01286 vinfos[2].foffset = j11;
01287 vinfos[2].indices[0] = _ij11[0];
01288 vinfos[2].indices[1] = _ij11[1];
01289 vinfos[2].maxsolutions = _nj11;
01290 vinfos[3].jointtype = 1;
01291 vinfos[3].foffset = j12;
01292 vinfos[3].indices[0] = _ij12[0];
01293 vinfos[3].indices[1] = _ij12[1];
01294 vinfos[3].maxsolutions = _nj12;
01295 vinfos[4].jointtype = 1;
01296 vinfos[4].foffset = j13;
01297 vinfos[4].indices[0] = _ij13[0];
01298 vinfos[4].indices[1] = _ij13[1];
01299 vinfos[4].maxsolutions = _nj13;
01300 vinfos[5].jointtype = 1;
01301 vinfos[5].foffset = j14;
01302 vinfos[5].indices[0] = _ij14[0];
01303 vinfos[5].indices[1] = _ij14[1];
01304 vinfos[5].maxsolutions = _nj14;
01305 std::vector<int> vfree(0);
01306 solutions.AddSolution(vinfos,vfree);
01307 }
01308 }
01309 }
01310
01311 }
01312
01313 }
01314 }
01315 }
01316
01317 }
01318
01319 }
01320
01321 } else
01322 {
01323 {
01324 IkReal j9array[1], cj9array[1], sj9array[1];
01325 bool j9valid[1]={false};
01326 _nj9 = 1;
01327 if( IKabs(((gconst66)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst66)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
01328 continue;
01329 j9array[0]=IKatan2(((gconst66)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst66)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
01330 sj9array[0]=IKsin(j9array[0]);
01331 cj9array[0]=IKcos(j9array[0]);
01332 if( j9array[0] > IKPI )
01333 {
01334 j9array[0]-=IK2PI;
01335 }
01336 else if( j9array[0] < -IKPI )
01337 { j9array[0]+=IK2PI;
01338 }
01339 j9valid[0] = true;
01340 for(int ij9 = 0; ij9 < 1; ++ij9)
01341 {
01342 if( !j9valid[ij9] )
01343 {
01344 continue;
01345 }
01346 _ij9[0] = ij9; _ij9[1] = -1;
01347 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
01348 {
01349 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
01350 {
01351 j9valid[iij9]=false; _ij9[1] = iij9; break;
01352 }
01353 }
01354 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
01355 {
01356 IkReal evalcond[4];
01357 IkReal x318=IKsin(j9);
01358 IkReal x319=IKcos(j9);
01359 IkReal x320=((r10)*(sj14));
01360 IkReal x321=((cj14)*(r11));
01361 IkReal x322=((cj14)*(r10));
01362 IkReal x323=((cj14)*(r00));
01363 IkReal x324=((r11)*(sj14));
01364 IkReal x325=((r00)*(sj14));
01365 IkReal x326=((IkReal(1.00000000000000))*(x318));
01366 IkReal x327=((cj13)*(x318));
01367 IkReal x328=((sj13)*(x319));
01368 IkReal x329=((r01)*(x318));
01369 IkReal x330=((IkReal(1.00000000000000))*(x319));
01370 evalcond[0]=((IkReal(1.00000000000000))+(((x318)*(x325)))+(((IkReal(-1.00000000000000))*(x321)*(x330)))+(((IkReal(-1.00000000000000))*(x320)*(x330)))+(((cj14)*(x329))));
01371 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x330)))+(((IkReal(-1.00000000000000))*(x321)*(x326)))+(((IkReal(-1.00000000000000))*(x320)*(x326)))+(((IkReal(-1.00000000000000))*(x325)*(x330))));
01372 evalcond[2]=((((r12)*(x328)))+(((IkReal(-1.00000000000000))*(cj13)*(x324)*(x330)))+(((cj13)*(x319)*(x322)))+(((r01)*(sj14)*(x327)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x326)))+(((IkReal(-1.00000000000000))*(cj13)*(x323)*(x326))));
01373 evalcond[3]=((((r02)*(x327)))+(((x322)*(x328)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x330)))+(((IkReal(-1.00000000000000))*(x324)*(x328)))+(((sj13)*(sj14)*(x329)))+(((IkReal(-1.00000000000000))*(sj13)*(x323)*(x326))));
01374 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01375 {
01376 continue;
01377 }
01378 }
01379
01380 {
01381 IkReal dummyeval[1];
01382 IkReal gconst69;
01383 gconst69=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
01384 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
01385 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01386 {
01387 {
01388 IkReal dummyeval[1];
01389 IkReal gconst70;
01390 gconst70=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
01391 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
01392 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01393 {
01394 continue;
01395
01396 } else
01397 {
01398 {
01399 IkReal j10array[1], cj10array[1], sj10array[1];
01400 bool j10valid[1]={false};
01401 _nj10 = 1;
01402 IkReal x331=((cj13)*(sj14));
01403 IkReal x332=((IkReal(1.00000000000000))*(cj11));
01404 IkReal x333=((r11)*(sj9));
01405 IkReal x334=((IkReal(1.00000000000000))*(sj11));
01406 IkReal x335=((cj13)*(cj14));
01407 IkReal x336=((cj11)*(sj13));
01408 IkReal x337=((r12)*(sj9));
01409 IkReal x338=((cj9)*(r01));
01410 IkReal x339=((sj11)*(sj13));
01411 IkReal x340=((r10)*(sj9));
01412 IkReal x341=((cj9)*(r00));
01413 IkReal x342=((cj9)*(r02));
01414 if( IKabs(((gconst70)*(((((x336)*(x342)))+(((x336)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x333)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x338)))+(((r21)*(sj11)*(x331)))+(((IkReal(-1.00000000000000))*(r20)*(x334)*(x335)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x334)))+(((cj11)*(x335)*(x340)))+(((cj11)*(x335)*(x341))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((r22)*(x336)))+(((x337)*(x339)))+(((sj11)*(x335)*(x340)))+(((sj11)*(x335)*(x341)))+(((IkReal(-1.00000000000000))*(x331)*(x333)*(x334)))+(((cj11)*(r20)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(x331)*(x332)))+(((x339)*(x342)))+(((IkReal(-1.00000000000000))*(x331)*(x334)*(x338))))))) < IKFAST_ATAN2_MAGTHRESH )
01415 continue;
01416 j10array[0]=IKatan2(((gconst70)*(((((x336)*(x342)))+(((x336)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x333)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x338)))+(((r21)*(sj11)*(x331)))+(((IkReal(-1.00000000000000))*(r20)*(x334)*(x335)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x334)))+(((cj11)*(x335)*(x340)))+(((cj11)*(x335)*(x341)))))), ((gconst70)*(((((r22)*(x336)))+(((x337)*(x339)))+(((sj11)*(x335)*(x340)))+(((sj11)*(x335)*(x341)))+(((IkReal(-1.00000000000000))*(x331)*(x333)*(x334)))+(((cj11)*(r20)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(x331)*(x332)))+(((x339)*(x342)))+(((IkReal(-1.00000000000000))*(x331)*(x334)*(x338)))))));
01417 sj10array[0]=IKsin(j10array[0]);
01418 cj10array[0]=IKcos(j10array[0]);
01419 if( j10array[0] > IKPI )
01420 {
01421 j10array[0]-=IK2PI;
01422 }
01423 else if( j10array[0] < -IKPI )
01424 { j10array[0]+=IK2PI;
01425 }
01426 j10valid[0] = true;
01427 for(int ij10 = 0; ij10 < 1; ++ij10)
01428 {
01429 if( !j10valid[ij10] )
01430 {
01431 continue;
01432 }
01433 _ij10[0] = ij10; _ij10[1] = -1;
01434 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
01435 {
01436 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
01437 {
01438 j10valid[iij10]=false; _ij10[1] = iij10; break;
01439 }
01440 }
01441 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
01442 {
01443 IkReal evalcond[4];
01444 IkReal x343=IKsin(j10);
01445 IkReal x344=IKcos(j10);
01446 IkReal x345=((IkReal(1.00000000000000))*(sj13));
01447 IkReal x346=((r11)*(sj9));
01448 IkReal x347=((cj9)*(r01));
01449 IkReal x348=((r21)*(sj14));
01450 IkReal x349=((cj9)*(r02));
01451 IkReal x350=((sj13)*(sj9));
01452 IkReal x351=((cj14)*(r10));
01453 IkReal x352=((IkReal(1.00000000000000))*(cj13));
01454 IkReal x353=((cj14)*(r20));
01455 IkReal x354=((cj11)*(x343));
01456 IkReal x355=((sj11)*(x344));
01457 IkReal x356=((sj14)*(x352));
01458 IkReal x357=((sj11)*(x343));
01459 IkReal x358=((cj11)*(x344));
01460 IkReal x359=((cj14)*(cj9)*(r00));
01461 IkReal x360=((x355)+(x354));
01462 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x345)))+(((cj13)*(x348)))+(((IkReal(-1.00000000000000))*(x358)))+(x357)+(((IkReal(-1.00000000000000))*(x352)*(x353))));
01463 evalcond[1]=((((sj13)*(x348)))+(((IkReal(-1.00000000000000))*(x345)*(x353)))+(x360)+(((cj13)*(r22))));
01464 evalcond[2]=((((cj13)*(sj9)*(x351)))+(((sj13)*(x349)))+(((r12)*(x350)))+(((cj13)*(x359)))+(((IkReal(-1.00000000000000))*(x346)*(x356)))+(x360)+(((IkReal(-1.00000000000000))*(x347)*(x356))));
01465 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x345)*(x347)))+(((IkReal(-1.00000000000000))*(sj14)*(x345)*(x346)))+(((IkReal(-1.00000000000000))*(x349)*(x352)))+(((IkReal(-1.00000000000000))*(x357)))+(x358)+(((sj13)*(x359)))+(((x350)*(x351)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x352))));
01466 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01467 {
01468 continue;
01469 }
01470 }
01471
01472 {
01473 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01474 vinfos[0].jointtype = 1;
01475 vinfos[0].foffset = j9;
01476 vinfos[0].indices[0] = _ij9[0];
01477 vinfos[0].indices[1] = _ij9[1];
01478 vinfos[0].maxsolutions = _nj9;
01479 vinfos[1].jointtype = 1;
01480 vinfos[1].foffset = j10;
01481 vinfos[1].indices[0] = _ij10[0];
01482 vinfos[1].indices[1] = _ij10[1];
01483 vinfos[1].maxsolutions = _nj10;
01484 vinfos[2].jointtype = 1;
01485 vinfos[2].foffset = j11;
01486 vinfos[2].indices[0] = _ij11[0];
01487 vinfos[2].indices[1] = _ij11[1];
01488 vinfos[2].maxsolutions = _nj11;
01489 vinfos[3].jointtype = 1;
01490 vinfos[3].foffset = j12;
01491 vinfos[3].indices[0] = _ij12[0];
01492 vinfos[3].indices[1] = _ij12[1];
01493 vinfos[3].maxsolutions = _nj12;
01494 vinfos[4].jointtype = 1;
01495 vinfos[4].foffset = j13;
01496 vinfos[4].indices[0] = _ij13[0];
01497 vinfos[4].indices[1] = _ij13[1];
01498 vinfos[4].maxsolutions = _nj13;
01499 vinfos[5].jointtype = 1;
01500 vinfos[5].foffset = j14;
01501 vinfos[5].indices[0] = _ij14[0];
01502 vinfos[5].indices[1] = _ij14[1];
01503 vinfos[5].maxsolutions = _nj14;
01504 std::vector<int> vfree(0);
01505 solutions.AddSolution(vinfos,vfree);
01506 }
01507 }
01508 }
01509
01510 }
01511
01512 }
01513
01514 } else
01515 {
01516 {
01517 IkReal j10array[1], cj10array[1], sj10array[1];
01518 bool j10valid[1]={false};
01519 _nj10 = 1;
01520 IkReal x361=((cj13)*(sj11));
01521 IkReal x362=((r21)*(sj14));
01522 IkReal x363=((cj11)*(cj13));
01523 IkReal x364=((r22)*(sj13));
01524 IkReal x365=((sj11)*(sj13));
01525 IkReal x366=((cj11)*(sj13));
01526 IkReal x367=((IkReal(1.00000000000000))*(cj14)*(r20));
01527 if( IKabs(((gconst69)*(((((x361)*(x362)))+(((IkReal(-1.00000000000000))*(x361)*(x367)))+(((IkReal(-1.00000000000000))*(sj11)*(x364)))+(((IkReal(-1.00000000000000))*(x366)*(x367)))+(((r22)*(x363)))+(((x362)*(x366))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((cj14)*(r20)*(x363)))+(((cj11)*(x364)))+(((IkReal(-1.00000000000000))*(x362)*(x363)))+(((r22)*(x361)))+(((IkReal(-1.00000000000000))*(x365)*(x367)))+(((x362)*(x365))))))) < IKFAST_ATAN2_MAGTHRESH )
01528 continue;
01529 j10array[0]=IKatan2(((gconst69)*(((((x361)*(x362)))+(((IkReal(-1.00000000000000))*(x361)*(x367)))+(((IkReal(-1.00000000000000))*(sj11)*(x364)))+(((IkReal(-1.00000000000000))*(x366)*(x367)))+(((r22)*(x363)))+(((x362)*(x366)))))), ((gconst69)*(((((cj14)*(r20)*(x363)))+(((cj11)*(x364)))+(((IkReal(-1.00000000000000))*(x362)*(x363)))+(((r22)*(x361)))+(((IkReal(-1.00000000000000))*(x365)*(x367)))+(((x362)*(x365)))))));
01530 sj10array[0]=IKsin(j10array[0]);
01531 cj10array[0]=IKcos(j10array[0]);
01532 if( j10array[0] > IKPI )
01533 {
01534 j10array[0]-=IK2PI;
01535 }
01536 else if( j10array[0] < -IKPI )
01537 { j10array[0]+=IK2PI;
01538 }
01539 j10valid[0] = true;
01540 for(int ij10 = 0; ij10 < 1; ++ij10)
01541 {
01542 if( !j10valid[ij10] )
01543 {
01544 continue;
01545 }
01546 _ij10[0] = ij10; _ij10[1] = -1;
01547 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
01548 {
01549 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
01550 {
01551 j10valid[iij10]=false; _ij10[1] = iij10; break;
01552 }
01553 }
01554 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
01555 {
01556 IkReal evalcond[4];
01557 IkReal x368=IKsin(j10);
01558 IkReal x369=IKcos(j10);
01559 IkReal x370=((IkReal(1.00000000000000))*(sj13));
01560 IkReal x371=((r11)*(sj9));
01561 IkReal x372=((cj9)*(r01));
01562 IkReal x373=((r21)*(sj14));
01563 IkReal x374=((cj9)*(r02));
01564 IkReal x375=((sj13)*(sj9));
01565 IkReal x376=((cj14)*(r10));
01566 IkReal x377=((IkReal(1.00000000000000))*(cj13));
01567 IkReal x378=((cj14)*(r20));
01568 IkReal x379=((cj11)*(x368));
01569 IkReal x380=((sj11)*(x369));
01570 IkReal x381=((sj14)*(x377));
01571 IkReal x382=((sj11)*(x368));
01572 IkReal x383=((cj11)*(x369));
01573 IkReal x384=((cj14)*(cj9)*(r00));
01574 IkReal x385=((x380)+(x379));
01575 evalcond[0]=((((IkReal(-1.00000000000000))*(x377)*(x378)))+(x382)+(((cj13)*(x373)))+(((IkReal(-1.00000000000000))*(x383)))+(((IkReal(-1.00000000000000))*(r22)*(x370))));
01576 evalcond[1]=((((IkReal(-1.00000000000000))*(x370)*(x378)))+(((sj13)*(x373)))+(x385)+(((cj13)*(r22))));
01577 evalcond[2]=((((cj13)*(x384)))+(((sj13)*(x374)))+(x385)+(((r12)*(x375)))+(((IkReal(-1.00000000000000))*(x371)*(x381)))+(((cj13)*(sj9)*(x376)))+(((IkReal(-1.00000000000000))*(x372)*(x381))));
01578 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x370)*(x371)))+(((IkReal(-1.00000000000000))*(sj14)*(x370)*(x372)))+(((x375)*(x376)))+(x383)+(((IkReal(-1.00000000000000))*(x374)*(x377)))+(((IkReal(-1.00000000000000))*(x382)))+(((sj13)*(x384)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x377))));
01579 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01580 {
01581 continue;
01582 }
01583 }
01584
01585 {
01586 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01587 vinfos[0].jointtype = 1;
01588 vinfos[0].foffset = j9;
01589 vinfos[0].indices[0] = _ij9[0];
01590 vinfos[0].indices[1] = _ij9[1];
01591 vinfos[0].maxsolutions = _nj9;
01592 vinfos[1].jointtype = 1;
01593 vinfos[1].foffset = j10;
01594 vinfos[1].indices[0] = _ij10[0];
01595 vinfos[1].indices[1] = _ij10[1];
01596 vinfos[1].maxsolutions = _nj10;
01597 vinfos[2].jointtype = 1;
01598 vinfos[2].foffset = j11;
01599 vinfos[2].indices[0] = _ij11[0];
01600 vinfos[2].indices[1] = _ij11[1];
01601 vinfos[2].maxsolutions = _nj11;
01602 vinfos[3].jointtype = 1;
01603 vinfos[3].foffset = j12;
01604 vinfos[3].indices[0] = _ij12[0];
01605 vinfos[3].indices[1] = _ij12[1];
01606 vinfos[3].maxsolutions = _nj12;
01607 vinfos[4].jointtype = 1;
01608 vinfos[4].foffset = j13;
01609 vinfos[4].indices[0] = _ij13[0];
01610 vinfos[4].indices[1] = _ij13[1];
01611 vinfos[4].maxsolutions = _nj13;
01612 vinfos[5].jointtype = 1;
01613 vinfos[5].foffset = j14;
01614 vinfos[5].indices[0] = _ij14[0];
01615 vinfos[5].indices[1] = _ij14[1];
01616 vinfos[5].maxsolutions = _nj14;
01617 std::vector<int> vfree(0);
01618 solutions.AddSolution(vinfos,vfree);
01619 }
01620 }
01621 }
01622
01623 }
01624
01625 }
01626 }
01627 }
01628
01629 }
01630
01631 }
01632 }
01633 }
01634
01635 } else
01636 {
01637 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
01638 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
01639 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
01640 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
01641 {
01642 {
01643 IkReal j11array[1], cj11array[1], sj11array[1];
01644 bool j11valid[1]={false};
01645 _nj11 = 1;
01646 IkReal x386=((IkReal(4.00000000000000))*(cj13));
01647 IkReal x387=((npy)*(sj14));
01648 IkReal x388=((cj14)*(npx));
01649 IkReal x389=((IkReal(4.00000000000000))*(sj13));
01650 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13)))))+IKsqr(((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))))-1) <= IKFAST_SINCOS_THRESH )
01651 continue;
01652 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13)))), ((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))));
01653 sj11array[0]=IKsin(j11array[0]);
01654 cj11array[0]=IKcos(j11array[0]);
01655 if( j11array[0] > IKPI )
01656 {
01657 j11array[0]-=IK2PI;
01658 }
01659 else if( j11array[0] < -IKPI )
01660 { j11array[0]+=IK2PI;
01661 }
01662 j11valid[0] = true;
01663 for(int ij11 = 0; ij11 < 1; ++ij11)
01664 {
01665 if( !j11valid[ij11] )
01666 {
01667 continue;
01668 }
01669 _ij11[0] = ij11; _ij11[1] = -1;
01670 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
01671 {
01672 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
01673 {
01674 j11valid[iij11]=false; _ij11[1] = iij11; break;
01675 }
01676 }
01677 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
01678 {
01679 IkReal evalcond[2];
01680 IkReal x390=((IkReal(1.00000000000000))*(sj13));
01681 IkReal x391=((cj14)*(npx));
01682 IkReal x392=((npy)*(sj14));
01683 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(x391)))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x392)))+(((IkReal(-1.00000000000000))*(npz)*(x390))));
01684 evalcond[1]=((IkReal(-0.0300000000000000))+(((sj13)*(x392)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x390)*(x391)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
01685 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
01686 {
01687 continue;
01688 }
01689 }
01690
01691 {
01692 IkReal dummyeval[1];
01693 IkReal gconst75;
01694 IkReal x393=(sj14)*(sj14);
01695 IkReal x394=(cj14)*(cj14);
01696 IkReal x395=((IkReal(1.00000000000000))*(x393));
01697 IkReal x396=((IkReal(2.00000000000000))*(cj14)*(sj14));
01698 IkReal x397=((IkReal(1.00000000000000))*(x394));
01699 gconst75=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r01)*(x396)))+(((IkReal(-1.00000000000000))*(x397)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x396)))+(((IkReal(-1.00000000000000))*(x397)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x395)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x395)*((r00)*(r00))))));
01700 IkReal x398=(sj14)*(sj14);
01701 IkReal x399=(cj14)*(cj14);
01702 IkReal x400=((IkReal(1.00000000000000))*(x398));
01703 IkReal x401=((IkReal(2.00000000000000))*(cj14)*(sj14));
01704 IkReal x402=((IkReal(1.00000000000000))*(x399));
01705 dummyeval[0]=((((IkReal(-1.00000000000000))*(x400)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x400)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x402)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x401)))+(((IkReal(-1.00000000000000))*(x402)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x401))));
01706 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01707 {
01708 {
01709 IkReal dummyeval[1];
01710 IkReal gconst77;
01711 gconst77=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
01712 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
01713 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01714 {
01715 {
01716 IkReal dummyeval[1];
01717 IkReal gconst76;
01718 IkReal x403=(sj14)*(sj14);
01719 IkReal x404=(cj14)*(cj14);
01720 IkReal x405=((cj14)*(sj13));
01721 IkReal x406=((IkReal(1.00000000000000))*(r11));
01722 IkReal x407=((cj13)*(r00));
01723 IkReal x408=((sj13)*(sj14));
01724 IkReal x409=((cj13)*(r01)*(r10));
01725 gconst76=IKsign(((((r01)*(r12)*(x405)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x408)))+(((IkReal(-1.00000000000000))*(x404)*(x406)*(x407)))+(((x404)*(x409)))+(((IkReal(-1.00000000000000))*(x403)*(x406)*(x407)))+(((IkReal(-1.00000000000000))*(r02)*(x405)*(x406)))+(((x403)*(x409)))+(((r00)*(r12)*(x408)))));
01726 IkReal x410=(sj14)*(sj14);
01727 IkReal x411=(cj14)*(cj14);
01728 IkReal x412=((cj14)*(sj13));
01729 IkReal x413=((IkReal(1.00000000000000))*(r11));
01730 IkReal x414=((cj13)*(r00));
01731 IkReal x415=((sj13)*(sj14));
01732 IkReal x416=((cj13)*(r01)*(r10));
01733 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(r10)*(x415)))+(((IkReal(-1.00000000000000))*(x411)*(x413)*(x414)))+(((r01)*(r12)*(x412)))+(((x411)*(x416)))+(((x410)*(x416)))+(((IkReal(-1.00000000000000))*(r02)*(x412)*(x413)))+(((IkReal(-1.00000000000000))*(x410)*(x413)*(x414)))+(((r00)*(r12)*(x415))));
01734 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01735 {
01736 continue;
01737
01738 } else
01739 {
01740 {
01741 IkReal j9array[1], cj9array[1], sj9array[1];
01742 bool j9valid[1]={false};
01743 _nj9 = 1;
01744 IkReal x417=((cj13)*(cj14));
01745 IkReal x418=((IkReal(1.00000000000000))*(cj13)*(sj14));
01746 if( IKabs(((gconst76)*(((((r12)*(sj13)))+(((r10)*(x417)))+(((IkReal(-1.00000000000000))*(r11)*(x418))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst76)*(((((r02)*(sj13)))+(((r00)*(x417)))+(((IkReal(-1.00000000000000))*(r01)*(x418))))))) < IKFAST_ATAN2_MAGTHRESH )
01747 continue;
01748 j9array[0]=IKatan2(((gconst76)*(((((r12)*(sj13)))+(((r10)*(x417)))+(((IkReal(-1.00000000000000))*(r11)*(x418)))))), ((gconst76)*(((((r02)*(sj13)))+(((r00)*(x417)))+(((IkReal(-1.00000000000000))*(r01)*(x418)))))));
01749 sj9array[0]=IKsin(j9array[0]);
01750 cj9array[0]=IKcos(j9array[0]);
01751 if( j9array[0] > IKPI )
01752 {
01753 j9array[0]-=IK2PI;
01754 }
01755 else if( j9array[0] < -IKPI )
01756 { j9array[0]+=IK2PI;
01757 }
01758 j9valid[0] = true;
01759 for(int ij9 = 0; ij9 < 1; ++ij9)
01760 {
01761 if( !j9valid[ij9] )
01762 {
01763 continue;
01764 }
01765 _ij9[0] = ij9; _ij9[1] = -1;
01766 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
01767 {
01768 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
01769 {
01770 j9valid[iij9]=false; _ij9[1] = iij9; break;
01771 }
01772 }
01773 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
01774 {
01775 IkReal evalcond[4];
01776 IkReal x419=IKsin(j9);
01777 IkReal x420=IKcos(j9);
01778 IkReal x421=((r10)*(sj14));
01779 IkReal x422=((cj14)*(r11));
01780 IkReal x423=((cj14)*(r10));
01781 IkReal x424=((cj14)*(r00));
01782 IkReal x425=((r11)*(sj14));
01783 IkReal x426=((r00)*(sj14));
01784 IkReal x427=((IkReal(1.00000000000000))*(x419));
01785 IkReal x428=((cj13)*(x419));
01786 IkReal x429=((sj13)*(x420));
01787 IkReal x430=((r01)*(x419));
01788 IkReal x431=((IkReal(1.00000000000000))*(x420));
01789 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x421)*(x431)))+(((x419)*(x426)))+(((IkReal(-1.00000000000000))*(x422)*(x431)))+(((cj14)*(x430))));
01790 evalcond[1]=((((IkReal(-1.00000000000000))*(x422)*(x427)))+(((IkReal(-1.00000000000000))*(x421)*(x427)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x431)))+(((IkReal(-1.00000000000000))*(x426)*(x431))));
01791 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x424)*(x427)))+(((cj13)*(x420)*(x423)))+(((IkReal(-1.00000000000000))*(cj13)*(x425)*(x431)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x427)))+(((r12)*(x429)))+(((r01)*(sj14)*(x428))));
01792 evalcond[3]=((((IkReal(-1.00000000000000))*(x425)*(x429)))+(((IkReal(-1.00000000000000))*(sj13)*(x424)*(x427)))+(((r02)*(x428)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x431)))+(((sj13)*(sj14)*(x430)))+(((x423)*(x429))));
01793 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01794 {
01795 continue;
01796 }
01797 }
01798
01799 {
01800 IkReal dummyeval[1];
01801 IkReal gconst78;
01802 gconst78=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
01803 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
01804 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01805 {
01806 {
01807 IkReal dummyeval[1];
01808 IkReal gconst79;
01809 gconst79=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
01810 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
01811 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01812 {
01813 continue;
01814
01815 } else
01816 {
01817 {
01818 IkReal j10array[1], cj10array[1], sj10array[1];
01819 bool j10valid[1]={false};
01820 _nj10 = 1;
01821 IkReal x432=((cj13)*(sj14));
01822 IkReal x433=((IkReal(1.00000000000000))*(cj11));
01823 IkReal x434=((r11)*(sj9));
01824 IkReal x435=((IkReal(1.00000000000000))*(sj11));
01825 IkReal x436=((cj13)*(cj14));
01826 IkReal x437=((cj11)*(sj13));
01827 IkReal x438=((r12)*(sj9));
01828 IkReal x439=((cj9)*(r01));
01829 IkReal x440=((sj11)*(sj13));
01830 IkReal x441=((cj9)*(r02));
01831 IkReal x442=((r10)*(sj9));
01832 IkReal x443=((cj9)*(r00));
01833 if( IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(x432)*(x433)*(x439)))+(((IkReal(-1.00000000000000))*(r20)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x435)))+(((x437)*(x438)))+(((x437)*(x441)))+(((r21)*(sj11)*(x432)))+(((cj11)*(x436)*(x443)))+(((cj11)*(x436)*(x442))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x434)*(x435)))+(((cj11)*(r20)*(x436)))+(((x440)*(x441)))+(((r22)*(x437)))+(((x438)*(x440)))+(((IkReal(-1.00000000000000))*(r21)*(x432)*(x433)))+(((IkReal(-1.00000000000000))*(x432)*(x435)*(x439)))+(((sj11)*(x436)*(x442)))+(((sj11)*(x436)*(x443))))))) < IKFAST_ATAN2_MAGTHRESH )
01834 continue;
01835 j10array[0]=IKatan2(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(x432)*(x433)*(x439)))+(((IkReal(-1.00000000000000))*(r20)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x435)))+(((x437)*(x438)))+(((x437)*(x441)))+(((r21)*(sj11)*(x432)))+(((cj11)*(x436)*(x443)))+(((cj11)*(x436)*(x442)))))), ((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x434)*(x435)))+(((cj11)*(r20)*(x436)))+(((x440)*(x441)))+(((r22)*(x437)))+(((x438)*(x440)))+(((IkReal(-1.00000000000000))*(r21)*(x432)*(x433)))+(((IkReal(-1.00000000000000))*(x432)*(x435)*(x439)))+(((sj11)*(x436)*(x442)))+(((sj11)*(x436)*(x443)))))));
01836 sj10array[0]=IKsin(j10array[0]);
01837 cj10array[0]=IKcos(j10array[0]);
01838 if( j10array[0] > IKPI )
01839 {
01840 j10array[0]-=IK2PI;
01841 }
01842 else if( j10array[0] < -IKPI )
01843 { j10array[0]+=IK2PI;
01844 }
01845 j10valid[0] = true;
01846 for(int ij10 = 0; ij10 < 1; ++ij10)
01847 {
01848 if( !j10valid[ij10] )
01849 {
01850 continue;
01851 }
01852 _ij10[0] = ij10; _ij10[1] = -1;
01853 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
01854 {
01855 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
01856 {
01857 j10valid[iij10]=false; _ij10[1] = iij10; break;
01858 }
01859 }
01860 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
01861 {
01862 IkReal evalcond[4];
01863 IkReal x444=IKsin(j10);
01864 IkReal x445=IKcos(j10);
01865 IkReal x446=((IkReal(1.00000000000000))*(sj13));
01866 IkReal x447=((r11)*(sj9));
01867 IkReal x448=((cj9)*(r01));
01868 IkReal x449=((IkReal(1.00000000000000))*(cj11));
01869 IkReal x450=((r21)*(sj14));
01870 IkReal x451=((cj9)*(r02));
01871 IkReal x452=((sj13)*(sj9));
01872 IkReal x453=((cj14)*(r10));
01873 IkReal x454=((IkReal(1.00000000000000))*(cj13));
01874 IkReal x455=((cj14)*(r20));
01875 IkReal x456=((sj11)*(x444));
01876 IkReal x457=((sj14)*(x454));
01877 IkReal x458=((sj11)*(x445));
01878 IkReal x459=((cj14)*(cj9)*(r00));
01879 IkReal x460=((x445)*(x449));
01880 evalcond[0]=((((cj13)*(x450)))+(((IkReal(-1.00000000000000))*(x460)))+(((IkReal(-1.00000000000000))*(x454)*(x455)))+(x456)+(((IkReal(-1.00000000000000))*(r22)*(x446))));
01881 evalcond[1]=((((IkReal(-1.00000000000000))*(x446)*(x455)))+(((IkReal(-1.00000000000000))*(x444)*(x449)))+(((IkReal(-1.00000000000000))*(x458)))+(((sj13)*(x450)))+(((cj13)*(r22))));
01882 evalcond[2]=((((cj11)*(x444)))+(((cj13)*(x459)))+(((IkReal(-1.00000000000000))*(x447)*(x457)))+(x458)+(((r12)*(x452)))+(((sj13)*(x451)))+(((cj13)*(sj9)*(x453)))+(((IkReal(-1.00000000000000))*(x448)*(x457))));
01883 evalcond[3]=((((x452)*(x453)))+(((IkReal(-1.00000000000000))*(sj14)*(x446)*(x448)))+(((IkReal(-1.00000000000000))*(sj14)*(x446)*(x447)))+(((IkReal(-1.00000000000000))*(x460)))+(x456)+(((sj13)*(x459)))+(((IkReal(-1.00000000000000))*(x451)*(x454)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x454))));
01884 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01885 {
01886 continue;
01887 }
01888 }
01889
01890 {
01891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
01892 vinfos[0].jointtype = 1;
01893 vinfos[0].foffset = j9;
01894 vinfos[0].indices[0] = _ij9[0];
01895 vinfos[0].indices[1] = _ij9[1];
01896 vinfos[0].maxsolutions = _nj9;
01897 vinfos[1].jointtype = 1;
01898 vinfos[1].foffset = j10;
01899 vinfos[1].indices[0] = _ij10[0];
01900 vinfos[1].indices[1] = _ij10[1];
01901 vinfos[1].maxsolutions = _nj10;
01902 vinfos[2].jointtype = 1;
01903 vinfos[2].foffset = j11;
01904 vinfos[2].indices[0] = _ij11[0];
01905 vinfos[2].indices[1] = _ij11[1];
01906 vinfos[2].maxsolutions = _nj11;
01907 vinfos[3].jointtype = 1;
01908 vinfos[3].foffset = j12;
01909 vinfos[3].indices[0] = _ij12[0];
01910 vinfos[3].indices[1] = _ij12[1];
01911 vinfos[3].maxsolutions = _nj12;
01912 vinfos[4].jointtype = 1;
01913 vinfos[4].foffset = j13;
01914 vinfos[4].indices[0] = _ij13[0];
01915 vinfos[4].indices[1] = _ij13[1];
01916 vinfos[4].maxsolutions = _nj13;
01917 vinfos[5].jointtype = 1;
01918 vinfos[5].foffset = j14;
01919 vinfos[5].indices[0] = _ij14[0];
01920 vinfos[5].indices[1] = _ij14[1];
01921 vinfos[5].maxsolutions = _nj14;
01922 std::vector<int> vfree(0);
01923 solutions.AddSolution(vinfos,vfree);
01924 }
01925 }
01926 }
01927
01928 }
01929
01930 }
01931
01932 } else
01933 {
01934 {
01935 IkReal j10array[1], cj10array[1], sj10array[1];
01936 bool j10valid[1]={false};
01937 _nj10 = 1;
01938 IkReal x461=((r22)*(sj11));
01939 IkReal x462=((IkReal(1.00000000000000))*(cj13));
01940 IkReal x463=((cj14)*(r20));
01941 IkReal x464=((r21)*(sj14));
01942 IkReal x465=((cj11)*(cj13));
01943 IkReal x466=((IkReal(1.00000000000000))*(sj13));
01944 if( IKabs(((gconst78)*(((((r22)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x462)*(x464)))+(((cj11)*(sj13)*(x464)))+(((cj13)*(sj11)*(x463)))+(((IkReal(-1.00000000000000))*(cj11)*(x463)*(x466)))+(((sj13)*(x461))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(cj11)*(r22)*(x466)))+(((cj13)*(x461)))+(((sj11)*(sj13)*(x464)))+(((IkReal(-1.00000000000000))*(cj11)*(x462)*(x463)))+(((x464)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x463)*(x466))))))) < IKFAST_ATAN2_MAGTHRESH )
01945 continue;
01946 j10array[0]=IKatan2(((gconst78)*(((((r22)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x462)*(x464)))+(((cj11)*(sj13)*(x464)))+(((cj13)*(sj11)*(x463)))+(((IkReal(-1.00000000000000))*(cj11)*(x463)*(x466)))+(((sj13)*(x461)))))), ((gconst78)*(((((IkReal(-1.00000000000000))*(cj11)*(r22)*(x466)))+(((cj13)*(x461)))+(((sj11)*(sj13)*(x464)))+(((IkReal(-1.00000000000000))*(cj11)*(x462)*(x463)))+(((x464)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x463)*(x466)))))));
01947 sj10array[0]=IKsin(j10array[0]);
01948 cj10array[0]=IKcos(j10array[0]);
01949 if( j10array[0] > IKPI )
01950 {
01951 j10array[0]-=IK2PI;
01952 }
01953 else if( j10array[0] < -IKPI )
01954 { j10array[0]+=IK2PI;
01955 }
01956 j10valid[0] = true;
01957 for(int ij10 = 0; ij10 < 1; ++ij10)
01958 {
01959 if( !j10valid[ij10] )
01960 {
01961 continue;
01962 }
01963 _ij10[0] = ij10; _ij10[1] = -1;
01964 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
01965 {
01966 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
01967 {
01968 j10valid[iij10]=false; _ij10[1] = iij10; break;
01969 }
01970 }
01971 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
01972 {
01973 IkReal evalcond[4];
01974 IkReal x467=IKsin(j10);
01975 IkReal x468=IKcos(j10);
01976 IkReal x469=((IkReal(1.00000000000000))*(sj13));
01977 IkReal x470=((r11)*(sj9));
01978 IkReal x471=((cj9)*(r01));
01979 IkReal x472=((IkReal(1.00000000000000))*(cj11));
01980 IkReal x473=((r21)*(sj14));
01981 IkReal x474=((cj9)*(r02));
01982 IkReal x475=((sj13)*(sj9));
01983 IkReal x476=((cj14)*(r10));
01984 IkReal x477=((IkReal(1.00000000000000))*(cj13));
01985 IkReal x478=((cj14)*(r20));
01986 IkReal x479=((sj11)*(x467));
01987 IkReal x480=((sj14)*(x477));
01988 IkReal x481=((sj11)*(x468));
01989 IkReal x482=((cj14)*(cj9)*(r00));
01990 IkReal x483=((x468)*(x472));
01991 evalcond[0]=((((IkReal(-1.00000000000000))*(x477)*(x478)))+(((IkReal(-1.00000000000000))*(x483)))+(((cj13)*(x473)))+(x479)+(((IkReal(-1.00000000000000))*(r22)*(x469))));
01992 evalcond[1]=((((IkReal(-1.00000000000000))*(x467)*(x472)))+(((IkReal(-1.00000000000000))*(x469)*(x478)))+(((IkReal(-1.00000000000000))*(x481)))+(((sj13)*(x473)))+(((cj13)*(r22))));
01993 evalcond[2]=((((cj11)*(x467)))+(((cj13)*(sj9)*(x476)))+(((IkReal(-1.00000000000000))*(x470)*(x480)))+(((sj13)*(x474)))+(x481)+(((r12)*(x475)))+(((IkReal(-1.00000000000000))*(x471)*(x480)))+(((cj13)*(x482))));
01994 evalcond[3]=((((IkReal(-1.00000000000000))*(x483)))+(((sj13)*(x482)))+(((IkReal(-1.00000000000000))*(sj14)*(x469)*(x471)))+(((IkReal(-1.00000000000000))*(sj14)*(x469)*(x470)))+(x479)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x477)))+(((x475)*(x476)))+(((IkReal(-1.00000000000000))*(x474)*(x477))));
01995 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
01996 {
01997 continue;
01998 }
01999 }
02000
02001 {
02002 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02003 vinfos[0].jointtype = 1;
02004 vinfos[0].foffset = j9;
02005 vinfos[0].indices[0] = _ij9[0];
02006 vinfos[0].indices[1] = _ij9[1];
02007 vinfos[0].maxsolutions = _nj9;
02008 vinfos[1].jointtype = 1;
02009 vinfos[1].foffset = j10;
02010 vinfos[1].indices[0] = _ij10[0];
02011 vinfos[1].indices[1] = _ij10[1];
02012 vinfos[1].maxsolutions = _nj10;
02013 vinfos[2].jointtype = 1;
02014 vinfos[2].foffset = j11;
02015 vinfos[2].indices[0] = _ij11[0];
02016 vinfos[2].indices[1] = _ij11[1];
02017 vinfos[2].maxsolutions = _nj11;
02018 vinfos[3].jointtype = 1;
02019 vinfos[3].foffset = j12;
02020 vinfos[3].indices[0] = _ij12[0];
02021 vinfos[3].indices[1] = _ij12[1];
02022 vinfos[3].maxsolutions = _nj12;
02023 vinfos[4].jointtype = 1;
02024 vinfos[4].foffset = j13;
02025 vinfos[4].indices[0] = _ij13[0];
02026 vinfos[4].indices[1] = _ij13[1];
02027 vinfos[4].maxsolutions = _nj13;
02028 vinfos[5].jointtype = 1;
02029 vinfos[5].foffset = j14;
02030 vinfos[5].indices[0] = _ij14[0];
02031 vinfos[5].indices[1] = _ij14[1];
02032 vinfos[5].maxsolutions = _nj14;
02033 std::vector<int> vfree(0);
02034 solutions.AddSolution(vinfos,vfree);
02035 }
02036 }
02037 }
02038
02039 }
02040
02041 }
02042 }
02043 }
02044
02045 }
02046
02047 }
02048
02049 } else
02050 {
02051 {
02052 IkReal j10array[1], cj10array[1], sj10array[1];
02053 bool j10valid[1]={false};
02054 _nj10 = 1;
02055 IkReal x484=((cj13)*(sj11));
02056 IkReal x485=((r21)*(sj14));
02057 IkReal x486=((cj11)*(cj13));
02058 IkReal x487=((IkReal(1.00000000000000))*(sj13));
02059 IkReal x488=((sj11)*(sj13));
02060 IkReal x489=((cj14)*(r20));
02061 if( IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(cj11)*(x487)*(x489)))+(((x484)*(x489)))+(((IkReal(-1.00000000000000))*(x484)*(x485)))+(((r22)*(x488)))+(((r22)*(x486)))+(((cj11)*(sj13)*(x485))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(x486)*(x489)))+(((x485)*(x486)))+(((x485)*(x488)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x487)))+(((r22)*(x484)))+(((IkReal(-1.00000000000000))*(sj11)*(x487)*(x489))))))) < IKFAST_ATAN2_MAGTHRESH )
02062 continue;
02063 j10array[0]=IKatan2(((gconst77)*(((((IkReal(-1.00000000000000))*(cj11)*(x487)*(x489)))+(((x484)*(x489)))+(((IkReal(-1.00000000000000))*(x484)*(x485)))+(((r22)*(x488)))+(((r22)*(x486)))+(((cj11)*(sj13)*(x485)))))), ((gconst77)*(((((IkReal(-1.00000000000000))*(x486)*(x489)))+(((x485)*(x486)))+(((x485)*(x488)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x487)))+(((r22)*(x484)))+(((IkReal(-1.00000000000000))*(sj11)*(x487)*(x489)))))));
02064 sj10array[0]=IKsin(j10array[0]);
02065 cj10array[0]=IKcos(j10array[0]);
02066 if( j10array[0] > IKPI )
02067 {
02068 j10array[0]-=IK2PI;
02069 }
02070 else if( j10array[0] < -IKPI )
02071 { j10array[0]+=IK2PI;
02072 }
02073 j10valid[0] = true;
02074 for(int ij10 = 0; ij10 < 1; ++ij10)
02075 {
02076 if( !j10valid[ij10] )
02077 {
02078 continue;
02079 }
02080 _ij10[0] = ij10; _ij10[1] = -1;
02081 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
02082 {
02083 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
02084 {
02085 j10valid[iij10]=false; _ij10[1] = iij10; break;
02086 }
02087 }
02088 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
02089 {
02090 IkReal evalcond[2];
02091 IkReal x490=IKsin(j10);
02092 IkReal x491=IKcos(j10);
02093 IkReal x492=((IkReal(1.00000000000000))*(sj13));
02094 IkReal x493=((cj14)*(r20));
02095 IkReal x494=((r21)*(sj14));
02096 IkReal x495=((IkReal(1.00000000000000))*(x491));
02097 evalcond[0]=((((cj13)*(x494)))+(((IkReal(-1.00000000000000))*(cj11)*(x495)))+(((IkReal(-1.00000000000000))*(cj13)*(x493)))+(((IkReal(-1.00000000000000))*(r22)*(x492)))+(((sj11)*(x490))));
02098 evalcond[1]=((((IkReal(-1.00000000000000))*(sj11)*(x495)))+(((IkReal(-1.00000000000000))*(cj11)*(x490)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x492)*(x493)))+(((sj13)*(x494))));
02099 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
02100 {
02101 continue;
02102 }
02103 }
02104
02105 {
02106 IkReal dummyeval[1];
02107 IkReal gconst80;
02108 IkReal x496=(sj14)*(sj14);
02109 IkReal x497=(cj14)*(cj14);
02110 IkReal x498=((IkReal(1.00000000000000))*(x496));
02111 IkReal x499=((IkReal(2.00000000000000))*(cj14)*(sj14));
02112 IkReal x500=((IkReal(1.00000000000000))*(x497));
02113 gconst80=IKsign(((((IkReal(-1.00000000000000))*(x498)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x499)))+(((IkReal(-1.00000000000000))*(x498)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x500)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x499)))+(((IkReal(-1.00000000000000))*(x500)*((r01)*(r01))))));
02114 IkReal x501=(sj14)*(sj14);
02115 IkReal x502=(cj14)*(cj14);
02116 IkReal x503=((IkReal(1.00000000000000))*(x501));
02117 IkReal x504=((IkReal(2.00000000000000))*(cj14)*(sj14));
02118 IkReal x505=((IkReal(1.00000000000000))*(x502));
02119 dummyeval[0]=((((IkReal(-1.00000000000000))*(x505)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x505)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x504)))+(((IkReal(-1.00000000000000))*(x503)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x504)))+(((IkReal(-1.00000000000000))*(x503)*((r10)*(r10)))));
02120 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02121 {
02122 {
02123 IkReal dummyeval[1];
02124 IkReal gconst81;
02125 IkReal x506=(sj14)*(sj14);
02126 IkReal x507=(cj14)*(cj14);
02127 IkReal x508=((cj14)*(sj13));
02128 IkReal x509=((IkReal(1.00000000000000))*(r11));
02129 IkReal x510=((cj13)*(r00));
02130 IkReal x511=((sj13)*(sj14));
02131 IkReal x512=((cj13)*(r01)*(r10));
02132 gconst81=IKsign(((((x507)*(x512)))+(((IkReal(-1.00000000000000))*(x506)*(x509)*(x510)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x511)))+(((r00)*(r12)*(x511)))+(((IkReal(-1.00000000000000))*(r02)*(x508)*(x509)))+(((IkReal(-1.00000000000000))*(x507)*(x509)*(x510)))+(((r01)*(r12)*(x508)))+(((x506)*(x512)))));
02133 IkReal x513=(sj14)*(sj14);
02134 IkReal x514=(cj14)*(cj14);
02135 IkReal x515=((cj14)*(sj13));
02136 IkReal x516=((IkReal(1.00000000000000))*(r11));
02137 IkReal x517=((cj13)*(r00));
02138 IkReal x518=((sj13)*(sj14));
02139 IkReal x519=((cj13)*(r01)*(r10));
02140 dummyeval[0]=((((r01)*(r12)*(x515)))+(((IkReal(-1.00000000000000))*(x514)*(x516)*(x517)))+(((x514)*(x519)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x518)))+(((x513)*(x519)))+(((r00)*(r12)*(x518)))+(((IkReal(-1.00000000000000))*(r02)*(x515)*(x516)))+(((IkReal(-1.00000000000000))*(x513)*(x516)*(x517))));
02141 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02142 {
02143 continue;
02144
02145 } else
02146 {
02147 {
02148 IkReal j9array[1], cj9array[1], sj9array[1];
02149 bool j9valid[1]={false};
02150 _nj9 = 1;
02151 IkReal x520=((cj13)*(cj14));
02152 IkReal x521=((IkReal(1.00000000000000))*(cj13)*(sj14));
02153 if( IKabs(((gconst81)*(((((r12)*(sj13)))+(((r10)*(x520)))+(((IkReal(-1.00000000000000))*(r11)*(x521))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst81)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x521)))+(((r00)*(x520))))))) < IKFAST_ATAN2_MAGTHRESH )
02154 continue;
02155 j9array[0]=IKatan2(((gconst81)*(((((r12)*(sj13)))+(((r10)*(x520)))+(((IkReal(-1.00000000000000))*(r11)*(x521)))))), ((gconst81)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x521)))+(((r00)*(x520)))))));
02156 sj9array[0]=IKsin(j9array[0]);
02157 cj9array[0]=IKcos(j9array[0]);
02158 if( j9array[0] > IKPI )
02159 {
02160 j9array[0]-=IK2PI;
02161 }
02162 else if( j9array[0] < -IKPI )
02163 { j9array[0]+=IK2PI;
02164 }
02165 j9valid[0] = true;
02166 for(int ij9 = 0; ij9 < 1; ++ij9)
02167 {
02168 if( !j9valid[ij9] )
02169 {
02170 continue;
02171 }
02172 _ij9[0] = ij9; _ij9[1] = -1;
02173 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
02174 {
02175 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
02176 {
02177 j9valid[iij9]=false; _ij9[1] = iij9; break;
02178 }
02179 }
02180 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
02181 {
02182 IkReal evalcond[6];
02183 IkReal x522=IKsin(j9);
02184 IkReal x523=IKcos(j9);
02185 IkReal x524=((IkReal(1.00000000000000))*(cj14));
02186 IkReal x525=((IkReal(1.00000000000000))*(sj14));
02187 IkReal x526=((cj13)*(cj14));
02188 IkReal x527=((cj14)*(r10));
02189 IkReal x528=((r01)*(sj14));
02190 IkReal x529=((IkReal(1.00000000000000))*(r12));
02191 IkReal x530=((sj13)*(x523));
02192 IkReal x531=((r02)*(x522));
02193 IkReal x532=((r11)*(x522));
02194 IkReal x533=((r10)*(x523));
02195 IkReal x534=((r01)*(x523));
02196 IkReal x535=((sj13)*(x522));
02197 IkReal x536=((r11)*(x523));
02198 IkReal x537=((cj13)*(x522));
02199 IkReal x538=((r10)*(x522));
02200 IkReal x539=((r00)*(x523));
02201 IkReal x540=((cj13)*(x523));
02202 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x524)*(x536)))+(((IkReal(-1.00000000000000))*(x525)*(x533)))+(((cj14)*(r01)*(x522)))+(((r00)*(sj14)*(x522))));
02203 evalcond[1]=((((IkReal(-1.00000000000000))*(x524)*(x532)))+(((IkReal(-1.00000000000000))*(x524)*(x534)))+(((IkReal(-1.00000000000000))*(x525)*(x539)))+(((IkReal(-1.00000000000000))*(x525)*(x538))));
02204 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x525)*(x536)))+(((x526)*(x533)))+(((r12)*(x530)))+(((IkReal(-1.00000000000000))*(sj13)*(x531)))+(((x528)*(x537)))+(((IkReal(-1.00000000000000))*(r00)*(x524)*(x537))));
02205 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x525)*(x530)))+(((cj13)*(x531)))+(((IkReal(-1.00000000000000))*(x529)*(x540)))+(((x528)*(x535)))+(((x527)*(x530)))+(((IkReal(-1.00000000000000))*(r00)*(x524)*(x535))));
02206 evalcond[4]=((((r02)*(x530)))+(((IkReal(-1.00000000000000))*(cj13)*(x525)*(x534)))+(((IkReal(-1.00000000000000))*(cj13)*(x525)*(x532)))+(((x526)*(x538)))+(((x526)*(x539)))+(((r12)*(x535)))+(((cj10)*(sj11)))+(((cj11)*(sj10))));
02207 evalcond[5]=((((IkReal(-1.00000000000000))*(x529)*(x537)))+(((cj14)*(r00)*(x530)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(r01)*(x525)*(x530)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((x527)*(x535)))+(((IkReal(-1.00000000000000))*(sj13)*(x525)*(x532)))+(((IkReal(-1.00000000000000))*(r02)*(x540))));
02208 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 )
02209 {
02210 continue;
02211 }
02212 }
02213
02214 {
02215 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02216 vinfos[0].jointtype = 1;
02217 vinfos[0].foffset = j9;
02218 vinfos[0].indices[0] = _ij9[0];
02219 vinfos[0].indices[1] = _ij9[1];
02220 vinfos[0].maxsolutions = _nj9;
02221 vinfos[1].jointtype = 1;
02222 vinfos[1].foffset = j10;
02223 vinfos[1].indices[0] = _ij10[0];
02224 vinfos[1].indices[1] = _ij10[1];
02225 vinfos[1].maxsolutions = _nj10;
02226 vinfos[2].jointtype = 1;
02227 vinfos[2].foffset = j11;
02228 vinfos[2].indices[0] = _ij11[0];
02229 vinfos[2].indices[1] = _ij11[1];
02230 vinfos[2].maxsolutions = _nj11;
02231 vinfos[3].jointtype = 1;
02232 vinfos[3].foffset = j12;
02233 vinfos[3].indices[0] = _ij12[0];
02234 vinfos[3].indices[1] = _ij12[1];
02235 vinfos[3].maxsolutions = _nj12;
02236 vinfos[4].jointtype = 1;
02237 vinfos[4].foffset = j13;
02238 vinfos[4].indices[0] = _ij13[0];
02239 vinfos[4].indices[1] = _ij13[1];
02240 vinfos[4].maxsolutions = _nj13;
02241 vinfos[5].jointtype = 1;
02242 vinfos[5].foffset = j14;
02243 vinfos[5].indices[0] = _ij14[0];
02244 vinfos[5].indices[1] = _ij14[1];
02245 vinfos[5].maxsolutions = _nj14;
02246 std::vector<int> vfree(0);
02247 solutions.AddSolution(vinfos,vfree);
02248 }
02249 }
02250 }
02251
02252 }
02253
02254 }
02255
02256 } else
02257 {
02258 {
02259 IkReal j9array[1], cj9array[1], sj9array[1];
02260 bool j9valid[1]={false};
02261 _nj9 = 1;
02262 if( IKabs(((gconst80)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst80)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
02263 continue;
02264 j9array[0]=IKatan2(((gconst80)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst80)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
02265 sj9array[0]=IKsin(j9array[0]);
02266 cj9array[0]=IKcos(j9array[0]);
02267 if( j9array[0] > IKPI )
02268 {
02269 j9array[0]-=IK2PI;
02270 }
02271 else if( j9array[0] < -IKPI )
02272 { j9array[0]+=IK2PI;
02273 }
02274 j9valid[0] = true;
02275 for(int ij9 = 0; ij9 < 1; ++ij9)
02276 {
02277 if( !j9valid[ij9] )
02278 {
02279 continue;
02280 }
02281 _ij9[0] = ij9; _ij9[1] = -1;
02282 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
02283 {
02284 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
02285 {
02286 j9valid[iij9]=false; _ij9[1] = iij9; break;
02287 }
02288 }
02289 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
02290 {
02291 IkReal evalcond[6];
02292 IkReal x541=IKsin(j9);
02293 IkReal x542=IKcos(j9);
02294 IkReal x543=((IkReal(1.00000000000000))*(cj14));
02295 IkReal x544=((IkReal(1.00000000000000))*(sj14));
02296 IkReal x545=((cj13)*(cj14));
02297 IkReal x546=((cj14)*(r10));
02298 IkReal x547=((r01)*(sj14));
02299 IkReal x548=((IkReal(1.00000000000000))*(r12));
02300 IkReal x549=((sj13)*(x542));
02301 IkReal x550=((r02)*(x541));
02302 IkReal x551=((r11)*(x541));
02303 IkReal x552=((r10)*(x542));
02304 IkReal x553=((r01)*(x542));
02305 IkReal x554=((sj13)*(x541));
02306 IkReal x555=((r11)*(x542));
02307 IkReal x556=((cj13)*(x541));
02308 IkReal x557=((r10)*(x541));
02309 IkReal x558=((r00)*(x542));
02310 IkReal x559=((cj13)*(x542));
02311 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x544)*(x552)))+(((cj14)*(r01)*(x541)))+(((r00)*(sj14)*(x541)))+(((IkReal(-1.00000000000000))*(x543)*(x555))));
02312 evalcond[1]=((((IkReal(-1.00000000000000))*(x544)*(x558)))+(((IkReal(-1.00000000000000))*(x544)*(x557)))+(((IkReal(-1.00000000000000))*(x543)*(x553)))+(((IkReal(-1.00000000000000))*(x543)*(x551))));
02313 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x543)*(x556)))+(((IkReal(-1.00000000000000))*(sj13)*(x550)))+(((r12)*(x549)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x555)))+(((x547)*(x556)))+(((x545)*(x552))));
02314 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x543)*(x554)))+(((cj13)*(x550)))+(((IkReal(-1.00000000000000))*(r11)*(x544)*(x549)))+(((IkReal(-1.00000000000000))*(x548)*(x559)))+(((x547)*(x554)))+(((x546)*(x549))));
02315 evalcond[4]=((((r02)*(x549)))+(((r12)*(x554)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x553)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x551)))+(((x545)*(x558)))+(((x545)*(x557)))+(((cj11)*(sj10))));
02316 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x544)*(x551)))+(((IkReal(-1.00000000000000))*(r02)*(x559)))+(((cj14)*(r00)*(x549)))+(((IkReal(-1.00000000000000))*(r01)*(x544)*(x549)))+(((IkReal(-1.00000000000000))*(x548)*(x556)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((x546)*(x554))));
02317 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 )
02318 {
02319 continue;
02320 }
02321 }
02322
02323 {
02324 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02325 vinfos[0].jointtype = 1;
02326 vinfos[0].foffset = j9;
02327 vinfos[0].indices[0] = _ij9[0];
02328 vinfos[0].indices[1] = _ij9[1];
02329 vinfos[0].maxsolutions = _nj9;
02330 vinfos[1].jointtype = 1;
02331 vinfos[1].foffset = j10;
02332 vinfos[1].indices[0] = _ij10[0];
02333 vinfos[1].indices[1] = _ij10[1];
02334 vinfos[1].maxsolutions = _nj10;
02335 vinfos[2].jointtype = 1;
02336 vinfos[2].foffset = j11;
02337 vinfos[2].indices[0] = _ij11[0];
02338 vinfos[2].indices[1] = _ij11[1];
02339 vinfos[2].maxsolutions = _nj11;
02340 vinfos[3].jointtype = 1;
02341 vinfos[3].foffset = j12;
02342 vinfos[3].indices[0] = _ij12[0];
02343 vinfos[3].indices[1] = _ij12[1];
02344 vinfos[3].maxsolutions = _nj12;
02345 vinfos[4].jointtype = 1;
02346 vinfos[4].foffset = j13;
02347 vinfos[4].indices[0] = _ij13[0];
02348 vinfos[4].indices[1] = _ij13[1];
02349 vinfos[4].maxsolutions = _nj13;
02350 vinfos[5].jointtype = 1;
02351 vinfos[5].foffset = j14;
02352 vinfos[5].indices[0] = _ij14[0];
02353 vinfos[5].indices[1] = _ij14[1];
02354 vinfos[5].maxsolutions = _nj14;
02355 std::vector<int> vfree(0);
02356 solutions.AddSolution(vinfos,vfree);
02357 }
02358 }
02359 }
02360
02361 }
02362
02363 }
02364 }
02365 }
02366
02367 }
02368
02369 }
02370
02371 } else
02372 {
02373 {
02374 IkReal j9array[1], cj9array[1], sj9array[1];
02375 bool j9valid[1]={false};
02376 _nj9 = 1;
02377 if( IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst75)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
02378 continue;
02379 j9array[0]=IKatan2(((gconst75)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst75)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
02380 sj9array[0]=IKsin(j9array[0]);
02381 cj9array[0]=IKcos(j9array[0]);
02382 if( j9array[0] > IKPI )
02383 {
02384 j9array[0]-=IK2PI;
02385 }
02386 else if( j9array[0] < -IKPI )
02387 { j9array[0]+=IK2PI;
02388 }
02389 j9valid[0] = true;
02390 for(int ij9 = 0; ij9 < 1; ++ij9)
02391 {
02392 if( !j9valid[ij9] )
02393 {
02394 continue;
02395 }
02396 _ij9[0] = ij9; _ij9[1] = -1;
02397 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
02398 {
02399 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
02400 {
02401 j9valid[iij9]=false; _ij9[1] = iij9; break;
02402 }
02403 }
02404 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
02405 {
02406 IkReal evalcond[4];
02407 IkReal x560=IKsin(j9);
02408 IkReal x561=IKcos(j9);
02409 IkReal x562=((r10)*(sj14));
02410 IkReal x563=((cj14)*(r11));
02411 IkReal x564=((cj14)*(r10));
02412 IkReal x565=((cj14)*(r00));
02413 IkReal x566=((r11)*(sj14));
02414 IkReal x567=((r00)*(sj14));
02415 IkReal x568=((IkReal(1.00000000000000))*(x560));
02416 IkReal x569=((cj13)*(x560));
02417 IkReal x570=((sj13)*(x561));
02418 IkReal x571=((r01)*(x560));
02419 IkReal x572=((IkReal(1.00000000000000))*(x561));
02420 evalcond[0]=((IkReal(-1.00000000000000))+(((cj14)*(x571)))+(((x560)*(x567)))+(((IkReal(-1.00000000000000))*(x563)*(x572)))+(((IkReal(-1.00000000000000))*(x562)*(x572))));
02421 evalcond[1]=((((IkReal(-1.00000000000000))*(x562)*(x568)))+(((IkReal(-1.00000000000000))*(x567)*(x572)))+(((IkReal(-1.00000000000000))*(x563)*(x568)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x572))));
02422 evalcond[2]=((((r12)*(x570)))+(((IkReal(-1.00000000000000))*(cj13)*(x565)*(x568)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x568)))+(((IkReal(-1.00000000000000))*(cj13)*(x566)*(x572)))+(((r01)*(sj14)*(x569)))+(((cj13)*(x561)*(x564))));
02423 evalcond[3]=((((sj13)*(sj14)*(x571)))+(((IkReal(-1.00000000000000))*(x566)*(x570)))+(((IkReal(-1.00000000000000))*(sj13)*(x565)*(x568)))+(((x564)*(x570)))+(((r02)*(x569)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x572))));
02424 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02425 {
02426 continue;
02427 }
02428 }
02429
02430 {
02431 IkReal dummyeval[1];
02432 IkReal gconst78;
02433 gconst78=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
02434 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
02435 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02436 {
02437 {
02438 IkReal dummyeval[1];
02439 IkReal gconst79;
02440 gconst79=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
02441 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
02442 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02443 {
02444 continue;
02445
02446 } else
02447 {
02448 {
02449 IkReal j10array[1], cj10array[1], sj10array[1];
02450 bool j10valid[1]={false};
02451 _nj10 = 1;
02452 IkReal x573=((cj13)*(sj14));
02453 IkReal x574=((IkReal(1.00000000000000))*(cj11));
02454 IkReal x575=((r11)*(sj9));
02455 IkReal x576=((IkReal(1.00000000000000))*(sj11));
02456 IkReal x577=((cj13)*(cj14));
02457 IkReal x578=((cj11)*(sj13));
02458 IkReal x579=((r12)*(sj9));
02459 IkReal x580=((cj9)*(r01));
02460 IkReal x581=((sj11)*(sj13));
02461 IkReal x582=((cj9)*(r02));
02462 IkReal x583=((r10)*(sj9));
02463 IkReal x584=((cj9)*(r00));
02464 if( IKabs(((gconst79)*(((((x578)*(x582)))+(((x578)*(x579)))+(((cj11)*(x577)*(x584)))+(((cj11)*(x577)*(x583)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x580)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x576)))+(((IkReal(-1.00000000000000))*(r20)*(x576)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x575)))+(((r21)*(sj11)*(x573))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x573)*(x576)*(x580)))+(((x581)*(x582)))+(((x579)*(x581)))+(((sj11)*(x577)*(x584)))+(((sj11)*(x577)*(x583)))+(((cj11)*(r20)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x575)*(x576)))+(((IkReal(-1.00000000000000))*(r21)*(x573)*(x574)))+(((r22)*(x578))))))) < IKFAST_ATAN2_MAGTHRESH )
02465 continue;
02466 j10array[0]=IKatan2(((gconst79)*(((((x578)*(x582)))+(((x578)*(x579)))+(((cj11)*(x577)*(x584)))+(((cj11)*(x577)*(x583)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x580)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x576)))+(((IkReal(-1.00000000000000))*(r20)*(x576)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x575)))+(((r21)*(sj11)*(x573)))))), ((gconst79)*(((((IkReal(-1.00000000000000))*(x573)*(x576)*(x580)))+(((x581)*(x582)))+(((x579)*(x581)))+(((sj11)*(x577)*(x584)))+(((sj11)*(x577)*(x583)))+(((cj11)*(r20)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x575)*(x576)))+(((IkReal(-1.00000000000000))*(r21)*(x573)*(x574)))+(((r22)*(x578)))))));
02467 sj10array[0]=IKsin(j10array[0]);
02468 cj10array[0]=IKcos(j10array[0]);
02469 if( j10array[0] > IKPI )
02470 {
02471 j10array[0]-=IK2PI;
02472 }
02473 else if( j10array[0] < -IKPI )
02474 { j10array[0]+=IK2PI;
02475 }
02476 j10valid[0] = true;
02477 for(int ij10 = 0; ij10 < 1; ++ij10)
02478 {
02479 if( !j10valid[ij10] )
02480 {
02481 continue;
02482 }
02483 _ij10[0] = ij10; _ij10[1] = -1;
02484 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
02485 {
02486 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
02487 {
02488 j10valid[iij10]=false; _ij10[1] = iij10; break;
02489 }
02490 }
02491 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
02492 {
02493 IkReal evalcond[4];
02494 IkReal x585=IKsin(j10);
02495 IkReal x586=IKcos(j10);
02496 IkReal x587=((IkReal(1.00000000000000))*(sj13));
02497 IkReal x588=((r11)*(sj9));
02498 IkReal x589=((cj9)*(r01));
02499 IkReal x590=((IkReal(1.00000000000000))*(cj11));
02500 IkReal x591=((r21)*(sj14));
02501 IkReal x592=((cj9)*(r02));
02502 IkReal x593=((sj13)*(sj9));
02503 IkReal x594=((cj14)*(r10));
02504 IkReal x595=((IkReal(1.00000000000000))*(cj13));
02505 IkReal x596=((cj14)*(r20));
02506 IkReal x597=((sj11)*(x585));
02507 IkReal x598=((sj14)*(x595));
02508 IkReal x599=((sj11)*(x586));
02509 IkReal x600=((cj14)*(cj9)*(r00));
02510 IkReal x601=((x586)*(x590));
02511 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x587)))+(((cj13)*(x591)))+(x597)+(((IkReal(-1.00000000000000))*(x595)*(x596)))+(((IkReal(-1.00000000000000))*(x601))));
02512 evalcond[1]=((((sj13)*(x591)))+(((IkReal(-1.00000000000000))*(x587)*(x596)))+(((IkReal(-1.00000000000000))*(x585)*(x590)))+(((IkReal(-1.00000000000000))*(x599)))+(((cj13)*(r22))));
02513 evalcond[2]=((((IkReal(-1.00000000000000))*(x588)*(x598)))+(((sj13)*(x592)))+(((IkReal(-1.00000000000000))*(x589)*(x598)))+(((cj13)*(sj9)*(x594)))+(((cj11)*(x585)))+(x599)+(((r12)*(x593)))+(((cj13)*(x600))));
02514 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x587)*(x589)))+(((IkReal(-1.00000000000000))*(sj14)*(x587)*(x588)))+(((sj13)*(x600)))+(((IkReal(-1.00000000000000))*(x592)*(x595)))+(x597)+(((IkReal(-1.00000000000000))*(x601)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x595)))+(((x593)*(x594))));
02515 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02516 {
02517 continue;
02518 }
02519 }
02520
02521 {
02522 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02523 vinfos[0].jointtype = 1;
02524 vinfos[0].foffset = j9;
02525 vinfos[0].indices[0] = _ij9[0];
02526 vinfos[0].indices[1] = _ij9[1];
02527 vinfos[0].maxsolutions = _nj9;
02528 vinfos[1].jointtype = 1;
02529 vinfos[1].foffset = j10;
02530 vinfos[1].indices[0] = _ij10[0];
02531 vinfos[1].indices[1] = _ij10[1];
02532 vinfos[1].maxsolutions = _nj10;
02533 vinfos[2].jointtype = 1;
02534 vinfos[2].foffset = j11;
02535 vinfos[2].indices[0] = _ij11[0];
02536 vinfos[2].indices[1] = _ij11[1];
02537 vinfos[2].maxsolutions = _nj11;
02538 vinfos[3].jointtype = 1;
02539 vinfos[3].foffset = j12;
02540 vinfos[3].indices[0] = _ij12[0];
02541 vinfos[3].indices[1] = _ij12[1];
02542 vinfos[3].maxsolutions = _nj12;
02543 vinfos[4].jointtype = 1;
02544 vinfos[4].foffset = j13;
02545 vinfos[4].indices[0] = _ij13[0];
02546 vinfos[4].indices[1] = _ij13[1];
02547 vinfos[4].maxsolutions = _nj13;
02548 vinfos[5].jointtype = 1;
02549 vinfos[5].foffset = j14;
02550 vinfos[5].indices[0] = _ij14[0];
02551 vinfos[5].indices[1] = _ij14[1];
02552 vinfos[5].maxsolutions = _nj14;
02553 std::vector<int> vfree(0);
02554 solutions.AddSolution(vinfos,vfree);
02555 }
02556 }
02557 }
02558
02559 }
02560
02561 }
02562
02563 } else
02564 {
02565 {
02566 IkReal j10array[1], cj10array[1], sj10array[1];
02567 bool j10valid[1]={false};
02568 _nj10 = 1;
02569 IkReal x602=((r22)*(sj11));
02570 IkReal x603=((IkReal(1.00000000000000))*(cj13));
02571 IkReal x604=((cj14)*(r20));
02572 IkReal x605=((r21)*(sj14));
02573 IkReal x606=((cj11)*(cj13));
02574 IkReal x607=((IkReal(1.00000000000000))*(sj13));
02575 if( IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(sj11)*(x603)*(x605)))+(((sj13)*(x602)))+(((cj13)*(sj11)*(x604)))+(((cj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x604)*(x607)))+(((r22)*(x606))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((x605)*(x606)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x607)))+(((sj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x603)*(x604)))+(((cj13)*(x602)))+(((IkReal(-1.00000000000000))*(sj11)*(x604)*(x607))))))) < IKFAST_ATAN2_MAGTHRESH )
02576 continue;
02577 j10array[0]=IKatan2(((gconst78)*(((((IkReal(-1.00000000000000))*(sj11)*(x603)*(x605)))+(((sj13)*(x602)))+(((cj13)*(sj11)*(x604)))+(((cj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x604)*(x607)))+(((r22)*(x606)))))), ((gconst78)*(((((x605)*(x606)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x607)))+(((sj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x603)*(x604)))+(((cj13)*(x602)))+(((IkReal(-1.00000000000000))*(sj11)*(x604)*(x607)))))));
02578 sj10array[0]=IKsin(j10array[0]);
02579 cj10array[0]=IKcos(j10array[0]);
02580 if( j10array[0] > IKPI )
02581 {
02582 j10array[0]-=IK2PI;
02583 }
02584 else if( j10array[0] < -IKPI )
02585 { j10array[0]+=IK2PI;
02586 }
02587 j10valid[0] = true;
02588 for(int ij10 = 0; ij10 < 1; ++ij10)
02589 {
02590 if( !j10valid[ij10] )
02591 {
02592 continue;
02593 }
02594 _ij10[0] = ij10; _ij10[1] = -1;
02595 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
02596 {
02597 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
02598 {
02599 j10valid[iij10]=false; _ij10[1] = iij10; break;
02600 }
02601 }
02602 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
02603 {
02604 IkReal evalcond[4];
02605 IkReal x608=IKsin(j10);
02606 IkReal x609=IKcos(j10);
02607 IkReal x610=((IkReal(1.00000000000000))*(sj13));
02608 IkReal x611=((r11)*(sj9));
02609 IkReal x612=((cj9)*(r01));
02610 IkReal x613=((IkReal(1.00000000000000))*(cj11));
02611 IkReal x614=((r21)*(sj14));
02612 IkReal x615=((cj9)*(r02));
02613 IkReal x616=((sj13)*(sj9));
02614 IkReal x617=((cj14)*(r10));
02615 IkReal x618=((IkReal(1.00000000000000))*(cj13));
02616 IkReal x619=((cj14)*(r20));
02617 IkReal x620=((sj11)*(x608));
02618 IkReal x621=((sj14)*(x618));
02619 IkReal x622=((sj11)*(x609));
02620 IkReal x623=((cj14)*(cj9)*(r00));
02621 IkReal x624=((x609)*(x613));
02622 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x610)))+(((IkReal(-1.00000000000000))*(x618)*(x619)))+(((IkReal(-1.00000000000000))*(x624)))+(x620)+(((cj13)*(x614))));
02623 evalcond[1]=((((IkReal(-1.00000000000000))*(x610)*(x619)))+(((sj13)*(x614)))+(((IkReal(-1.00000000000000))*(x622)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x608)*(x613))));
02624 evalcond[2]=((((cj13)*(x623)))+(((sj13)*(x615)))+(((cj11)*(x608)))+(((IkReal(-1.00000000000000))*(x611)*(x621)))+(((cj13)*(sj9)*(x617)))+(((r12)*(x616)))+(((IkReal(-1.00000000000000))*(x612)*(x621)))+(x622));
02625 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x610)*(x612)))+(((IkReal(-1.00000000000000))*(sj14)*(x610)*(x611)))+(((IkReal(-1.00000000000000))*(x624)))+(((x616)*(x617)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x618)))+(x620)+(((IkReal(-1.00000000000000))*(x615)*(x618)))+(((sj13)*(x623))));
02626 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02627 {
02628 continue;
02629 }
02630 }
02631
02632 {
02633 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
02634 vinfos[0].jointtype = 1;
02635 vinfos[0].foffset = j9;
02636 vinfos[0].indices[0] = _ij9[0];
02637 vinfos[0].indices[1] = _ij9[1];
02638 vinfos[0].maxsolutions = _nj9;
02639 vinfos[1].jointtype = 1;
02640 vinfos[1].foffset = j10;
02641 vinfos[1].indices[0] = _ij10[0];
02642 vinfos[1].indices[1] = _ij10[1];
02643 vinfos[1].maxsolutions = _nj10;
02644 vinfos[2].jointtype = 1;
02645 vinfos[2].foffset = j11;
02646 vinfos[2].indices[0] = _ij11[0];
02647 vinfos[2].indices[1] = _ij11[1];
02648 vinfos[2].maxsolutions = _nj11;
02649 vinfos[3].jointtype = 1;
02650 vinfos[3].foffset = j12;
02651 vinfos[3].indices[0] = _ij12[0];
02652 vinfos[3].indices[1] = _ij12[1];
02653 vinfos[3].maxsolutions = _nj12;
02654 vinfos[4].jointtype = 1;
02655 vinfos[4].foffset = j13;
02656 vinfos[4].indices[0] = _ij13[0];
02657 vinfos[4].indices[1] = _ij13[1];
02658 vinfos[4].maxsolutions = _nj13;
02659 vinfos[5].jointtype = 1;
02660 vinfos[5].foffset = j14;
02661 vinfos[5].indices[0] = _ij14[0];
02662 vinfos[5].indices[1] = _ij14[1];
02663 vinfos[5].maxsolutions = _nj14;
02664 std::vector<int> vfree(0);
02665 solutions.AddSolution(vinfos,vfree);
02666 }
02667 }
02668 }
02669
02670 }
02671
02672 }
02673 }
02674 }
02675
02676 }
02677
02678 }
02679 }
02680 }
02681
02682 } else
02683 {
02684 if( 1 )
02685 {
02686 continue;
02687
02688 } else
02689 {
02690 }
02691 }
02692 }
02693 }
02694
02695 } else
02696 {
02697 {
02698 IkReal j11array[1], cj11array[1], sj11array[1];
02699 bool j11valid[1]={false};
02700 _nj11 = 1;
02701 IkReal x625=((IkReal(4.00000000000000))*(sj14));
02702 IkReal x626=((IkReal(4.00000000000000))*(cj14));
02703 if( IKabs(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625)))))-1) <= IKFAST_SINCOS_THRESH )
02704 continue;
02705 j11array[0]=IKatan2(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625)))))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625)))));
02706 sj11array[0]=IKsin(j11array[0]);
02707 cj11array[0]=IKcos(j11array[0]);
02708 if( j11array[0] > IKPI )
02709 {
02710 j11array[0]-=IK2PI;
02711 }
02712 else if( j11array[0] < -IKPI )
02713 { j11array[0]+=IK2PI;
02714 }
02715 j11valid[0] = true;
02716 for(int ij11 = 0; ij11 < 1; ++ij11)
02717 {
02718 if( !j11valid[ij11] )
02719 {
02720 continue;
02721 }
02722 _ij11[0] = ij11; _ij11[1] = -1;
02723 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
02724 {
02725 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
02726 {
02727 j11valid[iij11]=false; _ij11[1] = iij11; break;
02728 }
02729 }
02730 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
02731 {
02732 IkReal evalcond[3];
02733 IkReal x627=IKsin(j11);
02734 IkReal x628=((IkReal(1.00000000000000))*(sj13));
02735 IkReal x629=((cj14)*(npx));
02736 IkReal x630=((npy)*(sj14));
02737 IkReal x631=((IkReal(0.250000000000000))*(x627));
02738 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14)))+(((sj12)*(x631))));
02739 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(x629)))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x630)))+(((IkReal(-1.00000000000000))*(npz)*(x628))));
02740 evalcond[2]=((((IkReal(0.0950000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x628)*(x629)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(cj12)*(x631)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x630))));
02741 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
02742 {
02743 continue;
02744 }
02745 }
02746
02747 {
02748 IkReal dummyeval[1];
02749 IkReal gconst4;
02750 gconst4=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
02751 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
02752 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02753 {
02754 {
02755 IkReal dummyeval[1];
02756 IkReal gconst5;
02757 gconst5=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
02758 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
02759 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02760 {
02761 {
02762 IkReal dummyeval[1];
02763 IkReal gconst2;
02764 IkReal x632=(cj14)*(cj14);
02765 IkReal x633=(sj14)*(sj14);
02766 IkReal x634=((IkReal(1.00000000000000))*(r01));
02767 IkReal x635=((sj13)*(sj14));
02768 IkReal x636=((cj14)*(sj13));
02769 IkReal x637=((r00)*(r11));
02770 IkReal x638=((cj13)*(x632));
02771 IkReal x639=((cj13)*(x633));
02772 gconst2=IKsign(((((r02)*(r11)*(x636)))+(((r02)*(r10)*(x635)))+(((IkReal(-1.00000000000000))*(r12)*(x634)*(x636)))+(((IkReal(-1.00000000000000))*(r10)*(x634)*(x639)))+(((IkReal(-1.00000000000000))*(r10)*(x634)*(x638)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x635)))+(((x637)*(x639)))+(((x637)*(x638)))));
02773 IkReal x640=(cj14)*(cj14);
02774 IkReal x641=(sj14)*(sj14);
02775 IkReal x642=((IkReal(1.00000000000000))*(r01));
02776 IkReal x643=((sj13)*(sj14));
02777 IkReal x644=((cj14)*(sj13));
02778 IkReal x645=((r00)*(r11));
02779 IkReal x646=((cj13)*(x640));
02780 IkReal x647=((cj13)*(x641));
02781 dummyeval[0]=((((r02)*(r10)*(x643)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x643)))+(((IkReal(-1.00000000000000))*(r12)*(x642)*(x644)))+(((x645)*(x647)))+(((x645)*(x646)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x647)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x646)))+(((r02)*(r11)*(x644))));
02782 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02783 {
02784 {
02785 IkReal evalcond[5];
02786 IkReal x648=((IkReal(1.00000000000000))*(sj13));
02787 IkReal x649=((npy)*(sj14));
02788 IkReal x650=((IkReal(1.00000000000000))*(cj14)*(npx));
02789 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
02790 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
02791 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x648)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x650)))+(((cj13)*(x649))));
02792 evalcond[3]=((((r21)*(sj13)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x648)))+(((cj13)*(r22))));
02793 evalcond[4]=((IkReal(0.0950000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x648)))+(((sj13)*(x649)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
02794 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 )
02795 {
02796 {
02797 IkReal dummyeval[1];
02798 IkReal gconst20;
02799 gconst20=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
02800 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
02801 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02802 {
02803 {
02804 IkReal dummyeval[1];
02805 IkReal gconst18;
02806 IkReal x651=(sj14)*(sj14);
02807 IkReal x652=(cj14)*(cj14);
02808 IkReal x653=((r01)*(r10));
02809 IkReal x654=((cj13)*(cj14));
02810 IkReal x655=((IkReal(1.00000000000000))*(r12));
02811 IkReal x656=((cj13)*(sj14));
02812 IkReal x657=((sj13)*(x651));
02813 IkReal x658=((IkReal(1.00000000000000))*(r00)*(r11));
02814 IkReal x659=((sj13)*(x652));
02815 gconst18=IKsign(((((IkReal(-1.00000000000000))*(r00)*(x655)*(x656)))+(((IkReal(-1.00000000000000))*(x657)*(x658)))+(((x653)*(x657)))+(((x653)*(x659)))+(((r02)*(r10)*(x656)))+(((IkReal(-1.00000000000000))*(r01)*(x654)*(x655)))+(((IkReal(-1.00000000000000))*(x658)*(x659)))+(((r02)*(r11)*(x654)))));
02816 IkReal x660=(sj14)*(sj14);
02817 IkReal x661=(cj14)*(cj14);
02818 IkReal x662=((r01)*(r10));
02819 IkReal x663=((cj13)*(cj14));
02820 IkReal x664=((IkReal(1.00000000000000))*(r12));
02821 IkReal x665=((cj13)*(sj14));
02822 IkReal x666=((sj13)*(x660));
02823 IkReal x667=((IkReal(1.00000000000000))*(r00)*(r11));
02824 IkReal x668=((sj13)*(x661));
02825 dummyeval[0]=((((x662)*(x668)))+(((x662)*(x666)))+(((IkReal(-1.00000000000000))*(r01)*(x663)*(x664)))+(((IkReal(-1.00000000000000))*(r00)*(x664)*(x665)))+(((r02)*(r11)*(x663)))+(((IkReal(-1.00000000000000))*(x666)*(x667)))+(((IkReal(-1.00000000000000))*(x667)*(x668)))+(((r02)*(r10)*(x665))));
02826 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02827 {
02828 {
02829 IkReal dummyeval[1];
02830 IkReal gconst19;
02831 IkReal x669=(sj13)*(sj13);
02832 IkReal x670=(cj13)*(cj13);
02833 IkReal x671=((cj14)*(r00));
02834 IkReal x672=((r11)*(sj14));
02835 IkReal x673=((r12)*(x670));
02836 IkReal x674=((IkReal(1.00000000000000))*(r01)*(sj14));
02837 IkReal x675=((r02)*(x669));
02838 IkReal x676=((IkReal(1.00000000000000))*(cj14)*(r10));
02839 IkReal x677=((r12)*(x669));
02840 IkReal x678=((r02)*(x670));
02841 gconst19=IKsign(((((x672)*(x675)))+(((x672)*(x678)))+(((IkReal(-1.00000000000000))*(x675)*(x676)))+(((x671)*(x673)))+(((x671)*(x677)))+(((IkReal(-1.00000000000000))*(x676)*(x678)))+(((IkReal(-1.00000000000000))*(x674)*(x677)))+(((IkReal(-1.00000000000000))*(x673)*(x674)))));
02842 IkReal x679=(sj13)*(sj13);
02843 IkReal x680=(cj13)*(cj13);
02844 IkReal x681=((cj14)*(r00));
02845 IkReal x682=((r11)*(sj14));
02846 IkReal x683=((r12)*(x680));
02847 IkReal x684=((IkReal(1.00000000000000))*(r01)*(sj14));
02848 IkReal x685=((r02)*(x679));
02849 IkReal x686=((IkReal(1.00000000000000))*(cj14)*(r10));
02850 IkReal x687=((r12)*(x679));
02851 IkReal x688=((r02)*(x680));
02852 dummyeval[0]=((((IkReal(-1.00000000000000))*(x685)*(x686)))+(((x682)*(x688)))+(((x682)*(x685)))+(((IkReal(-1.00000000000000))*(x686)*(x688)))+(((IkReal(-1.00000000000000))*(x684)*(x687)))+(((x681)*(x683)))+(((x681)*(x687)))+(((IkReal(-1.00000000000000))*(x683)*(x684))));
02853 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02854 {
02855 continue;
02856
02857 } else
02858 {
02859 {
02860 IkReal j9array[1], cj9array[1], sj9array[1];
02861 bool j9valid[1]={false};
02862 _nj9 = 1;
02863 IkReal x689=((cj13)*(cj14));
02864 IkReal x690=((IkReal(1.00000000000000))*(cj13)*(sj14));
02865 if( IKabs(((gconst19)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x690)))+(((r10)*(x689))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x690)))+(((r00)*(x689))))))) < IKFAST_ATAN2_MAGTHRESH )
02866 continue;
02867 j9array[0]=IKatan2(((gconst19)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x690)))+(((r10)*(x689)))))), ((gconst19)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x690)))+(((r00)*(x689)))))));
02868 sj9array[0]=IKsin(j9array[0]);
02869 cj9array[0]=IKcos(j9array[0]);
02870 if( j9array[0] > IKPI )
02871 {
02872 j9array[0]-=IK2PI;
02873 }
02874 else if( j9array[0] < -IKPI )
02875 { j9array[0]+=IK2PI;
02876 }
02877 j9valid[0] = true;
02878 for(int ij9 = 0; ij9 < 1; ++ij9)
02879 {
02880 if( !j9valid[ij9] )
02881 {
02882 continue;
02883 }
02884 _ij9[0] = ij9; _ij9[1] = -1;
02885 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
02886 {
02887 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
02888 {
02889 j9valid[iij9]=false; _ij9[1] = iij9; break;
02890 }
02891 }
02892 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
02893 {
02894 IkReal evalcond[4];
02895 IkReal x691=IKsin(j9);
02896 IkReal x692=IKcos(j9);
02897 IkReal x693=((IkReal(1.00000000000000))*(sj14));
02898 IkReal x694=((cj13)*(cj14));
02899 IkReal x695=((IkReal(1.00000000000000))*(r00));
02900 IkReal x696=((r01)*(sj14));
02901 IkReal x697=((IkReal(1.00000000000000))*(cj13));
02902 IkReal x698=((sj13)*(x692));
02903 IkReal x699=((r02)*(x691));
02904 IkReal x700=((r10)*(x692));
02905 IkReal x701=((r11)*(x692));
02906 IkReal x702=((sj13)*(x691));
02907 IkReal x703=((cj14)*(x702));
02908 evalcond[0]=((((IkReal(-1.00000000000000))*(x693)*(x700)))+(((r00)*(sj14)*(x691)))+(((cj14)*(r01)*(x691)))+(((IkReal(-1.00000000000000))*(cj14)*(x701))));
02909 evalcond[1]=((((IkReal(-1.00000000000000))*(x691)*(x694)*(x695)))+(((x694)*(x700)))+(((IkReal(-1.00000000000000))*(sj13)*(x699)))+(((r12)*(x698)))+(((cj13)*(x691)*(x696)))+(((IkReal(-1.00000000000000))*(cj13)*(x693)*(x701))));
02910 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x693)*(x698)))+(((IkReal(-1.00000000000000))*(x695)*(x703)))+(((cj13)*(x699)))+(((cj14)*(r10)*(x698)))+(((IkReal(-1.00000000000000))*(r12)*(x692)*(x697)))+(((x696)*(x702))));
02911 evalcond[3]=((((cj14)*(r00)*(x698)))+(((IkReal(-1.00000000000000))*(r02)*(x692)*(x697)))+(((IkReal(-1.00000000000000))*(r01)*(x693)*(x698)))+(((IkReal(-1.00000000000000))*(r12)*(x691)*(x697)))+(((IkReal(-1.00000000000000))*(r11)*(x693)*(x702)))+(((r10)*(x703))));
02912 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02913 {
02914 continue;
02915 }
02916 }
02917
02918 {
02919 IkReal dummyeval[1];
02920 IkReal gconst21;
02921 gconst21=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
02922 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
02923 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02924 {
02925 {
02926 IkReal dummyeval[1];
02927 IkReal gconst22;
02928 gconst22=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
02929 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
02930 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
02931 {
02932 continue;
02933
02934 } else
02935 {
02936 {
02937 IkReal j10array[1], cj10array[1], sj10array[1];
02938 bool j10valid[1]={false};
02939 _nj10 = 1;
02940 IkReal x704=((IkReal(1.00000000000000))*(cj11));
02941 IkReal x705=((r20)*(sj14));
02942 IkReal x706=((cj14)*(r21));
02943 IkReal x707=((cj14)*(cj9)*(r01));
02944 IkReal x708=((r10)*(sj14)*(sj9));
02945 IkReal x709=((cj9)*(r00)*(sj14));
02946 IkReal x710=((cj14)*(r11)*(sj9));
02947 if( IKabs(((gconst22)*(((((sj11)*(x708)))+(((sj11)*(x709)))+(((sj11)*(x707)))+(((sj11)*(x710)))+(((cj11)*(x706)))+(((cj11)*(x705))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((sj11)*(x705)))+(((sj11)*(x706)))+(((IkReal(-1.00000000000000))*(x704)*(x710)))+(((IkReal(-1.00000000000000))*(x704)*(x707)))+(((IkReal(-1.00000000000000))*(x704)*(x709)))+(((IkReal(-1.00000000000000))*(x704)*(x708))))))) < IKFAST_ATAN2_MAGTHRESH )
02948 continue;
02949 j10array[0]=IKatan2(((gconst22)*(((((sj11)*(x708)))+(((sj11)*(x709)))+(((sj11)*(x707)))+(((sj11)*(x710)))+(((cj11)*(x706)))+(((cj11)*(x705)))))), ((gconst22)*(((((sj11)*(x705)))+(((sj11)*(x706)))+(((IkReal(-1.00000000000000))*(x704)*(x710)))+(((IkReal(-1.00000000000000))*(x704)*(x707)))+(((IkReal(-1.00000000000000))*(x704)*(x709)))+(((IkReal(-1.00000000000000))*(x704)*(x708)))))));
02950 sj10array[0]=IKsin(j10array[0]);
02951 cj10array[0]=IKcos(j10array[0]);
02952 if( j10array[0] > IKPI )
02953 {
02954 j10array[0]-=IK2PI;
02955 }
02956 else if( j10array[0] < -IKPI )
02957 { j10array[0]+=IK2PI;
02958 }
02959 j10valid[0] = true;
02960 for(int ij10 = 0; ij10 < 1; ++ij10)
02961 {
02962 if( !j10valid[ij10] )
02963 {
02964 continue;
02965 }
02966 _ij10[0] = ij10; _ij10[1] = -1;
02967 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
02968 {
02969 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
02970 {
02971 j10valid[iij10]=false; _ij10[1] = iij10; break;
02972 }
02973 }
02974 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
02975 {
02976 IkReal evalcond[4];
02977 IkReal x711=IKsin(j10);
02978 IkReal x712=IKcos(j10);
02979 IkReal x713=((cj13)*(sj14));
02980 IkReal x714=((cj13)*(cj14));
02981 IkReal x715=((r10)*(sj9));
02982 IkReal x716=((IkReal(1.00000000000000))*(cj9));
02983 IkReal x717=((sj11)*(x711));
02984 IkReal x718=((IkReal(1.00000000000000))*(x712));
02985 IkReal x719=((cj11)*(x711));
02986 IkReal x720=((IkReal(1.00000000000000))*(r11)*(sj9));
02987 IkReal x721=((cj11)*(x718));
02988 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(x718)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x719))));
02989 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x714)))+(((IkReal(-1.00000000000000))*(x721)))+(((r21)*(x713)))+(x717));
02990 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x720)))+(((IkReal(-1.00000000000000))*(x721)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x716)))+(((IkReal(-1.00000000000000))*(sj14)*(x715)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x716)))+(x717));
02991 evalcond[3]=((((IkReal(-1.00000000000000))*(x713)*(x720)))+(((x714)*(x715)))+(((sj11)*(x712)))+(((IkReal(-1.00000000000000))*(r01)*(x713)*(x716)))+(((cj9)*(r00)*(x714)))+(((cj9)*(r02)*(sj13)))+(x719)+(((r12)*(sj13)*(sj9))));
02992 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
02993 {
02994 continue;
02995 }
02996 }
02997
02998 {
02999 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03000 vinfos[0].jointtype = 1;
03001 vinfos[0].foffset = j9;
03002 vinfos[0].indices[0] = _ij9[0];
03003 vinfos[0].indices[1] = _ij9[1];
03004 vinfos[0].maxsolutions = _nj9;
03005 vinfos[1].jointtype = 1;
03006 vinfos[1].foffset = j10;
03007 vinfos[1].indices[0] = _ij10[0];
03008 vinfos[1].indices[1] = _ij10[1];
03009 vinfos[1].maxsolutions = _nj10;
03010 vinfos[2].jointtype = 1;
03011 vinfos[2].foffset = j11;
03012 vinfos[2].indices[0] = _ij11[0];
03013 vinfos[2].indices[1] = _ij11[1];
03014 vinfos[2].maxsolutions = _nj11;
03015 vinfos[3].jointtype = 1;
03016 vinfos[3].foffset = j12;
03017 vinfos[3].indices[0] = _ij12[0];
03018 vinfos[3].indices[1] = _ij12[1];
03019 vinfos[3].maxsolutions = _nj12;
03020 vinfos[4].jointtype = 1;
03021 vinfos[4].foffset = j13;
03022 vinfos[4].indices[0] = _ij13[0];
03023 vinfos[4].indices[1] = _ij13[1];
03024 vinfos[4].maxsolutions = _nj13;
03025 vinfos[5].jointtype = 1;
03026 vinfos[5].foffset = j14;
03027 vinfos[5].indices[0] = _ij14[0];
03028 vinfos[5].indices[1] = _ij14[1];
03029 vinfos[5].maxsolutions = _nj14;
03030 std::vector<int> vfree(0);
03031 solutions.AddSolution(vinfos,vfree);
03032 }
03033 }
03034 }
03035
03036 }
03037
03038 }
03039
03040 } else
03041 {
03042 {
03043 IkReal j10array[1], cj10array[1], sj10array[1];
03044 bool j10valid[1]={false};
03045 _nj10 = 1;
03046 IkReal x722=((cj11)*(r20));
03047 IkReal x723=((IkReal(1.00000000000000))*(cj13));
03048 IkReal x724=((r21)*(sj14));
03049 IkReal x725=((r22)*(sj13));
03050 IkReal x726=((cj14)*(sj11));
03051 if( IKabs(((gconst21)*(((((sj14)*(x722)))+(((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x726)))+(((IkReal(-1.00000000000000))*(sj11)*(x723)*(x724)))+(((sj11)*(x725))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x725)))+(((cj11)*(cj13)*(x724)))+(((r20)*(sj11)*(sj14)))+(((r21)*(x726)))+(((IkReal(-1.00000000000000))*(cj14)*(x722)*(x723))))))) < IKFAST_ATAN2_MAGTHRESH )
03052 continue;
03053 j10array[0]=IKatan2(((gconst21)*(((((sj14)*(x722)))+(((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x726)))+(((IkReal(-1.00000000000000))*(sj11)*(x723)*(x724)))+(((sj11)*(x725)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x725)))+(((cj11)*(cj13)*(x724)))+(((r20)*(sj11)*(sj14)))+(((r21)*(x726)))+(((IkReal(-1.00000000000000))*(cj14)*(x722)*(x723)))))));
03054 sj10array[0]=IKsin(j10array[0]);
03055 cj10array[0]=IKcos(j10array[0]);
03056 if( j10array[0] > IKPI )
03057 {
03058 j10array[0]-=IK2PI;
03059 }
03060 else if( j10array[0] < -IKPI )
03061 { j10array[0]+=IK2PI;
03062 }
03063 j10valid[0] = true;
03064 for(int ij10 = 0; ij10 < 1; ++ij10)
03065 {
03066 if( !j10valid[ij10] )
03067 {
03068 continue;
03069 }
03070 _ij10[0] = ij10; _ij10[1] = -1;
03071 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
03072 {
03073 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
03074 {
03075 j10valid[iij10]=false; _ij10[1] = iij10; break;
03076 }
03077 }
03078 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
03079 {
03080 IkReal evalcond[4];
03081 IkReal x727=IKsin(j10);
03082 IkReal x728=IKcos(j10);
03083 IkReal x729=((cj13)*(sj14));
03084 IkReal x730=((cj13)*(cj14));
03085 IkReal x731=((r10)*(sj9));
03086 IkReal x732=((IkReal(1.00000000000000))*(cj9));
03087 IkReal x733=((sj11)*(x727));
03088 IkReal x734=((IkReal(1.00000000000000))*(x728));
03089 IkReal x735=((cj11)*(x727));
03090 IkReal x736=((IkReal(1.00000000000000))*(r11)*(sj9));
03091 IkReal x737=((cj11)*(x734));
03092 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x734)))+(((IkReal(-1.00000000000000))*(x735))));
03093 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x730)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x737)))+(x733)+(((r21)*(x729))));
03094 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x736)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x732)))+(((IkReal(-1.00000000000000))*(sj14)*(x731)))+(((IkReal(-1.00000000000000))*(x737)))+(x733)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x732))));
03095 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x729)*(x732)))+(((cj9)*(r00)*(x730)))+(((cj9)*(r02)*(sj13)))+(x735)+(((r12)*(sj13)*(sj9)))+(((sj11)*(x728)))+(((IkReal(-1.00000000000000))*(x729)*(x736)))+(((x730)*(x731))));
03096 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03097 {
03098 continue;
03099 }
03100 }
03101
03102 {
03103 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03104 vinfos[0].jointtype = 1;
03105 vinfos[0].foffset = j9;
03106 vinfos[0].indices[0] = _ij9[0];
03107 vinfos[0].indices[1] = _ij9[1];
03108 vinfos[0].maxsolutions = _nj9;
03109 vinfos[1].jointtype = 1;
03110 vinfos[1].foffset = j10;
03111 vinfos[1].indices[0] = _ij10[0];
03112 vinfos[1].indices[1] = _ij10[1];
03113 vinfos[1].maxsolutions = _nj10;
03114 vinfos[2].jointtype = 1;
03115 vinfos[2].foffset = j11;
03116 vinfos[2].indices[0] = _ij11[0];
03117 vinfos[2].indices[1] = _ij11[1];
03118 vinfos[2].maxsolutions = _nj11;
03119 vinfos[3].jointtype = 1;
03120 vinfos[3].foffset = j12;
03121 vinfos[3].indices[0] = _ij12[0];
03122 vinfos[3].indices[1] = _ij12[1];
03123 vinfos[3].maxsolutions = _nj12;
03124 vinfos[4].jointtype = 1;
03125 vinfos[4].foffset = j13;
03126 vinfos[4].indices[0] = _ij13[0];
03127 vinfos[4].indices[1] = _ij13[1];
03128 vinfos[4].maxsolutions = _nj13;
03129 vinfos[5].jointtype = 1;
03130 vinfos[5].foffset = j14;
03131 vinfos[5].indices[0] = _ij14[0];
03132 vinfos[5].indices[1] = _ij14[1];
03133 vinfos[5].maxsolutions = _nj14;
03134 std::vector<int> vfree(0);
03135 solutions.AddSolution(vinfos,vfree);
03136 }
03137 }
03138 }
03139
03140 }
03141
03142 }
03143 }
03144 }
03145
03146 }
03147
03148 }
03149
03150 } else
03151 {
03152 {
03153 IkReal j9array[1], cj9array[1], sj9array[1];
03154 bool j9valid[1]={false};
03155 _nj9 = 1;
03156 IkReal x738=((IkReal(1.00000000000000))*(cj14));
03157 IkReal x739=((IkReal(1.00000000000000))*(sj14));
03158 if( IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r10)*(x739)))+(((IkReal(-1.00000000000000))*(r11)*(x738))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x738)))+(((IkReal(-1.00000000000000))*(r00)*(x739))))))) < IKFAST_ATAN2_MAGTHRESH )
03159 continue;
03160 j9array[0]=IKatan2(((gconst18)*(((((IkReal(-1.00000000000000))*(r10)*(x739)))+(((IkReal(-1.00000000000000))*(r11)*(x738)))))), ((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x738)))+(((IkReal(-1.00000000000000))*(r00)*(x739)))))));
03161 sj9array[0]=IKsin(j9array[0]);
03162 cj9array[0]=IKcos(j9array[0]);
03163 if( j9array[0] > IKPI )
03164 {
03165 j9array[0]-=IK2PI;
03166 }
03167 else if( j9array[0] < -IKPI )
03168 { j9array[0]+=IK2PI;
03169 }
03170 j9valid[0] = true;
03171 for(int ij9 = 0; ij9 < 1; ++ij9)
03172 {
03173 if( !j9valid[ij9] )
03174 {
03175 continue;
03176 }
03177 _ij9[0] = ij9; _ij9[1] = -1;
03178 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
03179 {
03180 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
03181 {
03182 j9valid[iij9]=false; _ij9[1] = iij9; break;
03183 }
03184 }
03185 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
03186 {
03187 IkReal evalcond[4];
03188 IkReal x740=IKsin(j9);
03189 IkReal x741=IKcos(j9);
03190 IkReal x742=((IkReal(1.00000000000000))*(sj14));
03191 IkReal x743=((cj13)*(cj14));
03192 IkReal x744=((IkReal(1.00000000000000))*(r00));
03193 IkReal x745=((r01)*(sj14));
03194 IkReal x746=((IkReal(1.00000000000000))*(cj13));
03195 IkReal x747=((sj13)*(x741));
03196 IkReal x748=((r02)*(x740));
03197 IkReal x749=((r10)*(x741));
03198 IkReal x750=((r11)*(x741));
03199 IkReal x751=((sj13)*(x740));
03200 IkReal x752=((cj14)*(x751));
03201 evalcond[0]=((((cj14)*(r01)*(x740)))+(((r00)*(sj14)*(x740)))+(((IkReal(-1.00000000000000))*(x742)*(x749)))+(((IkReal(-1.00000000000000))*(cj14)*(x750))));
03202 evalcond[1]=((((IkReal(-1.00000000000000))*(x740)*(x743)*(x744)))+(((cj13)*(x740)*(x745)))+(((IkReal(-1.00000000000000))*(sj13)*(x748)))+(((x743)*(x749)))+(((r12)*(x747)))+(((IkReal(-1.00000000000000))*(cj13)*(x742)*(x750))));
03203 evalcond[2]=((IkReal(1.00000000000000))+(((x745)*(x751)))+(((IkReal(-1.00000000000000))*(r11)*(x742)*(x747)))+(((cj13)*(x748)))+(((cj14)*(r10)*(x747)))+(((IkReal(-1.00000000000000))*(r12)*(x741)*(x746)))+(((IkReal(-1.00000000000000))*(x744)*(x752))));
03204 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x742)*(x747)))+(((IkReal(-1.00000000000000))*(r12)*(x740)*(x746)))+(((r10)*(x752)))+(((IkReal(-1.00000000000000))*(r11)*(x742)*(x751)))+(((IkReal(-1.00000000000000))*(r02)*(x741)*(x746)))+(((cj14)*(r00)*(x747))));
03205 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03206 {
03207 continue;
03208 }
03209 }
03210
03211 {
03212 IkReal dummyeval[1];
03213 IkReal gconst21;
03214 gconst21=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
03215 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
03216 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03217 {
03218 {
03219 IkReal dummyeval[1];
03220 IkReal gconst22;
03221 gconst22=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
03222 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
03223 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03224 {
03225 continue;
03226
03227 } else
03228 {
03229 {
03230 IkReal j10array[1], cj10array[1], sj10array[1];
03231 bool j10valid[1]={false};
03232 _nj10 = 1;
03233 IkReal x753=((IkReal(1.00000000000000))*(cj11));
03234 IkReal x754=((r20)*(sj14));
03235 IkReal x755=((cj14)*(r21));
03236 IkReal x756=((cj14)*(cj9)*(r01));
03237 IkReal x757=((r10)*(sj14)*(sj9));
03238 IkReal x758=((cj9)*(r00)*(sj14));
03239 IkReal x759=((cj14)*(r11)*(sj9));
03240 if( IKabs(((gconst22)*(((((cj11)*(x755)))+(((cj11)*(x754)))+(((sj11)*(x756)))+(((sj11)*(x757)))+(((sj11)*(x758)))+(((sj11)*(x759))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(x753)*(x757)))+(((IkReal(-1.00000000000000))*(x753)*(x756)))+(((IkReal(-1.00000000000000))*(x753)*(x758)))+(((IkReal(-1.00000000000000))*(x753)*(x759)))+(((sj11)*(x754)))+(((sj11)*(x755))))))) < IKFAST_ATAN2_MAGTHRESH )
03241 continue;
03242 j10array[0]=IKatan2(((gconst22)*(((((cj11)*(x755)))+(((cj11)*(x754)))+(((sj11)*(x756)))+(((sj11)*(x757)))+(((sj11)*(x758)))+(((sj11)*(x759)))))), ((gconst22)*(((((IkReal(-1.00000000000000))*(x753)*(x757)))+(((IkReal(-1.00000000000000))*(x753)*(x756)))+(((IkReal(-1.00000000000000))*(x753)*(x758)))+(((IkReal(-1.00000000000000))*(x753)*(x759)))+(((sj11)*(x754)))+(((sj11)*(x755)))))));
03243 sj10array[0]=IKsin(j10array[0]);
03244 cj10array[0]=IKcos(j10array[0]);
03245 if( j10array[0] > IKPI )
03246 {
03247 j10array[0]-=IK2PI;
03248 }
03249 else if( j10array[0] < -IKPI )
03250 { j10array[0]+=IK2PI;
03251 }
03252 j10valid[0] = true;
03253 for(int ij10 = 0; ij10 < 1; ++ij10)
03254 {
03255 if( !j10valid[ij10] )
03256 {
03257 continue;
03258 }
03259 _ij10[0] = ij10; _ij10[1] = -1;
03260 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
03261 {
03262 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
03263 {
03264 j10valid[iij10]=false; _ij10[1] = iij10; break;
03265 }
03266 }
03267 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
03268 {
03269 IkReal evalcond[4];
03270 IkReal x760=IKsin(j10);
03271 IkReal x761=IKcos(j10);
03272 IkReal x762=((cj13)*(sj14));
03273 IkReal x763=((cj13)*(cj14));
03274 IkReal x764=((r10)*(sj9));
03275 IkReal x765=((IkReal(1.00000000000000))*(cj9));
03276 IkReal x766=((sj11)*(x760));
03277 IkReal x767=((IkReal(1.00000000000000))*(x761));
03278 IkReal x768=((cj11)*(x760));
03279 IkReal x769=((IkReal(1.00000000000000))*(r11)*(sj9));
03280 IkReal x770=((cj11)*(x767));
03281 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x768)))+(((IkReal(-1.00000000000000))*(sj11)*(x767))));
03282 evalcond[1]=((((r21)*(x762)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x763)))+(x766)+(((IkReal(-1.00000000000000))*(x770))));
03283 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x765)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x765)))+(((IkReal(-1.00000000000000))*(cj14)*(x769)))+(x766)+(((IkReal(-1.00000000000000))*(x770)))+(((IkReal(-1.00000000000000))*(sj14)*(x764))));
03284 evalcond[3]=((((sj11)*(x761)))+(((cj9)*(r00)*(x763)))+(((IkReal(-1.00000000000000))*(x762)*(x769)))+(((cj9)*(r02)*(sj13)))+(((x763)*(x764)))+(((IkReal(-1.00000000000000))*(r01)*(x762)*(x765)))+(x768)+(((r12)*(sj13)*(sj9))));
03285 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03286 {
03287 continue;
03288 }
03289 }
03290
03291 {
03292 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03293 vinfos[0].jointtype = 1;
03294 vinfos[0].foffset = j9;
03295 vinfos[0].indices[0] = _ij9[0];
03296 vinfos[0].indices[1] = _ij9[1];
03297 vinfos[0].maxsolutions = _nj9;
03298 vinfos[1].jointtype = 1;
03299 vinfos[1].foffset = j10;
03300 vinfos[1].indices[0] = _ij10[0];
03301 vinfos[1].indices[1] = _ij10[1];
03302 vinfos[1].maxsolutions = _nj10;
03303 vinfos[2].jointtype = 1;
03304 vinfos[2].foffset = j11;
03305 vinfos[2].indices[0] = _ij11[0];
03306 vinfos[2].indices[1] = _ij11[1];
03307 vinfos[2].maxsolutions = _nj11;
03308 vinfos[3].jointtype = 1;
03309 vinfos[3].foffset = j12;
03310 vinfos[3].indices[0] = _ij12[0];
03311 vinfos[3].indices[1] = _ij12[1];
03312 vinfos[3].maxsolutions = _nj12;
03313 vinfos[4].jointtype = 1;
03314 vinfos[4].foffset = j13;
03315 vinfos[4].indices[0] = _ij13[0];
03316 vinfos[4].indices[1] = _ij13[1];
03317 vinfos[4].maxsolutions = _nj13;
03318 vinfos[5].jointtype = 1;
03319 vinfos[5].foffset = j14;
03320 vinfos[5].indices[0] = _ij14[0];
03321 vinfos[5].indices[1] = _ij14[1];
03322 vinfos[5].maxsolutions = _nj14;
03323 std::vector<int> vfree(0);
03324 solutions.AddSolution(vinfos,vfree);
03325 }
03326 }
03327 }
03328
03329 }
03330
03331 }
03332
03333 } else
03334 {
03335 {
03336 IkReal j10array[1], cj10array[1], sj10array[1];
03337 bool j10valid[1]={false};
03338 _nj10 = 1;
03339 IkReal x771=((cj11)*(r20));
03340 IkReal x772=((IkReal(1.00000000000000))*(cj13));
03341 IkReal x773=((r21)*(sj14));
03342 IkReal x774=((r22)*(sj13));
03343 IkReal x775=((cj14)*(sj11));
03344 if( IKabs(((gconst21)*(((((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x775)))+(((sj11)*(x774)))+(((IkReal(-1.00000000000000))*(sj11)*(x772)*(x773)))+(((sj14)*(x771))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x774)))+(((cj11)*(cj13)*(x773)))+(((r21)*(x775)))+(((r20)*(sj11)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(x771)*(x772))))))) < IKFAST_ATAN2_MAGTHRESH )
03345 continue;
03346 j10array[0]=IKatan2(((gconst21)*(((((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x775)))+(((sj11)*(x774)))+(((IkReal(-1.00000000000000))*(sj11)*(x772)*(x773)))+(((sj14)*(x771)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x774)))+(((cj11)*(cj13)*(x773)))+(((r21)*(x775)))+(((r20)*(sj11)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(x771)*(x772)))))));
03347 sj10array[0]=IKsin(j10array[0]);
03348 cj10array[0]=IKcos(j10array[0]);
03349 if( j10array[0] > IKPI )
03350 {
03351 j10array[0]-=IK2PI;
03352 }
03353 else if( j10array[0] < -IKPI )
03354 { j10array[0]+=IK2PI;
03355 }
03356 j10valid[0] = true;
03357 for(int ij10 = 0; ij10 < 1; ++ij10)
03358 {
03359 if( !j10valid[ij10] )
03360 {
03361 continue;
03362 }
03363 _ij10[0] = ij10; _ij10[1] = -1;
03364 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
03365 {
03366 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
03367 {
03368 j10valid[iij10]=false; _ij10[1] = iij10; break;
03369 }
03370 }
03371 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
03372 {
03373 IkReal evalcond[4];
03374 IkReal x776=IKsin(j10);
03375 IkReal x777=IKcos(j10);
03376 IkReal x778=((cj13)*(sj14));
03377 IkReal x779=((cj13)*(cj14));
03378 IkReal x780=((r10)*(sj9));
03379 IkReal x781=((IkReal(1.00000000000000))*(cj9));
03380 IkReal x782=((sj11)*(x776));
03381 IkReal x783=((IkReal(1.00000000000000))*(x777));
03382 IkReal x784=((cj11)*(x776));
03383 IkReal x785=((IkReal(1.00000000000000))*(r11)*(sj9));
03384 IkReal x786=((cj11)*(x783));
03385 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x784)))+(((IkReal(-1.00000000000000))*(sj11)*(x783))));
03386 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x779)))+(((IkReal(-1.00000000000000))*(x786)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x778)))+(x782));
03387 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x781)))+(((IkReal(-1.00000000000000))*(x786)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x781)))+(((IkReal(-1.00000000000000))*(cj14)*(x785)))+(x782)+(((IkReal(-1.00000000000000))*(sj14)*(x780))));
03388 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x778)*(x781)))+(((cj9)*(r02)*(sj13)))+(((x779)*(x780)))+(((cj9)*(r00)*(x779)))+(((sj11)*(x777)))+(x784)+(((IkReal(-1.00000000000000))*(x778)*(x785)))+(((r12)*(sj13)*(sj9))));
03389 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03390 {
03391 continue;
03392 }
03393 }
03394
03395 {
03396 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03397 vinfos[0].jointtype = 1;
03398 vinfos[0].foffset = j9;
03399 vinfos[0].indices[0] = _ij9[0];
03400 vinfos[0].indices[1] = _ij9[1];
03401 vinfos[0].maxsolutions = _nj9;
03402 vinfos[1].jointtype = 1;
03403 vinfos[1].foffset = j10;
03404 vinfos[1].indices[0] = _ij10[0];
03405 vinfos[1].indices[1] = _ij10[1];
03406 vinfos[1].maxsolutions = _nj10;
03407 vinfos[2].jointtype = 1;
03408 vinfos[2].foffset = j11;
03409 vinfos[2].indices[0] = _ij11[0];
03410 vinfos[2].indices[1] = _ij11[1];
03411 vinfos[2].maxsolutions = _nj11;
03412 vinfos[3].jointtype = 1;
03413 vinfos[3].foffset = j12;
03414 vinfos[3].indices[0] = _ij12[0];
03415 vinfos[3].indices[1] = _ij12[1];
03416 vinfos[3].maxsolutions = _nj12;
03417 vinfos[4].jointtype = 1;
03418 vinfos[4].foffset = j13;
03419 vinfos[4].indices[0] = _ij13[0];
03420 vinfos[4].indices[1] = _ij13[1];
03421 vinfos[4].maxsolutions = _nj13;
03422 vinfos[5].jointtype = 1;
03423 vinfos[5].foffset = j14;
03424 vinfos[5].indices[0] = _ij14[0];
03425 vinfos[5].indices[1] = _ij14[1];
03426 vinfos[5].maxsolutions = _nj14;
03427 std::vector<int> vfree(0);
03428 solutions.AddSolution(vinfos,vfree);
03429 }
03430 }
03431 }
03432
03433 }
03434
03435 }
03436 }
03437 }
03438
03439 }
03440
03441 }
03442
03443 } else
03444 {
03445 {
03446 IkReal j10array[1], cj10array[1], sj10array[1];
03447 bool j10valid[1]={false};
03448 _nj10 = 1;
03449 IkReal x787=((cj11)*(r20));
03450 IkReal x788=((IkReal(1.00000000000000))*(cj13));
03451 IkReal x789=((r21)*(sj14));
03452 IkReal x790=((r22)*(sj13));
03453 IkReal x791=((r20)*(sj11));
03454 IkReal x792=((cj14)*(r21));
03455 if( IKabs(((gconst20)*(((((cj11)*(x792)))+(((IkReal(-1.00000000000000))*(sj11)*(x788)*(x789)))+(((cj13)*(cj14)*(x791)))+(((sj11)*(x790)))+(((sj14)*(x787))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(cj11)*(x790)))+(((cj11)*(cj13)*(x789)))+(((IkReal(-1.00000000000000))*(cj14)*(x787)*(x788)))+(((sj11)*(x792)))+(((sj14)*(x791))))))) < IKFAST_ATAN2_MAGTHRESH )
03456 continue;
03457 j10array[0]=IKatan2(((gconst20)*(((((cj11)*(x792)))+(((IkReal(-1.00000000000000))*(sj11)*(x788)*(x789)))+(((cj13)*(cj14)*(x791)))+(((sj11)*(x790)))+(((sj14)*(x787)))))), ((gconst20)*(((((IkReal(-1.00000000000000))*(cj11)*(x790)))+(((cj11)*(cj13)*(x789)))+(((IkReal(-1.00000000000000))*(cj14)*(x787)*(x788)))+(((sj11)*(x792)))+(((sj14)*(x791)))))));
03458 sj10array[0]=IKsin(j10array[0]);
03459 cj10array[0]=IKcos(j10array[0]);
03460 if( j10array[0] > IKPI )
03461 {
03462 j10array[0]-=IK2PI;
03463 }
03464 else if( j10array[0] < -IKPI )
03465 { j10array[0]+=IK2PI;
03466 }
03467 j10valid[0] = true;
03468 for(int ij10 = 0; ij10 < 1; ++ij10)
03469 {
03470 if( !j10valid[ij10] )
03471 {
03472 continue;
03473 }
03474 _ij10[0] = ij10; _ij10[1] = -1;
03475 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
03476 {
03477 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
03478 {
03479 j10valid[iij10]=false; _ij10[1] = iij10; break;
03480 }
03481 }
03482 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
03483 {
03484 IkReal evalcond[2];
03485 IkReal x793=IKsin(j10);
03486 IkReal x794=IKcos(j10);
03487 IkReal x795=((IkReal(1.00000000000000))*(x794));
03488 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(cj11)*(x793)))+(((IkReal(-1.00000000000000))*(sj11)*(x795))));
03489 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)))+(((IkReal(-1.00000000000000))*(cj11)*(x795)))+(((cj13)*(r21)*(sj14)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((sj11)*(x793))));
03490 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
03491 {
03492 continue;
03493 }
03494 }
03495
03496 {
03497 IkReal dummyeval[1];
03498 IkReal gconst24;
03499 IkReal x796=(sj14)*(sj14);
03500 IkReal x797=(cj14)*(cj14);
03501 IkReal x798=((r01)*(r10));
03502 IkReal x799=((cj13)*(cj14));
03503 IkReal x800=((IkReal(1.00000000000000))*(r12));
03504 IkReal x801=((cj13)*(sj14));
03505 IkReal x802=((sj13)*(x796));
03506 IkReal x803=((IkReal(1.00000000000000))*(r00)*(r11));
03507 IkReal x804=((sj13)*(x797));
03508 gconst24=IKsign(((((r02)*(r10)*(x801)))+(((r02)*(r11)*(x799)))+(((IkReal(-1.00000000000000))*(r00)*(x800)*(x801)))+(((x798)*(x802)))+(((x798)*(x804)))+(((IkReal(-1.00000000000000))*(r01)*(x799)*(x800)))+(((IkReal(-1.00000000000000))*(x803)*(x804)))+(((IkReal(-1.00000000000000))*(x802)*(x803)))));
03509 IkReal x805=(sj14)*(sj14);
03510 IkReal x806=(cj14)*(cj14);
03511 IkReal x807=((r01)*(r10));
03512 IkReal x808=((cj13)*(cj14));
03513 IkReal x809=((IkReal(1.00000000000000))*(r12));
03514 IkReal x810=((cj13)*(sj14));
03515 IkReal x811=((sj13)*(x805));
03516 IkReal x812=((IkReal(1.00000000000000))*(r00)*(r11));
03517 IkReal x813=((sj13)*(x806));
03518 dummyeval[0]=((((IkReal(-1.00000000000000))*(x812)*(x813)))+(((r02)*(r11)*(x808)))+(((IkReal(-1.00000000000000))*(r00)*(x809)*(x810)))+(((IkReal(-1.00000000000000))*(x811)*(x812)))+(((x807)*(x813)))+(((x807)*(x811)))+(((r02)*(r10)*(x810)))+(((IkReal(-1.00000000000000))*(r01)*(x808)*(x809))));
03519 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03520 {
03521 {
03522 IkReal dummyeval[1];
03523 IkReal gconst23;
03524 IkReal x814=(sj14)*(sj14);
03525 IkReal x815=(cj14)*(cj14);
03526 IkReal x816=((IkReal(1.00000000000000))*(x814));
03527 IkReal x817=((IkReal(2.00000000000000))*(cj14)*(sj14));
03528 IkReal x818=((IkReal(1.00000000000000))*(x815));
03529 gconst23=IKsign(((((IkReal(-1.00000000000000))*(x818)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x817)))+(((IkReal(-1.00000000000000))*(x816)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x816)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x818)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x817)))));
03530 IkReal x819=(sj14)*(sj14);
03531 IkReal x820=(cj14)*(cj14);
03532 IkReal x821=((IkReal(1.00000000000000))*(x819));
03533 IkReal x822=((IkReal(2.00000000000000))*(cj14)*(sj14));
03534 IkReal x823=((IkReal(1.00000000000000))*(x820));
03535 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r01)*(x822)))+(((IkReal(-1.00000000000000))*(x821)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x822)))+(((IkReal(-1.00000000000000))*(x821)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x823)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x823)*((r01)*(r01)))));
03536 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03537 {
03538 continue;
03539
03540 } else
03541 {
03542 {
03543 IkReal j9array[1], cj9array[1], sj9array[1];
03544 bool j9valid[1]={false};
03545 _nj9 = 1;
03546 IkReal x824=((cj14)*(r11));
03547 IkReal x825=((cj10)*(cj11));
03548 IkReal x826=((cj14)*(r01));
03549 IkReal x827=((sj14)*(x825));
03550 IkReal x828=((IkReal(1.00000000000000))*(sj10)*(sj11));
03551 if( IKabs(((gconst23)*(((((x824)*(x825)))+(((r10)*(x827)))+(((IkReal(-1.00000000000000))*(x824)*(x828)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(x828))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst23)*(((((x825)*(x826)))+(((r00)*(x827)))+(((IkReal(-1.00000000000000))*(x826)*(x828)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x828))))))) < IKFAST_ATAN2_MAGTHRESH )
03552 continue;
03553 j9array[0]=IKatan2(((gconst23)*(((((x824)*(x825)))+(((r10)*(x827)))+(((IkReal(-1.00000000000000))*(x824)*(x828)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(x828)))))), ((gconst23)*(((((x825)*(x826)))+(((r00)*(x827)))+(((IkReal(-1.00000000000000))*(x826)*(x828)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x828)))))));
03554 sj9array[0]=IKsin(j9array[0]);
03555 cj9array[0]=IKcos(j9array[0]);
03556 if( j9array[0] > IKPI )
03557 {
03558 j9array[0]-=IK2PI;
03559 }
03560 else if( j9array[0] < -IKPI )
03561 { j9array[0]+=IK2PI;
03562 }
03563 j9valid[0] = true;
03564 for(int ij9 = 0; ij9 < 1; ++ij9)
03565 {
03566 if( !j9valid[ij9] )
03567 {
03568 continue;
03569 }
03570 _ij9[0] = ij9; _ij9[1] = -1;
03571 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
03572 {
03573 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
03574 {
03575 j9valid[iij9]=false; _ij9[1] = iij9; break;
03576 }
03577 }
03578 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
03579 {
03580 IkReal evalcond[6];
03581 IkReal x829=IKsin(j9);
03582 IkReal x830=IKcos(j9);
03583 IkReal x831=((IkReal(1.00000000000000))*(cj14));
03584 IkReal x832=((IkReal(1.00000000000000))*(sj14));
03585 IkReal x833=((cj13)*(cj14));
03586 IkReal x834=((cj14)*(r10));
03587 IkReal x835=((r01)*(sj14));
03588 IkReal x836=((IkReal(1.00000000000000))*(r12));
03589 IkReal x837=((sj13)*(x830));
03590 IkReal x838=((r02)*(x829));
03591 IkReal x839=((r11)*(x829));
03592 IkReal x840=((r10)*(x830));
03593 IkReal x841=((r01)*(x830));
03594 IkReal x842=((sj13)*(x829));
03595 IkReal x843=((r11)*(x830));
03596 IkReal x844=((cj13)*(x829));
03597 IkReal x845=((r10)*(x829));
03598 IkReal x846=((r00)*(x830));
03599 IkReal x847=((cj13)*(x830));
03600 evalcond[0]=((((IkReal(-1.00000000000000))*(x831)*(x843)))+(((r00)*(sj14)*(x829)))+(((cj14)*(r01)*(x829)))+(((IkReal(-1.00000000000000))*(x832)*(x840))));
03601 evalcond[1]=((((IkReal(-1.00000000000000))*(x831)*(x841)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x832)*(x846)))+(((IkReal(-1.00000000000000))*(x832)*(x845)))+(((IkReal(-1.00000000000000))*(x831)*(x839))));
03602 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x838)))+(((x833)*(x840)))+(((x835)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x831)*(x844)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x843)))+(((r12)*(x837))));
03603 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x836)*(x847)))+(((x835)*(x842)))+(((IkReal(-1.00000000000000))*(r00)*(x831)*(x842)))+(((IkReal(-1.00000000000000))*(r11)*(x832)*(x837)))+(((x834)*(x837)))+(((cj13)*(x838))));
03604 evalcond[4]=((((x833)*(x845)))+(((x833)*(x846)))+(((r02)*(x837)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x839)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x841)))+(((r12)*(x842)))+(((cj11)*(sj10))));
03605 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x832)*(x839)))+(((IkReal(-1.00000000000000))*(x836)*(x844)))+(((IkReal(-1.00000000000000))*(r02)*(x847)))+(((cj14)*(r00)*(x837)))+(((x834)*(x842)))+(((IkReal(-1.00000000000000))*(r01)*(x832)*(x837))));
03606 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 )
03607 {
03608 continue;
03609 }
03610 }
03611
03612 {
03613 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03614 vinfos[0].jointtype = 1;
03615 vinfos[0].foffset = j9;
03616 vinfos[0].indices[0] = _ij9[0];
03617 vinfos[0].indices[1] = _ij9[1];
03618 vinfos[0].maxsolutions = _nj9;
03619 vinfos[1].jointtype = 1;
03620 vinfos[1].foffset = j10;
03621 vinfos[1].indices[0] = _ij10[0];
03622 vinfos[1].indices[1] = _ij10[1];
03623 vinfos[1].maxsolutions = _nj10;
03624 vinfos[2].jointtype = 1;
03625 vinfos[2].foffset = j11;
03626 vinfos[2].indices[0] = _ij11[0];
03627 vinfos[2].indices[1] = _ij11[1];
03628 vinfos[2].maxsolutions = _nj11;
03629 vinfos[3].jointtype = 1;
03630 vinfos[3].foffset = j12;
03631 vinfos[3].indices[0] = _ij12[0];
03632 vinfos[3].indices[1] = _ij12[1];
03633 vinfos[3].maxsolutions = _nj12;
03634 vinfos[4].jointtype = 1;
03635 vinfos[4].foffset = j13;
03636 vinfos[4].indices[0] = _ij13[0];
03637 vinfos[4].indices[1] = _ij13[1];
03638 vinfos[4].maxsolutions = _nj13;
03639 vinfos[5].jointtype = 1;
03640 vinfos[5].foffset = j14;
03641 vinfos[5].indices[0] = _ij14[0];
03642 vinfos[5].indices[1] = _ij14[1];
03643 vinfos[5].maxsolutions = _nj14;
03644 std::vector<int> vfree(0);
03645 solutions.AddSolution(vinfos,vfree);
03646 }
03647 }
03648 }
03649
03650 }
03651
03652 }
03653
03654 } else
03655 {
03656 {
03657 IkReal j9array[1], cj9array[1], sj9array[1];
03658 bool j9valid[1]={false};
03659 _nj9 = 1;
03660 IkReal x848=((IkReal(1.00000000000000))*(sj14));
03661 IkReal x849=((IkReal(1.00000000000000))*(cj14));
03662 if( IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r10)*(x848)))+(((IkReal(-1.00000000000000))*(r11)*(x849))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x849)))+(((IkReal(-1.00000000000000))*(r00)*(x848))))))) < IKFAST_ATAN2_MAGTHRESH )
03663 continue;
03664 j9array[0]=IKatan2(((gconst24)*(((((IkReal(-1.00000000000000))*(r10)*(x848)))+(((IkReal(-1.00000000000000))*(r11)*(x849)))))), ((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x849)))+(((IkReal(-1.00000000000000))*(r00)*(x848)))))));
03665 sj9array[0]=IKsin(j9array[0]);
03666 cj9array[0]=IKcos(j9array[0]);
03667 if( j9array[0] > IKPI )
03668 {
03669 j9array[0]-=IK2PI;
03670 }
03671 else if( j9array[0] < -IKPI )
03672 { j9array[0]+=IK2PI;
03673 }
03674 j9valid[0] = true;
03675 for(int ij9 = 0; ij9 < 1; ++ij9)
03676 {
03677 if( !j9valid[ij9] )
03678 {
03679 continue;
03680 }
03681 _ij9[0] = ij9; _ij9[1] = -1;
03682 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
03683 {
03684 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
03685 {
03686 j9valid[iij9]=false; _ij9[1] = iij9; break;
03687 }
03688 }
03689 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
03690 {
03691 IkReal evalcond[6];
03692 IkReal x850=IKsin(j9);
03693 IkReal x851=IKcos(j9);
03694 IkReal x852=((IkReal(1.00000000000000))*(cj14));
03695 IkReal x853=((IkReal(1.00000000000000))*(sj14));
03696 IkReal x854=((cj13)*(cj14));
03697 IkReal x855=((cj14)*(r10));
03698 IkReal x856=((r01)*(sj14));
03699 IkReal x857=((IkReal(1.00000000000000))*(r12));
03700 IkReal x858=((sj13)*(x851));
03701 IkReal x859=((r02)*(x850));
03702 IkReal x860=((r11)*(x850));
03703 IkReal x861=((r10)*(x851));
03704 IkReal x862=((r01)*(x851));
03705 IkReal x863=((sj13)*(x850));
03706 IkReal x864=((r11)*(x851));
03707 IkReal x865=((cj13)*(x850));
03708 IkReal x866=((r10)*(x850));
03709 IkReal x867=((r00)*(x851));
03710 IkReal x868=((cj13)*(x851));
03711 evalcond[0]=((((r00)*(sj14)*(x850)))+(((cj14)*(r01)*(x850)))+(((IkReal(-1.00000000000000))*(x852)*(x864)))+(((IkReal(-1.00000000000000))*(x853)*(x861))));
03712 evalcond[1]=((((IkReal(-1.00000000000000))*(x852)*(x860)))+(((IkReal(-1.00000000000000))*(x852)*(x862)))+(((IkReal(-1.00000000000000))*(x853)*(x867)))+(((IkReal(-1.00000000000000))*(x853)*(x866)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11))));
03713 evalcond[2]=((((r12)*(x858)))+(((x856)*(x865)))+(((IkReal(-1.00000000000000))*(r00)*(x852)*(x865)))+(((IkReal(-1.00000000000000))*(sj13)*(x859)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x864)))+(((x854)*(x861))));
03714 evalcond[3]=((IkReal(1.00000000000000))+(((x855)*(x858)))+(((cj13)*(x859)))+(((x856)*(x863)))+(((IkReal(-1.00000000000000))*(r00)*(x852)*(x863)))+(((IkReal(-1.00000000000000))*(r11)*(x853)*(x858)))+(((IkReal(-1.00000000000000))*(x857)*(x868))));
03715 evalcond[4]=((((r12)*(x863)))+(((cj10)*(sj11)))+(((r02)*(x858)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x860)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x862)))+(((x854)*(x866)))+(((x854)*(x867)))+(((cj11)*(sj10))));
03716 evalcond[5]=((((cj14)*(r00)*(x858)))+(((IkReal(-1.00000000000000))*(sj13)*(x853)*(x860)))+(((x855)*(x863)))+(((IkReal(-1.00000000000000))*(r02)*(x868)))+(((IkReal(-1.00000000000000))*(r01)*(x853)*(x858)))+(((IkReal(-1.00000000000000))*(x857)*(x865))));
03717 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 )
03718 {
03719 continue;
03720 }
03721 }
03722
03723 {
03724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03725 vinfos[0].jointtype = 1;
03726 vinfos[0].foffset = j9;
03727 vinfos[0].indices[0] = _ij9[0];
03728 vinfos[0].indices[1] = _ij9[1];
03729 vinfos[0].maxsolutions = _nj9;
03730 vinfos[1].jointtype = 1;
03731 vinfos[1].foffset = j10;
03732 vinfos[1].indices[0] = _ij10[0];
03733 vinfos[1].indices[1] = _ij10[1];
03734 vinfos[1].maxsolutions = _nj10;
03735 vinfos[2].jointtype = 1;
03736 vinfos[2].foffset = j11;
03737 vinfos[2].indices[0] = _ij11[0];
03738 vinfos[2].indices[1] = _ij11[1];
03739 vinfos[2].maxsolutions = _nj11;
03740 vinfos[3].jointtype = 1;
03741 vinfos[3].foffset = j12;
03742 vinfos[3].indices[0] = _ij12[0];
03743 vinfos[3].indices[1] = _ij12[1];
03744 vinfos[3].maxsolutions = _nj12;
03745 vinfos[4].jointtype = 1;
03746 vinfos[4].foffset = j13;
03747 vinfos[4].indices[0] = _ij13[0];
03748 vinfos[4].indices[1] = _ij13[1];
03749 vinfos[4].maxsolutions = _nj13;
03750 vinfos[5].jointtype = 1;
03751 vinfos[5].foffset = j14;
03752 vinfos[5].indices[0] = _ij14[0];
03753 vinfos[5].indices[1] = _ij14[1];
03754 vinfos[5].maxsolutions = _nj14;
03755 std::vector<int> vfree(0);
03756 solutions.AddSolution(vinfos,vfree);
03757 }
03758 }
03759 }
03760
03761 }
03762
03763 }
03764 }
03765 }
03766
03767 }
03768
03769 }
03770
03771 } else
03772 {
03773 IkReal x869=((IkReal(1.00000000000000))*(sj13));
03774 IkReal x870=((npy)*(sj14));
03775 IkReal x871=((IkReal(1.00000000000000))*(cj14)*(npx));
03776 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
03777 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
03778 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(x871)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x870)))+(((IkReal(-1.00000000000000))*(npz)*(x869))));
03779 evalcond[3]=((((r21)*(sj13)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x869)))+(((cj13)*(r22))));
03780 evalcond[4]=((IkReal(-0.0950000000000000))+(((sj13)*(x870)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x869)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
03781 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 )
03782 {
03783 {
03784 IkReal dummyeval[1];
03785 IkReal gconst27;
03786 gconst27=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
03787 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
03788 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03789 {
03790 {
03791 IkReal dummyeval[1];
03792 IkReal gconst25;
03793 IkReal x872=(sj14)*(sj14);
03794 IkReal x873=(cj14)*(cj14);
03795 IkReal x874=((cj13)*(r12));
03796 IkReal x875=((IkReal(1.00000000000000))*(r10));
03797 IkReal x876=((cj13)*(r02));
03798 IkReal x877=((r01)*(sj13));
03799 IkReal x878=((r00)*(r11)*(sj13));
03800 gconst25=IKsign(((((x872)*(x878)))+(((IkReal(-1.00000000000000))*(x872)*(x875)*(x877)))+(((IkReal(-1.00000000000000))*(x873)*(x875)*(x877)))+(((IkReal(-1.00000000000000))*(sj14)*(x875)*(x876)))+(((x873)*(x878)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x876)))+(((r00)*(sj14)*(x874)))+(((cj14)*(r01)*(x874)))));
03801 IkReal x879=(sj14)*(sj14);
03802 IkReal x880=(cj14)*(cj14);
03803 IkReal x881=((cj13)*(r12));
03804 IkReal x882=((IkReal(1.00000000000000))*(r10));
03805 IkReal x883=((cj13)*(r02));
03806 IkReal x884=((r01)*(sj13));
03807 IkReal x885=((r00)*(r11)*(sj13));
03808 dummyeval[0]=((((IkReal(-1.00000000000000))*(x880)*(x882)*(x884)))+(((x879)*(x885)))+(((IkReal(-1.00000000000000))*(sj14)*(x882)*(x883)))+(((x880)*(x885)))+(((cj14)*(r01)*(x881)))+(((r00)*(sj14)*(x881)))+(((IkReal(-1.00000000000000))*(x879)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x883))));
03809 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03810 {
03811 {
03812 IkReal dummyeval[1];
03813 IkReal gconst26;
03814 IkReal x886=(cj13)*(cj13);
03815 IkReal x887=(sj13)*(sj13);
03816 IkReal x888=((r01)*(sj14));
03817 IkReal x889=((r12)*(x887));
03818 IkReal x890=((IkReal(1.00000000000000))*(cj14)*(r00));
03819 IkReal x891=((cj14)*(r02)*(r10));
03820 IkReal x892=((r12)*(x886));
03821 IkReal x893=((IkReal(1.00000000000000))*(r02)*(r11)*(sj14));
03822 gconst26=IKsign(((((x886)*(x891)))+(((IkReal(-1.00000000000000))*(x889)*(x890)))+(((x888)*(x889)))+(((x888)*(x892)))+(((IkReal(-1.00000000000000))*(x890)*(x892)))+(((x887)*(x891)))+(((IkReal(-1.00000000000000))*(x886)*(x893)))+(((IkReal(-1.00000000000000))*(x887)*(x893)))));
03823 IkReal x894=(cj13)*(cj13);
03824 IkReal x895=(sj13)*(sj13);
03825 IkReal x896=((r01)*(sj14));
03826 IkReal x897=((r12)*(x895));
03827 IkReal x898=((IkReal(1.00000000000000))*(cj14)*(r00));
03828 IkReal x899=((cj14)*(r02)*(r10));
03829 IkReal x900=((r12)*(x894));
03830 IkReal x901=x893;
03831 dummyeval[0]=((((x894)*(x899)))+(((IkReal(-1.00000000000000))*(x895)*(x901)))+(((x896)*(x897)))+(((IkReal(-1.00000000000000))*(x894)*(x901)))+(((x895)*(x899)))+(((IkReal(-1.00000000000000))*(x898)*(x900)))+(((x896)*(x900)))+(((IkReal(-1.00000000000000))*(x897)*(x898))));
03832 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03833 {
03834 continue;
03835
03836 } else
03837 {
03838 {
03839 IkReal j9array[1], cj9array[1], sj9array[1];
03840 bool j9valid[1]={false};
03841 _nj9 = 1;
03842 IkReal x902=((cj13)*(cj14));
03843 IkReal x903=((IkReal(1.00000000000000))*(cj13)*(sj14));
03844 if( IKabs(((gconst26)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x903)))+(((r10)*(x902))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst26)*(((((r00)*(x902)))+(((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x903))))))) < IKFAST_ATAN2_MAGTHRESH )
03845 continue;
03846 j9array[0]=IKatan2(((gconst26)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x903)))+(((r10)*(x902)))))), ((gconst26)*(((((r00)*(x902)))+(((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x903)))))));
03847 sj9array[0]=IKsin(j9array[0]);
03848 cj9array[0]=IKcos(j9array[0]);
03849 if( j9array[0] > IKPI )
03850 {
03851 j9array[0]-=IK2PI;
03852 }
03853 else if( j9array[0] < -IKPI )
03854 { j9array[0]+=IK2PI;
03855 }
03856 j9valid[0] = true;
03857 for(int ij9 = 0; ij9 < 1; ++ij9)
03858 {
03859 if( !j9valid[ij9] )
03860 {
03861 continue;
03862 }
03863 _ij9[0] = ij9; _ij9[1] = -1;
03864 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
03865 {
03866 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
03867 {
03868 j9valid[iij9]=false; _ij9[1] = iij9; break;
03869 }
03870 }
03871 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
03872 {
03873 IkReal evalcond[4];
03874 IkReal x904=IKsin(j9);
03875 IkReal x905=IKcos(j9);
03876 IkReal x906=((IkReal(1.00000000000000))*(sj14));
03877 IkReal x907=((cj13)*(cj14));
03878 IkReal x908=((IkReal(1.00000000000000))*(r00));
03879 IkReal x909=((r01)*(sj14));
03880 IkReal x910=((IkReal(1.00000000000000))*(cj13));
03881 IkReal x911=((sj13)*(x905));
03882 IkReal x912=((r02)*(x904));
03883 IkReal x913=((r10)*(x905));
03884 IkReal x914=((r11)*(x905));
03885 IkReal x915=((sj13)*(x904));
03886 IkReal x916=((cj14)*(x915));
03887 evalcond[0]=((((IkReal(-1.00000000000000))*(x906)*(x913)))+(((cj14)*(r01)*(x904)))+(((r00)*(sj14)*(x904)))+(((IkReal(-1.00000000000000))*(cj14)*(x914))));
03888 evalcond[1]=((((IkReal(-1.00000000000000))*(sj13)*(x912)))+(((cj13)*(x904)*(x909)))+(((IkReal(-1.00000000000000))*(x904)*(x907)*(x908)))+(((r12)*(x911)))+(((x907)*(x913)))+(((IkReal(-1.00000000000000))*(cj13)*(x906)*(x914))));
03889 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x906)*(x911)))+(((x909)*(x915)))+(((IkReal(-1.00000000000000))*(x908)*(x916)))+(((IkReal(-1.00000000000000))*(r12)*(x905)*(x910)))+(((cj14)*(r10)*(x911)))+(((cj13)*(x912))));
03890 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x906)*(x915)))+(((cj14)*(r00)*(x911)))+(((IkReal(-1.00000000000000))*(r01)*(x906)*(x911)))+(((r10)*(x916)))+(((IkReal(-1.00000000000000))*(r12)*(x904)*(x910)))+(((IkReal(-1.00000000000000))*(r02)*(x905)*(x910))));
03891 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03892 {
03893 continue;
03894 }
03895 }
03896
03897 {
03898 IkReal dummyeval[1];
03899 IkReal gconst28;
03900 gconst28=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
03901 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
03902 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03903 {
03904 {
03905 IkReal dummyeval[1];
03906 IkReal gconst29;
03907 gconst29=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
03908 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
03909 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
03910 {
03911 continue;
03912
03913 } else
03914 {
03915 {
03916 IkReal j10array[1], cj10array[1], sj10array[1];
03917 bool j10valid[1]={false};
03918 _nj10 = 1;
03919 IkReal x917=((IkReal(1.00000000000000))*(sj11));
03920 IkReal x918=((cj14)*(r21));
03921 IkReal x919=((IkReal(1.00000000000000))*(cj11));
03922 IkReal x920=((r20)*(sj14));
03923 IkReal x921=((cj9)*(r00)*(sj14));
03924 IkReal x922=((cj14)*(r11)*(sj9));
03925 IkReal x923=((cj14)*(cj9)*(r01));
03926 IkReal x924=((r10)*(sj14)*(sj9));
03927 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(x918)*(x919)))+(((IkReal(-1.00000000000000))*(x919)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x924)))+(((IkReal(-1.00000000000000))*(x917)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x923)))+(((IkReal(-1.00000000000000))*(x917)*(x922))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((cj11)*(x924)))+(((cj11)*(x922)))+(((cj11)*(x923)))+(((cj11)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x918))))))) < IKFAST_ATAN2_MAGTHRESH )
03928 continue;
03929 j10array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(x918)*(x919)))+(((IkReal(-1.00000000000000))*(x919)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x924)))+(((IkReal(-1.00000000000000))*(x917)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x923)))+(((IkReal(-1.00000000000000))*(x917)*(x922)))))), ((gconst29)*(((((cj11)*(x924)))+(((cj11)*(x922)))+(((cj11)*(x923)))+(((cj11)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x918)))))));
03930 sj10array[0]=IKsin(j10array[0]);
03931 cj10array[0]=IKcos(j10array[0]);
03932 if( j10array[0] > IKPI )
03933 {
03934 j10array[0]-=IK2PI;
03935 }
03936 else if( j10array[0] < -IKPI )
03937 { j10array[0]+=IK2PI;
03938 }
03939 j10valid[0] = true;
03940 for(int ij10 = 0; ij10 < 1; ++ij10)
03941 {
03942 if( !j10valid[ij10] )
03943 {
03944 continue;
03945 }
03946 _ij10[0] = ij10; _ij10[1] = -1;
03947 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
03948 {
03949 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
03950 {
03951 j10valid[iij10]=false; _ij10[1] = iij10; break;
03952 }
03953 }
03954 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
03955 {
03956 IkReal evalcond[4];
03957 IkReal x925=IKsin(j10);
03958 IkReal x926=IKcos(j10);
03959 IkReal x927=((cj14)*(sj9));
03960 IkReal x928=((IkReal(1.00000000000000))*(r11));
03961 IkReal x929=((cj13)*(sj14));
03962 IkReal x930=((IkReal(1.00000000000000))*(cj9));
03963 IkReal x931=((cj13)*(cj14));
03964 IkReal x932=((cj11)*(x925));
03965 IkReal x933=((sj11)*(x926));
03966 IkReal x934=((cj11)*(x926));
03967 IkReal x935=((sj11)*(x925));
03968 IkReal x936=((x933)+(x932));
03969 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x936));
03970 evalcond[1]=((((r21)*(x929)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x931)))+(((IkReal(-1.00000000000000))*(x934)))+(x935));
03971 evalcond[2]=((((IkReal(-1.00000000000000))*(x927)*(x928)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x935)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x930)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x930)))+(x934));
03972 evalcond[3]=((((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x929)*(x930)))+(((cj9)*(r00)*(x931)))+(((IkReal(-1.00000000000000))*(sj9)*(x928)*(x929)))+(((cj13)*(r10)*(x927)))+(x936)+(((r12)*(sj13)*(sj9))));
03973 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
03974 {
03975 continue;
03976 }
03977 }
03978
03979 {
03980 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
03981 vinfos[0].jointtype = 1;
03982 vinfos[0].foffset = j9;
03983 vinfos[0].indices[0] = _ij9[0];
03984 vinfos[0].indices[1] = _ij9[1];
03985 vinfos[0].maxsolutions = _nj9;
03986 vinfos[1].jointtype = 1;
03987 vinfos[1].foffset = j10;
03988 vinfos[1].indices[0] = _ij10[0];
03989 vinfos[1].indices[1] = _ij10[1];
03990 vinfos[1].maxsolutions = _nj10;
03991 vinfos[2].jointtype = 1;
03992 vinfos[2].foffset = j11;
03993 vinfos[2].indices[0] = _ij11[0];
03994 vinfos[2].indices[1] = _ij11[1];
03995 vinfos[2].maxsolutions = _nj11;
03996 vinfos[3].jointtype = 1;
03997 vinfos[3].foffset = j12;
03998 vinfos[3].indices[0] = _ij12[0];
03999 vinfos[3].indices[1] = _ij12[1];
04000 vinfos[3].maxsolutions = _nj12;
04001 vinfos[4].jointtype = 1;
04002 vinfos[4].foffset = j13;
04003 vinfos[4].indices[0] = _ij13[0];
04004 vinfos[4].indices[1] = _ij13[1];
04005 vinfos[4].maxsolutions = _nj13;
04006 vinfos[5].jointtype = 1;
04007 vinfos[5].foffset = j14;
04008 vinfos[5].indices[0] = _ij14[0];
04009 vinfos[5].indices[1] = _ij14[1];
04010 vinfos[5].maxsolutions = _nj14;
04011 std::vector<int> vfree(0);
04012 solutions.AddSolution(vinfos,vfree);
04013 }
04014 }
04015 }
04016
04017 }
04018
04019 }
04020
04021 } else
04022 {
04023 {
04024 IkReal j10array[1], cj10array[1], sj10array[1];
04025 bool j10valid[1]={false};
04026 _nj10 = 1;
04027 IkReal x937=((cj13)*(sj11));
04028 IkReal x938=((r21)*(sj14));
04029 IkReal x939=((cj14)*(r20));
04030 IkReal x940=((cj11)*(cj13));
04031 IkReal x941=((r22)*(sj13));
04032 IkReal x942=((r20)*(sj14));
04033 IkReal x943=((cj14)*(r21));
04034 if( IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(sj11)*(x941)))+(((x937)*(x938)))+(((cj11)*(x942)))+(((cj11)*(x943)))+(((IkReal(-1.00000000000000))*(x937)*(x939))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((cj11)*(x941)))+(((x939)*(x940)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((sj11)*(x943)))+(((sj11)*(x942))))))) < IKFAST_ATAN2_MAGTHRESH )
04035 continue;
04036 j10array[0]=IKatan2(((gconst28)*(((((IkReal(-1.00000000000000))*(sj11)*(x941)))+(((x937)*(x938)))+(((cj11)*(x942)))+(((cj11)*(x943)))+(((IkReal(-1.00000000000000))*(x937)*(x939)))))), ((gconst28)*(((((cj11)*(x941)))+(((x939)*(x940)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((sj11)*(x943)))+(((sj11)*(x942)))))));
04037 sj10array[0]=IKsin(j10array[0]);
04038 cj10array[0]=IKcos(j10array[0]);
04039 if( j10array[0] > IKPI )
04040 {
04041 j10array[0]-=IK2PI;
04042 }
04043 else if( j10array[0] < -IKPI )
04044 { j10array[0]+=IK2PI;
04045 }
04046 j10valid[0] = true;
04047 for(int ij10 = 0; ij10 < 1; ++ij10)
04048 {
04049 if( !j10valid[ij10] )
04050 {
04051 continue;
04052 }
04053 _ij10[0] = ij10; _ij10[1] = -1;
04054 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
04055 {
04056 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
04057 {
04058 j10valid[iij10]=false; _ij10[1] = iij10; break;
04059 }
04060 }
04061 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
04062 {
04063 IkReal evalcond[4];
04064 IkReal x944=IKsin(j10);
04065 IkReal x945=IKcos(j10);
04066 IkReal x946=((cj14)*(sj9));
04067 IkReal x947=((IkReal(1.00000000000000))*(r11));
04068 IkReal x948=((cj13)*(sj14));
04069 IkReal x949=((IkReal(1.00000000000000))*(cj9));
04070 IkReal x950=((cj13)*(cj14));
04071 IkReal x951=((cj11)*(x944));
04072 IkReal x952=((sj11)*(x945));
04073 IkReal x953=((cj11)*(x945));
04074 IkReal x954=((sj11)*(x944));
04075 IkReal x955=((x952)+(x951));
04076 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x955));
04077 evalcond[1]=((((r21)*(x948)))+(((IkReal(-1.00000000000000))*(x953)))+(((IkReal(-1.00000000000000))*(r20)*(x950)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(x954));
04078 evalcond[2]=((((IkReal(-1.00000000000000))*(x946)*(x947)))+(((IkReal(-1.00000000000000))*(x954)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x949)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x949)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(x953));
04079 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x948)*(x949)))+(((cj13)*(r10)*(x946)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x947)*(x948)))+(((cj9)*(r00)*(x950)))+(x955)+(((r12)*(sj13)*(sj9))));
04080 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04081 {
04082 continue;
04083 }
04084 }
04085
04086 {
04087 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04088 vinfos[0].jointtype = 1;
04089 vinfos[0].foffset = j9;
04090 vinfos[0].indices[0] = _ij9[0];
04091 vinfos[0].indices[1] = _ij9[1];
04092 vinfos[0].maxsolutions = _nj9;
04093 vinfos[1].jointtype = 1;
04094 vinfos[1].foffset = j10;
04095 vinfos[1].indices[0] = _ij10[0];
04096 vinfos[1].indices[1] = _ij10[1];
04097 vinfos[1].maxsolutions = _nj10;
04098 vinfos[2].jointtype = 1;
04099 vinfos[2].foffset = j11;
04100 vinfos[2].indices[0] = _ij11[0];
04101 vinfos[2].indices[1] = _ij11[1];
04102 vinfos[2].maxsolutions = _nj11;
04103 vinfos[3].jointtype = 1;
04104 vinfos[3].foffset = j12;
04105 vinfos[3].indices[0] = _ij12[0];
04106 vinfos[3].indices[1] = _ij12[1];
04107 vinfos[3].maxsolutions = _nj12;
04108 vinfos[4].jointtype = 1;
04109 vinfos[4].foffset = j13;
04110 vinfos[4].indices[0] = _ij13[0];
04111 vinfos[4].indices[1] = _ij13[1];
04112 vinfos[4].maxsolutions = _nj13;
04113 vinfos[5].jointtype = 1;
04114 vinfos[5].foffset = j14;
04115 vinfos[5].indices[0] = _ij14[0];
04116 vinfos[5].indices[1] = _ij14[1];
04117 vinfos[5].maxsolutions = _nj14;
04118 std::vector<int> vfree(0);
04119 solutions.AddSolution(vinfos,vfree);
04120 }
04121 }
04122 }
04123
04124 }
04125
04126 }
04127 }
04128 }
04129
04130 }
04131
04132 }
04133
04134 } else
04135 {
04136 {
04137 IkReal j9array[1], cj9array[1], sj9array[1];
04138 bool j9valid[1]={false};
04139 _nj9 = 1;
04140 IkReal x956=((IkReal(1.00000000000000))*(sj14));
04141 IkReal x957=((IkReal(1.00000000000000))*(cj14));
04142 if( IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r10)*(x956)))+(((IkReal(-1.00000000000000))*(r11)*(x957))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x957)))+(((IkReal(-1.00000000000000))*(r00)*(x956))))))) < IKFAST_ATAN2_MAGTHRESH )
04143 continue;
04144 j9array[0]=IKatan2(((gconst25)*(((((IkReal(-1.00000000000000))*(r10)*(x956)))+(((IkReal(-1.00000000000000))*(r11)*(x957)))))), ((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x957)))+(((IkReal(-1.00000000000000))*(r00)*(x956)))))));
04145 sj9array[0]=IKsin(j9array[0]);
04146 cj9array[0]=IKcos(j9array[0]);
04147 if( j9array[0] > IKPI )
04148 {
04149 j9array[0]-=IK2PI;
04150 }
04151 else if( j9array[0] < -IKPI )
04152 { j9array[0]+=IK2PI;
04153 }
04154 j9valid[0] = true;
04155 for(int ij9 = 0; ij9 < 1; ++ij9)
04156 {
04157 if( !j9valid[ij9] )
04158 {
04159 continue;
04160 }
04161 _ij9[0] = ij9; _ij9[1] = -1;
04162 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
04163 {
04164 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
04165 {
04166 j9valid[iij9]=false; _ij9[1] = iij9; break;
04167 }
04168 }
04169 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
04170 {
04171 IkReal evalcond[4];
04172 IkReal x958=IKsin(j9);
04173 IkReal x959=IKcos(j9);
04174 IkReal x960=((IkReal(1.00000000000000))*(sj14));
04175 IkReal x961=((cj13)*(cj14));
04176 IkReal x962=((IkReal(1.00000000000000))*(r00));
04177 IkReal x963=((r01)*(sj14));
04178 IkReal x964=((IkReal(1.00000000000000))*(cj13));
04179 IkReal x965=((sj13)*(x959));
04180 IkReal x966=((r02)*(x958));
04181 IkReal x967=((r10)*(x959));
04182 IkReal x968=((r11)*(x959));
04183 IkReal x969=((sj13)*(x958));
04184 IkReal x970=((cj14)*(x969));
04185 evalcond[0]=((((cj14)*(r01)*(x958)))+(((IkReal(-1.00000000000000))*(cj14)*(x968)))+(((IkReal(-1.00000000000000))*(x960)*(x967)))+(((r00)*(sj14)*(x958))));
04186 evalcond[1]=((((cj13)*(x958)*(x963)))+(((x961)*(x967)))+(((IkReal(-1.00000000000000))*(x958)*(x961)*(x962)))+(((r12)*(x965)))+(((IkReal(-1.00000000000000))*(cj13)*(x960)*(x968)))+(((IkReal(-1.00000000000000))*(sj13)*(x966))));
04187 evalcond[2]=((IkReal(-1.00000000000000))+(((cj14)*(r10)*(x965)))+(((IkReal(-1.00000000000000))*(x962)*(x970)))+(((IkReal(-1.00000000000000))*(r11)*(x960)*(x965)))+(((x963)*(x969)))+(((cj13)*(x966)))+(((IkReal(-1.00000000000000))*(r12)*(x959)*(x964))));
04188 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x960)*(x969)))+(((IkReal(-1.00000000000000))*(r01)*(x960)*(x965)))+(((IkReal(-1.00000000000000))*(r12)*(x958)*(x964)))+(((IkReal(-1.00000000000000))*(r02)*(x959)*(x964)))+(((r10)*(x970)))+(((cj14)*(r00)*(x965))));
04189 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04190 {
04191 continue;
04192 }
04193 }
04194
04195 {
04196 IkReal dummyeval[1];
04197 IkReal gconst28;
04198 gconst28=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
04199 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
04200 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04201 {
04202 {
04203 IkReal dummyeval[1];
04204 IkReal gconst29;
04205 gconst29=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
04206 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
04207 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04208 {
04209 continue;
04210
04211 } else
04212 {
04213 {
04214 IkReal j10array[1], cj10array[1], sj10array[1];
04215 bool j10valid[1]={false};
04216 _nj10 = 1;
04217 IkReal x971=((IkReal(1.00000000000000))*(sj11));
04218 IkReal x972=((cj14)*(r21));
04219 IkReal x973=((IkReal(1.00000000000000))*(cj11));
04220 IkReal x974=((r20)*(sj14));
04221 IkReal x975=((cj9)*(r00)*(sj14));
04222 IkReal x976=((cj14)*(r11)*(sj9));
04223 IkReal x977=((cj14)*(cj9)*(r01));
04224 IkReal x978=((r10)*(sj14)*(sj9));
04225 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(x971)*(x976)))+(((IkReal(-1.00000000000000))*(x971)*(x977)))+(((IkReal(-1.00000000000000))*(x971)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x975)))+(((IkReal(-1.00000000000000))*(x972)*(x973)))+(((IkReal(-1.00000000000000))*(x973)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((cj11)*(x975)))+(((cj11)*(x977)))+(((cj11)*(x976)))+(((cj11)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x972)))+(((IkReal(-1.00000000000000))*(x971)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH )
04226 continue;
04227 j10array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(x971)*(x976)))+(((IkReal(-1.00000000000000))*(x971)*(x977)))+(((IkReal(-1.00000000000000))*(x971)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x975)))+(((IkReal(-1.00000000000000))*(x972)*(x973)))+(((IkReal(-1.00000000000000))*(x973)*(x974)))))), ((gconst29)*(((((cj11)*(x975)))+(((cj11)*(x977)))+(((cj11)*(x976)))+(((cj11)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x972)))+(((IkReal(-1.00000000000000))*(x971)*(x974)))))));
04228 sj10array[0]=IKsin(j10array[0]);
04229 cj10array[0]=IKcos(j10array[0]);
04230 if( j10array[0] > IKPI )
04231 {
04232 j10array[0]-=IK2PI;
04233 }
04234 else if( j10array[0] < -IKPI )
04235 { j10array[0]+=IK2PI;
04236 }
04237 j10valid[0] = true;
04238 for(int ij10 = 0; ij10 < 1; ++ij10)
04239 {
04240 if( !j10valid[ij10] )
04241 {
04242 continue;
04243 }
04244 _ij10[0] = ij10; _ij10[1] = -1;
04245 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
04246 {
04247 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
04248 {
04249 j10valid[iij10]=false; _ij10[1] = iij10; break;
04250 }
04251 }
04252 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
04253 {
04254 IkReal evalcond[4];
04255 IkReal x979=IKsin(j10);
04256 IkReal x980=IKcos(j10);
04257 IkReal x981=((cj14)*(sj9));
04258 IkReal x982=((IkReal(1.00000000000000))*(r11));
04259 IkReal x983=((cj13)*(sj14));
04260 IkReal x984=((IkReal(1.00000000000000))*(cj9));
04261 IkReal x985=((cj13)*(cj14));
04262 IkReal x986=((cj11)*(x979));
04263 IkReal x987=((sj11)*(x980));
04264 IkReal x988=((cj11)*(x980));
04265 IkReal x989=((sj11)*(x979));
04266 IkReal x990=((x986)+(x987));
04267 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x990));
04268 evalcond[1]=((((IkReal(-1.00000000000000))*(x988)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x983)))+(((IkReal(-1.00000000000000))*(r20)*(x985)))+(x989));
04269 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x984)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x984)))+(((IkReal(-1.00000000000000))*(x989)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x981)*(x982)))+(x988));
04270 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x982)*(x983)))+(((cj9)*(r02)*(sj13)))+(((cj13)*(r10)*(x981)))+(((cj9)*(r00)*(x985)))+(x990)+(((IkReal(-1.00000000000000))*(r01)*(x983)*(x984)))+(((r12)*(sj13)*(sj9))));
04271 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04272 {
04273 continue;
04274 }
04275 }
04276
04277 {
04278 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04279 vinfos[0].jointtype = 1;
04280 vinfos[0].foffset = j9;
04281 vinfos[0].indices[0] = _ij9[0];
04282 vinfos[0].indices[1] = _ij9[1];
04283 vinfos[0].maxsolutions = _nj9;
04284 vinfos[1].jointtype = 1;
04285 vinfos[1].foffset = j10;
04286 vinfos[1].indices[0] = _ij10[0];
04287 vinfos[1].indices[1] = _ij10[1];
04288 vinfos[1].maxsolutions = _nj10;
04289 vinfos[2].jointtype = 1;
04290 vinfos[2].foffset = j11;
04291 vinfos[2].indices[0] = _ij11[0];
04292 vinfos[2].indices[1] = _ij11[1];
04293 vinfos[2].maxsolutions = _nj11;
04294 vinfos[3].jointtype = 1;
04295 vinfos[3].foffset = j12;
04296 vinfos[3].indices[0] = _ij12[0];
04297 vinfos[3].indices[1] = _ij12[1];
04298 vinfos[3].maxsolutions = _nj12;
04299 vinfos[4].jointtype = 1;
04300 vinfos[4].foffset = j13;
04301 vinfos[4].indices[0] = _ij13[0];
04302 vinfos[4].indices[1] = _ij13[1];
04303 vinfos[4].maxsolutions = _nj13;
04304 vinfos[5].jointtype = 1;
04305 vinfos[5].foffset = j14;
04306 vinfos[5].indices[0] = _ij14[0];
04307 vinfos[5].indices[1] = _ij14[1];
04308 vinfos[5].maxsolutions = _nj14;
04309 std::vector<int> vfree(0);
04310 solutions.AddSolution(vinfos,vfree);
04311 }
04312 }
04313 }
04314
04315 }
04316
04317 }
04318
04319 } else
04320 {
04321 {
04322 IkReal j10array[1], cj10array[1], sj10array[1];
04323 bool j10valid[1]={false};
04324 _nj10 = 1;
04325 IkReal x991=((cj13)*(sj11));
04326 IkReal x992=((r21)*(sj14));
04327 IkReal x993=((cj14)*(r20));
04328 IkReal x994=((cj11)*(cj13));
04329 IkReal x995=((r22)*(sj13));
04330 IkReal x996=((r20)*(sj14));
04331 IkReal x997=((cj14)*(r21));
04332 if( IKabs(((gconst28)*(((((x991)*(x992)))+(((IkReal(-1.00000000000000))*(x991)*(x993)))+(((cj11)*(x997)))+(((cj11)*(x996)))+(((IkReal(-1.00000000000000))*(sj11)*(x995))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((x993)*(x994)))+(((IkReal(-1.00000000000000))*(x992)*(x994)))+(((sj11)*(x996)))+(((sj11)*(x997)))+(((cj11)*(x995))))))) < IKFAST_ATAN2_MAGTHRESH )
04333 continue;
04334 j10array[0]=IKatan2(((gconst28)*(((((x991)*(x992)))+(((IkReal(-1.00000000000000))*(x991)*(x993)))+(((cj11)*(x997)))+(((cj11)*(x996)))+(((IkReal(-1.00000000000000))*(sj11)*(x995)))))), ((gconst28)*(((((x993)*(x994)))+(((IkReal(-1.00000000000000))*(x992)*(x994)))+(((sj11)*(x996)))+(((sj11)*(x997)))+(((cj11)*(x995)))))));
04335 sj10array[0]=IKsin(j10array[0]);
04336 cj10array[0]=IKcos(j10array[0]);
04337 if( j10array[0] > IKPI )
04338 {
04339 j10array[0]-=IK2PI;
04340 }
04341 else if( j10array[0] < -IKPI )
04342 { j10array[0]+=IK2PI;
04343 }
04344 j10valid[0] = true;
04345 for(int ij10 = 0; ij10 < 1; ++ij10)
04346 {
04347 if( !j10valid[ij10] )
04348 {
04349 continue;
04350 }
04351 _ij10[0] = ij10; _ij10[1] = -1;
04352 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
04353 {
04354 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
04355 {
04356 j10valid[iij10]=false; _ij10[1] = iij10; break;
04357 }
04358 }
04359 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
04360 {
04361 IkReal evalcond[4];
04362 IkReal x998=IKsin(j10);
04363 IkReal x999=IKcos(j10);
04364 IkReal x1000=((cj14)*(sj9));
04365 IkReal x1001=((IkReal(1.00000000000000))*(r11));
04366 IkReal x1002=((cj13)*(sj14));
04367 IkReal x1003=((IkReal(1.00000000000000))*(cj9));
04368 IkReal x1004=((cj13)*(cj14));
04369 IkReal x1005=((cj11)*(x998));
04370 IkReal x1006=((sj11)*(x999));
04371 IkReal x1007=((cj11)*(x999));
04372 IkReal x1008=((sj11)*(x998));
04373 IkReal x1009=((x1006)+(x1005));
04374 evalcond[0]=((x1009)+(((cj14)*(r21)))+(((r20)*(sj14))));
04375 evalcond[1]=((((IkReal(-1.00000000000000))*(x1007)))+(x1008)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x1004)))+(((r21)*(x1002))));
04376 evalcond[2]=((((IkReal(-1.00000000000000))*(x1008)))+(x1007)+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1003)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1003)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001))));
04377 evalcond[3]=((x1009)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x1001)*(x1002)))+(((cj13)*(r10)*(x1000)))+(((IkReal(-1.00000000000000))*(r01)*(x1002)*(x1003)))+(((cj9)*(r00)*(x1004)))+(((r12)*(sj13)*(sj9))));
04378 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04379 {
04380 continue;
04381 }
04382 }
04383
04384 {
04385 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04386 vinfos[0].jointtype = 1;
04387 vinfos[0].foffset = j9;
04388 vinfos[0].indices[0] = _ij9[0];
04389 vinfos[0].indices[1] = _ij9[1];
04390 vinfos[0].maxsolutions = _nj9;
04391 vinfos[1].jointtype = 1;
04392 vinfos[1].foffset = j10;
04393 vinfos[1].indices[0] = _ij10[0];
04394 vinfos[1].indices[1] = _ij10[1];
04395 vinfos[1].maxsolutions = _nj10;
04396 vinfos[2].jointtype = 1;
04397 vinfos[2].foffset = j11;
04398 vinfos[2].indices[0] = _ij11[0];
04399 vinfos[2].indices[1] = _ij11[1];
04400 vinfos[2].maxsolutions = _nj11;
04401 vinfos[3].jointtype = 1;
04402 vinfos[3].foffset = j12;
04403 vinfos[3].indices[0] = _ij12[0];
04404 vinfos[3].indices[1] = _ij12[1];
04405 vinfos[3].maxsolutions = _nj12;
04406 vinfos[4].jointtype = 1;
04407 vinfos[4].foffset = j13;
04408 vinfos[4].indices[0] = _ij13[0];
04409 vinfos[4].indices[1] = _ij13[1];
04410 vinfos[4].maxsolutions = _nj13;
04411 vinfos[5].jointtype = 1;
04412 vinfos[5].foffset = j14;
04413 vinfos[5].indices[0] = _ij14[0];
04414 vinfos[5].indices[1] = _ij14[1];
04415 vinfos[5].maxsolutions = _nj14;
04416 std::vector<int> vfree(0);
04417 solutions.AddSolution(vinfos,vfree);
04418 }
04419 }
04420 }
04421
04422 }
04423
04424 }
04425 }
04426 }
04427
04428 }
04429
04430 }
04431
04432 } else
04433 {
04434 {
04435 IkReal j10array[1], cj10array[1], sj10array[1];
04436 bool j10valid[1]={false};
04437 _nj10 = 1;
04438 IkReal x1010=((cj13)*(sj11));
04439 IkReal x1011=((r21)*(sj14));
04440 IkReal x1012=((cj14)*(r20));
04441 IkReal x1013=((cj11)*(cj13));
04442 IkReal x1014=((r22)*(sj13));
04443 IkReal x1015=((r20)*(sj14));
04444 IkReal x1016=((cj14)*(r21));
04445 if( IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(x1010)*(x1012)))+(((IkReal(-1.00000000000000))*(sj11)*(x1014)))+(((cj11)*(x1015)))+(((cj11)*(x1016)))+(((x1010)*(x1011))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(x1011)*(x1013)))+(((x1012)*(x1013)))+(((sj11)*(x1016)))+(((sj11)*(x1015)))+(((cj11)*(x1014))))))) < IKFAST_ATAN2_MAGTHRESH )
04446 continue;
04447 j10array[0]=IKatan2(((gconst27)*(((((IkReal(-1.00000000000000))*(x1010)*(x1012)))+(((IkReal(-1.00000000000000))*(sj11)*(x1014)))+(((cj11)*(x1015)))+(((cj11)*(x1016)))+(((x1010)*(x1011)))))), ((gconst27)*(((((IkReal(-1.00000000000000))*(x1011)*(x1013)))+(((x1012)*(x1013)))+(((sj11)*(x1016)))+(((sj11)*(x1015)))+(((cj11)*(x1014)))))));
04448 sj10array[0]=IKsin(j10array[0]);
04449 cj10array[0]=IKcos(j10array[0]);
04450 if( j10array[0] > IKPI )
04451 {
04452 j10array[0]-=IK2PI;
04453 }
04454 else if( j10array[0] < -IKPI )
04455 { j10array[0]+=IK2PI;
04456 }
04457 j10valid[0] = true;
04458 for(int ij10 = 0; ij10 < 1; ++ij10)
04459 {
04460 if( !j10valid[ij10] )
04461 {
04462 continue;
04463 }
04464 _ij10[0] = ij10; _ij10[1] = -1;
04465 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
04466 {
04467 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
04468 {
04469 j10valid[iij10]=false; _ij10[1] = iij10; break;
04470 }
04471 }
04472 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
04473 {
04474 IkReal evalcond[2];
04475 IkReal x1017=IKsin(j10);
04476 IkReal x1018=IKcos(j10);
04477 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((sj11)*(x1018)))+(((cj11)*(x1017))));
04478 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)))+(((cj13)*(r21)*(sj14)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((sj11)*(x1017)))+(((IkReal(-1.00000000000000))*(cj11)*(x1018))));
04479 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
04480 {
04481 continue;
04482 }
04483 }
04484
04485 {
04486 IkReal dummyeval[1];
04487 IkReal gconst31;
04488 IkReal x1019=(sj14)*(sj14);
04489 IkReal x1020=(cj14)*(cj14);
04490 IkReal x1021=((cj13)*(r12));
04491 IkReal x1022=((IkReal(1.00000000000000))*(r10));
04492 IkReal x1023=((cj13)*(r02));
04493 IkReal x1024=((r01)*(sj13));
04494 IkReal x1025=((r00)*(r11)*(sj13));
04495 gconst31=IKsign(((((IkReal(-1.00000000000000))*(sj14)*(x1022)*(x1023)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1023)))+(((x1019)*(x1025)))+(((r00)*(sj14)*(x1021)))+(((IkReal(-1.00000000000000))*(x1019)*(x1022)*(x1024)))+(((x1020)*(x1025)))+(((cj14)*(r01)*(x1021)))+(((IkReal(-1.00000000000000))*(x1020)*(x1022)*(x1024)))));
04496 IkReal x1026=(sj14)*(sj14);
04497 IkReal x1027=(cj14)*(cj14);
04498 IkReal x1028=((cj13)*(r12));
04499 IkReal x1029=((IkReal(1.00000000000000))*(r10));
04500 IkReal x1030=((cj13)*(r02));
04501 IkReal x1031=((r01)*(sj13));
04502 IkReal x1032=((r00)*(r11)*(sj13));
04503 dummyeval[0]=((((x1026)*(x1032)))+(((IkReal(-1.00000000000000))*(x1026)*(x1029)*(x1031)))+(((IkReal(-1.00000000000000))*(sj14)*(x1029)*(x1030)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1030)))+(((IkReal(-1.00000000000000))*(x1027)*(x1029)*(x1031)))+(((r00)*(sj14)*(x1028)))+(((x1027)*(x1032)))+(((cj14)*(r01)*(x1028))));
04504 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04505 {
04506 {
04507 IkReal dummyeval[1];
04508 IkReal gconst30;
04509 IkReal x1033=(sj14)*(sj14);
04510 IkReal x1034=(cj14)*(cj14);
04511 IkReal x1035=((IkReal(1.00000000000000))*(x1033));
04512 IkReal x1036=((IkReal(2.00000000000000))*(cj14)*(sj14));
04513 IkReal x1037=((IkReal(1.00000000000000))*(x1034));
04514 gconst30=IKsign(((((IkReal(-1.00000000000000))*(x1037)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1037)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1036)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1036)))+(((IkReal(-1.00000000000000))*(x1035)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1035)*((r00)*(r00))))));
04515 IkReal x1038=(sj14)*(sj14);
04516 IkReal x1039=(cj14)*(cj14);
04517 IkReal x1040=((IkReal(1.00000000000000))*(x1038));
04518 IkReal x1041=((IkReal(2.00000000000000))*(cj14)*(sj14));
04519 IkReal x1042=((IkReal(1.00000000000000))*(x1039));
04520 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1041)))+(((IkReal(-1.00000000000000))*(x1042)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1040)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1040)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1041)))+(((IkReal(-1.00000000000000))*(x1042)*((r11)*(r11)))));
04521 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04522 {
04523 continue;
04524
04525 } else
04526 {
04527 {
04528 IkReal j9array[1], cj9array[1], sj9array[1];
04529 bool j9valid[1]={false};
04530 _nj9 = 1;
04531 IkReal x1043=((sj10)*(sj11));
04532 IkReal x1044=((r00)*(sj14));
04533 IkReal x1045=((cj14)*(r11));
04534 IkReal x1046=((r10)*(sj14));
04535 IkReal x1047=((cj14)*(r01));
04536 IkReal x1048=((IkReal(1.00000000000000))*(cj10)*(cj11));
04537 if( IKabs(((gconst30)*(((((IkReal(-1.00000000000000))*(x1046)*(x1048)))+(((x1043)*(x1045)))+(((x1043)*(x1046)))+(((IkReal(-1.00000000000000))*(x1045)*(x1048))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst30)*(((((x1043)*(x1044)))+(((x1043)*(x1047)))+(((IkReal(-1.00000000000000))*(x1044)*(x1048)))+(((IkReal(-1.00000000000000))*(x1047)*(x1048))))))) < IKFAST_ATAN2_MAGTHRESH )
04538 continue;
04539 j9array[0]=IKatan2(((gconst30)*(((((IkReal(-1.00000000000000))*(x1046)*(x1048)))+(((x1043)*(x1045)))+(((x1043)*(x1046)))+(((IkReal(-1.00000000000000))*(x1045)*(x1048)))))), ((gconst30)*(((((x1043)*(x1044)))+(((x1043)*(x1047)))+(((IkReal(-1.00000000000000))*(x1044)*(x1048)))+(((IkReal(-1.00000000000000))*(x1047)*(x1048)))))));
04540 sj9array[0]=IKsin(j9array[0]);
04541 cj9array[0]=IKcos(j9array[0]);
04542 if( j9array[0] > IKPI )
04543 {
04544 j9array[0]-=IK2PI;
04545 }
04546 else if( j9array[0] < -IKPI )
04547 { j9array[0]+=IK2PI;
04548 }
04549 j9valid[0] = true;
04550 for(int ij9 = 0; ij9 < 1; ++ij9)
04551 {
04552 if( !j9valid[ij9] )
04553 {
04554 continue;
04555 }
04556 _ij9[0] = ij9; _ij9[1] = -1;
04557 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
04558 {
04559 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
04560 {
04561 j9valid[iij9]=false; _ij9[1] = iij9; break;
04562 }
04563 }
04564 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
04565 {
04566 IkReal evalcond[6];
04567 IkReal x1049=IKsin(j9);
04568 IkReal x1050=IKcos(j9);
04569 IkReal x1051=((IkReal(1.00000000000000))*(cj14));
04570 IkReal x1052=((IkReal(1.00000000000000))*(sj14));
04571 IkReal x1053=((cj13)*(cj14));
04572 IkReal x1054=((cj14)*(r10));
04573 IkReal x1055=((r01)*(sj14));
04574 IkReal x1056=((IkReal(1.00000000000000))*(r12));
04575 IkReal x1057=((sj13)*(x1050));
04576 IkReal x1058=((r02)*(x1049));
04577 IkReal x1059=((r11)*(x1049));
04578 IkReal x1060=((r10)*(x1050));
04579 IkReal x1061=((r01)*(x1050));
04580 IkReal x1062=((sj13)*(x1049));
04581 IkReal x1063=((r11)*(x1050));
04582 IkReal x1064=((cj13)*(x1049));
04583 IkReal x1065=((r10)*(x1049));
04584 IkReal x1066=((r00)*(x1050));
04585 IkReal x1067=((cj13)*(x1050));
04586 evalcond[0]=((((cj14)*(r01)*(x1049)))+(((IkReal(-1.00000000000000))*(x1052)*(x1060)))+(((IkReal(-1.00000000000000))*(x1051)*(x1063)))+(((r00)*(sj14)*(x1049))));
04587 evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1052)*(x1066)))+(((IkReal(-1.00000000000000))*(x1052)*(x1065)))+(((IkReal(-1.00000000000000))*(x1051)*(x1061)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1051)*(x1059))));
04588 evalcond[2]=((((r12)*(x1057)))+(((x1055)*(x1064)))+(((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1063)))+(((IkReal(-1.00000000000000))*(r00)*(x1051)*(x1064)))+(((x1053)*(x1060)))+(((IkReal(-1.00000000000000))*(sj13)*(x1058))));
04589 evalcond[3]=((IkReal(-1.00000000000000))+(((x1054)*(x1057)))+(((x1055)*(x1062)))+(((IkReal(-1.00000000000000))*(r11)*(x1052)*(x1057)))+(((IkReal(-1.00000000000000))*(r00)*(x1051)*(x1062)))+(((IkReal(-1.00000000000000))*(x1056)*(x1067)))+(((cj13)*(x1058))));
04590 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1059)))+(((r12)*(x1062)))+(((r02)*(x1057)))+(((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1061)))+(((cj10)*(sj11)))+(((x1053)*(x1065)))+(((x1053)*(x1066)))+(((cj11)*(sj10))));
04591 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x1052)*(x1059)))+(((x1054)*(x1062)))+(((IkReal(-1.00000000000000))*(r02)*(x1067)))+(((cj14)*(r00)*(x1057)))+(((IkReal(-1.00000000000000))*(x1056)*(x1064)))+(((IkReal(-1.00000000000000))*(r01)*(x1052)*(x1057))));
04592 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 )
04593 {
04594 continue;
04595 }
04596 }
04597
04598 {
04599 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04600 vinfos[0].jointtype = 1;
04601 vinfos[0].foffset = j9;
04602 vinfos[0].indices[0] = _ij9[0];
04603 vinfos[0].indices[1] = _ij9[1];
04604 vinfos[0].maxsolutions = _nj9;
04605 vinfos[1].jointtype = 1;
04606 vinfos[1].foffset = j10;
04607 vinfos[1].indices[0] = _ij10[0];
04608 vinfos[1].indices[1] = _ij10[1];
04609 vinfos[1].maxsolutions = _nj10;
04610 vinfos[2].jointtype = 1;
04611 vinfos[2].foffset = j11;
04612 vinfos[2].indices[0] = _ij11[0];
04613 vinfos[2].indices[1] = _ij11[1];
04614 vinfos[2].maxsolutions = _nj11;
04615 vinfos[3].jointtype = 1;
04616 vinfos[3].foffset = j12;
04617 vinfos[3].indices[0] = _ij12[0];
04618 vinfos[3].indices[1] = _ij12[1];
04619 vinfos[3].maxsolutions = _nj12;
04620 vinfos[4].jointtype = 1;
04621 vinfos[4].foffset = j13;
04622 vinfos[4].indices[0] = _ij13[0];
04623 vinfos[4].indices[1] = _ij13[1];
04624 vinfos[4].maxsolutions = _nj13;
04625 vinfos[5].jointtype = 1;
04626 vinfos[5].foffset = j14;
04627 vinfos[5].indices[0] = _ij14[0];
04628 vinfos[5].indices[1] = _ij14[1];
04629 vinfos[5].maxsolutions = _nj14;
04630 std::vector<int> vfree(0);
04631 solutions.AddSolution(vinfos,vfree);
04632 }
04633 }
04634 }
04635
04636 }
04637
04638 }
04639
04640 } else
04641 {
04642 {
04643 IkReal j9array[1], cj9array[1], sj9array[1];
04644 bool j9valid[1]={false};
04645 _nj9 = 1;
04646 IkReal x1068=((IkReal(1.00000000000000))*(sj14));
04647 IkReal x1069=((IkReal(1.00000000000000))*(cj14));
04648 if( IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1069)))+(((IkReal(-1.00000000000000))*(r10)*(x1068))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r01)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(x1068))))))) < IKFAST_ATAN2_MAGTHRESH )
04649 continue;
04650 j9array[0]=IKatan2(((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1069)))+(((IkReal(-1.00000000000000))*(r10)*(x1068)))))), ((gconst31)*(((((IkReal(-1.00000000000000))*(r01)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(x1068)))))));
04651 sj9array[0]=IKsin(j9array[0]);
04652 cj9array[0]=IKcos(j9array[0]);
04653 if( j9array[0] > IKPI )
04654 {
04655 j9array[0]-=IK2PI;
04656 }
04657 else if( j9array[0] < -IKPI )
04658 { j9array[0]+=IK2PI;
04659 }
04660 j9valid[0] = true;
04661 for(int ij9 = 0; ij9 < 1; ++ij9)
04662 {
04663 if( !j9valid[ij9] )
04664 {
04665 continue;
04666 }
04667 _ij9[0] = ij9; _ij9[1] = -1;
04668 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
04669 {
04670 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
04671 {
04672 j9valid[iij9]=false; _ij9[1] = iij9; break;
04673 }
04674 }
04675 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
04676 {
04677 IkReal evalcond[6];
04678 IkReal x1070=IKsin(j9);
04679 IkReal x1071=IKcos(j9);
04680 IkReal x1072=((IkReal(1.00000000000000))*(cj14));
04681 IkReal x1073=((IkReal(1.00000000000000))*(sj14));
04682 IkReal x1074=((cj13)*(cj14));
04683 IkReal x1075=((cj14)*(r10));
04684 IkReal x1076=((r01)*(sj14));
04685 IkReal x1077=((IkReal(1.00000000000000))*(r12));
04686 IkReal x1078=((sj13)*(x1071));
04687 IkReal x1079=((r02)*(x1070));
04688 IkReal x1080=((r11)*(x1070));
04689 IkReal x1081=((r10)*(x1071));
04690 IkReal x1082=((r01)*(x1071));
04691 IkReal x1083=((sj13)*(x1070));
04692 IkReal x1084=((r11)*(x1071));
04693 IkReal x1085=((cj13)*(x1070));
04694 IkReal x1086=((r10)*(x1070));
04695 IkReal x1087=((r00)*(x1071));
04696 IkReal x1088=((cj13)*(x1071));
04697 evalcond[0]=((((cj14)*(r01)*(x1070)))+(((IkReal(-1.00000000000000))*(x1073)*(x1081)))+(((r00)*(sj14)*(x1070)))+(((IkReal(-1.00000000000000))*(x1072)*(x1084))));
04698 evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1073)*(x1086)))+(((IkReal(-1.00000000000000))*(x1073)*(x1087)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1072)*(x1082)))+(((IkReal(-1.00000000000000))*(x1072)*(x1080))));
04699 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x1079)))+(((x1076)*(x1085)))+(((r12)*(x1078)))+(((IkReal(-1.00000000000000))*(r00)*(x1072)*(x1085)))+(((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1084)))+(((x1074)*(x1081))));
04700 evalcond[3]=((IkReal(-1.00000000000000))+(((x1076)*(x1083)))+(((IkReal(-1.00000000000000))*(r00)*(x1072)*(x1083)))+(((IkReal(-1.00000000000000))*(r11)*(x1073)*(x1078)))+(((x1075)*(x1078)))+(((IkReal(-1.00000000000000))*(x1077)*(x1088)))+(((cj13)*(x1079))));
04701 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1082)))+(((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1080)))+(((r12)*(x1083)))+(((cj10)*(sj11)))+(((x1074)*(x1087)))+(((x1074)*(x1086)))+(((cj11)*(sj10)))+(((r02)*(x1078))));
04702 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x1088)))+(((x1075)*(x1083)))+(((IkReal(-1.00000000000000))*(r01)*(x1073)*(x1078)))+(((IkReal(-1.00000000000000))*(sj13)*(x1073)*(x1080)))+(((cj14)*(r00)*(x1078)))+(((IkReal(-1.00000000000000))*(x1077)*(x1085))));
04703 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 )
04704 {
04705 continue;
04706 }
04707 }
04708
04709 {
04710 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04711 vinfos[0].jointtype = 1;
04712 vinfos[0].foffset = j9;
04713 vinfos[0].indices[0] = _ij9[0];
04714 vinfos[0].indices[1] = _ij9[1];
04715 vinfos[0].maxsolutions = _nj9;
04716 vinfos[1].jointtype = 1;
04717 vinfos[1].foffset = j10;
04718 vinfos[1].indices[0] = _ij10[0];
04719 vinfos[1].indices[1] = _ij10[1];
04720 vinfos[1].maxsolutions = _nj10;
04721 vinfos[2].jointtype = 1;
04722 vinfos[2].foffset = j11;
04723 vinfos[2].indices[0] = _ij11[0];
04724 vinfos[2].indices[1] = _ij11[1];
04725 vinfos[2].maxsolutions = _nj11;
04726 vinfos[3].jointtype = 1;
04727 vinfos[3].foffset = j12;
04728 vinfos[3].indices[0] = _ij12[0];
04729 vinfos[3].indices[1] = _ij12[1];
04730 vinfos[3].maxsolutions = _nj12;
04731 vinfos[4].jointtype = 1;
04732 vinfos[4].foffset = j13;
04733 vinfos[4].indices[0] = _ij13[0];
04734 vinfos[4].indices[1] = _ij13[1];
04735 vinfos[4].maxsolutions = _nj13;
04736 vinfos[5].jointtype = 1;
04737 vinfos[5].foffset = j14;
04738 vinfos[5].indices[0] = _ij14[0];
04739 vinfos[5].indices[1] = _ij14[1];
04740 vinfos[5].maxsolutions = _nj14;
04741 std::vector<int> vfree(0);
04742 solutions.AddSolution(vinfos,vfree);
04743 }
04744 }
04745 }
04746
04747 }
04748
04749 }
04750 }
04751 }
04752
04753 }
04754
04755 }
04756
04757 } else
04758 {
04759 IkReal x1089=((IkReal(1.00000000000000))*(sj13));
04760 IkReal x1090=((cj14)*(npx));
04761 IkReal x1091=((npy)*(sj14));
04762 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
04763 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
04764 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
04765 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1091)))+(((IkReal(-1.00000000000000))*(cj13)*(x1090)))+(((IkReal(-1.00000000000000))*(npz)*(x1089))));
04766 evalcond[4]=((IkReal(0.0300000000000000))+(((sj13)*(x1091)))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x1089)*(x1090)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
04767 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 )
04768 {
04769 {
04770 IkReal dummyeval[1];
04771 IkReal gconst32;
04772 IkReal x1092=(sj14)*(sj14);
04773 IkReal x1093=(cj14)*(cj14);
04774 IkReal x1094=((IkReal(2.00000000000000))*(cj14)*(sj14));
04775 gconst32=IKsign(((((x1092)*((r10)*(r10))))+(((r00)*(r01)*(x1094)))+(((x1093)*((r01)*(r01))))+(((r10)*(r11)*(x1094)))+(((x1092)*((r00)*(r00))))+(((x1093)*((r11)*(r11))))));
04776 IkReal x1095=(sj14)*(sj14);
04777 IkReal x1096=(cj14)*(cj14);
04778 IkReal x1097=((IkReal(2.00000000000000))*(cj14)*(sj14));
04779 dummyeval[0]=((((x1096)*((r01)*(r01))))+(((x1095)*((r10)*(r10))))+(((x1095)*((r00)*(r00))))+(((x1096)*((r11)*(r11))))+(((r00)*(r01)*(x1097)))+(((r10)*(r11)*(x1097))));
04780 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04781 {
04782 {
04783 IkReal dummyeval[1];
04784 IkReal gconst34;
04785 gconst34=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
04786 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
04787 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04788 {
04789 {
04790 IkReal dummyeval[1];
04791 IkReal gconst33;
04792 IkReal x1098=(cj14)*(cj14);
04793 IkReal x1099=(sj14)*(sj14);
04794 IkReal x1100=((IkReal(1.00000000000000))*(r01));
04795 IkReal x1101=((sj13)*(sj14));
04796 IkReal x1102=((cj14)*(sj13));
04797 IkReal x1103=((r00)*(r11));
04798 IkReal x1104=((cj13)*(x1098));
04799 IkReal x1105=((cj13)*(x1099));
04800 gconst33=IKsign(((((IkReal(-1.00000000000000))*(r10)*(x1100)*(x1104)))+(((IkReal(-1.00000000000000))*(r10)*(x1100)*(x1105)))+(((IkReal(-1.00000000000000))*(r12)*(x1100)*(x1102)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1101)))+(((r02)*(r11)*(x1102)))+(((r02)*(r10)*(x1101)))+(((x1103)*(x1105)))+(((x1103)*(x1104)))));
04801 IkReal x1106=(cj14)*(cj14);
04802 IkReal x1107=(sj14)*(sj14);
04803 IkReal x1108=((IkReal(1.00000000000000))*(r01));
04804 IkReal x1109=((sj13)*(sj14));
04805 IkReal x1110=((cj14)*(sj13));
04806 IkReal x1111=((r00)*(r11));
04807 IkReal x1112=((cj13)*(x1106));
04808 IkReal x1113=((cj13)*(x1107));
04809 dummyeval[0]=((((x1111)*(x1112)))+(((x1111)*(x1113)))+(((IkReal(-1.00000000000000))*(r12)*(x1108)*(x1110)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1109)))+(((r02)*(r11)*(x1110)))+(((r02)*(r10)*(x1109)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)*(x1113)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)*(x1112))));
04810 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04811 {
04812 continue;
04813
04814 } else
04815 {
04816 {
04817 IkReal j9array[1], cj9array[1], sj9array[1];
04818 bool j9valid[1]={false};
04819 _nj9 = 1;
04820 IkReal x1114=((cj13)*(cj14));
04821 IkReal x1115=((IkReal(1.00000000000000))*(cj13)*(sj14));
04822 if( IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1115)))+(((r12)*(sj13)))+(((r10)*(x1114))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1115)))+(((r02)*(sj13)))+(((r00)*(x1114))))))) < IKFAST_ATAN2_MAGTHRESH )
04823 continue;
04824 j9array[0]=IKatan2(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1115)))+(((r12)*(sj13)))+(((r10)*(x1114)))))), ((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1115)))+(((r02)*(sj13)))+(((r00)*(x1114)))))));
04825 sj9array[0]=IKsin(j9array[0]);
04826 cj9array[0]=IKcos(j9array[0]);
04827 if( j9array[0] > IKPI )
04828 {
04829 j9array[0]-=IK2PI;
04830 }
04831 else if( j9array[0] < -IKPI )
04832 { j9array[0]+=IK2PI;
04833 }
04834 j9valid[0] = true;
04835 for(int ij9 = 0; ij9 < 1; ++ij9)
04836 {
04837 if( !j9valid[ij9] )
04838 {
04839 continue;
04840 }
04841 _ij9[0] = ij9; _ij9[1] = -1;
04842 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
04843 {
04844 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
04845 {
04846 j9valid[iij9]=false; _ij9[1] = iij9; break;
04847 }
04848 }
04849 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
04850 {
04851 IkReal evalcond[4];
04852 IkReal x1116=IKsin(j9);
04853 IkReal x1117=IKcos(j9);
04854 IkReal x1118=((r10)*(sj14));
04855 IkReal x1119=((cj14)*(r11));
04856 IkReal x1120=((cj14)*(r10));
04857 IkReal x1121=((cj14)*(r00));
04858 IkReal x1122=((r11)*(sj14));
04859 IkReal x1123=((r00)*(sj14));
04860 IkReal x1124=((IkReal(1.00000000000000))*(x1116));
04861 IkReal x1125=((cj13)*(x1116));
04862 IkReal x1126=((sj13)*(x1117));
04863 IkReal x1127=((r01)*(x1116));
04864 IkReal x1128=((IkReal(1.00000000000000))*(x1117));
04865 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(x1127)))+(((IkReal(-1.00000000000000))*(x1119)*(x1128)))+(((x1116)*(x1123)))+(((IkReal(-1.00000000000000))*(x1118)*(x1128))));
04866 evalcond[1]=((((IkReal(-1.00000000000000))*(x1123)*(x1128)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1128)))+(((IkReal(-1.00000000000000))*(x1119)*(x1124)))+(((IkReal(-1.00000000000000))*(x1118)*(x1124))));
04867 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x1122)*(x1128)))+(((cj13)*(x1117)*(x1120)))+(((r01)*(sj14)*(x1125)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1124)))+(((r12)*(x1126)))+(((IkReal(-1.00000000000000))*(cj13)*(x1121)*(x1124))));
04868 evalcond[3]=((((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1128)))+(((x1120)*(x1126)))+(((sj13)*(sj14)*(x1127)))+(((IkReal(-1.00000000000000))*(x1122)*(x1126)))+(((IkReal(-1.00000000000000))*(sj13)*(x1121)*(x1124)))+(((r02)*(x1125))));
04869 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04870 {
04871 continue;
04872 }
04873 }
04874
04875 {
04876 IkReal dummyeval[1];
04877 IkReal gconst35;
04878 gconst35=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
04879 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
04880 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04881 {
04882 {
04883 IkReal dummyeval[1];
04884 IkReal gconst36;
04885 gconst36=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
04886 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
04887 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
04888 {
04889 continue;
04890
04891 } else
04892 {
04893 {
04894 IkReal j10array[1], cj10array[1], sj10array[1];
04895 bool j10valid[1]={false};
04896 _nj10 = 1;
04897 IkReal x1129=((cj13)*(sj14));
04898 IkReal x1130=((IkReal(1.00000000000000))*(cj11));
04899 IkReal x1131=((r11)*(sj9));
04900 IkReal x1132=((cj13)*(cj14));
04901 IkReal x1133=((IkReal(1.00000000000000))*(sj11));
04902 IkReal x1134=((cj11)*(sj13));
04903 IkReal x1135=((r10)*(sj9));
04904 IkReal x1136=((sj11)*(sj13));
04905 IkReal x1137=((cj9)*(r02));
04906 IkReal x1138=((r12)*(sj9));
04907 IkReal x1139=((cj9)*(r00));
04908 IkReal x1140=((cj9)*(r01));
04909 if( IKabs(((gconst36)*(((((r21)*(sj11)*(x1129)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1140)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1133)))+(((x1134)*(x1137)))+(((x1134)*(x1138)))+(((cj11)*(x1132)*(x1139)))+(((cj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1131)))+(((IkReal(-1.00000000000000))*(r20)*(x1132)*(x1133))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((cj11)*(r20)*(x1132)))+(((sj11)*(x1132)*(x1139)))+(((sj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1131)*(x1133)))+(((x1136)*(x1137)))+(((x1136)*(x1138)))+(((IkReal(-1.00000000000000))*(r21)*(x1129)*(x1130)))+(((IkReal(-1.00000000000000))*(x1129)*(x1133)*(x1140)))+(((r22)*(x1134))))))) < IKFAST_ATAN2_MAGTHRESH )
04910 continue;
04911 j10array[0]=IKatan2(((gconst36)*(((((r21)*(sj11)*(x1129)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1140)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1133)))+(((x1134)*(x1137)))+(((x1134)*(x1138)))+(((cj11)*(x1132)*(x1139)))+(((cj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1131)))+(((IkReal(-1.00000000000000))*(r20)*(x1132)*(x1133)))))), ((gconst36)*(((((cj11)*(r20)*(x1132)))+(((sj11)*(x1132)*(x1139)))+(((sj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1131)*(x1133)))+(((x1136)*(x1137)))+(((x1136)*(x1138)))+(((IkReal(-1.00000000000000))*(r21)*(x1129)*(x1130)))+(((IkReal(-1.00000000000000))*(x1129)*(x1133)*(x1140)))+(((r22)*(x1134)))))));
04912 sj10array[0]=IKsin(j10array[0]);
04913 cj10array[0]=IKcos(j10array[0]);
04914 if( j10array[0] > IKPI )
04915 {
04916 j10array[0]-=IK2PI;
04917 }
04918 else if( j10array[0] < -IKPI )
04919 { j10array[0]+=IK2PI;
04920 }
04921 j10valid[0] = true;
04922 for(int ij10 = 0; ij10 < 1; ++ij10)
04923 {
04924 if( !j10valid[ij10] )
04925 {
04926 continue;
04927 }
04928 _ij10[0] = ij10; _ij10[1] = -1;
04929 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
04930 {
04931 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
04932 {
04933 j10valid[iij10]=false; _ij10[1] = iij10; break;
04934 }
04935 }
04936 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
04937 {
04938 IkReal evalcond[4];
04939 IkReal x1141=IKsin(j10);
04940 IkReal x1142=IKcos(j10);
04941 IkReal x1143=((IkReal(1.00000000000000))*(sj13));
04942 IkReal x1144=((r11)*(sj9));
04943 IkReal x1145=((cj9)*(r01));
04944 IkReal x1146=((r21)*(sj14));
04945 IkReal x1147=((cj9)*(r02));
04946 IkReal x1148=((sj13)*(sj9));
04947 IkReal x1149=((cj14)*(r10));
04948 IkReal x1150=((IkReal(1.00000000000000))*(cj13));
04949 IkReal x1151=((cj14)*(r20));
04950 IkReal x1152=((cj11)*(x1141));
04951 IkReal x1153=((sj11)*(x1142));
04952 IkReal x1154=((sj14)*(x1150));
04953 IkReal x1155=((sj11)*(x1141));
04954 IkReal x1156=((cj11)*(x1142));
04955 IkReal x1157=((cj14)*(cj9)*(r00));
04956 IkReal x1158=((x1153)+(x1152));
04957 evalcond[0]=((x1155)+(((cj13)*(x1146)))+(((IkReal(-1.00000000000000))*(x1156)))+(((IkReal(-1.00000000000000))*(r22)*(x1143)))+(((IkReal(-1.00000000000000))*(x1150)*(x1151))));
04958 evalcond[1]=((x1158)+(((IkReal(-1.00000000000000))*(x1143)*(x1151)))+(((sj13)*(x1146)))+(((cj13)*(r22))));
04959 evalcond[2]=((((IkReal(-1.00000000000000))*(x1144)*(x1154)))+(x1158)+(((cj13)*(sj9)*(x1149)))+(((cj13)*(x1157)))+(((sj13)*(x1147)))+(((IkReal(-1.00000000000000))*(x1145)*(x1154)))+(((r12)*(x1148))));
04960 evalcond[3]=((x1156)+(((IkReal(-1.00000000000000))*(x1147)*(x1150)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1150)))+(((IkReal(-1.00000000000000))*(x1155)))+(((sj13)*(x1157)))+(((IkReal(-1.00000000000000))*(sj14)*(x1143)*(x1144)))+(((IkReal(-1.00000000000000))*(sj14)*(x1143)*(x1145)))+(((x1148)*(x1149))));
04961 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
04962 {
04963 continue;
04964 }
04965 }
04966
04967 {
04968 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
04969 vinfos[0].jointtype = 1;
04970 vinfos[0].foffset = j9;
04971 vinfos[0].indices[0] = _ij9[0];
04972 vinfos[0].indices[1] = _ij9[1];
04973 vinfos[0].maxsolutions = _nj9;
04974 vinfos[1].jointtype = 1;
04975 vinfos[1].foffset = j10;
04976 vinfos[1].indices[0] = _ij10[0];
04977 vinfos[1].indices[1] = _ij10[1];
04978 vinfos[1].maxsolutions = _nj10;
04979 vinfos[2].jointtype = 1;
04980 vinfos[2].foffset = j11;
04981 vinfos[2].indices[0] = _ij11[0];
04982 vinfos[2].indices[1] = _ij11[1];
04983 vinfos[2].maxsolutions = _nj11;
04984 vinfos[3].jointtype = 1;
04985 vinfos[3].foffset = j12;
04986 vinfos[3].indices[0] = _ij12[0];
04987 vinfos[3].indices[1] = _ij12[1];
04988 vinfos[3].maxsolutions = _nj12;
04989 vinfos[4].jointtype = 1;
04990 vinfos[4].foffset = j13;
04991 vinfos[4].indices[0] = _ij13[0];
04992 vinfos[4].indices[1] = _ij13[1];
04993 vinfos[4].maxsolutions = _nj13;
04994 vinfos[5].jointtype = 1;
04995 vinfos[5].foffset = j14;
04996 vinfos[5].indices[0] = _ij14[0];
04997 vinfos[5].indices[1] = _ij14[1];
04998 vinfos[5].maxsolutions = _nj14;
04999 std::vector<int> vfree(0);
05000 solutions.AddSolution(vinfos,vfree);
05001 }
05002 }
05003 }
05004
05005 }
05006
05007 }
05008
05009 } else
05010 {
05011 {
05012 IkReal j10array[1], cj10array[1], sj10array[1];
05013 bool j10valid[1]={false};
05014 _nj10 = 1;
05015 IkReal x1159=((cj13)*(sj11));
05016 IkReal x1160=((r21)*(sj14));
05017 IkReal x1161=((cj11)*(cj13));
05018 IkReal x1162=((cj11)*(sj13));
05019 IkReal x1163=((sj11)*(sj13));
05020 IkReal x1164=((IkReal(1.00000000000000))*(cj14)*(r20));
05021 if( IKabs(((gconst35)*(((((x1159)*(x1160)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-1.00000000000000))*(x1162)*(x1164)))+(((IkReal(-1.00000000000000))*(r22)*(x1163)))+(((r22)*(x1161)))+(((x1160)*(x1162))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((cj14)*(r20)*(x1161)))+(((r22)*(x1159)))+(((IkReal(-1.00000000000000))*(x1163)*(x1164)))+(((r22)*(x1162)))+(((IkReal(-1.00000000000000))*(x1160)*(x1161)))+(((x1160)*(x1163))))))) < IKFAST_ATAN2_MAGTHRESH )
05022 continue;
05023 j10array[0]=IKatan2(((gconst35)*(((((x1159)*(x1160)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-1.00000000000000))*(x1162)*(x1164)))+(((IkReal(-1.00000000000000))*(r22)*(x1163)))+(((r22)*(x1161)))+(((x1160)*(x1162)))))), ((gconst35)*(((((cj14)*(r20)*(x1161)))+(((r22)*(x1159)))+(((IkReal(-1.00000000000000))*(x1163)*(x1164)))+(((r22)*(x1162)))+(((IkReal(-1.00000000000000))*(x1160)*(x1161)))+(((x1160)*(x1163)))))));
05024 sj10array[0]=IKsin(j10array[0]);
05025 cj10array[0]=IKcos(j10array[0]);
05026 if( j10array[0] > IKPI )
05027 {
05028 j10array[0]-=IK2PI;
05029 }
05030 else if( j10array[0] < -IKPI )
05031 { j10array[0]+=IK2PI;
05032 }
05033 j10valid[0] = true;
05034 for(int ij10 = 0; ij10 < 1; ++ij10)
05035 {
05036 if( !j10valid[ij10] )
05037 {
05038 continue;
05039 }
05040 _ij10[0] = ij10; _ij10[1] = -1;
05041 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
05042 {
05043 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
05044 {
05045 j10valid[iij10]=false; _ij10[1] = iij10; break;
05046 }
05047 }
05048 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
05049 {
05050 IkReal evalcond[4];
05051 IkReal x1165=IKsin(j10);
05052 IkReal x1166=IKcos(j10);
05053 IkReal x1167=((IkReal(1.00000000000000))*(sj13));
05054 IkReal x1168=((r11)*(sj9));
05055 IkReal x1169=((cj9)*(r01));
05056 IkReal x1170=((r21)*(sj14));
05057 IkReal x1171=((cj9)*(r02));
05058 IkReal x1172=((sj13)*(sj9));
05059 IkReal x1173=((cj14)*(r10));
05060 IkReal x1174=((IkReal(1.00000000000000))*(cj13));
05061 IkReal x1175=((cj14)*(r20));
05062 IkReal x1176=((cj11)*(x1165));
05063 IkReal x1177=((sj11)*(x1166));
05064 IkReal x1178=((sj14)*(x1174));
05065 IkReal x1179=((sj11)*(x1165));
05066 IkReal x1180=((cj11)*(x1166));
05067 IkReal x1181=((cj14)*(cj9)*(r00));
05068 IkReal x1182=((x1177)+(x1176));
05069 evalcond[0]=((((cj13)*(x1170)))+(x1179)+(((IkReal(-1.00000000000000))*(r22)*(x1167)))+(((IkReal(-1.00000000000000))*(x1180)))+(((IkReal(-1.00000000000000))*(x1174)*(x1175))));
05070 evalcond[1]=((x1182)+(((sj13)*(x1170)))+(((IkReal(-1.00000000000000))*(x1167)*(x1175)))+(((cj13)*(r22))));
05071 evalcond[2]=((x1182)+(((cj13)*(sj9)*(x1173)))+(((sj13)*(x1171)))+(((IkReal(-1.00000000000000))*(x1168)*(x1178)))+(((r12)*(x1172)))+(((IkReal(-1.00000000000000))*(x1169)*(x1178)))+(((cj13)*(x1181))));
05072 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1174)))+(x1180)+(((IkReal(-1.00000000000000))*(x1171)*(x1174)))+(((IkReal(-1.00000000000000))*(sj14)*(x1167)*(x1168)))+(((IkReal(-1.00000000000000))*(sj14)*(x1167)*(x1169)))+(((x1172)*(x1173)))+(((sj13)*(x1181)))+(((IkReal(-1.00000000000000))*(x1179))));
05073 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05074 {
05075 continue;
05076 }
05077 }
05078
05079 {
05080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05081 vinfos[0].jointtype = 1;
05082 vinfos[0].foffset = j9;
05083 vinfos[0].indices[0] = _ij9[0];
05084 vinfos[0].indices[1] = _ij9[1];
05085 vinfos[0].maxsolutions = _nj9;
05086 vinfos[1].jointtype = 1;
05087 vinfos[1].foffset = j10;
05088 vinfos[1].indices[0] = _ij10[0];
05089 vinfos[1].indices[1] = _ij10[1];
05090 vinfos[1].maxsolutions = _nj10;
05091 vinfos[2].jointtype = 1;
05092 vinfos[2].foffset = j11;
05093 vinfos[2].indices[0] = _ij11[0];
05094 vinfos[2].indices[1] = _ij11[1];
05095 vinfos[2].maxsolutions = _nj11;
05096 vinfos[3].jointtype = 1;
05097 vinfos[3].foffset = j12;
05098 vinfos[3].indices[0] = _ij12[0];
05099 vinfos[3].indices[1] = _ij12[1];
05100 vinfos[3].maxsolutions = _nj12;
05101 vinfos[4].jointtype = 1;
05102 vinfos[4].foffset = j13;
05103 vinfos[4].indices[0] = _ij13[0];
05104 vinfos[4].indices[1] = _ij13[1];
05105 vinfos[4].maxsolutions = _nj13;
05106 vinfos[5].jointtype = 1;
05107 vinfos[5].foffset = j14;
05108 vinfos[5].indices[0] = _ij14[0];
05109 vinfos[5].indices[1] = _ij14[1];
05110 vinfos[5].maxsolutions = _nj14;
05111 std::vector<int> vfree(0);
05112 solutions.AddSolution(vinfos,vfree);
05113 }
05114 }
05115 }
05116
05117 }
05118
05119 }
05120 }
05121 }
05122
05123 }
05124
05125 }
05126
05127 } else
05128 {
05129 {
05130 IkReal j10array[1], cj10array[1], sj10array[1];
05131 bool j10valid[1]={false};
05132 _nj10 = 1;
05133 IkReal x1183=((cj13)*(sj11));
05134 IkReal x1184=((r21)*(sj14));
05135 IkReal x1185=((cj11)*(cj13));
05136 IkReal x1186=((cj11)*(sj13));
05137 IkReal x1187=((sj11)*(sj13));
05138 IkReal x1188=((IkReal(1.00000000000000))*(cj14)*(r20));
05139 if( IKabs(((gconst34)*(((((x1184)*(x1186)))+(((x1183)*(x1184)))+(((r22)*(x1185)))+(((IkReal(-1.00000000000000))*(x1186)*(x1188)))+(((IkReal(-1.00000000000000))*(x1183)*(x1188)))+(((IkReal(-1.00000000000000))*(r22)*(x1187))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst34)*(((((cj14)*(r20)*(x1185)))+(((x1184)*(x1187)))+(((r22)*(x1186)))+(((r22)*(x1183)))+(((IkReal(-1.00000000000000))*(x1187)*(x1188)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185))))))) < IKFAST_ATAN2_MAGTHRESH )
05140 continue;
05141 j10array[0]=IKatan2(((gconst34)*(((((x1184)*(x1186)))+(((x1183)*(x1184)))+(((r22)*(x1185)))+(((IkReal(-1.00000000000000))*(x1186)*(x1188)))+(((IkReal(-1.00000000000000))*(x1183)*(x1188)))+(((IkReal(-1.00000000000000))*(r22)*(x1187)))))), ((gconst34)*(((((cj14)*(r20)*(x1185)))+(((x1184)*(x1187)))+(((r22)*(x1186)))+(((r22)*(x1183)))+(((IkReal(-1.00000000000000))*(x1187)*(x1188)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185)))))));
05142 sj10array[0]=IKsin(j10array[0]);
05143 cj10array[0]=IKcos(j10array[0]);
05144 if( j10array[0] > IKPI )
05145 {
05146 j10array[0]-=IK2PI;
05147 }
05148 else if( j10array[0] < -IKPI )
05149 { j10array[0]+=IK2PI;
05150 }
05151 j10valid[0] = true;
05152 for(int ij10 = 0; ij10 < 1; ++ij10)
05153 {
05154 if( !j10valid[ij10] )
05155 {
05156 continue;
05157 }
05158 _ij10[0] = ij10; _ij10[1] = -1;
05159 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
05160 {
05161 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
05162 {
05163 j10valid[iij10]=false; _ij10[1] = iij10; break;
05164 }
05165 }
05166 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
05167 {
05168 IkReal evalcond[2];
05169 IkReal x1189=IKsin(j10);
05170 IkReal x1190=IKcos(j10);
05171 IkReal x1191=((IkReal(1.00000000000000))*(sj13));
05172 IkReal x1192=((cj14)*(r20));
05173 IkReal x1193=((r21)*(sj14));
05174 evalcond[0]=((((IkReal(-1.00000000000000))*(cj11)*(x1190)))+(((IkReal(-1.00000000000000))*(r22)*(x1191)))+(((sj11)*(x1189)))+(((IkReal(-1.00000000000000))*(cj13)*(x1192)))+(((cj13)*(x1193))));
05175 evalcond[1]=((((IkReal(-1.00000000000000))*(x1191)*(x1192)))+(((sj11)*(x1190)))+(((cj11)*(x1189)))+(((sj13)*(x1193)))+(((cj13)*(r22))));
05176 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
05177 {
05178 continue;
05179 }
05180 }
05181
05182 {
05183 IkReal dummyeval[1];
05184 IkReal gconst37;
05185 IkReal x1194=(sj14)*(sj14);
05186 IkReal x1195=(cj14)*(cj14);
05187 IkReal x1196=((IkReal(2.00000000000000))*(cj14)*(sj14));
05188 gconst37=IKsign(((((x1194)*((r10)*(r10))))+(((x1195)*((r01)*(r01))))+(((r10)*(r11)*(x1196)))+(((x1195)*((r11)*(r11))))+(((x1194)*((r00)*(r00))))+(((r00)*(r01)*(x1196)))));
05189 IkReal x1197=(sj14)*(sj14);
05190 IkReal x1198=(cj14)*(cj14);
05191 IkReal x1199=((IkReal(2.00000000000000))*(cj14)*(sj14));
05192 dummyeval[0]=((((x1197)*((r00)*(r00))))+(((r10)*(r11)*(x1199)))+(((x1198)*((r01)*(r01))))+(((x1197)*((r10)*(r10))))+(((r00)*(r01)*(x1199)))+(((x1198)*((r11)*(r11)))));
05193 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05194 {
05195 {
05196 IkReal dummyeval[1];
05197 IkReal gconst38;
05198 IkReal x1200=(cj14)*(cj14);
05199 IkReal x1201=(sj14)*(sj14);
05200 IkReal x1202=((IkReal(1.00000000000000))*(r01));
05201 IkReal x1203=((sj13)*(sj14));
05202 IkReal x1204=((cj14)*(sj13));
05203 IkReal x1205=((r00)*(r11));
05204 IkReal x1206=((cj13)*(x1200));
05205 IkReal x1207=((cj13)*(x1201));
05206 gconst38=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x1202)*(x1204)))+(((x1205)*(x1206)))+(((x1205)*(x1207)))+(((IkReal(-1.00000000000000))*(r10)*(x1202)*(x1207)))+(((IkReal(-1.00000000000000))*(r10)*(x1202)*(x1206)))+(((r02)*(r11)*(x1204)))+(((r02)*(r10)*(x1203)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1203)))));
05207 IkReal x1208=(cj14)*(cj14);
05208 IkReal x1209=(sj14)*(sj14);
05209 IkReal x1210=((IkReal(1.00000000000000))*(r01));
05210 IkReal x1211=((sj13)*(sj14));
05211 IkReal x1212=((cj14)*(sj13));
05212 IkReal x1213=((r00)*(r11));
05213 IkReal x1214=((cj13)*(x1208));
05214 IkReal x1215=((cj13)*(x1209));
05215 dummyeval[0]=((((x1213)*(x1215)))+(((x1213)*(x1214)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1211)))+(((IkReal(-1.00000000000000))*(r10)*(x1210)*(x1215)))+(((IkReal(-1.00000000000000))*(r10)*(x1210)*(x1214)))+(((IkReal(-1.00000000000000))*(r12)*(x1210)*(x1212)))+(((r02)*(r10)*(x1211)))+(((r02)*(r11)*(x1212))));
05216 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05217 {
05218 continue;
05219
05220 } else
05221 {
05222 {
05223 IkReal j9array[1], cj9array[1], sj9array[1];
05224 bool j9valid[1]={false};
05225 _nj9 = 1;
05226 IkReal x1216=((cj13)*(cj14));
05227 IkReal x1217=((IkReal(1.00000000000000))*(cj13)*(sj14));
05228 if( IKabs(((gconst38)*(((((r12)*(sj13)))+(((r10)*(x1216)))+(((IkReal(-1.00000000000000))*(r11)*(x1217))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst38)*(((((r02)*(sj13)))+(((r00)*(x1216)))+(((IkReal(-1.00000000000000))*(r01)*(x1217))))))) < IKFAST_ATAN2_MAGTHRESH )
05229 continue;
05230 j9array[0]=IKatan2(((gconst38)*(((((r12)*(sj13)))+(((r10)*(x1216)))+(((IkReal(-1.00000000000000))*(r11)*(x1217)))))), ((gconst38)*(((((r02)*(sj13)))+(((r00)*(x1216)))+(((IkReal(-1.00000000000000))*(r01)*(x1217)))))));
05231 sj9array[0]=IKsin(j9array[0]);
05232 cj9array[0]=IKcos(j9array[0]);
05233 if( j9array[0] > IKPI )
05234 {
05235 j9array[0]-=IK2PI;
05236 }
05237 else if( j9array[0] < -IKPI )
05238 { j9array[0]+=IK2PI;
05239 }
05240 j9valid[0] = true;
05241 for(int ij9 = 0; ij9 < 1; ++ij9)
05242 {
05243 if( !j9valid[ij9] )
05244 {
05245 continue;
05246 }
05247 _ij9[0] = ij9; _ij9[1] = -1;
05248 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
05249 {
05250 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
05251 {
05252 j9valid[iij9]=false; _ij9[1] = iij9; break;
05253 }
05254 }
05255 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
05256 {
05257 IkReal evalcond[6];
05258 IkReal x1218=IKsin(j9);
05259 IkReal x1219=IKcos(j9);
05260 IkReal x1220=((IkReal(1.00000000000000))*(cj14));
05261 IkReal x1221=((IkReal(1.00000000000000))*(sj14));
05262 IkReal x1222=((cj13)*(cj14));
05263 IkReal x1223=((cj14)*(r10));
05264 IkReal x1224=((r01)*(sj14));
05265 IkReal x1225=((IkReal(1.00000000000000))*(r12));
05266 IkReal x1226=((sj13)*(x1219));
05267 IkReal x1227=((r02)*(x1218));
05268 IkReal x1228=((r11)*(x1218));
05269 IkReal x1229=((r10)*(x1219));
05270 IkReal x1230=((r01)*(x1219));
05271 IkReal x1231=((sj13)*(x1218));
05272 IkReal x1232=((r11)*(x1219));
05273 IkReal x1233=((cj13)*(x1218));
05274 IkReal x1234=((r10)*(x1218));
05275 IkReal x1235=((r00)*(x1219));
05276 IkReal x1236=((cj13)*(x1219));
05277 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1220)*(x1232)))+(((IkReal(-1.00000000000000))*(x1221)*(x1229)))+(((cj14)*(r01)*(x1218)))+(((r00)*(sj14)*(x1218))));
05278 evalcond[1]=((((IkReal(-1.00000000000000))*(x1220)*(x1230)))+(((IkReal(-1.00000000000000))*(x1220)*(x1228)))+(((IkReal(-1.00000000000000))*(x1221)*(x1234)))+(((IkReal(-1.00000000000000))*(x1221)*(x1235))));
05279 evalcond[2]=((((x1224)*(x1233)))+(((x1222)*(x1229)))+(((IkReal(-1.00000000000000))*(r00)*(x1220)*(x1233)))+(((IkReal(-1.00000000000000))*(sj13)*(x1227)))+(((r12)*(x1226)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1232))));
05280 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(x1225)*(x1236)))+(((x1223)*(x1226)))+(((x1224)*(x1231)))+(((cj13)*(x1227)))+(((IkReal(-1.00000000000000))*(r00)*(x1220)*(x1231))));
05281 evalcond[4]=((((cj10)*(sj11)))+(((r02)*(x1226)))+(((x1222)*(x1235)))+(((x1222)*(x1234)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1228)))+(((cj11)*(sj10)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1230)))+(((r12)*(x1231))));
05282 evalcond[5]=((((IkReal(-1.00000000000000))*(x1225)*(x1233)))+(((IkReal(-1.00000000000000))*(r01)*(x1221)*(x1226)))+(((x1223)*(x1231)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(sj13)*(x1221)*(x1228)))+(((IkReal(-1.00000000000000))*(r02)*(x1236)))+(((cj14)*(r00)*(x1226)))+(((cj10)*(cj11))));
05283 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 )
05284 {
05285 continue;
05286 }
05287 }
05288
05289 {
05290 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05291 vinfos[0].jointtype = 1;
05292 vinfos[0].foffset = j9;
05293 vinfos[0].indices[0] = _ij9[0];
05294 vinfos[0].indices[1] = _ij9[1];
05295 vinfos[0].maxsolutions = _nj9;
05296 vinfos[1].jointtype = 1;
05297 vinfos[1].foffset = j10;
05298 vinfos[1].indices[0] = _ij10[0];
05299 vinfos[1].indices[1] = _ij10[1];
05300 vinfos[1].maxsolutions = _nj10;
05301 vinfos[2].jointtype = 1;
05302 vinfos[2].foffset = j11;
05303 vinfos[2].indices[0] = _ij11[0];
05304 vinfos[2].indices[1] = _ij11[1];
05305 vinfos[2].maxsolutions = _nj11;
05306 vinfos[3].jointtype = 1;
05307 vinfos[3].foffset = j12;
05308 vinfos[3].indices[0] = _ij12[0];
05309 vinfos[3].indices[1] = _ij12[1];
05310 vinfos[3].maxsolutions = _nj12;
05311 vinfos[4].jointtype = 1;
05312 vinfos[4].foffset = j13;
05313 vinfos[4].indices[0] = _ij13[0];
05314 vinfos[4].indices[1] = _ij13[1];
05315 vinfos[4].maxsolutions = _nj13;
05316 vinfos[5].jointtype = 1;
05317 vinfos[5].foffset = j14;
05318 vinfos[5].indices[0] = _ij14[0];
05319 vinfos[5].indices[1] = _ij14[1];
05320 vinfos[5].maxsolutions = _nj14;
05321 std::vector<int> vfree(0);
05322 solutions.AddSolution(vinfos,vfree);
05323 }
05324 }
05325 }
05326
05327 }
05328
05329 }
05330
05331 } else
05332 {
05333 {
05334 IkReal j9array[1], cj9array[1], sj9array[1];
05335 bool j9valid[1]={false};
05336 _nj9 = 1;
05337 if( IKabs(((gconst37)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst37)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
05338 continue;
05339 j9array[0]=IKatan2(((gconst37)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst37)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
05340 sj9array[0]=IKsin(j9array[0]);
05341 cj9array[0]=IKcos(j9array[0]);
05342 if( j9array[0] > IKPI )
05343 {
05344 j9array[0]-=IK2PI;
05345 }
05346 else if( j9array[0] < -IKPI )
05347 { j9array[0]+=IK2PI;
05348 }
05349 j9valid[0] = true;
05350 for(int ij9 = 0; ij9 < 1; ++ij9)
05351 {
05352 if( !j9valid[ij9] )
05353 {
05354 continue;
05355 }
05356 _ij9[0] = ij9; _ij9[1] = -1;
05357 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
05358 {
05359 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
05360 {
05361 j9valid[iij9]=false; _ij9[1] = iij9; break;
05362 }
05363 }
05364 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
05365 {
05366 IkReal evalcond[6];
05367 IkReal x1237=IKsin(j9);
05368 IkReal x1238=IKcos(j9);
05369 IkReal x1239=((IkReal(1.00000000000000))*(cj14));
05370 IkReal x1240=((IkReal(1.00000000000000))*(sj14));
05371 IkReal x1241=((cj13)*(cj14));
05372 IkReal x1242=((cj14)*(r10));
05373 IkReal x1243=((r01)*(sj14));
05374 IkReal x1244=((IkReal(1.00000000000000))*(r12));
05375 IkReal x1245=((sj13)*(x1238));
05376 IkReal x1246=((r02)*(x1237));
05377 IkReal x1247=((r11)*(x1237));
05378 IkReal x1248=((r10)*(x1238));
05379 IkReal x1249=((r01)*(x1238));
05380 IkReal x1250=((sj13)*(x1237));
05381 IkReal x1251=((r11)*(x1238));
05382 IkReal x1252=((cj13)*(x1237));
05383 IkReal x1253=((r10)*(x1237));
05384 IkReal x1254=((r00)*(x1238));
05385 IkReal x1255=((cj13)*(x1238));
05386 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(r01)*(x1237)))+(((r00)*(sj14)*(x1237)))+(((IkReal(-1.00000000000000))*(x1239)*(x1251)))+(((IkReal(-1.00000000000000))*(x1240)*(x1248))));
05387 evalcond[1]=((((IkReal(-1.00000000000000))*(x1239)*(x1247)))+(((IkReal(-1.00000000000000))*(x1239)*(x1249)))+(((IkReal(-1.00000000000000))*(x1240)*(x1253)))+(((IkReal(-1.00000000000000))*(x1240)*(x1254))));
05388 evalcond[2]=((((x1243)*(x1252)))+(((IkReal(-1.00000000000000))*(r00)*(x1239)*(x1252)))+(((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1251)))+(((IkReal(-1.00000000000000))*(sj13)*(x1246)))+(((x1241)*(x1248)))+(((r12)*(x1245))));
05389 evalcond[3]=((((x1243)*(x1250)))+(((IkReal(-1.00000000000000))*(r00)*(x1239)*(x1250)))+(((IkReal(-1.00000000000000))*(x1244)*(x1255)))+(((x1242)*(x1245)))+(((IkReal(-1.00000000000000))*(r11)*(x1240)*(x1245)))+(((cj13)*(x1246))));
05390 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1249)))+(((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1247)))+(((x1241)*(x1254)))+(((x1241)*(x1253)))+(((r12)*(x1250)))+(((cj10)*(sj11)))+(((r02)*(x1245)))+(((cj11)*(sj10))));
05391 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x1240)*(x1245)))+(((IkReal(-1.00000000000000))*(sj13)*(x1240)*(x1247)))+(((IkReal(-1.00000000000000))*(r02)*(x1255)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1244)*(x1252)))+(((cj14)*(r00)*(x1245)))+(((cj10)*(cj11)))+(((x1242)*(x1250))));
05392 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 )
05393 {
05394 continue;
05395 }
05396 }
05397
05398 {
05399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05400 vinfos[0].jointtype = 1;
05401 vinfos[0].foffset = j9;
05402 vinfos[0].indices[0] = _ij9[0];
05403 vinfos[0].indices[1] = _ij9[1];
05404 vinfos[0].maxsolutions = _nj9;
05405 vinfos[1].jointtype = 1;
05406 vinfos[1].foffset = j10;
05407 vinfos[1].indices[0] = _ij10[0];
05408 vinfos[1].indices[1] = _ij10[1];
05409 vinfos[1].maxsolutions = _nj10;
05410 vinfos[2].jointtype = 1;
05411 vinfos[2].foffset = j11;
05412 vinfos[2].indices[0] = _ij11[0];
05413 vinfos[2].indices[1] = _ij11[1];
05414 vinfos[2].maxsolutions = _nj11;
05415 vinfos[3].jointtype = 1;
05416 vinfos[3].foffset = j12;
05417 vinfos[3].indices[0] = _ij12[0];
05418 vinfos[3].indices[1] = _ij12[1];
05419 vinfos[3].maxsolutions = _nj12;
05420 vinfos[4].jointtype = 1;
05421 vinfos[4].foffset = j13;
05422 vinfos[4].indices[0] = _ij13[0];
05423 vinfos[4].indices[1] = _ij13[1];
05424 vinfos[4].maxsolutions = _nj13;
05425 vinfos[5].jointtype = 1;
05426 vinfos[5].foffset = j14;
05427 vinfos[5].indices[0] = _ij14[0];
05428 vinfos[5].indices[1] = _ij14[1];
05429 vinfos[5].maxsolutions = _nj14;
05430 std::vector<int> vfree(0);
05431 solutions.AddSolution(vinfos,vfree);
05432 }
05433 }
05434 }
05435
05436 }
05437
05438 }
05439 }
05440 }
05441
05442 }
05443
05444 }
05445
05446 } else
05447 {
05448 {
05449 IkReal j9array[1], cj9array[1], sj9array[1];
05450 bool j9valid[1]={false};
05451 _nj9 = 1;
05452 if( IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst32)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
05453 continue;
05454 j9array[0]=IKatan2(((gconst32)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst32)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
05455 sj9array[0]=IKsin(j9array[0]);
05456 cj9array[0]=IKcos(j9array[0]);
05457 if( j9array[0] > IKPI )
05458 {
05459 j9array[0]-=IK2PI;
05460 }
05461 else if( j9array[0] < -IKPI )
05462 { j9array[0]+=IK2PI;
05463 }
05464 j9valid[0] = true;
05465 for(int ij9 = 0; ij9 < 1; ++ij9)
05466 {
05467 if( !j9valid[ij9] )
05468 {
05469 continue;
05470 }
05471 _ij9[0] = ij9; _ij9[1] = -1;
05472 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
05473 {
05474 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
05475 {
05476 j9valid[iij9]=false; _ij9[1] = iij9; break;
05477 }
05478 }
05479 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
05480 {
05481 IkReal evalcond[4];
05482 IkReal x1256=IKsin(j9);
05483 IkReal x1257=IKcos(j9);
05484 IkReal x1258=((r10)*(sj14));
05485 IkReal x1259=((cj14)*(r11));
05486 IkReal x1260=((cj14)*(r10));
05487 IkReal x1261=((cj14)*(r00));
05488 IkReal x1262=((r11)*(sj14));
05489 IkReal x1263=((r00)*(sj14));
05490 IkReal x1264=((IkReal(1.00000000000000))*(x1256));
05491 IkReal x1265=((cj13)*(x1256));
05492 IkReal x1266=((sj13)*(x1257));
05493 IkReal x1267=((r01)*(x1256));
05494 IkReal x1268=((IkReal(1.00000000000000))*(x1257));
05495 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1258)*(x1268)))+(((cj14)*(x1267)))+(((x1256)*(x1263)))+(((IkReal(-1.00000000000000))*(x1259)*(x1268))));
05496 evalcond[1]=((((IkReal(-1.00000000000000))*(x1258)*(x1264)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1268)))+(((IkReal(-1.00000000000000))*(x1263)*(x1268)))+(((IkReal(-1.00000000000000))*(x1259)*(x1264))));
05497 evalcond[2]=((((r01)*(sj14)*(x1265)))+(((cj13)*(x1257)*(x1260)))+(((IkReal(-1.00000000000000))*(cj13)*(x1262)*(x1268)))+(((r12)*(x1266)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1264)))+(((IkReal(-1.00000000000000))*(cj13)*(x1261)*(x1264))));
05498 evalcond[3]=((((r02)*(x1265)))+(((IkReal(-1.00000000000000))*(x1262)*(x1266)))+(((x1260)*(x1266)))+(((sj13)*(sj14)*(x1267)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1268)))+(((IkReal(-1.00000000000000))*(sj13)*(x1261)*(x1264))));
05499 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05500 {
05501 continue;
05502 }
05503 }
05504
05505 {
05506 IkReal dummyeval[1];
05507 IkReal gconst35;
05508 gconst35=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
05509 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
05510 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05511 {
05512 {
05513 IkReal dummyeval[1];
05514 IkReal gconst36;
05515 gconst36=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
05516 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
05517 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05518 {
05519 continue;
05520
05521 } else
05522 {
05523 {
05524 IkReal j10array[1], cj10array[1], sj10array[1];
05525 bool j10valid[1]={false};
05526 _nj10 = 1;
05527 IkReal x1269=((cj13)*(sj14));
05528 IkReal x1270=((IkReal(1.00000000000000))*(cj11));
05529 IkReal x1271=((r11)*(sj9));
05530 IkReal x1272=((cj13)*(cj14));
05531 IkReal x1273=((IkReal(1.00000000000000))*(sj11));
05532 IkReal x1274=((cj11)*(sj13));
05533 IkReal x1275=((r10)*(sj9));
05534 IkReal x1276=((sj11)*(sj13));
05535 IkReal x1277=((cj9)*(r02));
05536 IkReal x1278=((r12)*(sj9));
05537 IkReal x1279=((cj9)*(r00));
05538 IkReal x1280=((cj9)*(r01));
05539 if( IKabs(((gconst36)*(((((cj11)*(x1272)*(x1279)))+(((cj11)*(x1272)*(x1275)))+(((x1274)*(x1278)))+(((x1274)*(x1277)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1280)))+(((IkReal(-1.00000000000000))*(r20)*(x1272)*(x1273)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1273)))+(((r21)*(sj11)*(x1269)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1271))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(x1269)*(x1273)*(x1280)))+(((x1276)*(x1277)))+(((x1276)*(x1278)))+(((IkReal(-1.00000000000000))*(r21)*(x1269)*(x1270)))+(((sj11)*(x1272)*(x1275)))+(((sj11)*(x1272)*(x1279)))+(((r22)*(x1274)))+(((cj11)*(r20)*(x1272)))+(((IkReal(-1.00000000000000))*(x1269)*(x1271)*(x1273))))))) < IKFAST_ATAN2_MAGTHRESH )
05540 continue;
05541 j10array[0]=IKatan2(((gconst36)*(((((cj11)*(x1272)*(x1279)))+(((cj11)*(x1272)*(x1275)))+(((x1274)*(x1278)))+(((x1274)*(x1277)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1280)))+(((IkReal(-1.00000000000000))*(r20)*(x1272)*(x1273)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1273)))+(((r21)*(sj11)*(x1269)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1271)))))), ((gconst36)*(((((IkReal(-1.00000000000000))*(x1269)*(x1273)*(x1280)))+(((x1276)*(x1277)))+(((x1276)*(x1278)))+(((IkReal(-1.00000000000000))*(r21)*(x1269)*(x1270)))+(((sj11)*(x1272)*(x1275)))+(((sj11)*(x1272)*(x1279)))+(((r22)*(x1274)))+(((cj11)*(r20)*(x1272)))+(((IkReal(-1.00000000000000))*(x1269)*(x1271)*(x1273)))))));
05542 sj10array[0]=IKsin(j10array[0]);
05543 cj10array[0]=IKcos(j10array[0]);
05544 if( j10array[0] > IKPI )
05545 {
05546 j10array[0]-=IK2PI;
05547 }
05548 else if( j10array[0] < -IKPI )
05549 { j10array[0]+=IK2PI;
05550 }
05551 j10valid[0] = true;
05552 for(int ij10 = 0; ij10 < 1; ++ij10)
05553 {
05554 if( !j10valid[ij10] )
05555 {
05556 continue;
05557 }
05558 _ij10[0] = ij10; _ij10[1] = -1;
05559 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
05560 {
05561 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
05562 {
05563 j10valid[iij10]=false; _ij10[1] = iij10; break;
05564 }
05565 }
05566 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
05567 {
05568 IkReal evalcond[4];
05569 IkReal x1281=IKsin(j10);
05570 IkReal x1282=IKcos(j10);
05571 IkReal x1283=((IkReal(1.00000000000000))*(sj13));
05572 IkReal x1284=((r11)*(sj9));
05573 IkReal x1285=((cj9)*(r01));
05574 IkReal x1286=((r21)*(sj14));
05575 IkReal x1287=((cj9)*(r02));
05576 IkReal x1288=((sj13)*(sj9));
05577 IkReal x1289=((cj14)*(r10));
05578 IkReal x1290=((IkReal(1.00000000000000))*(cj13));
05579 IkReal x1291=((cj14)*(r20));
05580 IkReal x1292=((cj11)*(x1281));
05581 IkReal x1293=((sj11)*(x1282));
05582 IkReal x1294=((sj14)*(x1290));
05583 IkReal x1295=((sj11)*(x1281));
05584 IkReal x1296=((cj11)*(x1282));
05585 IkReal x1297=((cj14)*(cj9)*(r00));
05586 IkReal x1298=((x1292)+(x1293));
05587 evalcond[0]=((x1295)+(((IkReal(-1.00000000000000))*(r22)*(x1283)))+(((IkReal(-1.00000000000000))*(x1290)*(x1291)))+(((IkReal(-1.00000000000000))*(x1296)))+(((cj13)*(x1286))));
05588 evalcond[1]=((x1298)+(((IkReal(-1.00000000000000))*(x1283)*(x1291)))+(((sj13)*(x1286)))+(((cj13)*(r22))));
05589 evalcond[2]=((((IkReal(-1.00000000000000))*(x1284)*(x1294)))+(x1298)+(((cj13)*(x1297)))+(((sj13)*(x1287)))+(((IkReal(-1.00000000000000))*(x1285)*(x1294)))+(((r12)*(x1288)))+(((cj13)*(sj9)*(x1289))));
05590 evalcond[3]=((((x1288)*(x1289)))+(x1296)+(((sj13)*(x1297)))+(((IkReal(-1.00000000000000))*(x1287)*(x1290)))+(((IkReal(-1.00000000000000))*(sj14)*(x1283)*(x1284)))+(((IkReal(-1.00000000000000))*(sj14)*(x1283)*(x1285)))+(((IkReal(-1.00000000000000))*(x1295)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1290))));
05591 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05592 {
05593 continue;
05594 }
05595 }
05596
05597 {
05598 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05599 vinfos[0].jointtype = 1;
05600 vinfos[0].foffset = j9;
05601 vinfos[0].indices[0] = _ij9[0];
05602 vinfos[0].indices[1] = _ij9[1];
05603 vinfos[0].maxsolutions = _nj9;
05604 vinfos[1].jointtype = 1;
05605 vinfos[1].foffset = j10;
05606 vinfos[1].indices[0] = _ij10[0];
05607 vinfos[1].indices[1] = _ij10[1];
05608 vinfos[1].maxsolutions = _nj10;
05609 vinfos[2].jointtype = 1;
05610 vinfos[2].foffset = j11;
05611 vinfos[2].indices[0] = _ij11[0];
05612 vinfos[2].indices[1] = _ij11[1];
05613 vinfos[2].maxsolutions = _nj11;
05614 vinfos[3].jointtype = 1;
05615 vinfos[3].foffset = j12;
05616 vinfos[3].indices[0] = _ij12[0];
05617 vinfos[3].indices[1] = _ij12[1];
05618 vinfos[3].maxsolutions = _nj12;
05619 vinfos[4].jointtype = 1;
05620 vinfos[4].foffset = j13;
05621 vinfos[4].indices[0] = _ij13[0];
05622 vinfos[4].indices[1] = _ij13[1];
05623 vinfos[4].maxsolutions = _nj13;
05624 vinfos[5].jointtype = 1;
05625 vinfos[5].foffset = j14;
05626 vinfos[5].indices[0] = _ij14[0];
05627 vinfos[5].indices[1] = _ij14[1];
05628 vinfos[5].maxsolutions = _nj14;
05629 std::vector<int> vfree(0);
05630 solutions.AddSolution(vinfos,vfree);
05631 }
05632 }
05633 }
05634
05635 }
05636
05637 }
05638
05639 } else
05640 {
05641 {
05642 IkReal j10array[1], cj10array[1], sj10array[1];
05643 bool j10valid[1]={false};
05644 _nj10 = 1;
05645 IkReal x1299=((cj13)*(sj11));
05646 IkReal x1300=((r21)*(sj14));
05647 IkReal x1301=((cj11)*(cj13));
05648 IkReal x1302=((cj11)*(sj13));
05649 IkReal x1303=((sj11)*(sj13));
05650 IkReal x1304=((IkReal(1.00000000000000))*(cj14)*(r20));
05651 if( IKabs(((gconst35)*(((((x1299)*(x1300)))+(((IkReal(-1.00000000000000))*(x1299)*(x1304)))+(((x1300)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1303)))+(((r22)*(x1301)))+(((IkReal(-1.00000000000000))*(x1302)*(x1304))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((x1300)*(x1303)))+(((IkReal(-1.00000000000000))*(x1303)*(x1304)))+(((r22)*(x1302)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)))+(((r22)*(x1299)))+(((cj14)*(r20)*(x1301))))))) < IKFAST_ATAN2_MAGTHRESH )
05652 continue;
05653 j10array[0]=IKatan2(((gconst35)*(((((x1299)*(x1300)))+(((IkReal(-1.00000000000000))*(x1299)*(x1304)))+(((x1300)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1303)))+(((r22)*(x1301)))+(((IkReal(-1.00000000000000))*(x1302)*(x1304)))))), ((gconst35)*(((((x1300)*(x1303)))+(((IkReal(-1.00000000000000))*(x1303)*(x1304)))+(((r22)*(x1302)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)))+(((r22)*(x1299)))+(((cj14)*(r20)*(x1301)))))));
05654 sj10array[0]=IKsin(j10array[0]);
05655 cj10array[0]=IKcos(j10array[0]);
05656 if( j10array[0] > IKPI )
05657 {
05658 j10array[0]-=IK2PI;
05659 }
05660 else if( j10array[0] < -IKPI )
05661 { j10array[0]+=IK2PI;
05662 }
05663 j10valid[0] = true;
05664 for(int ij10 = 0; ij10 < 1; ++ij10)
05665 {
05666 if( !j10valid[ij10] )
05667 {
05668 continue;
05669 }
05670 _ij10[0] = ij10; _ij10[1] = -1;
05671 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
05672 {
05673 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
05674 {
05675 j10valid[iij10]=false; _ij10[1] = iij10; break;
05676 }
05677 }
05678 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
05679 {
05680 IkReal evalcond[4];
05681 IkReal x1305=IKsin(j10);
05682 IkReal x1306=IKcos(j10);
05683 IkReal x1307=((IkReal(1.00000000000000))*(sj13));
05684 IkReal x1308=((r11)*(sj9));
05685 IkReal x1309=((cj9)*(r01));
05686 IkReal x1310=((r21)*(sj14));
05687 IkReal x1311=((cj9)*(r02));
05688 IkReal x1312=((sj13)*(sj9));
05689 IkReal x1313=((cj14)*(r10));
05690 IkReal x1314=((IkReal(1.00000000000000))*(cj13));
05691 IkReal x1315=((cj14)*(r20));
05692 IkReal x1316=((cj11)*(x1305));
05693 IkReal x1317=((sj11)*(x1306));
05694 IkReal x1318=((sj14)*(x1314));
05695 IkReal x1319=((sj11)*(x1305));
05696 IkReal x1320=((cj11)*(x1306));
05697 IkReal x1321=((cj14)*(cj9)*(r00));
05698 IkReal x1322=((x1317)+(x1316));
05699 evalcond[0]=((x1319)+(((cj13)*(x1310)))+(((IkReal(-1.00000000000000))*(r22)*(x1307)))+(((IkReal(-1.00000000000000))*(x1314)*(x1315)))+(((IkReal(-1.00000000000000))*(x1320))));
05700 evalcond[1]=((x1322)+(((sj13)*(x1310)))+(((IkReal(-1.00000000000000))*(x1307)*(x1315)))+(((cj13)*(r22))));
05701 evalcond[2]=((x1322)+(((sj13)*(x1311)))+(((cj13)*(x1321)))+(((IkReal(-1.00000000000000))*(x1308)*(x1318)))+(((r12)*(x1312)))+(((cj13)*(sj9)*(x1313)))+(((IkReal(-1.00000000000000))*(x1309)*(x1318))));
05702 evalcond[3]=((x1320)+(((IkReal(-1.00000000000000))*(x1311)*(x1314)))+(((sj13)*(x1321)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1314)))+(((IkReal(-1.00000000000000))*(x1319)))+(((IkReal(-1.00000000000000))*(sj14)*(x1307)*(x1308)))+(((IkReal(-1.00000000000000))*(sj14)*(x1307)*(x1309)))+(((x1312)*(x1313))));
05703 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05704 {
05705 continue;
05706 }
05707 }
05708
05709 {
05710 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05711 vinfos[0].jointtype = 1;
05712 vinfos[0].foffset = j9;
05713 vinfos[0].indices[0] = _ij9[0];
05714 vinfos[0].indices[1] = _ij9[1];
05715 vinfos[0].maxsolutions = _nj9;
05716 vinfos[1].jointtype = 1;
05717 vinfos[1].foffset = j10;
05718 vinfos[1].indices[0] = _ij10[0];
05719 vinfos[1].indices[1] = _ij10[1];
05720 vinfos[1].maxsolutions = _nj10;
05721 vinfos[2].jointtype = 1;
05722 vinfos[2].foffset = j11;
05723 vinfos[2].indices[0] = _ij11[0];
05724 vinfos[2].indices[1] = _ij11[1];
05725 vinfos[2].maxsolutions = _nj11;
05726 vinfos[3].jointtype = 1;
05727 vinfos[3].foffset = j12;
05728 vinfos[3].indices[0] = _ij12[0];
05729 vinfos[3].indices[1] = _ij12[1];
05730 vinfos[3].maxsolutions = _nj12;
05731 vinfos[4].jointtype = 1;
05732 vinfos[4].foffset = j13;
05733 vinfos[4].indices[0] = _ij13[0];
05734 vinfos[4].indices[1] = _ij13[1];
05735 vinfos[4].maxsolutions = _nj13;
05736 vinfos[5].jointtype = 1;
05737 vinfos[5].foffset = j14;
05738 vinfos[5].indices[0] = _ij14[0];
05739 vinfos[5].indices[1] = _ij14[1];
05740 vinfos[5].maxsolutions = _nj14;
05741 std::vector<int> vfree(0);
05742 solutions.AddSolution(vinfos,vfree);
05743 }
05744 }
05745 }
05746
05747 }
05748
05749 }
05750 }
05751 }
05752
05753 }
05754
05755 }
05756
05757 } else
05758 {
05759 IkReal x1323=((IkReal(1.00000000000000))*(sj13));
05760 IkReal x1324=((cj14)*(npx));
05761 IkReal x1325=((npy)*(sj14));
05762 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
05763 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
05764 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
05765 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x1325)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x1323)))+(((IkReal(-1.00000000000000))*(cj13)*(x1324))));
05766 evalcond[4]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x1323)*(x1324)))+(((sj13)*(x1325)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
05767 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 )
05768 {
05769 {
05770 IkReal dummyeval[1];
05771 IkReal gconst39;
05772 IkReal x1326=(sj14)*(sj14);
05773 IkReal x1327=(cj14)*(cj14);
05774 IkReal x1328=((IkReal(1.00000000000000))*(x1326));
05775 IkReal x1329=((IkReal(2.00000000000000))*(cj14)*(sj14));
05776 IkReal x1330=((IkReal(1.00000000000000))*(x1327));
05777 gconst39=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1329)))+(((IkReal(-1.00000000000000))*(x1328)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1329)))+(((IkReal(-1.00000000000000))*(x1328)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1330)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1330)*((r01)*(r01))))));
05778 IkReal x1331=(sj14)*(sj14);
05779 IkReal x1332=(cj14)*(cj14);
05780 IkReal x1333=((IkReal(1.00000000000000))*(x1331));
05781 IkReal x1334=((IkReal(2.00000000000000))*(cj14)*(sj14));
05782 IkReal x1335=((IkReal(1.00000000000000))*(x1332));
05783 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1333)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1334)))+(((IkReal(-1.00000000000000))*(x1335)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1335)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1334)))+(((IkReal(-1.00000000000000))*(x1333)*((r00)*(r00)))));
05784 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05785 {
05786 {
05787 IkReal dummyeval[1];
05788 IkReal gconst41;
05789 gconst41=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
05790 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
05791 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05792 {
05793 {
05794 IkReal dummyeval[1];
05795 IkReal gconst40;
05796 IkReal x1336=(sj14)*(sj14);
05797 IkReal x1337=(cj14)*(cj14);
05798 IkReal x1338=((cj14)*(sj13));
05799 IkReal x1339=((IkReal(1.00000000000000))*(r11));
05800 IkReal x1340=((cj13)*(r00));
05801 IkReal x1341=((sj13)*(sj14));
05802 IkReal x1342=((cj13)*(r01)*(r10));
05803 gconst40=IKsign(((((IkReal(-1.00000000000000))*(r02)*(r10)*(x1341)))+(((r01)*(r12)*(x1338)))+(((IkReal(-1.00000000000000))*(x1336)*(x1339)*(x1340)))+(((x1337)*(x1342)))+(((x1336)*(x1342)))+(((IkReal(-1.00000000000000))*(r02)*(x1338)*(x1339)))+(((IkReal(-1.00000000000000))*(x1337)*(x1339)*(x1340)))+(((r00)*(r12)*(x1341)))));
05804 IkReal x1343=(sj14)*(sj14);
05805 IkReal x1344=(cj14)*(cj14);
05806 IkReal x1345=((cj14)*(sj13));
05807 IkReal x1346=((IkReal(1.00000000000000))*(r11));
05808 IkReal x1347=((cj13)*(r00));
05809 IkReal x1348=((sj13)*(sj14));
05810 IkReal x1349=((cj13)*(r01)*(r10));
05811 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(x1345)*(x1346)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1348)))+(((IkReal(-1.00000000000000))*(x1344)*(x1346)*(x1347)))+(((x1344)*(x1349)))+(((r00)*(r12)*(x1348)))+(((r01)*(r12)*(x1345)))+(((IkReal(-1.00000000000000))*(x1343)*(x1346)*(x1347)))+(((x1343)*(x1349))));
05812 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05813 {
05814 continue;
05815
05816 } else
05817 {
05818 {
05819 IkReal j9array[1], cj9array[1], sj9array[1];
05820 bool j9valid[1]={false};
05821 _nj9 = 1;
05822 IkReal x1350=((cj13)*(cj14));
05823 IkReal x1351=((IkReal(1.00000000000000))*(cj13)*(sj14));
05824 if( IKabs(((gconst40)*(((((r10)*(x1350)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1351))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst40)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1351)))+(((r00)*(x1350))))))) < IKFAST_ATAN2_MAGTHRESH )
05825 continue;
05826 j9array[0]=IKatan2(((gconst40)*(((((r10)*(x1350)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1351)))))), ((gconst40)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1351)))+(((r00)*(x1350)))))));
05827 sj9array[0]=IKsin(j9array[0]);
05828 cj9array[0]=IKcos(j9array[0]);
05829 if( j9array[0] > IKPI )
05830 {
05831 j9array[0]-=IK2PI;
05832 }
05833 else if( j9array[0] < -IKPI )
05834 { j9array[0]+=IK2PI;
05835 }
05836 j9valid[0] = true;
05837 for(int ij9 = 0; ij9 < 1; ++ij9)
05838 {
05839 if( !j9valid[ij9] )
05840 {
05841 continue;
05842 }
05843 _ij9[0] = ij9; _ij9[1] = -1;
05844 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
05845 {
05846 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
05847 {
05848 j9valid[iij9]=false; _ij9[1] = iij9; break;
05849 }
05850 }
05851 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
05852 {
05853 IkReal evalcond[4];
05854 IkReal x1352=IKsin(j9);
05855 IkReal x1353=IKcos(j9);
05856 IkReal x1354=((r10)*(sj14));
05857 IkReal x1355=((cj14)*(r11));
05858 IkReal x1356=((cj14)*(r10));
05859 IkReal x1357=((cj14)*(r00));
05860 IkReal x1358=((r11)*(sj14));
05861 IkReal x1359=((r00)*(sj14));
05862 IkReal x1360=((IkReal(1.00000000000000))*(x1352));
05863 IkReal x1361=((cj13)*(x1352));
05864 IkReal x1362=((sj13)*(x1353));
05865 IkReal x1363=((r01)*(x1352));
05866 IkReal x1364=((IkReal(1.00000000000000))*(x1353));
05867 evalcond[0]=((IkReal(-1.00000000000000))+(((cj14)*(x1363)))+(((IkReal(-1.00000000000000))*(x1354)*(x1364)))+(((IkReal(-1.00000000000000))*(x1355)*(x1364)))+(((x1352)*(x1359))));
05868 evalcond[1]=((((IkReal(-1.00000000000000))*(x1354)*(x1360)))+(((IkReal(-1.00000000000000))*(x1359)*(x1364)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1364)))+(((IkReal(-1.00000000000000))*(x1355)*(x1360))));
05869 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x1357)*(x1360)))+(((IkReal(-1.00000000000000))*(cj13)*(x1358)*(x1364)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1360)))+(((cj13)*(x1353)*(x1356)))+(((r01)*(sj14)*(x1361)))+(((r12)*(x1362))));
05870 evalcond[3]=((((x1356)*(x1362)))+(((IkReal(-1.00000000000000))*(sj13)*(x1357)*(x1360)))+(((r02)*(x1361)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1364)))+(((IkReal(-1.00000000000000))*(x1358)*(x1362)))+(((sj13)*(sj14)*(x1363))));
05871 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05872 {
05873 continue;
05874 }
05875 }
05876
05877 {
05878 IkReal dummyeval[1];
05879 IkReal gconst42;
05880 gconst42=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
05881 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
05882 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05883 {
05884 {
05885 IkReal dummyeval[1];
05886 IkReal gconst43;
05887 gconst43=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
05888 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
05889 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
05890 {
05891 continue;
05892
05893 } else
05894 {
05895 {
05896 IkReal j10array[1], cj10array[1], sj10array[1];
05897 bool j10valid[1]={false};
05898 _nj10 = 1;
05899 IkReal x1365=((cj13)*(sj14));
05900 IkReal x1366=((IkReal(1.00000000000000))*(cj11));
05901 IkReal x1367=((r11)*(sj9));
05902 IkReal x1368=((IkReal(1.00000000000000))*(sj11));
05903 IkReal x1369=((cj13)*(cj14));
05904 IkReal x1370=((cj11)*(sj13));
05905 IkReal x1371=((r12)*(sj9));
05906 IkReal x1372=((cj9)*(r01));
05907 IkReal x1373=((sj11)*(sj13));
05908 IkReal x1374=((cj9)*(r02));
05909 IkReal x1375=((r10)*(sj9));
05910 IkReal x1376=((cj9)*(r00));
05911 if( IKabs(((gconst43)*(((((r21)*(sj11)*(x1365)))+(((IkReal(-1.00000000000000))*(r20)*(x1368)*(x1369)))+(((cj11)*(x1369)*(x1376)))+(((cj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1372)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1367)))+(((x1370)*(x1371)))+(((x1370)*(x1374)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1368))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((IkReal(-1.00000000000000))*(x1365)*(x1368)*(x1372)))+(((r22)*(x1370)))+(((x1371)*(x1373)))+(((cj11)*(r20)*(x1369)))+(((IkReal(-1.00000000000000))*(x1365)*(x1367)*(x1368)))+(((sj11)*(x1369)*(x1376)))+(((sj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(r21)*(x1365)*(x1366)))+(((x1373)*(x1374))))))) < IKFAST_ATAN2_MAGTHRESH )
05912 continue;
05913 j10array[0]=IKatan2(((gconst43)*(((((r21)*(sj11)*(x1365)))+(((IkReal(-1.00000000000000))*(r20)*(x1368)*(x1369)))+(((cj11)*(x1369)*(x1376)))+(((cj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1372)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1367)))+(((x1370)*(x1371)))+(((x1370)*(x1374)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1368)))))), ((gconst43)*(((((IkReal(-1.00000000000000))*(x1365)*(x1368)*(x1372)))+(((r22)*(x1370)))+(((x1371)*(x1373)))+(((cj11)*(r20)*(x1369)))+(((IkReal(-1.00000000000000))*(x1365)*(x1367)*(x1368)))+(((sj11)*(x1369)*(x1376)))+(((sj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(r21)*(x1365)*(x1366)))+(((x1373)*(x1374)))))));
05914 sj10array[0]=IKsin(j10array[0]);
05915 cj10array[0]=IKcos(j10array[0]);
05916 if( j10array[0] > IKPI )
05917 {
05918 j10array[0]-=IK2PI;
05919 }
05920 else if( j10array[0] < -IKPI )
05921 { j10array[0]+=IK2PI;
05922 }
05923 j10valid[0] = true;
05924 for(int ij10 = 0; ij10 < 1; ++ij10)
05925 {
05926 if( !j10valid[ij10] )
05927 {
05928 continue;
05929 }
05930 _ij10[0] = ij10; _ij10[1] = -1;
05931 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
05932 {
05933 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
05934 {
05935 j10valid[iij10]=false; _ij10[1] = iij10; break;
05936 }
05937 }
05938 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
05939 {
05940 IkReal evalcond[4];
05941 IkReal x1377=IKsin(j10);
05942 IkReal x1378=IKcos(j10);
05943 IkReal x1379=((IkReal(1.00000000000000))*(sj13));
05944 IkReal x1380=((r11)*(sj9));
05945 IkReal x1381=((cj9)*(r01));
05946 IkReal x1382=((IkReal(1.00000000000000))*(cj11));
05947 IkReal x1383=((r21)*(sj14));
05948 IkReal x1384=((cj9)*(r02));
05949 IkReal x1385=((sj13)*(sj9));
05950 IkReal x1386=((cj14)*(r10));
05951 IkReal x1387=((IkReal(1.00000000000000))*(cj13));
05952 IkReal x1388=((cj14)*(r20));
05953 IkReal x1389=((sj11)*(x1377));
05954 IkReal x1390=((sj14)*(x1387));
05955 IkReal x1391=((sj11)*(x1378));
05956 IkReal x1392=((cj14)*(cj9)*(r00));
05957 IkReal x1393=((x1378)*(x1382));
05958 evalcond[0]=((x1389)+(((IkReal(-1.00000000000000))*(r22)*(x1379)))+(((cj13)*(x1383)))+(((IkReal(-1.00000000000000))*(x1387)*(x1388)))+(((IkReal(-1.00000000000000))*(x1393))));
05959 evalcond[1]=((((IkReal(-1.00000000000000))*(x1377)*(x1382)))+(((sj13)*(x1383)))+(((IkReal(-1.00000000000000))*(x1391)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1379)*(x1388))));
05960 evalcond[2]=((x1391)+(((cj13)*(x1392)))+(((sj13)*(x1384)))+(((IkReal(-1.00000000000000))*(x1381)*(x1390)))+(((IkReal(-1.00000000000000))*(x1380)*(x1390)))+(((cj11)*(x1377)))+(((r12)*(x1385)))+(((cj13)*(sj9)*(x1386))));
05961 evalcond[3]=((x1389)+(((IkReal(-1.00000000000000))*(x1384)*(x1387)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1387)))+(((IkReal(-1.00000000000000))*(sj14)*(x1379)*(x1380)))+(((IkReal(-1.00000000000000))*(sj14)*(x1379)*(x1381)))+(((sj13)*(x1392)))+(((x1385)*(x1386)))+(((IkReal(-1.00000000000000))*(x1393))));
05962 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
05963 {
05964 continue;
05965 }
05966 }
05967
05968 {
05969 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
05970 vinfos[0].jointtype = 1;
05971 vinfos[0].foffset = j9;
05972 vinfos[0].indices[0] = _ij9[0];
05973 vinfos[0].indices[1] = _ij9[1];
05974 vinfos[0].maxsolutions = _nj9;
05975 vinfos[1].jointtype = 1;
05976 vinfos[1].foffset = j10;
05977 vinfos[1].indices[0] = _ij10[0];
05978 vinfos[1].indices[1] = _ij10[1];
05979 vinfos[1].maxsolutions = _nj10;
05980 vinfos[2].jointtype = 1;
05981 vinfos[2].foffset = j11;
05982 vinfos[2].indices[0] = _ij11[0];
05983 vinfos[2].indices[1] = _ij11[1];
05984 vinfos[2].maxsolutions = _nj11;
05985 vinfos[3].jointtype = 1;
05986 vinfos[3].foffset = j12;
05987 vinfos[3].indices[0] = _ij12[0];
05988 vinfos[3].indices[1] = _ij12[1];
05989 vinfos[3].maxsolutions = _nj12;
05990 vinfos[4].jointtype = 1;
05991 vinfos[4].foffset = j13;
05992 vinfos[4].indices[0] = _ij13[0];
05993 vinfos[4].indices[1] = _ij13[1];
05994 vinfos[4].maxsolutions = _nj13;
05995 vinfos[5].jointtype = 1;
05996 vinfos[5].foffset = j14;
05997 vinfos[5].indices[0] = _ij14[0];
05998 vinfos[5].indices[1] = _ij14[1];
05999 vinfos[5].maxsolutions = _nj14;
06000 std::vector<int> vfree(0);
06001 solutions.AddSolution(vinfos,vfree);
06002 }
06003 }
06004 }
06005
06006 }
06007
06008 }
06009
06010 } else
06011 {
06012 {
06013 IkReal j10array[1], cj10array[1], sj10array[1];
06014 bool j10valid[1]={false};
06015 _nj10 = 1;
06016 IkReal x1394=((cj11)*(r22));
06017 IkReal x1395=((IkReal(1.00000000000000))*(sj13));
06018 IkReal x1396=((cj13)*(sj11));
06019 IkReal x1397=((r21)*(sj14));
06020 IkReal x1398=((cj11)*(cj13));
06021 IkReal x1399=((sj11)*(sj13));
06022 IkReal x1400=((cj14)*(r20));
06023 if( IKabs(((gconst42)*(((((cj11)*(sj13)*(x1397)))+(((x1396)*(x1400)))+(((cj13)*(x1394)))+(((r22)*(x1399)))+(((IkReal(-1.00000000000000))*(x1396)*(x1397)))+(((IkReal(-1.00000000000000))*(cj11)*(x1395)*(x1400))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1398)*(x1400)))+(((IkReal(-1.00000000000000))*(x1394)*(x1395)))+(((IkReal(-1.00000000000000))*(sj11)*(x1395)*(x1400)))+(((x1397)*(x1399)))+(((x1397)*(x1398)))+(((r22)*(x1396))))))) < IKFAST_ATAN2_MAGTHRESH )
06024 continue;
06025 j10array[0]=IKatan2(((gconst42)*(((((cj11)*(sj13)*(x1397)))+(((x1396)*(x1400)))+(((cj13)*(x1394)))+(((r22)*(x1399)))+(((IkReal(-1.00000000000000))*(x1396)*(x1397)))+(((IkReal(-1.00000000000000))*(cj11)*(x1395)*(x1400)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1398)*(x1400)))+(((IkReal(-1.00000000000000))*(x1394)*(x1395)))+(((IkReal(-1.00000000000000))*(sj11)*(x1395)*(x1400)))+(((x1397)*(x1399)))+(((x1397)*(x1398)))+(((r22)*(x1396)))))));
06026 sj10array[0]=IKsin(j10array[0]);
06027 cj10array[0]=IKcos(j10array[0]);
06028 if( j10array[0] > IKPI )
06029 {
06030 j10array[0]-=IK2PI;
06031 }
06032 else if( j10array[0] < -IKPI )
06033 { j10array[0]+=IK2PI;
06034 }
06035 j10valid[0] = true;
06036 for(int ij10 = 0; ij10 < 1; ++ij10)
06037 {
06038 if( !j10valid[ij10] )
06039 {
06040 continue;
06041 }
06042 _ij10[0] = ij10; _ij10[1] = -1;
06043 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
06044 {
06045 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
06046 {
06047 j10valid[iij10]=false; _ij10[1] = iij10; break;
06048 }
06049 }
06050 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
06051 {
06052 IkReal evalcond[4];
06053 IkReal x1401=IKsin(j10);
06054 IkReal x1402=IKcos(j10);
06055 IkReal x1403=((IkReal(1.00000000000000))*(sj13));
06056 IkReal x1404=((r11)*(sj9));
06057 IkReal x1405=((cj9)*(r01));
06058 IkReal x1406=((IkReal(1.00000000000000))*(cj11));
06059 IkReal x1407=((r21)*(sj14));
06060 IkReal x1408=((cj9)*(r02));
06061 IkReal x1409=((sj13)*(sj9));
06062 IkReal x1410=((cj14)*(r10));
06063 IkReal x1411=((IkReal(1.00000000000000))*(cj13));
06064 IkReal x1412=((cj14)*(r20));
06065 IkReal x1413=((sj11)*(x1401));
06066 IkReal x1414=((sj14)*(x1411));
06067 IkReal x1415=((sj11)*(x1402));
06068 IkReal x1416=((cj14)*(cj9)*(r00));
06069 IkReal x1417=((x1402)*(x1406));
06070 evalcond[0]=((((IkReal(-1.00000000000000))*(x1417)))+(((IkReal(-1.00000000000000))*(r22)*(x1403)))+(((cj13)*(x1407)))+(((IkReal(-1.00000000000000))*(x1411)*(x1412)))+(x1413));
06071 evalcond[1]=((((IkReal(-1.00000000000000))*(x1403)*(x1412)))+(((IkReal(-1.00000000000000))*(x1401)*(x1406)))+(((cj13)*(r22)))+(((sj13)*(x1407)))+(((IkReal(-1.00000000000000))*(x1415))));
06072 evalcond[2]=((((cj13)*(sj9)*(x1410)))+(((cj13)*(x1416)))+(((r12)*(x1409)))+(((IkReal(-1.00000000000000))*(x1404)*(x1414)))+(x1415)+(((IkReal(-1.00000000000000))*(x1405)*(x1414)))+(((cj11)*(x1401)))+(((sj13)*(x1408))));
06073 evalcond[3]=((((IkReal(-1.00000000000000))*(x1417)))+(((IkReal(-1.00000000000000))*(sj14)*(x1403)*(x1405)))+(((IkReal(-1.00000000000000))*(sj14)*(x1403)*(x1404)))+(((IkReal(-1.00000000000000))*(x1408)*(x1411)))+(((sj13)*(x1416)))+(x1413)+(((x1409)*(x1410)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1411))));
06074 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06075 {
06076 continue;
06077 }
06078 }
06079
06080 {
06081 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06082 vinfos[0].jointtype = 1;
06083 vinfos[0].foffset = j9;
06084 vinfos[0].indices[0] = _ij9[0];
06085 vinfos[0].indices[1] = _ij9[1];
06086 vinfos[0].maxsolutions = _nj9;
06087 vinfos[1].jointtype = 1;
06088 vinfos[1].foffset = j10;
06089 vinfos[1].indices[0] = _ij10[0];
06090 vinfos[1].indices[1] = _ij10[1];
06091 vinfos[1].maxsolutions = _nj10;
06092 vinfos[2].jointtype = 1;
06093 vinfos[2].foffset = j11;
06094 vinfos[2].indices[0] = _ij11[0];
06095 vinfos[2].indices[1] = _ij11[1];
06096 vinfos[2].maxsolutions = _nj11;
06097 vinfos[3].jointtype = 1;
06098 vinfos[3].foffset = j12;
06099 vinfos[3].indices[0] = _ij12[0];
06100 vinfos[3].indices[1] = _ij12[1];
06101 vinfos[3].maxsolutions = _nj12;
06102 vinfos[4].jointtype = 1;
06103 vinfos[4].foffset = j13;
06104 vinfos[4].indices[0] = _ij13[0];
06105 vinfos[4].indices[1] = _ij13[1];
06106 vinfos[4].maxsolutions = _nj13;
06107 vinfos[5].jointtype = 1;
06108 vinfos[5].foffset = j14;
06109 vinfos[5].indices[0] = _ij14[0];
06110 vinfos[5].indices[1] = _ij14[1];
06111 vinfos[5].maxsolutions = _nj14;
06112 std::vector<int> vfree(0);
06113 solutions.AddSolution(vinfos,vfree);
06114 }
06115 }
06116 }
06117
06118 }
06119
06120 }
06121 }
06122 }
06123
06124 }
06125
06126 }
06127
06128 } else
06129 {
06130 {
06131 IkReal j10array[1], cj10array[1], sj10array[1];
06132 bool j10valid[1]={false};
06133 _nj10 = 1;
06134 IkReal x1418=((r22)*(sj13));
06135 IkReal x1419=((IkReal(1.00000000000000))*(cj11));
06136 IkReal x1420=((cj13)*(sj11));
06137 IkReal x1421=((cj14)*(r20));
06138 IkReal x1422=((r21)*(sj14));
06139 IkReal x1423=((cj11)*(cj13));
06140 IkReal x1424=((sj11)*(sj13));
06141 if( IKabs(((gconst41)*(((((IkReal(-1.00000000000000))*(sj13)*(x1419)*(x1421)))+(((x1420)*(x1421)))+(((IkReal(-1.00000000000000))*(x1420)*(x1422)))+(((r22)*(x1423)))+(((sj11)*(x1418)))+(((cj11)*(sj13)*(x1422))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst41)*(((((x1422)*(x1423)))+(((x1422)*(x1424)))+(((r22)*(x1420)))+(((IkReal(-1.00000000000000))*(cj13)*(x1419)*(x1421)))+(((IkReal(-1.00000000000000))*(x1418)*(x1419)))+(((IkReal(-1.00000000000000))*(x1421)*(x1424))))))) < IKFAST_ATAN2_MAGTHRESH )
06142 continue;
06143 j10array[0]=IKatan2(((gconst41)*(((((IkReal(-1.00000000000000))*(sj13)*(x1419)*(x1421)))+(((x1420)*(x1421)))+(((IkReal(-1.00000000000000))*(x1420)*(x1422)))+(((r22)*(x1423)))+(((sj11)*(x1418)))+(((cj11)*(sj13)*(x1422)))))), ((gconst41)*(((((x1422)*(x1423)))+(((x1422)*(x1424)))+(((r22)*(x1420)))+(((IkReal(-1.00000000000000))*(cj13)*(x1419)*(x1421)))+(((IkReal(-1.00000000000000))*(x1418)*(x1419)))+(((IkReal(-1.00000000000000))*(x1421)*(x1424)))))));
06144 sj10array[0]=IKsin(j10array[0]);
06145 cj10array[0]=IKcos(j10array[0]);
06146 if( j10array[0] > IKPI )
06147 {
06148 j10array[0]-=IK2PI;
06149 }
06150 else if( j10array[0] < -IKPI )
06151 { j10array[0]+=IK2PI;
06152 }
06153 j10valid[0] = true;
06154 for(int ij10 = 0; ij10 < 1; ++ij10)
06155 {
06156 if( !j10valid[ij10] )
06157 {
06158 continue;
06159 }
06160 _ij10[0] = ij10; _ij10[1] = -1;
06161 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
06162 {
06163 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
06164 {
06165 j10valid[iij10]=false; _ij10[1] = iij10; break;
06166 }
06167 }
06168 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
06169 {
06170 IkReal evalcond[2];
06171 IkReal x1425=IKsin(j10);
06172 IkReal x1426=IKcos(j10);
06173 IkReal x1427=((IkReal(1.00000000000000))*(sj13));
06174 IkReal x1428=((cj14)*(r20));
06175 IkReal x1429=((r21)*(sj14));
06176 IkReal x1430=((IkReal(1.00000000000000))*(x1426));
06177 evalcond[0]=((((IkReal(-1.00000000000000))*(cj11)*(x1430)))+(((IkReal(-1.00000000000000))*(r22)*(x1427)))+(((IkReal(-1.00000000000000))*(cj13)*(x1428)))+(((cj13)*(x1429)))+(((sj11)*(x1425))));
06178 evalcond[1]=((((IkReal(-1.00000000000000))*(x1427)*(x1428)))+(((IkReal(-1.00000000000000))*(sj11)*(x1430)))+(((sj13)*(x1429)))+(((IkReal(-1.00000000000000))*(cj11)*(x1425)))+(((cj13)*(r22))));
06179 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
06180 {
06181 continue;
06182 }
06183 }
06184
06185 {
06186 IkReal dummyeval[1];
06187 IkReal gconst44;
06188 IkReal x1431=(sj14)*(sj14);
06189 IkReal x1432=(cj14)*(cj14);
06190 IkReal x1433=((IkReal(1.00000000000000))*(x1431));
06191 IkReal x1434=((IkReal(2.00000000000000))*(cj14)*(sj14));
06192 IkReal x1435=((IkReal(1.00000000000000))*(x1432));
06193 gconst44=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1434)))+(((IkReal(-1.00000000000000))*(x1433)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1435)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1435)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1434)))+(((IkReal(-1.00000000000000))*(x1433)*((r00)*(r00))))));
06194 IkReal x1436=(sj14)*(sj14);
06195 IkReal x1437=(cj14)*(cj14);
06196 IkReal x1438=((IkReal(1.00000000000000))*(x1436));
06197 IkReal x1439=((IkReal(2.00000000000000))*(cj14)*(sj14));
06198 IkReal x1440=((IkReal(1.00000000000000))*(x1437));
06199 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1439)))+(((IkReal(-1.00000000000000))*(x1438)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1440)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1440)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1439)))+(((IkReal(-1.00000000000000))*(x1438)*((r00)*(r00)))));
06200 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06201 {
06202 {
06203 IkReal dummyeval[1];
06204 IkReal gconst45;
06205 IkReal x1441=(sj14)*(sj14);
06206 IkReal x1442=(cj14)*(cj14);
06207 IkReal x1443=((cj14)*(sj13));
06208 IkReal x1444=((IkReal(1.00000000000000))*(r11));
06209 IkReal x1445=((cj13)*(r00));
06210 IkReal x1446=((sj13)*(sj14));
06211 IkReal x1447=((cj13)*(r01)*(r10));
06212 gconst45=IKsign(((((x1442)*(x1447)))+(((x1441)*(x1447)))+(((r01)*(r12)*(x1443)))+(((IkReal(-1.00000000000000))*(x1442)*(x1444)*(x1445)))+(((IkReal(-1.00000000000000))*(r02)*(x1443)*(x1444)))+(((r00)*(r12)*(x1446)))+(((IkReal(-1.00000000000000))*(x1441)*(x1444)*(x1445)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1446)))));
06213 IkReal x1448=(sj14)*(sj14);
06214 IkReal x1449=(cj14)*(cj14);
06215 IkReal x1450=((cj14)*(sj13));
06216 IkReal x1451=((IkReal(1.00000000000000))*(r11));
06217 IkReal x1452=((cj13)*(r00));
06218 IkReal x1453=((sj13)*(sj14));
06219 IkReal x1454=((cj13)*(r01)*(r10));
06220 dummyeval[0]=((((r00)*(r12)*(x1453)))+(((IkReal(-1.00000000000000))*(x1449)*(x1451)*(x1452)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1453)))+(((x1448)*(x1454)))+(((IkReal(-1.00000000000000))*(x1448)*(x1451)*(x1452)))+(((r01)*(r12)*(x1450)))+(((IkReal(-1.00000000000000))*(r02)*(x1450)*(x1451)))+(((x1449)*(x1454))));
06221 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06222 {
06223 continue;
06224
06225 } else
06226 {
06227 {
06228 IkReal j9array[1], cj9array[1], sj9array[1];
06229 bool j9valid[1]={false};
06230 _nj9 = 1;
06231 IkReal x1455=((cj13)*(cj14));
06232 IkReal x1456=((IkReal(1.00000000000000))*(cj13)*(sj14));
06233 if( IKabs(((gconst45)*(((((r10)*(x1455)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1456))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst45)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1456)))+(((r00)*(x1455))))))) < IKFAST_ATAN2_MAGTHRESH )
06234 continue;
06235 j9array[0]=IKatan2(((gconst45)*(((((r10)*(x1455)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1456)))))), ((gconst45)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1456)))+(((r00)*(x1455)))))));
06236 sj9array[0]=IKsin(j9array[0]);
06237 cj9array[0]=IKcos(j9array[0]);
06238 if( j9array[0] > IKPI )
06239 {
06240 j9array[0]-=IK2PI;
06241 }
06242 else if( j9array[0] < -IKPI )
06243 { j9array[0]+=IK2PI;
06244 }
06245 j9valid[0] = true;
06246 for(int ij9 = 0; ij9 < 1; ++ij9)
06247 {
06248 if( !j9valid[ij9] )
06249 {
06250 continue;
06251 }
06252 _ij9[0] = ij9; _ij9[1] = -1;
06253 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
06254 {
06255 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
06256 {
06257 j9valid[iij9]=false; _ij9[1] = iij9; break;
06258 }
06259 }
06260 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
06261 {
06262 IkReal evalcond[6];
06263 IkReal x1457=IKsin(j9);
06264 IkReal x1458=IKcos(j9);
06265 IkReal x1459=((IkReal(1.00000000000000))*(cj14));
06266 IkReal x1460=((IkReal(1.00000000000000))*(sj14));
06267 IkReal x1461=((cj13)*(cj14));
06268 IkReal x1462=((cj14)*(r10));
06269 IkReal x1463=((r01)*(sj14));
06270 IkReal x1464=((IkReal(1.00000000000000))*(r12));
06271 IkReal x1465=((sj13)*(x1458));
06272 IkReal x1466=((r02)*(x1457));
06273 IkReal x1467=((r11)*(x1457));
06274 IkReal x1468=((r10)*(x1458));
06275 IkReal x1469=((r01)*(x1458));
06276 IkReal x1470=((sj13)*(x1457));
06277 IkReal x1471=((r11)*(x1458));
06278 IkReal x1472=((cj13)*(x1457));
06279 IkReal x1473=((r10)*(x1457));
06280 IkReal x1474=((r00)*(x1458));
06281 IkReal x1475=((cj13)*(x1458));
06282 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1460)*(x1468)))+(((r00)*(sj14)*(x1457)))+(((cj14)*(r01)*(x1457)))+(((IkReal(-1.00000000000000))*(x1459)*(x1471))));
06283 evalcond[1]=((((IkReal(-1.00000000000000))*(x1459)*(x1467)))+(((IkReal(-1.00000000000000))*(x1459)*(x1469)))+(((IkReal(-1.00000000000000))*(x1460)*(x1474)))+(((IkReal(-1.00000000000000))*(x1460)*(x1473))));
06284 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1472)))+(((x1461)*(x1468)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1471)))+(((IkReal(-1.00000000000000))*(sj13)*(x1466)))+(((x1463)*(x1472)))+(((r12)*(x1465))));
06285 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1470)))+(((cj13)*(x1466)))+(((IkReal(-1.00000000000000))*(r11)*(x1460)*(x1465)))+(((x1463)*(x1470)))+(((x1462)*(x1465)))+(((IkReal(-1.00000000000000))*(x1464)*(x1475))));
06286 evalcond[4]=((((r12)*(x1470)))+(((cj10)*(sj11)))+(((r02)*(x1465)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1469)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1467)))+(((cj11)*(sj10)))+(((x1461)*(x1474)))+(((x1461)*(x1473))));
06287 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x1460)*(x1465)))+(((IkReal(-1.00000000000000))*(sj13)*(x1460)*(x1467)))+(((x1462)*(x1470)))+(((IkReal(-1.00000000000000))*(r02)*(x1475)))+(((sj10)*(sj11)))+(((cj14)*(r00)*(x1465)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1464)*(x1472))));
06288 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 )
06289 {
06290 continue;
06291 }
06292 }
06293
06294 {
06295 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06296 vinfos[0].jointtype = 1;
06297 vinfos[0].foffset = j9;
06298 vinfos[0].indices[0] = _ij9[0];
06299 vinfos[0].indices[1] = _ij9[1];
06300 vinfos[0].maxsolutions = _nj9;
06301 vinfos[1].jointtype = 1;
06302 vinfos[1].foffset = j10;
06303 vinfos[1].indices[0] = _ij10[0];
06304 vinfos[1].indices[1] = _ij10[1];
06305 vinfos[1].maxsolutions = _nj10;
06306 vinfos[2].jointtype = 1;
06307 vinfos[2].foffset = j11;
06308 vinfos[2].indices[0] = _ij11[0];
06309 vinfos[2].indices[1] = _ij11[1];
06310 vinfos[2].maxsolutions = _nj11;
06311 vinfos[3].jointtype = 1;
06312 vinfos[3].foffset = j12;
06313 vinfos[3].indices[0] = _ij12[0];
06314 vinfos[3].indices[1] = _ij12[1];
06315 vinfos[3].maxsolutions = _nj12;
06316 vinfos[4].jointtype = 1;
06317 vinfos[4].foffset = j13;
06318 vinfos[4].indices[0] = _ij13[0];
06319 vinfos[4].indices[1] = _ij13[1];
06320 vinfos[4].maxsolutions = _nj13;
06321 vinfos[5].jointtype = 1;
06322 vinfos[5].foffset = j14;
06323 vinfos[5].indices[0] = _ij14[0];
06324 vinfos[5].indices[1] = _ij14[1];
06325 vinfos[5].maxsolutions = _nj14;
06326 std::vector<int> vfree(0);
06327 solutions.AddSolution(vinfos,vfree);
06328 }
06329 }
06330 }
06331
06332 }
06333
06334 }
06335
06336 } else
06337 {
06338 {
06339 IkReal j9array[1], cj9array[1], sj9array[1];
06340 bool j9valid[1]={false};
06341 _nj9 = 1;
06342 if( IKabs(((gconst44)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
06343 continue;
06344 j9array[0]=IKatan2(((gconst44)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst44)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
06345 sj9array[0]=IKsin(j9array[0]);
06346 cj9array[0]=IKcos(j9array[0]);
06347 if( j9array[0] > IKPI )
06348 {
06349 j9array[0]-=IK2PI;
06350 }
06351 else if( j9array[0] < -IKPI )
06352 { j9array[0]+=IK2PI;
06353 }
06354 j9valid[0] = true;
06355 for(int ij9 = 0; ij9 < 1; ++ij9)
06356 {
06357 if( !j9valid[ij9] )
06358 {
06359 continue;
06360 }
06361 _ij9[0] = ij9; _ij9[1] = -1;
06362 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
06363 {
06364 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
06365 {
06366 j9valid[iij9]=false; _ij9[1] = iij9; break;
06367 }
06368 }
06369 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
06370 {
06371 IkReal evalcond[6];
06372 IkReal x1476=IKsin(j9);
06373 IkReal x1477=IKcos(j9);
06374 IkReal x1478=((IkReal(1.00000000000000))*(cj14));
06375 IkReal x1479=((IkReal(1.00000000000000))*(sj14));
06376 IkReal x1480=((cj13)*(cj14));
06377 IkReal x1481=((cj14)*(r10));
06378 IkReal x1482=((r01)*(sj14));
06379 IkReal x1483=((IkReal(1.00000000000000))*(r12));
06380 IkReal x1484=((sj13)*(x1477));
06381 IkReal x1485=((r02)*(x1476));
06382 IkReal x1486=((r11)*(x1476));
06383 IkReal x1487=((r10)*(x1477));
06384 IkReal x1488=((r01)*(x1477));
06385 IkReal x1489=((sj13)*(x1476));
06386 IkReal x1490=((r11)*(x1477));
06387 IkReal x1491=((cj13)*(x1476));
06388 IkReal x1492=((r10)*(x1476));
06389 IkReal x1493=((r00)*(x1477));
06390 IkReal x1494=((cj13)*(x1477));
06391 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1479)*(x1487)))+(((r00)*(sj14)*(x1476)))+(((cj14)*(r01)*(x1476)))+(((IkReal(-1.00000000000000))*(x1478)*(x1490))));
06392 evalcond[1]=((((IkReal(-1.00000000000000))*(x1479)*(x1493)))+(((IkReal(-1.00000000000000))*(x1479)*(x1492)))+(((IkReal(-1.00000000000000))*(x1478)*(x1486)))+(((IkReal(-1.00000000000000))*(x1478)*(x1488))));
06393 evalcond[2]=((((x1482)*(x1491)))+(((IkReal(-1.00000000000000))*(sj13)*(x1485)))+(((x1480)*(x1487)))+(((IkReal(-1.00000000000000))*(r00)*(x1478)*(x1491)))+(((r12)*(x1484)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1490))));
06394 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1478)*(x1489)))+(((x1482)*(x1489)))+(((cj13)*(x1485)))+(((x1481)*(x1484)))+(((IkReal(-1.00000000000000))*(x1483)*(x1494)))+(((IkReal(-1.00000000000000))*(r11)*(x1479)*(x1484))));
06395 evalcond[4]=((((r12)*(x1489)))+(((x1480)*(x1492)))+(((x1480)*(x1493)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1488)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1486)))+(((r02)*(x1484)))+(((cj11)*(sj10))));
06396 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x1494)))+(((cj14)*(r00)*(x1484)))+(((x1481)*(x1489)))+(((IkReal(-1.00000000000000))*(sj13)*(x1479)*(x1486)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1483)*(x1491)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(r01)*(x1479)*(x1484))));
06397 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 )
06398 {
06399 continue;
06400 }
06401 }
06402
06403 {
06404 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06405 vinfos[0].jointtype = 1;
06406 vinfos[0].foffset = j9;
06407 vinfos[0].indices[0] = _ij9[0];
06408 vinfos[0].indices[1] = _ij9[1];
06409 vinfos[0].maxsolutions = _nj9;
06410 vinfos[1].jointtype = 1;
06411 vinfos[1].foffset = j10;
06412 vinfos[1].indices[0] = _ij10[0];
06413 vinfos[1].indices[1] = _ij10[1];
06414 vinfos[1].maxsolutions = _nj10;
06415 vinfos[2].jointtype = 1;
06416 vinfos[2].foffset = j11;
06417 vinfos[2].indices[0] = _ij11[0];
06418 vinfos[2].indices[1] = _ij11[1];
06419 vinfos[2].maxsolutions = _nj11;
06420 vinfos[3].jointtype = 1;
06421 vinfos[3].foffset = j12;
06422 vinfos[3].indices[0] = _ij12[0];
06423 vinfos[3].indices[1] = _ij12[1];
06424 vinfos[3].maxsolutions = _nj12;
06425 vinfos[4].jointtype = 1;
06426 vinfos[4].foffset = j13;
06427 vinfos[4].indices[0] = _ij13[0];
06428 vinfos[4].indices[1] = _ij13[1];
06429 vinfos[4].maxsolutions = _nj13;
06430 vinfos[5].jointtype = 1;
06431 vinfos[5].foffset = j14;
06432 vinfos[5].indices[0] = _ij14[0];
06433 vinfos[5].indices[1] = _ij14[1];
06434 vinfos[5].maxsolutions = _nj14;
06435 std::vector<int> vfree(0);
06436 solutions.AddSolution(vinfos,vfree);
06437 }
06438 }
06439 }
06440
06441 }
06442
06443 }
06444 }
06445 }
06446
06447 }
06448
06449 }
06450
06451 } else
06452 {
06453 {
06454 IkReal j9array[1], cj9array[1], sj9array[1];
06455 bool j9valid[1]={false};
06456 _nj9 = 1;
06457 if( IKabs(((gconst39)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst39)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
06458 continue;
06459 j9array[0]=IKatan2(((gconst39)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst39)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
06460 sj9array[0]=IKsin(j9array[0]);
06461 cj9array[0]=IKcos(j9array[0]);
06462 if( j9array[0] > IKPI )
06463 {
06464 j9array[0]-=IK2PI;
06465 }
06466 else if( j9array[0] < -IKPI )
06467 { j9array[0]+=IK2PI;
06468 }
06469 j9valid[0] = true;
06470 for(int ij9 = 0; ij9 < 1; ++ij9)
06471 {
06472 if( !j9valid[ij9] )
06473 {
06474 continue;
06475 }
06476 _ij9[0] = ij9; _ij9[1] = -1;
06477 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
06478 {
06479 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
06480 {
06481 j9valid[iij9]=false; _ij9[1] = iij9; break;
06482 }
06483 }
06484 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
06485 {
06486 IkReal evalcond[4];
06487 IkReal x1495=IKsin(j9);
06488 IkReal x1496=IKcos(j9);
06489 IkReal x1497=((r10)*(sj14));
06490 IkReal x1498=((cj14)*(r11));
06491 IkReal x1499=((cj14)*(r10));
06492 IkReal x1500=((cj14)*(r00));
06493 IkReal x1501=((r11)*(sj14));
06494 IkReal x1502=((r00)*(sj14));
06495 IkReal x1503=((IkReal(1.00000000000000))*(x1495));
06496 IkReal x1504=((cj13)*(x1495));
06497 IkReal x1505=((sj13)*(x1496));
06498 IkReal x1506=((r01)*(x1495));
06499 IkReal x1507=((IkReal(1.00000000000000))*(x1496));
06500 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1498)*(x1507)))+(((x1495)*(x1502)))+(((IkReal(-1.00000000000000))*(x1497)*(x1507)))+(((cj14)*(x1506))));
06501 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1507)))+(((IkReal(-1.00000000000000))*(x1498)*(x1503)))+(((IkReal(-1.00000000000000))*(x1497)*(x1503)))+(((IkReal(-1.00000000000000))*(x1502)*(x1507))));
06502 evalcond[2]=((((cj13)*(x1496)*(x1499)))+(((r12)*(x1505)))+(((IkReal(-1.00000000000000))*(cj13)*(x1501)*(x1507)))+(((r01)*(sj14)*(x1504)))+(((IkReal(-1.00000000000000))*(cj13)*(x1500)*(x1503)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1503))));
06503 evalcond[3]=((((x1499)*(x1505)))+(((sj13)*(sj14)*(x1506)))+(((r02)*(x1504)))+(((IkReal(-1.00000000000000))*(sj13)*(x1500)*(x1503)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1507)))+(((IkReal(-1.00000000000000))*(x1501)*(x1505))));
06504 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06505 {
06506 continue;
06507 }
06508 }
06509
06510 {
06511 IkReal dummyeval[1];
06512 IkReal gconst42;
06513 gconst42=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
06514 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
06515 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06516 {
06517 {
06518 IkReal dummyeval[1];
06519 IkReal gconst43;
06520 gconst43=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
06521 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
06522 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06523 {
06524 continue;
06525
06526 } else
06527 {
06528 {
06529 IkReal j10array[1], cj10array[1], sj10array[1];
06530 bool j10valid[1]={false};
06531 _nj10 = 1;
06532 IkReal x1508=((cj13)*(sj14));
06533 IkReal x1509=((IkReal(1.00000000000000))*(cj11));
06534 IkReal x1510=((r11)*(sj9));
06535 IkReal x1511=((IkReal(1.00000000000000))*(sj11));
06536 IkReal x1512=((cj13)*(cj14));
06537 IkReal x1513=((cj11)*(sj13));
06538 IkReal x1514=((r12)*(sj9));
06539 IkReal x1515=((cj9)*(r01));
06540 IkReal x1516=((sj11)*(sj13));
06541 IkReal x1517=((cj9)*(r02));
06542 IkReal x1518=((r10)*(sj9));
06543 IkReal x1519=((cj9)*(r00));
06544 if( IKabs(((gconst43)*(((((x1513)*(x1514)))+(((x1513)*(x1517)))+(((r21)*(sj11)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1515)))+(((IkReal(-1.00000000000000))*(r20)*(x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1511)))+(((cj11)*(x1512)*(x1518)))+(((cj11)*(x1512)*(x1519))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((x1514)*(x1516)))+(((x1516)*(x1517)))+(((cj11)*(r20)*(x1512)))+(((IkReal(-1.00000000000000))*(r21)*(x1508)*(x1509)))+(((r22)*(x1513)))+(((sj11)*(x1512)*(x1519)))+(((sj11)*(x1512)*(x1518)))+(((IkReal(-1.00000000000000))*(x1508)*(x1510)*(x1511)))+(((IkReal(-1.00000000000000))*(x1508)*(x1511)*(x1515))))))) < IKFAST_ATAN2_MAGTHRESH )
06545 continue;
06546 j10array[0]=IKatan2(((gconst43)*(((((x1513)*(x1514)))+(((x1513)*(x1517)))+(((r21)*(sj11)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1515)))+(((IkReal(-1.00000000000000))*(r20)*(x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1511)))+(((cj11)*(x1512)*(x1518)))+(((cj11)*(x1512)*(x1519)))))), ((gconst43)*(((((x1514)*(x1516)))+(((x1516)*(x1517)))+(((cj11)*(r20)*(x1512)))+(((IkReal(-1.00000000000000))*(r21)*(x1508)*(x1509)))+(((r22)*(x1513)))+(((sj11)*(x1512)*(x1519)))+(((sj11)*(x1512)*(x1518)))+(((IkReal(-1.00000000000000))*(x1508)*(x1510)*(x1511)))+(((IkReal(-1.00000000000000))*(x1508)*(x1511)*(x1515)))))));
06547 sj10array[0]=IKsin(j10array[0]);
06548 cj10array[0]=IKcos(j10array[0]);
06549 if( j10array[0] > IKPI )
06550 {
06551 j10array[0]-=IK2PI;
06552 }
06553 else if( j10array[0] < -IKPI )
06554 { j10array[0]+=IK2PI;
06555 }
06556 j10valid[0] = true;
06557 for(int ij10 = 0; ij10 < 1; ++ij10)
06558 {
06559 if( !j10valid[ij10] )
06560 {
06561 continue;
06562 }
06563 _ij10[0] = ij10; _ij10[1] = -1;
06564 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
06565 {
06566 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
06567 {
06568 j10valid[iij10]=false; _ij10[1] = iij10; break;
06569 }
06570 }
06571 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
06572 {
06573 IkReal evalcond[4];
06574 IkReal x1520=IKsin(j10);
06575 IkReal x1521=IKcos(j10);
06576 IkReal x1522=((IkReal(1.00000000000000))*(sj13));
06577 IkReal x1523=((r11)*(sj9));
06578 IkReal x1524=((cj9)*(r01));
06579 IkReal x1525=((IkReal(1.00000000000000))*(cj11));
06580 IkReal x1526=((r21)*(sj14));
06581 IkReal x1527=((cj9)*(r02));
06582 IkReal x1528=((sj13)*(sj9));
06583 IkReal x1529=((cj14)*(r10));
06584 IkReal x1530=((IkReal(1.00000000000000))*(cj13));
06585 IkReal x1531=((cj14)*(r20));
06586 IkReal x1532=((sj11)*(x1520));
06587 IkReal x1533=((sj14)*(x1530));
06588 IkReal x1534=((sj11)*(x1521));
06589 IkReal x1535=((cj14)*(cj9)*(r00));
06590 IkReal x1536=((x1521)*(x1525));
06591 evalcond[0]=((((IkReal(-1.00000000000000))*(x1530)*(x1531)))+(((IkReal(-1.00000000000000))*(x1536)))+(((IkReal(-1.00000000000000))*(r22)*(x1522)))+(x1532)+(((cj13)*(x1526))));
06592 evalcond[1]=((((IkReal(-1.00000000000000))*(x1534)))+(((sj13)*(x1526)))+(((IkReal(-1.00000000000000))*(x1522)*(x1531)))+(((IkReal(-1.00000000000000))*(x1520)*(x1525)))+(((cj13)*(r22))));
06593 evalcond[2]=((((IkReal(-1.00000000000000))*(x1524)*(x1533)))+(((cj11)*(x1520)))+(((r12)*(x1528)))+(((cj13)*(sj9)*(x1529)))+(x1534)+(((cj13)*(x1535)))+(((sj13)*(x1527)))+(((IkReal(-1.00000000000000))*(x1523)*(x1533))));
06594 evalcond[3]=((((x1528)*(x1529)))+(((IkReal(-1.00000000000000))*(x1536)))+(x1532)+(((IkReal(-1.00000000000000))*(sj14)*(x1522)*(x1523)))+(((IkReal(-1.00000000000000))*(sj14)*(x1522)*(x1524)))+(((sj13)*(x1535)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1530)))+(((IkReal(-1.00000000000000))*(x1527)*(x1530))));
06595 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06596 {
06597 continue;
06598 }
06599 }
06600
06601 {
06602 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06603 vinfos[0].jointtype = 1;
06604 vinfos[0].foffset = j9;
06605 vinfos[0].indices[0] = _ij9[0];
06606 vinfos[0].indices[1] = _ij9[1];
06607 vinfos[0].maxsolutions = _nj9;
06608 vinfos[1].jointtype = 1;
06609 vinfos[1].foffset = j10;
06610 vinfos[1].indices[0] = _ij10[0];
06611 vinfos[1].indices[1] = _ij10[1];
06612 vinfos[1].maxsolutions = _nj10;
06613 vinfos[2].jointtype = 1;
06614 vinfos[2].foffset = j11;
06615 vinfos[2].indices[0] = _ij11[0];
06616 vinfos[2].indices[1] = _ij11[1];
06617 vinfos[2].maxsolutions = _nj11;
06618 vinfos[3].jointtype = 1;
06619 vinfos[3].foffset = j12;
06620 vinfos[3].indices[0] = _ij12[0];
06621 vinfos[3].indices[1] = _ij12[1];
06622 vinfos[3].maxsolutions = _nj12;
06623 vinfos[4].jointtype = 1;
06624 vinfos[4].foffset = j13;
06625 vinfos[4].indices[0] = _ij13[0];
06626 vinfos[4].indices[1] = _ij13[1];
06627 vinfos[4].maxsolutions = _nj13;
06628 vinfos[5].jointtype = 1;
06629 vinfos[5].foffset = j14;
06630 vinfos[5].indices[0] = _ij14[0];
06631 vinfos[5].indices[1] = _ij14[1];
06632 vinfos[5].maxsolutions = _nj14;
06633 std::vector<int> vfree(0);
06634 solutions.AddSolution(vinfos,vfree);
06635 }
06636 }
06637 }
06638
06639 }
06640
06641 }
06642
06643 } else
06644 {
06645 {
06646 IkReal j10array[1], cj10array[1], sj10array[1];
06647 bool j10valid[1]={false};
06648 _nj10 = 1;
06649 IkReal x1537=((cj11)*(r22));
06650 IkReal x1538=((IkReal(1.00000000000000))*(sj13));
06651 IkReal x1539=((cj13)*(sj11));
06652 IkReal x1540=((r21)*(sj14));
06653 IkReal x1541=((cj11)*(cj13));
06654 IkReal x1542=((sj11)*(sj13));
06655 IkReal x1543=((cj14)*(r20));
06656 if( IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1539)*(x1540)))+(((cj11)*(sj13)*(x1540)))+(((x1539)*(x1543)))+(((IkReal(-1.00000000000000))*(cj11)*(x1538)*(x1543)))+(((cj13)*(x1537)))+(((r22)*(x1542))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1541)*(x1543)))+(((IkReal(-1.00000000000000))*(x1537)*(x1538)))+(((x1540)*(x1541)))+(((x1540)*(x1542)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(sj11)*(x1538)*(x1543))))))) < IKFAST_ATAN2_MAGTHRESH )
06657 continue;
06658 j10array[0]=IKatan2(((gconst42)*(((((IkReal(-1.00000000000000))*(x1539)*(x1540)))+(((cj11)*(sj13)*(x1540)))+(((x1539)*(x1543)))+(((IkReal(-1.00000000000000))*(cj11)*(x1538)*(x1543)))+(((cj13)*(x1537)))+(((r22)*(x1542)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1541)*(x1543)))+(((IkReal(-1.00000000000000))*(x1537)*(x1538)))+(((x1540)*(x1541)))+(((x1540)*(x1542)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(sj11)*(x1538)*(x1543)))))));
06659 sj10array[0]=IKsin(j10array[0]);
06660 cj10array[0]=IKcos(j10array[0]);
06661 if( j10array[0] > IKPI )
06662 {
06663 j10array[0]-=IK2PI;
06664 }
06665 else if( j10array[0] < -IKPI )
06666 { j10array[0]+=IK2PI;
06667 }
06668 j10valid[0] = true;
06669 for(int ij10 = 0; ij10 < 1; ++ij10)
06670 {
06671 if( !j10valid[ij10] )
06672 {
06673 continue;
06674 }
06675 _ij10[0] = ij10; _ij10[1] = -1;
06676 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
06677 {
06678 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
06679 {
06680 j10valid[iij10]=false; _ij10[1] = iij10; break;
06681 }
06682 }
06683 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
06684 {
06685 IkReal evalcond[4];
06686 IkReal x1544=IKsin(j10);
06687 IkReal x1545=IKcos(j10);
06688 IkReal x1546=((IkReal(1.00000000000000))*(sj13));
06689 IkReal x1547=((r11)*(sj9));
06690 IkReal x1548=((cj9)*(r01));
06691 IkReal x1549=((IkReal(1.00000000000000))*(cj11));
06692 IkReal x1550=((r21)*(sj14));
06693 IkReal x1551=((cj9)*(r02));
06694 IkReal x1552=((sj13)*(sj9));
06695 IkReal x1553=((cj14)*(r10));
06696 IkReal x1554=((IkReal(1.00000000000000))*(cj13));
06697 IkReal x1555=((cj14)*(r20));
06698 IkReal x1556=((sj11)*(x1544));
06699 IkReal x1557=((sj14)*(x1554));
06700 IkReal x1558=((sj11)*(x1545));
06701 IkReal x1559=((cj14)*(cj9)*(r00));
06702 IkReal x1560=((x1545)*(x1549));
06703 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1546)))+(x1556)+(((cj13)*(x1550)))+(((IkReal(-1.00000000000000))*(x1560)))+(((IkReal(-1.00000000000000))*(x1554)*(x1555))));
06704 evalcond[1]=((((IkReal(-1.00000000000000))*(x1558)))+(((IkReal(-1.00000000000000))*(x1546)*(x1555)))+(((sj13)*(x1550)))+(((IkReal(-1.00000000000000))*(x1544)*(x1549)))+(((cj13)*(r22))));
06705 evalcond[2]=((((sj13)*(x1551)))+(((cj11)*(x1544)))+(((IkReal(-1.00000000000000))*(x1548)*(x1557)))+(((r12)*(x1552)))+(x1558)+(((cj13)*(x1559)))+(((IkReal(-1.00000000000000))*(x1547)*(x1557)))+(((cj13)*(sj9)*(x1553))));
06706 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x1546)*(x1548)))+(((IkReal(-1.00000000000000))*(sj14)*(x1546)*(x1547)))+(((x1552)*(x1553)))+(((sj13)*(x1559)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1554)))+(x1556)+(((IkReal(-1.00000000000000))*(x1551)*(x1554)))+(((IkReal(-1.00000000000000))*(x1560))));
06707 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06708 {
06709 continue;
06710 }
06711 }
06712
06713 {
06714 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06715 vinfos[0].jointtype = 1;
06716 vinfos[0].foffset = j9;
06717 vinfos[0].indices[0] = _ij9[0];
06718 vinfos[0].indices[1] = _ij9[1];
06719 vinfos[0].maxsolutions = _nj9;
06720 vinfos[1].jointtype = 1;
06721 vinfos[1].foffset = j10;
06722 vinfos[1].indices[0] = _ij10[0];
06723 vinfos[1].indices[1] = _ij10[1];
06724 vinfos[1].maxsolutions = _nj10;
06725 vinfos[2].jointtype = 1;
06726 vinfos[2].foffset = j11;
06727 vinfos[2].indices[0] = _ij11[0];
06728 vinfos[2].indices[1] = _ij11[1];
06729 vinfos[2].maxsolutions = _nj11;
06730 vinfos[3].jointtype = 1;
06731 vinfos[3].foffset = j12;
06732 vinfos[3].indices[0] = _ij12[0];
06733 vinfos[3].indices[1] = _ij12[1];
06734 vinfos[3].maxsolutions = _nj12;
06735 vinfos[4].jointtype = 1;
06736 vinfos[4].foffset = j13;
06737 vinfos[4].indices[0] = _ij13[0];
06738 vinfos[4].indices[1] = _ij13[1];
06739 vinfos[4].maxsolutions = _nj13;
06740 vinfos[5].jointtype = 1;
06741 vinfos[5].foffset = j14;
06742 vinfos[5].indices[0] = _ij14[0];
06743 vinfos[5].indices[1] = _ij14[1];
06744 vinfos[5].maxsolutions = _nj14;
06745 std::vector<int> vfree(0);
06746 solutions.AddSolution(vinfos,vfree);
06747 }
06748 }
06749 }
06750
06751 }
06752
06753 }
06754 }
06755 }
06756
06757 }
06758
06759 }
06760
06761 } else
06762 {
06763 if( 1 )
06764 {
06765 continue;
06766
06767 } else
06768 {
06769 }
06770 }
06771 }
06772 }
06773 }
06774 }
06775
06776 } else
06777 {
06778 {
06779 IkReal j9array[1], cj9array[1], sj9array[1];
06780 bool j9valid[1]={false};
06781 _nj9 = 1;
06782 IkReal x1561=((cj12)*(sj13));
06783 IkReal x1562=((cj12)*(cj13));
06784 IkReal x1563=((IkReal(1.00000000000000))*(sj14));
06785 if( IKabs(((gconst2)*(((((r12)*(x1561)))+(((IkReal(-1.00000000000000))*(r11)*(x1562)*(x1563)))+(((cj14)*(r10)*(x1562))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(r01)*(x1562)*(x1563)))+(((cj14)*(r00)*(x1562)))+(((r02)*(x1561))))))) < IKFAST_ATAN2_MAGTHRESH )
06786 continue;
06787 j9array[0]=IKatan2(((gconst2)*(((((r12)*(x1561)))+(((IkReal(-1.00000000000000))*(r11)*(x1562)*(x1563)))+(((cj14)*(r10)*(x1562)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(r01)*(x1562)*(x1563)))+(((cj14)*(r00)*(x1562)))+(((r02)*(x1561)))))));
06788 sj9array[0]=IKsin(j9array[0]);
06789 cj9array[0]=IKcos(j9array[0]);
06790 if( j9array[0] > IKPI )
06791 {
06792 j9array[0]-=IK2PI;
06793 }
06794 else if( j9array[0] < -IKPI )
06795 { j9array[0]+=IK2PI;
06796 }
06797 j9valid[0] = true;
06798 for(int ij9 = 0; ij9 < 1; ++ij9)
06799 {
06800 if( !j9valid[ij9] )
06801 {
06802 continue;
06803 }
06804 _ij9[0] = ij9; _ij9[1] = -1;
06805 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
06806 {
06807 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
06808 {
06809 j9valid[iij9]=false; _ij9[1] = iij9; break;
06810 }
06811 }
06812 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
06813 {
06814 IkReal evalcond[3];
06815 IkReal x1564=IKsin(j9);
06816 IkReal x1565=IKcos(j9);
06817 IkReal x1566=((IkReal(1.00000000000000))*(sj14));
06818 IkReal x1567=((sj13)*(x1565));
06819 IkReal x1568=((cj13)*(x1564));
06820 IkReal x1569=((IkReal(1.00000000000000))*(cj14)*(r00));
06821 IkReal x1570=((r01)*(x1564));
06822 IkReal x1571=((r10)*(x1565));
06823 IkReal x1572=((sj13)*(x1564));
06824 IkReal x1573=((cj13)*(x1565));
06825 evalcond[0]=((((IkReal(-1.00000000000000))*(x1566)*(x1571)))+(cj12)+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1565)))+(((r00)*(sj14)*(x1564)))+(((cj14)*(x1570))));
06826 evalcond[1]=((((r01)*(sj14)*(x1568)))+(((cj13)*(cj14)*(x1571)))+(((r12)*(x1567)))+(((IkReal(-1.00000000000000))*(x1568)*(x1569)))+(((IkReal(-1.00000000000000))*(r11)*(x1566)*(x1573)))+(((IkReal(-1.00000000000000))*(r02)*(x1572))));
06827 evalcond[2]=((sj12)+(((IkReal(-1.00000000000000))*(x1569)*(x1572)))+(((cj14)*(r10)*(x1567)))+(((sj13)*(sj14)*(x1570)))+(((IkReal(-1.00000000000000))*(r12)*(x1573)))+(((IkReal(-1.00000000000000))*(r11)*(x1566)*(x1567)))+(((r02)*(x1568))));
06828 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
06829 {
06830 continue;
06831 }
06832 }
06833
06834 {
06835 IkReal dummyeval[1];
06836 IkReal gconst6;
06837 gconst6=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
06838 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
06839 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06840 {
06841 {
06842 IkReal dummyeval[1];
06843 IkReal gconst7;
06844 gconst7=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
06845 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
06846 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06847 {
06848 {
06849 IkReal evalcond[9];
06850 IkReal x1574=((r00)*(sj9));
06851 IkReal x1575=((IkReal(1.00000000000000))*(sj13));
06852 IkReal x1576=((cj13)*(sj14));
06853 IkReal x1577=((cj9)*(sj14));
06854 IkReal x1578=((cj13)*(r02));
06855 IkReal x1579=((sj13)*(sj14));
06856 IkReal x1580=((r01)*(sj9));
06857 IkReal x1581=((cj9)*(sj13));
06858 IkReal x1582=((IkReal(1.00000000000000))*(cj9));
06859 IkReal x1583=((cj14)*(r10));
06860 IkReal x1584=((cj14)*(npx));
06861 IkReal x1585=((IkReal(1.00000000000000))*(cj13));
06862 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
06863 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
06864 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x1575)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x1584)*(x1585)))+(((npy)*(x1576))));
06865 evalcond[3]=((((sj14)*(x1574)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1582)))+(((IkReal(-1.00000000000000))*(r10)*(x1577)))+(((cj14)*(x1580))));
06866 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x1575)))+(((r21)*(x1579)))+(((cj13)*(r22))));
06867 evalcond[5]=((IkReal(0.0950000000000000))+(((npy)*(x1579)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x1575)*(x1584))));
06868 evalcond[6]=((((cj13)*(cj9)*(x1583)))+(((IkReal(-1.00000000000000))*(cj14)*(x1574)*(x1585)))+(((x1576)*(x1580)))+(((r12)*(x1581)))+(((IkReal(-1.00000000000000))*(r11)*(x1576)*(x1582)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x1575))));
06869 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1582)))+(((x1579)*(x1580)))+(((IkReal(-1.00000000000000))*(r11)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(cj14)*(x1574)*(x1575)))+(((sj9)*(x1578)))+(((x1581)*(x1583))));
06870 evalcond[8]=((((IkReal(-1.00000000000000))*(x1578)*(x1582)))+(((IkReal(-1.00000000000000))*(r01)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x1575)))+(((sj13)*(sj9)*(x1583)))+(((cj14)*(r00)*(x1581)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1585))));
06871 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 )
06872 {
06873 {
06874 IkReal dummyeval[1];
06875 IkReal gconst8;
06876 gconst8=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
06877 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
06878 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06879 {
06880 {
06881 IkReal dummyeval[1];
06882 IkReal gconst9;
06883 gconst9=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
06884 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
06885 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
06886 {
06887 continue;
06888
06889 } else
06890 {
06891 {
06892 IkReal j10array[1], cj10array[1], sj10array[1];
06893 bool j10valid[1]={false};
06894 _nj10 = 1;
06895 IkReal x1586=((IkReal(1.00000000000000))*(cj11));
06896 IkReal x1587=((r20)*(sj14));
06897 IkReal x1588=((cj14)*(r21));
06898 IkReal x1589=((cj14)*(cj9)*(r01));
06899 IkReal x1590=((r10)*(sj14)*(sj9));
06900 IkReal x1591=((cj9)*(r00)*(sj14));
06901 IkReal x1592=((cj14)*(r11)*(sj9));
06902 if( IKabs(((gconst9)*(((((sj11)*(x1592)))+(((sj11)*(x1590)))+(((sj11)*(x1591)))+(((sj11)*(x1589)))+(((cj11)*(x1588)))+(((cj11)*(x1587))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((sj11)*(x1588)))+(((sj11)*(x1587)))+(((IkReal(-1.00000000000000))*(x1586)*(x1591)))+(((IkReal(-1.00000000000000))*(x1586)*(x1590)))+(((IkReal(-1.00000000000000))*(x1586)*(x1592)))+(((IkReal(-1.00000000000000))*(x1586)*(x1589))))))) < IKFAST_ATAN2_MAGTHRESH )
06903 continue;
06904 j10array[0]=IKatan2(((gconst9)*(((((sj11)*(x1592)))+(((sj11)*(x1590)))+(((sj11)*(x1591)))+(((sj11)*(x1589)))+(((cj11)*(x1588)))+(((cj11)*(x1587)))))), ((gconst9)*(((((sj11)*(x1588)))+(((sj11)*(x1587)))+(((IkReal(-1.00000000000000))*(x1586)*(x1591)))+(((IkReal(-1.00000000000000))*(x1586)*(x1590)))+(((IkReal(-1.00000000000000))*(x1586)*(x1592)))+(((IkReal(-1.00000000000000))*(x1586)*(x1589)))))));
06905 sj10array[0]=IKsin(j10array[0]);
06906 cj10array[0]=IKcos(j10array[0]);
06907 if( j10array[0] > IKPI )
06908 {
06909 j10array[0]-=IK2PI;
06910 }
06911 else if( j10array[0] < -IKPI )
06912 { j10array[0]+=IK2PI;
06913 }
06914 j10valid[0] = true;
06915 for(int ij10 = 0; ij10 < 1; ++ij10)
06916 {
06917 if( !j10valid[ij10] )
06918 {
06919 continue;
06920 }
06921 _ij10[0] = ij10; _ij10[1] = -1;
06922 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
06923 {
06924 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
06925 {
06926 j10valid[iij10]=false; _ij10[1] = iij10; break;
06927 }
06928 }
06929 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
06930 {
06931 IkReal evalcond[4];
06932 IkReal x1593=IKsin(j10);
06933 IkReal x1594=IKcos(j10);
06934 IkReal x1595=((cj13)*(sj14));
06935 IkReal x1596=((cj13)*(cj14));
06936 IkReal x1597=((r10)*(sj9));
06937 IkReal x1598=((IkReal(1.00000000000000))*(cj9));
06938 IkReal x1599=((sj11)*(x1593));
06939 IkReal x1600=((IkReal(1.00000000000000))*(x1594));
06940 IkReal x1601=((cj11)*(x1593));
06941 IkReal x1602=((IkReal(1.00000000000000))*(r11)*(sj9));
06942 IkReal x1603=((cj11)*(x1600));
06943 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x1601)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x1600))));
06944 evalcond[1]=((((IkReal(-1.00000000000000))*(x1603)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(x1599)+(((r21)*(x1595)))+(((IkReal(-1.00000000000000))*(r20)*(x1596))));
06945 evalcond[2]=((((IkReal(-1.00000000000000))*(x1603)))+(((IkReal(-1.00000000000000))*(cj14)*(x1602)))+(x1599)+(((IkReal(-1.00000000000000))*(sj14)*(x1597)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1598)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1598))));
06946 evalcond[3]=((((x1596)*(x1597)))+(((cj9)*(r00)*(x1596)))+(((sj11)*(x1594)))+(x1601)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(x1595)*(x1602)))+(((IkReal(-1.00000000000000))*(r01)*(x1595)*(x1598)))+(((r12)*(sj13)*(sj9))));
06947 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
06948 {
06949 continue;
06950 }
06951 }
06952
06953 {
06954 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
06955 vinfos[0].jointtype = 1;
06956 vinfos[0].foffset = j9;
06957 vinfos[0].indices[0] = _ij9[0];
06958 vinfos[0].indices[1] = _ij9[1];
06959 vinfos[0].maxsolutions = _nj9;
06960 vinfos[1].jointtype = 1;
06961 vinfos[1].foffset = j10;
06962 vinfos[1].indices[0] = _ij10[0];
06963 vinfos[1].indices[1] = _ij10[1];
06964 vinfos[1].maxsolutions = _nj10;
06965 vinfos[2].jointtype = 1;
06966 vinfos[2].foffset = j11;
06967 vinfos[2].indices[0] = _ij11[0];
06968 vinfos[2].indices[1] = _ij11[1];
06969 vinfos[2].maxsolutions = _nj11;
06970 vinfos[3].jointtype = 1;
06971 vinfos[3].foffset = j12;
06972 vinfos[3].indices[0] = _ij12[0];
06973 vinfos[3].indices[1] = _ij12[1];
06974 vinfos[3].maxsolutions = _nj12;
06975 vinfos[4].jointtype = 1;
06976 vinfos[4].foffset = j13;
06977 vinfos[4].indices[0] = _ij13[0];
06978 vinfos[4].indices[1] = _ij13[1];
06979 vinfos[4].maxsolutions = _nj13;
06980 vinfos[5].jointtype = 1;
06981 vinfos[5].foffset = j14;
06982 vinfos[5].indices[0] = _ij14[0];
06983 vinfos[5].indices[1] = _ij14[1];
06984 vinfos[5].maxsolutions = _nj14;
06985 std::vector<int> vfree(0);
06986 solutions.AddSolution(vinfos,vfree);
06987 }
06988 }
06989 }
06990
06991 }
06992
06993 }
06994
06995 } else
06996 {
06997 {
06998 IkReal j10array[1], cj10array[1], sj10array[1];
06999 bool j10valid[1]={false};
07000 _nj10 = 1;
07001 IkReal x1604=((cj11)*(r20));
07002 IkReal x1605=((IkReal(1.00000000000000))*(cj13));
07003 IkReal x1606=((r21)*(sj14));
07004 IkReal x1607=((r22)*(sj13));
07005 IkReal x1608=((cj14)*(sj11));
07006 if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(sj11)*(x1605)*(x1606)))+(((cj11)*(cj14)*(r21)))+(((sj11)*(x1607)))+(((sj14)*(x1604)))+(((cj13)*(r20)*(x1608))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((r21)*(x1608)))+(((IkReal(-1.00000000000000))*(cj14)*(x1604)*(x1605)))+(((IkReal(-1.00000000000000))*(cj11)*(x1607)))+(((r20)*(sj11)*(sj14)))+(((cj11)*(cj13)*(x1606))))))) < IKFAST_ATAN2_MAGTHRESH )
07007 continue;
07008 j10array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(sj11)*(x1605)*(x1606)))+(((cj11)*(cj14)*(r21)))+(((sj11)*(x1607)))+(((sj14)*(x1604)))+(((cj13)*(r20)*(x1608)))))), ((gconst8)*(((((r21)*(x1608)))+(((IkReal(-1.00000000000000))*(cj14)*(x1604)*(x1605)))+(((IkReal(-1.00000000000000))*(cj11)*(x1607)))+(((r20)*(sj11)*(sj14)))+(((cj11)*(cj13)*(x1606)))))));
07009 sj10array[0]=IKsin(j10array[0]);
07010 cj10array[0]=IKcos(j10array[0]);
07011 if( j10array[0] > IKPI )
07012 {
07013 j10array[0]-=IK2PI;
07014 }
07015 else if( j10array[0] < -IKPI )
07016 { j10array[0]+=IK2PI;
07017 }
07018 j10valid[0] = true;
07019 for(int ij10 = 0; ij10 < 1; ++ij10)
07020 {
07021 if( !j10valid[ij10] )
07022 {
07023 continue;
07024 }
07025 _ij10[0] = ij10; _ij10[1] = -1;
07026 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07027 {
07028 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07029 {
07030 j10valid[iij10]=false; _ij10[1] = iij10; break;
07031 }
07032 }
07033 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07034 {
07035 IkReal evalcond[4];
07036 IkReal x1609=IKsin(j10);
07037 IkReal x1610=IKcos(j10);
07038 IkReal x1611=((cj13)*(sj14));
07039 IkReal x1612=((cj13)*(cj14));
07040 IkReal x1613=((r10)*(sj9));
07041 IkReal x1614=((IkReal(1.00000000000000))*(cj9));
07042 IkReal x1615=((sj11)*(x1609));
07043 IkReal x1616=((IkReal(1.00000000000000))*(x1610));
07044 IkReal x1617=((cj11)*(x1609));
07045 IkReal x1618=((IkReal(1.00000000000000))*(r11)*(sj9));
07046 IkReal x1619=((cj11)*(x1616));
07047 evalcond[0]=((((IkReal(-1.00000000000000))*(x1617)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x1616))));
07048 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1612)))+(((r21)*(x1611)))+(x1615)+(((IkReal(-1.00000000000000))*(x1619)))+(((IkReal(-1.00000000000000))*(r22)*(sj13))));
07049 evalcond[2]=((((IkReal(-1.00000000000000))*(sj14)*(x1613)))+(x1615)+(((IkReal(-1.00000000000000))*(x1619)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1614)))+(((IkReal(-1.00000000000000))*(cj14)*(x1618)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1614))));
07050 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x1611)*(x1614)))+(((IkReal(-1.00000000000000))*(x1611)*(x1618)))+(x1617)+(((cj9)*(r02)*(sj13)))+(((sj11)*(x1610)))+(((cj9)*(r00)*(x1612)))+(((x1612)*(x1613)))+(((r12)*(sj13)*(sj9))));
07051 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07052 {
07053 continue;
07054 }
07055 }
07056
07057 {
07058 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07059 vinfos[0].jointtype = 1;
07060 vinfos[0].foffset = j9;
07061 vinfos[0].indices[0] = _ij9[0];
07062 vinfos[0].indices[1] = _ij9[1];
07063 vinfos[0].maxsolutions = _nj9;
07064 vinfos[1].jointtype = 1;
07065 vinfos[1].foffset = j10;
07066 vinfos[1].indices[0] = _ij10[0];
07067 vinfos[1].indices[1] = _ij10[1];
07068 vinfos[1].maxsolutions = _nj10;
07069 vinfos[2].jointtype = 1;
07070 vinfos[2].foffset = j11;
07071 vinfos[2].indices[0] = _ij11[0];
07072 vinfos[2].indices[1] = _ij11[1];
07073 vinfos[2].maxsolutions = _nj11;
07074 vinfos[3].jointtype = 1;
07075 vinfos[3].foffset = j12;
07076 vinfos[3].indices[0] = _ij12[0];
07077 vinfos[3].indices[1] = _ij12[1];
07078 vinfos[3].maxsolutions = _nj12;
07079 vinfos[4].jointtype = 1;
07080 vinfos[4].foffset = j13;
07081 vinfos[4].indices[0] = _ij13[0];
07082 vinfos[4].indices[1] = _ij13[1];
07083 vinfos[4].maxsolutions = _nj13;
07084 vinfos[5].jointtype = 1;
07085 vinfos[5].foffset = j14;
07086 vinfos[5].indices[0] = _ij14[0];
07087 vinfos[5].indices[1] = _ij14[1];
07088 vinfos[5].maxsolutions = _nj14;
07089 std::vector<int> vfree(0);
07090 solutions.AddSolution(vinfos,vfree);
07091 }
07092 }
07093 }
07094
07095 }
07096
07097 }
07098
07099 } else
07100 {
07101 IkReal x1620=((r00)*(sj9));
07102 IkReal x1621=((IkReal(1.00000000000000))*(sj13));
07103 IkReal x1622=((cj13)*(sj14));
07104 IkReal x1623=((cj9)*(sj14));
07105 IkReal x1624=((cj13)*(r02));
07106 IkReal x1625=((sj13)*(sj14));
07107 IkReal x1626=((r01)*(sj9));
07108 IkReal x1627=((cj9)*(sj13));
07109 IkReal x1628=((IkReal(1.00000000000000))*(cj9));
07110 IkReal x1629=((cj14)*(r10));
07111 IkReal x1630=((cj14)*(npx));
07112 IkReal x1631=((IkReal(1.00000000000000))*(cj13));
07113 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
07114 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
07115 evalcond[2]=((IkReal(0.235000000000000))+(((npy)*(x1622)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x1630)*(x1631)))+(((IkReal(-1.00000000000000))*(npz)*(x1621))));
07116 evalcond[3]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1628)))+(((sj14)*(x1620)))+(((IkReal(-1.00000000000000))*(r10)*(x1623)))+(((cj14)*(x1626))));
07117 evalcond[4]=((((r21)*(x1625)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x1621)))+(((cj13)*(r22))));
07118 evalcond[5]=((IkReal(-0.0950000000000000))+(((npy)*(x1625)))+(((IkReal(-1.00000000000000))*(x1621)*(x1630)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
07119 evalcond[6]=((((cj13)*(cj9)*(x1629)))+(((IkReal(-1.00000000000000))*(r11)*(x1622)*(x1628)))+(((x1622)*(x1626)))+(((r12)*(x1627)))+(((IkReal(-1.00000000000000))*(cj14)*(x1620)*(x1631)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x1621))));
07120 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(x1620)*(x1621)))+(((x1627)*(x1629)))+(((IkReal(-1.00000000000000))*(r11)*(x1621)*(x1623)))+(((sj9)*(x1624)))+(((x1625)*(x1626)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1628))));
07121 evalcond[8]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1631)))+(((IkReal(-1.00000000000000))*(r01)*(x1621)*(x1623)))+(((cj14)*(r00)*(x1627)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x1621)))+(((IkReal(-1.00000000000000))*(x1624)*(x1628)))+(((sj13)*(sj9)*(x1629))));
07122 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 )
07123 {
07124 {
07125 IkReal dummyeval[1];
07126 IkReal gconst10;
07127 gconst10=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
07128 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
07129 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07130 {
07131 {
07132 IkReal dummyeval[1];
07133 IkReal gconst11;
07134 gconst11=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
07135 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
07136 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07137 {
07138 continue;
07139
07140 } else
07141 {
07142 {
07143 IkReal j10array[1], cj10array[1], sj10array[1];
07144 bool j10valid[1]={false};
07145 _nj10 = 1;
07146 IkReal x1632=((IkReal(1.00000000000000))*(sj11));
07147 IkReal x1633=((cj14)*(r21));
07148 IkReal x1634=((IkReal(1.00000000000000))*(cj11));
07149 IkReal x1635=((r20)*(sj14));
07150 IkReal x1636=((cj9)*(r00)*(sj14));
07151 IkReal x1637=((cj14)*(r11)*(sj9));
07152 IkReal x1638=((cj14)*(cj9)*(r01));
07153 IkReal x1639=((r10)*(sj14)*(sj9));
07154 if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(x1634)*(x1635)))+(((IkReal(-1.00000000000000))*(x1633)*(x1634)))+(((IkReal(-1.00000000000000))*(x1632)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1638)))+(((IkReal(-1.00000000000000))*(x1632)*(x1637)))+(((IkReal(-1.00000000000000))*(x1632)*(x1636))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj11)*(x1636)))+(((cj11)*(x1637)))+(((cj11)*(x1638)))+(((cj11)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1635)))+(((IkReal(-1.00000000000000))*(x1632)*(x1633))))))) < IKFAST_ATAN2_MAGTHRESH )
07155 continue;
07156 j10array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(x1634)*(x1635)))+(((IkReal(-1.00000000000000))*(x1633)*(x1634)))+(((IkReal(-1.00000000000000))*(x1632)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1638)))+(((IkReal(-1.00000000000000))*(x1632)*(x1637)))+(((IkReal(-1.00000000000000))*(x1632)*(x1636)))))), ((gconst11)*(((((cj11)*(x1636)))+(((cj11)*(x1637)))+(((cj11)*(x1638)))+(((cj11)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1635)))+(((IkReal(-1.00000000000000))*(x1632)*(x1633)))))));
07157 sj10array[0]=IKsin(j10array[0]);
07158 cj10array[0]=IKcos(j10array[0]);
07159 if( j10array[0] > IKPI )
07160 {
07161 j10array[0]-=IK2PI;
07162 }
07163 else if( j10array[0] < -IKPI )
07164 { j10array[0]+=IK2PI;
07165 }
07166 j10valid[0] = true;
07167 for(int ij10 = 0; ij10 < 1; ++ij10)
07168 {
07169 if( !j10valid[ij10] )
07170 {
07171 continue;
07172 }
07173 _ij10[0] = ij10; _ij10[1] = -1;
07174 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07175 {
07176 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07177 {
07178 j10valid[iij10]=false; _ij10[1] = iij10; break;
07179 }
07180 }
07181 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07182 {
07183 IkReal evalcond[4];
07184 IkReal x1640=IKsin(j10);
07185 IkReal x1641=IKcos(j10);
07186 IkReal x1642=((cj14)*(sj9));
07187 IkReal x1643=((IkReal(1.00000000000000))*(r11));
07188 IkReal x1644=((cj13)*(sj14));
07189 IkReal x1645=((IkReal(1.00000000000000))*(cj9));
07190 IkReal x1646=((cj13)*(cj14));
07191 IkReal x1647=((cj11)*(x1640));
07192 IkReal x1648=((sj11)*(x1641));
07193 IkReal x1649=((cj11)*(x1641));
07194 IkReal x1650=((sj11)*(x1640));
07195 IkReal x1651=((x1647)+(x1648));
07196 evalcond[0]=((((cj14)*(r21)))+(x1651)+(((r20)*(sj14))));
07197 evalcond[1]=((((r21)*(x1644)))+(x1650)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x1649)))+(((IkReal(-1.00000000000000))*(r20)*(x1646))));
07198 evalcond[2]=((x1649)+(((IkReal(-1.00000000000000))*(x1650)))+(((IkReal(-1.00000000000000))*(x1642)*(x1643)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1645)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1645))));
07199 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x1643)*(x1644)))+(x1651)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1644)*(x1645)))+(((cj13)*(r10)*(x1642)))+(((cj9)*(r00)*(x1646)))+(((r12)*(sj13)*(sj9))));
07200 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07201 {
07202 continue;
07203 }
07204 }
07205
07206 {
07207 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07208 vinfos[0].jointtype = 1;
07209 vinfos[0].foffset = j9;
07210 vinfos[0].indices[0] = _ij9[0];
07211 vinfos[0].indices[1] = _ij9[1];
07212 vinfos[0].maxsolutions = _nj9;
07213 vinfos[1].jointtype = 1;
07214 vinfos[1].foffset = j10;
07215 vinfos[1].indices[0] = _ij10[0];
07216 vinfos[1].indices[1] = _ij10[1];
07217 vinfos[1].maxsolutions = _nj10;
07218 vinfos[2].jointtype = 1;
07219 vinfos[2].foffset = j11;
07220 vinfos[2].indices[0] = _ij11[0];
07221 vinfos[2].indices[1] = _ij11[1];
07222 vinfos[2].maxsolutions = _nj11;
07223 vinfos[3].jointtype = 1;
07224 vinfos[3].foffset = j12;
07225 vinfos[3].indices[0] = _ij12[0];
07226 vinfos[3].indices[1] = _ij12[1];
07227 vinfos[3].maxsolutions = _nj12;
07228 vinfos[4].jointtype = 1;
07229 vinfos[4].foffset = j13;
07230 vinfos[4].indices[0] = _ij13[0];
07231 vinfos[4].indices[1] = _ij13[1];
07232 vinfos[4].maxsolutions = _nj13;
07233 vinfos[5].jointtype = 1;
07234 vinfos[5].foffset = j14;
07235 vinfos[5].indices[0] = _ij14[0];
07236 vinfos[5].indices[1] = _ij14[1];
07237 vinfos[5].maxsolutions = _nj14;
07238 std::vector<int> vfree(0);
07239 solutions.AddSolution(vinfos,vfree);
07240 }
07241 }
07242 }
07243
07244 }
07245
07246 }
07247
07248 } else
07249 {
07250 {
07251 IkReal j10array[1], cj10array[1], sj10array[1];
07252 bool j10valid[1]={false};
07253 _nj10 = 1;
07254 IkReal x1652=((cj13)*(sj11));
07255 IkReal x1653=((r21)*(sj14));
07256 IkReal x1654=((cj14)*(r20));
07257 IkReal x1655=((cj11)*(cj13));
07258 IkReal x1656=((r22)*(sj13));
07259 IkReal x1657=((r20)*(sj14));
07260 IkReal x1658=((cj14)*(r21));
07261 if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(sj11)*(x1656)))+(((IkReal(-1.00000000000000))*(x1652)*(x1654)))+(((cj11)*(x1658)))+(((cj11)*(x1657)))+(((x1652)*(x1653))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((sj11)*(x1657)))+(((sj11)*(x1658)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1653)*(x1655)))+(((cj11)*(x1656))))))) < IKFAST_ATAN2_MAGTHRESH )
07262 continue;
07263 j10array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(sj11)*(x1656)))+(((IkReal(-1.00000000000000))*(x1652)*(x1654)))+(((cj11)*(x1658)))+(((cj11)*(x1657)))+(((x1652)*(x1653)))))), ((gconst10)*(((((sj11)*(x1657)))+(((sj11)*(x1658)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1653)*(x1655)))+(((cj11)*(x1656)))))));
07264 sj10array[0]=IKsin(j10array[0]);
07265 cj10array[0]=IKcos(j10array[0]);
07266 if( j10array[0] > IKPI )
07267 {
07268 j10array[0]-=IK2PI;
07269 }
07270 else if( j10array[0] < -IKPI )
07271 { j10array[0]+=IK2PI;
07272 }
07273 j10valid[0] = true;
07274 for(int ij10 = 0; ij10 < 1; ++ij10)
07275 {
07276 if( !j10valid[ij10] )
07277 {
07278 continue;
07279 }
07280 _ij10[0] = ij10; _ij10[1] = -1;
07281 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07282 {
07283 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07284 {
07285 j10valid[iij10]=false; _ij10[1] = iij10; break;
07286 }
07287 }
07288 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07289 {
07290 IkReal evalcond[4];
07291 IkReal x1659=IKsin(j10);
07292 IkReal x1660=IKcos(j10);
07293 IkReal x1661=((cj14)*(sj9));
07294 IkReal x1662=((IkReal(1.00000000000000))*(r11));
07295 IkReal x1663=((cj13)*(sj14));
07296 IkReal x1664=((IkReal(1.00000000000000))*(cj9));
07297 IkReal x1665=((cj13)*(cj14));
07298 IkReal x1666=((cj11)*(x1659));
07299 IkReal x1667=((sj11)*(x1660));
07300 IkReal x1668=((cj11)*(x1660));
07301 IkReal x1669=((sj11)*(x1659));
07302 IkReal x1670=((x1667)+(x1666));
07303 evalcond[0]=((((cj14)*(r21)))+(x1670)+(((r20)*(sj14))));
07304 evalcond[1]=((x1669)+(((IkReal(-1.00000000000000))*(x1668)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x1665)))+(((r21)*(x1663))));
07305 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1664)))+(((IkReal(-1.00000000000000))*(x1661)*(x1662)))+(x1668)+(((IkReal(-1.00000000000000))*(x1669)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1664))));
07306 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x1662)*(x1663)))+(((IkReal(-1.00000000000000))*(r01)*(x1663)*(x1664)))+(x1670)+(((cj9)*(r02)*(sj13)))+(((cj9)*(r00)*(x1665)))+(((r12)*(sj13)*(sj9)))+(((cj13)*(r10)*(x1661))));
07307 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07308 {
07309 continue;
07310 }
07311 }
07312
07313 {
07314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07315 vinfos[0].jointtype = 1;
07316 vinfos[0].foffset = j9;
07317 vinfos[0].indices[0] = _ij9[0];
07318 vinfos[0].indices[1] = _ij9[1];
07319 vinfos[0].maxsolutions = _nj9;
07320 vinfos[1].jointtype = 1;
07321 vinfos[1].foffset = j10;
07322 vinfos[1].indices[0] = _ij10[0];
07323 vinfos[1].indices[1] = _ij10[1];
07324 vinfos[1].maxsolutions = _nj10;
07325 vinfos[2].jointtype = 1;
07326 vinfos[2].foffset = j11;
07327 vinfos[2].indices[0] = _ij11[0];
07328 vinfos[2].indices[1] = _ij11[1];
07329 vinfos[2].maxsolutions = _nj11;
07330 vinfos[3].jointtype = 1;
07331 vinfos[3].foffset = j12;
07332 vinfos[3].indices[0] = _ij12[0];
07333 vinfos[3].indices[1] = _ij12[1];
07334 vinfos[3].maxsolutions = _nj12;
07335 vinfos[4].jointtype = 1;
07336 vinfos[4].foffset = j13;
07337 vinfos[4].indices[0] = _ij13[0];
07338 vinfos[4].indices[1] = _ij13[1];
07339 vinfos[4].maxsolutions = _nj13;
07340 vinfos[5].jointtype = 1;
07341 vinfos[5].foffset = j14;
07342 vinfos[5].indices[0] = _ij14[0];
07343 vinfos[5].indices[1] = _ij14[1];
07344 vinfos[5].maxsolutions = _nj14;
07345 std::vector<int> vfree(0);
07346 solutions.AddSolution(vinfos,vfree);
07347 }
07348 }
07349 }
07350
07351 }
07352
07353 }
07354
07355 } else
07356 {
07357 IkReal x1671=((cj9)*(r10));
07358 IkReal x1672=((cj13)*(cj14));
07359 IkReal x1673=((sj14)*(sj9));
07360 IkReal x1674=((IkReal(1.00000000000000))*(sj14));
07361 IkReal x1675=((cj9)*(sj13));
07362 IkReal x1676=((r02)*(sj9));
07363 IkReal x1677=((cj13)*(cj9));
07364 IkReal x1678=((cj14)*(r01));
07365 IkReal x1679=((IkReal(1.00000000000000))*(npx));
07366 IkReal x1680=((cj14)*(sj13));
07367 IkReal x1681=((IkReal(1.00000000000000))*(cj9));
07368 IkReal x1682=((npy)*(sj14));
07369 IkReal x1683=((IkReal(1.00000000000000))*(sj13));
07370 IkReal x1684=((IkReal(1.00000000000000))*(cj14)*(sj9));
07371 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
07372 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
07373 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
07374 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(x1672)*(x1679)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1682)))+(((IkReal(-1.00000000000000))*(npz)*(x1683))));
07375 evalcond[4]=((IkReal(1.00000000000000))+(((r00)*(x1673)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1681)))+(((IkReal(-1.00000000000000))*(x1671)*(x1674)))+(((sj9)*(x1678))));
07376 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x1679)*(x1680)))+(((cj13)*(npz)))+(((sj13)*(x1682)))+(((IkReal(0.0900000000000000))*(sj13))));
07377 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x1684)))+(((IkReal(-1.00000000000000))*(r10)*(x1673)))+(((IkReal(-1.00000000000000))*(x1678)*(x1681)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x1674))));
07378 evalcond[7]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1672)))+(((cj13)*(r01)*(x1673)))+(((IkReal(-1.00000000000000))*(x1676)*(x1683)))+(((IkReal(-1.00000000000000))*(r11)*(x1674)*(x1677)))+(((x1671)*(x1672)))+(((r12)*(x1675))));
07379 evalcond[8]=((((r01)*(sj13)*(x1673)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1680)))+(((x1671)*(x1680)))+(((IkReal(-1.00000000000000))*(r11)*(x1674)*(x1675)))+(((IkReal(-1.00000000000000))*(r12)*(x1677)))+(((cj13)*(x1676))));
07380 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 )
07381 {
07382 {
07383 IkReal dummyeval[1];
07384 IkReal gconst12;
07385 gconst12=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
07386 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
07387 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07388 {
07389 {
07390 IkReal dummyeval[1];
07391 IkReal gconst13;
07392 gconst13=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
07393 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
07394 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07395 {
07396 continue;
07397
07398 } else
07399 {
07400 {
07401 IkReal j10array[1], cj10array[1], sj10array[1];
07402 bool j10valid[1]={false};
07403 _nj10 = 1;
07404 IkReal x1685=((cj13)*(sj14));
07405 IkReal x1686=((IkReal(1.00000000000000))*(cj11));
07406 IkReal x1687=((r11)*(sj9));
07407 IkReal x1688=((IkReal(1.00000000000000))*(sj11));
07408 IkReal x1689=((cj13)*(cj14));
07409 IkReal x1690=((cj9)*(r01));
07410 IkReal x1691=((cj9)*(sj11));
07411 IkReal x1692=((r02)*(sj13));
07412 IkReal x1693=((r10)*(sj9));
07413 IkReal x1694=((cj11)*(cj9));
07414 IkReal x1695=((r22)*(sj13));
07415 IkReal x1696=((r12)*(sj13)*(sj9));
07416 if( IKabs(((gconst13)*(((((IkReal(-1.00000000000000))*(x1688)*(x1695)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1690)))+(((IkReal(-1.00000000000000))*(r20)*(x1688)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1687)))+(((r00)*(x1689)*(x1694)))+(((x1692)*(x1694)))+(((cj11)*(x1696)))+(((r21)*(sj11)*(x1685)))+(((cj11)*(x1689)*(x1693))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst13)*(((((sj11)*(x1696)))+(((r00)*(x1689)*(x1691)))+(((x1691)*(x1692)))+(((sj11)*(x1689)*(x1693)))+(((cj11)*(x1695)))+(((cj11)*(r20)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1687)*(x1688)))+(((IkReal(-1.00000000000000))*(r21)*(x1685)*(x1686)))+(((IkReal(-1.00000000000000))*(x1685)*(x1688)*(x1690))))))) < IKFAST_ATAN2_MAGTHRESH )
07417 continue;
07418 j10array[0]=IKatan2(((gconst13)*(((((IkReal(-1.00000000000000))*(x1688)*(x1695)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1690)))+(((IkReal(-1.00000000000000))*(r20)*(x1688)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1687)))+(((r00)*(x1689)*(x1694)))+(((x1692)*(x1694)))+(((cj11)*(x1696)))+(((r21)*(sj11)*(x1685)))+(((cj11)*(x1689)*(x1693)))))), ((gconst13)*(((((sj11)*(x1696)))+(((r00)*(x1689)*(x1691)))+(((x1691)*(x1692)))+(((sj11)*(x1689)*(x1693)))+(((cj11)*(x1695)))+(((cj11)*(r20)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1687)*(x1688)))+(((IkReal(-1.00000000000000))*(r21)*(x1685)*(x1686)))+(((IkReal(-1.00000000000000))*(x1685)*(x1688)*(x1690)))))));
07419 sj10array[0]=IKsin(j10array[0]);
07420 cj10array[0]=IKcos(j10array[0]);
07421 if( j10array[0] > IKPI )
07422 {
07423 j10array[0]-=IK2PI;
07424 }
07425 else if( j10array[0] < -IKPI )
07426 { j10array[0]+=IK2PI;
07427 }
07428 j10valid[0] = true;
07429 for(int ij10 = 0; ij10 < 1; ++ij10)
07430 {
07431 if( !j10valid[ij10] )
07432 {
07433 continue;
07434 }
07435 _ij10[0] = ij10; _ij10[1] = -1;
07436 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07437 {
07438 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07439 {
07440 j10valid[iij10]=false; _ij10[1] = iij10; break;
07441 }
07442 }
07443 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07444 {
07445 IkReal evalcond[4];
07446 IkReal x1697=IKsin(j10);
07447 IkReal x1698=IKcos(j10);
07448 IkReal x1699=((IkReal(1.00000000000000))*(sj13));
07449 IkReal x1700=((r11)*(sj9));
07450 IkReal x1701=((cj9)*(r01));
07451 IkReal x1702=((r21)*(sj14));
07452 IkReal x1703=((cj9)*(r02));
07453 IkReal x1704=((sj13)*(sj9));
07454 IkReal x1705=((cj14)*(r10));
07455 IkReal x1706=((IkReal(1.00000000000000))*(cj13));
07456 IkReal x1707=((cj14)*(r20));
07457 IkReal x1708=((cj11)*(x1697));
07458 IkReal x1709=((sj11)*(x1698));
07459 IkReal x1710=((sj14)*(x1706));
07460 IkReal x1711=((sj11)*(x1697));
07461 IkReal x1712=((cj11)*(x1698));
07462 IkReal x1713=((cj14)*(cj9)*(r00));
07463 IkReal x1714=((x1708)+(x1709));
07464 evalcond[0]=((x1711)+(((IkReal(-1.00000000000000))*(x1712)))+(((IkReal(-1.00000000000000))*(r22)*(x1699)))+(((IkReal(-1.00000000000000))*(x1706)*(x1707)))+(((cj13)*(x1702))));
07465 evalcond[1]=((x1714)+(((IkReal(-1.00000000000000))*(x1699)*(x1707)))+(((sj13)*(x1702)))+(((cj13)*(r22))));
07466 evalcond[2]=((((IkReal(-1.00000000000000))*(x1701)*(x1710)))+(((cj13)*(x1713)))+(((r12)*(x1704)))+(x1714)+(((sj13)*(x1703)))+(((IkReal(-1.00000000000000))*(x1700)*(x1710)))+(((cj13)*(sj9)*(x1705))));
07467 evalcond[3]=((x1712)+(((sj13)*(x1713)))+(((IkReal(-1.00000000000000))*(x1711)))+(((x1704)*(x1705)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1706)))+(((IkReal(-1.00000000000000))*(x1703)*(x1706)))+(((IkReal(-1.00000000000000))*(sj14)*(x1699)*(x1700)))+(((IkReal(-1.00000000000000))*(sj14)*(x1699)*(x1701))));
07468 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07469 {
07470 continue;
07471 }
07472 }
07473
07474 {
07475 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07476 vinfos[0].jointtype = 1;
07477 vinfos[0].foffset = j9;
07478 vinfos[0].indices[0] = _ij9[0];
07479 vinfos[0].indices[1] = _ij9[1];
07480 vinfos[0].maxsolutions = _nj9;
07481 vinfos[1].jointtype = 1;
07482 vinfos[1].foffset = j10;
07483 vinfos[1].indices[0] = _ij10[0];
07484 vinfos[1].indices[1] = _ij10[1];
07485 vinfos[1].maxsolutions = _nj10;
07486 vinfos[2].jointtype = 1;
07487 vinfos[2].foffset = j11;
07488 vinfos[2].indices[0] = _ij11[0];
07489 vinfos[2].indices[1] = _ij11[1];
07490 vinfos[2].maxsolutions = _nj11;
07491 vinfos[3].jointtype = 1;
07492 vinfos[3].foffset = j12;
07493 vinfos[3].indices[0] = _ij12[0];
07494 vinfos[3].indices[1] = _ij12[1];
07495 vinfos[3].maxsolutions = _nj12;
07496 vinfos[4].jointtype = 1;
07497 vinfos[4].foffset = j13;
07498 vinfos[4].indices[0] = _ij13[0];
07499 vinfos[4].indices[1] = _ij13[1];
07500 vinfos[4].maxsolutions = _nj13;
07501 vinfos[5].jointtype = 1;
07502 vinfos[5].foffset = j14;
07503 vinfos[5].indices[0] = _ij14[0];
07504 vinfos[5].indices[1] = _ij14[1];
07505 vinfos[5].maxsolutions = _nj14;
07506 std::vector<int> vfree(0);
07507 solutions.AddSolution(vinfos,vfree);
07508 }
07509 }
07510 }
07511
07512 }
07513
07514 }
07515
07516 } else
07517 {
07518 {
07519 IkReal j10array[1], cj10array[1], sj10array[1];
07520 bool j10valid[1]={false};
07521 _nj10 = 1;
07522 IkReal x1715=((IkReal(1.00000000000000))*(cj11));
07523 IkReal x1716=((cj11)*(r22));
07524 IkReal x1717=((sj11)*(sj13));
07525 IkReal x1718=((r21)*(sj14));
07526 IkReal x1719=((cj13)*(sj11));
07527 IkReal x1720=((cj14)*(r20));
07528 IkReal x1721=((cj13)*(x1718));
07529 IkReal x1722=((IkReal(1.00000000000000))*(x1720));
07530 if( IKabs(((gconst12)*(((((cj13)*(x1716)))+(((cj11)*(sj13)*(x1718)))+(((IkReal(-1.00000000000000))*(r22)*(x1717)))+(((IkReal(-1.00000000000000))*(sj13)*(x1715)*(x1720)))+(((IkReal(-1.00000000000000))*(x1719)*(x1722)))+(((x1718)*(x1719))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((r22)*(x1719)))+(((IkReal(-1.00000000000000))*(x1715)*(x1721)))+(((sj13)*(x1716)))+(((IkReal(-1.00000000000000))*(x1717)*(x1722)))+(((cj11)*(cj13)*(x1720)))+(((x1717)*(x1718))))))) < IKFAST_ATAN2_MAGTHRESH )
07531 continue;
07532 j10array[0]=IKatan2(((gconst12)*(((((cj13)*(x1716)))+(((cj11)*(sj13)*(x1718)))+(((IkReal(-1.00000000000000))*(r22)*(x1717)))+(((IkReal(-1.00000000000000))*(sj13)*(x1715)*(x1720)))+(((IkReal(-1.00000000000000))*(x1719)*(x1722)))+(((x1718)*(x1719)))))), ((gconst12)*(((((r22)*(x1719)))+(((IkReal(-1.00000000000000))*(x1715)*(x1721)))+(((sj13)*(x1716)))+(((IkReal(-1.00000000000000))*(x1717)*(x1722)))+(((cj11)*(cj13)*(x1720)))+(((x1717)*(x1718)))))));
07533 sj10array[0]=IKsin(j10array[0]);
07534 cj10array[0]=IKcos(j10array[0]);
07535 if( j10array[0] > IKPI )
07536 {
07537 j10array[0]-=IK2PI;
07538 }
07539 else if( j10array[0] < -IKPI )
07540 { j10array[0]+=IK2PI;
07541 }
07542 j10valid[0] = true;
07543 for(int ij10 = 0; ij10 < 1; ++ij10)
07544 {
07545 if( !j10valid[ij10] )
07546 {
07547 continue;
07548 }
07549 _ij10[0] = ij10; _ij10[1] = -1;
07550 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07551 {
07552 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07553 {
07554 j10valid[iij10]=false; _ij10[1] = iij10; break;
07555 }
07556 }
07557 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07558 {
07559 IkReal evalcond[4];
07560 IkReal x1723=IKsin(j10);
07561 IkReal x1724=IKcos(j10);
07562 IkReal x1725=((IkReal(1.00000000000000))*(sj13));
07563 IkReal x1726=((r11)*(sj9));
07564 IkReal x1727=((cj9)*(r01));
07565 IkReal x1728=((r21)*(sj14));
07566 IkReal x1729=((cj9)*(r02));
07567 IkReal x1730=((sj13)*(sj9));
07568 IkReal x1731=((cj14)*(r10));
07569 IkReal x1732=((IkReal(1.00000000000000))*(cj13));
07570 IkReal x1733=((cj14)*(r20));
07571 IkReal x1734=((cj11)*(x1723));
07572 IkReal x1735=((sj11)*(x1724));
07573 IkReal x1736=((sj14)*(x1732));
07574 IkReal x1737=((sj11)*(x1723));
07575 IkReal x1738=((cj11)*(x1724));
07576 IkReal x1739=((cj14)*(cj9)*(r00));
07577 IkReal x1740=((x1735)+(x1734));
07578 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1725)))+(x1737)+(((cj13)*(x1728)))+(((IkReal(-1.00000000000000))*(x1738)))+(((IkReal(-1.00000000000000))*(x1732)*(x1733))));
07579 evalcond[1]=((x1740)+(((IkReal(-1.00000000000000))*(x1725)*(x1733)))+(((cj13)*(r22)))+(((sj13)*(x1728))));
07580 evalcond[2]=((((cj13)*(sj9)*(x1731)))+(x1740)+(((IkReal(-1.00000000000000))*(x1726)*(x1736)))+(((cj13)*(x1739)))+(((r12)*(x1730)))+(((IkReal(-1.00000000000000))*(x1727)*(x1736)))+(((sj13)*(x1729))));
07581 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1732)))+(((x1730)*(x1731)))+(x1738)+(((IkReal(-1.00000000000000))*(sj14)*(x1725)*(x1727)))+(((IkReal(-1.00000000000000))*(sj14)*(x1725)*(x1726)))+(((IkReal(-1.00000000000000))*(x1729)*(x1732)))+(((IkReal(-1.00000000000000))*(x1737)))+(((sj13)*(x1739))));
07582 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07583 {
07584 continue;
07585 }
07586 }
07587
07588 {
07589 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07590 vinfos[0].jointtype = 1;
07591 vinfos[0].foffset = j9;
07592 vinfos[0].indices[0] = _ij9[0];
07593 vinfos[0].indices[1] = _ij9[1];
07594 vinfos[0].maxsolutions = _nj9;
07595 vinfos[1].jointtype = 1;
07596 vinfos[1].foffset = j10;
07597 vinfos[1].indices[0] = _ij10[0];
07598 vinfos[1].indices[1] = _ij10[1];
07599 vinfos[1].maxsolutions = _nj10;
07600 vinfos[2].jointtype = 1;
07601 vinfos[2].foffset = j11;
07602 vinfos[2].indices[0] = _ij11[0];
07603 vinfos[2].indices[1] = _ij11[1];
07604 vinfos[2].maxsolutions = _nj11;
07605 vinfos[3].jointtype = 1;
07606 vinfos[3].foffset = j12;
07607 vinfos[3].indices[0] = _ij12[0];
07608 vinfos[3].indices[1] = _ij12[1];
07609 vinfos[3].maxsolutions = _nj12;
07610 vinfos[4].jointtype = 1;
07611 vinfos[4].foffset = j13;
07612 vinfos[4].indices[0] = _ij13[0];
07613 vinfos[4].indices[1] = _ij13[1];
07614 vinfos[4].maxsolutions = _nj13;
07615 vinfos[5].jointtype = 1;
07616 vinfos[5].foffset = j14;
07617 vinfos[5].indices[0] = _ij14[0];
07618 vinfos[5].indices[1] = _ij14[1];
07619 vinfos[5].maxsolutions = _nj14;
07620 std::vector<int> vfree(0);
07621 solutions.AddSolution(vinfos,vfree);
07622 }
07623 }
07624 }
07625
07626 }
07627
07628 }
07629
07630 } else
07631 {
07632 IkReal x1741=((cj9)*(r10));
07633 IkReal x1742=((cj13)*(cj14));
07634 IkReal x1743=((sj14)*(sj9));
07635 IkReal x1744=((IkReal(1.00000000000000))*(sj14));
07636 IkReal x1745=((cj9)*(sj13));
07637 IkReal x1746=((r02)*(sj9));
07638 IkReal x1747=((cj13)*(cj9));
07639 IkReal x1748=((cj14)*(r01));
07640 IkReal x1749=((IkReal(1.00000000000000))*(npx));
07641 IkReal x1750=((cj14)*(sj13));
07642 IkReal x1751=((IkReal(1.00000000000000))*(cj9));
07643 IkReal x1752=((npy)*(sj14));
07644 IkReal x1753=((IkReal(1.00000000000000))*(sj13));
07645 IkReal x1754=((IkReal(1.00000000000000))*(cj14)*(sj9));
07646 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
07647 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
07648 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
07649 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x1753)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1752)))+(((IkReal(-1.00000000000000))*(x1742)*(x1749))));
07650 evalcond[4]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1741)*(x1744)))+(((sj9)*(x1748)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1751)))+(((r00)*(x1743))));
07651 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x1749)*(x1750)))+(((cj13)*(npz)))+(((sj13)*(x1752)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
07652 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x1754)))+(((IkReal(-1.00000000000000))*(r10)*(x1743)))+(((IkReal(-1.00000000000000))*(x1748)*(x1751)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x1744))));
07653 evalcond[7]=((((r12)*(x1745)))+(((cj13)*(r01)*(x1743)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1742)))+(((IkReal(-1.00000000000000))*(x1746)*(x1753)))+(((IkReal(-1.00000000000000))*(r11)*(x1744)*(x1747)))+(((x1741)*(x1742))));
07654 evalcond[8]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1750)))+(((IkReal(-1.00000000000000))*(r12)*(x1747)))+(((cj13)*(x1746)))+(((r01)*(sj13)*(x1743)))+(((IkReal(-1.00000000000000))*(r11)*(x1744)*(x1745)))+(((x1741)*(x1750))));
07655 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 )
07656 {
07657 {
07658 IkReal dummyeval[1];
07659 IkReal gconst14;
07660 gconst14=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
07661 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
07662 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07663 {
07664 {
07665 IkReal dummyeval[1];
07666 IkReal gconst15;
07667 gconst15=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
07668 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
07669 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
07670 {
07671 continue;
07672
07673 } else
07674 {
07675 {
07676 IkReal j10array[1], cj10array[1], sj10array[1];
07677 bool j10valid[1]={false};
07678 _nj10 = 1;
07679 IkReal x1755=((cj13)*(sj14));
07680 IkReal x1756=((IkReal(1.00000000000000))*(cj11));
07681 IkReal x1757=((r11)*(sj9));
07682 IkReal x1758=((IkReal(1.00000000000000))*(sj11));
07683 IkReal x1759=((cj13)*(cj14));
07684 IkReal x1760=((cj11)*(sj13));
07685 IkReal x1761=((r12)*(sj9));
07686 IkReal x1762=((cj9)*(r01));
07687 IkReal x1763=((sj11)*(sj13));
07688 IkReal x1764=((cj9)*(r02));
07689 IkReal x1765=((r10)*(sj9));
07690 IkReal x1766=((cj9)*(r00));
07691 if( IKabs(((gconst15)*(((((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1757)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1758)))+(((r21)*(sj11)*(x1755)))+(((IkReal(-1.00000000000000))*(r20)*(x1758)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1762)))+(((cj11)*(x1759)*(x1765)))+(((cj11)*(x1759)*(x1766)))+(((x1760)*(x1761)))+(((x1760)*(x1764))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((cj11)*(r20)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1758)*(x1762)))+(((IkReal(-1.00000000000000))*(r21)*(x1755)*(x1756)))+(((r22)*(x1760)))+(((IkReal(-1.00000000000000))*(x1755)*(x1757)*(x1758)))+(((x1761)*(x1763)))+(((sj11)*(x1759)*(x1766)))+(((sj11)*(x1759)*(x1765)))+(((x1763)*(x1764))))))) < IKFAST_ATAN2_MAGTHRESH )
07692 continue;
07693 j10array[0]=IKatan2(((gconst15)*(((((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1757)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1758)))+(((r21)*(sj11)*(x1755)))+(((IkReal(-1.00000000000000))*(r20)*(x1758)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1762)))+(((cj11)*(x1759)*(x1765)))+(((cj11)*(x1759)*(x1766)))+(((x1760)*(x1761)))+(((x1760)*(x1764)))))), ((gconst15)*(((((cj11)*(r20)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1758)*(x1762)))+(((IkReal(-1.00000000000000))*(r21)*(x1755)*(x1756)))+(((r22)*(x1760)))+(((IkReal(-1.00000000000000))*(x1755)*(x1757)*(x1758)))+(((x1761)*(x1763)))+(((sj11)*(x1759)*(x1766)))+(((sj11)*(x1759)*(x1765)))+(((x1763)*(x1764)))))));
07694 sj10array[0]=IKsin(j10array[0]);
07695 cj10array[0]=IKcos(j10array[0]);
07696 if( j10array[0] > IKPI )
07697 {
07698 j10array[0]-=IK2PI;
07699 }
07700 else if( j10array[0] < -IKPI )
07701 { j10array[0]+=IK2PI;
07702 }
07703 j10valid[0] = true;
07704 for(int ij10 = 0; ij10 < 1; ++ij10)
07705 {
07706 if( !j10valid[ij10] )
07707 {
07708 continue;
07709 }
07710 _ij10[0] = ij10; _ij10[1] = -1;
07711 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07712 {
07713 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07714 {
07715 j10valid[iij10]=false; _ij10[1] = iij10; break;
07716 }
07717 }
07718 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07719 {
07720 IkReal evalcond[4];
07721 IkReal x1767=IKsin(j10);
07722 IkReal x1768=IKcos(j10);
07723 IkReal x1769=((IkReal(1.00000000000000))*(sj13));
07724 IkReal x1770=((r11)*(sj9));
07725 IkReal x1771=((cj9)*(r01));
07726 IkReal x1772=((IkReal(1.00000000000000))*(cj11));
07727 IkReal x1773=((r21)*(sj14));
07728 IkReal x1774=((cj9)*(r02));
07729 IkReal x1775=((sj13)*(sj9));
07730 IkReal x1776=((cj14)*(r10));
07731 IkReal x1777=((IkReal(1.00000000000000))*(cj13));
07732 IkReal x1778=((cj14)*(r20));
07733 IkReal x1779=((sj11)*(x1767));
07734 IkReal x1780=((sj14)*(x1777));
07735 IkReal x1781=((sj11)*(x1768));
07736 IkReal x1782=((cj14)*(cj9)*(r00));
07737 IkReal x1783=((x1768)*(x1772));
07738 evalcond[0]=((((IkReal(-1.00000000000000))*(x1783)))+(x1779)+(((IkReal(-1.00000000000000))*(r22)*(x1769)))+(((IkReal(-1.00000000000000))*(x1777)*(x1778)))+(((cj13)*(x1773))));
07739 evalcond[1]=((((IkReal(-1.00000000000000))*(x1767)*(x1772)))+(((sj13)*(x1773)))+(((IkReal(-1.00000000000000))*(x1781)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1769)*(x1778))));
07740 evalcond[2]=((((IkReal(-1.00000000000000))*(x1770)*(x1780)))+(((r12)*(x1775)))+(x1781)+(((cj11)*(x1767)))+(((sj13)*(x1774)))+(((cj13)*(x1782)))+(((cj13)*(sj9)*(x1776)))+(((IkReal(-1.00000000000000))*(x1771)*(x1780))));
07741 evalcond[3]=((((IkReal(-1.00000000000000))*(x1783)))+(x1779)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1777)))+(((x1775)*(x1776)))+(((IkReal(-1.00000000000000))*(x1774)*(x1777)))+(((sj13)*(x1782)))+(((IkReal(-1.00000000000000))*(sj14)*(x1769)*(x1771)))+(((IkReal(-1.00000000000000))*(sj14)*(x1769)*(x1770))));
07742 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07743 {
07744 continue;
07745 }
07746 }
07747
07748 {
07749 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07750 vinfos[0].jointtype = 1;
07751 vinfos[0].foffset = j9;
07752 vinfos[0].indices[0] = _ij9[0];
07753 vinfos[0].indices[1] = _ij9[1];
07754 vinfos[0].maxsolutions = _nj9;
07755 vinfos[1].jointtype = 1;
07756 vinfos[1].foffset = j10;
07757 vinfos[1].indices[0] = _ij10[0];
07758 vinfos[1].indices[1] = _ij10[1];
07759 vinfos[1].maxsolutions = _nj10;
07760 vinfos[2].jointtype = 1;
07761 vinfos[2].foffset = j11;
07762 vinfos[2].indices[0] = _ij11[0];
07763 vinfos[2].indices[1] = _ij11[1];
07764 vinfos[2].maxsolutions = _nj11;
07765 vinfos[3].jointtype = 1;
07766 vinfos[3].foffset = j12;
07767 vinfos[3].indices[0] = _ij12[0];
07768 vinfos[3].indices[1] = _ij12[1];
07769 vinfos[3].maxsolutions = _nj12;
07770 vinfos[4].jointtype = 1;
07771 vinfos[4].foffset = j13;
07772 vinfos[4].indices[0] = _ij13[0];
07773 vinfos[4].indices[1] = _ij13[1];
07774 vinfos[4].maxsolutions = _nj13;
07775 vinfos[5].jointtype = 1;
07776 vinfos[5].foffset = j14;
07777 vinfos[5].indices[0] = _ij14[0];
07778 vinfos[5].indices[1] = _ij14[1];
07779 vinfos[5].maxsolutions = _nj14;
07780 std::vector<int> vfree(0);
07781 solutions.AddSolution(vinfos,vfree);
07782 }
07783 }
07784 }
07785
07786 }
07787
07788 }
07789
07790 } else
07791 {
07792 {
07793 IkReal j10array[1], cj10array[1], sj10array[1];
07794 bool j10valid[1]={false};
07795 _nj10 = 1;
07796 IkReal x1784=((IkReal(1.00000000000000))*(sj11));
07797 IkReal x1785=((cj11)*(r22));
07798 IkReal x1786=((IkReal(1.00000000000000))*(sj13));
07799 IkReal x1787=((sj11)*(sj13));
07800 IkReal x1788=((r21)*(sj14));
07801 IkReal x1789=((cj14)*(r20));
07802 IkReal x1790=((cj13)*(sj11));
07803 IkReal x1791=((cj13)*(x1788));
07804 if( IKabs(((gconst14)*(((((cj13)*(x1785)))+(((x1789)*(x1790)))+(((r22)*(x1787)))+(((IkReal(-1.00000000000000))*(x1784)*(x1791)))+(((cj11)*(sj13)*(x1788)))+(((IkReal(-1.00000000000000))*(cj11)*(x1786)*(x1789))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst14)*(((((IkReal(-1.00000000000000))*(sj13)*(x1784)*(x1789)))+(((x1787)*(x1788)))+(((IkReal(-1.00000000000000))*(x1785)*(x1786)))+(((r22)*(x1790)))+(((cj11)*(x1791)))+(((IkReal(-1.00000000000000))*(cj11)*(cj13)*(x1789))))))) < IKFAST_ATAN2_MAGTHRESH )
07805 continue;
07806 j10array[0]=IKatan2(((gconst14)*(((((cj13)*(x1785)))+(((x1789)*(x1790)))+(((r22)*(x1787)))+(((IkReal(-1.00000000000000))*(x1784)*(x1791)))+(((cj11)*(sj13)*(x1788)))+(((IkReal(-1.00000000000000))*(cj11)*(x1786)*(x1789)))))), ((gconst14)*(((((IkReal(-1.00000000000000))*(sj13)*(x1784)*(x1789)))+(((x1787)*(x1788)))+(((IkReal(-1.00000000000000))*(x1785)*(x1786)))+(((r22)*(x1790)))+(((cj11)*(x1791)))+(((IkReal(-1.00000000000000))*(cj11)*(cj13)*(x1789)))))));
07807 sj10array[0]=IKsin(j10array[0]);
07808 cj10array[0]=IKcos(j10array[0]);
07809 if( j10array[0] > IKPI )
07810 {
07811 j10array[0]-=IK2PI;
07812 }
07813 else if( j10array[0] < -IKPI )
07814 { j10array[0]+=IK2PI;
07815 }
07816 j10valid[0] = true;
07817 for(int ij10 = 0; ij10 < 1; ++ij10)
07818 {
07819 if( !j10valid[ij10] )
07820 {
07821 continue;
07822 }
07823 _ij10[0] = ij10; _ij10[1] = -1;
07824 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07825 {
07826 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07827 {
07828 j10valid[iij10]=false; _ij10[1] = iij10; break;
07829 }
07830 }
07831 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07832 {
07833 IkReal evalcond[4];
07834 IkReal x1792=IKsin(j10);
07835 IkReal x1793=IKcos(j10);
07836 IkReal x1794=((IkReal(1.00000000000000))*(sj13));
07837 IkReal x1795=((r11)*(sj9));
07838 IkReal x1796=((cj9)*(r01));
07839 IkReal x1797=((IkReal(1.00000000000000))*(cj11));
07840 IkReal x1798=((r21)*(sj14));
07841 IkReal x1799=((cj9)*(r02));
07842 IkReal x1800=((sj13)*(sj9));
07843 IkReal x1801=((cj14)*(r10));
07844 IkReal x1802=((IkReal(1.00000000000000))*(cj13));
07845 IkReal x1803=((cj14)*(r20));
07846 IkReal x1804=((sj11)*(x1792));
07847 IkReal x1805=((sj14)*(x1802));
07848 IkReal x1806=((sj11)*(x1793));
07849 IkReal x1807=((cj14)*(cj9)*(r00));
07850 IkReal x1808=((x1793)*(x1797));
07851 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1794)))+(((cj13)*(x1798)))+(((IkReal(-1.00000000000000))*(x1802)*(x1803)))+(((IkReal(-1.00000000000000))*(x1808)))+(x1804));
07852 evalcond[1]=((((sj13)*(x1798)))+(((IkReal(-1.00000000000000))*(x1792)*(x1797)))+(((IkReal(-1.00000000000000))*(x1794)*(x1803)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1806))));
07853 evalcond[2]=((((sj13)*(x1799)))+(((IkReal(-1.00000000000000))*(x1795)*(x1805)))+(((IkReal(-1.00000000000000))*(x1796)*(x1805)))+(((cj13)*(x1807)))+(((r12)*(x1800)))+(x1806)+(((cj11)*(x1792)))+(((cj13)*(sj9)*(x1801))));
07854 evalcond[3]=((((x1800)*(x1801)))+(((IkReal(-1.00000000000000))*(x1799)*(x1802)))+(((IkReal(-1.00000000000000))*(x1808)))+(x1804)+(((IkReal(-1.00000000000000))*(sj14)*(x1794)*(x1795)))+(((IkReal(-1.00000000000000))*(sj14)*(x1794)*(x1796)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1802)))+(((sj13)*(x1807))));
07855 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
07856 {
07857 continue;
07858 }
07859 }
07860
07861 {
07862 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07863 vinfos[0].jointtype = 1;
07864 vinfos[0].foffset = j9;
07865 vinfos[0].indices[0] = _ij9[0];
07866 vinfos[0].indices[1] = _ij9[1];
07867 vinfos[0].maxsolutions = _nj9;
07868 vinfos[1].jointtype = 1;
07869 vinfos[1].foffset = j10;
07870 vinfos[1].indices[0] = _ij10[0];
07871 vinfos[1].indices[1] = _ij10[1];
07872 vinfos[1].maxsolutions = _nj10;
07873 vinfos[2].jointtype = 1;
07874 vinfos[2].foffset = j11;
07875 vinfos[2].indices[0] = _ij11[0];
07876 vinfos[2].indices[1] = _ij11[1];
07877 vinfos[2].maxsolutions = _nj11;
07878 vinfos[3].jointtype = 1;
07879 vinfos[3].foffset = j12;
07880 vinfos[3].indices[0] = _ij12[0];
07881 vinfos[3].indices[1] = _ij12[1];
07882 vinfos[3].maxsolutions = _nj12;
07883 vinfos[4].jointtype = 1;
07884 vinfos[4].foffset = j13;
07885 vinfos[4].indices[0] = _ij13[0];
07886 vinfos[4].indices[1] = _ij13[1];
07887 vinfos[4].maxsolutions = _nj13;
07888 vinfos[5].jointtype = 1;
07889 vinfos[5].foffset = j14;
07890 vinfos[5].indices[0] = _ij14[0];
07891 vinfos[5].indices[1] = _ij14[1];
07892 vinfos[5].maxsolutions = _nj14;
07893 std::vector<int> vfree(0);
07894 solutions.AddSolution(vinfos,vfree);
07895 }
07896 }
07897 }
07898
07899 }
07900
07901 }
07902
07903 } else
07904 {
07905 if( 1 )
07906 {
07907 continue;
07908
07909 } else
07910 {
07911 }
07912 }
07913 }
07914 }
07915 }
07916 }
07917
07918 } else
07919 {
07920 {
07921 IkReal j10array[1], cj10array[1], sj10array[1];
07922 bool j10valid[1]={false};
07923 _nj10 = 1;
07924 IkReal x1809=((r21)*(sj14));
07925 IkReal x1810=((cj12)*(cj13));
07926 IkReal x1811=((sj11)*(sj13));
07927 IkReal x1812=((cj14)*(r20));
07928 IkReal x1813=((IkReal(1.00000000000000))*(sj11));
07929 IkReal x1814=((cj12)*(r22));
07930 IkReal x1815=((IkReal(1.00000000000000))*(cj11));
07931 IkReal x1816=((cj13)*(r22));
07932 IkReal x1817=((sj13)*(x1815));
07933 if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x1809)*(x1817)))+(((sj11)*(x1810)*(x1812)))+(((IkReal(-1.00000000000000))*(x1809)*(x1810)*(x1813)))+(((cj11)*(sj13)*(x1812)))+(((IkReal(-1.00000000000000))*(x1815)*(x1816)))+(((x1811)*(x1814))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x1810)*(x1812)*(x1815)))+(((IkReal(-1.00000000000000))*(x1813)*(x1816)))+(((IkReal(-1.00000000000000))*(x1809)*(x1811)))+(((IkReal(-1.00000000000000))*(x1814)*(x1817)))+(((x1811)*(x1812)))+(((cj11)*(x1809)*(x1810))))))) < IKFAST_ATAN2_MAGTHRESH )
07934 continue;
07935 j10array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x1809)*(x1817)))+(((sj11)*(x1810)*(x1812)))+(((IkReal(-1.00000000000000))*(x1809)*(x1810)*(x1813)))+(((cj11)*(sj13)*(x1812)))+(((IkReal(-1.00000000000000))*(x1815)*(x1816)))+(((x1811)*(x1814)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(x1810)*(x1812)*(x1815)))+(((IkReal(-1.00000000000000))*(x1813)*(x1816)))+(((IkReal(-1.00000000000000))*(x1809)*(x1811)))+(((IkReal(-1.00000000000000))*(x1814)*(x1817)))+(((x1811)*(x1812)))+(((cj11)*(x1809)*(x1810)))))));
07936 sj10array[0]=IKsin(j10array[0]);
07937 cj10array[0]=IKcos(j10array[0]);
07938 if( j10array[0] > IKPI )
07939 {
07940 j10array[0]-=IK2PI;
07941 }
07942 else if( j10array[0] < -IKPI )
07943 { j10array[0]+=IK2PI;
07944 }
07945 j10valid[0] = true;
07946 for(int ij10 = 0; ij10 < 1; ++ij10)
07947 {
07948 if( !j10valid[ij10] )
07949 {
07950 continue;
07951 }
07952 _ij10[0] = ij10; _ij10[1] = -1;
07953 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
07954 {
07955 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
07956 {
07957 j10valid[iij10]=false; _ij10[1] = iij10; break;
07958 }
07959 }
07960 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
07961 {
07962 IkReal evalcond[6];
07963 IkReal x1818=IKsin(j10);
07964 IkReal x1819=IKcos(j10);
07965 IkReal x1820=((IkReal(1.00000000000000))*(cj13));
07966 IkReal x1821=((cj9)*(r02));
07967 IkReal x1822=((IkReal(1.00000000000000))*(sj13));
07968 IkReal x1823=((r11)*(sj9));
07969 IkReal x1824=((IkReal(1.00000000000000))*(cj14));
07970 IkReal x1825=((cj9)*(r01));
07971 IkReal x1826=((r21)*(sj14));
07972 IkReal x1827=((IkReal(1.00000000000000))*(sj12));
07973 IkReal x1828=((r10)*(sj9));
07974 IkReal x1829=((cj14)*(sj13));
07975 IkReal x1830=((cj14)*(r20));
07976 IkReal x1831=((IkReal(1.00000000000000))*(sj14));
07977 IkReal x1832=((r12)*(sj9));
07978 IkReal x1833=((cj13)*(cj14));
07979 IkReal x1834=((cj9)*(r00));
07980 IkReal x1835=((sj11)*(x1818));
07981 IkReal x1836=((cj11)*(x1818));
07982 IkReal x1837=((sj11)*(x1819));
07983 IkReal x1838=((cj11)*(x1819));
07984 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x1827)*(x1836)))+(((IkReal(-1.00000000000000))*(x1827)*(x1837))));
07985 evalcond[1]=((((IkReal(-1.00000000000000))*(x1838)))+(((cj13)*(x1826)))+(((IkReal(-1.00000000000000))*(r22)*(x1822)))+(x1835)+(((IkReal(-1.00000000000000))*(x1820)*(x1830))));
07986 evalcond[2]=((((sj13)*(x1826)))+(((cj12)*(x1836)))+(((cj12)*(x1837)))+(((IkReal(-1.00000000000000))*(x1822)*(x1830)))+(((cj13)*(r22))));
07987 evalcond[3]=((((IkReal(-1.00000000000000))*(x1828)*(x1831)))+(((IkReal(-1.00000000000000))*(x1827)*(x1838)))+(((IkReal(-1.00000000000000))*(x1823)*(x1824)))+(((IkReal(-1.00000000000000))*(x1831)*(x1834)))+(((IkReal(-1.00000000000000))*(x1824)*(x1825)))+(((sj12)*(x1835))));
07988 evalcond[4]=((((sj13)*(x1821)))+(x1836)+(x1837)+(((IkReal(-1.00000000000000))*(sj14)*(x1820)*(x1823)))+(((IkReal(-1.00000000000000))*(sj14)*(x1820)*(x1825)))+(((x1833)*(x1834)))+(((x1828)*(x1833)))+(((sj13)*(x1832))));
07989 evalcond[5]=((((x1829)*(x1834)))+(((IkReal(-1.00000000000000))*(sj14)*(x1822)*(x1825)))+(((IkReal(-1.00000000000000))*(sj14)*(x1822)*(x1823)))+(((cj12)*(x1838)))+(((IkReal(-1.00000000000000))*(cj12)*(x1835)))+(((x1828)*(x1829)))+(((IkReal(-1.00000000000000))*(x1820)*(x1832)))+(((IkReal(-1.00000000000000))*(x1820)*(x1821))));
07990 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 )
07991 {
07992 continue;
07993 }
07994 }
07995
07996 {
07997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
07998 vinfos[0].jointtype = 1;
07999 vinfos[0].foffset = j9;
08000 vinfos[0].indices[0] = _ij9[0];
08001 vinfos[0].indices[1] = _ij9[1];
08002 vinfos[0].maxsolutions = _nj9;
08003 vinfos[1].jointtype = 1;
08004 vinfos[1].foffset = j10;
08005 vinfos[1].indices[0] = _ij10[0];
08006 vinfos[1].indices[1] = _ij10[1];
08007 vinfos[1].maxsolutions = _nj10;
08008 vinfos[2].jointtype = 1;
08009 vinfos[2].foffset = j11;
08010 vinfos[2].indices[0] = _ij11[0];
08011 vinfos[2].indices[1] = _ij11[1];
08012 vinfos[2].maxsolutions = _nj11;
08013 vinfos[3].jointtype = 1;
08014 vinfos[3].foffset = j12;
08015 vinfos[3].indices[0] = _ij12[0];
08016 vinfos[3].indices[1] = _ij12[1];
08017 vinfos[3].maxsolutions = _nj12;
08018 vinfos[4].jointtype = 1;
08019 vinfos[4].foffset = j13;
08020 vinfos[4].indices[0] = _ij13[0];
08021 vinfos[4].indices[1] = _ij13[1];
08022 vinfos[4].maxsolutions = _nj13;
08023 vinfos[5].jointtype = 1;
08024 vinfos[5].foffset = j14;
08025 vinfos[5].indices[0] = _ij14[0];
08026 vinfos[5].indices[1] = _ij14[1];
08027 vinfos[5].maxsolutions = _nj14;
08028 std::vector<int> vfree(0);
08029 solutions.AddSolution(vinfos,vfree);
08030 }
08031 }
08032 }
08033
08034 }
08035
08036 }
08037
08038 } else
08039 {
08040 {
08041 IkReal j10array[1], cj10array[1], sj10array[1];
08042 bool j10valid[1]={false};
08043 _nj10 = 1;
08044 IkReal x1839=((cj11)*(sj12));
08045 IkReal x1840=((r22)*(sj13));
08046 IkReal x1841=((r20)*(sj14));
08047 IkReal x1842=((cj14)*(sj11));
08048 IkReal x1843=((cj13)*(r20));
08049 IkReal x1844=((sj11)*(sj12));
08050 IkReal x1845=((cj13)*(r21)*(sj14));
08051 if( IKabs(((gconst6)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1842)*(x1843)))+(((x1840)*(x1844)))+(((IkReal(-1.00000000000000))*(x1844)*(x1845)))+(((cj11)*(x1841))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((sj11)*(x1841)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1839)*(x1845)))+(((r21)*(x1842)))+(((IkReal(-1.00000000000000))*(cj14)*(x1839)*(x1843))))))) < IKFAST_ATAN2_MAGTHRESH )
08052 continue;
08053 j10array[0]=IKatan2(((gconst6)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1842)*(x1843)))+(((x1840)*(x1844)))+(((IkReal(-1.00000000000000))*(x1844)*(x1845)))+(((cj11)*(x1841)))))), ((gconst6)*(((((sj11)*(x1841)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1839)*(x1845)))+(((r21)*(x1842)))+(((IkReal(-1.00000000000000))*(cj14)*(x1839)*(x1843)))))));
08054 sj10array[0]=IKsin(j10array[0]);
08055 cj10array[0]=IKcos(j10array[0]);
08056 if( j10array[0] > IKPI )
08057 {
08058 j10array[0]-=IK2PI;
08059 }
08060 else if( j10array[0] < -IKPI )
08061 { j10array[0]+=IK2PI;
08062 }
08063 j10valid[0] = true;
08064 for(int ij10 = 0; ij10 < 1; ++ij10)
08065 {
08066 if( !j10valid[ij10] )
08067 {
08068 continue;
08069 }
08070 _ij10[0] = ij10; _ij10[1] = -1;
08071 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08072 {
08073 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08074 {
08075 j10valid[iij10]=false; _ij10[1] = iij10; break;
08076 }
08077 }
08078 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08079 {
08080 IkReal evalcond[6];
08081 IkReal x1846=IKsin(j10);
08082 IkReal x1847=IKcos(j10);
08083 IkReal x1848=((IkReal(1.00000000000000))*(cj13));
08084 IkReal x1849=((cj9)*(r02));
08085 IkReal x1850=((IkReal(1.00000000000000))*(sj13));
08086 IkReal x1851=((r11)*(sj9));
08087 IkReal x1852=((IkReal(1.00000000000000))*(cj14));
08088 IkReal x1853=((cj9)*(r01));
08089 IkReal x1854=((r21)*(sj14));
08090 IkReal x1855=((IkReal(1.00000000000000))*(sj12));
08091 IkReal x1856=((r10)*(sj9));
08092 IkReal x1857=((cj14)*(sj13));
08093 IkReal x1858=((cj14)*(r20));
08094 IkReal x1859=((IkReal(1.00000000000000))*(sj14));
08095 IkReal x1860=((r12)*(sj9));
08096 IkReal x1861=((cj13)*(cj14));
08097 IkReal x1862=((cj9)*(r00));
08098 IkReal x1863=((sj11)*(x1846));
08099 IkReal x1864=((cj11)*(x1846));
08100 IkReal x1865=((sj11)*(x1847));
08101 IkReal x1866=((cj11)*(x1847));
08102 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x1855)*(x1864)))+(((IkReal(-1.00000000000000))*(x1855)*(x1865)))+(((r20)*(sj14))));
08103 evalcond[1]=((((IkReal(-1.00000000000000))*(x1848)*(x1858)))+(((cj13)*(x1854)))+(((IkReal(-1.00000000000000))*(r22)*(x1850)))+(x1863)+(((IkReal(-1.00000000000000))*(x1866))));
08104 evalcond[2]=((((sj13)*(x1854)))+(((IkReal(-1.00000000000000))*(x1850)*(x1858)))+(((cj13)*(r22)))+(((cj12)*(x1864)))+(((cj12)*(x1865))));
08105 evalcond[3]=((((sj12)*(x1863)))+(((IkReal(-1.00000000000000))*(x1855)*(x1866)))+(((IkReal(-1.00000000000000))*(x1852)*(x1853)))+(((IkReal(-1.00000000000000))*(x1851)*(x1852)))+(((IkReal(-1.00000000000000))*(x1856)*(x1859)))+(((IkReal(-1.00000000000000))*(x1859)*(x1862))));
08106 evalcond[4]=((((sj13)*(x1860)))+(((IkReal(-1.00000000000000))*(sj14)*(x1848)*(x1853)))+(((IkReal(-1.00000000000000))*(sj14)*(x1848)*(x1851)))+(((x1856)*(x1861)))+(x1865)+(x1864)+(((sj13)*(x1849)))+(((x1861)*(x1862))));
08107 evalcond[5]=((((x1856)*(x1857)))+(((IkReal(-1.00000000000000))*(x1848)*(x1860)))+(((IkReal(-1.00000000000000))*(sj14)*(x1850)*(x1851)))+(((IkReal(-1.00000000000000))*(sj14)*(x1850)*(x1853)))+(((IkReal(-1.00000000000000))*(x1848)*(x1849)))+(((x1857)*(x1862)))+(((IkReal(-1.00000000000000))*(cj12)*(x1863)))+(((cj12)*(x1866))));
08108 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 )
08109 {
08110 continue;
08111 }
08112 }
08113
08114 {
08115 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08116 vinfos[0].jointtype = 1;
08117 vinfos[0].foffset = j9;
08118 vinfos[0].indices[0] = _ij9[0];
08119 vinfos[0].indices[1] = _ij9[1];
08120 vinfos[0].maxsolutions = _nj9;
08121 vinfos[1].jointtype = 1;
08122 vinfos[1].foffset = j10;
08123 vinfos[1].indices[0] = _ij10[0];
08124 vinfos[1].indices[1] = _ij10[1];
08125 vinfos[1].maxsolutions = _nj10;
08126 vinfos[2].jointtype = 1;
08127 vinfos[2].foffset = j11;
08128 vinfos[2].indices[0] = _ij11[0];
08129 vinfos[2].indices[1] = _ij11[1];
08130 vinfos[2].maxsolutions = _nj11;
08131 vinfos[3].jointtype = 1;
08132 vinfos[3].foffset = j12;
08133 vinfos[3].indices[0] = _ij12[0];
08134 vinfos[3].indices[1] = _ij12[1];
08135 vinfos[3].maxsolutions = _nj12;
08136 vinfos[4].jointtype = 1;
08137 vinfos[4].foffset = j13;
08138 vinfos[4].indices[0] = _ij13[0];
08139 vinfos[4].indices[1] = _ij13[1];
08140 vinfos[4].maxsolutions = _nj13;
08141 vinfos[5].jointtype = 1;
08142 vinfos[5].foffset = j14;
08143 vinfos[5].indices[0] = _ij14[0];
08144 vinfos[5].indices[1] = _ij14[1];
08145 vinfos[5].maxsolutions = _nj14;
08146 std::vector<int> vfree(0);
08147 solutions.AddSolution(vinfos,vfree);
08148 }
08149 }
08150 }
08151
08152 }
08153
08154 }
08155 }
08156 }
08157
08158 }
08159
08160 }
08161
08162 } else
08163 {
08164 {
08165 IkReal j10array[1], cj10array[1], sj10array[1];
08166 bool j10valid[1]={false};
08167 _nj10 = 1;
08168 IkReal x1867=((sj11)*(sj13));
08169 IkReal x1868=((cj14)*(r20));
08170 IkReal x1869=((IkReal(1.00000000000000))*(sj11));
08171 IkReal x1870=((cj12)*(r22));
08172 IkReal x1871=((r21)*(sj14));
08173 IkReal x1872=((cj12)*(cj13));
08174 IkReal x1873=((IkReal(1.00000000000000))*(cj11));
08175 IkReal x1874=((cj13)*(r22));
08176 IkReal x1875=((sj13)*(x1873));
08177 IkReal x1876=((x1871)*(x1872));
08178 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x1873)*(x1874)))+(((IkReal(-1.00000000000000))*(x1869)*(x1876)))+(((IkReal(-1.00000000000000))*(x1871)*(x1875)))+(((cj11)*(sj13)*(x1868)))+(((sj11)*(x1868)*(x1872)))+(((x1867)*(x1870))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((x1867)*(x1868)))+(((IkReal(-1.00000000000000))*(x1869)*(x1874)))+(((IkReal(-1.00000000000000))*(x1870)*(x1875)))+(((cj11)*(x1876)))+(((IkReal(-1.00000000000000))*(x1867)*(x1871)))+(((IkReal(-1.00000000000000))*(x1868)*(x1872)*(x1873))))))) < IKFAST_ATAN2_MAGTHRESH )
08179 continue;
08180 j10array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x1873)*(x1874)))+(((IkReal(-1.00000000000000))*(x1869)*(x1876)))+(((IkReal(-1.00000000000000))*(x1871)*(x1875)))+(((cj11)*(sj13)*(x1868)))+(((sj11)*(x1868)*(x1872)))+(((x1867)*(x1870)))))), ((gconst5)*(((((x1867)*(x1868)))+(((IkReal(-1.00000000000000))*(x1869)*(x1874)))+(((IkReal(-1.00000000000000))*(x1870)*(x1875)))+(((cj11)*(x1876)))+(((IkReal(-1.00000000000000))*(x1867)*(x1871)))+(((IkReal(-1.00000000000000))*(x1868)*(x1872)*(x1873)))))));
08181 sj10array[0]=IKsin(j10array[0]);
08182 cj10array[0]=IKcos(j10array[0]);
08183 if( j10array[0] > IKPI )
08184 {
08185 j10array[0]-=IK2PI;
08186 }
08187 else if( j10array[0] < -IKPI )
08188 { j10array[0]+=IK2PI;
08189 }
08190 j10valid[0] = true;
08191 for(int ij10 = 0; ij10 < 1; ++ij10)
08192 {
08193 if( !j10valid[ij10] )
08194 {
08195 continue;
08196 }
08197 _ij10[0] = ij10; _ij10[1] = -1;
08198 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08199 {
08200 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08201 {
08202 j10valid[iij10]=false; _ij10[1] = iij10; break;
08203 }
08204 }
08205 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08206 {
08207 IkReal evalcond[3];
08208 IkReal x1877=IKsin(j10);
08209 IkReal x1878=IKcos(j10);
08210 IkReal x1879=((IkReal(1.00000000000000))*(sj13));
08211 IkReal x1880=((cj14)*(r20));
08212 IkReal x1881=((r21)*(sj14));
08213 IkReal x1882=((IkReal(1.00000000000000))*(x1878));
08214 IkReal x1883=((cj11)*(x1877));
08215 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj12)*(x1883)))+(((IkReal(-1.00000000000000))*(sj11)*(sj12)*(x1882))));
08216 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(x1880)))+(((sj11)*(x1877)))+(((IkReal(-1.00000000000000))*(r22)*(x1879)))+(((cj13)*(x1881)))+(((IkReal(-1.00000000000000))*(cj11)*(x1882))));
08217 evalcond[2]=((((IkReal(-1.00000000000000))*(x1879)*(x1880)))+(((cj12)*(x1883)))+(((sj13)*(x1881)))+(((cj12)*(sj11)*(x1878)))+(((cj13)*(r22))));
08218 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08219 {
08220 continue;
08221 }
08222 }
08223
08224 {
08225 IkReal dummyeval[1];
08226 IkReal gconst16;
08227 IkReal x1884=(cj14)*(cj14);
08228 IkReal x1885=(sj14)*(sj14);
08229 IkReal x1886=((IkReal(1.00000000000000))*(r01));
08230 IkReal x1887=((sj13)*(sj14));
08231 IkReal x1888=((cj14)*(sj13));
08232 IkReal x1889=((r00)*(r11));
08233 IkReal x1890=((cj13)*(x1884));
08234 IkReal x1891=((cj13)*(x1885));
08235 gconst16=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1887)))+(((r02)*(r10)*(x1887)))+(((x1889)*(x1891)))+(((x1889)*(x1890)))+(((r02)*(r11)*(x1888)))+(((IkReal(-1.00000000000000))*(r12)*(x1886)*(x1888)))+(((IkReal(-1.00000000000000))*(r10)*(x1886)*(x1890)))+(((IkReal(-1.00000000000000))*(r10)*(x1886)*(x1891)))));
08236 IkReal x1892=(cj14)*(cj14);
08237 IkReal x1893=(sj14)*(sj14);
08238 IkReal x1894=((IkReal(1.00000000000000))*(r01));
08239 IkReal x1895=((sj13)*(sj14));
08240 IkReal x1896=((cj14)*(sj13));
08241 IkReal x1897=((r00)*(r11));
08242 IkReal x1898=((cj13)*(x1892));
08243 IkReal x1899=((cj13)*(x1893));
08244 dummyeval[0]=((((x1897)*(x1899)))+(((x1897)*(x1898)))+(((IkReal(-1.00000000000000))*(r10)*(x1894)*(x1899)))+(((IkReal(-1.00000000000000))*(r10)*(x1894)*(x1898)))+(((IkReal(-1.00000000000000))*(r12)*(x1894)*(x1896)))+(((r02)*(r11)*(x1896)))+(((r02)*(r10)*(x1895)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1895))));
08245 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08246 {
08247 {
08248 IkReal dummyeval[1];
08249 IkReal gconst17;
08250 IkReal x1900=(sj14)*(sj14);
08251 IkReal x1901=(cj14)*(cj14);
08252 IkReal x1902=((cj13)*(r12));
08253 IkReal x1903=((IkReal(1.00000000000000))*(r10));
08254 IkReal x1904=((cj13)*(r02));
08255 IkReal x1905=((r01)*(sj13));
08256 IkReal x1906=((r00)*(r11)*(sj13));
08257 gconst17=IKsign(((((x1901)*(x1906)))+(((IkReal(-1.00000000000000))*(sj14)*(x1903)*(x1904)))+(((IkReal(-1.00000000000000))*(x1901)*(x1903)*(x1905)))+(((r00)*(sj14)*(x1902)))+(((IkReal(-1.00000000000000))*(x1900)*(x1903)*(x1905)))+(((x1900)*(x1906)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1904)))+(((cj14)*(r01)*(x1902)))));
08258 IkReal x1907=(sj14)*(sj14);
08259 IkReal x1908=(cj14)*(cj14);
08260 IkReal x1909=((cj13)*(r12));
08261 IkReal x1910=((IkReal(1.00000000000000))*(r10));
08262 IkReal x1911=((cj13)*(r02));
08263 IkReal x1912=((r01)*(sj13));
08264 IkReal x1913=((r00)*(r11)*(sj13));
08265 dummyeval[0]=((((x1907)*(x1913)))+(((IkReal(-1.00000000000000))*(x1907)*(x1910)*(x1912)))+(((r00)*(sj14)*(x1909)))+(((IkReal(-1.00000000000000))*(x1908)*(x1910)*(x1912)))+(((x1908)*(x1913)))+(((IkReal(-1.00000000000000))*(sj14)*(x1910)*(x1911)))+(((cj14)*(r01)*(x1909)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1911))));
08266 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08267 {
08268 continue;
08269
08270 } else
08271 {
08272 {
08273 IkReal j9array[1], cj9array[1], sj9array[1];
08274 bool j9valid[1]={false};
08275 _nj9 = 1;
08276 IkReal x1914=((sj12)*(sj14));
08277 IkReal x1915=((cj14)*(sj12));
08278 IkReal x1916=((cj12)*(cj14)*(sj13));
08279 IkReal x1917=((IkReal(1.00000000000000))*(cj12)*(cj13));
08280 IkReal x1918=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
08281 if( IKabs(((gconst17)*(((((r10)*(x1914)))+(((r10)*(x1916)))+(((IkReal(-1.00000000000000))*(r12)*(x1917)))+(((IkReal(-1.00000000000000))*(r11)*(x1918)))+(((r11)*(x1915))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1918)))+(((IkReal(-1.00000000000000))*(r02)*(x1917)))+(((r00)*(x1916)))+(((r00)*(x1914)))+(((r01)*(x1915))))))) < IKFAST_ATAN2_MAGTHRESH )
08282 continue;
08283 j9array[0]=IKatan2(((gconst17)*(((((r10)*(x1914)))+(((r10)*(x1916)))+(((IkReal(-1.00000000000000))*(r12)*(x1917)))+(((IkReal(-1.00000000000000))*(r11)*(x1918)))+(((r11)*(x1915)))))), ((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1918)))+(((IkReal(-1.00000000000000))*(r02)*(x1917)))+(((r00)*(x1916)))+(((r00)*(x1914)))+(((r01)*(x1915)))))));
08284 sj9array[0]=IKsin(j9array[0]);
08285 cj9array[0]=IKcos(j9array[0]);
08286 if( j9array[0] > IKPI )
08287 {
08288 j9array[0]-=IK2PI;
08289 }
08290 else if( j9array[0] < -IKPI )
08291 { j9array[0]+=IK2PI;
08292 }
08293 j9valid[0] = true;
08294 for(int ij9 = 0; ij9 < 1; ++ij9)
08295 {
08296 if( !j9valid[ij9] )
08297 {
08298 continue;
08299 }
08300 _ij9[0] = ij9; _ij9[1] = -1;
08301 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
08302 {
08303 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
08304 {
08305 j9valid[iij9]=false; _ij9[1] = iij9; break;
08306 }
08307 }
08308 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
08309 {
08310 IkReal evalcond[6];
08311 IkReal x1919=IKsin(j9);
08312 IkReal x1920=IKcos(j9);
08313 IkReal x1921=((cj10)*(cj11));
08314 IkReal x1922=((IkReal(1.00000000000000))*(cj14));
08315 IkReal x1923=((IkReal(1.00000000000000))*(sj14));
08316 IkReal x1924=((cj13)*(cj14));
08317 IkReal x1925=((cj14)*(r10));
08318 IkReal x1926=((r01)*(sj14));
08319 IkReal x1927=((sj10)*(sj11));
08320 IkReal x1928=((IkReal(1.00000000000000))*(r12));
08321 IkReal x1929=((sj13)*(x1920));
08322 IkReal x1930=((r02)*(x1919));
08323 IkReal x1931=((r11)*(x1919));
08324 IkReal x1932=((r10)*(x1920));
08325 IkReal x1933=((r01)*(x1920));
08326 IkReal x1934=((sj13)*(x1919));
08327 IkReal x1935=((r11)*(x1920));
08328 IkReal x1936=((cj13)*(x1919));
08329 IkReal x1937=((r10)*(x1919));
08330 IkReal x1938=((r00)*(x1920));
08331 IkReal x1939=((cj13)*(x1920));
08332 evalcond[0]=((((r00)*(sj14)*(x1919)))+(((IkReal(-1.00000000000000))*(x1923)*(x1932)))+(cj12)+(((cj14)*(r01)*(x1919)))+(((IkReal(-1.00000000000000))*(x1922)*(x1935))));
08333 evalcond[1]=((((IkReal(-1.00000000000000))*(sj12)*(x1921)))+(((IkReal(-1.00000000000000))*(x1923)*(x1938)))+(((IkReal(-1.00000000000000))*(x1923)*(x1937)))+(((IkReal(-1.00000000000000))*(x1922)*(x1931)))+(((IkReal(-1.00000000000000))*(x1922)*(x1933)))+(((sj12)*(x1927))));
08334 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x1930)))+(((IkReal(-1.00000000000000))*(r00)*(x1922)*(x1936)))+(((x1924)*(x1932)))+(((x1926)*(x1936)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1935)))+(((r12)*(x1929))));
08335 evalcond[3]=((((IkReal(-1.00000000000000))*(x1928)*(x1939)))+(((IkReal(-1.00000000000000))*(r00)*(x1922)*(x1934)))+(((cj13)*(x1930)))+(((IkReal(-1.00000000000000))*(r11)*(x1923)*(x1929)))+(((x1926)*(x1934)))+(sj12)+(((x1925)*(x1929))));
08336 evalcond[4]=((((cj10)*(sj11)))+(((x1924)*(x1938)))+(((x1924)*(x1937)))+(((r12)*(x1934)))+(((r02)*(x1929)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1933)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1931)))+(((cj11)*(sj10))));
08337 evalcond[5]=((((IkReal(-1.00000000000000))*(x1928)*(x1936)))+(((IkReal(-1.00000000000000))*(sj13)*(x1923)*(x1931)))+(((IkReal(-1.00000000000000))*(r01)*(x1923)*(x1929)))+(((x1925)*(x1934)))+(((cj12)*(x1921)))+(((IkReal(-1.00000000000000))*(cj12)*(x1927)))+(((cj14)*(r00)*(x1929)))+(((IkReal(-1.00000000000000))*(r02)*(x1939))));
08338 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 )
08339 {
08340 continue;
08341 }
08342 }
08343
08344 {
08345 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08346 vinfos[0].jointtype = 1;
08347 vinfos[0].foffset = j9;
08348 vinfos[0].indices[0] = _ij9[0];
08349 vinfos[0].indices[1] = _ij9[1];
08350 vinfos[0].maxsolutions = _nj9;
08351 vinfos[1].jointtype = 1;
08352 vinfos[1].foffset = j10;
08353 vinfos[1].indices[0] = _ij10[0];
08354 vinfos[1].indices[1] = _ij10[1];
08355 vinfos[1].maxsolutions = _nj10;
08356 vinfos[2].jointtype = 1;
08357 vinfos[2].foffset = j11;
08358 vinfos[2].indices[0] = _ij11[0];
08359 vinfos[2].indices[1] = _ij11[1];
08360 vinfos[2].maxsolutions = _nj11;
08361 vinfos[3].jointtype = 1;
08362 vinfos[3].foffset = j12;
08363 vinfos[3].indices[0] = _ij12[0];
08364 vinfos[3].indices[1] = _ij12[1];
08365 vinfos[3].maxsolutions = _nj12;
08366 vinfos[4].jointtype = 1;
08367 vinfos[4].foffset = j13;
08368 vinfos[4].indices[0] = _ij13[0];
08369 vinfos[4].indices[1] = _ij13[1];
08370 vinfos[4].maxsolutions = _nj13;
08371 vinfos[5].jointtype = 1;
08372 vinfos[5].foffset = j14;
08373 vinfos[5].indices[0] = _ij14[0];
08374 vinfos[5].indices[1] = _ij14[1];
08375 vinfos[5].maxsolutions = _nj14;
08376 std::vector<int> vfree(0);
08377 solutions.AddSolution(vinfos,vfree);
08378 }
08379 }
08380 }
08381
08382 }
08383
08384 }
08385
08386 } else
08387 {
08388 {
08389 IkReal j9array[1], cj9array[1], sj9array[1];
08390 bool j9valid[1]={false};
08391 _nj9 = 1;
08392 IkReal x1940=((cj12)*(sj13));
08393 IkReal x1941=((cj12)*(cj13));
08394 IkReal x1942=((IkReal(1.00000000000000))*(sj14));
08395 if( IKabs(((gconst16)*(((((r12)*(x1940)))+(((cj14)*(r10)*(x1941)))+(((IkReal(-1.00000000000000))*(r11)*(x1941)*(x1942))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x1941)*(x1942)))+(((cj14)*(r00)*(x1941)))+(((r02)*(x1940))))))) < IKFAST_ATAN2_MAGTHRESH )
08396 continue;
08397 j9array[0]=IKatan2(((gconst16)*(((((r12)*(x1940)))+(((cj14)*(r10)*(x1941)))+(((IkReal(-1.00000000000000))*(r11)*(x1941)*(x1942)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x1941)*(x1942)))+(((cj14)*(r00)*(x1941)))+(((r02)*(x1940)))))));
08398 sj9array[0]=IKsin(j9array[0]);
08399 cj9array[0]=IKcos(j9array[0]);
08400 if( j9array[0] > IKPI )
08401 {
08402 j9array[0]-=IK2PI;
08403 }
08404 else if( j9array[0] < -IKPI )
08405 { j9array[0]+=IK2PI;
08406 }
08407 j9valid[0] = true;
08408 for(int ij9 = 0; ij9 < 1; ++ij9)
08409 {
08410 if( !j9valid[ij9] )
08411 {
08412 continue;
08413 }
08414 _ij9[0] = ij9; _ij9[1] = -1;
08415 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
08416 {
08417 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
08418 {
08419 j9valid[iij9]=false; _ij9[1] = iij9; break;
08420 }
08421 }
08422 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
08423 {
08424 IkReal evalcond[6];
08425 IkReal x1943=IKsin(j9);
08426 IkReal x1944=IKcos(j9);
08427 IkReal x1945=((cj10)*(cj11));
08428 IkReal x1946=((IkReal(1.00000000000000))*(cj14));
08429 IkReal x1947=((IkReal(1.00000000000000))*(sj14));
08430 IkReal x1948=((cj13)*(cj14));
08431 IkReal x1949=((cj14)*(r10));
08432 IkReal x1950=((r01)*(sj14));
08433 IkReal x1951=((sj10)*(sj11));
08434 IkReal x1952=((IkReal(1.00000000000000))*(r12));
08435 IkReal x1953=((sj13)*(x1944));
08436 IkReal x1954=((r02)*(x1943));
08437 IkReal x1955=((r11)*(x1943));
08438 IkReal x1956=((r10)*(x1944));
08439 IkReal x1957=((r01)*(x1944));
08440 IkReal x1958=((sj13)*(x1943));
08441 IkReal x1959=((r11)*(x1944));
08442 IkReal x1960=((cj13)*(x1943));
08443 IkReal x1961=((r10)*(x1943));
08444 IkReal x1962=((r00)*(x1944));
08445 IkReal x1963=((cj13)*(x1944));
08446 evalcond[0]=((((cj14)*(r01)*(x1943)))+(((IkReal(-1.00000000000000))*(x1947)*(x1956)))+(cj12)+(((IkReal(-1.00000000000000))*(x1946)*(x1959)))+(((r00)*(sj14)*(x1943))));
08447 evalcond[1]=((((sj12)*(x1951)))+(((IkReal(-1.00000000000000))*(x1947)*(x1962)))+(((IkReal(-1.00000000000000))*(x1947)*(x1961)))+(((IkReal(-1.00000000000000))*(x1946)*(x1957)))+(((IkReal(-1.00000000000000))*(x1946)*(x1955)))+(((IkReal(-1.00000000000000))*(sj12)*(x1945))));
08448 evalcond[2]=((((r12)*(x1953)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1959)))+(((x1950)*(x1960)))+(((x1948)*(x1956)))+(((IkReal(-1.00000000000000))*(sj13)*(x1954)))+(((IkReal(-1.00000000000000))*(r00)*(x1946)*(x1960))));
08449 evalcond[3]=((((IkReal(-1.00000000000000))*(x1952)*(x1963)))+(((IkReal(-1.00000000000000))*(r11)*(x1947)*(x1953)))+(((IkReal(-1.00000000000000))*(r00)*(x1946)*(x1958)))+(((cj13)*(x1954)))+(((x1950)*(x1958)))+(sj12)+(((x1949)*(x1953))));
08450 evalcond[4]=((((r12)*(x1958)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1955)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1957)))+(((cj10)*(sj11)))+(((r02)*(x1953)))+(((cj11)*(sj10)))+(((x1948)*(x1961)))+(((x1948)*(x1962))));
08451 evalcond[5]=((((IkReal(-1.00000000000000))*(x1952)*(x1960)))+(((IkReal(-1.00000000000000))*(r01)*(x1947)*(x1953)))+(((cj12)*(x1945)))+(((IkReal(-1.00000000000000))*(sj13)*(x1947)*(x1955)))+(((IkReal(-1.00000000000000))*(cj12)*(x1951)))+(((cj14)*(r00)*(x1953)))+(((IkReal(-1.00000000000000))*(r02)*(x1963)))+(((x1949)*(x1958))));
08452 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 )
08453 {
08454 continue;
08455 }
08456 }
08457
08458 {
08459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08460 vinfos[0].jointtype = 1;
08461 vinfos[0].foffset = j9;
08462 vinfos[0].indices[0] = _ij9[0];
08463 vinfos[0].indices[1] = _ij9[1];
08464 vinfos[0].maxsolutions = _nj9;
08465 vinfos[1].jointtype = 1;
08466 vinfos[1].foffset = j10;
08467 vinfos[1].indices[0] = _ij10[0];
08468 vinfos[1].indices[1] = _ij10[1];
08469 vinfos[1].maxsolutions = _nj10;
08470 vinfos[2].jointtype = 1;
08471 vinfos[2].foffset = j11;
08472 vinfos[2].indices[0] = _ij11[0];
08473 vinfos[2].indices[1] = _ij11[1];
08474 vinfos[2].maxsolutions = _nj11;
08475 vinfos[3].jointtype = 1;
08476 vinfos[3].foffset = j12;
08477 vinfos[3].indices[0] = _ij12[0];
08478 vinfos[3].indices[1] = _ij12[1];
08479 vinfos[3].maxsolutions = _nj12;
08480 vinfos[4].jointtype = 1;
08481 vinfos[4].foffset = j13;
08482 vinfos[4].indices[0] = _ij13[0];
08483 vinfos[4].indices[1] = _ij13[1];
08484 vinfos[4].maxsolutions = _nj13;
08485 vinfos[5].jointtype = 1;
08486 vinfos[5].foffset = j14;
08487 vinfos[5].indices[0] = _ij14[0];
08488 vinfos[5].indices[1] = _ij14[1];
08489 vinfos[5].maxsolutions = _nj14;
08490 std::vector<int> vfree(0);
08491 solutions.AddSolution(vinfos,vfree);
08492 }
08493 }
08494 }
08495
08496 }
08497
08498 }
08499 }
08500 }
08501
08502 }
08503
08504 }
08505
08506 } else
08507 {
08508 {
08509 IkReal j10array[1], cj10array[1], sj10array[1];
08510 bool j10valid[1]={false};
08511 _nj10 = 1;
08512 IkReal x1964=((cj11)*(sj12));
08513 IkReal x1965=((r22)*(sj13));
08514 IkReal x1966=((r20)*(sj14));
08515 IkReal x1967=((cj14)*(sj11));
08516 IkReal x1968=((cj13)*(r20));
08517 IkReal x1969=((sj11)*(sj12));
08518 IkReal x1970=((cj13)*(r21)*(sj14));
08519 if( IKabs(((gconst4)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1967)*(x1968)))+(((IkReal(-1.00000000000000))*(x1969)*(x1970)))+(((cj11)*(x1966)))+(((x1965)*(x1969))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((x1964)*(x1970)))+(((IkReal(-1.00000000000000))*(x1964)*(x1965)))+(((r21)*(x1967)))+(((sj11)*(x1966)))+(((IkReal(-1.00000000000000))*(cj14)*(x1964)*(x1968))))))) < IKFAST_ATAN2_MAGTHRESH )
08520 continue;
08521 j10array[0]=IKatan2(((gconst4)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1967)*(x1968)))+(((IkReal(-1.00000000000000))*(x1969)*(x1970)))+(((cj11)*(x1966)))+(((x1965)*(x1969)))))), ((gconst4)*(((((x1964)*(x1970)))+(((IkReal(-1.00000000000000))*(x1964)*(x1965)))+(((r21)*(x1967)))+(((sj11)*(x1966)))+(((IkReal(-1.00000000000000))*(cj14)*(x1964)*(x1968)))))));
08522 sj10array[0]=IKsin(j10array[0]);
08523 cj10array[0]=IKcos(j10array[0]);
08524 if( j10array[0] > IKPI )
08525 {
08526 j10array[0]-=IK2PI;
08527 }
08528 else if( j10array[0] < -IKPI )
08529 { j10array[0]+=IK2PI;
08530 }
08531 j10valid[0] = true;
08532 for(int ij10 = 0; ij10 < 1; ++ij10)
08533 {
08534 if( !j10valid[ij10] )
08535 {
08536 continue;
08537 }
08538 _ij10[0] = ij10; _ij10[1] = -1;
08539 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
08540 {
08541 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
08542 {
08543 j10valid[iij10]=false; _ij10[1] = iij10; break;
08544 }
08545 }
08546 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
08547 {
08548 IkReal evalcond[3];
08549 IkReal x1971=IKsin(j10);
08550 IkReal x1972=IKcos(j10);
08551 IkReal x1973=((IkReal(1.00000000000000))*(sj13));
08552 IkReal x1974=((cj14)*(r20));
08553 IkReal x1975=((r21)*(sj14));
08554 IkReal x1976=((IkReal(1.00000000000000))*(x1972));
08555 IkReal x1977=((cj11)*(x1971));
08556 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(sj12)*(x1976)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj12)*(x1977))));
08557 evalcond[1]=((((cj13)*(x1975)))+(((IkReal(-1.00000000000000))*(cj11)*(x1976)))+(((IkReal(-1.00000000000000))*(r22)*(x1973)))+(((IkReal(-1.00000000000000))*(cj13)*(x1974)))+(((sj11)*(x1971))));
08558 evalcond[2]=((((cj12)*(sj11)*(x1972)))+(((cj12)*(x1977)))+(((IkReal(-1.00000000000000))*(x1973)*(x1974)))+(((sj13)*(x1975)))+(((cj13)*(r22))));
08559 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08560 {
08561 continue;
08562 }
08563 }
08564
08565 {
08566 IkReal dummyeval[1];
08567 IkReal gconst16;
08568 IkReal x1978=(cj14)*(cj14);
08569 IkReal x1979=(sj14)*(sj14);
08570 IkReal x1980=((IkReal(1.00000000000000))*(r01));
08571 IkReal x1981=((sj13)*(sj14));
08572 IkReal x1982=((cj14)*(sj13));
08573 IkReal x1983=((r00)*(r11));
08574 IkReal x1984=((cj13)*(x1978));
08575 IkReal x1985=((cj13)*(x1979));
08576 gconst16=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1981)))+(((IkReal(-1.00000000000000))*(r10)*(x1980)*(x1985)))+(((IkReal(-1.00000000000000))*(r10)*(x1980)*(x1984)))+(((r02)*(r11)*(x1982)))+(((x1983)*(x1984)))+(((x1983)*(x1985)))+(((IkReal(-1.00000000000000))*(r12)*(x1980)*(x1982)))+(((r02)*(r10)*(x1981)))));
08577 IkReal x1986=(cj14)*(cj14);
08578 IkReal x1987=(sj14)*(sj14);
08579 IkReal x1988=((IkReal(1.00000000000000))*(r01));
08580 IkReal x1989=((sj13)*(sj14));
08581 IkReal x1990=((cj14)*(sj13));
08582 IkReal x1991=((r00)*(r11));
08583 IkReal x1992=((cj13)*(x1986));
08584 IkReal x1993=((cj13)*(x1987));
08585 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1989)))+(((x1991)*(x1993)))+(((x1991)*(x1992)))+(((IkReal(-1.00000000000000))*(r12)*(x1988)*(x1990)))+(((IkReal(-1.00000000000000))*(r10)*(x1988)*(x1992)))+(((IkReal(-1.00000000000000))*(r10)*(x1988)*(x1993)))+(((r02)*(r11)*(x1990)))+(((r02)*(r10)*(x1989))));
08586 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08587 {
08588 {
08589 IkReal dummyeval[1];
08590 IkReal gconst17;
08591 IkReal x1994=(sj14)*(sj14);
08592 IkReal x1995=(cj14)*(cj14);
08593 IkReal x1996=((cj13)*(r12));
08594 IkReal x1997=((IkReal(1.00000000000000))*(r10));
08595 IkReal x1998=((cj13)*(r02));
08596 IkReal x1999=((r01)*(sj13));
08597 IkReal x2000=((r00)*(r11)*(sj13));
08598 gconst17=IKsign(((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1998)))+(((cj14)*(r01)*(x1996)))+(((IkReal(-1.00000000000000))*(x1995)*(x1997)*(x1999)))+(((x1994)*(x2000)))+(((x1995)*(x2000)))+(((IkReal(-1.00000000000000))*(sj14)*(x1997)*(x1998)))+(((r00)*(sj14)*(x1996)))+(((IkReal(-1.00000000000000))*(x1994)*(x1997)*(x1999)))));
08599 IkReal x2001=(sj14)*(sj14);
08600 IkReal x2002=(cj14)*(cj14);
08601 IkReal x2003=((cj13)*(r12));
08602 IkReal x2004=((IkReal(1.00000000000000))*(r10));
08603 IkReal x2005=((cj13)*(r02));
08604 IkReal x2006=((r01)*(sj13));
08605 IkReal x2007=((r00)*(r11)*(sj13));
08606 dummyeval[0]=((((x2002)*(x2007)))+(((IkReal(-1.00000000000000))*(x2002)*(x2004)*(x2006)))+(((cj14)*(r01)*(x2003)))+(((IkReal(-1.00000000000000))*(x2001)*(x2004)*(x2006)))+(((IkReal(-1.00000000000000))*(sj14)*(x2004)*(x2005)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2005)))+(((r00)*(sj14)*(x2003)))+(((x2001)*(x2007))));
08607 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08608 {
08609 continue;
08610
08611 } else
08612 {
08613 {
08614 IkReal j9array[1], cj9array[1], sj9array[1];
08615 bool j9valid[1]={false};
08616 _nj9 = 1;
08617 IkReal x2008=((sj12)*(sj14));
08618 IkReal x2009=((cj14)*(sj12));
08619 IkReal x2010=((cj12)*(cj14)*(sj13));
08620 IkReal x2011=((IkReal(1.00000000000000))*(cj12)*(cj13));
08621 IkReal x2012=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
08622 if( IKabs(((gconst17)*(((((r11)*(x2009)))+(((r10)*(x2008)))+(((r10)*(x2010)))+(((IkReal(-1.00000000000000))*(r12)*(x2011)))+(((IkReal(-1.00000000000000))*(r11)*(x2012))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((r01)*(x2009)))+(((r00)*(x2008)))+(((r00)*(x2010)))+(((IkReal(-1.00000000000000))*(r02)*(x2011)))+(((IkReal(-1.00000000000000))*(r01)*(x2012))))))) < IKFAST_ATAN2_MAGTHRESH )
08623 continue;
08624 j9array[0]=IKatan2(((gconst17)*(((((r11)*(x2009)))+(((r10)*(x2008)))+(((r10)*(x2010)))+(((IkReal(-1.00000000000000))*(r12)*(x2011)))+(((IkReal(-1.00000000000000))*(r11)*(x2012)))))), ((gconst17)*(((((r01)*(x2009)))+(((r00)*(x2008)))+(((r00)*(x2010)))+(((IkReal(-1.00000000000000))*(r02)*(x2011)))+(((IkReal(-1.00000000000000))*(r01)*(x2012)))))));
08625 sj9array[0]=IKsin(j9array[0]);
08626 cj9array[0]=IKcos(j9array[0]);
08627 if( j9array[0] > IKPI )
08628 {
08629 j9array[0]-=IK2PI;
08630 }
08631 else if( j9array[0] < -IKPI )
08632 { j9array[0]+=IK2PI;
08633 }
08634 j9valid[0] = true;
08635 for(int ij9 = 0; ij9 < 1; ++ij9)
08636 {
08637 if( !j9valid[ij9] )
08638 {
08639 continue;
08640 }
08641 _ij9[0] = ij9; _ij9[1] = -1;
08642 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
08643 {
08644 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
08645 {
08646 j9valid[iij9]=false; _ij9[1] = iij9; break;
08647 }
08648 }
08649 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
08650 {
08651 IkReal evalcond[6];
08652 IkReal x2013=IKsin(j9);
08653 IkReal x2014=IKcos(j9);
08654 IkReal x2015=((cj10)*(cj11));
08655 IkReal x2016=((IkReal(1.00000000000000))*(cj14));
08656 IkReal x2017=((IkReal(1.00000000000000))*(sj14));
08657 IkReal x2018=((cj13)*(cj14));
08658 IkReal x2019=((cj14)*(r10));
08659 IkReal x2020=((r01)*(sj14));
08660 IkReal x2021=((sj10)*(sj11));
08661 IkReal x2022=((IkReal(1.00000000000000))*(r12));
08662 IkReal x2023=((sj13)*(x2014));
08663 IkReal x2024=((r02)*(x2013));
08664 IkReal x2025=((r11)*(x2013));
08665 IkReal x2026=((r10)*(x2014));
08666 IkReal x2027=((r01)*(x2014));
08667 IkReal x2028=((sj13)*(x2013));
08668 IkReal x2029=((r11)*(x2014));
08669 IkReal x2030=((cj13)*(x2013));
08670 IkReal x2031=((r10)*(x2013));
08671 IkReal x2032=((r00)*(x2014));
08672 IkReal x2033=((cj13)*(x2014));
08673 evalcond[0]=((((r00)*(sj14)*(x2013)))+(((IkReal(-1.00000000000000))*(x2016)*(x2029)))+(((IkReal(-1.00000000000000))*(x2017)*(x2026)))+(cj12)+(((cj14)*(r01)*(x2013))));
08674 evalcond[1]=((((IkReal(-1.00000000000000))*(x2016)*(x2025)))+(((IkReal(-1.00000000000000))*(x2016)*(x2027)))+(((sj12)*(x2021)))+(((IkReal(-1.00000000000000))*(sj12)*(x2015)))+(((IkReal(-1.00000000000000))*(x2017)*(x2032)))+(((IkReal(-1.00000000000000))*(x2017)*(x2031))));
08675 evalcond[2]=((((x2018)*(x2026)))+(((IkReal(-1.00000000000000))*(r00)*(x2016)*(x2030)))+(((IkReal(-1.00000000000000))*(sj13)*(x2024)))+(((x2020)*(x2030)))+(((r12)*(x2023)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2029))));
08676 evalcond[3]=((((x2020)*(x2028)))+(((IkReal(-1.00000000000000))*(r11)*(x2017)*(x2023)))+(((cj13)*(x2024)))+(((x2019)*(x2023)))+(((IkReal(-1.00000000000000))*(x2022)*(x2033)))+(sj12)+(((IkReal(-1.00000000000000))*(r00)*(x2016)*(x2028))));
08677 evalcond[4]=((((x2018)*(x2031)))+(((x2018)*(x2032)))+(((cj10)*(sj11)))+(((r02)*(x2023)))+(((cj11)*(sj10)))+(((r12)*(x2028)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2025)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2027))));
08678 evalcond[5]=((((x2019)*(x2028)))+(((IkReal(-1.00000000000000))*(cj12)*(x2021)))+(((IkReal(-1.00000000000000))*(x2022)*(x2030)))+(((IkReal(-1.00000000000000))*(sj13)*(x2017)*(x2025)))+(((IkReal(-1.00000000000000))*(r01)*(x2017)*(x2023)))+(((cj14)*(r00)*(x2023)))+(((cj12)*(x2015)))+(((IkReal(-1.00000000000000))*(r02)*(x2033))));
08679 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 )
08680 {
08681 continue;
08682 }
08683 }
08684
08685 {
08686 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08687 vinfos[0].jointtype = 1;
08688 vinfos[0].foffset = j9;
08689 vinfos[0].indices[0] = _ij9[0];
08690 vinfos[0].indices[1] = _ij9[1];
08691 vinfos[0].maxsolutions = _nj9;
08692 vinfos[1].jointtype = 1;
08693 vinfos[1].foffset = j10;
08694 vinfos[1].indices[0] = _ij10[0];
08695 vinfos[1].indices[1] = _ij10[1];
08696 vinfos[1].maxsolutions = _nj10;
08697 vinfos[2].jointtype = 1;
08698 vinfos[2].foffset = j11;
08699 vinfos[2].indices[0] = _ij11[0];
08700 vinfos[2].indices[1] = _ij11[1];
08701 vinfos[2].maxsolutions = _nj11;
08702 vinfos[3].jointtype = 1;
08703 vinfos[3].foffset = j12;
08704 vinfos[3].indices[0] = _ij12[0];
08705 vinfos[3].indices[1] = _ij12[1];
08706 vinfos[3].maxsolutions = _nj12;
08707 vinfos[4].jointtype = 1;
08708 vinfos[4].foffset = j13;
08709 vinfos[4].indices[0] = _ij13[0];
08710 vinfos[4].indices[1] = _ij13[1];
08711 vinfos[4].maxsolutions = _nj13;
08712 vinfos[5].jointtype = 1;
08713 vinfos[5].foffset = j14;
08714 vinfos[5].indices[0] = _ij14[0];
08715 vinfos[5].indices[1] = _ij14[1];
08716 vinfos[5].maxsolutions = _nj14;
08717 std::vector<int> vfree(0);
08718 solutions.AddSolution(vinfos,vfree);
08719 }
08720 }
08721 }
08722
08723 }
08724
08725 }
08726
08727 } else
08728 {
08729 {
08730 IkReal j9array[1], cj9array[1], sj9array[1];
08731 bool j9valid[1]={false};
08732 _nj9 = 1;
08733 IkReal x2034=((cj12)*(sj13));
08734 IkReal x2035=((cj12)*(cj13));
08735 IkReal x2036=((IkReal(1.00000000000000))*(sj14));
08736 if( IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r11)*(x2035)*(x2036)))+(((r12)*(x2034)))+(((cj14)*(r10)*(x2035))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x2035)*(x2036)))+(((r02)*(x2034)))+(((cj14)*(r00)*(x2035))))))) < IKFAST_ATAN2_MAGTHRESH )
08737 continue;
08738 j9array[0]=IKatan2(((gconst16)*(((((IkReal(-1.00000000000000))*(r11)*(x2035)*(x2036)))+(((r12)*(x2034)))+(((cj14)*(r10)*(x2035)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x2035)*(x2036)))+(((r02)*(x2034)))+(((cj14)*(r00)*(x2035)))))));
08739 sj9array[0]=IKsin(j9array[0]);
08740 cj9array[0]=IKcos(j9array[0]);
08741 if( j9array[0] > IKPI )
08742 {
08743 j9array[0]-=IK2PI;
08744 }
08745 else if( j9array[0] < -IKPI )
08746 { j9array[0]+=IK2PI;
08747 }
08748 j9valid[0] = true;
08749 for(int ij9 = 0; ij9 < 1; ++ij9)
08750 {
08751 if( !j9valid[ij9] )
08752 {
08753 continue;
08754 }
08755 _ij9[0] = ij9; _ij9[1] = -1;
08756 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
08757 {
08758 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
08759 {
08760 j9valid[iij9]=false; _ij9[1] = iij9; break;
08761 }
08762 }
08763 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
08764 {
08765 IkReal evalcond[6];
08766 IkReal x2037=IKsin(j9);
08767 IkReal x2038=IKcos(j9);
08768 IkReal x2039=((cj10)*(cj11));
08769 IkReal x2040=((IkReal(1.00000000000000))*(cj14));
08770 IkReal x2041=((IkReal(1.00000000000000))*(sj14));
08771 IkReal x2042=((cj13)*(cj14));
08772 IkReal x2043=((cj14)*(r10));
08773 IkReal x2044=((r01)*(sj14));
08774 IkReal x2045=((sj10)*(sj11));
08775 IkReal x2046=((IkReal(1.00000000000000))*(r12));
08776 IkReal x2047=((sj13)*(x2038));
08777 IkReal x2048=((r02)*(x2037));
08778 IkReal x2049=((r11)*(x2037));
08779 IkReal x2050=((r10)*(x2038));
08780 IkReal x2051=((r01)*(x2038));
08781 IkReal x2052=((sj13)*(x2037));
08782 IkReal x2053=((r11)*(x2038));
08783 IkReal x2054=((cj13)*(x2037));
08784 IkReal x2055=((r10)*(x2037));
08785 IkReal x2056=((r00)*(x2038));
08786 IkReal x2057=((cj13)*(x2038));
08787 evalcond[0]=((((cj14)*(r01)*(x2037)))+(cj12)+(((IkReal(-1.00000000000000))*(x2040)*(x2053)))+(((IkReal(-1.00000000000000))*(x2041)*(x2050)))+(((r00)*(sj14)*(x2037))));
08788 evalcond[1]=((((sj12)*(x2045)))+(((IkReal(-1.00000000000000))*(sj12)*(x2039)))+(((IkReal(-1.00000000000000))*(x2040)*(x2049)))+(((IkReal(-1.00000000000000))*(x2040)*(x2051)))+(((IkReal(-1.00000000000000))*(x2041)*(x2055)))+(((IkReal(-1.00000000000000))*(x2041)*(x2056))));
08789 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x2048)))+(((x2042)*(x2050)))+(((r12)*(x2047)))+(((IkReal(-1.00000000000000))*(r00)*(x2040)*(x2054)))+(((x2044)*(x2054)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2053))));
08790 evalcond[3]=((((x2043)*(x2047)))+(((cj13)*(x2048)))+(((IkReal(-1.00000000000000))*(r00)*(x2040)*(x2052)))+(((x2044)*(x2052)))+(((IkReal(-1.00000000000000))*(x2046)*(x2057)))+(sj12)+(((IkReal(-1.00000000000000))*(r11)*(x2041)*(x2047))));
08791 evalcond[4]=((((r12)*(x2052)))+(((x2042)*(x2055)))+(((x2042)*(x2056)))+(((r02)*(x2047)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2051)))+(((cj11)*(sj10)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2049))));
08792 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x2041)*(x2047)))+(((IkReal(-1.00000000000000))*(r02)*(x2057)))+(((cj12)*(x2039)))+(((x2043)*(x2052)))+(((IkReal(-1.00000000000000))*(sj13)*(x2041)*(x2049)))+(((IkReal(-1.00000000000000))*(x2046)*(x2054)))+(((cj14)*(r00)*(x2047)))+(((IkReal(-1.00000000000000))*(cj12)*(x2045))));
08793 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 )
08794 {
08795 continue;
08796 }
08797 }
08798
08799 {
08800 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
08801 vinfos[0].jointtype = 1;
08802 vinfos[0].foffset = j9;
08803 vinfos[0].indices[0] = _ij9[0];
08804 vinfos[0].indices[1] = _ij9[1];
08805 vinfos[0].maxsolutions = _nj9;
08806 vinfos[1].jointtype = 1;
08807 vinfos[1].foffset = j10;
08808 vinfos[1].indices[0] = _ij10[0];
08809 vinfos[1].indices[1] = _ij10[1];
08810 vinfos[1].maxsolutions = _nj10;
08811 vinfos[2].jointtype = 1;
08812 vinfos[2].foffset = j11;
08813 vinfos[2].indices[0] = _ij11[0];
08814 vinfos[2].indices[1] = _ij11[1];
08815 vinfos[2].maxsolutions = _nj11;
08816 vinfos[3].jointtype = 1;
08817 vinfos[3].foffset = j12;
08818 vinfos[3].indices[0] = _ij12[0];
08819 vinfos[3].indices[1] = _ij12[1];
08820 vinfos[3].maxsolutions = _nj12;
08821 vinfos[4].jointtype = 1;
08822 vinfos[4].foffset = j13;
08823 vinfos[4].indices[0] = _ij13[0];
08824 vinfos[4].indices[1] = _ij13[1];
08825 vinfos[4].maxsolutions = _nj13;
08826 vinfos[5].jointtype = 1;
08827 vinfos[5].foffset = j14;
08828 vinfos[5].indices[0] = _ij14[0];
08829 vinfos[5].indices[1] = _ij14[1];
08830 vinfos[5].maxsolutions = _nj14;
08831 std::vector<int> vfree(0);
08832 solutions.AddSolution(vinfos,vfree);
08833 }
08834 }
08835 }
08836
08837 }
08838
08839 }
08840 }
08841 }
08842
08843 }
08844
08845 }
08846 }
08847 }
08848
08849 }
08850
08851 }
08852
08853 } else
08854 {
08855 {
08856 IkReal j9array[1], cj9array[1], sj9array[1];
08857 bool j9valid[1]={false};
08858 _nj9 = 1;
08859 IkReal x2058=((sj12)*(sj14));
08860 IkReal x2059=((cj14)*(sj12));
08861 IkReal x2060=((cj12)*(cj14)*(sj13));
08862 IkReal x2061=((IkReal(1.00000000000000))*(cj12)*(cj13));
08863 IkReal x2062=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
08864 if( IKabs(((gconst1)*(((((r10)*(x2058)))+(((IkReal(-1.00000000000000))*(r12)*(x2061)))+(((r11)*(x2059)))+(((IkReal(-1.00000000000000))*(r11)*(x2062)))+(((r10)*(x2060))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((r01)*(x2059)))+(((IkReal(-1.00000000000000))*(r02)*(x2061)))+(((r00)*(x2060)))+(((IkReal(-1.00000000000000))*(r01)*(x2062)))+(((r00)*(x2058))))))) < IKFAST_ATAN2_MAGTHRESH )
08865 continue;
08866 j9array[0]=IKatan2(((gconst1)*(((((r10)*(x2058)))+(((IkReal(-1.00000000000000))*(r12)*(x2061)))+(((r11)*(x2059)))+(((IkReal(-1.00000000000000))*(r11)*(x2062)))+(((r10)*(x2060)))))), ((gconst1)*(((((r01)*(x2059)))+(((IkReal(-1.00000000000000))*(r02)*(x2061)))+(((r00)*(x2060)))+(((IkReal(-1.00000000000000))*(r01)*(x2062)))+(((r00)*(x2058)))))));
08867 sj9array[0]=IKsin(j9array[0]);
08868 cj9array[0]=IKcos(j9array[0]);
08869 if( j9array[0] > IKPI )
08870 {
08871 j9array[0]-=IK2PI;
08872 }
08873 else if( j9array[0] < -IKPI )
08874 { j9array[0]+=IK2PI;
08875 }
08876 j9valid[0] = true;
08877 for(int ij9 = 0; ij9 < 1; ++ij9)
08878 {
08879 if( !j9valid[ij9] )
08880 {
08881 continue;
08882 }
08883 _ij9[0] = ij9; _ij9[1] = -1;
08884 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
08885 {
08886 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
08887 {
08888 j9valid[iij9]=false; _ij9[1] = iij9; break;
08889 }
08890 }
08891 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
08892 {
08893 IkReal evalcond[3];
08894 IkReal x2063=IKsin(j9);
08895 IkReal x2064=IKcos(j9);
08896 IkReal x2065=((IkReal(1.00000000000000))*(sj14));
08897 IkReal x2066=((sj13)*(x2064));
08898 IkReal x2067=((cj13)*(x2063));
08899 IkReal x2068=((IkReal(1.00000000000000))*(cj14)*(r00));
08900 IkReal x2069=((r01)*(x2063));
08901 IkReal x2070=((r10)*(x2064));
08902 IkReal x2071=((sj13)*(x2063));
08903 IkReal x2072=((cj13)*(x2064));
08904 evalcond[0]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2064)))+(((cj14)*(x2069)))+(cj12)+(((r00)*(sj14)*(x2063)))+(((IkReal(-1.00000000000000))*(x2065)*(x2070))));
08905 evalcond[1]=((((cj13)*(cj14)*(x2070)))+(((IkReal(-1.00000000000000))*(r11)*(x2065)*(x2072)))+(((r12)*(x2066)))+(((IkReal(-1.00000000000000))*(r02)*(x2071)))+(((r01)*(sj14)*(x2067)))+(((IkReal(-1.00000000000000))*(x2067)*(x2068))));
08906 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x2065)*(x2066)))+(((r02)*(x2067)))+(sj12)+(((cj14)*(r10)*(x2066)))+(((IkReal(-1.00000000000000))*(r12)*(x2072)))+(((sj13)*(sj14)*(x2069)))+(((IkReal(-1.00000000000000))*(x2068)*(x2071))));
08907 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
08908 {
08909 continue;
08910 }
08911 }
08912
08913 {
08914 IkReal dummyeval[1];
08915 dummyeval[0]=sj12;
08916 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08917 {
08918 {
08919 IkReal dummyeval[1];
08920 dummyeval[0]=cj12;
08921 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08922 {
08923 {
08924 IkReal evalcond[7];
08925 IkReal x2073=((IkReal(1.00000000000000))*(sj13));
08926 IkReal x2074=((cj14)*(sj9));
08927 IkReal x2075=((cj9)*(sj14));
08928 IkReal x2076=((cj13)*(sj9));
08929 IkReal x2077=((sj13)*(sj14));
08930 IkReal x2078=((cj9)*(sj13));
08931 IkReal x2079=((IkReal(1.00000000000000))*(cj13));
08932 IkReal x2080=((cj14)*(r10));
08933 IkReal x2081=((sj14)*(sj9));
08934 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
08935 evalcond[1]=((((IkReal(-1.00000000000000))*(r10)*(x2075)))+(((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r01)*(x2074)))+(((r00)*(x2081))));
08936 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2073)))+(((cj13)*(r22)))+(((r21)*(x2077))));
08937 evalcond[3]=((IkReal(0.0950000000000000))+(((npy)*(x2077)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2073))));
08938 evalcond[4]=((((r12)*(x2078)))+(((IkReal(-1.00000000000000))*(r00)*(x2074)*(x2079)))+(((cj13)*(cj9)*(x2080)))+(((IkReal(-1.00000000000000))*(r11)*(x2075)*(x2079)))+(((r01)*(sj14)*(x2076)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2073))));
08939 evalcond[5]=((IkReal(1.00000000000000))+(((x2078)*(x2080)))+(((r02)*(x2076)))+(((IkReal(-1.00000000000000))*(r00)*(x2073)*(x2074)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2079)))+(((IkReal(-1.00000000000000))*(r11)*(x2073)*(x2075)))+(((r01)*(sj9)*(x2077))));
08940 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x2073)*(x2081)))+(((IkReal(-1.00000000000000))*(r01)*(x2073)*(x2075)))+(((cj14)*(r00)*(x2078)))+(((r10)*(sj13)*(x2074)))+(((IkReal(-1.00000000000000))*(r12)*(x2076)))+(((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2079))));
08941 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 )
08942 {
08943 {
08944 IkReal j11array[1], cj11array[1], sj11array[1];
08945 bool j11valid[1]={false};
08946 _nj11 = 1;
08947 IkReal x2082=((IkReal(4.00000000000000))*(sj14));
08948 IkReal x2083=((IkReal(4.00000000000000))*(cj14));
08949 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
08950 continue;
08951 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13)))));
08952 sj11array[0]=IKsin(j11array[0]);
08953 cj11array[0]=IKcos(j11array[0]);
08954 if( j11array[0] > IKPI )
08955 {
08956 j11array[0]-=IK2PI;
08957 }
08958 else if( j11array[0] < -IKPI )
08959 { j11array[0]+=IK2PI;
08960 }
08961 j11valid[0] = true;
08962 for(int ij11 = 0; ij11 < 1; ++ij11)
08963 {
08964 if( !j11valid[ij11] )
08965 {
08966 continue;
08967 }
08968 _ij11[0] = ij11; _ij11[1] = -1;
08969 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
08970 {
08971 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
08972 {
08973 j11valid[iij11]=false; _ij11[1] = iij11; break;
08974 }
08975 }
08976 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
08977 {
08978 IkReal evalcond[2];
08979 evalcond[0]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
08980 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
08981 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
08982 {
08983 continue;
08984 }
08985 }
08986
08987 {
08988 IkReal dummyeval[1];
08989 IkReal gconst56;
08990 gconst56=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
08991 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
08992 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
08993 {
08994 {
08995 IkReal dummyeval[1];
08996 IkReal gconst57;
08997 gconst57=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
08998 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
08999 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09000 {
09001 continue;
09002
09003 } else
09004 {
09005 {
09006 IkReal j10array[1], cj10array[1], sj10array[1];
09007 bool j10valid[1]={false};
09008 _nj10 = 1;
09009 IkReal x2084=((IkReal(1.00000000000000))*(cj11));
09010 IkReal x2085=((r20)*(sj14));
09011 IkReal x2086=((cj14)*(r21));
09012 IkReal x2087=((cj14)*(cj9)*(r01));
09013 IkReal x2088=((r10)*(sj14)*(sj9));
09014 IkReal x2089=((cj9)*(r00)*(sj14));
09015 IkReal x2090=((cj14)*(r11)*(sj9));
09016 if( IKabs(((gconst57)*(((((cj11)*(x2086)))+(((cj11)*(x2085)))+(((sj11)*(x2087)))+(((sj11)*(x2089)))+(((sj11)*(x2088)))+(((sj11)*(x2090))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst57)*(((((IkReal(-1.00000000000000))*(x2084)*(x2087)))+(((IkReal(-1.00000000000000))*(x2084)*(x2088)))+(((IkReal(-1.00000000000000))*(x2084)*(x2089)))+(((sj11)*(x2086)))+(((sj11)*(x2085)))+(((IkReal(-1.00000000000000))*(x2084)*(x2090))))))) < IKFAST_ATAN2_MAGTHRESH )
09017 continue;
09018 j10array[0]=IKatan2(((gconst57)*(((((cj11)*(x2086)))+(((cj11)*(x2085)))+(((sj11)*(x2087)))+(((sj11)*(x2089)))+(((sj11)*(x2088)))+(((sj11)*(x2090)))))), ((gconst57)*(((((IkReal(-1.00000000000000))*(x2084)*(x2087)))+(((IkReal(-1.00000000000000))*(x2084)*(x2088)))+(((IkReal(-1.00000000000000))*(x2084)*(x2089)))+(((sj11)*(x2086)))+(((sj11)*(x2085)))+(((IkReal(-1.00000000000000))*(x2084)*(x2090)))))));
09019 sj10array[0]=IKsin(j10array[0]);
09020 cj10array[0]=IKcos(j10array[0]);
09021 if( j10array[0] > IKPI )
09022 {
09023 j10array[0]-=IK2PI;
09024 }
09025 else if( j10array[0] < -IKPI )
09026 { j10array[0]+=IK2PI;
09027 }
09028 j10valid[0] = true;
09029 for(int ij10 = 0; ij10 < 1; ++ij10)
09030 {
09031 if( !j10valid[ij10] )
09032 {
09033 continue;
09034 }
09035 _ij10[0] = ij10; _ij10[1] = -1;
09036 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09037 {
09038 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09039 {
09040 j10valid[iij10]=false; _ij10[1] = iij10; break;
09041 }
09042 }
09043 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09044 {
09045 IkReal evalcond[4];
09046 IkReal x2091=IKsin(j10);
09047 IkReal x2092=IKcos(j10);
09048 IkReal x2093=((cj13)*(sj14));
09049 IkReal x2094=((cj13)*(cj14));
09050 IkReal x2095=((r10)*(sj9));
09051 IkReal x2096=((IkReal(1.00000000000000))*(cj9));
09052 IkReal x2097=((sj11)*(x2091));
09053 IkReal x2098=((IkReal(1.00000000000000))*(x2092));
09054 IkReal x2099=((cj11)*(x2091));
09055 IkReal x2100=((IkReal(1.00000000000000))*(r11)*(sj9));
09056 IkReal x2101=((cj11)*(x2098));
09057 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(x2098)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2099))));
09058 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x2094)))+(((IkReal(-1.00000000000000))*(x2101)))+(x2097)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2093))));
09059 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x2100)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2096)))+(((IkReal(-1.00000000000000))*(x2101)))+(x2097)+(((IkReal(-1.00000000000000))*(sj14)*(x2095)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2096))));
09060 evalcond[3]=((((IkReal(-1.00000000000000))*(x2093)*(x2100)))+(((x2094)*(x2095)))+(x2099)+(((cj9)*(r02)*(sj13)))+(((sj11)*(x2092)))+(((IkReal(-1.00000000000000))*(r01)*(x2093)*(x2096)))+(((cj9)*(r00)*(x2094)))+(((r12)*(sj13)*(sj9))));
09061 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09062 {
09063 continue;
09064 }
09065 }
09066
09067 {
09068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09069 vinfos[0].jointtype = 1;
09070 vinfos[0].foffset = j9;
09071 vinfos[0].indices[0] = _ij9[0];
09072 vinfos[0].indices[1] = _ij9[1];
09073 vinfos[0].maxsolutions = _nj9;
09074 vinfos[1].jointtype = 1;
09075 vinfos[1].foffset = j10;
09076 vinfos[1].indices[0] = _ij10[0];
09077 vinfos[1].indices[1] = _ij10[1];
09078 vinfos[1].maxsolutions = _nj10;
09079 vinfos[2].jointtype = 1;
09080 vinfos[2].foffset = j11;
09081 vinfos[2].indices[0] = _ij11[0];
09082 vinfos[2].indices[1] = _ij11[1];
09083 vinfos[2].maxsolutions = _nj11;
09084 vinfos[3].jointtype = 1;
09085 vinfos[3].foffset = j12;
09086 vinfos[3].indices[0] = _ij12[0];
09087 vinfos[3].indices[1] = _ij12[1];
09088 vinfos[3].maxsolutions = _nj12;
09089 vinfos[4].jointtype = 1;
09090 vinfos[4].foffset = j13;
09091 vinfos[4].indices[0] = _ij13[0];
09092 vinfos[4].indices[1] = _ij13[1];
09093 vinfos[4].maxsolutions = _nj13;
09094 vinfos[5].jointtype = 1;
09095 vinfos[5].foffset = j14;
09096 vinfos[5].indices[0] = _ij14[0];
09097 vinfos[5].indices[1] = _ij14[1];
09098 vinfos[5].maxsolutions = _nj14;
09099 std::vector<int> vfree(0);
09100 solutions.AddSolution(vinfos,vfree);
09101 }
09102 }
09103 }
09104
09105 }
09106
09107 }
09108
09109 } else
09110 {
09111 {
09112 IkReal j10array[1], cj10array[1], sj10array[1];
09113 bool j10valid[1]={false};
09114 _nj10 = 1;
09115 IkReal x2102=((r20)*(sj14));
09116 IkReal x2103=((IkReal(1.00000000000000))*(cj13));
09117 IkReal x2104=((cj14)*(r20));
09118 IkReal x2105=((r21)*(sj14));
09119 IkReal x2106=((r22)*(sj13));
09120 IkReal x2107=((cj14)*(r21));
09121 if( IKabs(((gconst56)*(((((cj13)*(sj11)*(x2104)))+(((IkReal(-1.00000000000000))*(sj11)*(x2103)*(x2105)))+(((cj11)*(x2102)))+(((cj11)*(x2107)))+(((sj11)*(x2106))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2106)))+(((cj11)*(cj13)*(x2105)))+(((IkReal(-1.00000000000000))*(cj11)*(x2103)*(x2104)))+(((sj11)*(x2102)))+(((sj11)*(x2107))))))) < IKFAST_ATAN2_MAGTHRESH )
09122 continue;
09123 j10array[0]=IKatan2(((gconst56)*(((((cj13)*(sj11)*(x2104)))+(((IkReal(-1.00000000000000))*(sj11)*(x2103)*(x2105)))+(((cj11)*(x2102)))+(((cj11)*(x2107)))+(((sj11)*(x2106)))))), ((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2106)))+(((cj11)*(cj13)*(x2105)))+(((IkReal(-1.00000000000000))*(cj11)*(x2103)*(x2104)))+(((sj11)*(x2102)))+(((sj11)*(x2107)))))));
09124 sj10array[0]=IKsin(j10array[0]);
09125 cj10array[0]=IKcos(j10array[0]);
09126 if( j10array[0] > IKPI )
09127 {
09128 j10array[0]-=IK2PI;
09129 }
09130 else if( j10array[0] < -IKPI )
09131 { j10array[0]+=IK2PI;
09132 }
09133 j10valid[0] = true;
09134 for(int ij10 = 0; ij10 < 1; ++ij10)
09135 {
09136 if( !j10valid[ij10] )
09137 {
09138 continue;
09139 }
09140 _ij10[0] = ij10; _ij10[1] = -1;
09141 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09142 {
09143 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09144 {
09145 j10valid[iij10]=false; _ij10[1] = iij10; break;
09146 }
09147 }
09148 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09149 {
09150 IkReal evalcond[4];
09151 IkReal x2108=IKsin(j10);
09152 IkReal x2109=IKcos(j10);
09153 IkReal x2110=((cj13)*(sj14));
09154 IkReal x2111=((cj13)*(cj14));
09155 IkReal x2112=((r10)*(sj9));
09156 IkReal x2113=((IkReal(1.00000000000000))*(cj9));
09157 IkReal x2114=((sj11)*(x2108));
09158 IkReal x2115=((IkReal(1.00000000000000))*(x2109));
09159 IkReal x2116=((cj11)*(x2108));
09160 IkReal x2117=((IkReal(1.00000000000000))*(r11)*(sj9));
09161 IkReal x2118=((cj11)*(x2115));
09162 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2116)))+(((IkReal(-1.00000000000000))*(sj11)*(x2115))));
09163 evalcond[1]=((((r21)*(x2110)))+(x2114)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2118)))+(((IkReal(-1.00000000000000))*(r20)*(x2111))));
09164 evalcond[2]=((x2114)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2113)))+(((IkReal(-1.00000000000000))*(cj14)*(x2117)))+(((IkReal(-1.00000000000000))*(x2118)))+(((IkReal(-1.00000000000000))*(sj14)*(x2112)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2113))));
09165 evalcond[3]=((((cj9)*(r02)*(sj13)))+(x2116)+(((x2111)*(x2112)))+(((cj9)*(r00)*(x2111)))+(((IkReal(-1.00000000000000))*(x2110)*(x2117)))+(((IkReal(-1.00000000000000))*(r01)*(x2110)*(x2113)))+(((r12)*(sj13)*(sj9)))+(((sj11)*(x2109))));
09166 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09167 {
09168 continue;
09169 }
09170 }
09171
09172 {
09173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09174 vinfos[0].jointtype = 1;
09175 vinfos[0].foffset = j9;
09176 vinfos[0].indices[0] = _ij9[0];
09177 vinfos[0].indices[1] = _ij9[1];
09178 vinfos[0].maxsolutions = _nj9;
09179 vinfos[1].jointtype = 1;
09180 vinfos[1].foffset = j10;
09181 vinfos[1].indices[0] = _ij10[0];
09182 vinfos[1].indices[1] = _ij10[1];
09183 vinfos[1].maxsolutions = _nj10;
09184 vinfos[2].jointtype = 1;
09185 vinfos[2].foffset = j11;
09186 vinfos[2].indices[0] = _ij11[0];
09187 vinfos[2].indices[1] = _ij11[1];
09188 vinfos[2].maxsolutions = _nj11;
09189 vinfos[3].jointtype = 1;
09190 vinfos[3].foffset = j12;
09191 vinfos[3].indices[0] = _ij12[0];
09192 vinfos[3].indices[1] = _ij12[1];
09193 vinfos[3].maxsolutions = _nj12;
09194 vinfos[4].jointtype = 1;
09195 vinfos[4].foffset = j13;
09196 vinfos[4].indices[0] = _ij13[0];
09197 vinfos[4].indices[1] = _ij13[1];
09198 vinfos[4].maxsolutions = _nj13;
09199 vinfos[5].jointtype = 1;
09200 vinfos[5].foffset = j14;
09201 vinfos[5].indices[0] = _ij14[0];
09202 vinfos[5].indices[1] = _ij14[1];
09203 vinfos[5].maxsolutions = _nj14;
09204 std::vector<int> vfree(0);
09205 solutions.AddSolution(vinfos,vfree);
09206 }
09207 }
09208 }
09209
09210 }
09211
09212 }
09213 }
09214 }
09215
09216 } else
09217 {
09218 IkReal x2119=((IkReal(1.00000000000000))*(sj13));
09219 IkReal x2120=((cj14)*(sj9));
09220 IkReal x2121=((cj9)*(sj14));
09221 IkReal x2122=((cj13)*(sj9));
09222 IkReal x2123=((sj13)*(sj14));
09223 IkReal x2124=((cj9)*(sj13));
09224 IkReal x2125=((IkReal(1.00000000000000))*(cj13));
09225 IkReal x2126=((cj14)*(r10));
09226 IkReal x2127=((sj14)*(sj9));
09227 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
09228 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r01)*(x2120)))+(((IkReal(-1.00000000000000))*(r10)*(x2121)))+(((r00)*(x2127))));
09229 evalcond[2]=((((r21)*(x2123)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2119))));
09230 evalcond[3]=((IkReal(-0.0950000000000000))+(((npy)*(x2123)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2119))));
09231 evalcond[4]=((((r12)*(x2124)))+(((r01)*(sj14)*(x2122)))+(((cj13)*(cj9)*(x2126)))+(((IkReal(-1.00000000000000))*(r00)*(x2120)*(x2125)))+(((IkReal(-1.00000000000000))*(r11)*(x2121)*(x2125)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2119))));
09232 evalcond[5]=((IkReal(-1.00000000000000))+(((r01)*(sj9)*(x2123)))+(((IkReal(-1.00000000000000))*(r00)*(x2119)*(x2120)))+(((IkReal(-1.00000000000000))*(r11)*(x2119)*(x2121)))+(((r02)*(x2122)))+(((x2124)*(x2126)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2125))));
09233 evalcond[6]=((((IkReal(-1.00000000000000))*(r01)*(x2119)*(x2121)))+(((IkReal(-1.00000000000000))*(r11)*(x2119)*(x2127)))+(((IkReal(-1.00000000000000))*(r12)*(x2122)))+(((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2125)))+(((cj14)*(r00)*(x2124)))+(((r10)*(sj13)*(x2120))));
09234 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 )
09235 {
09236 {
09237 IkReal j11array[1], cj11array[1], sj11array[1];
09238 bool j11valid[1]={false};
09239 _nj11 = 1;
09240 IkReal x2128=((IkReal(4.00000000000000))*(sj14));
09241 IkReal x2129=((IkReal(4.00000000000000))*(cj14));
09242 if( IKabs(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
09243 continue;
09244 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13)))));
09245 sj11array[0]=IKsin(j11array[0]);
09246 cj11array[0]=IKcos(j11array[0]);
09247 if( j11array[0] > IKPI )
09248 {
09249 j11array[0]-=IK2PI;
09250 }
09251 else if( j11array[0] < -IKPI )
09252 { j11array[0]+=IK2PI;
09253 }
09254 j11valid[0] = true;
09255 for(int ij11 = 0; ij11 < 1; ++ij11)
09256 {
09257 if( !j11valid[ij11] )
09258 {
09259 continue;
09260 }
09261 _ij11[0] = ij11; _ij11[1] = -1;
09262 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
09263 {
09264 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
09265 {
09266 j11valid[iij11]=false; _ij11[1] = iij11; break;
09267 }
09268 }
09269 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
09270 {
09271 IkReal evalcond[2];
09272 evalcond[0]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((cj14)*(npy)))+(((npx)*(sj14))));
09273 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
09274 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09275 {
09276 continue;
09277 }
09278 }
09279
09280 {
09281 IkReal dummyeval[1];
09282 IkReal gconst58;
09283 gconst58=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
09284 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
09285 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09286 {
09287 {
09288 IkReal dummyeval[1];
09289 IkReal gconst59;
09290 gconst59=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
09291 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
09292 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09293 {
09294 continue;
09295
09296 } else
09297 {
09298 {
09299 IkReal j10array[1], cj10array[1], sj10array[1];
09300 bool j10valid[1]={false};
09301 _nj10 = 1;
09302 IkReal x2130=((IkReal(1.00000000000000))*(sj11));
09303 IkReal x2131=((cj14)*(r21));
09304 IkReal x2132=((IkReal(1.00000000000000))*(cj11));
09305 IkReal x2133=((r20)*(sj14));
09306 IkReal x2134=((cj9)*(r00)*(sj14));
09307 IkReal x2135=((cj14)*(r11)*(sj9));
09308 IkReal x2136=((cj14)*(cj9)*(r01));
09309 IkReal x2137=((r10)*(sj14)*(sj9));
09310 if( IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(x2132)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2137)))+(((IkReal(-1.00000000000000))*(x2130)*(x2136)))+(((IkReal(-1.00000000000000))*(x2130)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2134)))+(((IkReal(-1.00000000000000))*(x2131)*(x2132))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst59)*(((((cj11)*(x2136)))+(((cj11)*(x2137)))+(((cj11)*(x2134)))+(((cj11)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2131))))))) < IKFAST_ATAN2_MAGTHRESH )
09311 continue;
09312 j10array[0]=IKatan2(((gconst59)*(((((IkReal(-1.00000000000000))*(x2132)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2137)))+(((IkReal(-1.00000000000000))*(x2130)*(x2136)))+(((IkReal(-1.00000000000000))*(x2130)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2134)))+(((IkReal(-1.00000000000000))*(x2131)*(x2132)))))), ((gconst59)*(((((cj11)*(x2136)))+(((cj11)*(x2137)))+(((cj11)*(x2134)))+(((cj11)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2131)))))));
09313 sj10array[0]=IKsin(j10array[0]);
09314 cj10array[0]=IKcos(j10array[0]);
09315 if( j10array[0] > IKPI )
09316 {
09317 j10array[0]-=IK2PI;
09318 }
09319 else if( j10array[0] < -IKPI )
09320 { j10array[0]+=IK2PI;
09321 }
09322 j10valid[0] = true;
09323 for(int ij10 = 0; ij10 < 1; ++ij10)
09324 {
09325 if( !j10valid[ij10] )
09326 {
09327 continue;
09328 }
09329 _ij10[0] = ij10; _ij10[1] = -1;
09330 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09331 {
09332 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09333 {
09334 j10valid[iij10]=false; _ij10[1] = iij10; break;
09335 }
09336 }
09337 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09338 {
09339 IkReal evalcond[4];
09340 IkReal x2138=IKsin(j10);
09341 IkReal x2139=IKcos(j10);
09342 IkReal x2140=((cj14)*(sj9));
09343 IkReal x2141=((IkReal(1.00000000000000))*(r11));
09344 IkReal x2142=((cj13)*(sj14));
09345 IkReal x2143=((IkReal(1.00000000000000))*(cj9));
09346 IkReal x2144=((cj13)*(cj14));
09347 IkReal x2145=((cj11)*(x2138));
09348 IkReal x2146=((sj11)*(x2139));
09349 IkReal x2147=((cj11)*(x2139));
09350 IkReal x2148=((sj11)*(x2138));
09351 IkReal x2149=((x2146)+(x2145));
09352 evalcond[0]=((((cj14)*(r21)))+(x2149)+(((r20)*(sj14))));
09353 evalcond[1]=((((IkReal(-1.00000000000000))*(x2147)))+(((IkReal(-1.00000000000000))*(r20)*(x2144)))+(x2148)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2142))));
09354 evalcond[2]=((((IkReal(-1.00000000000000))*(x2148)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2143)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2143)))+(x2147)+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2140)*(x2141))));
09355 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x2142)*(x2143)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x2141)*(x2142)))+(x2149)+(((cj9)*(r00)*(x2144)))+(((r12)*(sj13)*(sj9)))+(((cj13)*(r10)*(x2140))));
09356 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09357 {
09358 continue;
09359 }
09360 }
09361
09362 {
09363 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09364 vinfos[0].jointtype = 1;
09365 vinfos[0].foffset = j9;
09366 vinfos[0].indices[0] = _ij9[0];
09367 vinfos[0].indices[1] = _ij9[1];
09368 vinfos[0].maxsolutions = _nj9;
09369 vinfos[1].jointtype = 1;
09370 vinfos[1].foffset = j10;
09371 vinfos[1].indices[0] = _ij10[0];
09372 vinfos[1].indices[1] = _ij10[1];
09373 vinfos[1].maxsolutions = _nj10;
09374 vinfos[2].jointtype = 1;
09375 vinfos[2].foffset = j11;
09376 vinfos[2].indices[0] = _ij11[0];
09377 vinfos[2].indices[1] = _ij11[1];
09378 vinfos[2].maxsolutions = _nj11;
09379 vinfos[3].jointtype = 1;
09380 vinfos[3].foffset = j12;
09381 vinfos[3].indices[0] = _ij12[0];
09382 vinfos[3].indices[1] = _ij12[1];
09383 vinfos[3].maxsolutions = _nj12;
09384 vinfos[4].jointtype = 1;
09385 vinfos[4].foffset = j13;
09386 vinfos[4].indices[0] = _ij13[0];
09387 vinfos[4].indices[1] = _ij13[1];
09388 vinfos[4].maxsolutions = _nj13;
09389 vinfos[5].jointtype = 1;
09390 vinfos[5].foffset = j14;
09391 vinfos[5].indices[0] = _ij14[0];
09392 vinfos[5].indices[1] = _ij14[1];
09393 vinfos[5].maxsolutions = _nj14;
09394 std::vector<int> vfree(0);
09395 solutions.AddSolution(vinfos,vfree);
09396 }
09397 }
09398 }
09399
09400 }
09401
09402 }
09403
09404 } else
09405 {
09406 {
09407 IkReal j10array[1], cj10array[1], sj10array[1];
09408 bool j10valid[1]={false};
09409 _nj10 = 1;
09410 IkReal x2150=((cj13)*(sj11));
09411 IkReal x2151=((r21)*(sj14));
09412 IkReal x2152=((cj14)*(r20));
09413 IkReal x2153=((cj11)*(cj13));
09414 IkReal x2154=((r22)*(sj13));
09415 IkReal x2155=((r20)*(sj14));
09416 IkReal x2156=((cj14)*(r21));
09417 if( IKabs(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2150)*(x2152)))+(((cj11)*(x2155)))+(((cj11)*(x2156)))+(((x2150)*(x2151))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst58)*(((((x2152)*(x2153)))+(((cj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2151)*(x2153)))+(((sj11)*(x2156)))+(((sj11)*(x2155))))))) < IKFAST_ATAN2_MAGTHRESH )
09418 continue;
09419 j10array[0]=IKatan2(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2150)*(x2152)))+(((cj11)*(x2155)))+(((cj11)*(x2156)))+(((x2150)*(x2151)))))), ((gconst58)*(((((x2152)*(x2153)))+(((cj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2151)*(x2153)))+(((sj11)*(x2156)))+(((sj11)*(x2155)))))));
09420 sj10array[0]=IKsin(j10array[0]);
09421 cj10array[0]=IKcos(j10array[0]);
09422 if( j10array[0] > IKPI )
09423 {
09424 j10array[0]-=IK2PI;
09425 }
09426 else if( j10array[0] < -IKPI )
09427 { j10array[0]+=IK2PI;
09428 }
09429 j10valid[0] = true;
09430 for(int ij10 = 0; ij10 < 1; ++ij10)
09431 {
09432 if( !j10valid[ij10] )
09433 {
09434 continue;
09435 }
09436 _ij10[0] = ij10; _ij10[1] = -1;
09437 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09438 {
09439 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09440 {
09441 j10valid[iij10]=false; _ij10[1] = iij10; break;
09442 }
09443 }
09444 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09445 {
09446 IkReal evalcond[4];
09447 IkReal x2157=IKsin(j10);
09448 IkReal x2158=IKcos(j10);
09449 IkReal x2159=((cj14)*(sj9));
09450 IkReal x2160=((IkReal(1.00000000000000))*(r11));
09451 IkReal x2161=((cj13)*(sj14));
09452 IkReal x2162=((IkReal(1.00000000000000))*(cj9));
09453 IkReal x2163=((cj13)*(cj14));
09454 IkReal x2164=((cj11)*(x2157));
09455 IkReal x2165=((sj11)*(x2158));
09456 IkReal x2166=((cj11)*(x2158));
09457 IkReal x2167=((sj11)*(x2157));
09458 IkReal x2168=((x2165)+(x2164));
09459 evalcond[0]=((((cj14)*(r21)))+(x2168)+(((r20)*(sj14))));
09460 evalcond[1]=((x2167)+(((r21)*(x2161)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2166)))+(((IkReal(-1.00000000000000))*(r20)*(x2163))));
09461 evalcond[2]=((x2166)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2162)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2159)*(x2160)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2162)))+(((IkReal(-1.00000000000000))*(x2167))));
09462 evalcond[3]=((((cj13)*(r10)*(x2159)))+(((cj9)*(r02)*(sj13)))+(x2168)+(((cj9)*(r00)*(x2163)))+(((IkReal(-1.00000000000000))*(sj9)*(x2160)*(x2161)))+(((IkReal(-1.00000000000000))*(r01)*(x2161)*(x2162)))+(((r12)*(sj13)*(sj9))));
09463 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09464 {
09465 continue;
09466 }
09467 }
09468
09469 {
09470 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09471 vinfos[0].jointtype = 1;
09472 vinfos[0].foffset = j9;
09473 vinfos[0].indices[0] = _ij9[0];
09474 vinfos[0].indices[1] = _ij9[1];
09475 vinfos[0].maxsolutions = _nj9;
09476 vinfos[1].jointtype = 1;
09477 vinfos[1].foffset = j10;
09478 vinfos[1].indices[0] = _ij10[0];
09479 vinfos[1].indices[1] = _ij10[1];
09480 vinfos[1].maxsolutions = _nj10;
09481 vinfos[2].jointtype = 1;
09482 vinfos[2].foffset = j11;
09483 vinfos[2].indices[0] = _ij11[0];
09484 vinfos[2].indices[1] = _ij11[1];
09485 vinfos[2].maxsolutions = _nj11;
09486 vinfos[3].jointtype = 1;
09487 vinfos[3].foffset = j12;
09488 vinfos[3].indices[0] = _ij12[0];
09489 vinfos[3].indices[1] = _ij12[1];
09490 vinfos[3].maxsolutions = _nj12;
09491 vinfos[4].jointtype = 1;
09492 vinfos[4].foffset = j13;
09493 vinfos[4].indices[0] = _ij13[0];
09494 vinfos[4].indices[1] = _ij13[1];
09495 vinfos[4].maxsolutions = _nj13;
09496 vinfos[5].jointtype = 1;
09497 vinfos[5].foffset = j14;
09498 vinfos[5].indices[0] = _ij14[0];
09499 vinfos[5].indices[1] = _ij14[1];
09500 vinfos[5].maxsolutions = _nj14;
09501 std::vector<int> vfree(0);
09502 solutions.AddSolution(vinfos,vfree);
09503 }
09504 }
09505 }
09506
09507 }
09508
09509 }
09510 }
09511 }
09512
09513 } else
09514 {
09515 IkReal x2169=((cj13)*(sj9));
09516 IkReal x2170=((r01)*(sj14));
09517 IkReal x2171=((cj9)*(sj13));
09518 IkReal x2172=((sj13)*(sj9));
09519 IkReal x2173=((cj14)*(r01));
09520 IkReal x2174=((cj14)*(r10));
09521 IkReal x2175=((cj13)*(cj9));
09522 IkReal x2176=((IkReal(1.00000000000000))*(cj9));
09523 IkReal x2177=((sj14)*(sj9));
09524 IkReal x2178=((IkReal(1.00000000000000))*(cj14)*(sj9));
09525 IkReal x2179=((sj14)*(x2176));
09526 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
09527 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
09528 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
09529 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(x2177)))+(((sj9)*(x2173)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2176)))+(((IkReal(-1.00000000000000))*(r10)*(x2179))));
09530 evalcond[4]=((((IkReal(-1.00000000000000))*(r10)*(x2177)))+(((IkReal(-1.00000000000000))*(x2173)*(x2176)))+(((IkReal(-1.00000000000000))*(r11)*(x2178)))+(((IkReal(-1.00000000000000))*(r00)*(x2179))));
09531 evalcond[5]=((((x2174)*(x2175)))+(((IkReal(-1.00000000000000))*(r02)*(x2172)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2175)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2169)))+(((x2169)*(x2170)))+(((r12)*(x2171))));
09532 evalcond[6]=((((IkReal(-1.00000000000000))*(r12)*(x2175)))+(((x2170)*(x2172)))+(((x2171)*(x2174)))+(((r02)*(x2169)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2171)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2172))));
09533 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 )
09534 {
09535 {
09536 IkReal j11array[1], cj11array[1], sj11array[1];
09537 bool j11valid[1]={false};
09538 _nj11 = 1;
09539 IkReal x2180=((IkReal(4.00000000000000))*(cj13));
09540 IkReal x2181=((npy)*(sj14));
09541 IkReal x2182=((cj14)*(npx));
09542 IkReal x2183=((IkReal(4.00000000000000))*(sj13));
09543 if( IKabs(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
09544 continue;
09545 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183)))), ((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13)))));
09546 sj11array[0]=IKsin(j11array[0]);
09547 cj11array[0]=IKcos(j11array[0]);
09548 if( j11array[0] > IKPI )
09549 {
09550 j11array[0]-=IK2PI;
09551 }
09552 else if( j11array[0] < -IKPI )
09553 { j11array[0]+=IK2PI;
09554 }
09555 j11valid[0] = true;
09556 for(int ij11 = 0; ij11 < 1; ++ij11)
09557 {
09558 if( !j11valid[ij11] )
09559 {
09560 continue;
09561 }
09562 _ij11[0] = ij11; _ij11[1] = -1;
09563 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
09564 {
09565 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
09566 {
09567 j11valid[iij11]=false; _ij11[1] = iij11; break;
09568 }
09569 }
09570 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
09571 {
09572 IkReal evalcond[2];
09573 IkReal x2184=((IkReal(1.00000000000000))*(sj13));
09574 IkReal x2185=((cj14)*(npx));
09575 IkReal x2186=((npy)*(sj14));
09576 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2184)))+(((cj13)*(x2186)))+(((IkReal(-1.00000000000000))*(cj13)*(x2185))));
09577 evalcond[1]=((IkReal(0.0300000000000000))+(((sj13)*(x2186)))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2184)*(x2185))));
09578 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09579 {
09580 continue;
09581 }
09582 }
09583
09584 {
09585 IkReal dummyeval[1];
09586 IkReal gconst60;
09587 gconst60=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
09588 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
09589 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09590 {
09591 {
09592 IkReal dummyeval[1];
09593 IkReal gconst61;
09594 gconst61=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
09595 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
09596 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09597 {
09598 continue;
09599
09600 } else
09601 {
09602 {
09603 IkReal j10array[1], cj10array[1], sj10array[1];
09604 bool j10valid[1]={false};
09605 _nj10 = 1;
09606 IkReal x2187=((cj13)*(sj14));
09607 IkReal x2188=((IkReal(1.00000000000000))*(cj11));
09608 IkReal x2189=((r11)*(sj9));
09609 IkReal x2190=((cj11)*(sj13));
09610 IkReal x2191=((r12)*(sj9));
09611 IkReal x2192=((cj9)*(r00));
09612 IkReal x2193=((sj11)*(sj13));
09613 IkReal x2194=((cj9)*(r02));
09614 IkReal x2195=((cj9)*(r01));
09615 IkReal x2196=((IkReal(1.00000000000000))*(sj11));
09616 IkReal x2197=((r10)*(sj9));
09617 IkReal x2198=((cj13)*(cj14)*(sj11));
09618 IkReal x2199=((cj11)*(cj13)*(cj14));
09619 if( IKabs(((gconst61)*(((((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2189)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2196)))+(((x2192)*(x2199)))+(((r21)*(sj11)*(x2187)))+(((x2190)*(x2194)))+(((x2190)*(x2191)))+(((IkReal(-1.00000000000000))*(r22)*(x2193)))+(((x2197)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2195))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst61)*(((((r22)*(x2190)))+(((x2191)*(x2193)))+(((x2193)*(x2194)))+(((x2192)*(x2198)))+(((r20)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2195)*(x2196)))+(((IkReal(-1.00000000000000))*(x2187)*(x2189)*(x2196)))+(((x2197)*(x2198)))+(((IkReal(-1.00000000000000))*(r21)*(x2187)*(x2188))))))) < IKFAST_ATAN2_MAGTHRESH )
09620 continue;
09621 j10array[0]=IKatan2(((gconst61)*(((((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2189)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2196)))+(((x2192)*(x2199)))+(((r21)*(sj11)*(x2187)))+(((x2190)*(x2194)))+(((x2190)*(x2191)))+(((IkReal(-1.00000000000000))*(r22)*(x2193)))+(((x2197)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2195)))))), ((gconst61)*(((((r22)*(x2190)))+(((x2191)*(x2193)))+(((x2193)*(x2194)))+(((x2192)*(x2198)))+(((r20)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2195)*(x2196)))+(((IkReal(-1.00000000000000))*(x2187)*(x2189)*(x2196)))+(((x2197)*(x2198)))+(((IkReal(-1.00000000000000))*(r21)*(x2187)*(x2188)))))));
09622 sj10array[0]=IKsin(j10array[0]);
09623 cj10array[0]=IKcos(j10array[0]);
09624 if( j10array[0] > IKPI )
09625 {
09626 j10array[0]-=IK2PI;
09627 }
09628 else if( j10array[0] < -IKPI )
09629 { j10array[0]+=IK2PI;
09630 }
09631 j10valid[0] = true;
09632 for(int ij10 = 0; ij10 < 1; ++ij10)
09633 {
09634 if( !j10valid[ij10] )
09635 {
09636 continue;
09637 }
09638 _ij10[0] = ij10; _ij10[1] = -1;
09639 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09640 {
09641 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09642 {
09643 j10valid[iij10]=false; _ij10[1] = iij10; break;
09644 }
09645 }
09646 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09647 {
09648 IkReal evalcond[4];
09649 IkReal x2200=IKsin(j10);
09650 IkReal x2201=IKcos(j10);
09651 IkReal x2202=((IkReal(1.00000000000000))*(sj13));
09652 IkReal x2203=((r11)*(sj9));
09653 IkReal x2204=((cj9)*(r01));
09654 IkReal x2205=((r21)*(sj14));
09655 IkReal x2206=((cj9)*(r02));
09656 IkReal x2207=((sj13)*(sj9));
09657 IkReal x2208=((cj14)*(r10));
09658 IkReal x2209=((IkReal(1.00000000000000))*(cj13));
09659 IkReal x2210=((cj14)*(r20));
09660 IkReal x2211=((cj11)*(x2200));
09661 IkReal x2212=((sj11)*(x2201));
09662 IkReal x2213=((sj14)*(x2209));
09663 IkReal x2214=((sj11)*(x2200));
09664 IkReal x2215=((cj11)*(x2201));
09665 IkReal x2216=((cj14)*(cj9)*(r00));
09666 IkReal x2217=((x2212)+(x2211));
09667 evalcond[0]=((((IkReal(-1.00000000000000))*(x2215)))+(x2214)+(((IkReal(-1.00000000000000))*(r22)*(x2202)))+(((cj13)*(x2205)))+(((IkReal(-1.00000000000000))*(x2209)*(x2210))));
09668 evalcond[1]=((x2217)+(((IkReal(-1.00000000000000))*(x2202)*(x2210)))+(((sj13)*(x2205)))+(((cj13)*(r22))));
09669 evalcond[2]=((x2217)+(((IkReal(-1.00000000000000))*(x2204)*(x2213)))+(((r12)*(x2207)))+(((cj13)*(x2216)))+(((cj13)*(sj9)*(x2208)))+(((sj13)*(x2206)))+(((IkReal(-1.00000000000000))*(x2203)*(x2213))));
09670 evalcond[3]=((((IkReal(-1.00000000000000))*(x2214)))+(x2215)+(((IkReal(-1.00000000000000))*(sj14)*(x2202)*(x2204)))+(((IkReal(-1.00000000000000))*(sj14)*(x2202)*(x2203)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2209)))+(((IkReal(-1.00000000000000))*(x2206)*(x2209)))+(((x2207)*(x2208)))+(((sj13)*(x2216))));
09671 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09672 {
09673 continue;
09674 }
09675 }
09676
09677 {
09678 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09679 vinfos[0].jointtype = 1;
09680 vinfos[0].foffset = j9;
09681 vinfos[0].indices[0] = _ij9[0];
09682 vinfos[0].indices[1] = _ij9[1];
09683 vinfos[0].maxsolutions = _nj9;
09684 vinfos[1].jointtype = 1;
09685 vinfos[1].foffset = j10;
09686 vinfos[1].indices[0] = _ij10[0];
09687 vinfos[1].indices[1] = _ij10[1];
09688 vinfos[1].maxsolutions = _nj10;
09689 vinfos[2].jointtype = 1;
09690 vinfos[2].foffset = j11;
09691 vinfos[2].indices[0] = _ij11[0];
09692 vinfos[2].indices[1] = _ij11[1];
09693 vinfos[2].maxsolutions = _nj11;
09694 vinfos[3].jointtype = 1;
09695 vinfos[3].foffset = j12;
09696 vinfos[3].indices[0] = _ij12[0];
09697 vinfos[3].indices[1] = _ij12[1];
09698 vinfos[3].maxsolutions = _nj12;
09699 vinfos[4].jointtype = 1;
09700 vinfos[4].foffset = j13;
09701 vinfos[4].indices[0] = _ij13[0];
09702 vinfos[4].indices[1] = _ij13[1];
09703 vinfos[4].maxsolutions = _nj13;
09704 vinfos[5].jointtype = 1;
09705 vinfos[5].foffset = j14;
09706 vinfos[5].indices[0] = _ij14[0];
09707 vinfos[5].indices[1] = _ij14[1];
09708 vinfos[5].maxsolutions = _nj14;
09709 std::vector<int> vfree(0);
09710 solutions.AddSolution(vinfos,vfree);
09711 }
09712 }
09713 }
09714
09715 }
09716
09717 }
09718
09719 } else
09720 {
09721 {
09722 IkReal j10array[1], cj10array[1], sj10array[1];
09723 bool j10valid[1]={false};
09724 _nj10 = 1;
09725 IkReal x2218=((cj13)*(sj11));
09726 IkReal x2219=((r21)*(sj14));
09727 IkReal x2220=((cj11)*(cj13));
09728 IkReal x2221=((sj11)*(sj13));
09729 IkReal x2222=((cj11)*(sj13));
09730 IkReal x2223=((IkReal(1.00000000000000))*(cj14)*(r20));
09731 if( IKabs(((gconst60)*(((((IkReal(-1.00000000000000))*(x2222)*(x2223)))+(((r22)*(x2220)))+(((IkReal(-1.00000000000000))*(r22)*(x2221)))+(((x2219)*(x2222)))+(((IkReal(-1.00000000000000))*(x2218)*(x2223)))+(((x2218)*(x2219))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst60)*(((((r22)*(x2222)))+(((IkReal(-1.00000000000000))*(x2219)*(x2220)))+(((IkReal(-1.00000000000000))*(x2221)*(x2223)))+(((x2219)*(x2221)))+(((r22)*(x2218)))+(((cj14)*(r20)*(x2220))))))) < IKFAST_ATAN2_MAGTHRESH )
09732 continue;
09733 j10array[0]=IKatan2(((gconst60)*(((((IkReal(-1.00000000000000))*(x2222)*(x2223)))+(((r22)*(x2220)))+(((IkReal(-1.00000000000000))*(r22)*(x2221)))+(((x2219)*(x2222)))+(((IkReal(-1.00000000000000))*(x2218)*(x2223)))+(((x2218)*(x2219)))))), ((gconst60)*(((((r22)*(x2222)))+(((IkReal(-1.00000000000000))*(x2219)*(x2220)))+(((IkReal(-1.00000000000000))*(x2221)*(x2223)))+(((x2219)*(x2221)))+(((r22)*(x2218)))+(((cj14)*(r20)*(x2220)))))));
09734 sj10array[0]=IKsin(j10array[0]);
09735 cj10array[0]=IKcos(j10array[0]);
09736 if( j10array[0] > IKPI )
09737 {
09738 j10array[0]-=IK2PI;
09739 }
09740 else if( j10array[0] < -IKPI )
09741 { j10array[0]+=IK2PI;
09742 }
09743 j10valid[0] = true;
09744 for(int ij10 = 0; ij10 < 1; ++ij10)
09745 {
09746 if( !j10valid[ij10] )
09747 {
09748 continue;
09749 }
09750 _ij10[0] = ij10; _ij10[1] = -1;
09751 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09752 {
09753 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09754 {
09755 j10valid[iij10]=false; _ij10[1] = iij10; break;
09756 }
09757 }
09758 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09759 {
09760 IkReal evalcond[4];
09761 IkReal x2224=IKsin(j10);
09762 IkReal x2225=IKcos(j10);
09763 IkReal x2226=((IkReal(1.00000000000000))*(sj13));
09764 IkReal x2227=((r11)*(sj9));
09765 IkReal x2228=((cj9)*(r01));
09766 IkReal x2229=((r21)*(sj14));
09767 IkReal x2230=((cj9)*(r02));
09768 IkReal x2231=((sj13)*(sj9));
09769 IkReal x2232=((cj14)*(r10));
09770 IkReal x2233=((IkReal(1.00000000000000))*(cj13));
09771 IkReal x2234=((cj14)*(r20));
09772 IkReal x2235=((cj11)*(x2224));
09773 IkReal x2236=((sj11)*(x2225));
09774 IkReal x2237=((sj14)*(x2233));
09775 IkReal x2238=((sj11)*(x2224));
09776 IkReal x2239=((cj11)*(x2225));
09777 IkReal x2240=((cj14)*(cj9)*(r00));
09778 IkReal x2241=((x2235)+(x2236));
09779 evalcond[0]=((((cj13)*(x2229)))+(x2238)+(((IkReal(-1.00000000000000))*(x2239)))+(((IkReal(-1.00000000000000))*(r22)*(x2226)))+(((IkReal(-1.00000000000000))*(x2233)*(x2234))));
09780 evalcond[1]=((x2241)+(((IkReal(-1.00000000000000))*(x2226)*(x2234)))+(((sj13)*(x2229)))+(((cj13)*(r22))));
09781 evalcond[2]=((x2241)+(((sj13)*(x2230)))+(((IkReal(-1.00000000000000))*(x2227)*(x2237)))+(((cj13)*(x2240)))+(((IkReal(-1.00000000000000))*(x2228)*(x2237)))+(((cj13)*(sj9)*(x2232)))+(((r12)*(x2231))));
09782 evalcond[3]=((x2239)+(((IkReal(-1.00000000000000))*(x2238)))+(((x2231)*(x2232)))+(((IkReal(-1.00000000000000))*(x2230)*(x2233)))+(((IkReal(-1.00000000000000))*(sj14)*(x2226)*(x2227)))+(((IkReal(-1.00000000000000))*(sj14)*(x2226)*(x2228)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2233)))+(((sj13)*(x2240))));
09783 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09784 {
09785 continue;
09786 }
09787 }
09788
09789 {
09790 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09791 vinfos[0].jointtype = 1;
09792 vinfos[0].foffset = j9;
09793 vinfos[0].indices[0] = _ij9[0];
09794 vinfos[0].indices[1] = _ij9[1];
09795 vinfos[0].maxsolutions = _nj9;
09796 vinfos[1].jointtype = 1;
09797 vinfos[1].foffset = j10;
09798 vinfos[1].indices[0] = _ij10[0];
09799 vinfos[1].indices[1] = _ij10[1];
09800 vinfos[1].maxsolutions = _nj10;
09801 vinfos[2].jointtype = 1;
09802 vinfos[2].foffset = j11;
09803 vinfos[2].indices[0] = _ij11[0];
09804 vinfos[2].indices[1] = _ij11[1];
09805 vinfos[2].maxsolutions = _nj11;
09806 vinfos[3].jointtype = 1;
09807 vinfos[3].foffset = j12;
09808 vinfos[3].indices[0] = _ij12[0];
09809 vinfos[3].indices[1] = _ij12[1];
09810 vinfos[3].maxsolutions = _nj12;
09811 vinfos[4].jointtype = 1;
09812 vinfos[4].foffset = j13;
09813 vinfos[4].indices[0] = _ij13[0];
09814 vinfos[4].indices[1] = _ij13[1];
09815 vinfos[4].maxsolutions = _nj13;
09816 vinfos[5].jointtype = 1;
09817 vinfos[5].foffset = j14;
09818 vinfos[5].indices[0] = _ij14[0];
09819 vinfos[5].indices[1] = _ij14[1];
09820 vinfos[5].maxsolutions = _nj14;
09821 std::vector<int> vfree(0);
09822 solutions.AddSolution(vinfos,vfree);
09823 }
09824 }
09825 }
09826
09827 }
09828
09829 }
09830 }
09831 }
09832
09833 } else
09834 {
09835 IkReal x2242=((cj13)*(sj9));
09836 IkReal x2243=((r01)*(sj14));
09837 IkReal x2244=((cj9)*(sj13));
09838 IkReal x2245=((sj13)*(sj9));
09839 IkReal x2246=((cj14)*(r01));
09840 IkReal x2247=((cj14)*(r10));
09841 IkReal x2248=((cj13)*(cj9));
09842 IkReal x2249=((IkReal(1.00000000000000))*(cj9));
09843 IkReal x2250=((sj14)*(sj9));
09844 IkReal x2251=((IkReal(1.00000000000000))*(cj14)*(sj9));
09845 IkReal x2252=((sj14)*(x2249));
09846 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
09847 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
09848 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
09849 evalcond[3]=((IkReal(-1.00000000000000))+(((sj9)*(x2246)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2249)))+(((IkReal(-1.00000000000000))*(r10)*(x2252)))+(((r00)*(x2250))));
09850 evalcond[4]=((((IkReal(-1.00000000000000))*(r10)*(x2250)))+(((IkReal(-1.00000000000000))*(r00)*(x2252)))+(((IkReal(-1.00000000000000))*(r11)*(x2251)))+(((IkReal(-1.00000000000000))*(x2246)*(x2249))));
09851 evalcond[5]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2248)))+(((IkReal(-1.00000000000000))*(r02)*(x2245)))+(((x2247)*(x2248)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2242)))+(((x2242)*(x2243)))+(((r12)*(x2244))));
09852 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2244)))+(((x2243)*(x2245)))+(((x2244)*(x2247)))+(((IkReal(-1.00000000000000))*(r12)*(x2248)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2245)))+(((r02)*(x2242))));
09853 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 )
09854 {
09855 {
09856 IkReal j11array[1], cj11array[1], sj11array[1];
09857 bool j11valid[1]={false};
09858 _nj11 = 1;
09859 IkReal x2253=((IkReal(4.00000000000000))*(cj13));
09860 IkReal x2254=((npy)*(sj14));
09861 IkReal x2255=((cj14)*(npx));
09862 IkReal x2256=((IkReal(4.00000000000000))*(sj13));
09863 if( IKabs(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13)))))+IKsqr(((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
09864 continue;
09865 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13)))), ((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13)))));
09866 sj11array[0]=IKsin(j11array[0]);
09867 cj11array[0]=IKcos(j11array[0]);
09868 if( j11array[0] > IKPI )
09869 {
09870 j11array[0]-=IK2PI;
09871 }
09872 else if( j11array[0] < -IKPI )
09873 { j11array[0]+=IK2PI;
09874 }
09875 j11valid[0] = true;
09876 for(int ij11 = 0; ij11 < 1; ++ij11)
09877 {
09878 if( !j11valid[ij11] )
09879 {
09880 continue;
09881 }
09882 _ij11[0] = ij11; _ij11[1] = -1;
09883 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
09884 {
09885 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
09886 {
09887 j11valid[iij11]=false; _ij11[1] = iij11; break;
09888 }
09889 }
09890 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
09891 {
09892 IkReal evalcond[2];
09893 IkReal x2257=((IkReal(1.00000000000000))*(sj13));
09894 IkReal x2258=((cj14)*(npx));
09895 IkReal x2259=((npy)*(sj14));
09896 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2257)))+(((IkReal(-1.00000000000000))*(cj13)*(x2258)))+(((cj13)*(x2259))));
09897 evalcond[1]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x2257)*(x2258)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x2259)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
09898 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
09899 {
09900 continue;
09901 }
09902 }
09903
09904 {
09905 IkReal dummyeval[1];
09906 IkReal gconst62;
09907 gconst62=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
09908 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
09909 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09910 {
09911 {
09912 IkReal dummyeval[1];
09913 IkReal gconst63;
09914 gconst63=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
09915 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
09916 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
09917 {
09918 continue;
09919
09920 } else
09921 {
09922 {
09923 IkReal j10array[1], cj10array[1], sj10array[1];
09924 bool j10valid[1]={false};
09925 _nj10 = 1;
09926 IkReal x2260=((cj13)*(sj14));
09927 IkReal x2261=((IkReal(1.00000000000000))*(cj11));
09928 IkReal x2262=((r11)*(sj9));
09929 IkReal x2263=((IkReal(1.00000000000000))*(sj11));
09930 IkReal x2264=((cj13)*(cj14));
09931 IkReal x2265=((cj11)*(sj13));
09932 IkReal x2266=((r12)*(sj9));
09933 IkReal x2267=((sj11)*(sj13));
09934 IkReal x2268=((cj9)*(r02));
09935 IkReal x2269=((cj9)*(r01));
09936 IkReal x2270=((r10)*(sj9));
09937 IkReal x2271=((cj9)*(r00));
09938 if( IKabs(((gconst63)*(((((r21)*(sj11)*(x2260)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2269)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2262)))+(((IkReal(-1.00000000000000))*(r20)*(x2263)*(x2264)))+(((cj11)*(x2264)*(x2271)))+(((cj11)*(x2264)*(x2270)))+(((x2265)*(x2266)))+(((x2265)*(x2268))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst63)*(((((IkReal(-1.00000000000000))*(x2260)*(x2262)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2263)*(x2269)))+(((r22)*(x2265)))+(((sj11)*(x2264)*(x2270)))+(((sj11)*(x2264)*(x2271)))+(((IkReal(-1.00000000000000))*(r21)*(x2260)*(x2261)))+(((x2266)*(x2267)))+(((x2267)*(x2268)))+(((cj11)*(r20)*(x2264))))))) < IKFAST_ATAN2_MAGTHRESH )
09939 continue;
09940 j10array[0]=IKatan2(((gconst63)*(((((r21)*(sj11)*(x2260)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2269)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2262)))+(((IkReal(-1.00000000000000))*(r20)*(x2263)*(x2264)))+(((cj11)*(x2264)*(x2271)))+(((cj11)*(x2264)*(x2270)))+(((x2265)*(x2266)))+(((x2265)*(x2268)))))), ((gconst63)*(((((IkReal(-1.00000000000000))*(x2260)*(x2262)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2263)*(x2269)))+(((r22)*(x2265)))+(((sj11)*(x2264)*(x2270)))+(((sj11)*(x2264)*(x2271)))+(((IkReal(-1.00000000000000))*(r21)*(x2260)*(x2261)))+(((x2266)*(x2267)))+(((x2267)*(x2268)))+(((cj11)*(r20)*(x2264)))))));
09941 sj10array[0]=IKsin(j10array[0]);
09942 cj10array[0]=IKcos(j10array[0]);
09943 if( j10array[0] > IKPI )
09944 {
09945 j10array[0]-=IK2PI;
09946 }
09947 else if( j10array[0] < -IKPI )
09948 { j10array[0]+=IK2PI;
09949 }
09950 j10valid[0] = true;
09951 for(int ij10 = 0; ij10 < 1; ++ij10)
09952 {
09953 if( !j10valid[ij10] )
09954 {
09955 continue;
09956 }
09957 _ij10[0] = ij10; _ij10[1] = -1;
09958 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
09959 {
09960 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
09961 {
09962 j10valid[iij10]=false; _ij10[1] = iij10; break;
09963 }
09964 }
09965 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
09966 {
09967 IkReal evalcond[4];
09968 IkReal x2272=IKsin(j10);
09969 IkReal x2273=IKcos(j10);
09970 IkReal x2274=((IkReal(1.00000000000000))*(sj13));
09971 IkReal x2275=((r11)*(sj9));
09972 IkReal x2276=((cj9)*(r01));
09973 IkReal x2277=((IkReal(1.00000000000000))*(cj11));
09974 IkReal x2278=((r21)*(sj14));
09975 IkReal x2279=((cj9)*(r02));
09976 IkReal x2280=((sj13)*(sj9));
09977 IkReal x2281=((cj14)*(r10));
09978 IkReal x2282=((IkReal(1.00000000000000))*(cj13));
09979 IkReal x2283=((cj14)*(r20));
09980 IkReal x2284=((sj11)*(x2272));
09981 IkReal x2285=((sj14)*(x2282));
09982 IkReal x2286=((sj11)*(x2273));
09983 IkReal x2287=((cj14)*(cj9)*(r00));
09984 IkReal x2288=((x2273)*(x2277));
09985 evalcond[0]=((((IkReal(-1.00000000000000))*(x2282)*(x2283)))+(x2284)+(((cj13)*(x2278)))+(((IkReal(-1.00000000000000))*(x2288)))+(((IkReal(-1.00000000000000))*(r22)*(x2274))));
09986 evalcond[1]=((((IkReal(-1.00000000000000))*(x2286)))+(((sj13)*(x2278)))+(((IkReal(-1.00000000000000))*(x2274)*(x2283)))+(((IkReal(-1.00000000000000))*(x2272)*(x2277)))+(((cj13)*(r22))));
09987 evalcond[2]=((((IkReal(-1.00000000000000))*(x2276)*(x2285)))+(x2286)+(((cj13)*(sj9)*(x2281)))+(((r12)*(x2280)))+(((cj11)*(x2272)))+(((sj13)*(x2279)))+(((cj13)*(x2287)))+(((IkReal(-1.00000000000000))*(x2275)*(x2285))));
09988 evalcond[3]=((x2284)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2282)))+(((IkReal(-1.00000000000000))*(x2288)))+(((x2280)*(x2281)))+(((IkReal(-1.00000000000000))*(x2279)*(x2282)))+(((IkReal(-1.00000000000000))*(sj14)*(x2274)*(x2275)))+(((IkReal(-1.00000000000000))*(sj14)*(x2274)*(x2276)))+(((sj13)*(x2287))));
09989 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
09990 {
09991 continue;
09992 }
09993 }
09994
09995 {
09996 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
09997 vinfos[0].jointtype = 1;
09998 vinfos[0].foffset = j9;
09999 vinfos[0].indices[0] = _ij9[0];
10000 vinfos[0].indices[1] = _ij9[1];
10001 vinfos[0].maxsolutions = _nj9;
10002 vinfos[1].jointtype = 1;
10003 vinfos[1].foffset = j10;
10004 vinfos[1].indices[0] = _ij10[0];
10005 vinfos[1].indices[1] = _ij10[1];
10006 vinfos[1].maxsolutions = _nj10;
10007 vinfos[2].jointtype = 1;
10008 vinfos[2].foffset = j11;
10009 vinfos[2].indices[0] = _ij11[0];
10010 vinfos[2].indices[1] = _ij11[1];
10011 vinfos[2].maxsolutions = _nj11;
10012 vinfos[3].jointtype = 1;
10013 vinfos[3].foffset = j12;
10014 vinfos[3].indices[0] = _ij12[0];
10015 vinfos[3].indices[1] = _ij12[1];
10016 vinfos[3].maxsolutions = _nj12;
10017 vinfos[4].jointtype = 1;
10018 vinfos[4].foffset = j13;
10019 vinfos[4].indices[0] = _ij13[0];
10020 vinfos[4].indices[1] = _ij13[1];
10021 vinfos[4].maxsolutions = _nj13;
10022 vinfos[5].jointtype = 1;
10023 vinfos[5].foffset = j14;
10024 vinfos[5].indices[0] = _ij14[0];
10025 vinfos[5].indices[1] = _ij14[1];
10026 vinfos[5].maxsolutions = _nj14;
10027 std::vector<int> vfree(0);
10028 solutions.AddSolution(vinfos,vfree);
10029 }
10030 }
10031 }
10032
10033 }
10034
10035 }
10036
10037 } else
10038 {
10039 {
10040 IkReal j10array[1], cj10array[1], sj10array[1];
10041 bool j10valid[1]={false};
10042 _nj10 = 1;
10043 IkReal x2289=((cj13)*(r22));
10044 IkReal x2290=((IkReal(1.00000000000000))*(sj11));
10045 IkReal x2291=((IkReal(1.00000000000000))*(cj11));
10046 IkReal x2292=((r22)*(sj13));
10047 IkReal x2293=((cj14)*(r20));
10048 IkReal x2294=((cj13)*(r21)*(sj14));
10049 IkReal x2295=((r21)*(sj13)*(sj14));
10050 if( IKabs(((gconst62)*(((((sj11)*(x2292)))+(((cj13)*(sj11)*(x2293)))+(((IkReal(-1.00000000000000))*(sj13)*(x2291)*(x2293)))+(((cj11)*(x2295)))+(((IkReal(-1.00000000000000))*(x2290)*(x2294)))+(((cj11)*(x2289))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst62)*(((((sj11)*(x2295)))+(((sj11)*(x2289)))+(((IkReal(-1.00000000000000))*(cj13)*(x2291)*(x2293)))+(((IkReal(-1.00000000000000))*(x2291)*(x2292)))+(((cj11)*(x2294)))+(((IkReal(-1.00000000000000))*(sj13)*(x2290)*(x2293))))))) < IKFAST_ATAN2_MAGTHRESH )
10051 continue;
10052 j10array[0]=IKatan2(((gconst62)*(((((sj11)*(x2292)))+(((cj13)*(sj11)*(x2293)))+(((IkReal(-1.00000000000000))*(sj13)*(x2291)*(x2293)))+(((cj11)*(x2295)))+(((IkReal(-1.00000000000000))*(x2290)*(x2294)))+(((cj11)*(x2289)))))), ((gconst62)*(((((sj11)*(x2295)))+(((sj11)*(x2289)))+(((IkReal(-1.00000000000000))*(cj13)*(x2291)*(x2293)))+(((IkReal(-1.00000000000000))*(x2291)*(x2292)))+(((cj11)*(x2294)))+(((IkReal(-1.00000000000000))*(sj13)*(x2290)*(x2293)))))));
10053 sj10array[0]=IKsin(j10array[0]);
10054 cj10array[0]=IKcos(j10array[0]);
10055 if( j10array[0] > IKPI )
10056 {
10057 j10array[0]-=IK2PI;
10058 }
10059 else if( j10array[0] < -IKPI )
10060 { j10array[0]+=IK2PI;
10061 }
10062 j10valid[0] = true;
10063 for(int ij10 = 0; ij10 < 1; ++ij10)
10064 {
10065 if( !j10valid[ij10] )
10066 {
10067 continue;
10068 }
10069 _ij10[0] = ij10; _ij10[1] = -1;
10070 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10071 {
10072 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10073 {
10074 j10valid[iij10]=false; _ij10[1] = iij10; break;
10075 }
10076 }
10077 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10078 {
10079 IkReal evalcond[4];
10080 IkReal x2296=IKsin(j10);
10081 IkReal x2297=IKcos(j10);
10082 IkReal x2298=((IkReal(1.00000000000000))*(sj13));
10083 IkReal x2299=((r11)*(sj9));
10084 IkReal x2300=((cj9)*(r01));
10085 IkReal x2301=((IkReal(1.00000000000000))*(cj11));
10086 IkReal x2302=((r21)*(sj14));
10087 IkReal x2303=((cj9)*(r02));
10088 IkReal x2304=((sj13)*(sj9));
10089 IkReal x2305=((cj14)*(r10));
10090 IkReal x2306=((IkReal(1.00000000000000))*(cj13));
10091 IkReal x2307=((cj14)*(r20));
10092 IkReal x2308=((sj11)*(x2296));
10093 IkReal x2309=((sj14)*(x2306));
10094 IkReal x2310=((sj11)*(x2297));
10095 IkReal x2311=((cj14)*(cj9)*(r00));
10096 IkReal x2312=((x2297)*(x2301));
10097 evalcond[0]=((((IkReal(-1.00000000000000))*(x2306)*(x2307)))+(((cj13)*(x2302)))+(x2308)+(((IkReal(-1.00000000000000))*(r22)*(x2298)))+(((IkReal(-1.00000000000000))*(x2312))));
10098 evalcond[1]=((((IkReal(-1.00000000000000))*(x2310)))+(((sj13)*(x2302)))+(((IkReal(-1.00000000000000))*(x2296)*(x2301)))+(((IkReal(-1.00000000000000))*(x2298)*(x2307)))+(((cj13)*(r22))));
10099 evalcond[2]=((((r12)*(x2304)))+(((sj13)*(x2303)))+(((IkReal(-1.00000000000000))*(x2300)*(x2309)))+(x2310)+(((IkReal(-1.00000000000000))*(x2299)*(x2309)))+(((cj11)*(x2296)))+(((cj13)*(x2311)))+(((cj13)*(sj9)*(x2305))));
10100 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2306)))+(((x2304)*(x2305)))+(x2308)+(((IkReal(-1.00000000000000))*(x2303)*(x2306)))+(((IkReal(-1.00000000000000))*(sj14)*(x2298)*(x2299)))+(((sj13)*(x2311)))+(((IkReal(-1.00000000000000))*(sj14)*(x2298)*(x2300)))+(((IkReal(-1.00000000000000))*(x2312))));
10101 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10102 {
10103 continue;
10104 }
10105 }
10106
10107 {
10108 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10109 vinfos[0].jointtype = 1;
10110 vinfos[0].foffset = j9;
10111 vinfos[0].indices[0] = _ij9[0];
10112 vinfos[0].indices[1] = _ij9[1];
10113 vinfos[0].maxsolutions = _nj9;
10114 vinfos[1].jointtype = 1;
10115 vinfos[1].foffset = j10;
10116 vinfos[1].indices[0] = _ij10[0];
10117 vinfos[1].indices[1] = _ij10[1];
10118 vinfos[1].maxsolutions = _nj10;
10119 vinfos[2].jointtype = 1;
10120 vinfos[2].foffset = j11;
10121 vinfos[2].indices[0] = _ij11[0];
10122 vinfos[2].indices[1] = _ij11[1];
10123 vinfos[2].maxsolutions = _nj11;
10124 vinfos[3].jointtype = 1;
10125 vinfos[3].foffset = j12;
10126 vinfos[3].indices[0] = _ij12[0];
10127 vinfos[3].indices[1] = _ij12[1];
10128 vinfos[3].maxsolutions = _nj12;
10129 vinfos[4].jointtype = 1;
10130 vinfos[4].foffset = j13;
10131 vinfos[4].indices[0] = _ij13[0];
10132 vinfos[4].indices[1] = _ij13[1];
10133 vinfos[4].maxsolutions = _nj13;
10134 vinfos[5].jointtype = 1;
10135 vinfos[5].foffset = j14;
10136 vinfos[5].indices[0] = _ij14[0];
10137 vinfos[5].indices[1] = _ij14[1];
10138 vinfos[5].maxsolutions = _nj14;
10139 std::vector<int> vfree(0);
10140 solutions.AddSolution(vinfos,vfree);
10141 }
10142 }
10143 }
10144
10145 }
10146
10147 }
10148 }
10149 }
10150
10151 } else
10152 {
10153 if( 1 )
10154 {
10155 continue;
10156
10157 } else
10158 {
10159 }
10160 }
10161 }
10162 }
10163 }
10164 }
10165
10166 } else
10167 {
10168 {
10169 IkReal j11array[1], cj11array[1], sj11array[1];
10170 bool j11valid[1]={false};
10171 _nj11 = 1;
10172 IkReal x2313=((IkReal(4.00000000000000))*(cj13));
10173 IkReal x2314=((npy)*(sj14));
10174 IkReal x2315=((IkReal(4.00000000000000))*(sj13));
10175 IkReal x2316=((cj14)*(npx));
10176 if( IKabs(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315)))))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316)))))-1) <= IKFAST_SINCOS_THRESH )
10177 continue;
10178 j11array[0]=IKatan2(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315)))))), ((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316)))));
10179 sj11array[0]=IKsin(j11array[0]);
10180 cj11array[0]=IKcos(j11array[0]);
10181 if( j11array[0] > IKPI )
10182 {
10183 j11array[0]-=IK2PI;
10184 }
10185 else if( j11array[0] < -IKPI )
10186 { j11array[0]+=IK2PI;
10187 }
10188 j11valid[0] = true;
10189 for(int ij11 = 0; ij11 < 1; ++ij11)
10190 {
10191 if( !j11valid[ij11] )
10192 {
10193 continue;
10194 }
10195 _ij11[0] = ij11; _ij11[1] = -1;
10196 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
10197 {
10198 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
10199 {
10200 j11valid[iij11]=false; _ij11[1] = iij11; break;
10201 }
10202 }
10203 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
10204 {
10205 IkReal evalcond[3];
10206 IkReal x2317=IKsin(j11);
10207 IkReal x2318=((IkReal(1.00000000000000))*(sj13));
10208 IkReal x2319=((cj14)*(npx));
10209 IkReal x2320=((npy)*(sj14));
10210 IkReal x2321=((IkReal(0.250000000000000))*(x2317));
10211 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14)))+(((sj12)*(x2321))));
10212 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(x2319)))+(((IkReal(-1.00000000000000))*(npz)*(x2318)))+(((cj13)*(x2320))));
10213 evalcond[2]=((((IkReal(-1.00000000000000))*(x2318)*(x2319)))+(((IkReal(0.0950000000000000))*(sj12)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(cj12)*(x2321)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x2320))));
10214 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
10215 {
10216 continue;
10217 }
10218 }
10219
10220 {
10221 IkReal dummyeval[1];
10222 IkReal gconst46;
10223 gconst46=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
10224 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
10225 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10226 {
10227 {
10228 IkReal dummyeval[1];
10229 IkReal gconst47;
10230 gconst47=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
10231 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
10232 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10233 {
10234 {
10235 IkReal evalcond[9];
10236 IkReal x2322=((r00)*(sj9));
10237 IkReal x2323=((IkReal(1.00000000000000))*(sj13));
10238 IkReal x2324=((cj13)*(sj14));
10239 IkReal x2325=((cj9)*(sj14));
10240 IkReal x2326=((cj13)*(r02));
10241 IkReal x2327=((sj13)*(sj14));
10242 IkReal x2328=((r01)*(sj9));
10243 IkReal x2329=((cj9)*(sj13));
10244 IkReal x2330=((IkReal(1.00000000000000))*(cj9));
10245 IkReal x2331=((cj14)*(r10));
10246 IkReal x2332=((cj14)*(npx));
10247 IkReal x2333=((IkReal(1.00000000000000))*(cj13));
10248 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
10249 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
10250 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x2332)*(x2333)))+(((npy)*(x2324)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2323))));
10251 evalcond[3]=((((sj14)*(x2322)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2330)))+(((cj14)*(x2328)))+(((IkReal(-1.00000000000000))*(r10)*(x2325))));
10252 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2323)))+(((r21)*(x2327)))+(((cj13)*(r22))));
10253 evalcond[5]=((IkReal(0.0950000000000000))+(((npy)*(x2327)))+(((cj13)*(npz)))+(((IkReal(-1.00000000000000))*(x2323)*(x2332)))+(((IkReal(0.0900000000000000))*(sj13))));
10254 evalcond[6]=((((r12)*(x2329)))+(((IkReal(-1.00000000000000))*(cj14)*(x2322)*(x2333)))+(((cj13)*(cj9)*(x2331)))+(((IkReal(-1.00000000000000))*(r11)*(x2324)*(x2330)))+(((x2324)*(x2328)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2323))));
10255 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x2323)*(x2325)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2330)))+(((x2329)*(x2331)))+(((x2327)*(x2328)))+(((IkReal(-1.00000000000000))*(cj14)*(x2322)*(x2323)))+(((sj9)*(x2326))));
10256 evalcond[8]=((((sj13)*(sj9)*(x2331)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2323)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2333)))+(((cj14)*(r00)*(x2329)))+(((IkReal(-1.00000000000000))*(r01)*(x2323)*(x2325)))+(((IkReal(-1.00000000000000))*(x2326)*(x2330))));
10257 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 )
10258 {
10259 {
10260 IkReal dummyeval[1];
10261 IkReal gconst48;
10262 gconst48=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
10263 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
10264 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10265 {
10266 {
10267 IkReal dummyeval[1];
10268 IkReal gconst49;
10269 gconst49=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
10270 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
10271 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10272 {
10273 continue;
10274
10275 } else
10276 {
10277 {
10278 IkReal j10array[1], cj10array[1], sj10array[1];
10279 bool j10valid[1]={false};
10280 _nj10 = 1;
10281 IkReal x2334=((IkReal(1.00000000000000))*(cj11));
10282 IkReal x2335=((sj11)*(sj14));
10283 IkReal x2336=((r10)*(sj9));
10284 IkReal x2337=((cj9)*(r00));
10285 IkReal x2338=((cj14)*(sj11));
10286 IkReal x2339=((r11)*(sj9));
10287 IkReal x2340=((cj14)*(cj9)*(r01));
10288 if( IKabs(((gconst49)*(((((cj11)*(cj14)*(r21)))+(((cj9)*(r01)*(x2338)))+(((cj11)*(r20)*(sj14)))+(((x2335)*(x2337)))+(((x2335)*(x2336)))+(((x2338)*(x2339))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((r21)*(x2338)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2337)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2336)))+(((IkReal(-1.00000000000000))*(x2334)*(x2340)))+(((IkReal(-1.00000000000000))*(cj14)*(x2334)*(x2339)))+(((r20)*(x2335))))))) < IKFAST_ATAN2_MAGTHRESH )
10289 continue;
10290 j10array[0]=IKatan2(((gconst49)*(((((cj11)*(cj14)*(r21)))+(((cj9)*(r01)*(x2338)))+(((cj11)*(r20)*(sj14)))+(((x2335)*(x2337)))+(((x2335)*(x2336)))+(((x2338)*(x2339)))))), ((gconst49)*(((((r21)*(x2338)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2337)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2336)))+(((IkReal(-1.00000000000000))*(x2334)*(x2340)))+(((IkReal(-1.00000000000000))*(cj14)*(x2334)*(x2339)))+(((r20)*(x2335)))))));
10291 sj10array[0]=IKsin(j10array[0]);
10292 cj10array[0]=IKcos(j10array[0]);
10293 if( j10array[0] > IKPI )
10294 {
10295 j10array[0]-=IK2PI;
10296 }
10297 else if( j10array[0] < -IKPI )
10298 { j10array[0]+=IK2PI;
10299 }
10300 j10valid[0] = true;
10301 for(int ij10 = 0; ij10 < 1; ++ij10)
10302 {
10303 if( !j10valid[ij10] )
10304 {
10305 continue;
10306 }
10307 _ij10[0] = ij10; _ij10[1] = -1;
10308 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10309 {
10310 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10311 {
10312 j10valid[iij10]=false; _ij10[1] = iij10; break;
10313 }
10314 }
10315 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10316 {
10317 IkReal evalcond[4];
10318 IkReal x2341=IKsin(j10);
10319 IkReal x2342=IKcos(j10);
10320 IkReal x2343=((cj13)*(sj14));
10321 IkReal x2344=((cj13)*(cj14));
10322 IkReal x2345=((r10)*(sj9));
10323 IkReal x2346=((IkReal(1.00000000000000))*(cj9));
10324 IkReal x2347=((sj11)*(x2341));
10325 IkReal x2348=((IkReal(1.00000000000000))*(x2342));
10326 IkReal x2349=((cj11)*(x2341));
10327 IkReal x2350=((IkReal(1.00000000000000))*(r11)*(sj9));
10328 IkReal x2351=((cj11)*(x2348));
10329 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2349)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2348))));
10330 evalcond[1]=((x2347)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x2344)))+(((r21)*(x2343)))+(((IkReal(-1.00000000000000))*(x2351))));
10331 evalcond[2]=((x2347)+(((IkReal(-1.00000000000000))*(sj14)*(x2345)))+(((IkReal(-1.00000000000000))*(cj14)*(x2350)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2346)))+(((IkReal(-1.00000000000000))*(x2351)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2346))));
10332 evalcond[3]=((x2349)+(((cj9)*(r00)*(x2344)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x2343)*(x2346)))+(((x2344)*(x2345)))+(((IkReal(-1.00000000000000))*(x2343)*(x2350)))+(((r12)*(sj13)*(sj9)))+(((sj11)*(x2342))));
10333 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10334 {
10335 continue;
10336 }
10337 }
10338
10339 {
10340 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10341 vinfos[0].jointtype = 1;
10342 vinfos[0].foffset = j9;
10343 vinfos[0].indices[0] = _ij9[0];
10344 vinfos[0].indices[1] = _ij9[1];
10345 vinfos[0].maxsolutions = _nj9;
10346 vinfos[1].jointtype = 1;
10347 vinfos[1].foffset = j10;
10348 vinfos[1].indices[0] = _ij10[0];
10349 vinfos[1].indices[1] = _ij10[1];
10350 vinfos[1].maxsolutions = _nj10;
10351 vinfos[2].jointtype = 1;
10352 vinfos[2].foffset = j11;
10353 vinfos[2].indices[0] = _ij11[0];
10354 vinfos[2].indices[1] = _ij11[1];
10355 vinfos[2].maxsolutions = _nj11;
10356 vinfos[3].jointtype = 1;
10357 vinfos[3].foffset = j12;
10358 vinfos[3].indices[0] = _ij12[0];
10359 vinfos[3].indices[1] = _ij12[1];
10360 vinfos[3].maxsolutions = _nj12;
10361 vinfos[4].jointtype = 1;
10362 vinfos[4].foffset = j13;
10363 vinfos[4].indices[0] = _ij13[0];
10364 vinfos[4].indices[1] = _ij13[1];
10365 vinfos[4].maxsolutions = _nj13;
10366 vinfos[5].jointtype = 1;
10367 vinfos[5].foffset = j14;
10368 vinfos[5].indices[0] = _ij14[0];
10369 vinfos[5].indices[1] = _ij14[1];
10370 vinfos[5].maxsolutions = _nj14;
10371 std::vector<int> vfree(0);
10372 solutions.AddSolution(vinfos,vfree);
10373 }
10374 }
10375 }
10376
10377 }
10378
10379 }
10380
10381 } else
10382 {
10383 {
10384 IkReal j10array[1], cj10array[1], sj10array[1];
10385 bool j10valid[1]={false};
10386 _nj10 = 1;
10387 IkReal x2352=((cj11)*(r20));
10388 IkReal x2353=((IkReal(1.00000000000000))*(cj13));
10389 IkReal x2354=((r21)*(sj14));
10390 IkReal x2355=((r22)*(sj13));
10391 IkReal x2356=((r20)*(sj11));
10392 IkReal x2357=((cj14)*(r21));
10393 if( IKabs(((gconst48)*(((((cj11)*(x2357)))+(((IkReal(-1.00000000000000))*(sj11)*(x2353)*(x2354)))+(((sj11)*(x2355)))+(((cj13)*(cj14)*(x2356)))+(((sj14)*(x2352))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((sj11)*(x2357)))+(((cj11)*(cj13)*(x2354)))+(((IkReal(-1.00000000000000))*(cj11)*(x2355)))+(((IkReal(-1.00000000000000))*(cj14)*(x2352)*(x2353)))+(((sj14)*(x2356))))))) < IKFAST_ATAN2_MAGTHRESH )
10394 continue;
10395 j10array[0]=IKatan2(((gconst48)*(((((cj11)*(x2357)))+(((IkReal(-1.00000000000000))*(sj11)*(x2353)*(x2354)))+(((sj11)*(x2355)))+(((cj13)*(cj14)*(x2356)))+(((sj14)*(x2352)))))), ((gconst48)*(((((sj11)*(x2357)))+(((cj11)*(cj13)*(x2354)))+(((IkReal(-1.00000000000000))*(cj11)*(x2355)))+(((IkReal(-1.00000000000000))*(cj14)*(x2352)*(x2353)))+(((sj14)*(x2356)))))));
10396 sj10array[0]=IKsin(j10array[0]);
10397 cj10array[0]=IKcos(j10array[0]);
10398 if( j10array[0] > IKPI )
10399 {
10400 j10array[0]-=IK2PI;
10401 }
10402 else if( j10array[0] < -IKPI )
10403 { j10array[0]+=IK2PI;
10404 }
10405 j10valid[0] = true;
10406 for(int ij10 = 0; ij10 < 1; ++ij10)
10407 {
10408 if( !j10valid[ij10] )
10409 {
10410 continue;
10411 }
10412 _ij10[0] = ij10; _ij10[1] = -1;
10413 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10414 {
10415 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10416 {
10417 j10valid[iij10]=false; _ij10[1] = iij10; break;
10418 }
10419 }
10420 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10421 {
10422 IkReal evalcond[4];
10423 IkReal x2358=IKsin(j10);
10424 IkReal x2359=IKcos(j10);
10425 IkReal x2360=((cj13)*(sj14));
10426 IkReal x2361=((cj13)*(cj14));
10427 IkReal x2362=((r10)*(sj9));
10428 IkReal x2363=((IkReal(1.00000000000000))*(cj9));
10429 IkReal x2364=((sj11)*(x2358));
10430 IkReal x2365=((IkReal(1.00000000000000))*(x2359));
10431 IkReal x2366=((cj11)*(x2358));
10432 IkReal x2367=((IkReal(1.00000000000000))*(r11)*(sj9));
10433 IkReal x2368=((cj11)*(x2365));
10434 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2366)))+(((IkReal(-1.00000000000000))*(sj11)*(x2365))));
10435 evalcond[1]=((x2364)+(((IkReal(-1.00000000000000))*(x2368)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2360)))+(((IkReal(-1.00000000000000))*(r20)*(x2361))));
10436 evalcond[2]=((x2364)+(((IkReal(-1.00000000000000))*(x2368)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2363)))+(((IkReal(-1.00000000000000))*(cj14)*(x2367)))+(((IkReal(-1.00000000000000))*(sj14)*(x2362)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2363))));
10437 evalcond[3]=((((sj11)*(x2359)))+(x2366)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x2360)*(x2363)))+(((x2361)*(x2362)))+(((cj9)*(r00)*(x2361)))+(((IkReal(-1.00000000000000))*(x2360)*(x2367)))+(((r12)*(sj13)*(sj9))));
10438 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10439 {
10440 continue;
10441 }
10442 }
10443
10444 {
10445 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10446 vinfos[0].jointtype = 1;
10447 vinfos[0].foffset = j9;
10448 vinfos[0].indices[0] = _ij9[0];
10449 vinfos[0].indices[1] = _ij9[1];
10450 vinfos[0].maxsolutions = _nj9;
10451 vinfos[1].jointtype = 1;
10452 vinfos[1].foffset = j10;
10453 vinfos[1].indices[0] = _ij10[0];
10454 vinfos[1].indices[1] = _ij10[1];
10455 vinfos[1].maxsolutions = _nj10;
10456 vinfos[2].jointtype = 1;
10457 vinfos[2].foffset = j11;
10458 vinfos[2].indices[0] = _ij11[0];
10459 vinfos[2].indices[1] = _ij11[1];
10460 vinfos[2].maxsolutions = _nj11;
10461 vinfos[3].jointtype = 1;
10462 vinfos[3].foffset = j12;
10463 vinfos[3].indices[0] = _ij12[0];
10464 vinfos[3].indices[1] = _ij12[1];
10465 vinfos[3].maxsolutions = _nj12;
10466 vinfos[4].jointtype = 1;
10467 vinfos[4].foffset = j13;
10468 vinfos[4].indices[0] = _ij13[0];
10469 vinfos[4].indices[1] = _ij13[1];
10470 vinfos[4].maxsolutions = _nj13;
10471 vinfos[5].jointtype = 1;
10472 vinfos[5].foffset = j14;
10473 vinfos[5].indices[0] = _ij14[0];
10474 vinfos[5].indices[1] = _ij14[1];
10475 vinfos[5].maxsolutions = _nj14;
10476 std::vector<int> vfree(0);
10477 solutions.AddSolution(vinfos,vfree);
10478 }
10479 }
10480 }
10481
10482 }
10483
10484 }
10485
10486 } else
10487 {
10488 IkReal x2369=((r00)*(sj9));
10489 IkReal x2370=((IkReal(1.00000000000000))*(sj13));
10490 IkReal x2371=((cj13)*(sj14));
10491 IkReal x2372=((cj9)*(sj14));
10492 IkReal x2373=((cj13)*(r02));
10493 IkReal x2374=((sj13)*(sj14));
10494 IkReal x2375=((r01)*(sj9));
10495 IkReal x2376=((cj9)*(sj13));
10496 IkReal x2377=((IkReal(1.00000000000000))*(cj9));
10497 IkReal x2378=((cj14)*(r10));
10498 IkReal x2379=((cj14)*(npx));
10499 IkReal x2380=((IkReal(1.00000000000000))*(cj13));
10500 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
10501 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
10502 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2370)))+(((IkReal(-1.00000000000000))*(x2379)*(x2380)))+(((npy)*(x2371))));
10503 evalcond[3]=((((cj14)*(x2375)))+(((IkReal(-1.00000000000000))*(r10)*(x2372)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2377)))+(((sj14)*(x2369))));
10504 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2370)))+(((cj13)*(r22)))+(((r21)*(x2374))));
10505 evalcond[5]=((IkReal(-0.0950000000000000))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2370)*(x2379)))+(((npy)*(x2374))));
10506 evalcond[6]=((((r12)*(x2376)))+(((x2371)*(x2375)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2370)))+(((IkReal(-1.00000000000000))*(cj14)*(x2369)*(x2380)))+(((IkReal(-1.00000000000000))*(r11)*(x2371)*(x2377)))+(((cj13)*(cj9)*(x2378))));
10507 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(x2369)*(x2370)))+(((IkReal(-1.00000000000000))*(r11)*(x2370)*(x2372)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2377)))+(((x2374)*(x2375)))+(((sj9)*(x2373)))+(((x2376)*(x2378))));
10508 evalcond[8]=((((sj13)*(sj9)*(x2378)))+(((cj14)*(r00)*(x2376)))+(((IkReal(-1.00000000000000))*(r01)*(x2370)*(x2372)))+(((IkReal(-1.00000000000000))*(x2373)*(x2377)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2380)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2370))));
10509 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 )
10510 {
10511 {
10512 IkReal dummyeval[1];
10513 IkReal gconst50;
10514 gconst50=