fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
23 
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
27 
28 #include <cmath>
29 #include <vector>
30 #include <limits>
31 #include <algorithm>
32 #include <complex>
33 
34 #ifndef IKFAST_ASSERT
35 #include <stdexcept>
36 #include <sstream>
37 #include <iostream>
38 
39 #ifdef _MSC_VER
40 #ifndef __PRETTY_FUNCTION__
41 #define __PRETTY_FUNCTION__ __FUNCDNAME__
42 #endif
43 #endif
44 
45 #ifndef __PRETTY_FUNCTION__
46 #define __PRETTY_FUNCTION__ __func__
47 #endif
48 
49 #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()); } }
50 
51 #endif
52 
53 #if defined(_MSC_VER)
54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
55 #else
56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
57 #endif
58 
59 #define IK2PI ((IkReal)6.28318530717959)
60 #define IKPI ((IkReal)3.14159265358979)
61 #define IKPI_2 ((IkReal)1.57079632679490)
62 
63 #ifdef _MSC_VER
64 #ifndef isnan
65 #define isnan _isnan
66 #endif
67 #ifndef isinf
68 #define isinf _isinf
69 #endif
70 //#ifndef isfinite
71 //#define isfinite _isfinite
72 //#endif
73 #endif // _MSC_VER
74 
75 // lapack routines
76 extern "C" {
77  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
78  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
79  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
80  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
81  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);
82  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);
83 }
84 
85 using namespace std; // necessary to get std math routines
86 
87 #ifdef IKFAST_NAMESPACE
88 namespace IKFAST_NAMESPACE {
89 #endif
90 
91 inline float IKabs(float f) { return fabsf(f); }
92 inline double IKabs(double f) { return fabs(f); }
93 
94 inline float IKsqr(float f) { return f*f; }
95 inline double IKsqr(double f) { return f*f; }
96 
97 inline float IKlog(float f) { return logf(f); }
98 inline double IKlog(double f) { return log(f); }
99 
100 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
101 #ifndef IKFAST_SINCOS_THRESH
102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
103 #endif
104 
105 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
106 #ifndef IKFAST_ATAN2_MAGTHRESH
107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
108 #endif
109 
110 // minimum distance of separate solutions
111 #ifndef IKFAST_SOLUTION_THRESH
112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
113 #endif
114 
115 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
116 #ifndef IKFAST_EVALCOND_THRESH
117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
118 #endif
119 
120 
121 inline float IKasin(float f)
122 {
123 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
124 if( f <= -1 ) return float(-IKPI_2);
125 else if( f >= 1 ) return float(IKPI_2);
126 return asinf(f);
127 }
128 inline double IKasin(double f)
129 {
130 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
131 if( f <= -1 ) return -IKPI_2;
132 else if( f >= 1 ) return IKPI_2;
133 return asin(f);
134 }
135 
136 // return positive value in [0,y)
137 inline float IKfmod(float x, float y)
138 {
139  while(x < 0) {
140  x += y;
141  }
142  return fmodf(x,y);
143 }
144 
145 // return positive value in [0,y)
146 inline double IKfmod(double x, double y)
147 {
148  while(x < 0) {
149  x += y;
150  }
151  return fmod(x,y);
152 }
153 
154 inline float IKacos(float f)
155 {
156 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
157 if( f <= -1 ) return float(IKPI);
158 else if( f >= 1 ) return float(0);
159 return acosf(f);
160 }
161 inline double IKacos(double f)
162 {
163 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
164 if( f <= -1 ) return IKPI;
165 else if( f >= 1 ) return 0;
166 return acos(f);
167 }
168 inline float IKsin(float f) { return sinf(f); }
169 inline double IKsin(double f) { return sin(f); }
170 inline float IKcos(float f) { return cosf(f); }
171 inline double IKcos(double f) { return cos(f); }
172 inline float IKtan(float f) { return tanf(f); }
173 inline double IKtan(double f) { return tan(f); }
174 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
175 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
176 inline float IKatan2Simple(float fy, float fx) {
177  return atan2f(fy,fx);
178 }
179 inline float IKatan2(float fy, float fx) {
180  if( isnan(fy) ) {
181  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
182  return float(IKPI_2);
183  }
184  else if( isnan(fx) ) {
185  return 0;
186  }
187  return atan2f(fy,fx);
188 }
189 inline double IKatan2Simple(double fy, double fx) {
190  return atan2(fy,fx);
191 }
192 inline double IKatan2(double fy, double fx) {
193  if( isnan(fy) ) {
194  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
195  return IKPI_2;
196  }
197  else if( isnan(fx) ) {
198  return 0;
199  }
200  return atan2(fy,fx);
201 }
202 
203 template <typename T>
204 struct CheckValue
205 {
206  T value;
207  bool valid;
208 };
209 
210 template <typename T>
211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
212 {
213  CheckValue<T> ret;
214  ret.valid = false;
215  ret.value = 0;
216  if( !isnan(fy) && !isnan(fx) ) {
217  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
218  ret.value = IKatan2Simple(fy,fx);
219  ret.valid = true;
220  }
221  }
222  return ret;
223 }
224 
225 inline float IKsign(float f) {
226  if( f > 0 ) {
227  return float(1);
228  }
229  else if( f < 0 ) {
230  return float(-1);
231  }
232  return 0;
233 }
234 
235 inline double IKsign(double f) {
236  if( f > 0 ) {
237  return 1.0;
238  }
239  else if( f < 0 ) {
240  return -1.0;
241  }
242  return 0;
243 }
244 
245 template <typename T>
246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
247 {
248  CheckValue<T> ret;
249  ret.valid = true;
250  if( n == 0 ) {
251  ret.value = 1.0;
252  return ret;
253  }
254  else if( n == 1 )
255  {
256  ret.value = f;
257  return ret;
258  }
259  else if( n < 0 )
260  {
261  if( f == 0 )
262  {
263  ret.valid = false;
264  ret.value = (T)1.0e30;
265  return ret;
266  }
267  if( n == -1 ) {
268  ret.value = T(1.0)/f;
269  return ret;
270  }
271  }
272 
273  int num = n > 0 ? n : -n;
274  if( num == 2 ) {
275  ret.value = f*f;
276  }
277  else if( num == 3 ) {
278  ret.value = f*f*f;
279  }
280  else {
281  ret.value = 1.0;
282  while(num>0) {
283  if( num & 1 ) {
284  ret.value *= f;
285  }
286  num >>= 1;
287  f *= f;
288  }
289  }
290 
291  if( n < 0 ) {
292  ret.value = T(1.0)/ret.value;
293  }
294  return ret;
295 }
296 
299 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
300 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;
301 x0=IKcos(j[0]);
302 x1=IKcos(j[3]);
303 x2=IKcos(j[2]);
304 x3=IKsin(j[1]);
305 x4=IKsin(j[0]);
306 x5=IKsin(j[3]);
307 x6=IKcos(j[1]);
308 x7=IKsin(j[2]);
309 x8=IKsin(j[5]);
310 x9=IKcos(j[5]);
311 x10=IKcos(j[4]);
312 x11=IKsin(j[4]);
313 x12=((0.08)*x2);
314 x13=((0.42)*x4);
315 x14=((0.035)*x2);
316 x15=((1.0)*x5);
317 x16=((1.0)*x4);
318 x17=((1.0)*x2);
319 x18=((1.0)*x0);
320 x19=((0.42)*x0);
321 x20=((0.08)*x5);
322 x21=((0.08)*x1);
323 x22=(x3*x7);
324 x23=(x2*x6);
325 x24=(x0*x3);
326 x25=(x6*x7);
327 x26=(x3*x4);
328 x27=(x1*x11);
329 x28=(x1*x16);
330 x29=(x17*x3);
331 x30=(x18*x25);
332 x31=(x16*x25);
333 x32=(x22+x23);
334 x33=((((-1.0)*x30))+((x2*x24)));
335 x34=((((-1.0)*x31))+((x2*x26)));
336 x35=((((-1.0)*x17*x24))+x30);
337 x36=((((-1.0)*x16*x2*x3))+x31);
338 x37=(x1*x33);
339 x38=(x1*x34);
340 x39=(x36*x5);
341 x40=(x37+(((-1.0)*x15*x4)));
342 x41=(((x0*x5))+x38);
343 x42=(((x11*(((((-1.0)*x25))+x29))))+((x1*x10*x32)));
344 x43=(((x10*x40))+((x11*(((((-1.0)*x0*x17*x6))+(((-1.0)*x18*x22)))))));
345 IkReal x45=((1.0)*x16);
346 x44=(((x10*x41))+((x11*(((((-1.0)*x22*x45))+(((-1.0)*x23*x45)))))));
347 eerot[0]=(((x8*(((((-1.0)*x28))+((x35*x5))))))+((x43*x9)));
348 eerot[1]=(((x43*x8))+((x9*((x28+(((-1.0)*x15*x35)))))));
349 eerot[2]=(((x11*x40))+((x10*((((x0*x22))+((x0*x23)))))));
350 eetrans[0]=((((-0.035)*x0*x25))+(((0.44)*x24))+((x11*((((x21*x33))+(((-1.0)*x20*x4))))))+((x19*x22))+((x19*x23))+(((0.05)*x0))+((x14*x24))+((x10*(((((0.08)*x0*x22))+((x0*x12*x6)))))));
351 eerot[3]=(((x44*x9))+((x8*((((x0*x1))+x39)))));
352 eerot[4]=(((x9*(((((-1.0)*x1*x18))+(((-1.0)*x15*x36))))))+((x44*x8)));
353 eerot[5]=(((x10*((((x22*x4))+((x23*x4))))))+((x11*x41)));
354 eetrans[1]=(((x10*(((((0.08)*x22*x4))+((x12*x4*x6))))))+(((0.44)*x26))+(((0.05)*x4))+((x14*x26))+(((-0.035)*x25*x4))+((x11*((((x0*x20))+((x21*x34))))))+((x13*x23))+((x13*x22)));
355 eerot[6]=(((x5*x8*(((((-1.0)*x22))+(((-1.0)*x17*x6))))))+((x42*x9)));
356 eerot[7]=(((x42*x8))+((x32*x5*x9)));
357 eerot[8]=(((x27*x32))+((x10*(((((-1.0)*x29))+x25)))));
358 eetrans[2]=((0.33)+((x14*x6))+((x27*(((((0.08)*x22))+((x12*x6))))))+((x10*(((((-1.0)*x12*x3))+(((0.08)*x25))))))+(((0.42)*x25))+(((0.44)*x6))+(((0.035)*x22))+(((-0.42)*x2*x3)));
359 }
360 
361 IKFAST_API int GetNumFreeParameters() { return 0; }
362 IKFAST_API int* GetFreeParameters() { return NULL; }
363 IKFAST_API int GetNumJoints() { return 6; }
364 
365 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
366 
367 IKFAST_API int GetIkType() { return 0x67000001; }
368 
369 class IKSolver {
370 public:
371 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,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;
372 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
373 
374 IkReal j100, cj100, sj100;
375 unsigned char _ij100[2], _nj100;
376 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
377 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
378 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
379  solutions.Clear();
380 r00 = eerot[0*3+0];
381 r01 = eerot[0*3+1];
382 r02 = eerot[0*3+2];
383 r10 = eerot[1*3+0];
384 r11 = eerot[1*3+1];
385 r12 = eerot[1*3+2];
386 r20 = eerot[2*3+0];
387 r21 = eerot[2*3+1];
388 r22 = eerot[2*3+2];
389 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
390 
391 new_r00=r00;
392 new_r01=((-1.0)*r01);
393 new_r02=((-1.0)*r02);
394 new_px=((((-0.08)*r02))+px);
395 new_r10=r10;
396 new_r11=((-1.0)*r11);
397 new_r12=((-1.0)*r12);
398 new_py=((((-0.08)*r12))+py);
399 new_r20=r20;
400 new_r21=((-1.0)*r21);
401 new_r22=((-1.0)*r22);
402 new_pz=((-0.33)+(((-0.08)*r22))+pz);
403 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;
404 IkReal x46=((1.0)*px);
405 IkReal x47=((1.0)*pz);
406 IkReal x48=((1.0)*py);
407 pp=((px*px)+(py*py)+(pz*pz));
408 npx=(((px*r00))+((py*r10))+((pz*r20)));
409 npy=(((px*r01))+((py*r11))+((pz*r21)));
410 npz=(((px*r02))+((py*r12))+((pz*r22)));
411 rxp0_0=((((-1.0)*r20*x48))+((pz*r10)));
412 rxp0_1=(((px*r20))+(((-1.0)*r00*x47)));
413 rxp0_2=((((-1.0)*r10*x46))+((py*r00)));
414 rxp1_0=((((-1.0)*r21*x48))+((pz*r11)));
415 rxp1_1=(((px*r21))+(((-1.0)*r01*x47)));
416 rxp1_2=((((-1.0)*r11*x46))+((py*r01)));
417 rxp2_0=((((-1.0)*r22*x48))+((pz*r12)));
418 rxp2_1=((((-1.0)*r02*x47))+((px*r22)));
419 rxp2_2=((((-1.0)*r12*x46))+((py*r02)));
420 {
421 IkReal j0eval[1];
422 j0eval[0]=((IKabs(px))+(IKabs(py)));
423 if( IKabs(j0eval[0]) < 0.0000010000000000 )
424 {
425 continue; // no branches [j0, j1, j2]
426 
427 } else
428 {
429 {
430 IkReal j0array[2], cj0array[2], sj0array[2];
431 bool j0valid[2]={false};
432 _nj0 = 2;
433 CheckValue<IkReal> x50 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
434 if(!x50.valid){
435 continue;
436 }
437 IkReal x49=x50.value;
438 j0array[0]=((-1.0)*x49);
439 sj0array[0]=IKsin(j0array[0]);
440 cj0array[0]=IKcos(j0array[0]);
441 j0array[1]=((3.14159265358979)+(((-1.0)*x49)));
442 sj0array[1]=IKsin(j0array[1]);
443 cj0array[1]=IKcos(j0array[1]);
444 if( j0array[0] > IKPI )
445 {
446  j0array[0]-=IK2PI;
447 }
448 else if( j0array[0] < -IKPI )
449 { j0array[0]+=IK2PI;
450 }
451 j0valid[0] = true;
452 if( j0array[1] > IKPI )
453 {
454  j0array[1]-=IK2PI;
455 }
456 else if( j0array[1] < -IKPI )
457 { j0array[1]+=IK2PI;
458 }
459 j0valid[1] = true;
460 for(int ij0 = 0; ij0 < 2; ++ij0)
461 {
462 if( !j0valid[ij0] )
463 {
464  continue;
465 }
466 _ij0[0] = ij0; _ij0[1] = -1;
467 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
468 {
469 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
470 {
471  j0valid[iij0]=false; _ij0[1] = iij0; break;
472 }
473 }
474 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
475 
476 {
477 IkReal j2array[2], cj2array[2], sj2array[2];
478 bool j2valid[2]={false};
479 _nj2 = 2;
480 if( (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) < -1-IKFAST_SINCOS_THRESH || (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) > 1+IKFAST_SINCOS_THRESH )
481  continue;
482 IkReal x51=IKasin(((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px))));
483 j2array[0]=((-0.0831412318884412)+(((-1.0)*x51)));
484 sj2array[0]=IKsin(j2array[0]);
485 cj2array[0]=IKcos(j2array[0]);
486 j2array[1]=((3.05845142170135)+x51);
487 sj2array[1]=IKsin(j2array[1]);
488 cj2array[1]=IKcos(j2array[1]);
489 if( j2array[0] > IKPI )
490 {
491  j2array[0]-=IK2PI;
492 }
493 else if( j2array[0] < -IKPI )
494 { j2array[0]+=IK2PI;
495 }
496 j2valid[0] = true;
497 if( j2array[1] > IKPI )
498 {
499  j2array[1]-=IK2PI;
500 }
501 else if( j2array[1] < -IKPI )
502 { j2array[1]+=IK2PI;
503 }
504 j2valid[1] = true;
505 for(int ij2 = 0; ij2 < 2; ++ij2)
506 {
507 if( !j2valid[ij2] )
508 {
509  continue;
510 }
511 _ij2[0] = ij2; _ij2[1] = -1;
512 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
513 {
514 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
515 {
516  j2valid[iij2]=false; _ij2[1] = iij2; break;
517 }
518 }
519 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
520 
521 {
522 IkReal j1eval[3];
523 IkReal x52=cj2*cj2;
524 IkReal x53=(py*sj0);
525 IkReal x54=((240.0)*cj2);
526 IkReal x55=((20.0)*sj2);
527 IkReal x56=((0.42)*cj2);
528 IkReal x57=(cj0*px);
529 IkReal x58=(cj2*sj2);
530 IkReal x59=((0.035)*sj2);
531 IkReal x60=(pz*sj2);
532 IkReal x61=(cj2*pz);
533 j1eval[0]=((((-1.0)*x53*x54))+(((12.0)*cj2))+(((-1.0)*sj2))+(((20.0)*x61))+(((251.428571428571)*pz))+(((-1.0)*x54*x57))+((x53*x55))+(((240.0)*x60))+((x55*x57)));
534 j1eval[1]=((IKabs(((-0.001225)+(((-0.175175)*x52))+(pz*pz)+(((0.0294)*x58)))))+(IKabs(((0.0147)+(((-0.0294)*x52))+(((-0.05)*pz))+(((-0.175175)*x58))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x57))+((pz*x53))))));
535 j1eval[2]=IKsign(((((-1.0)*x53*x56))+(((0.021)*cj2))+(((-1.0)*x56*x57))+(((0.42)*x60))+(((0.035)*x61))+((x57*x59))+(((-0.00175)*sj2))+((x53*x59))+(((0.44)*pz))));
536 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
537 {
538 {
539 IkReal j1eval[2];
540 IkReal x62=(py*sj0);
541 IkReal x63=(pz*sj2);
542 IkReal x64=(cj0*px);
543 IkReal x65=((240.0)*sj2);
544 IkReal x66=((0.035)*cj2);
545 IkReal x67=((20.0)*cj2);
546 IkReal x68=(cj2*pz);
547 IkReal x69=((0.42)*sj2);
548 j1eval[0]=((12.5714285714286)+cj2+(((12.0)*sj2))+(((20.0)*x63))+(((-251.428571428571)*x62))+(((-251.428571428571)*x64))+(((-240.0)*x68))+(((-1.0)*x64*x67))+(((-1.0)*x64*x65))+(((-1.0)*x62*x67))+(((-1.0)*x62*x65)));
549 j1eval[1]=IKsign(((0.022)+(((0.035)*x63))+(((-0.44)*x64))+(((-0.44)*x62))+(((-0.42)*x68))+(((-1.0)*x64*x69))+(((-1.0)*x64*x66))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-1.0)*x62*x69))+(((-1.0)*x62*x66))));
550 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
551 {
552 {
553 IkReal j1eval[2];
554 IkReal x70=py*py;
555 IkReal x71=cj0*cj0;
556 IkReal x72=px*px;
557 IkReal x73=pz*pz;
558 IkReal x74=(py*sj0);
559 IkReal x75=(cj0*px);
560 IkReal x76=(x70*x71);
561 IkReal x77=(x71*x72);
562 j1eval[0]=((-1.0)+(((40.0)*x75))+(((40.0)*x74))+(((-800.0)*x74*x75))+(((400.0)*x76))+(((-400.0)*x70))+(((-400.0)*x77))+(((-400.0)*x73)));
563 j1eval[1]=IKsign(((-0.0025)+(((-2.0)*x74*x75))+(((0.1)*x75))+(((0.1)*x74))+x76+(((-1.0)*x70))+(((-1.0)*x73))+(((-1.0)*x77))));
564 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
565 {
566 continue; // no branches [j1]
567 
568 } else
569 {
570 {
571 IkReal j1array[1], cj1array[1], sj1array[1];
572 bool j1valid[1]={false};
573 _nj1 = 1;
574 IkReal x78=py*py;
575 IkReal x79=cj0*cj0;
576 IkReal x80=(py*sj0);
577 IkReal x81=((0.035)*sj2);
578 IkReal x82=(cj0*px);
579 IkReal x83=((0.035)*cj2);
580 IkReal x84=((0.42)*cj2);
581 IkReal x85=((0.42)*sj2);
582 IkReal x86=((0.42)*x82);
583 CheckValue<IkReal> x87 = IKatan2WithCheck(IkReal(((0.022)+(((-1.0)*x82*x83))+(((-1.0)*x82*x85))+(((-1.0)*x80*x85))+(((-1.0)*x80*x83))+((pz*x84))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-0.44)*x80))+(((-0.44)*x82))+(((-1.0)*pz*x81)))),IkReal(((((0.021)*cj2))+(((-1.0)*x82*x84))+(((-1.0)*x80*x84))+((x81*x82))+(((-0.00175)*sj2))+(((-0.44)*pz))+((x80*x81))+(((-1.0)*pz*x83))+(((-1.0)*pz*x85)))),IKFAST_ATAN2_MAGTHRESH);
584 if(!x87.valid){
585 continue;
586 }
587 CheckValue<IkReal> x88=IKPowWithIntegerCheck(IKsign(((-0.0025)+(((0.1)*x82))+(((0.1)*x80))+((x78*x79))+(((-2.0)*x80*x82))+(((-1.0)*x79*(px*px)))+(((-1.0)*(pz*pz)))+(((-1.0)*x78)))),-1);
588 if(!x88.valid){
589 continue;
590 }
591 j1array[0]=((-1.5707963267949)+(x87.value)+(((1.5707963267949)*(x88.value))));
592 sj1array[0]=IKsin(j1array[0]);
593 cj1array[0]=IKcos(j1array[0]);
594 if( j1array[0] > IKPI )
595 {
596  j1array[0]-=IK2PI;
597 }
598 else if( j1array[0] < -IKPI )
599 { j1array[0]+=IK2PI;
600 }
601 j1valid[0] = true;
602 for(int ij1 = 0; ij1 < 1; ++ij1)
603 {
604 if( !j1valid[ij1] )
605 {
606  continue;
607 }
608 _ij1[0] = ij1; _ij1[1] = -1;
609 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
610 {
611 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
612 {
613  j1valid[iij1]=false; _ij1[1] = iij1; break;
614 }
615 }
616 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
617 {
618 IkReal evalcond[5];
619 IkReal x89=IKcos(j1);
620 IkReal x90=IKsin(j1);
621 IkReal x91=((0.035)*sj2);
622 IkReal x92=(py*sj0);
623 IkReal x93=((0.42)*sj2);
624 IkReal x94=((0.035)*cj2);
625 IkReal x95=(cj0*px);
626 IkReal x96=((0.42)*x90);
627 IkReal x97=((1.0)*x95);
628 IkReal x98=((1.0)*x89);
629 IkReal x99=(cj2*x89);
630 IkReal x100=((0.88)*x90);
631 evalcond[0]=((((0.05)*x89))+((pz*x90))+(((-1.0)*x92*x98))+(((0.42)*cj2))+(((-1.0)*x91))+(((-1.0)*x89*x97)));
632 evalcond[1]=((((-1.0)*x90*x91))+((cj2*x96))+pz+(((-0.44)*x89))+(((-1.0)*x89*x94))+(((-1.0)*x89*x93)));
633 evalcond[2]=((0.44)+(((-1.0)*pz*x98))+(((0.05)*x90))+(((-1.0)*x90*x97))+(((-1.0)*x90*x92))+x93+x94);
634 evalcond[3]=((-0.018475)+((x100*x95))+((x100*x92))+(((0.1)*x92))+(((0.1)*x95))+(((-0.044)*x90))+(((-1.0)*pp))+(((0.88)*pz*x89)));
635 evalcond[4]=((0.05)+(((0.42)*x99))+((x90*x93))+((x90*x94))+(((-1.0)*x92))+(((-1.0)*x97))+(((0.44)*x90))+(((-1.0)*x89*x91)));
636 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
637 {
638 continue;
639 }
640 }
641 
642 rotationfunction0(solutions);
643 }
644 }
645 
646 }
647 
648 }
649 
650 } else
651 {
652 {
653 IkReal j1array[1], cj1array[1], sj1array[1];
654 bool j1valid[1]={false};
655 _nj1 = 1;
656 IkReal x1520=cj2*cj2;
657 IkReal x1521=(cj2*sj2);
658 IkReal x1522=(cj0*px);
659 IkReal x1523=((0.42)*sj2);
660 IkReal x1524=((0.035)*cj2);
661 IkReal x1525=((1.0)*pz);
662 IkReal x1526=(py*sj0);
663 CheckValue<IkReal> x1527 = IKatan2WithCheck(IkReal(((-0.37)+(((-0.0294)*x1521))+(((-0.3696)*sj2))+(((-0.0308)*cj2))+(pz*pz)+(((0.175175)*x1520)))),IkReal(((0.0147)+(((-0.0294)*x1520))+(((-0.175175)*x1521))+(((-1.0)*x1525*x1526))+(((0.0154)*sj2))+(((-1.0)*x1522*x1525))+(((-0.1848)*cj2))+(((0.05)*pz)))),IKFAST_ATAN2_MAGTHRESH);
664 if(!x1527.valid){
665 continue;
666 }
667 CheckValue<IkReal> x1528=IKPowWithIntegerCheck(IKsign(((0.022)+(((-1.0)*x1524*x1526))+(((-0.44)*x1522))+(((-0.44)*x1526))+(((-0.42)*cj2*pz))+(((-1.0)*x1522*x1524))+(((-1.0)*x1522*x1523))+(((0.00175)*cj2))+(((-1.0)*x1523*x1526))+(((0.035)*pz*sj2))+(((0.021)*sj2)))),-1);
668 if(!x1528.valid){
669 continue;
670 }
671 j1array[0]=((-1.5707963267949)+(x1527.value)+(((1.5707963267949)*(x1528.value))));
672 sj1array[0]=IKsin(j1array[0]);
673 cj1array[0]=IKcos(j1array[0]);
674 if( j1array[0] > IKPI )
675 {
676  j1array[0]-=IK2PI;
677 }
678 else if( j1array[0] < -IKPI )
679 { j1array[0]+=IK2PI;
680 }
681 j1valid[0] = true;
682 for(int ij1 = 0; ij1 < 1; ++ij1)
683 {
684 if( !j1valid[ij1] )
685 {
686  continue;
687 }
688 _ij1[0] = ij1; _ij1[1] = -1;
689 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
690 {
691 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
692 {
693  j1valid[iij1]=false; _ij1[1] = iij1; break;
694 }
695 }
696 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
697 {
698 IkReal evalcond[5];
699 IkReal x1529=IKcos(j1);
700 IkReal x1530=IKsin(j1);
701 IkReal x1531=((0.035)*sj2);
702 IkReal x1532=(py*sj0);
703 IkReal x1533=((0.42)*sj2);
704 IkReal x1534=((0.035)*cj2);
705 IkReal x1535=(cj0*px);
706 IkReal x1536=((0.42)*x1530);
707 IkReal x1537=((1.0)*x1535);
708 IkReal x1538=((1.0)*x1529);
709 IkReal x1539=(cj2*x1529);
710 IkReal x1540=((0.88)*x1530);
711 evalcond[0]=((((-1.0)*x1529*x1537))+(((0.05)*x1529))+(((-1.0)*x1531))+(((-1.0)*x1532*x1538))+(((0.42)*cj2))+((pz*x1530)));
712 evalcond[1]=((((-1.0)*x1529*x1533))+(((-1.0)*x1529*x1534))+(((-1.0)*x1530*x1531))+((cj2*x1536))+(((-0.44)*x1529))+pz);
713 evalcond[2]=((0.44)+(((-1.0)*x1530*x1532))+(((0.05)*x1530))+(((-1.0)*x1530*x1537))+x1534+x1533+(((-1.0)*pz*x1538)));
714 evalcond[3]=((-0.018475)+((x1532*x1540))+(((-1.0)*pp))+(((0.1)*x1535))+(((0.1)*x1532))+(((0.88)*pz*x1529))+(((-0.044)*x1530))+((x1535*x1540)));
715 evalcond[4]=((0.05)+(((-1.0)*x1529*x1531))+(((0.44)*x1530))+(((-1.0)*x1532))+(((-1.0)*x1537))+((x1530*x1534))+((x1530*x1533))+(((0.42)*x1539)));
716 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
717 {
718 continue;
719 }
720 }
721 
722 rotationfunction0(solutions);
723 }
724 }
725 
726 }
727 
728 }
729 
730 } else
731 {
732 {
733 IkReal j1array[1], cj1array[1], sj1array[1];
734 bool j1valid[1]={false};
735 _nj1 = 1;
736 IkReal x1541=cj2*cj2;
737 IkReal x1542=(py*sj0);
738 IkReal x1543=((0.035)*sj2);
739 IkReal x1544=((0.42)*cj2);
740 IkReal x1545=(cj0*px);
741 IkReal x1546=(cj2*sj2);
742 CheckValue<IkReal> x1547=IKPowWithIntegerCheck(IKsign(((((0.021)*cj2))+(((0.42)*pz*sj2))+(((-0.00175)*sj2))+(((0.035)*cj2*pz))+((x1543*x1545))+(((-1.0)*x1542*x1544))+(((-1.0)*x1544*x1545))+((x1542*x1543))+(((0.44)*pz)))),-1);
743 if(!x1547.valid){
744 continue;
745 }
746 CheckValue<IkReal> x1548 = IKatan2WithCheck(IkReal(((0.0147)+(((-0.0294)*x1541))+(((-0.05)*pz))+(((-0.175175)*x1546))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x1542))+((pz*x1545)))),IkReal(((-0.001225)+(((-0.175175)*x1541))+(pz*pz)+(((0.0294)*x1546)))),IKFAST_ATAN2_MAGTHRESH);
747 if(!x1548.valid){
748 continue;
749 }
750 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1547.value)))+(x1548.value));
751 sj1array[0]=IKsin(j1array[0]);
752 cj1array[0]=IKcos(j1array[0]);
753 if( j1array[0] > IKPI )
754 {
755  j1array[0]-=IK2PI;
756 }
757 else if( j1array[0] < -IKPI )
758 { j1array[0]+=IK2PI;
759 }
760 j1valid[0] = true;
761 for(int ij1 = 0; ij1 < 1; ++ij1)
762 {
763 if( !j1valid[ij1] )
764 {
765  continue;
766 }
767 _ij1[0] = ij1; _ij1[1] = -1;
768 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
769 {
770 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
771 {
772  j1valid[iij1]=false; _ij1[1] = iij1; break;
773 }
774 }
775 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
776 {
777 IkReal evalcond[5];
778 IkReal x1549=IKcos(j1);
779 IkReal x1550=IKsin(j1);
780 IkReal x1551=((0.035)*sj2);
781 IkReal x1552=(py*sj0);
782 IkReal x1553=((0.42)*sj2);
783 IkReal x1554=((0.035)*cj2);
784 IkReal x1555=(cj0*px);
785 IkReal x1556=((0.42)*x1550);
786 IkReal x1557=((1.0)*x1555);
787 IkReal x1558=((1.0)*x1549);
788 IkReal x1559=(cj2*x1549);
789 IkReal x1560=((0.88)*x1550);
790 evalcond[0]=((((-1.0)*x1549*x1557))+(((-1.0)*x1552*x1558))+(((0.05)*x1549))+((pz*x1550))+(((0.42)*cj2))+(((-1.0)*x1551)));
791 evalcond[1]=((((-1.0)*x1549*x1554))+(((-1.0)*x1549*x1553))+((cj2*x1556))+(((-0.44)*x1549))+pz+(((-1.0)*x1550*x1551)));
792 evalcond[2]=((0.44)+(((-1.0)*x1550*x1552))+(((-1.0)*pz*x1558))+(((0.05)*x1550))+x1553+x1554+(((-1.0)*x1550*x1557)));
793 evalcond[3]=((-0.018475)+((x1555*x1560))+(((0.1)*x1552))+(((0.1)*x1555))+((x1552*x1560))+(((0.88)*pz*x1549))+(((-1.0)*pp))+(((-0.044)*x1550)));
794 evalcond[4]=((0.05)+(((-1.0)*x1552))+(((-1.0)*x1549*x1551))+((x1550*x1554))+((x1550*x1553))+(((0.42)*x1559))+(((-1.0)*x1557))+(((0.44)*x1550)));
795 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
796 {
797 continue;
798 }
799 }
800 
801 rotationfunction0(solutions);
802 }
803 }
804 
805 }
806 
807 }
808 }
809 }
810 }
811 }
812 
813 }
814 
815 }
816 }
817 return solutions.GetNumSolutions()>0;
818 }
819 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
820 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
821 IkReal x101=(r11*sj0);
822 IkReal x102=(cj0*r02);
823 IkReal x103=(sj1*sj2);
824 IkReal x104=(r10*sj0);
825 IkReal x105=((1.0)*cj1);
826 IkReal x106=((1.0)*sj0);
827 IkReal x107=(cj0*r00);
828 IkReal x108=(r12*sj0);
829 IkReal x109=(cj0*r01);
830 IkReal x110=((((-1.0)*sj2*x105))+((cj2*sj1)));
831 IkReal x111=(x103+((cj1*cj2)));
832 IkReal x112=(sj0*x110);
833 IkReal x113=(cj0*x110);
834 IkReal x114=((((-1.0)*cj2*x105))+(((-1.0)*x103)));
835 new_r00=(((r20*x111))+((x104*x110))+((x107*x110)));
836 new_r01=(((x109*x110))+((r21*x111))+((x101*x110)));
837 new_r02=(((r22*x111))+((x102*x110))+((x108*x110)));
838 new_r10=((((-1.0)*r00*x106))+((cj0*r10)));
839 new_r11=((((-1.0)*r01*x106))+((cj0*r11)));
840 new_r12=((((-1.0)*r02*x106))+((cj0*r12)));
841 new_r20=(((r20*x110))+((x104*x114))+((x107*x114)));
842 new_r21=(((x109*x114))+((r21*x110))+((x101*x114)));
843 new_r22=(((r22*x110))+((x102*x114))+((x108*x114)));
844 {
845 IkReal j4array[2], cj4array[2], sj4array[2];
846 bool j4valid[2]={false};
847 _nj4 = 2;
848 cj4array[0]=new_r22;
849 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
850 {
851  j4valid[0] = j4valid[1] = true;
852  j4array[0] = IKacos(cj4array[0]);
853  sj4array[0] = IKsin(j4array[0]);
854  cj4array[1] = cj4array[0];
855  j4array[1] = -j4array[0];
856  sj4array[1] = -sj4array[0];
857 }
858 else if( isnan(cj4array[0]) )
859 {
860  // probably any value will work
861  j4valid[0] = true;
862  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
863 }
864 for(int ij4 = 0; ij4 < 2; ++ij4)
865 {
866 if( !j4valid[ij4] )
867 {
868  continue;
869 }
870 _ij4[0] = ij4; _ij4[1] = -1;
871 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
872 {
873 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
874 {
875  j4valid[iij4]=false; _ij4[1] = iij4; break;
876 }
877 }
878 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
879 
880 {
881 IkReal j5eval[3];
882 j5eval[0]=sj4;
883 j5eval[1]=IKsign(sj4);
884 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
885 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
886 {
887 {
888 IkReal j3eval[3];
889 j3eval[0]=sj4;
890 j3eval[1]=IKsign(sj4);
891 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
892 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
893 {
894 {
895 IkReal j3eval[2];
896 j3eval[0]=new_r12;
897 j3eval[1]=sj4;
898 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
899 {
900 {
901 IkReal evalcond[5];
902 bool bgotonextstatement = true;
903 do
904 {
905 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
906 evalcond[1]=new_r02;
907 evalcond[2]=new_r12;
908 evalcond[3]=new_r21;
909 evalcond[4]=new_r20;
910 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
911 {
912 bgotonextstatement=false;
913 IkReal j5mul = 1;
914 j5=0;
915 j3mul=-1.0;
916 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
917  continue;
918 j3=IKatan2(((-1.0)*new_r01), new_r00);
919 {
920 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
921 vinfos[0].jointtype = 1;
922 vinfos[0].foffset = j0;
923 vinfos[0].indices[0] = _ij0[0];
924 vinfos[0].indices[1] = _ij0[1];
925 vinfos[0].maxsolutions = _nj0;
926 vinfos[1].jointtype = 1;
927 vinfos[1].foffset = j1;
928 vinfos[1].indices[0] = _ij1[0];
929 vinfos[1].indices[1] = _ij1[1];
930 vinfos[1].maxsolutions = _nj1;
931 vinfos[2].jointtype = 1;
932 vinfos[2].foffset = j2;
933 vinfos[2].indices[0] = _ij2[0];
934 vinfos[2].indices[1] = _ij2[1];
935 vinfos[2].maxsolutions = _nj2;
936 vinfos[3].jointtype = 1;
937 vinfos[3].foffset = j3;
938 vinfos[3].fmul = j3mul;
939 vinfos[3].freeind = 0;
940 vinfos[3].maxsolutions = 0;
941 vinfos[4].jointtype = 1;
942 vinfos[4].foffset = j4;
943 vinfos[4].indices[0] = _ij4[0];
944 vinfos[4].indices[1] = _ij4[1];
945 vinfos[4].maxsolutions = _nj4;
946 vinfos[5].jointtype = 1;
947 vinfos[5].foffset = j5;
948 vinfos[5].fmul = j5mul;
949 vinfos[5].freeind = 0;
950 vinfos[5].maxsolutions = 0;
951 std::vector<int> vfree(1);
952 vfree[0] = 5;
953 solutions.AddSolution(vinfos,vfree);
954 }
955 
956 }
957 } while(0);
958 if( bgotonextstatement )
959 {
960 bool bgotonextstatement = true;
961 do
962 {
963 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
964 evalcond[1]=new_r02;
965 evalcond[2]=new_r12;
966 evalcond[3]=new_r21;
967 evalcond[4]=new_r20;
968 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
969 {
970 bgotonextstatement=false;
971 IkReal j5mul = 1;
972 j5=0;
973 j3mul=1.0;
974 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
975  continue;
976 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
977 {
978 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
979 vinfos[0].jointtype = 1;
980 vinfos[0].foffset = j0;
981 vinfos[0].indices[0] = _ij0[0];
982 vinfos[0].indices[1] = _ij0[1];
983 vinfos[0].maxsolutions = _nj0;
984 vinfos[1].jointtype = 1;
985 vinfos[1].foffset = j1;
986 vinfos[1].indices[0] = _ij1[0];
987 vinfos[1].indices[1] = _ij1[1];
988 vinfos[1].maxsolutions = _nj1;
989 vinfos[2].jointtype = 1;
990 vinfos[2].foffset = j2;
991 vinfos[2].indices[0] = _ij2[0];
992 vinfos[2].indices[1] = _ij2[1];
993 vinfos[2].maxsolutions = _nj2;
994 vinfos[3].jointtype = 1;
995 vinfos[3].foffset = j3;
996 vinfos[3].fmul = j3mul;
997 vinfos[3].freeind = 0;
998 vinfos[3].maxsolutions = 0;
999 vinfos[4].jointtype = 1;
1000 vinfos[4].foffset = j4;
1001 vinfos[4].indices[0] = _ij4[0];
1002 vinfos[4].indices[1] = _ij4[1];
1003 vinfos[4].maxsolutions = _nj4;
1004 vinfos[5].jointtype = 1;
1005 vinfos[5].foffset = j5;
1006 vinfos[5].fmul = j5mul;
1007 vinfos[5].freeind = 0;
1008 vinfos[5].maxsolutions = 0;
1009 std::vector<int> vfree(1);
1010 vfree[0] = 5;
1011 solutions.AddSolution(vinfos,vfree);
1012 }
1013 
1014 }
1015 } while(0);
1016 if( bgotonextstatement )
1017 {
1018 bool bgotonextstatement = true;
1019 do
1020 {
1021 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1022 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1023 {
1024 bgotonextstatement=false;
1025 {
1026 IkReal j3eval[1];
1027 new_r21=0;
1028 new_r20=0;
1029 new_r02=0;
1030 new_r12=0;
1031 IkReal x115=new_r22*new_r22;
1032 IkReal x116=((16.0)*new_r10);
1033 IkReal x117=((16.0)*new_r01);
1034 IkReal x118=((16.0)*new_r22);
1035 IkReal x119=((8.0)*new_r11);
1036 IkReal x120=((8.0)*new_r00);
1037 IkReal x121=(x115*x116);
1038 IkReal x122=(x115*x117);
1039 j3eval[0]=((IKabs((((new_r11*x118))+(((16.0)*new_r00))+(((-32.0)*new_r00*x115)))))+(IKabs((((new_r22*x120))+(((-1.0)*x115*x119)))))+(IKabs(((((-32.0)*new_r11))+((new_r00*x118))+(((16.0)*new_r11*x115)))))+(IKabs(((((-1.0)*x121))+x116)))+(IKabs(((((-1.0)*x122))+x117)))+(IKabs(((((-1.0)*x116))+x121)))+(IKabs(((((-1.0)*x117))+x122)))+(IKabs((((new_r22*x119))+(((-1.0)*x120))))));
1040 if( IKabs(j3eval[0]) < 0.0000000010000000 )
1041 {
1042 continue; // no branches [j3, j5]
1043 
1044 } else
1045 {
1046 IkReal op[4+1], zeror[4];
1047 int numroots;
1048 IkReal j3evalpoly[1];
1049 IkReal x123=new_r22*new_r22;
1050 IkReal x124=((16.0)*new_r10);
1051 IkReal x125=(new_r11*new_r22);
1052 IkReal x126=(x123*x124);
1053 IkReal x127=((((-8.0)*new_r00))+(((8.0)*x125)));
1054 op[0]=x127;
1055 op[1]=((((-1.0)*x126))+x124);
1056 op[2]=((((16.0)*x125))+(((16.0)*new_r00))+(((-32.0)*new_r00*x123)));
1057 op[3]=((((-1.0)*x124))+x126);
1058 op[4]=x127;
1059 polyroots4(op,zeror,numroots);
1060 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1061 int numsolutions = 0;
1062 for(int ij3 = 0; ij3 < numroots; ++ij3)
1063 {
1064 IkReal htj3 = zeror[ij3];
1065 tempj3array[0]=((2.0)*(atan(htj3)));
1066 for(int kj3 = 0; kj3 < 1; ++kj3)
1067 {
1068 j3array[numsolutions] = tempj3array[kj3];
1069 if( j3array[numsolutions] > IKPI )
1070 {
1071  j3array[numsolutions]-=IK2PI;
1072 }
1073 else if( j3array[numsolutions] < -IKPI )
1074 {
1075  j3array[numsolutions]+=IK2PI;
1076 }
1077 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1078 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1079 numsolutions++;
1080 }
1081 }
1082 bool j3valid[4]={true,true,true,true};
1083 _nj3 = 4;
1084 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1085  {
1086 if( !j3valid[ij3] )
1087 {
1088  continue;
1089 }
1090  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1091 htj3 = IKtan(j3/2);
1092 
1093 IkReal x128=((16.0)*new_r01);
1094 IkReal x129=new_r22*new_r22;
1095 IkReal x130=(new_r00*new_r22);
1096 IkReal x131=((8.0)*x130);
1097 IkReal x132=(new_r11*x129);
1098 IkReal x133=(x128*x129);
1099 IkReal x134=((8.0)*x132);
1100 j3evalpoly[0]=(((htj3*(((((-1.0)*x128))+x133))))+(((htj3*htj3)*(((((16.0)*x132))+(((16.0)*x130))+(((-32.0)*new_r11))))))+x131+(((htj3*htj3*htj3)*((x128+(((-1.0)*x133))))))+(((-1.0)*x134))+(((htj3*htj3*htj3*htj3)*((x131+(((-1.0)*x134)))))));
1101 if( IKabs(j3evalpoly[0]) > 0.0000000010000000 )
1102 {
1103  continue;
1104 }
1105 _ij3[0] = ij3; _ij3[1] = -1;
1106 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1107 {
1108 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1109 {
1110  j3valid[iij3]=false; _ij3[1] = iij3; break;
1111 }
1112 }
1113 {
1114 IkReal j5eval[3];
1115 new_r21=0;
1116 new_r20=0;
1117 new_r02=0;
1118 new_r12=0;
1119 IkReal x135=cj3*cj3;
1120 IkReal x136=(cj3*new_r22);
1121 IkReal x137=((-1.0)+(((-1.0)*x135*(new_r22*new_r22)))+x135);
1122 j5eval[0]=x137;
1123 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x136)))))+(IKabs((((new_r01*x136))+((new_r00*sj3))))));
1124 j5eval[2]=IKsign(x137);
1125 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1126 {
1127 {
1128 IkReal j5eval[1];
1129 new_r21=0;
1130 new_r20=0;
1131 new_r02=0;
1132 new_r12=0;
1133 j5eval[0]=new_r22;
1134 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1135 {
1136 {
1137 IkReal j5eval[1];
1138 new_r21=0;
1139 new_r20=0;
1140 new_r02=0;
1141 new_r12=0;
1142 j5eval[0]=sj3;
1143 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1144 {
1145 {
1146 IkReal evalcond[1];
1147 bool bgotonextstatement = true;
1148 do
1149 {
1150 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1151 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1152 {
1153 bgotonextstatement=false;
1154 {
1155 IkReal j5array[1], cj5array[1], sj5array[1];
1156 bool j5valid[1]={false};
1157 _nj5 = 1;
1158 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1159  continue;
1160 j5array[0]=IKatan2(new_r10, new_r11);
1161 sj5array[0]=IKsin(j5array[0]);
1162 cj5array[0]=IKcos(j5array[0]);
1163 if( j5array[0] > IKPI )
1164 {
1165  j5array[0]-=IK2PI;
1166 }
1167 else if( j5array[0] < -IKPI )
1168 { j5array[0]+=IK2PI;
1169 }
1170 j5valid[0] = true;
1171 for(int ij5 = 0; ij5 < 1; ++ij5)
1172 {
1173 if( !j5valid[ij5] )
1174 {
1175  continue;
1176 }
1177 _ij5[0] = ij5; _ij5[1] = -1;
1178 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1179 {
1180 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1181 {
1182  j5valid[iij5]=false; _ij5[1] = iij5; break;
1183 }
1184 }
1185 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1186 {
1187 IkReal evalcond[4];
1188 IkReal x138=IKsin(j5);
1189 IkReal x139=IKcos(j5);
1190 evalcond[0]=x138;
1191 evalcond[1]=((-1.0)*x139);
1192 evalcond[2]=(x138+(((-1.0)*new_r10)));
1193 evalcond[3]=(x139+(((-1.0)*new_r11)));
1194 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1195 {
1196 continue;
1197 }
1198 }
1199 
1200 {
1201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1202 vinfos[0].jointtype = 1;
1203 vinfos[0].foffset = j0;
1204 vinfos[0].indices[0] = _ij0[0];
1205 vinfos[0].indices[1] = _ij0[1];
1206 vinfos[0].maxsolutions = _nj0;
1207 vinfos[1].jointtype = 1;
1208 vinfos[1].foffset = j1;
1209 vinfos[1].indices[0] = _ij1[0];
1210 vinfos[1].indices[1] = _ij1[1];
1211 vinfos[1].maxsolutions = _nj1;
1212 vinfos[2].jointtype = 1;
1213 vinfos[2].foffset = j2;
1214 vinfos[2].indices[0] = _ij2[0];
1215 vinfos[2].indices[1] = _ij2[1];
1216 vinfos[2].maxsolutions = _nj2;
1217 vinfos[3].jointtype = 1;
1218 vinfos[3].foffset = j3;
1219 vinfos[3].indices[0] = _ij3[0];
1220 vinfos[3].indices[1] = _ij3[1];
1221 vinfos[3].maxsolutions = _nj3;
1222 vinfos[4].jointtype = 1;
1223 vinfos[4].foffset = j4;
1224 vinfos[4].indices[0] = _ij4[0];
1225 vinfos[4].indices[1] = _ij4[1];
1226 vinfos[4].maxsolutions = _nj4;
1227 vinfos[5].jointtype = 1;
1228 vinfos[5].foffset = j5;
1229 vinfos[5].indices[0] = _ij5[0];
1230 vinfos[5].indices[1] = _ij5[1];
1231 vinfos[5].maxsolutions = _nj5;
1232 std::vector<int> vfree(0);
1233 solutions.AddSolution(vinfos,vfree);
1234 }
1235 }
1236 }
1237 
1238 }
1239 } while(0);
1240 if( bgotonextstatement )
1241 {
1242 bool bgotonextstatement = true;
1243 do
1244 {
1245 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1246 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1247 {
1248 bgotonextstatement=false;
1249 {
1250 IkReal j5array[1], cj5array[1], sj5array[1];
1251 bool j5valid[1]={false};
1252 _nj5 = 1;
1253 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
1254  continue;
1255 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1256 sj5array[0]=IKsin(j5array[0]);
1257 cj5array[0]=IKcos(j5array[0]);
1258 if( j5array[0] > IKPI )
1259 {
1260  j5array[0]-=IK2PI;
1261 }
1262 else if( j5array[0] < -IKPI )
1263 { j5array[0]+=IK2PI;
1264 }
1265 j5valid[0] = true;
1266 for(int ij5 = 0; ij5 < 1; ++ij5)
1267 {
1268 if( !j5valid[ij5] )
1269 {
1270  continue;
1271 }
1272 _ij5[0] = ij5; _ij5[1] = -1;
1273 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1274 {
1275 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1276 {
1277  j5valid[iij5]=false; _ij5[1] = iij5; break;
1278 }
1279 }
1280 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1281 {
1282 IkReal evalcond[4];
1283 IkReal x140=IKsin(j5);
1284 IkReal x141=IKcos(j5);
1285 evalcond[0]=x140;
1286 evalcond[1]=(x140+new_r10);
1287 evalcond[2]=(x141+new_r11);
1288 evalcond[3]=((-1.0)*x141);
1289 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1290 {
1291 continue;
1292 }
1293 }
1294 
1295 {
1296 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1297 vinfos[0].jointtype = 1;
1298 vinfos[0].foffset = j0;
1299 vinfos[0].indices[0] = _ij0[0];
1300 vinfos[0].indices[1] = _ij0[1];
1301 vinfos[0].maxsolutions = _nj0;
1302 vinfos[1].jointtype = 1;
1303 vinfos[1].foffset = j1;
1304 vinfos[1].indices[0] = _ij1[0];
1305 vinfos[1].indices[1] = _ij1[1];
1306 vinfos[1].maxsolutions = _nj1;
1307 vinfos[2].jointtype = 1;
1308 vinfos[2].foffset = j2;
1309 vinfos[2].indices[0] = _ij2[0];
1310 vinfos[2].indices[1] = _ij2[1];
1311 vinfos[2].maxsolutions = _nj2;
1312 vinfos[3].jointtype = 1;
1313 vinfos[3].foffset = j3;
1314 vinfos[3].indices[0] = _ij3[0];
1315 vinfos[3].indices[1] = _ij3[1];
1316 vinfos[3].maxsolutions = _nj3;
1317 vinfos[4].jointtype = 1;
1318 vinfos[4].foffset = j4;
1319 vinfos[4].indices[0] = _ij4[0];
1320 vinfos[4].indices[1] = _ij4[1];
1321 vinfos[4].maxsolutions = _nj4;
1322 vinfos[5].jointtype = 1;
1323 vinfos[5].foffset = j5;
1324 vinfos[5].indices[0] = _ij5[0];
1325 vinfos[5].indices[1] = _ij5[1];
1326 vinfos[5].maxsolutions = _nj5;
1327 std::vector<int> vfree(0);
1328 solutions.AddSolution(vinfos,vfree);
1329 }
1330 }
1331 }
1332 
1333 }
1334 } while(0);
1335 if( bgotonextstatement )
1336 {
1337 bool bgotonextstatement = true;
1338 do
1339 {
1340 CheckValue<IkReal> x142=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1341 if(!x142.valid){
1342 continue;
1343 }
1344 if((x142.value) < -0.00001)
1345 continue;
1346 IkReal gconst24=((-1.0)*(IKsqrt(x142.value)));
1347 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1348 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1349 {
1350 bgotonextstatement=false;
1351 {
1352 IkReal j5eval[1];
1353 new_r21=0;
1354 new_r20=0;
1355 new_r02=0;
1356 new_r12=0;
1357 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1358 continue;
1359 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
1360 cj3=gconst24;
1361 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1362  continue;
1363 j3=IKacos(gconst24);
1364 CheckValue<IkReal> x143=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1365 if(!x143.valid){
1366 continue;
1367 }
1368 if((x143.value) < -0.00001)
1369 continue;
1370 IkReal gconst24=((-1.0)*(IKsqrt(x143.value)));
1371 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1372 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1373 {
1374 {
1375 IkReal j5array[1], cj5array[1], sj5array[1];
1376 bool j5valid[1]={false};
1377 _nj5 = 1;
1378 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1379 continue;
1380 CheckValue<IkReal> x144=IKPowWithIntegerCheck(gconst24,-1);
1381 if(!x144.valid){
1382 continue;
1383 }
1384 if( IKabs(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x144.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x144.value)))-1) <= IKFAST_SINCOS_THRESH )
1385  continue;
1386 j5array[0]=IKatan2(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x144.value)));
1387 sj5array[0]=IKsin(j5array[0]);
1388 cj5array[0]=IKcos(j5array[0]);
1389 if( j5array[0] > IKPI )
1390 {
1391  j5array[0]-=IK2PI;
1392 }
1393 else if( j5array[0] < -IKPI )
1394 { j5array[0]+=IK2PI;
1395 }
1396 j5valid[0] = true;
1397 for(int ij5 = 0; ij5 < 1; ++ij5)
1398 {
1399 if( !j5valid[ij5] )
1400 {
1401  continue;
1402 }
1403 _ij5[0] = ij5; _ij5[1] = -1;
1404 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1405 {
1406 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1407 {
1408  j5valid[iij5]=false; _ij5[1] = iij5; break;
1409 }
1410 }
1411 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1412 {
1413 IkReal evalcond[8];
1414 IkReal x145=IKcos(j5);
1415 IkReal x146=IKsin(j5);
1416 IkReal x147=((1.0)*gconst24);
1417 if((((1.0)+(((-1.0)*gconst24*x147)))) < -0.00001)
1418 continue;
1419 IkReal x148=IKsqrt(((1.0)+(((-1.0)*gconst24*x147))));
1420 evalcond[0]=x146;
1421 evalcond[1]=((-1.0)*x145);
1422 evalcond[2]=((((-1.0)*x145*x147))+new_r11);
1423 evalcond[3]=((((-1.0)*x146*x147))+new_r10);
1424 evalcond[4]=(((x145*x148))+new_r01);
1425 evalcond[5]=(((x146*x148))+new_r00);
1426 evalcond[6]=(((new_r00*x148))+x146+(((-1.0)*new_r10*x147)));
1427 evalcond[7]=(((new_r01*x148))+(((-1.0)*new_r11*x147))+x145);
1428 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1429 {
1430 continue;
1431 }
1432 }
1433 
1434 {
1435 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1436 vinfos[0].jointtype = 1;
1437 vinfos[0].foffset = j0;
1438 vinfos[0].indices[0] = _ij0[0];
1439 vinfos[0].indices[1] = _ij0[1];
1440 vinfos[0].maxsolutions = _nj0;
1441 vinfos[1].jointtype = 1;
1442 vinfos[1].foffset = j1;
1443 vinfos[1].indices[0] = _ij1[0];
1444 vinfos[1].indices[1] = _ij1[1];
1445 vinfos[1].maxsolutions = _nj1;
1446 vinfos[2].jointtype = 1;
1447 vinfos[2].foffset = j2;
1448 vinfos[2].indices[0] = _ij2[0];
1449 vinfos[2].indices[1] = _ij2[1];
1450 vinfos[2].maxsolutions = _nj2;
1451 vinfos[3].jointtype = 1;
1452 vinfos[3].foffset = j3;
1453 vinfos[3].indices[0] = _ij3[0];
1454 vinfos[3].indices[1] = _ij3[1];
1455 vinfos[3].maxsolutions = _nj3;
1456 vinfos[4].jointtype = 1;
1457 vinfos[4].foffset = j4;
1458 vinfos[4].indices[0] = _ij4[0];
1459 vinfos[4].indices[1] = _ij4[1];
1460 vinfos[4].maxsolutions = _nj4;
1461 vinfos[5].jointtype = 1;
1462 vinfos[5].foffset = j5;
1463 vinfos[5].indices[0] = _ij5[0];
1464 vinfos[5].indices[1] = _ij5[1];
1465 vinfos[5].maxsolutions = _nj5;
1466 std::vector<int> vfree(0);
1467 solutions.AddSolution(vinfos,vfree);
1468 }
1469 }
1470 }
1471 
1472 } else
1473 {
1474 {
1475 IkReal j5array[1], cj5array[1], sj5array[1];
1476 bool j5valid[1]={false};
1477 _nj5 = 1;
1478 CheckValue<IkReal> x149=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1479 if(!x149.valid){
1480 continue;
1481 }
1482 CheckValue<IkReal> x150 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1483 if(!x150.valid){
1484 continue;
1485 }
1486 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x149.value)))+(x150.value));
1487 sj5array[0]=IKsin(j5array[0]);
1488 cj5array[0]=IKcos(j5array[0]);
1489 if( j5array[0] > IKPI )
1490 {
1491  j5array[0]-=IK2PI;
1492 }
1493 else if( j5array[0] < -IKPI )
1494 { j5array[0]+=IK2PI;
1495 }
1496 j5valid[0] = true;
1497 for(int ij5 = 0; ij5 < 1; ++ij5)
1498 {
1499 if( !j5valid[ij5] )
1500 {
1501  continue;
1502 }
1503 _ij5[0] = ij5; _ij5[1] = -1;
1504 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1505 {
1506 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1507 {
1508  j5valid[iij5]=false; _ij5[1] = iij5; break;
1509 }
1510 }
1511 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1512 {
1513 IkReal evalcond[8];
1514 IkReal x151=IKcos(j5);
1515 IkReal x152=IKsin(j5);
1516 IkReal x153=((1.0)*gconst24);
1517 if((((1.0)+(((-1.0)*gconst24*x153)))) < -0.00001)
1518 continue;
1519 IkReal x154=IKsqrt(((1.0)+(((-1.0)*gconst24*x153))));
1520 evalcond[0]=x152;
1521 evalcond[1]=((-1.0)*x151);
1522 evalcond[2]=((((-1.0)*x151*x153))+new_r11);
1523 evalcond[3]=((((-1.0)*x152*x153))+new_r10);
1524 evalcond[4]=(((x151*x154))+new_r01);
1525 evalcond[5]=(((x152*x154))+new_r00);
1526 evalcond[6]=(((new_r00*x154))+x152+(((-1.0)*new_r10*x153)));
1527 evalcond[7]=(((new_r01*x154))+x151+(((-1.0)*new_r11*x153)));
1528 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1529 {
1530 continue;
1531 }
1532 }
1533 
1534 {
1535 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1536 vinfos[0].jointtype = 1;
1537 vinfos[0].foffset = j0;
1538 vinfos[0].indices[0] = _ij0[0];
1539 vinfos[0].indices[1] = _ij0[1];
1540 vinfos[0].maxsolutions = _nj0;
1541 vinfos[1].jointtype = 1;
1542 vinfos[1].foffset = j1;
1543 vinfos[1].indices[0] = _ij1[0];
1544 vinfos[1].indices[1] = _ij1[1];
1545 vinfos[1].maxsolutions = _nj1;
1546 vinfos[2].jointtype = 1;
1547 vinfos[2].foffset = j2;
1548 vinfos[2].indices[0] = _ij2[0];
1549 vinfos[2].indices[1] = _ij2[1];
1550 vinfos[2].maxsolutions = _nj2;
1551 vinfos[3].jointtype = 1;
1552 vinfos[3].foffset = j3;
1553 vinfos[3].indices[0] = _ij3[0];
1554 vinfos[3].indices[1] = _ij3[1];
1555 vinfos[3].maxsolutions = _nj3;
1556 vinfos[4].jointtype = 1;
1557 vinfos[4].foffset = j4;
1558 vinfos[4].indices[0] = _ij4[0];
1559 vinfos[4].indices[1] = _ij4[1];
1560 vinfos[4].maxsolutions = _nj4;
1561 vinfos[5].jointtype = 1;
1562 vinfos[5].foffset = j5;
1563 vinfos[5].indices[0] = _ij5[0];
1564 vinfos[5].indices[1] = _ij5[1];
1565 vinfos[5].maxsolutions = _nj5;
1566 std::vector<int> vfree(0);
1567 solutions.AddSolution(vinfos,vfree);
1568 }
1569 }
1570 }
1571 
1572 }
1573 
1574 }
1575 
1576 }
1577 } while(0);
1578 if( bgotonextstatement )
1579 {
1580 bool bgotonextstatement = true;
1581 do
1582 {
1583 CheckValue<IkReal> x155=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1584 if(!x155.valid){
1585 continue;
1586 }
1587 if((x155.value) < -0.00001)
1588 continue;
1589 IkReal gconst24=((-1.0)*(IKsqrt(x155.value)));
1590 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1591 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1592 {
1593 bgotonextstatement=false;
1594 {
1595 IkReal j5eval[1];
1596 new_r21=0;
1597 new_r20=0;
1598 new_r02=0;
1599 new_r12=0;
1600 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1601 continue;
1602 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
1603 cj3=gconst24;
1604 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1605  continue;
1606 j3=((-1.0)*(IKacos(gconst24)));
1607 CheckValue<IkReal> x156=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1608 if(!x156.valid){
1609 continue;
1610 }
1611 if((x156.value) < -0.00001)
1612 continue;
1613 IkReal gconst24=((-1.0)*(IKsqrt(x156.value)));
1614 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1615 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1616 {
1617 {
1618 IkReal j5array[1], cj5array[1], sj5array[1];
1619 bool j5valid[1]={false};
1620 _nj5 = 1;
1621 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1622 continue;
1623 CheckValue<IkReal> x157=IKPowWithIntegerCheck(gconst24,-1);
1624 if(!x157.valid){
1625 continue;
1626 }
1627 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x157.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x157.value)))-1) <= IKFAST_SINCOS_THRESH )
1628  continue;
1629 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x157.value)));
1630 sj5array[0]=IKsin(j5array[0]);
1631 cj5array[0]=IKcos(j5array[0]);
1632 if( j5array[0] > IKPI )
1633 {
1634  j5array[0]-=IK2PI;
1635 }
1636 else if( j5array[0] < -IKPI )
1637 { j5array[0]+=IK2PI;
1638 }
1639 j5valid[0] = true;
1640 for(int ij5 = 0; ij5 < 1; ++ij5)
1641 {
1642 if( !j5valid[ij5] )
1643 {
1644  continue;
1645 }
1646 _ij5[0] = ij5; _ij5[1] = -1;
1647 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1648 {
1649 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1650 {
1651  j5valid[iij5]=false; _ij5[1] = iij5; break;
1652 }
1653 }
1654 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1655 {
1656 IkReal evalcond[8];
1657 IkReal x158=IKcos(j5);
1658 IkReal x159=IKsin(j5);
1659 IkReal x160=((1.0)*gconst24);
1660 IkReal x161=((1.0)*x159);
1661 if((((1.0)+(((-1.0)*gconst24*x160)))) < -0.00001)
1662 continue;
1663 IkReal x162=IKsqrt(((1.0)+(((-1.0)*gconst24*x160))));
1664 IkReal x163=((1.0)*x162);
1665 evalcond[0]=x159;
1666 evalcond[1]=((-1.0)*x158);
1667 evalcond[2]=((((-1.0)*x158*x160))+new_r11);
1668 evalcond[3]=((((-1.0)*x159*x160))+new_r10);
1669 evalcond[4]=((((-1.0)*x158*x163))+new_r01);
1670 evalcond[5]=((((-1.0)*x161*x162))+new_r00);
1671 evalcond[6]=((((-1.0)*new_r00*x163))+(((-1.0)*new_r10*x160))+x159);
1672 evalcond[7]=((((-1.0)*new_r11*x160))+x158+(((-1.0)*new_r01*x163)));
1673 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1674 {
1675 continue;
1676 }
1677 }
1678 
1679 {
1680 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1681 vinfos[0].jointtype = 1;
1682 vinfos[0].foffset = j0;
1683 vinfos[0].indices[0] = _ij0[0];
1684 vinfos[0].indices[1] = _ij0[1];
1685 vinfos[0].maxsolutions = _nj0;
1686 vinfos[1].jointtype = 1;
1687 vinfos[1].foffset = j1;
1688 vinfos[1].indices[0] = _ij1[0];
1689 vinfos[1].indices[1] = _ij1[1];
1690 vinfos[1].maxsolutions = _nj1;
1691 vinfos[2].jointtype = 1;
1692 vinfos[2].foffset = j2;
1693 vinfos[2].indices[0] = _ij2[0];
1694 vinfos[2].indices[1] = _ij2[1];
1695 vinfos[2].maxsolutions = _nj2;
1696 vinfos[3].jointtype = 1;
1697 vinfos[3].foffset = j3;
1698 vinfos[3].indices[0] = _ij3[0];
1699 vinfos[3].indices[1] = _ij3[1];
1700 vinfos[3].maxsolutions = _nj3;
1701 vinfos[4].jointtype = 1;
1702 vinfos[4].foffset = j4;
1703 vinfos[4].indices[0] = _ij4[0];
1704 vinfos[4].indices[1] = _ij4[1];
1705 vinfos[4].maxsolutions = _nj4;
1706 vinfos[5].jointtype = 1;
1707 vinfos[5].foffset = j5;
1708 vinfos[5].indices[0] = _ij5[0];
1709 vinfos[5].indices[1] = _ij5[1];
1710 vinfos[5].maxsolutions = _nj5;
1711 std::vector<int> vfree(0);
1712 solutions.AddSolution(vinfos,vfree);
1713 }
1714 }
1715 }
1716 
1717 } else
1718 {
1719 {
1720 IkReal j5array[1], cj5array[1], sj5array[1];
1721 bool j5valid[1]={false};
1722 _nj5 = 1;
1723 CheckValue<IkReal> x164=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1724 if(!x164.valid){
1725 continue;
1726 }
1727 CheckValue<IkReal> x165 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1728 if(!x165.valid){
1729 continue;
1730 }
1731 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x164.value)))+(x165.value));
1732 sj5array[0]=IKsin(j5array[0]);
1733 cj5array[0]=IKcos(j5array[0]);
1734 if( j5array[0] > IKPI )
1735 {
1736  j5array[0]-=IK2PI;
1737 }
1738 else if( j5array[0] < -IKPI )
1739 { j5array[0]+=IK2PI;
1740 }
1741 j5valid[0] = true;
1742 for(int ij5 = 0; ij5 < 1; ++ij5)
1743 {
1744 if( !j5valid[ij5] )
1745 {
1746  continue;
1747 }
1748 _ij5[0] = ij5; _ij5[1] = -1;
1749 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1750 {
1751 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1752 {
1753  j5valid[iij5]=false; _ij5[1] = iij5; break;
1754 }
1755 }
1756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1757 {
1758 IkReal evalcond[8];
1759 IkReal x166=IKcos(j5);
1760 IkReal x167=IKsin(j5);
1761 IkReal x168=((1.0)*gconst24);
1762 IkReal x169=((1.0)*x167);
1763 if((((1.0)+(((-1.0)*gconst24*x168)))) < -0.00001)
1764 continue;
1765 IkReal x170=IKsqrt(((1.0)+(((-1.0)*gconst24*x168))));
1766 IkReal x171=((1.0)*x170);
1767 evalcond[0]=x167;
1768 evalcond[1]=((-1.0)*x166);
1769 evalcond[2]=((((-1.0)*x166*x168))+new_r11);
1770 evalcond[3]=(new_r10+(((-1.0)*x167*x168)));
1771 evalcond[4]=(new_r01+(((-1.0)*x166*x171)));
1772 evalcond[5]=(new_r00+(((-1.0)*x169*x170)));
1773 evalcond[6]=((((-1.0)*new_r10*x168))+x167+(((-1.0)*new_r00*x171)));
1774 evalcond[7]=((((-1.0)*new_r11*x168))+x166+(((-1.0)*new_r01*x171)));
1775 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1776 {
1777 continue;
1778 }
1779 }
1780 
1781 {
1782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1783 vinfos[0].jointtype = 1;
1784 vinfos[0].foffset = j0;
1785 vinfos[0].indices[0] = _ij0[0];
1786 vinfos[0].indices[1] = _ij0[1];
1787 vinfos[0].maxsolutions = _nj0;
1788 vinfos[1].jointtype = 1;
1789 vinfos[1].foffset = j1;
1790 vinfos[1].indices[0] = _ij1[0];
1791 vinfos[1].indices[1] = _ij1[1];
1792 vinfos[1].maxsolutions = _nj1;
1793 vinfos[2].jointtype = 1;
1794 vinfos[2].foffset = j2;
1795 vinfos[2].indices[0] = _ij2[0];
1796 vinfos[2].indices[1] = _ij2[1];
1797 vinfos[2].maxsolutions = _nj2;
1798 vinfos[3].jointtype = 1;
1799 vinfos[3].foffset = j3;
1800 vinfos[3].indices[0] = _ij3[0];
1801 vinfos[3].indices[1] = _ij3[1];
1802 vinfos[3].maxsolutions = _nj3;
1803 vinfos[4].jointtype = 1;
1804 vinfos[4].foffset = j4;
1805 vinfos[4].indices[0] = _ij4[0];
1806 vinfos[4].indices[1] = _ij4[1];
1807 vinfos[4].maxsolutions = _nj4;
1808 vinfos[5].jointtype = 1;
1809 vinfos[5].foffset = j5;
1810 vinfos[5].indices[0] = _ij5[0];
1811 vinfos[5].indices[1] = _ij5[1];
1812 vinfos[5].maxsolutions = _nj5;
1813 std::vector<int> vfree(0);
1814 solutions.AddSolution(vinfos,vfree);
1815 }
1816 }
1817 }
1818 
1819 }
1820 
1821 }
1822 
1823 }
1824 } while(0);
1825 if( bgotonextstatement )
1826 {
1827 bool bgotonextstatement = true;
1828 do
1829 {
1830 CheckValue<IkReal> x172=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1831 if(!x172.valid){
1832 continue;
1833 }
1834 if((x172.value) < -0.00001)
1835 continue;
1836 IkReal gconst25=IKsqrt(x172.value);
1837 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1838 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1839 {
1840 bgotonextstatement=false;
1841 {
1842 IkReal j5eval[1];
1843 new_r21=0;
1844 new_r20=0;
1845 new_r02=0;
1846 new_r12=0;
1847 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1848 continue;
1849 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
1850 cj3=gconst25;
1851 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
1852  continue;
1853 j3=IKacos(gconst25);
1854 CheckValue<IkReal> x173=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1855 if(!x173.valid){
1856 continue;
1857 }
1858 if((x173.value) < -0.00001)
1859 continue;
1860 IkReal gconst25=IKsqrt(x173.value);
1861 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1862 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1863 {
1864 {
1865 IkReal j5array[1], cj5array[1], sj5array[1];
1866 bool j5valid[1]={false};
1867 _nj5 = 1;
1868 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1869 continue;
1870 CheckValue<IkReal> x174=IKPowWithIntegerCheck(gconst25,-1);
1871 if(!x174.valid){
1872 continue;
1873 }
1874 if( IKabs((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x174.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x174.value)))-1) <= IKFAST_SINCOS_THRESH )
1875  continue;
1876 j5array[0]=IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x174.value)));
1877 sj5array[0]=IKsin(j5array[0]);
1878 cj5array[0]=IKcos(j5array[0]);
1879 if( j5array[0] > IKPI )
1880 {
1881  j5array[0]-=IK2PI;
1882 }
1883 else if( j5array[0] < -IKPI )
1884 { j5array[0]+=IK2PI;
1885 }
1886 j5valid[0] = true;
1887 for(int ij5 = 0; ij5 < 1; ++ij5)
1888 {
1889 if( !j5valid[ij5] )
1890 {
1891  continue;
1892 }
1893 _ij5[0] = ij5; _ij5[1] = -1;
1894 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1895 {
1896 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1897 {
1898  j5valid[iij5]=false; _ij5[1] = iij5; break;
1899 }
1900 }
1901 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1902 {
1903 IkReal evalcond[8];
1904 IkReal x175=IKcos(j5);
1905 IkReal x176=IKsin(j5);
1906 IkReal x177=((1.0)*gconst25);
1907 if((((1.0)+(((-1.0)*gconst25*x177)))) < -0.00001)
1908 continue;
1909 IkReal x178=IKsqrt(((1.0)+(((-1.0)*gconst25*x177))));
1910 evalcond[0]=x176;
1911 evalcond[1]=((-1.0)*x175);
1912 evalcond[2]=((((-1.0)*x175*x177))+new_r11);
1913 evalcond[3]=((((-1.0)*x176*x177))+new_r10);
1914 evalcond[4]=(((x175*x178))+new_r01);
1915 evalcond[5]=(((x176*x178))+new_r00);
1916 evalcond[6]=(((new_r00*x178))+(((-1.0)*new_r10*x177))+x176);
1917 evalcond[7]=(((new_r01*x178))+(((-1.0)*new_r11*x177))+x175);
1918 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1919 {
1920 continue;
1921 }
1922 }
1923 
1924 {
1925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1926 vinfos[0].jointtype = 1;
1927 vinfos[0].foffset = j0;
1928 vinfos[0].indices[0] = _ij0[0];
1929 vinfos[0].indices[1] = _ij0[1];
1930 vinfos[0].maxsolutions = _nj0;
1931 vinfos[1].jointtype = 1;
1932 vinfos[1].foffset = j1;
1933 vinfos[1].indices[0] = _ij1[0];
1934 vinfos[1].indices[1] = _ij1[1];
1935 vinfos[1].maxsolutions = _nj1;
1936 vinfos[2].jointtype = 1;
1937 vinfos[2].foffset = j2;
1938 vinfos[2].indices[0] = _ij2[0];
1939 vinfos[2].indices[1] = _ij2[1];
1940 vinfos[2].maxsolutions = _nj2;
1941 vinfos[3].jointtype = 1;
1942 vinfos[3].foffset = j3;
1943 vinfos[3].indices[0] = _ij3[0];
1944 vinfos[3].indices[1] = _ij3[1];
1945 vinfos[3].maxsolutions = _nj3;
1946 vinfos[4].jointtype = 1;
1947 vinfos[4].foffset = j4;
1948 vinfos[4].indices[0] = _ij4[0];
1949 vinfos[4].indices[1] = _ij4[1];
1950 vinfos[4].maxsolutions = _nj4;
1951 vinfos[5].jointtype = 1;
1952 vinfos[5].foffset = j5;
1953 vinfos[5].indices[0] = _ij5[0];
1954 vinfos[5].indices[1] = _ij5[1];
1955 vinfos[5].maxsolutions = _nj5;
1956 std::vector<int> vfree(0);
1957 solutions.AddSolution(vinfos,vfree);
1958 }
1959 }
1960 }
1961 
1962 } else
1963 {
1964 {
1965 IkReal j5array[1], cj5array[1], sj5array[1];
1966 bool j5valid[1]={false};
1967 _nj5 = 1;
1968 CheckValue<IkReal> x179=IKPowWithIntegerCheck(IKsign(gconst25),-1);
1969 if(!x179.valid){
1970 continue;
1971 }
1972 CheckValue<IkReal> x180 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1973 if(!x180.valid){
1974 continue;
1975 }
1976 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x179.value)))+(x180.value));
1977 sj5array[0]=IKsin(j5array[0]);
1978 cj5array[0]=IKcos(j5array[0]);
1979 if( j5array[0] > IKPI )
1980 {
1981  j5array[0]-=IK2PI;
1982 }
1983 else if( j5array[0] < -IKPI )
1984 { j5array[0]+=IK2PI;
1985 }
1986 j5valid[0] = true;
1987 for(int ij5 = 0; ij5 < 1; ++ij5)
1988 {
1989 if( !j5valid[ij5] )
1990 {
1991  continue;
1992 }
1993 _ij5[0] = ij5; _ij5[1] = -1;
1994 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1995 {
1996 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1997 {
1998  j5valid[iij5]=false; _ij5[1] = iij5; break;
1999 }
2000 }
2001 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2002 {
2003 IkReal evalcond[8];
2004 IkReal x181=IKcos(j5);
2005 IkReal x182=IKsin(j5);
2006 IkReal x183=((1.0)*gconst25);
2007 if((((1.0)+(((-1.0)*gconst25*x183)))) < -0.00001)
2008 continue;
2009 IkReal x184=IKsqrt(((1.0)+(((-1.0)*gconst25*x183))));
2010 evalcond[0]=x182;
2011 evalcond[1]=((-1.0)*x181);
2012 evalcond[2]=(new_r11+(((-1.0)*x181*x183)));
2013 evalcond[3]=(new_r10+(((-1.0)*x182*x183)));
2014 evalcond[4]=(new_r01+((x181*x184)));
2015 evalcond[5]=(new_r00+((x182*x184)));
2016 evalcond[6]=(((new_r00*x184))+x182+(((-1.0)*new_r10*x183)));
2017 evalcond[7]=(((new_r01*x184))+x181+(((-1.0)*new_r11*x183)));
2018 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2019 {
2020 continue;
2021 }
2022 }
2023 
2024 {
2025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2026 vinfos[0].jointtype = 1;
2027 vinfos[0].foffset = j0;
2028 vinfos[0].indices[0] = _ij0[0];
2029 vinfos[0].indices[1] = _ij0[1];
2030 vinfos[0].maxsolutions = _nj0;
2031 vinfos[1].jointtype = 1;
2032 vinfos[1].foffset = j1;
2033 vinfos[1].indices[0] = _ij1[0];
2034 vinfos[1].indices[1] = _ij1[1];
2035 vinfos[1].maxsolutions = _nj1;
2036 vinfos[2].jointtype = 1;
2037 vinfos[2].foffset = j2;
2038 vinfos[2].indices[0] = _ij2[0];
2039 vinfos[2].indices[1] = _ij2[1];
2040 vinfos[2].maxsolutions = _nj2;
2041 vinfos[3].jointtype = 1;
2042 vinfos[3].foffset = j3;
2043 vinfos[3].indices[0] = _ij3[0];
2044 vinfos[3].indices[1] = _ij3[1];
2045 vinfos[3].maxsolutions = _nj3;
2046 vinfos[4].jointtype = 1;
2047 vinfos[4].foffset = j4;
2048 vinfos[4].indices[0] = _ij4[0];
2049 vinfos[4].indices[1] = _ij4[1];
2050 vinfos[4].maxsolutions = _nj4;
2051 vinfos[5].jointtype = 1;
2052 vinfos[5].foffset = j5;
2053 vinfos[5].indices[0] = _ij5[0];
2054 vinfos[5].indices[1] = _ij5[1];
2055 vinfos[5].maxsolutions = _nj5;
2056 std::vector<int> vfree(0);
2057 solutions.AddSolution(vinfos,vfree);
2058 }
2059 }
2060 }
2061 
2062 }
2063 
2064 }
2065 
2066 }
2067 } while(0);
2068 if( bgotonextstatement )
2069 {
2070 bool bgotonextstatement = true;
2071 do
2072 {
2073 CheckValue<IkReal> x185=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2074 if(!x185.valid){
2075 continue;
2076 }
2077 if((x185.value) < -0.00001)
2078 continue;
2079 IkReal gconst25=IKsqrt(x185.value);
2080 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
2081 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2082 {
2083 bgotonextstatement=false;
2084 {
2085 IkReal j5eval[1];
2086 new_r21=0;
2087 new_r20=0;
2088 new_r02=0;
2089 new_r12=0;
2090 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2091 continue;
2092 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
2093 cj3=gconst25;
2094 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
2095  continue;
2096 j3=((-1.0)*(IKacos(gconst25)));
2097 CheckValue<IkReal> x186=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2098 if(!x186.valid){
2099 continue;
2100 }
2101 if((x186.value) < -0.00001)
2102 continue;
2103 IkReal gconst25=IKsqrt(x186.value);
2104 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2105 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2106 {
2107 {
2108 IkReal j5array[1], cj5array[1], sj5array[1];
2109 bool j5valid[1]={false};
2110 _nj5 = 1;
2111 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2112 continue;
2113 CheckValue<IkReal> x187=IKPowWithIntegerCheck(gconst25,-1);
2114 if(!x187.valid){
2115 continue;
2116 }
2117 if( IKabs((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x187.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x187.value)))-1) <= IKFAST_SINCOS_THRESH )
2118  continue;
2119 j5array[0]=IKatan2((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x187.value)));
2120 sj5array[0]=IKsin(j5array[0]);
2121 cj5array[0]=IKcos(j5array[0]);
2122 if( j5array[0] > IKPI )
2123 {
2124  j5array[0]-=IK2PI;
2125 }
2126 else if( j5array[0] < -IKPI )
2127 { j5array[0]+=IK2PI;
2128 }
2129 j5valid[0] = true;
2130 for(int ij5 = 0; ij5 < 1; ++ij5)
2131 {
2132 if( !j5valid[ij5] )
2133 {
2134  continue;
2135 }
2136 _ij5[0] = ij5; _ij5[1] = -1;
2137 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2138 {
2139 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2140 {
2141  j5valid[iij5]=false; _ij5[1] = iij5; break;
2142 }
2143 }
2144 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2145 {
2146 IkReal evalcond[8];
2147 IkReal x188=IKcos(j5);
2148 IkReal x189=IKsin(j5);
2149 IkReal x190=((1.0)*gconst25);
2150 if((((1.0)+(((-1.0)*gconst25*x190)))) < -0.00001)
2151 continue;
2152 IkReal x191=IKsqrt(((1.0)+(((-1.0)*gconst25*x190))));
2153 IkReal x192=((1.0)*x191);
2154 evalcond[0]=x189;
2155 evalcond[1]=((-1.0)*x188);
2156 evalcond[2]=((((-1.0)*x188*x190))+new_r11);
2157 evalcond[3]=((((-1.0)*x189*x190))+new_r10);
2158 evalcond[4]=((((-1.0)*x188*x192))+new_r01);
2159 evalcond[5]=((((-1.0)*x189*x192))+new_r00);
2160 evalcond[6]=(x189+(((-1.0)*new_r10*x190))+(((-1.0)*new_r00*x192)));
2161 evalcond[7]=(x188+(((-1.0)*new_r11*x190))+(((-1.0)*new_r01*x192)));
2162 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2163 {
2164 continue;
2165 }
2166 }
2167 
2168 {
2169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2170 vinfos[0].jointtype = 1;
2171 vinfos[0].foffset = j0;
2172 vinfos[0].indices[0] = _ij0[0];
2173 vinfos[0].indices[1] = _ij0[1];
2174 vinfos[0].maxsolutions = _nj0;
2175 vinfos[1].jointtype = 1;
2176 vinfos[1].foffset = j1;
2177 vinfos[1].indices[0] = _ij1[0];
2178 vinfos[1].indices[1] = _ij1[1];
2179 vinfos[1].maxsolutions = _nj1;
2180 vinfos[2].jointtype = 1;
2181 vinfos[2].foffset = j2;
2182 vinfos[2].indices[0] = _ij2[0];
2183 vinfos[2].indices[1] = _ij2[1];
2184 vinfos[2].maxsolutions = _nj2;
2185 vinfos[3].jointtype = 1;
2186 vinfos[3].foffset = j3;
2187 vinfos[3].indices[0] = _ij3[0];
2188 vinfos[3].indices[1] = _ij3[1];
2189 vinfos[3].maxsolutions = _nj3;
2190 vinfos[4].jointtype = 1;
2191 vinfos[4].foffset = j4;
2192 vinfos[4].indices[0] = _ij4[0];
2193 vinfos[4].indices[1] = _ij4[1];
2194 vinfos[4].maxsolutions = _nj4;
2195 vinfos[5].jointtype = 1;
2196 vinfos[5].foffset = j5;
2197 vinfos[5].indices[0] = _ij5[0];
2198 vinfos[5].indices[1] = _ij5[1];
2199 vinfos[5].maxsolutions = _nj5;
2200 std::vector<int> vfree(0);
2201 solutions.AddSolution(vinfos,vfree);
2202 }
2203 }
2204 }
2205 
2206 } else
2207 {
2208 {
2209 IkReal j5array[1], cj5array[1], sj5array[1];
2210 bool j5valid[1]={false};
2211 _nj5 = 1;
2212 CheckValue<IkReal> x193=IKPowWithIntegerCheck(IKsign(gconst25),-1);
2213 if(!x193.valid){
2214 continue;
2215 }
2216 CheckValue<IkReal> x194 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2217 if(!x194.valid){
2218 continue;
2219 }
2220 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x193.value)))+(x194.value));
2221 sj5array[0]=IKsin(j5array[0]);
2222 cj5array[0]=IKcos(j5array[0]);
2223 if( j5array[0] > IKPI )
2224 {
2225  j5array[0]-=IK2PI;
2226 }
2227 else if( j5array[0] < -IKPI )
2228 { j5array[0]+=IK2PI;
2229 }
2230 j5valid[0] = true;
2231 for(int ij5 = 0; ij5 < 1; ++ij5)
2232 {
2233 if( !j5valid[ij5] )
2234 {
2235  continue;
2236 }
2237 _ij5[0] = ij5; _ij5[1] = -1;
2238 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2239 {
2240 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2241 {
2242  j5valid[iij5]=false; _ij5[1] = iij5; break;
2243 }
2244 }
2245 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2246 {
2247 IkReal evalcond[8];
2248 IkReal x195=IKcos(j5);
2249 IkReal x196=IKsin(j5);
2250 IkReal x197=((1.0)*gconst25);
2251 if((((1.0)+(((-1.0)*gconst25*x197)))) < -0.00001)
2252 continue;
2253 IkReal x198=IKsqrt(((1.0)+(((-1.0)*gconst25*x197))));
2254 IkReal x199=((1.0)*x198);
2255 evalcond[0]=x196;
2256 evalcond[1]=((-1.0)*x195);
2257 evalcond[2]=((((-1.0)*x195*x197))+new_r11);
2258 evalcond[3]=((((-1.0)*x196*x197))+new_r10);
2259 evalcond[4]=((((-1.0)*x195*x199))+new_r01);
2260 evalcond[5]=((((-1.0)*x196*x199))+new_r00);
2261 evalcond[6]=(x196+(((-1.0)*new_r10*x197))+(((-1.0)*new_r00*x199)));
2262 evalcond[7]=(x195+(((-1.0)*new_r11*x197))+(((-1.0)*new_r01*x199)));
2263 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2264 {
2265 continue;
2266 }
2267 }
2268 
2269 {
2270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2271 vinfos[0].jointtype = 1;
2272 vinfos[0].foffset = j0;
2273 vinfos[0].indices[0] = _ij0[0];
2274 vinfos[0].indices[1] = _ij0[1];
2275 vinfos[0].maxsolutions = _nj0;
2276 vinfos[1].jointtype = 1;
2277 vinfos[1].foffset = j1;
2278 vinfos[1].indices[0] = _ij1[0];
2279 vinfos[1].indices[1] = _ij1[1];
2280 vinfos[1].maxsolutions = _nj1;
2281 vinfos[2].jointtype = 1;
2282 vinfos[2].foffset = j2;
2283 vinfos[2].indices[0] = _ij2[0];
2284 vinfos[2].indices[1] = _ij2[1];
2285 vinfos[2].maxsolutions = _nj2;
2286 vinfos[3].jointtype = 1;
2287 vinfos[3].foffset = j3;
2288 vinfos[3].indices[0] = _ij3[0];
2289 vinfos[3].indices[1] = _ij3[1];
2290 vinfos[3].maxsolutions = _nj3;
2291 vinfos[4].jointtype = 1;
2292 vinfos[4].foffset = j4;
2293 vinfos[4].indices[0] = _ij4[0];
2294 vinfos[4].indices[1] = _ij4[1];
2295 vinfos[4].maxsolutions = _nj4;
2296 vinfos[5].jointtype = 1;
2297 vinfos[5].foffset = j5;
2298 vinfos[5].indices[0] = _ij5[0];
2299 vinfos[5].indices[1] = _ij5[1];
2300 vinfos[5].maxsolutions = _nj5;
2301 std::vector<int> vfree(0);
2302 solutions.AddSolution(vinfos,vfree);
2303 }
2304 }
2305 }
2306 
2307 }
2308 
2309 }
2310 
2311 }
2312 } while(0);
2313 if( bgotonextstatement )
2314 {
2315 bool bgotonextstatement = true;
2316 do
2317 {
2318 if( 1 )
2319 {
2320 bgotonextstatement=false;
2321 continue; // branch miss [j5]
2322 
2323 }
2324 } while(0);
2325 if( bgotonextstatement )
2326 {
2327 }
2328 }
2329 }
2330 }
2331 }
2332 }
2333 }
2334 }
2335 
2336 } else
2337 {
2338 {
2339 IkReal j5array[1], cj5array[1], sj5array[1];
2340 bool j5valid[1]={false};
2341 _nj5 = 1;
2342 IkReal x200=(new_r00*sj3);
2343 CheckValue<IkReal> x201=IKPowWithIntegerCheck(sj3,-1);
2344 if(!x201.valid){
2345 continue;
2346 }
2347 if( IKabs((((cj3*new_r10))+(((-1.0)*x200)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r10))+(((-1.0)*x200))))+IKsqr(((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2348  continue;
2349 j5array[0]=IKatan2((((cj3*new_r10))+(((-1.0)*x200))), ((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2350 sj5array[0]=IKsin(j5array[0]);
2351 cj5array[0]=IKcos(j5array[0]);
2352 if( j5array[0] > IKPI )
2353 {
2354  j5array[0]-=IK2PI;
2355 }
2356 else if( j5array[0] < -IKPI )
2357 { j5array[0]+=IK2PI;
2358 }
2359 j5valid[0] = true;
2360 for(int ij5 = 0; ij5 < 1; ++ij5)
2361 {
2362 if( !j5valid[ij5] )
2363 {
2364  continue;
2365 }
2366 _ij5[0] = ij5; _ij5[1] = -1;
2367 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2368 {
2369 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2370 {
2371  j5valid[iij5]=false; _ij5[1] = iij5; break;
2372 }
2373 }
2374 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2375 {
2376 IkReal evalcond[10];
2377 IkReal x202=IKsin(j5);
2378 IkReal x203=IKcos(j5);
2379 IkReal x204=((1.0)*cj3);
2380 IkReal x205=(cj3*new_r00);
2381 IkReal x206=(cj3*new_r22);
2382 IkReal x207=(new_r22*sj3);
2383 IkReal x208=(new_r22*x203);
2384 IkReal x209=(sj3*x202);
2385 evalcond[0]=(((new_r00*sj3))+x202+(((-1.0)*new_r10*x204)));
2386 evalcond[1]=(((new_r01*sj3))+x203+(((-1.0)*new_r11*x204)));
2387 evalcond[2]=(((new_r11*sj3))+((new_r22*x202))+((cj3*new_r01)));
2388 evalcond[3]=(((new_r11*x207))+((new_r01*x206))+x202);
2389 evalcond[4]=(((sj3*x203))+((x202*x206))+new_r01);
2390 evalcond[5]=(((new_r10*sj3))+x205+(((-1.0)*x208)));
2391 evalcond[6]=((((-1.0)*x204*x208))+x209+new_r00);
2392 evalcond[7]=((((-1.0)*x203*x204))+((x202*x207))+new_r11);
2393 evalcond[8]=(((new_r10*x207))+((new_r22*x205))+(((-1.0)*x203)));
2394 evalcond[9]=((((-1.0)*x202*x204))+(((-1.0)*x203*x207))+new_r10);
2395 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2396 {
2397 continue;
2398 }
2399 }
2400 
2401 {
2402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2403 vinfos[0].jointtype = 1;
2404 vinfos[0].foffset = j0;
2405 vinfos[0].indices[0] = _ij0[0];
2406 vinfos[0].indices[1] = _ij0[1];
2407 vinfos[0].maxsolutions = _nj0;
2408 vinfos[1].jointtype = 1;
2409 vinfos[1].foffset = j1;
2410 vinfos[1].indices[0] = _ij1[0];
2411 vinfos[1].indices[1] = _ij1[1];
2412 vinfos[1].maxsolutions = _nj1;
2413 vinfos[2].jointtype = 1;
2414 vinfos[2].foffset = j2;
2415 vinfos[2].indices[0] = _ij2[0];
2416 vinfos[2].indices[1] = _ij2[1];
2417 vinfos[2].maxsolutions = _nj2;
2418 vinfos[3].jointtype = 1;
2419 vinfos[3].foffset = j3;
2420 vinfos[3].indices[0] = _ij3[0];
2421 vinfos[3].indices[1] = _ij3[1];
2422 vinfos[3].maxsolutions = _nj3;
2423 vinfos[4].jointtype = 1;
2424 vinfos[4].foffset = j4;
2425 vinfos[4].indices[0] = _ij4[0];
2426 vinfos[4].indices[1] = _ij4[1];
2427 vinfos[4].maxsolutions = _nj4;
2428 vinfos[5].jointtype = 1;
2429 vinfos[5].foffset = j5;
2430 vinfos[5].indices[0] = _ij5[0];
2431 vinfos[5].indices[1] = _ij5[1];
2432 vinfos[5].maxsolutions = _nj5;
2433 std::vector<int> vfree(0);
2434 solutions.AddSolution(vinfos,vfree);
2435 }
2436 }
2437 }
2438 
2439 }
2440 
2441 }
2442 
2443 } else
2444 {
2445 {
2446 IkReal j5array[1], cj5array[1], sj5array[1];
2447 bool j5valid[1]={false};
2448 _nj5 = 1;
2449 IkReal x210=((1.0)*new_r01);
2450 CheckValue<IkReal> x211=IKPowWithIntegerCheck(new_r22,-1);
2451 if(!x211.valid){
2452 continue;
2453 }
2454 if( IKabs(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x210))+((cj3*new_r11)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3))))))+IKsqr(((((-1.0)*sj3*x210))+((cj3*new_r11))))-1) <= IKFAST_SINCOS_THRESH )
2455  continue;
2456 j5array[0]=IKatan2(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3))))), ((((-1.0)*sj3*x210))+((cj3*new_r11))));
2457 sj5array[0]=IKsin(j5array[0]);
2458 cj5array[0]=IKcos(j5array[0]);
2459 if( j5array[0] > IKPI )
2460 {
2461  j5array[0]-=IK2PI;
2462 }
2463 else if( j5array[0] < -IKPI )
2464 { j5array[0]+=IK2PI;
2465 }
2466 j5valid[0] = true;
2467 for(int ij5 = 0; ij5 < 1; ++ij5)
2468 {
2469 if( !j5valid[ij5] )
2470 {
2471  continue;
2472 }
2473 _ij5[0] = ij5; _ij5[1] = -1;
2474 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2475 {
2476 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2477 {
2478  j5valid[iij5]=false; _ij5[1] = iij5; break;
2479 }
2480 }
2481 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2482 {
2483 IkReal evalcond[10];
2484 IkReal x212=IKsin(j5);
2485 IkReal x213=IKcos(j5);
2486 IkReal x214=((1.0)*cj3);
2487 IkReal x215=(cj3*new_r00);
2488 IkReal x216=(cj3*new_r22);
2489 IkReal x217=(new_r22*sj3);
2490 IkReal x218=(new_r22*x213);
2491 IkReal x219=(sj3*x212);
2492 evalcond[0]=(((new_r00*sj3))+(((-1.0)*new_r10*x214))+x212);
2493 evalcond[1]=((((-1.0)*new_r11*x214))+((new_r01*sj3))+x213);
2494 evalcond[2]=(((new_r22*x212))+((new_r11*sj3))+((cj3*new_r01)));
2495 evalcond[3]=(x212+((new_r01*x216))+((new_r11*x217)));
2496 evalcond[4]=(((sj3*x213))+((x212*x216))+new_r01);
2497 evalcond[5]=(((new_r10*sj3))+x215+(((-1.0)*x218)));
2498 evalcond[6]=(x219+(((-1.0)*x214*x218))+new_r00);
2499 evalcond[7]=(((x212*x217))+(((-1.0)*x213*x214))+new_r11);
2500 evalcond[8]=(((new_r22*x215))+(((-1.0)*x213))+((new_r10*x217)));
2501 evalcond[9]=((((-1.0)*x213*x217))+new_r10+(((-1.0)*x212*x214)));
2502 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2503 {
2504 continue;
2505 }
2506 }
2507 
2508 {
2509 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2510 vinfos[0].jointtype = 1;
2511 vinfos[0].foffset = j0;
2512 vinfos[0].indices[0] = _ij0[0];
2513 vinfos[0].indices[1] = _ij0[1];
2514 vinfos[0].maxsolutions = _nj0;
2515 vinfos[1].jointtype = 1;
2516 vinfos[1].foffset = j1;
2517 vinfos[1].indices[0] = _ij1[0];
2518 vinfos[1].indices[1] = _ij1[1];
2519 vinfos[1].maxsolutions = _nj1;
2520 vinfos[2].jointtype = 1;
2521 vinfos[2].foffset = j2;
2522 vinfos[2].indices[0] = _ij2[0];
2523 vinfos[2].indices[1] = _ij2[1];
2524 vinfos[2].maxsolutions = _nj2;
2525 vinfos[3].jointtype = 1;
2526 vinfos[3].foffset = j3;
2527 vinfos[3].indices[0] = _ij3[0];
2528 vinfos[3].indices[1] = _ij3[1];
2529 vinfos[3].maxsolutions = _nj3;
2530 vinfos[4].jointtype = 1;
2531 vinfos[4].foffset = j4;
2532 vinfos[4].indices[0] = _ij4[0];
2533 vinfos[4].indices[1] = _ij4[1];
2534 vinfos[4].maxsolutions = _nj4;
2535 vinfos[5].jointtype = 1;
2536 vinfos[5].foffset = j5;
2537 vinfos[5].indices[0] = _ij5[0];
2538 vinfos[5].indices[1] = _ij5[1];
2539 vinfos[5].maxsolutions = _nj5;
2540 std::vector<int> vfree(0);
2541 solutions.AddSolution(vinfos,vfree);
2542 }
2543 }
2544 }
2545 
2546 }
2547 
2548 }
2549 
2550 } else
2551 {
2552 {
2553 IkReal j5array[1], cj5array[1], sj5array[1];
2554 bool j5valid[1]={false};
2555 _nj5 = 1;
2556 IkReal x220=cj3*cj3;
2557 IkReal x221=(cj3*new_r22);
2558 CheckValue<IkReal> x222=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x220*(new_r22*new_r22)))+x220)),-1);
2559 if(!x222.valid){
2560 continue;
2561 }
2562 CheckValue<IkReal> x223 = IKatan2WithCheck(IkReal((((new_r01*x221))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x221)))),IKFAST_ATAN2_MAGTHRESH);
2563 if(!x223.valid){
2564 continue;
2565 }
2566 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x222.value)))+(x223.value));
2567 sj5array[0]=IKsin(j5array[0]);
2568 cj5array[0]=IKcos(j5array[0]);
2569 if( j5array[0] > IKPI )
2570 {
2571  j5array[0]-=IK2PI;
2572 }
2573 else if( j5array[0] < -IKPI )
2574 { j5array[0]+=IK2PI;
2575 }
2576 j5valid[0] = true;
2577 for(int ij5 = 0; ij5 < 1; ++ij5)
2578 {
2579 if( !j5valid[ij5] )
2580 {
2581  continue;
2582 }
2583 _ij5[0] = ij5; _ij5[1] = -1;
2584 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2585 {
2586 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2587 {
2588  j5valid[iij5]=false; _ij5[1] = iij5; break;
2589 }
2590 }
2591 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2592 {
2593 IkReal evalcond[10];
2594 IkReal x224=IKsin(j5);
2595 IkReal x225=IKcos(j5);
2596 IkReal x226=((1.0)*cj3);
2597 IkReal x227=(cj3*new_r00);
2598 IkReal x228=(cj3*new_r22);
2599 IkReal x229=(new_r22*sj3);
2600 IkReal x230=(new_r22*x225);
2601 IkReal x231=(sj3*x224);
2602 evalcond[0]=((((-1.0)*new_r10*x226))+((new_r00*sj3))+x224);
2603 evalcond[1]=((((-1.0)*new_r11*x226))+((new_r01*sj3))+x225);
2604 evalcond[2]=(((new_r11*sj3))+((new_r22*x224))+((cj3*new_r01)));
2605 evalcond[3]=(((new_r01*x228))+x224+((new_r11*x229)));
2606 evalcond[4]=(((x224*x228))+((sj3*x225))+new_r01);
2607 evalcond[5]=(((new_r10*sj3))+x227+(((-1.0)*x230)));
2608 evalcond[6]=(x231+(((-1.0)*x226*x230))+new_r00);
2609 evalcond[7]=(((x224*x229))+(((-1.0)*x225*x226))+new_r11);
2610 evalcond[8]=(((new_r22*x227))+((new_r10*x229))+(((-1.0)*x225)));
2611 evalcond[9]=((((-1.0)*x224*x226))+(((-1.0)*x225*x229))+new_r10);
2612 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2613 {
2614 continue;
2615 }
2616 }
2617 
2618 {
2619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2620 vinfos[0].jointtype = 1;
2621 vinfos[0].foffset = j0;
2622 vinfos[0].indices[0] = _ij0[0];
2623 vinfos[0].indices[1] = _ij0[1];
2624 vinfos[0].maxsolutions = _nj0;
2625 vinfos[1].jointtype = 1;
2626 vinfos[1].foffset = j1;
2627 vinfos[1].indices[0] = _ij1[0];
2628 vinfos[1].indices[1] = _ij1[1];
2629 vinfos[1].maxsolutions = _nj1;
2630 vinfos[2].jointtype = 1;
2631 vinfos[2].foffset = j2;
2632 vinfos[2].indices[0] = _ij2[0];
2633 vinfos[2].indices[1] = _ij2[1];
2634 vinfos[2].maxsolutions = _nj2;
2635 vinfos[3].jointtype = 1;
2636 vinfos[3].foffset = j3;
2637 vinfos[3].indices[0] = _ij3[0];
2638 vinfos[3].indices[1] = _ij3[1];
2639 vinfos[3].maxsolutions = _nj3;
2640 vinfos[4].jointtype = 1;
2641 vinfos[4].foffset = j4;
2642 vinfos[4].indices[0] = _ij4[0];
2643 vinfos[4].indices[1] = _ij4[1];
2644 vinfos[4].maxsolutions = _nj4;
2645 vinfos[5].jointtype = 1;
2646 vinfos[5].foffset = j5;
2647 vinfos[5].indices[0] = _ij5[0];
2648 vinfos[5].indices[1] = _ij5[1];
2649 vinfos[5].maxsolutions = _nj5;
2650 std::vector<int> vfree(0);
2651 solutions.AddSolution(vinfos,vfree);
2652 }
2653 }
2654 }
2655 
2656 }
2657 
2658 }
2659  }
2660 
2661 }
2662 
2663 }
2664 
2665 }
2666 } while(0);
2667 if( bgotonextstatement )
2668 {
2669 bool bgotonextstatement = true;
2670 do
2671 {
2672 if( 1 )
2673 {
2674 bgotonextstatement=false;
2675 continue; // branch miss [j3, j5]
2676 
2677 }
2678 } while(0);
2679 if( bgotonextstatement )
2680 {
2681 }
2682 }
2683 }
2684 }
2685 }
2686 
2687 } else
2688 {
2689 {
2690 IkReal j3array[1], cj3array[1], sj3array[1];
2691 bool j3valid[1]={false};
2692 _nj3 = 1;
2693 CheckValue<IkReal> x233=IKPowWithIntegerCheck(sj4,-1);
2694 if(!x233.valid){
2695 continue;
2696 }
2697 IkReal x232=x233.value;
2698 CheckValue<IkReal> x234=IKPowWithIntegerCheck(new_r12,-1);
2699 if(!x234.valid){
2700 continue;
2701 }
2702 if( IKabs((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x232)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x232))-1) <= IKFAST_SINCOS_THRESH )
2703  continue;
2704 j3array[0]=IKatan2((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x232));
2705 sj3array[0]=IKsin(j3array[0]);
2706 cj3array[0]=IKcos(j3array[0]);
2707 if( j3array[0] > IKPI )
2708 {
2709  j3array[0]-=IK2PI;
2710 }
2711 else if( j3array[0] < -IKPI )
2712 { j3array[0]+=IK2PI;
2713 }
2714 j3valid[0] = true;
2715 for(int ij3 = 0; ij3 < 1; ++ij3)
2716 {
2717 if( !j3valid[ij3] )
2718 {
2719  continue;
2720 }
2721 _ij3[0] = ij3; _ij3[1] = -1;
2722 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2723 {
2724 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2725 {
2726  j3valid[iij3]=false; _ij3[1] = iij3; break;
2727 }
2728 }
2729 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2730 {
2731 IkReal evalcond[8];
2732 IkReal x235=IKcos(j3);
2733 IkReal x236=IKsin(j3);
2734 IkReal x237=((1.0)*cj4);
2735 IkReal x238=(new_r02*x235);
2736 IkReal x239=(sj4*x236);
2737 IkReal x240=(sj4*x235);
2738 IkReal x241=(new_r12*x236);
2739 evalcond[0]=(x240+new_r02);
2740 evalcond[1]=(x239+new_r12);
2741 evalcond[2]=((((-1.0)*new_r02*x236))+((new_r12*x235)));
2742 evalcond[3]=(sj4+x238+x241);
2743 evalcond[4]=(((cj4*x241))+((new_r22*sj4))+((cj4*x238)));
2744 evalcond[5]=(((new_r10*x239))+(((-1.0)*new_r20*x237))+((new_r00*x240)));
2745 evalcond[6]=((((-1.0)*new_r21*x237))+((new_r11*x239))+((new_r01*x240)));
2746 evalcond[7]=((1.0)+(((-1.0)*new_r22*x237))+((new_r12*x239))+((sj4*x238)));
2747 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2748 {
2749 continue;
2750 }
2751 }
2752 
2753 {
2754 IkReal j5eval[3];
2755 j5eval[0]=sj4;
2756 j5eval[1]=IKsign(sj4);
2757 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2758 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2759 {
2760 {
2761 IkReal j5eval[2];
2762 j5eval[0]=sj4;
2763 j5eval[1]=sj3;
2764 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2765 {
2766 {
2767 IkReal j5eval[2];
2768 j5eval[0]=sj4;
2769 j5eval[1]=cj3;
2770 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2771 {
2772 {
2773 IkReal evalcond[5];
2774 bool bgotonextstatement = true;
2775 do
2776 {
2777 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2778 evalcond[1]=new_r02;
2779 evalcond[2]=new_r12;
2780 evalcond[3]=new_r21;
2781 evalcond[4]=new_r20;
2782 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
2783 {
2784 bgotonextstatement=false;
2785 {
2786 IkReal j5array[1], cj5array[1], sj5array[1];
2787 bool j5valid[1]={false};
2788 _nj5 = 1;
2789 IkReal x242=((1.0)*new_r01);
2790 if( IKabs(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x242))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x242))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2791  continue;
2792 j5array[0]=IKatan2(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x242))+((cj3*new_r00))));
2793 sj5array[0]=IKsin(j5array[0]);
2794 cj5array[0]=IKcos(j5array[0]);
2795 if( j5array[0] > IKPI )
2796 {
2797  j5array[0]-=IK2PI;
2798 }
2799 else if( j5array[0] < -IKPI )
2800 { j5array[0]+=IK2PI;
2801 }
2802 j5valid[0] = true;
2803 for(int ij5 = 0; ij5 < 1; ++ij5)
2804 {
2805 if( !j5valid[ij5] )
2806 {
2807  continue;
2808 }
2809 _ij5[0] = ij5; _ij5[1] = -1;
2810 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2811 {
2812 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2813 {
2814  j5valid[iij5]=false; _ij5[1] = iij5; break;
2815 }
2816 }
2817 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2818 {
2819 IkReal evalcond[8];
2820 IkReal x243=IKsin(j5);
2821 IkReal x244=IKcos(j5);
2822 IkReal x245=((1.0)*cj3);
2823 IkReal x246=(sj3*x243);
2824 IkReal x247=((1.0)*x244);
2825 IkReal x248=(x244*x245);
2826 evalcond[0]=(((new_r11*sj3))+x243+((cj3*new_r01)));
2827 evalcond[1]=(((new_r00*sj3))+x243+(((-1.0)*new_r10*x245)));
2828 evalcond[2]=(((new_r01*sj3))+x244+(((-1.0)*new_r11*x245)));
2829 evalcond[3]=(((sj3*x244))+new_r01+((cj3*x243)));
2830 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x247)));
2831 evalcond[5]=(x246+(((-1.0)*x248))+new_r00);
2832 evalcond[6]=(x246+(((-1.0)*x248))+new_r11);
2833 evalcond[7]=((((-1.0)*sj3*x247))+(((-1.0)*x243*x245))+new_r10);
2834 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2835 {
2836 continue;
2837 }
2838 }
2839 
2840 {
2841 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2842 vinfos[0].jointtype = 1;
2843 vinfos[0].foffset = j0;
2844 vinfos[0].indices[0] = _ij0[0];
2845 vinfos[0].indices[1] = _ij0[1];
2846 vinfos[0].maxsolutions = _nj0;
2847 vinfos[1].jointtype = 1;
2848 vinfos[1].foffset = j1;
2849 vinfos[1].indices[0] = _ij1[0];
2850 vinfos[1].indices[1] = _ij1[1];
2851 vinfos[1].maxsolutions = _nj1;
2852 vinfos[2].jointtype = 1;
2853 vinfos[2].foffset = j2;
2854 vinfos[2].indices[0] = _ij2[0];
2855 vinfos[2].indices[1] = _ij2[1];
2856 vinfos[2].maxsolutions = _nj2;
2857 vinfos[3].jointtype = 1;
2858 vinfos[3].foffset = j3;
2859 vinfos[3].indices[0] = _ij3[0];
2860 vinfos[3].indices[1] = _ij3[1];
2861 vinfos[3].maxsolutions = _nj3;
2862 vinfos[4].jointtype = 1;
2863 vinfos[4].foffset = j4;
2864 vinfos[4].indices[0] = _ij4[0];
2865 vinfos[4].indices[1] = _ij4[1];
2866 vinfos[4].maxsolutions = _nj4;
2867 vinfos[5].jointtype = 1;
2868 vinfos[5].foffset = j5;
2869 vinfos[5].indices[0] = _ij5[0];
2870 vinfos[5].indices[1] = _ij5[1];
2871 vinfos[5].maxsolutions = _nj5;
2872 std::vector<int> vfree(0);
2873 solutions.AddSolution(vinfos,vfree);
2874 }
2875 }
2876 }
2877 
2878 }
2879 } while(0);
2880 if( bgotonextstatement )
2881 {
2882 bool bgotonextstatement = true;
2883 do
2884 {
2885 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2886 evalcond[1]=new_r02;
2887 evalcond[2]=new_r12;
2888 evalcond[3]=new_r21;
2889 evalcond[4]=new_r20;
2890 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
2891 {
2892 bgotonextstatement=false;
2893 {
2894 IkReal j5array[1], cj5array[1], sj5array[1];
2895 bool j5valid[1]={false};
2896 _nj5 = 1;
2897 IkReal x249=((1.0)*sj3);
2898 if( IKabs(((((-1.0)*new_r00*x249))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x249))+((cj3*new_r01))))+IKsqr(((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2899  continue;
2900 j5array[0]=IKatan2(((((-1.0)*new_r00*x249))+((cj3*new_r01))), ((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00))));
2901 sj5array[0]=IKsin(j5array[0]);
2902 cj5array[0]=IKcos(j5array[0]);
2903 if( j5array[0] > IKPI )
2904 {
2905  j5array[0]-=IK2PI;
2906 }
2907 else if( j5array[0] < -IKPI )
2908 { j5array[0]+=IK2PI;
2909 }
2910 j5valid[0] = true;
2911 for(int ij5 = 0; ij5 < 1; ++ij5)
2912 {
2913 if( !j5valid[ij5] )
2914 {
2915  continue;
2916 }
2917 _ij5[0] = ij5; _ij5[1] = -1;
2918 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2919 {
2920 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2921 {
2922  j5valid[iij5]=false; _ij5[1] = iij5; break;
2923 }
2924 }
2925 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2926 {
2927 IkReal evalcond[8];
2928 IkReal x250=IKcos(j5);
2929 IkReal x251=IKsin(j5);
2930 IkReal x252=((1.0)*cj3);
2931 IkReal x253=(sj3*x250);
2932 IkReal x254=((1.0)*x251);
2933 IkReal x255=(x251*x252);
2934 evalcond[0]=(((new_r10*sj3))+x250+((cj3*new_r00)));
2935 evalcond[1]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x251);
2936 evalcond[2]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x250);
2937 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x254)));
2938 evalcond[4]=(((cj3*x250))+((sj3*x251))+new_r00);
2939 evalcond[5]=(x253+new_r01+(((-1.0)*x255)));
2940 evalcond[6]=(x253+new_r10+(((-1.0)*x255)));
2941 evalcond[7]=((((-1.0)*x250*x252))+(((-1.0)*sj3*x254))+new_r11);
2942 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2943 {
2944 continue;
2945 }
2946 }
2947 
2948 {
2949 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2950 vinfos[0].jointtype = 1;
2951 vinfos[0].foffset = j0;
2952 vinfos[0].indices[0] = _ij0[0];
2953 vinfos[0].indices[1] = _ij0[1];
2954 vinfos[0].maxsolutions = _nj0;
2955 vinfos[1].jointtype = 1;
2956 vinfos[1].foffset = j1;
2957 vinfos[1].indices[0] = _ij1[0];
2958 vinfos[1].indices[1] = _ij1[1];
2959 vinfos[1].maxsolutions = _nj1;
2960 vinfos[2].jointtype = 1;
2961 vinfos[2].foffset = j2;
2962 vinfos[2].indices[0] = _ij2[0];
2963 vinfos[2].indices[1] = _ij2[1];
2964 vinfos[2].maxsolutions = _nj2;
2965 vinfos[3].jointtype = 1;
2966 vinfos[3].foffset = j3;
2967 vinfos[3].indices[0] = _ij3[0];
2968 vinfos[3].indices[1] = _ij3[1];
2969 vinfos[3].maxsolutions = _nj3;
2970 vinfos[4].jointtype = 1;
2971 vinfos[4].foffset = j4;
2972 vinfos[4].indices[0] = _ij4[0];
2973 vinfos[4].indices[1] = _ij4[1];
2974 vinfos[4].maxsolutions = _nj4;
2975 vinfos[5].jointtype = 1;
2976 vinfos[5].foffset = j5;
2977 vinfos[5].indices[0] = _ij5[0];
2978 vinfos[5].indices[1] = _ij5[1];
2979 vinfos[5].maxsolutions = _nj5;
2980 std::vector<int> vfree(0);
2981 solutions.AddSolution(vinfos,vfree);
2982 }
2983 }
2984 }
2985 
2986 }
2987 } while(0);
2988 if( bgotonextstatement )
2989 {
2990 bool bgotonextstatement = true;
2991 do
2992 {
2993 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
2994 evalcond[1]=new_r02;
2995 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
2996 {
2997 bgotonextstatement=false;
2998 {
2999 IkReal j5array[1], cj5array[1], sj5array[1];
3000 bool j5valid[1]={false};
3001 _nj5 = 1;
3002 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
3003  continue;
3004 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3005 sj5array[0]=IKsin(j5array[0]);
3006 cj5array[0]=IKcos(j5array[0]);
3007 if( j5array[0] > IKPI )
3008 {
3009  j5array[0]-=IK2PI;
3010 }
3011 else if( j5array[0] < -IKPI )
3012 { j5array[0]+=IK2PI;
3013 }
3014 j5valid[0] = true;
3015 for(int ij5 = 0; ij5 < 1; ++ij5)
3016 {
3017 if( !j5valid[ij5] )
3018 {
3019  continue;
3020 }
3021 _ij5[0] = ij5; _ij5[1] = -1;
3022 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3023 {
3024 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3025 {
3026  j5valid[iij5]=false; _ij5[1] = iij5; break;
3027 }
3028 }
3029 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3030 {
3031 IkReal evalcond[8];
3032 IkReal x256=IKsin(j5);
3033 IkReal x257=IKcos(j5);
3034 IkReal x258=((1.0)*x257);
3035 evalcond[0]=(x256+new_r00);
3036 evalcond[1]=(x257+new_r01);
3037 evalcond[2]=(((sj4*x256))+new_r21);
3038 evalcond[3]=(new_r11+((cj4*x256)));
3039 evalcond[4]=((((-1.0)*sj4*x258))+new_r20);
3040 evalcond[5]=((((-1.0)*cj4*x258))+new_r10);
3041 evalcond[6]=(((cj4*new_r11))+x256+((new_r21*sj4)));
3042 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x258)));
3043 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3044 {
3045 continue;
3046 }
3047 }
3048 
3049 {
3050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3051 vinfos[0].jointtype = 1;
3052 vinfos[0].foffset = j0;
3053 vinfos[0].indices[0] = _ij0[0];
3054 vinfos[0].indices[1] = _ij0[1];
3055 vinfos[0].maxsolutions = _nj0;
3056 vinfos[1].jointtype = 1;
3057 vinfos[1].foffset = j1;
3058 vinfos[1].indices[0] = _ij1[0];
3059 vinfos[1].indices[1] = _ij1[1];
3060 vinfos[1].maxsolutions = _nj1;
3061 vinfos[2].jointtype = 1;
3062 vinfos[2].foffset = j2;
3063 vinfos[2].indices[0] = _ij2[0];
3064 vinfos[2].indices[1] = _ij2[1];
3065 vinfos[2].maxsolutions = _nj2;
3066 vinfos[3].jointtype = 1;
3067 vinfos[3].foffset = j3;
3068 vinfos[3].indices[0] = _ij3[0];
3069 vinfos[3].indices[1] = _ij3[1];
3070 vinfos[3].maxsolutions = _nj3;
3071 vinfos[4].jointtype = 1;
3072 vinfos[4].foffset = j4;
3073 vinfos[4].indices[0] = _ij4[0];
3074 vinfos[4].indices[1] = _ij4[1];
3075 vinfos[4].maxsolutions = _nj4;
3076 vinfos[5].jointtype = 1;
3077 vinfos[5].foffset = j5;
3078 vinfos[5].indices[0] = _ij5[0];
3079 vinfos[5].indices[1] = _ij5[1];
3080 vinfos[5].maxsolutions = _nj5;
3081 std::vector<int> vfree(0);
3082 solutions.AddSolution(vinfos,vfree);
3083 }
3084 }
3085 }
3086 
3087 }
3088 } while(0);
3089 if( bgotonextstatement )
3090 {
3091 bool bgotonextstatement = true;
3092 do
3093 {
3094 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3095 evalcond[1]=new_r02;
3096 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3097 {
3098 bgotonextstatement=false;
3099 {
3100 IkReal j5array[1], cj5array[1], sj5array[1];
3101 bool j5valid[1]={false};
3102 _nj5 = 1;
3103 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3104  continue;
3105 j5array[0]=IKatan2(new_r00, new_r01);
3106 sj5array[0]=IKsin(j5array[0]);
3107 cj5array[0]=IKcos(j5array[0]);
3108 if( j5array[0] > IKPI )
3109 {
3110  j5array[0]-=IK2PI;
3111 }
3112 else if( j5array[0] < -IKPI )
3113 { j5array[0]+=IK2PI;
3114 }
3115 j5valid[0] = true;
3116 for(int ij5 = 0; ij5 < 1; ++ij5)
3117 {
3118 if( !j5valid[ij5] )
3119 {
3120  continue;
3121 }
3122 _ij5[0] = ij5; _ij5[1] = -1;
3123 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3124 {
3125 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3126 {
3127  j5valid[iij5]=false; _ij5[1] = iij5; break;
3128 }
3129 }
3130 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3131 {
3132 IkReal evalcond[8];
3133 IkReal x259=IKsin(j5);
3134 IkReal x260=IKcos(j5);
3135 IkReal x261=((1.0)*cj4);
3136 IkReal x262=((1.0)*x260);
3137 evalcond[0]=(((sj4*x259))+new_r21);
3138 evalcond[1]=(x259+(((-1.0)*new_r00)));
3139 evalcond[2]=(x260+(((-1.0)*new_r01)));
3140 evalcond[3]=((((-1.0)*sj4*x262))+new_r20);
3141 evalcond[4]=((((-1.0)*new_r11))+((cj4*x259)));
3142 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x260*x261)));
3143 evalcond[6]=(x259+(((-1.0)*new_r11*x261))+((new_r21*sj4)));
3144 evalcond[7]=(((new_r20*sj4))+(((-1.0)*new_r10*x261))+(((-1.0)*x262)));
3145 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3146 {
3147 continue;
3148 }
3149 }
3150 
3151 {
3152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3153 vinfos[0].jointtype = 1;
3154 vinfos[0].foffset = j0;
3155 vinfos[0].indices[0] = _ij0[0];
3156 vinfos[0].indices[1] = _ij0[1];
3157 vinfos[0].maxsolutions = _nj0;
3158 vinfos[1].jointtype = 1;
3159 vinfos[1].foffset = j1;
3160 vinfos[1].indices[0] = _ij1[0];
3161 vinfos[1].indices[1] = _ij1[1];
3162 vinfos[1].maxsolutions = _nj1;
3163 vinfos[2].jointtype = 1;
3164 vinfos[2].foffset = j2;
3165 vinfos[2].indices[0] = _ij2[0];
3166 vinfos[2].indices[1] = _ij2[1];
3167 vinfos[2].maxsolutions = _nj2;
3168 vinfos[3].jointtype = 1;
3169 vinfos[3].foffset = j3;
3170 vinfos[3].indices[0] = _ij3[0];
3171 vinfos[3].indices[1] = _ij3[1];
3172 vinfos[3].maxsolutions = _nj3;
3173 vinfos[4].jointtype = 1;
3174 vinfos[4].foffset = j4;
3175 vinfos[4].indices[0] = _ij4[0];
3176 vinfos[4].indices[1] = _ij4[1];
3177 vinfos[4].maxsolutions = _nj4;
3178 vinfos[5].jointtype = 1;
3179 vinfos[5].foffset = j5;
3180 vinfos[5].indices[0] = _ij5[0];
3181 vinfos[5].indices[1] = _ij5[1];
3182 vinfos[5].maxsolutions = _nj5;
3183 std::vector<int> vfree(0);
3184 solutions.AddSolution(vinfos,vfree);
3185 }
3186 }
3187 }
3188 
3189 }
3190 } while(0);
3191 if( bgotonextstatement )
3192 {
3193 bool bgotonextstatement = true;
3194 do
3195 {
3196 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3197 evalcond[1]=new_r12;
3198 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3199 {
3200 bgotonextstatement=false;
3201 {
3202 IkReal j5array[1], cj5array[1], sj5array[1];
3203 bool j5valid[1]={false};
3204 _nj5 = 1;
3205 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3206  continue;
3207 j5array[0]=IKatan2(new_r10, new_r11);
3208 sj5array[0]=IKsin(j5array[0]);
3209 cj5array[0]=IKcos(j5array[0]);
3210 if( j5array[0] > IKPI )
3211 {
3212  j5array[0]-=IK2PI;
3213 }
3214 else if( j5array[0] < -IKPI )
3215 { j5array[0]+=IK2PI;
3216 }
3217 j5valid[0] = true;
3218 for(int ij5 = 0; ij5 < 1; ++ij5)
3219 {
3220 if( !j5valid[ij5] )
3221 {
3222  continue;
3223 }
3224 _ij5[0] = ij5; _ij5[1] = -1;
3225 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3226 {
3227 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3228 {
3229  j5valid[iij5]=false; _ij5[1] = iij5; break;
3230 }
3231 }
3232 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3233 {
3234 IkReal evalcond[8];
3235 IkReal x263=IKcos(j5);
3236 IkReal x264=IKsin(j5);
3237 IkReal x265=((1.0)*x263);
3238 evalcond[0]=(new_r20+((new_r02*x263)));
3239 evalcond[1]=(x264+(((-1.0)*new_r10)));
3240 evalcond[2]=(x263+(((-1.0)*new_r11)));
3241 evalcond[3]=(((cj4*x264))+new_r01);
3242 evalcond[4]=((((-1.0)*new_r02*x264))+new_r21);
3243 evalcond[5]=((((-1.0)*cj4*x265))+new_r00);
3244 evalcond[6]=(((cj4*new_r01))+x264+((new_r21*sj4)));
3245 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x265)));
3246 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3247 {
3248 continue;
3249 }
3250 }
3251 
3252 {
3253 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3254 vinfos[0].jointtype = 1;
3255 vinfos[0].foffset = j0;
3256 vinfos[0].indices[0] = _ij0[0];
3257 vinfos[0].indices[1] = _ij0[1];
3258 vinfos[0].maxsolutions = _nj0;
3259 vinfos[1].jointtype = 1;
3260 vinfos[1].foffset = j1;
3261 vinfos[1].indices[0] = _ij1[0];
3262 vinfos[1].indices[1] = _ij1[1];
3263 vinfos[1].maxsolutions = _nj1;
3264 vinfos[2].jointtype = 1;
3265 vinfos[2].foffset = j2;
3266 vinfos[2].indices[0] = _ij2[0];
3267 vinfos[2].indices[1] = _ij2[1];
3268 vinfos[2].maxsolutions = _nj2;
3269 vinfos[3].jointtype = 1;
3270 vinfos[3].foffset = j3;
3271 vinfos[3].indices[0] = _ij3[0];
3272 vinfos[3].indices[1] = _ij3[1];
3273 vinfos[3].maxsolutions = _nj3;
3274 vinfos[4].jointtype = 1;
3275 vinfos[4].foffset = j4;
3276 vinfos[4].indices[0] = _ij4[0];
3277 vinfos[4].indices[1] = _ij4[1];
3278 vinfos[4].maxsolutions = _nj4;
3279 vinfos[5].jointtype = 1;
3280 vinfos[5].foffset = j5;
3281 vinfos[5].indices[0] = _ij5[0];
3282 vinfos[5].indices[1] = _ij5[1];
3283 vinfos[5].maxsolutions = _nj5;
3284 std::vector<int> vfree(0);
3285 solutions.AddSolution(vinfos,vfree);
3286 }
3287 }
3288 }
3289 
3290 }
3291 } while(0);
3292 if( bgotonextstatement )
3293 {
3294 bool bgotonextstatement = true;
3295 do
3296 {
3297 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3298 evalcond[1]=new_r12;
3299 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3300 {
3301 bgotonextstatement=false;
3302 {
3303 IkReal j5array[1], cj5array[1], sj5array[1];
3304 bool j5valid[1]={false};
3305 _nj5 = 1;
3306 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
3307  continue;
3308 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3309 sj5array[0]=IKsin(j5array[0]);
3310 cj5array[0]=IKcos(j5array[0]);
3311 if( j5array[0] > IKPI )
3312 {
3313  j5array[0]-=IK2PI;
3314 }
3315 else if( j5array[0] < -IKPI )
3316 { j5array[0]+=IK2PI;
3317 }
3318 j5valid[0] = true;
3319 for(int ij5 = 0; ij5 < 1; ++ij5)
3320 {
3321 if( !j5valid[ij5] )
3322 {
3323  continue;
3324 }
3325 _ij5[0] = ij5; _ij5[1] = -1;
3326 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3327 {
3328 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3329 {
3330  j5valid[iij5]=false; _ij5[1] = iij5; break;
3331 }
3332 }
3333 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3334 {
3335 IkReal evalcond[8];
3336 IkReal x266=IKsin(j5);
3337 IkReal x267=IKcos(j5);
3338 IkReal x268=((1.0)*new_r00);
3339 IkReal x269=((1.0)*new_r01);
3340 IkReal x270=((1.0)*x267);
3341 evalcond[0]=(x266+new_r10);
3342 evalcond[1]=(x267+new_r11);
3343 evalcond[2]=(new_r21+((new_r02*x266)));
3344 evalcond[3]=((((-1.0)*new_r02*x270))+new_r20);
3345 evalcond[4]=(((cj4*x266))+(((-1.0)*x269)));
3346 evalcond[5]=((((-1.0)*cj4*x270))+(((-1.0)*x268)));
3347 evalcond[6]=((((-1.0)*cj4*x269))+x266+((new_r21*sj4)));
3348 evalcond[7]=((((-1.0)*cj4*x268))+((new_r20*sj4))+(((-1.0)*x270)));
3349 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3350 {
3351 continue;
3352 }
3353 }
3354 
3355 {
3356 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3357 vinfos[0].jointtype = 1;
3358 vinfos[0].foffset = j0;
3359 vinfos[0].indices[0] = _ij0[0];
3360 vinfos[0].indices[1] = _ij0[1];
3361 vinfos[0].maxsolutions = _nj0;
3362 vinfos[1].jointtype = 1;
3363 vinfos[1].foffset = j1;
3364 vinfos[1].indices[0] = _ij1[0];
3365 vinfos[1].indices[1] = _ij1[1];
3366 vinfos[1].maxsolutions = _nj1;
3367 vinfos[2].jointtype = 1;
3368 vinfos[2].foffset = j2;
3369 vinfos[2].indices[0] = _ij2[0];
3370 vinfos[2].indices[1] = _ij2[1];
3371 vinfos[2].maxsolutions = _nj2;
3372 vinfos[3].jointtype = 1;
3373 vinfos[3].foffset = j3;
3374 vinfos[3].indices[0] = _ij3[0];
3375 vinfos[3].indices[1] = _ij3[1];
3376 vinfos[3].maxsolutions = _nj3;
3377 vinfos[4].jointtype = 1;
3378 vinfos[4].foffset = j4;
3379 vinfos[4].indices[0] = _ij4[0];
3380 vinfos[4].indices[1] = _ij4[1];
3381 vinfos[4].maxsolutions = _nj4;
3382 vinfos[5].jointtype = 1;
3383 vinfos[5].foffset = j5;
3384 vinfos[5].indices[0] = _ij5[0];
3385 vinfos[5].indices[1] = _ij5[1];
3386 vinfos[5].maxsolutions = _nj5;
3387 std::vector<int> vfree(0);
3388 solutions.AddSolution(vinfos,vfree);
3389 }
3390 }
3391 }
3392 
3393 }
3394 } while(0);
3395 if( bgotonextstatement )
3396 {
3397 bool bgotonextstatement = true;
3398 do
3399 {
3400 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3401 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3402 {
3403 bgotonextstatement=false;
3404 {
3405 IkReal j5eval[1];
3406 new_r21=0;
3407 new_r20=0;
3408 new_r02=0;
3409 new_r12=0;
3410 j5eval[0]=1.0;
3411 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3412 {
3413 continue; // no branches [j5]
3414 
3415 } else
3416 {
3417 IkReal op[2+1], zeror[2];
3418 int numroots;
3419 op[0]=-1.0;
3420 op[1]=0;
3421 op[2]=1.0;
3422 polyroots2(op,zeror,numroots);
3423 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3424 int numsolutions = 0;
3425 for(int ij5 = 0; ij5 < numroots; ++ij5)
3426 {
3427 IkReal htj5 = zeror[ij5];
3428 tempj5array[0]=((2.0)*(atan(htj5)));
3429 for(int kj5 = 0; kj5 < 1; ++kj5)
3430 {
3431 j5array[numsolutions] = tempj5array[kj5];
3432 if( j5array[numsolutions] > IKPI )
3433 {
3434  j5array[numsolutions]-=IK2PI;
3435 }
3436 else if( j5array[numsolutions] < -IKPI )
3437 {
3438  j5array[numsolutions]+=IK2PI;
3439 }
3440 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3441 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3442 numsolutions++;
3443 }
3444 }
3445 bool j5valid[2]={true,true};
3446 _nj5 = 2;
3447 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3448  {
3449 if( !j5valid[ij5] )
3450 {
3451  continue;
3452 }
3453  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3454 htj5 = IKtan(j5/2);
3455 
3456 _ij5[0] = ij5; _ij5[1] = -1;
3457 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3458 {
3459 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3460 {
3461  j5valid[iij5]=false; _ij5[1] = iij5; break;
3462 }
3463 }
3464 {
3465 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3466 vinfos[0].jointtype = 1;
3467 vinfos[0].foffset = j0;
3468 vinfos[0].indices[0] = _ij0[0];
3469 vinfos[0].indices[1] = _ij0[1];
3470 vinfos[0].maxsolutions = _nj0;
3471 vinfos[1].jointtype = 1;
3472 vinfos[1].foffset = j1;
3473 vinfos[1].indices[0] = _ij1[0];
3474 vinfos[1].indices[1] = _ij1[1];
3475 vinfos[1].maxsolutions = _nj1;
3476 vinfos[2].jointtype = 1;
3477 vinfos[2].foffset = j2;
3478 vinfos[2].indices[0] = _ij2[0];
3479 vinfos[2].indices[1] = _ij2[1];
3480 vinfos[2].maxsolutions = _nj2;
3481 vinfos[3].jointtype = 1;
3482 vinfos[3].foffset = j3;
3483 vinfos[3].indices[0] = _ij3[0];
3484 vinfos[3].indices[1] = _ij3[1];
3485 vinfos[3].maxsolutions = _nj3;
3486 vinfos[4].jointtype = 1;
3487 vinfos[4].foffset = j4;
3488 vinfos[4].indices[0] = _ij4[0];
3489 vinfos[4].indices[1] = _ij4[1];
3490 vinfos[4].maxsolutions = _nj4;
3491 vinfos[5].jointtype = 1;
3492 vinfos[5].foffset = j5;
3493 vinfos[5].indices[0] = _ij5[0];
3494 vinfos[5].indices[1] = _ij5[1];
3495 vinfos[5].maxsolutions = _nj5;
3496 std::vector<int> vfree(0);
3497 solutions.AddSolution(vinfos,vfree);
3498 }
3499  }
3500 
3501 }
3502 
3503 }
3504 
3505 }
3506 } while(0);
3507 if( bgotonextstatement )
3508 {
3509 bool bgotonextstatement = true;
3510 do
3511 {
3512 if( 1 )
3513 {
3514 bgotonextstatement=false;
3515 continue; // branch miss [j5]
3516 
3517 }
3518 } while(0);
3519 if( bgotonextstatement )
3520 {
3521 }
3522 }
3523 }
3524 }
3525 }
3526 }
3527 }
3528 }
3529 }
3530 
3531 } else
3532 {
3533 {
3534 IkReal j5array[1], cj5array[1], sj5array[1];
3535 bool j5valid[1]={false};
3536 _nj5 = 1;
3537 CheckValue<IkReal> x272=IKPowWithIntegerCheck(sj4,-1);
3538 if(!x272.valid){
3539 continue;
3540 }
3541 IkReal x271=x272.value;
3542 CheckValue<IkReal> x273=IKPowWithIntegerCheck(cj3,-1);
3543 if(!x273.valid){
3544 continue;
3545 }
3546 if( IKabs(((-1.0)*new_r21*x271)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x271))+IKsqr((x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3547  continue;
3548 j5array[0]=IKatan2(((-1.0)*new_r21*x271), (x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3549 sj5array[0]=IKsin(j5array[0]);
3550 cj5array[0]=IKcos(j5array[0]);
3551 if( j5array[0] > IKPI )
3552 {
3553  j5array[0]-=IK2PI;
3554 }
3555 else if( j5array[0] < -IKPI )
3556 { j5array[0]+=IK2PI;
3557 }
3558 j5valid[0] = true;
3559 for(int ij5 = 0; ij5 < 1; ++ij5)
3560 {
3561 if( !j5valid[ij5] )
3562 {
3563  continue;
3564 }
3565 _ij5[0] = ij5; _ij5[1] = -1;
3566 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3567 {
3568 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3569 {
3570  j5valid[iij5]=false; _ij5[1] = iij5; break;
3571 }
3572 }
3573 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3574 {
3575 IkReal evalcond[12];
3576 IkReal x274=IKsin(j5);
3577 IkReal x275=IKcos(j5);
3578 IkReal x276=(cj3*new_r00);
3579 IkReal x277=(cj3*cj4);
3580 IkReal x278=(cj4*sj3);
3581 IkReal x279=((1.0)*cj3);
3582 IkReal x280=(sj3*x274);
3583 IkReal x281=((1.0)*x275);
3584 evalcond[0]=(new_r21+((sj4*x274)));
3585 evalcond[1]=((((-1.0)*sj4*x281))+new_r20);
3586 evalcond[2]=(((new_r00*sj3))+x274+(((-1.0)*new_r10*x279)));
3587 evalcond[3]=(((new_r01*sj3))+x275+(((-1.0)*new_r11*x279)));
3588 evalcond[4]=(((cj4*x274))+((new_r11*sj3))+((cj3*new_r01)));
3589 evalcond[5]=(((sj3*x275))+((x274*x277))+new_r01);
3590 evalcond[6]=(((new_r10*sj3))+x276+(((-1.0)*cj4*x281)));
3591 evalcond[7]=((((-1.0)*x277*x281))+x280+new_r00);
3592 evalcond[8]=(((x274*x278))+(((-1.0)*x275*x279))+new_r11);
3593 evalcond[9]=((((-1.0)*x274*x279))+new_r10+(((-1.0)*x278*x281)));
3594 evalcond[10]=(x274+((new_r01*x277))+((new_r21*sj4))+((new_r11*x278)));
3595 evalcond[11]=(((new_r20*sj4))+((cj4*x276))+(((-1.0)*x281))+((new_r10*x278)));
3596 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3597 {
3598 continue;
3599 }
3600 }
3601 
3602 {
3603 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3604 vinfos[0].jointtype = 1;
3605 vinfos[0].foffset = j0;
3606 vinfos[0].indices[0] = _ij0[0];
3607 vinfos[0].indices[1] = _ij0[1];
3608 vinfos[0].maxsolutions = _nj0;
3609 vinfos[1].jointtype = 1;
3610 vinfos[1].foffset = j1;
3611 vinfos[1].indices[0] = _ij1[0];
3612 vinfos[1].indices[1] = _ij1[1];
3613 vinfos[1].maxsolutions = _nj1;
3614 vinfos[2].jointtype = 1;
3615 vinfos[2].foffset = j2;
3616 vinfos[2].indices[0] = _ij2[0];
3617 vinfos[2].indices[1] = _ij2[1];
3618 vinfos[2].maxsolutions = _nj2;
3619 vinfos[3].jointtype = 1;
3620 vinfos[3].foffset = j3;
3621 vinfos[3].indices[0] = _ij3[0];
3622 vinfos[3].indices[1] = _ij3[1];
3623 vinfos[3].maxsolutions = _nj3;
3624 vinfos[4].jointtype = 1;
3625 vinfos[4].foffset = j4;
3626 vinfos[4].indices[0] = _ij4[0];
3627 vinfos[4].indices[1] = _ij4[1];
3628 vinfos[4].maxsolutions = _nj4;
3629 vinfos[5].jointtype = 1;
3630 vinfos[5].foffset = j5;
3631 vinfos[5].indices[0] = _ij5[0];
3632 vinfos[5].indices[1] = _ij5[1];
3633 vinfos[5].maxsolutions = _nj5;
3634 std::vector<int> vfree(0);
3635 solutions.AddSolution(vinfos,vfree);
3636 }
3637 }
3638 }
3639 
3640 }
3641 
3642 }
3643 
3644 } else
3645 {
3646 {
3647 IkReal j5array[1], cj5array[1], sj5array[1];
3648 bool j5valid[1]={false};
3649 _nj5 = 1;
3650 CheckValue<IkReal> x283=IKPowWithIntegerCheck(sj4,-1);
3651 if(!x283.valid){
3652 continue;
3653 }
3654 IkReal x282=x283.value;
3655 CheckValue<IkReal> x284=IKPowWithIntegerCheck(sj3,-1);
3656 if(!x284.valid){
3657 continue;
3658 }
3659 if( IKabs(((-1.0)*new_r21*x282)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x282))+IKsqr((x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3660  continue;
3661 j5array[0]=IKatan2(((-1.0)*new_r21*x282), (x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3662 sj5array[0]=IKsin(j5array[0]);
3663 cj5array[0]=IKcos(j5array[0]);
3664 if( j5array[0] > IKPI )
3665 {
3666  j5array[0]-=IK2PI;
3667 }
3668 else if( j5array[0] < -IKPI )
3669 { j5array[0]+=IK2PI;
3670 }
3671 j5valid[0] = true;
3672 for(int ij5 = 0; ij5 < 1; ++ij5)
3673 {
3674 if( !j5valid[ij5] )
3675 {
3676  continue;
3677 }
3678 _ij5[0] = ij5; _ij5[1] = -1;
3679 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3680 {
3681 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3682 {
3683  j5valid[iij5]=false; _ij5[1] = iij5; break;
3684 }
3685 }
3686 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3687 {
3688 IkReal evalcond[12];
3689 IkReal x285=IKsin(j5);
3690 IkReal x286=IKcos(j5);
3691 IkReal x287=(cj3*new_r00);
3692 IkReal x288=(cj3*cj4);
3693 IkReal x289=(cj4*sj3);
3694 IkReal x290=((1.0)*cj3);
3695 IkReal x291=(sj3*x285);
3696 IkReal x292=((1.0)*x286);
3697 evalcond[0]=(new_r21+((sj4*x285)));
3698 evalcond[1]=(new_r20+(((-1.0)*sj4*x292)));
3699 evalcond[2]=((((-1.0)*new_r10*x290))+((new_r00*sj3))+x285);
3700 evalcond[3]=((((-1.0)*new_r11*x290))+((new_r01*sj3))+x286);
3701 evalcond[4]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x285)));
3702 evalcond[5]=(((x285*x288))+((sj3*x286))+new_r01);
3703 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x292))+x287);
3704 evalcond[7]=(x291+(((-1.0)*x288*x292))+new_r00);
3705 evalcond[8]=(((x285*x289))+(((-1.0)*x286*x290))+new_r11);
3706 evalcond[9]=((((-1.0)*x285*x290))+(((-1.0)*x289*x292))+new_r10);
3707 evalcond[10]=(x285+((new_r21*sj4))+((new_r11*x289))+((new_r01*x288)));
3708 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x292))+((cj4*x287))+((new_r10*x289)));
3709 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3710 {
3711 continue;
3712 }
3713 }
3714 
3715 {
3716 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3717 vinfos[0].jointtype = 1;
3718 vinfos[0].foffset = j0;
3719 vinfos[0].indices[0] = _ij0[0];
3720 vinfos[0].indices[1] = _ij0[1];
3721 vinfos[0].maxsolutions = _nj0;
3722 vinfos[1].jointtype = 1;
3723 vinfos[1].foffset = j1;
3724 vinfos[1].indices[0] = _ij1[0];
3725 vinfos[1].indices[1] = _ij1[1];
3726 vinfos[1].maxsolutions = _nj1;
3727 vinfos[2].jointtype = 1;
3728 vinfos[2].foffset = j2;
3729 vinfos[2].indices[0] = _ij2[0];
3730 vinfos[2].indices[1] = _ij2[1];
3731 vinfos[2].maxsolutions = _nj2;
3732 vinfos[3].jointtype = 1;
3733 vinfos[3].foffset = j3;
3734 vinfos[3].indices[0] = _ij3[0];
3735 vinfos[3].indices[1] = _ij3[1];
3736 vinfos[3].maxsolutions = _nj3;
3737 vinfos[4].jointtype = 1;
3738 vinfos[4].foffset = j4;
3739 vinfos[4].indices[0] = _ij4[0];
3740 vinfos[4].indices[1] = _ij4[1];
3741 vinfos[4].maxsolutions = _nj4;
3742 vinfos[5].jointtype = 1;
3743 vinfos[5].foffset = j5;
3744 vinfos[5].indices[0] = _ij5[0];
3745 vinfos[5].indices[1] = _ij5[1];
3746 vinfos[5].maxsolutions = _nj5;
3747 std::vector<int> vfree(0);
3748 solutions.AddSolution(vinfos,vfree);
3749 }
3750 }
3751 }
3752 
3753 }
3754 
3755 }
3756 
3757 } else
3758 {
3759 {
3760 IkReal j5array[1], cj5array[1], sj5array[1];
3761 bool j5valid[1]={false};
3762 _nj5 = 1;
3763 CheckValue<IkReal> x293 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3764 if(!x293.valid){
3765 continue;
3766 }
3767 CheckValue<IkReal> x294=IKPowWithIntegerCheck(IKsign(sj4),-1);
3768 if(!x294.valid){
3769 continue;
3770 }
3771 j5array[0]=((-1.5707963267949)+(x293.value)+(((1.5707963267949)*(x294.value))));
3772 sj5array[0]=IKsin(j5array[0]);
3773 cj5array[0]=IKcos(j5array[0]);
3774 if( j5array[0] > IKPI )
3775 {
3776  j5array[0]-=IK2PI;
3777 }
3778 else if( j5array[0] < -IKPI )
3779 { j5array[0]+=IK2PI;
3780 }
3781 j5valid[0] = true;
3782 for(int ij5 = 0; ij5 < 1; ++ij5)
3783 {
3784 if( !j5valid[ij5] )
3785 {
3786  continue;
3787 }
3788 _ij5[0] = ij5; _ij5[1] = -1;
3789 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3790 {
3791 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3792 {
3793  j5valid[iij5]=false; _ij5[1] = iij5; break;
3794 }
3795 }
3796 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3797 {
3798 IkReal evalcond[12];
3799 IkReal x295=IKsin(j5);
3800 IkReal x296=IKcos(j5);
3801 IkReal x297=(cj3*new_r00);
3802 IkReal x298=(cj3*cj4);
3803 IkReal x299=(cj4*sj3);
3804 IkReal x300=((1.0)*cj3);
3805 IkReal x301=(sj3*x295);
3806 IkReal x302=((1.0)*x296);
3807 evalcond[0]=(((sj4*x295))+new_r21);
3808 evalcond[1]=((((-1.0)*sj4*x302))+new_r20);
3809 evalcond[2]=(((new_r00*sj3))+x295+(((-1.0)*new_r10*x300)));
3810 evalcond[3]=(((new_r01*sj3))+x296+(((-1.0)*new_r11*x300)));
3811 evalcond[4]=(((new_r11*sj3))+((cj4*x295))+((cj3*new_r01)));
3812 evalcond[5]=(((x295*x298))+new_r01+((sj3*x296)));
3813 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x302))+x297);
3814 evalcond[7]=((((-1.0)*x298*x302))+x301+new_r00);
3815 evalcond[8]=((((-1.0)*x296*x300))+((x295*x299))+new_r11);
3816 evalcond[9]=((((-1.0)*x295*x300))+(((-1.0)*x299*x302))+new_r10);
3817 evalcond[10]=(((new_r11*x299))+x295+((new_r01*x298))+((new_r21*sj4)));
3818 evalcond[11]=(((new_r20*sj4))+((new_r10*x299))+((cj4*x297))+(((-1.0)*x302)));
3819 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3820 {
3821 continue;
3822 }
3823 }
3824 
3825 {
3826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3827 vinfos[0].jointtype = 1;
3828 vinfos[0].foffset = j0;
3829 vinfos[0].indices[0] = _ij0[0];
3830 vinfos[0].indices[1] = _ij0[1];
3831 vinfos[0].maxsolutions = _nj0;
3832 vinfos[1].jointtype = 1;
3833 vinfos[1].foffset = j1;
3834 vinfos[1].indices[0] = _ij1[0];
3835 vinfos[1].indices[1] = _ij1[1];
3836 vinfos[1].maxsolutions = _nj1;
3837 vinfos[2].jointtype = 1;
3838 vinfos[2].foffset = j2;
3839 vinfos[2].indices[0] = _ij2[0];
3840 vinfos[2].indices[1] = _ij2[1];
3841 vinfos[2].maxsolutions = _nj2;
3842 vinfos[3].jointtype = 1;
3843 vinfos[3].foffset = j3;
3844 vinfos[3].indices[0] = _ij3[0];
3845 vinfos[3].indices[1] = _ij3[1];
3846 vinfos[3].maxsolutions = _nj3;
3847 vinfos[4].jointtype = 1;
3848 vinfos[4].foffset = j4;
3849 vinfos[4].indices[0] = _ij4[0];
3850 vinfos[4].indices[1] = _ij4[1];
3851 vinfos[4].maxsolutions = _nj4;
3852 vinfos[5].jointtype = 1;
3853 vinfos[5].foffset = j5;
3854 vinfos[5].indices[0] = _ij5[0];
3855 vinfos[5].indices[1] = _ij5[1];
3856 vinfos[5].maxsolutions = _nj5;
3857 std::vector<int> vfree(0);
3858 solutions.AddSolution(vinfos,vfree);
3859 }
3860 }
3861 }
3862 
3863 }
3864 
3865 }
3866 }
3867 }
3868 
3869 }
3870 
3871 }
3872 
3873 } else
3874 {
3875 {
3876 IkReal j3array[1], cj3array[1], sj3array[1];
3877 bool j3valid[1]={false};
3878 _nj3 = 1;
3879 CheckValue<IkReal> x303=IKPowWithIntegerCheck(IKsign(sj4),-1);
3880 if(!x303.valid){
3881 continue;
3882 }
3883 CheckValue<IkReal> x304 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3884 if(!x304.valid){
3885 continue;
3886 }
3887 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x303.value)))+(x304.value));
3888 sj3array[0]=IKsin(j3array[0]);
3889 cj3array[0]=IKcos(j3array[0]);
3890 if( j3array[0] > IKPI )
3891 {
3892  j3array[0]-=IK2PI;
3893 }
3894 else if( j3array[0] < -IKPI )
3895 { j3array[0]+=IK2PI;
3896 }
3897 j3valid[0] = true;
3898 for(int ij3 = 0; ij3 < 1; ++ij3)
3899 {
3900 if( !j3valid[ij3] )
3901 {
3902  continue;
3903 }
3904 _ij3[0] = ij3; _ij3[1] = -1;
3905 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3906 {
3907 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3908 {
3909  j3valid[iij3]=false; _ij3[1] = iij3; break;
3910 }
3911 }
3912 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3913 {
3914 IkReal evalcond[8];
3915 IkReal x305=IKcos(j3);
3916 IkReal x306=IKsin(j3);
3917 IkReal x307=((1.0)*cj4);
3918 IkReal x308=(new_r02*x305);
3919 IkReal x309=(sj4*x306);
3920 IkReal x310=(sj4*x305);
3921 IkReal x311=(new_r12*x306);
3922 evalcond[0]=(x310+new_r02);
3923 evalcond[1]=(x309+new_r12);
3924 evalcond[2]=((((-1.0)*new_r02*x306))+((new_r12*x305)));
3925 evalcond[3]=(sj4+x308+x311);
3926 evalcond[4]=(((cj4*x311))+((new_r22*sj4))+((cj4*x308)));
3927 evalcond[5]=(((new_r00*x310))+((new_r10*x309))+(((-1.0)*new_r20*x307)));
3928 evalcond[6]=(((new_r11*x309))+(((-1.0)*new_r21*x307))+((new_r01*x310)));
3929 evalcond[7]=((1.0)+(((-1.0)*new_r22*x307))+((new_r12*x309))+((sj4*x308)));
3930 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3931 {
3932 continue;
3933 }
3934 }
3935 
3936 {
3937 IkReal j5eval[3];
3938 j5eval[0]=sj4;
3939 j5eval[1]=IKsign(sj4);
3940 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
3941 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
3942 {
3943 {
3944 IkReal j5eval[2];
3945 j5eval[0]=sj4;
3946 j5eval[1]=sj3;
3947 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3948 {
3949 {
3950 IkReal j5eval[2];
3951 j5eval[0]=sj4;
3952 j5eval[1]=cj3;
3953 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3954 {
3955 {
3956 IkReal evalcond[5];
3957 bool bgotonextstatement = true;
3958 do
3959 {
3960 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
3961 evalcond[1]=new_r02;
3962 evalcond[2]=new_r12;
3963 evalcond[3]=new_r21;
3964 evalcond[4]=new_r20;
3965 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
3966 {
3967 bgotonextstatement=false;
3968 {
3969 IkReal j5array[1], cj5array[1], sj5array[1];
3970 bool j5valid[1]={false};
3971 _nj5 = 1;
3972 IkReal x312=((1.0)*new_r01);
3973 if( IKabs(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x312))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312))))+IKsqr(((((-1.0)*sj3*x312))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
3974  continue;
3975 j5array[0]=IKatan2(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312))), ((((-1.0)*sj3*x312))+((cj3*new_r00))));
3976 sj5array[0]=IKsin(j5array[0]);
3977 cj5array[0]=IKcos(j5array[0]);
3978 if( j5array[0] > IKPI )
3979 {
3980  j5array[0]-=IK2PI;
3981 }
3982 else if( j5array[0] < -IKPI )
3983 { j5array[0]+=IK2PI;
3984 }
3985 j5valid[0] = true;
3986 for(int ij5 = 0; ij5 < 1; ++ij5)
3987 {
3988 if( !j5valid[ij5] )
3989 {
3990  continue;
3991 }
3992 _ij5[0] = ij5; _ij5[1] = -1;
3993 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3994 {
3995 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3996 {
3997  j5valid[iij5]=false; _ij5[1] = iij5; break;
3998 }
3999 }
4000 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4001 {
4002 IkReal evalcond[8];
4003 IkReal x313=IKsin(j5);
4004 IkReal x314=IKcos(j5);
4005 IkReal x315=((1.0)*cj3);
4006 IkReal x316=(sj3*x313);
4007 IkReal x317=((1.0)*x314);
4008 IkReal x318=(x314*x315);
4009 evalcond[0]=(((new_r11*sj3))+x313+((cj3*new_r01)));
4010 evalcond[1]=(((new_r00*sj3))+x313+(((-1.0)*new_r10*x315)));
4011 evalcond[2]=(((new_r01*sj3))+(((-1.0)*new_r11*x315))+x314);
4012 evalcond[3]=(((cj3*x313))+new_r01+((sj3*x314)));
4013 evalcond[4]=(((new_r10*sj3))+(((-1.0)*x317))+((cj3*new_r00)));
4014 evalcond[5]=(x316+(((-1.0)*x318))+new_r00);
4015 evalcond[6]=(x316+(((-1.0)*x318))+new_r11);
4016 evalcond[7]=((((-1.0)*x313*x315))+(((-1.0)*sj3*x317))+new_r10);
4017 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4018 {
4019 continue;
4020 }
4021 }
4022 
4023 {
4024 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4025 vinfos[0].jointtype = 1;
4026 vinfos[0].foffset = j0;
4027 vinfos[0].indices[0] = _ij0[0];
4028 vinfos[0].indices[1] = _ij0[1];
4029 vinfos[0].maxsolutions = _nj0;
4030 vinfos[1].jointtype = 1;
4031 vinfos[1].foffset = j1;
4032 vinfos[1].indices[0] = _ij1[0];
4033 vinfos[1].indices[1] = _ij1[1];
4034 vinfos[1].maxsolutions = _nj1;
4035 vinfos[2].jointtype = 1;
4036 vinfos[2].foffset = j2;
4037 vinfos[2].indices[0] = _ij2[0];
4038 vinfos[2].indices[1] = _ij2[1];
4039 vinfos[2].maxsolutions = _nj2;
4040 vinfos[3].jointtype = 1;
4041 vinfos[3].foffset = j3;
4042 vinfos[3].indices[0] = _ij3[0];
4043 vinfos[3].indices[1] = _ij3[1];
4044 vinfos[3].maxsolutions = _nj3;
4045 vinfos[4].jointtype = 1;
4046 vinfos[4].foffset = j4;
4047 vinfos[4].indices[0] = _ij4[0];
4048 vinfos[4].indices[1] = _ij4[1];
4049 vinfos[4].maxsolutions = _nj4;
4050 vinfos[5].jointtype = 1;
4051 vinfos[5].foffset = j5;
4052 vinfos[5].indices[0] = _ij5[0];
4053 vinfos[5].indices[1] = _ij5[1];
4054 vinfos[5].maxsolutions = _nj5;
4055 std::vector<int> vfree(0);
4056 solutions.AddSolution(vinfos,vfree);
4057 }
4058 }
4059 }
4060 
4061 }
4062 } while(0);
4063 if( bgotonextstatement )
4064 {
4065 bool bgotonextstatement = true;
4066 do
4067 {
4068 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4069 evalcond[1]=new_r02;
4070 evalcond[2]=new_r12;
4071 evalcond[3]=new_r21;
4072 evalcond[4]=new_r20;
4073 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
4074 {
4075 bgotonextstatement=false;
4076 {
4077 IkReal j5array[1], cj5array[1], sj5array[1];
4078 bool j5valid[1]={false};
4079 _nj5 = 1;
4080 IkReal x319=((1.0)*sj3);
4081 if( IKabs(((((-1.0)*new_r00*x319))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x319))+((cj3*new_r01))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319))))-1) <= IKFAST_SINCOS_THRESH )
4082  continue;
4083 j5array[0]=IKatan2(((((-1.0)*new_r00*x319))+((cj3*new_r01))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319))));
4084 sj5array[0]=IKsin(j5array[0]);
4085 cj5array[0]=IKcos(j5array[0]);
4086 if( j5array[0] > IKPI )
4087 {
4088  j5array[0]-=IK2PI;
4089 }
4090 else if( j5array[0] < -IKPI )
4091 { j5array[0]+=IK2PI;
4092 }
4093 j5valid[0] = true;
4094 for(int ij5 = 0; ij5 < 1; ++ij5)
4095 {
4096 if( !j5valid[ij5] )
4097 {
4098  continue;
4099 }
4100 _ij5[0] = ij5; _ij5[1] = -1;
4101 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4102 {
4103 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4104 {
4105  j5valid[iij5]=false; _ij5[1] = iij5; break;
4106 }
4107 }
4108 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4109 {
4110 IkReal evalcond[8];
4111 IkReal x320=IKcos(j5);
4112 IkReal x321=IKsin(j5);
4113 IkReal x322=((1.0)*cj3);
4114 IkReal x323=(sj3*x320);
4115 IkReal x324=((1.0)*x321);
4116 IkReal x325=(x321*x322);
4117 evalcond[0]=(((new_r10*sj3))+x320+((cj3*new_r00)));
4118 evalcond[1]=(((new_r00*sj3))+x321+(((-1.0)*new_r10*x322)));
4119 evalcond[2]=(((new_r01*sj3))+x320+(((-1.0)*new_r11*x322)));
4120 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x324))+((cj3*new_r01)));
4121 evalcond[4]=(((sj3*x321))+new_r00+((cj3*x320)));
4122 evalcond[5]=((((-1.0)*x325))+x323+new_r01);
4123 evalcond[6]=((((-1.0)*x325))+x323+new_r10);
4124 evalcond[7]=((((-1.0)*x320*x322))+new_r11+(((-1.0)*sj3*x324)));
4125 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4126 {
4127 continue;
4128 }
4129 }
4130 
4131 {
4132 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4133 vinfos[0].jointtype = 1;
4134 vinfos[0].foffset = j0;
4135 vinfos[0].indices[0] = _ij0[0];
4136 vinfos[0].indices[1] = _ij0[1];
4137 vinfos[0].maxsolutions = _nj0;
4138 vinfos[1].jointtype = 1;
4139 vinfos[1].foffset = j1;
4140 vinfos[1].indices[0] = _ij1[0];
4141 vinfos[1].indices[1] = _ij1[1];
4142 vinfos[1].maxsolutions = _nj1;
4143 vinfos[2].jointtype = 1;
4144 vinfos[2].foffset = j2;
4145 vinfos[2].indices[0] = _ij2[0];
4146 vinfos[2].indices[1] = _ij2[1];
4147 vinfos[2].maxsolutions = _nj2;
4148 vinfos[3].jointtype = 1;
4149 vinfos[3].foffset = j3;
4150 vinfos[3].indices[0] = _ij3[0];
4151 vinfos[3].indices[1] = _ij3[1];
4152 vinfos[3].maxsolutions = _nj3;
4153 vinfos[4].jointtype = 1;
4154 vinfos[4].foffset = j4;
4155 vinfos[4].indices[0] = _ij4[0];
4156 vinfos[4].indices[1] = _ij4[1];
4157 vinfos[4].maxsolutions = _nj4;
4158 vinfos[5].jointtype = 1;
4159 vinfos[5].foffset = j5;
4160 vinfos[5].indices[0] = _ij5[0];
4161 vinfos[5].indices[1] = _ij5[1];
4162 vinfos[5].maxsolutions = _nj5;
4163 std::vector<int> vfree(0);
4164 solutions.AddSolution(vinfos,vfree);
4165 }
4166 }
4167 }
4168 
4169 }
4170 } while(0);
4171 if( bgotonextstatement )
4172 {
4173 bool bgotonextstatement = true;
4174 do
4175 {
4176 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4177 evalcond[1]=new_r02;
4178 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4179 {
4180 bgotonextstatement=false;
4181 {
4182 IkReal j5array[1], cj5array[1], sj5array[1];
4183 bool j5valid[1]={false};
4184 _nj5 = 1;
4185 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
4186  continue;
4187 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4188 sj5array[0]=IKsin(j5array[0]);
4189 cj5array[0]=IKcos(j5array[0]);
4190 if( j5array[0] > IKPI )
4191 {
4192  j5array[0]-=IK2PI;
4193 }
4194 else if( j5array[0] < -IKPI )
4195 { j5array[0]+=IK2PI;
4196 }
4197 j5valid[0] = true;
4198 for(int ij5 = 0; ij5 < 1; ++ij5)
4199 {
4200 if( !j5valid[ij5] )
4201 {
4202  continue;
4203 }
4204 _ij5[0] = ij5; _ij5[1] = -1;
4205 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4206 {
4207 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4208 {
4209  j5valid[iij5]=false; _ij5[1] = iij5; break;
4210 }
4211 }
4212 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4213 {
4214 IkReal evalcond[8];
4215 IkReal x326=IKsin(j5);
4216 IkReal x327=IKcos(j5);
4217 IkReal x328=((1.0)*x327);
4218 evalcond[0]=(x326+new_r00);
4219 evalcond[1]=(x327+new_r01);
4220 evalcond[2]=(((sj4*x326))+new_r21);
4221 evalcond[3]=(((cj4*x326))+new_r11);
4222 evalcond[4]=(new_r20+(((-1.0)*sj4*x328)));
4223 evalcond[5]=(new_r10+(((-1.0)*cj4*x328)));
4224 evalcond[6]=(((cj4*new_r11))+x326+((new_r21*sj4)));
4225 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x328)));
4226 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4227 {
4228 continue;
4229 }
4230 }
4231 
4232 {
4233 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4234 vinfos[0].jointtype = 1;
4235 vinfos[0].foffset = j0;
4236 vinfos[0].indices[0] = _ij0[0];
4237 vinfos[0].indices[1] = _ij0[1];
4238 vinfos[0].maxsolutions = _nj0;
4239 vinfos[1].jointtype = 1;
4240 vinfos[1].foffset = j1;
4241 vinfos[1].indices[0] = _ij1[0];
4242 vinfos[1].indices[1] = _ij1[1];
4243 vinfos[1].maxsolutions = _nj1;
4244 vinfos[2].jointtype = 1;
4245 vinfos[2].foffset = j2;
4246 vinfos[2].indices[0] = _ij2[0];
4247 vinfos[2].indices[1] = _ij2[1];
4248 vinfos[2].maxsolutions = _nj2;
4249 vinfos[3].jointtype = 1;
4250 vinfos[3].foffset = j3;
4251 vinfos[3].indices[0] = _ij3[0];
4252 vinfos[3].indices[1] = _ij3[1];
4253 vinfos[3].maxsolutions = _nj3;
4254 vinfos[4].jointtype = 1;
4255 vinfos[4].foffset = j4;
4256 vinfos[4].indices[0] = _ij4[0];
4257 vinfos[4].indices[1] = _ij4[1];
4258 vinfos[4].maxsolutions = _nj4;
4259 vinfos[5].jointtype = 1;
4260 vinfos[5].foffset = j5;
4261 vinfos[5].indices[0] = _ij5[0];
4262 vinfos[5].indices[1] = _ij5[1];
4263 vinfos[5].maxsolutions = _nj5;
4264 std::vector<int> vfree(0);
4265 solutions.AddSolution(vinfos,vfree);
4266 }
4267 }
4268 }
4269 
4270 }
4271 } while(0);
4272 if( bgotonextstatement )
4273 {
4274 bool bgotonextstatement = true;
4275 do
4276 {
4277 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4278 evalcond[1]=new_r02;
4279 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4280 {
4281 bgotonextstatement=false;
4282 {
4283 IkReal j5array[1], cj5array[1], sj5array[1];
4284 bool j5valid[1]={false};
4285 _nj5 = 1;
4286 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4287  continue;
4288 j5array[0]=IKatan2(new_r00, new_r01);
4289 sj5array[0]=IKsin(j5array[0]);
4290 cj5array[0]=IKcos(j5array[0]);
4291 if( j5array[0] > IKPI )
4292 {
4293  j5array[0]-=IK2PI;
4294 }
4295 else if( j5array[0] < -IKPI )
4296 { j5array[0]+=IK2PI;
4297 }
4298 j5valid[0] = true;
4299 for(int ij5 = 0; ij5 < 1; ++ij5)
4300 {
4301 if( !j5valid[ij5] )
4302 {
4303  continue;
4304 }
4305 _ij5[0] = ij5; _ij5[1] = -1;
4306 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4307 {
4308 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4309 {
4310  j5valid[iij5]=false; _ij5[1] = iij5; break;
4311 }
4312 }
4313 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4314 {
4315 IkReal evalcond[8];
4316 IkReal x329=IKsin(j5);
4317 IkReal x330=IKcos(j5);
4318 IkReal x331=((1.0)*cj4);
4319 IkReal x332=((1.0)*x330);
4320 evalcond[0]=(((sj4*x329))+new_r21);
4321 evalcond[1]=(x329+(((-1.0)*new_r00)));
4322 evalcond[2]=(x330+(((-1.0)*new_r01)));
4323 evalcond[3]=((((-1.0)*sj4*x332))+new_r20);
4324 evalcond[4]=(((cj4*x329))+(((-1.0)*new_r11)));
4325 evalcond[5]=((((-1.0)*x330*x331))+(((-1.0)*new_r10)));
4326 evalcond[6]=((((-1.0)*new_r11*x331))+x329+((new_r21*sj4)));
4327 evalcond[7]=((((-1.0)*new_r10*x331))+((new_r20*sj4))+(((-1.0)*x332)));
4328 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4329 {
4330 continue;
4331 }
4332 }
4333 
4334 {
4335 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4336 vinfos[0].jointtype = 1;
4337 vinfos[0].foffset = j0;
4338 vinfos[0].indices[0] = _ij0[0];
4339 vinfos[0].indices[1] = _ij0[1];
4340 vinfos[0].maxsolutions = _nj0;
4341 vinfos[1].jointtype = 1;
4342 vinfos[1].foffset = j1;
4343 vinfos[1].indices[0] = _ij1[0];
4344 vinfos[1].indices[1] = _ij1[1];
4345 vinfos[1].maxsolutions = _nj1;
4346 vinfos[2].jointtype = 1;
4347 vinfos[2].foffset = j2;
4348 vinfos[2].indices[0] = _ij2[0];
4349 vinfos[2].indices[1] = _ij2[1];
4350 vinfos[2].maxsolutions = _nj2;
4351 vinfos[3].jointtype = 1;
4352 vinfos[3].foffset = j3;
4353 vinfos[3].indices[0] = _ij3[0];
4354 vinfos[3].indices[1] = _ij3[1];
4355 vinfos[3].maxsolutions = _nj3;
4356 vinfos[4].jointtype = 1;
4357 vinfos[4].foffset = j4;
4358 vinfos[4].indices[0] = _ij4[0];
4359 vinfos[4].indices[1] = _ij4[1];
4360 vinfos[4].maxsolutions = _nj4;
4361 vinfos[5].jointtype = 1;
4362 vinfos[5].foffset = j5;
4363 vinfos[5].indices[0] = _ij5[0];
4364 vinfos[5].indices[1] = _ij5[1];
4365 vinfos[5].maxsolutions = _nj5;
4366 std::vector<int> vfree(0);
4367 solutions.AddSolution(vinfos,vfree);
4368 }
4369 }
4370 }
4371 
4372 }
4373 } while(0);
4374 if( bgotonextstatement )
4375 {
4376 bool bgotonextstatement = true;
4377 do
4378 {
4379 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4380 evalcond[1]=new_r12;
4381 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4382 {
4383 bgotonextstatement=false;
4384 {
4385 IkReal j5array[1], cj5array[1], sj5array[1];
4386 bool j5valid[1]={false};
4387 _nj5 = 1;
4388 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4389  continue;
4390 j5array[0]=IKatan2(new_r10, new_r11);
4391 sj5array[0]=IKsin(j5array[0]);
4392 cj5array[0]=IKcos(j5array[0]);
4393 if( j5array[0] > IKPI )
4394 {
4395  j5array[0]-=IK2PI;
4396 }
4397 else if( j5array[0] < -IKPI )
4398 { j5array[0]+=IK2PI;
4399 }
4400 j5valid[0] = true;
4401 for(int ij5 = 0; ij5 < 1; ++ij5)
4402 {
4403 if( !j5valid[ij5] )
4404 {
4405  continue;
4406 }
4407 _ij5[0] = ij5; _ij5[1] = -1;
4408 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4409 {
4410 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4411 {
4412  j5valid[iij5]=false; _ij5[1] = iij5; break;
4413 }
4414 }
4415 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4416 {
4417 IkReal evalcond[8];
4418 IkReal x333=IKcos(j5);
4419 IkReal x334=IKsin(j5);
4420 IkReal x335=((1.0)*x333);
4421 evalcond[0]=(((new_r02*x333))+new_r20);
4422 evalcond[1]=(x334+(((-1.0)*new_r10)));
4423 evalcond[2]=(x333+(((-1.0)*new_r11)));
4424 evalcond[3]=(((cj4*x334))+new_r01);
4425 evalcond[4]=((((-1.0)*new_r02*x334))+new_r21);
4426 evalcond[5]=((((-1.0)*cj4*x335))+new_r00);
4427 evalcond[6]=(((cj4*new_r01))+x334+((new_r21*sj4)));
4428 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x335)));
4429 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4430 {
4431 continue;
4432 }
4433 }
4434 
4435 {
4436 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4437 vinfos[0].jointtype = 1;
4438 vinfos[0].foffset = j0;
4439 vinfos[0].indices[0] = _ij0[0];
4440 vinfos[0].indices[1] = _ij0[1];
4441 vinfos[0].maxsolutions = _nj0;
4442 vinfos[1].jointtype = 1;
4443 vinfos[1].foffset = j1;
4444 vinfos[1].indices[0] = _ij1[0];
4445 vinfos[1].indices[1] = _ij1[1];
4446 vinfos[1].maxsolutions = _nj1;
4447 vinfos[2].jointtype = 1;
4448 vinfos[2].foffset = j2;
4449 vinfos[2].indices[0] = _ij2[0];
4450 vinfos[2].indices[1] = _ij2[1];
4451 vinfos[2].maxsolutions = _nj2;
4452 vinfos[3].jointtype = 1;
4453 vinfos[3].foffset = j3;
4454 vinfos[3].indices[0] = _ij3[0];
4455 vinfos[3].indices[1] = _ij3[1];
4456 vinfos[3].maxsolutions = _nj3;
4457 vinfos[4].jointtype = 1;
4458 vinfos[4].foffset = j4;
4459 vinfos[4].indices[0] = _ij4[0];
4460 vinfos[4].indices[1] = _ij4[1];
4461 vinfos[4].maxsolutions = _nj4;
4462 vinfos[5].jointtype = 1;
4463 vinfos[5].foffset = j5;
4464 vinfos[5].indices[0] = _ij5[0];
4465 vinfos[5].indices[1] = _ij5[1];
4466 vinfos[5].maxsolutions = _nj5;
4467 std::vector<int> vfree(0);
4468 solutions.AddSolution(vinfos,vfree);
4469 }
4470 }
4471 }
4472 
4473 }
4474 } while(0);
4475 if( bgotonextstatement )
4476 {
4477 bool bgotonextstatement = true;
4478 do
4479 {
4480 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4481 evalcond[1]=new_r12;
4482 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4483 {
4484 bgotonextstatement=false;
4485 {
4486 IkReal j5array[1], cj5array[1], sj5array[1];
4487 bool j5valid[1]={false};
4488 _nj5 = 1;
4489 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
4490  continue;
4491 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4492 sj5array[0]=IKsin(j5array[0]);
4493 cj5array[0]=IKcos(j5array[0]);
4494 if( j5array[0] > IKPI )
4495 {
4496  j5array[0]-=IK2PI;
4497 }
4498 else if( j5array[0] < -IKPI )
4499 { j5array[0]+=IK2PI;
4500 }
4501 j5valid[0] = true;
4502 for(int ij5 = 0; ij5 < 1; ++ij5)
4503 {
4504 if( !j5valid[ij5] )
4505 {
4506  continue;
4507 }
4508 _ij5[0] = ij5; _ij5[1] = -1;
4509 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4510 {
4511 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4512 {
4513  j5valid[iij5]=false; _ij5[1] = iij5; break;
4514 }
4515 }
4516 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4517 {
4518 IkReal evalcond[8];
4519 IkReal x336=IKsin(j5);
4520 IkReal x337=IKcos(j5);
4521 IkReal x338=((1.0)*new_r00);
4522 IkReal x339=((1.0)*new_r01);
4523 IkReal x340=((1.0)*x337);
4524 evalcond[0]=(x336+new_r10);
4525 evalcond[1]=(x337+new_r11);
4526 evalcond[2]=(((new_r02*x336))+new_r21);
4527 evalcond[3]=((((-1.0)*new_r02*x340))+new_r20);
4528 evalcond[4]=((((-1.0)*x339))+((cj4*x336)));
4529 evalcond[5]=((((-1.0)*cj4*x340))+(((-1.0)*x338)));
4530 evalcond[6]=((((-1.0)*cj4*x339))+x336+((new_r21*sj4)));
4531 evalcond[7]=(((new_r20*sj4))+(((-1.0)*cj4*x338))+(((-1.0)*x340)));
4532 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4533 {
4534 continue;
4535 }
4536 }
4537 
4538 {
4539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4540 vinfos[0].jointtype = 1;
4541 vinfos[0].foffset = j0;
4542 vinfos[0].indices[0] = _ij0[0];
4543 vinfos[0].indices[1] = _ij0[1];
4544 vinfos[0].maxsolutions = _nj0;
4545 vinfos[1].jointtype = 1;
4546 vinfos[1].foffset = j1;
4547 vinfos[1].indices[0] = _ij1[0];
4548 vinfos[1].indices[1] = _ij1[1];
4549 vinfos[1].maxsolutions = _nj1;
4550 vinfos[2].jointtype = 1;
4551 vinfos[2].foffset = j2;
4552 vinfos[2].indices[0] = _ij2[0];
4553 vinfos[2].indices[1] = _ij2[1];
4554 vinfos[2].maxsolutions = _nj2;
4555 vinfos[3].jointtype = 1;
4556 vinfos[3].foffset = j3;
4557 vinfos[3].indices[0] = _ij3[0];
4558 vinfos[3].indices[1] = _ij3[1];
4559 vinfos[3].maxsolutions = _nj3;
4560 vinfos[4].jointtype = 1;
4561 vinfos[4].foffset = j4;
4562 vinfos[4].indices[0] = _ij4[0];
4563 vinfos[4].indices[1] = _ij4[1];
4564 vinfos[4].maxsolutions = _nj4;
4565 vinfos[5].jointtype = 1;
4566 vinfos[5].foffset = j5;
4567 vinfos[5].indices[0] = _ij5[0];
4568 vinfos[5].indices[1] = _ij5[1];
4569 vinfos[5].maxsolutions = _nj5;
4570 std::vector<int> vfree(0);
4571 solutions.AddSolution(vinfos,vfree);
4572 }
4573 }
4574 }
4575 
4576 }
4577 } while(0);
4578 if( bgotonextstatement )
4579 {
4580 bool bgotonextstatement = true;
4581 do
4582 {
4583 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4584 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4585 {
4586 bgotonextstatement=false;
4587 {
4588 IkReal j5eval[1];
4589 new_r21=0;
4590 new_r20=0;
4591 new_r02=0;
4592 new_r12=0;
4593 j5eval[0]=1.0;
4594 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4595 {
4596 continue; // no branches [j5]
4597 
4598 } else
4599 {
4600 IkReal op[2+1], zeror[2];
4601 int numroots;
4602 op[0]=-1.0;
4603 op[1]=0;
4604 op[2]=1.0;
4605 polyroots2(op,zeror,numroots);
4606 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4607 int numsolutions = 0;
4608 for(int ij5 = 0; ij5 < numroots; ++ij5)
4609 {
4610 IkReal htj5 = zeror[ij5];
4611 tempj5array[0]=((2.0)*(atan(htj5)));
4612 for(int kj5 = 0; kj5 < 1; ++kj5)
4613 {
4614 j5array[numsolutions] = tempj5array[kj5];
4615 if( j5array[numsolutions] > IKPI )
4616 {
4617  j5array[numsolutions]-=IK2PI;
4618 }
4619 else if( j5array[numsolutions] < -IKPI )
4620 {
4621  j5array[numsolutions]+=IK2PI;
4622 }
4623 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4624 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4625 numsolutions++;
4626 }
4627 }
4628 bool j5valid[2]={true,true};
4629 _nj5 = 2;
4630 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4631  {
4632 if( !j5valid[ij5] )
4633 {
4634  continue;
4635 }
4636  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4637 htj5 = IKtan(j5/2);
4638 
4639 _ij5[0] = ij5; _ij5[1] = -1;
4640 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4641 {
4642 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4643 {
4644  j5valid[iij5]=false; _ij5[1] = iij5; break;
4645 }
4646 }
4647 {
4648 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4649 vinfos[0].jointtype = 1;
4650 vinfos[0].foffset = j0;
4651 vinfos[0].indices[0] = _ij0[0];
4652 vinfos[0].indices[1] = _ij0[1];
4653 vinfos[0].maxsolutions = _nj0;
4654 vinfos[1].jointtype = 1;
4655 vinfos[1].foffset = j1;
4656 vinfos[1].indices[0] = _ij1[0];
4657 vinfos[1].indices[1] = _ij1[1];
4658 vinfos[1].maxsolutions = _nj1;
4659 vinfos[2].jointtype = 1;
4660 vinfos[2].foffset = j2;
4661 vinfos[2].indices[0] = _ij2[0];
4662 vinfos[2].indices[1] = _ij2[1];
4663 vinfos[2].maxsolutions = _nj2;
4664 vinfos[3].jointtype = 1;
4665 vinfos[3].foffset = j3;
4666 vinfos[3].indices[0] = _ij3[0];
4667 vinfos[3].indices[1] = _ij3[1];
4668 vinfos[3].maxsolutions = _nj3;
4669 vinfos[4].jointtype = 1;
4670 vinfos[4].foffset = j4;
4671 vinfos[4].indices[0] = _ij4[0];
4672 vinfos[4].indices[1] = _ij4[1];
4673 vinfos[4].maxsolutions = _nj4;
4674 vinfos[5].jointtype = 1;
4675 vinfos[5].foffset = j5;
4676 vinfos[5].indices[0] = _ij5[0];
4677 vinfos[5].indices[1] = _ij5[1];
4678 vinfos[5].maxsolutions = _nj5;
4679 std::vector<int> vfree(0);
4680 solutions.AddSolution(vinfos,vfree);
4681 }
4682  }
4683 
4684 }
4685 
4686 }
4687 
4688 }
4689 } while(0);
4690 if( bgotonextstatement )
4691 {
4692 bool bgotonextstatement = true;
4693 do
4694 {
4695 if( 1 )
4696 {
4697 bgotonextstatement=false;
4698 continue; // branch miss [j5]
4699 
4700 }
4701 } while(0);
4702 if( bgotonextstatement )
4703 {
4704 }
4705 }
4706 }
4707 }
4708 }
4709 }
4710 }
4711 }
4712 }
4713 
4714 } else
4715 {
4716 {
4717 IkReal j5array[1], cj5array[1], sj5array[1];
4718 bool j5valid[1]={false};
4719 _nj5 = 1;
4720 CheckValue<IkReal> x342=IKPowWithIntegerCheck(sj4,-1);
4721 if(!x342.valid){
4722 continue;
4723 }
4724 IkReal x341=x342.value;
4725 CheckValue<IkReal> x343=IKPowWithIntegerCheck(cj3,-1);
4726 if(!x343.valid){
4727 continue;
4728 }
4729 if( IKabs(((-1.0)*new_r21*x341)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x341))+IKsqr((x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4730  continue;
4731 j5array[0]=IKatan2(((-1.0)*new_r21*x341), (x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4732 sj5array[0]=IKsin(j5array[0]);
4733 cj5array[0]=IKcos(j5array[0]);
4734 if( j5array[0] > IKPI )
4735 {
4736  j5array[0]-=IK2PI;
4737 }
4738 else if( j5array[0] < -IKPI )
4739 { j5array[0]+=IK2PI;
4740 }
4741 j5valid[0] = true;
4742 for(int ij5 = 0; ij5 < 1; ++ij5)
4743 {
4744 if( !j5valid[ij5] )
4745 {
4746  continue;
4747 }
4748 _ij5[0] = ij5; _ij5[1] = -1;
4749 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4750 {
4751 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4752 {
4753  j5valid[iij5]=false; _ij5[1] = iij5; break;
4754 }
4755 }
4756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4757 {
4758 IkReal evalcond[12];
4759 IkReal x344=IKsin(j5);
4760 IkReal x345=IKcos(j5);
4761 IkReal x346=(cj3*new_r00);
4762 IkReal x347=(cj3*cj4);
4763 IkReal x348=(cj4*sj3);
4764 IkReal x349=((1.0)*cj3);
4765 IkReal x350=(sj3*x344);
4766 IkReal x351=((1.0)*x345);
4767 evalcond[0]=(((sj4*x344))+new_r21);
4768 evalcond[1]=(new_r20+(((-1.0)*sj4*x351)));
4769 evalcond[2]=((((-1.0)*new_r10*x349))+((new_r00*sj3))+x344);
4770 evalcond[3]=((((-1.0)*new_r11*x349))+((new_r01*sj3))+x345);
4771 evalcond[4]=(((new_r11*sj3))+((cj4*x344))+((cj3*new_r01)));
4772 evalcond[5]=(((sj3*x345))+((x344*x347))+new_r01);
4773 evalcond[6]=(((new_r10*sj3))+x346+(((-1.0)*cj4*x351)));
4774 evalcond[7]=(x350+(((-1.0)*x347*x351))+new_r00);
4775 evalcond[8]=(((x344*x348))+(((-1.0)*x345*x349))+new_r11);
4776 evalcond[9]=((((-1.0)*x348*x351))+(((-1.0)*x344*x349))+new_r10);
4777 evalcond[10]=(((new_r11*x348))+((new_r01*x347))+x344+((new_r21*sj4)));
4778 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x351))+((new_r10*x348))+((cj4*x346)));
4779 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
4780 {
4781 continue;
4782 }
4783 }
4784 
4785 {
4786 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4787 vinfos[0].jointtype = 1;
4788 vinfos[0].foffset = j0;
4789 vinfos[0].indices[0] = _ij0[0];
4790 vinfos[0].indices[1] = _ij0[1];
4791 vinfos[0].maxsolutions = _nj0;
4792 vinfos[1].jointtype = 1;
4793 vinfos[1].foffset = j1;
4794 vinfos[1].indices[0] = _ij1[0];
4795 vinfos[1].indices[1] = _ij1[1];
4796 vinfos[1].maxsolutions = _nj1;
4797 vinfos[2].jointtype = 1;
4798 vinfos[2].foffset = j2;
4799 vinfos[2].indices[0] = _ij2[0];
4800 vinfos[2].indices[1] = _ij2[1];
4801 vinfos[2].maxsolutions = _nj2;
4802 vinfos[3].jointtype = 1;
4803 vinfos[3].foffset = j3;
4804 vinfos[3].indices[0] = _ij3[0];
4805 vinfos[3].indices[1] = _ij3[1];
4806 vinfos[3].maxsolutions = _nj3;
4807 vinfos[4].jointtype = 1;
4808 vinfos[4].foffset = j4;
4809 vinfos[4].indices[0] = _ij4[0];
4810 vinfos[4].indices[1] = _ij4[1];
4811 vinfos[4].maxsolutions = _nj4;
4812 vinfos[5].jointtype = 1;
4813 vinfos[5].foffset = j5;
4814 vinfos[5].indices[0] = _ij5[0];
4815 vinfos[5].indices[1] = _ij5[1];
4816 vinfos[5].maxsolutions = _nj5;
4817 std::vector<int> vfree(0);
4818 solutions.AddSolution(vinfos,vfree);
4819 }
4820 }
4821 }
4822 
4823 }
4824 
4825 }
4826 
4827 } else
4828 {
4829 {
4830 IkReal j5array[1], cj5array[1], sj5array[1];
4831 bool j5valid[1]={false};
4832 _nj5 = 1;
4833 CheckValue<IkReal> x353=IKPowWithIntegerCheck(sj4,-1);
4834 if(!x353.valid){
4835 continue;
4836 }
4837 IkReal x352=x353.value;
4838 CheckValue<IkReal> x354=IKPowWithIntegerCheck(sj3,-1);
4839 if(!x354.valid){
4840 continue;
4841 }
4842 if( IKabs(((-1.0)*new_r21*x352)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x352))+IKsqr((x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4843  continue;
4844 j5array[0]=IKatan2(((-1.0)*new_r21*x352), (x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4845 sj5array[0]=IKsin(j5array[0]);
4846 cj5array[0]=IKcos(j5array[0]);
4847 if( j5array[0] > IKPI )
4848 {
4849  j5array[0]-=IK2PI;
4850 }
4851 else if( j5array[0] < -IKPI )
4852 { j5array[0]+=IK2PI;
4853 }
4854 j5valid[0] = true;
4855 for(int ij5 = 0; ij5 < 1; ++ij5)
4856 {
4857 if( !j5valid[ij5] )
4858 {
4859  continue;
4860 }
4861 _ij5[0] = ij5; _ij5[1] = -1;
4862 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4863 {
4864 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4865 {
4866  j5valid[iij5]=false; _ij5[1] = iij5; break;
4867 }
4868 }
4869 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4870 {
4871 IkReal evalcond[12];
4872 IkReal x355=IKsin(j5);
4873 IkReal x356=IKcos(j5);
4874 IkReal x357=(cj3*new_r00);
4875 IkReal x358=(cj3*cj4);
4876 IkReal x359=(cj4*sj3);
4877 IkReal x360=((1.0)*cj3);
4878 IkReal x361=(sj3*x355);
4879 IkReal x362=((1.0)*x356);
4880 evalcond[0]=(((sj4*x355))+new_r21);
4881 evalcond[1]=((((-1.0)*sj4*x362))+new_r20);
4882 evalcond[2]=(((new_r00*sj3))+x355+(((-1.0)*new_r10*x360)));
4883 evalcond[3]=(((new_r01*sj3))+x356+(((-1.0)*new_r11*x360)));
4884 evalcond[4]=(((new_r11*sj3))+((cj4*x355))+((cj3*new_r01)));
4885 evalcond[5]=(((x355*x358))+((sj3*x356))+new_r01);
4886 evalcond[6]=((((-1.0)*cj4*x362))+((new_r10*sj3))+x357);
4887 evalcond[7]=((((-1.0)*x358*x362))+x361+new_r00);
4888 evalcond[8]=(((x355*x359))+(((-1.0)*x356*x360))+new_r11);
4889 evalcond[9]=((((-1.0)*x355*x360))+(((-1.0)*x359*x362))+new_r10);
4890 evalcond[10]=(((new_r01*x358))+x355+((new_r21*sj4))+((new_r11*x359)));
4891 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x362))+((cj4*x357))+((new_r10*x359)));
4892 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
4893 {
4894 continue;
4895 }
4896 }
4897 
4898 {
4899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4900 vinfos[0].jointtype = 1;
4901 vinfos[0].foffset = j0;
4902 vinfos[0].indices[0] = _ij0[0];
4903 vinfos[0].indices[1] = _ij0[1];
4904 vinfos[0].maxsolutions = _nj0;
4905 vinfos[1].jointtype = 1;
4906 vinfos[1].foffset = j1;
4907 vinfos[1].indices[0] = _ij1[0];
4908 vinfos[1].indices[1] = _ij1[1];
4909 vinfos[1].maxsolutions = _nj1;
4910 vinfos[2].jointtype = 1;
4911 vinfos[2].foffset = j2;
4912 vinfos[2].indices[0] = _ij2[0];
4913 vinfos[2].indices[1] = _ij2[1];
4914 vinfos[2].maxsolutions = _nj2;
4915 vinfos[3].jointtype = 1;
4916 vinfos[3].foffset = j3;
4917 vinfos[3].indices[0] = _ij3[0];
4918 vinfos[3].indices[1] = _ij3[1];
4919 vinfos[3].maxsolutions = _nj3;
4920 vinfos[4].jointtype = 1;
4921 vinfos[4].foffset = j4;
4922 vinfos[4].indices[0] = _ij4[0];
4923 vinfos[4].indices[1] = _ij4[1];
4924 vinfos[4].maxsolutions = _nj4;
4925 vinfos[5].jointtype = 1;
4926 vinfos[5].foffset = j5;
4927 vinfos[5].indices[0] = _ij5[0];
4928 vinfos[5].indices[1] = _ij5[1];
4929 vinfos[5].maxsolutions = _nj5;
4930 std::vector<int> vfree(0);
4931 solutions.AddSolution(vinfos,vfree);
4932 }
4933 }
4934 }
4935 
4936 }
4937 
4938 }
4939 
4940 } else
4941 {
4942 {
4943 IkReal j5array[1], cj5array[1], sj5array[1];
4944 bool j5valid[1]={false};
4945 _nj5 = 1;
4946 CheckValue<IkReal> x363 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
4947 if(!x363.valid){
4948 continue;
4949 }
4950 CheckValue<IkReal> x364=IKPowWithIntegerCheck(IKsign(sj4),-1);
4951 if(!x364.valid){
4952 continue;
4953 }
4954 j5array[0]=((-1.5707963267949)+(x363.value)+(((1.5707963267949)*(x364.value))));
4955 sj5array[0]=IKsin(j5array[0]);
4956 cj5array[0]=IKcos(j5array[0]);
4957 if( j5array[0] > IKPI )
4958 {
4959  j5array[0]-=IK2PI;
4960 }
4961 else if( j5array[0] < -IKPI )
4962 { j5array[0]+=IK2PI;
4963 }
4964 j5valid[0] = true;
4965 for(int ij5 = 0; ij5 < 1; ++ij5)
4966 {
4967 if( !j5valid[ij5] )
4968 {
4969  continue;
4970 }
4971 _ij5[0] = ij5; _ij5[1] = -1;
4972 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4973 {
4974 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4975 {
4976  j5valid[iij5]=false; _ij5[1] = iij5; break;
4977 }
4978 }
4979 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4980 {
4981 IkReal evalcond[12];
4982 IkReal x365=IKsin(j5);
4983 IkReal x366=IKcos(j5);
4984 IkReal x367=(cj3*new_r00);
4985 IkReal x368=(cj3*cj4);
4986 IkReal x369=(cj4*sj3);
4987 IkReal x370=((1.0)*cj3);
4988 IkReal x371=(sj3*x365);
4989 IkReal x372=((1.0)*x366);
4990 evalcond[0]=(((sj4*x365))+new_r21);
4991 evalcond[1]=((((-1.0)*sj4*x372))+new_r20);
4992 evalcond[2]=(((new_r00*sj3))+x365+(((-1.0)*new_r10*x370)));
4993 evalcond[3]=(((new_r01*sj3))+x366+(((-1.0)*new_r11*x370)));
4994 evalcond[4]=(((new_r11*sj3))+((cj4*x365))+((cj3*new_r01)));
4995 evalcond[5]=(((x365*x368))+((sj3*x366))+new_r01);
4996 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x372))+x367);
4997 evalcond[7]=((((-1.0)*x368*x372))+x371+new_r00);
4998 evalcond[8]=(((x365*x369))+(((-1.0)*x366*x370))+new_r11);
4999 evalcond[9]=((((-1.0)*x369*x372))+new_r10+(((-1.0)*x365*x370)));
5000 evalcond[10]=(x365+((new_r01*x368))+((new_r21*sj4))+((new_r11*x369)));
5001 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x372))+((cj4*x367))+((new_r10*x369)));
5002 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
5003 {
5004 continue;
5005 }
5006 }
5007 
5008 {
5009 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5010 vinfos[0].jointtype = 1;
5011 vinfos[0].foffset = j0;
5012 vinfos[0].indices[0] = _ij0[0];
5013 vinfos[0].indices[1] = _ij0[1];
5014 vinfos[0].maxsolutions = _nj0;
5015 vinfos[1].jointtype = 1;
5016 vinfos[1].foffset = j1;
5017 vinfos[1].indices[0] = _ij1[0];
5018 vinfos[1].indices[1] = _ij1[1];
5019 vinfos[1].maxsolutions = _nj1;
5020 vinfos[2].jointtype = 1;
5021 vinfos[2].foffset = j2;
5022 vinfos[2].indices[0] = _ij2[0];
5023 vinfos[2].indices[1] = _ij2[1];
5024 vinfos[2].maxsolutions = _nj2;
5025 vinfos[3].jointtype = 1;
5026 vinfos[3].foffset = j3;
5027 vinfos[3].indices[0] = _ij3[0];
5028 vinfos[3].indices[1] = _ij3[1];
5029 vinfos[3].maxsolutions = _nj3;
5030 vinfos[4].jointtype = 1;
5031 vinfos[4].foffset = j4;
5032 vinfos[4].indices[0] = _ij4[0];
5033 vinfos[4].indices[1] = _ij4[1];
5034 vinfos[4].maxsolutions = _nj4;
5035 vinfos[5].jointtype = 1;
5036 vinfos[5].foffset = j5;
5037 vinfos[5].indices[0] = _ij5[0];
5038 vinfos[5].indices[1] = _ij5[1];
5039 vinfos[5].maxsolutions = _nj5;
5040 std::vector<int> vfree(0);
5041 solutions.AddSolution(vinfos,vfree);
5042 }
5043 }
5044 }
5045 
5046 }
5047 
5048 }
5049 }
5050 }
5051 
5052 }
5053 
5054 }
5055 
5056 } else
5057 {
5058 {
5059 IkReal j5array[1], cj5array[1], sj5array[1];
5060 bool j5valid[1]={false};
5061 _nj5 = 1;
5062 CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5063 if(!x373.valid){
5064 continue;
5065 }
5066 CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5067 if(!x374.valid){
5068 continue;
5069 }
5070 j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5071 sj5array[0]=IKsin(j5array[0]);
5072 cj5array[0]=IKcos(j5array[0]);
5073 if( j5array[0] > IKPI )
5074 {
5075  j5array[0]-=IK2PI;
5076 }
5077 else if( j5array[0] < -IKPI )
5078 { j5array[0]+=IK2PI;
5079 }
5080 j5valid[0] = true;
5081 for(int ij5 = 0; ij5 < 1; ++ij5)
5082 {
5083 if( !j5valid[ij5] )
5084 {
5085  continue;
5086 }
5087 _ij5[0] = ij5; _ij5[1] = -1;
5088 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5089 {
5090 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5091 {
5092  j5valid[iij5]=false; _ij5[1] = iij5; break;
5093 }
5094 }
5095 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5096 {
5097 IkReal evalcond[2];
5098 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5099 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5100 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5101 {
5102 continue;
5103 }
5104 }
5105 
5106 {
5107 IkReal j3eval[3];
5108 j3eval[0]=sj4;
5109 j3eval[1]=IKsign(sj4);
5110 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5111 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5112 {
5113 {
5114 IkReal j3eval[2];
5115 j3eval[0]=new_r12;
5116 j3eval[1]=sj4;
5117 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5118 {
5119 {
5120 IkReal evalcond[5];
5121 bool bgotonextstatement = true;
5122 do
5123 {
5124 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5125 evalcond[1]=new_r02;
5126 evalcond[2]=new_r12;
5127 evalcond[3]=new_r21;
5128 evalcond[4]=new_r20;
5129 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
5130 {
5131 bgotonextstatement=false;
5132 {
5133 IkReal j3eval[3];
5134 sj4=0;
5135 cj4=1.0;
5136 j4=0;
5137 IkReal x375=((1.0)*new_r11);
5138 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5139 j3eval[0]=x376;
5140 j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5141 j3eval[2]=IKsign(x376);
5142 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5143 {
5144 {
5145 IkReal j3eval[3];
5146 sj4=0;
5147 cj4=1.0;
5148 j4=0;
5149 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5150 j3eval[0]=x377;
5151 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5152 j3eval[2]=IKsign(x377);
5153 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5154 {
5155 {
5156 IkReal j3eval[3];
5157 sj4=0;
5158 cj4=1.0;
5159 j4=0;
5160 IkReal x378=((1.0)*new_r11);
5161 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5162 j3eval[0]=x379;
5163 j3eval[1]=IKsign(x379);
5164 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5165 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5166 {
5167 {
5168 IkReal evalcond[1];
5169 bool bgotonextstatement = true;
5170 do
5171 {
5172 IkReal x380=((-1.0)*new_r01);
5173 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5174 if(IKabs(x382)==0){
5175 continue;
5176 }
5177 IkReal x381=pow(x382,-0.5);
5178 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5179 if(!x383.valid){
5180 continue;
5181 }
5182 IkReal gconst6=((-1.0)*(x383.value));
5183 IkReal gconst7=(new_r11*x381);
5184 IkReal gconst8=(x380*x381);
5185 CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5186 if(!x384.valid){
5187 continue;
5188 }
5189 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5190 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5191 {
5192 bgotonextstatement=false;
5193 {
5194 IkReal j3eval[3];
5195 IkReal x385=((-1.0)*new_r01);
5196 CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5197 if(!x388.valid){
5198 continue;
5199 }
5200 IkReal x386=((-1.0)*(x388.value));
5201 IkReal x387=x381;
5202 sj4=0;
5203 cj4=1.0;
5204 j4=0;
5205 sj5=gconst7;
5206 cj5=gconst8;
5207 j5=x386;
5208 IkReal gconst6=x386;
5209 IkReal gconst7=(new_r11*x387);
5210 IkReal gconst8=(x385*x387);
5211 IkReal x389=new_r11*new_r11;
5212 IkReal x390=(new_r10*new_r11);
5213 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5214 IkReal x392=x381;
5215 IkReal x393=(new_r11*x392);
5216 j3eval[0]=x391;
5217 j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5218 j3eval[2]=IKsign(x391);
5219 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5220 {
5221 {
5222 IkReal j3eval[1];
5223 IkReal x394=((-1.0)*new_r01);
5224 CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5225 if(!x397.valid){
5226 continue;
5227 }
5228 IkReal x395=((-1.0)*(x397.value));
5229 IkReal x396=x381;
5230 sj4=0;
5231 cj4=1.0;
5232 j4=0;
5233 sj5=gconst7;
5234 cj5=gconst8;
5235 j5=x395;
5236 IkReal gconst6=x395;
5237 IkReal gconst7=(new_r11*x396);
5238 IkReal gconst8=(x394*x396);
5239 IkReal x398=new_r11*new_r11;
5240 CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5241 if(!x401.valid){
5242 continue;
5243 }
5244 IkReal x399=x401.value;
5245 IkReal x400=(x398*x399);
5246 j3eval[0]=((IKabs((((new_r01*new_r10))+x400)))+(IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5247 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5248 {
5249 {
5250 IkReal j3eval[1];
5251 IkReal x402=((-1.0)*new_r01);
5252 CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5253 if(!x405.valid){
5254 continue;
5255 }
5256 IkReal x403=((-1.0)*(x405.value));
5257 IkReal x404=x381;
5258 sj4=0;
5259 cj4=1.0;
5260 j4=0;
5261 sj5=gconst7;
5262 cj5=gconst8;
5263 j5=x403;
5264 IkReal gconst6=x403;
5265 IkReal gconst7=(new_r11*x404);
5266 IkReal gconst8=(x402*x404);
5267 IkReal x406=new_r01*new_r01;
5268 IkReal x407=new_r11*new_r11;
5269 CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5270 if(!x414.valid){
5271 continue;
5272 }
5273 IkReal x408=x414.value;
5274 IkReal x409=(x407*x408);
5275 CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5276 if(!x415.valid){
5277 continue;
5278 }
5279 IkReal x410=x415.value;
5280 IkReal x411=((1.0)*x410);
5281 IkReal x412=(new_r11*x411);
5282 IkReal x413=(new_r01*x411);
5283 j3eval[0]=((IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5284 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5285 {
5286 {
5287 IkReal evalcond[2];
5288 bool bgotonextstatement = true;
5289 do
5290 {
5291 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5292 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5293 {
5294 bgotonextstatement=false;
5295 {
5296 IkReal j3array[2], cj3array[2], sj3array[2];
5297 bool j3valid[2]={false};
5298 _nj3 = 2;
5299 CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst8,-1);
5300 if(!x416.valid){
5301 continue;
5302 }
5303 sj3array[0]=(new_r10*(x416.value));
5304 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5305 {
5306  j3valid[0] = j3valid[1] = true;
5307  j3array[0] = IKasin(sj3array[0]);
5308  cj3array[0] = IKcos(j3array[0]);
5309  sj3array[1] = sj3array[0];
5310  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5311  cj3array[1] = -cj3array[0];
5312 }
5313 else if( isnan(sj3array[0]) )
5314 {
5315  // probably any value will work
5316  j3valid[0] = true;
5317  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5318 }
5319 for(int ij3 = 0; ij3 < 2; ++ij3)
5320 {
5321 if( !j3valid[ij3] )
5322 {
5323  continue;
5324 }
5325 _ij3[0] = ij3; _ij3[1] = -1;
5326 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5327 {
5328 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5329 {
5330  j3valid[iij3]=false; _ij3[1] = iij3; break;
5331 }
5332 }
5333 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5334 {
5335 IkReal evalcond[6];
5336 IkReal x417=IKcos(j3);
5337 IkReal x418=IKsin(j3);
5338 IkReal x419=((-1.0)*x417);
5339 evalcond[0]=(new_r01*x417);
5340 evalcond[1]=(new_r10*x419);
5341 evalcond[2]=(gconst8*x419);
5342 evalcond[3]=(gconst8+((new_r01*x418)));
5343 evalcond[4]=(((gconst8*x418))+new_r01);
5344 evalcond[5]=((((-1.0)*gconst8))+((new_r10*x418)));
5345 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5346 {
5347 continue;
5348 }
5349 }
5350 
5351 {
5352 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5353 vinfos[0].jointtype = 1;
5354 vinfos[0].foffset = j0;
5355 vinfos[0].indices[0] = _ij0[0];
5356 vinfos[0].indices[1] = _ij0[1];
5357 vinfos[0].maxsolutions = _nj0;
5358 vinfos[1].jointtype = 1;
5359 vinfos[1].foffset = j1;
5360 vinfos[1].indices[0] = _ij1[0];
5361 vinfos[1].indices[1] = _ij1[1];
5362 vinfos[1].maxsolutions = _nj1;
5363 vinfos[2].jointtype = 1;
5364 vinfos[2].foffset = j2;
5365 vinfos[2].indices[0] = _ij2[0];
5366 vinfos[2].indices[1] = _ij2[1];
5367 vinfos[2].maxsolutions = _nj2;
5368 vinfos[3].jointtype = 1;
5369 vinfos[3].foffset = j3;
5370 vinfos[3].indices[0] = _ij3[0];
5371 vinfos[3].indices[1] = _ij3[1];
5372 vinfos[3].maxsolutions = _nj3;
5373 vinfos[4].jointtype = 1;
5374 vinfos[4].foffset = j4;
5375 vinfos[4].indices[0] = _ij4[0];
5376 vinfos[4].indices[1] = _ij4[1];
5377 vinfos[4].maxsolutions = _nj4;
5378 vinfos[5].jointtype = 1;
5379 vinfos[5].foffset = j5;
5380 vinfos[5].indices[0] = _ij5[0];
5381 vinfos[5].indices[1] = _ij5[1];
5382 vinfos[5].maxsolutions = _nj5;
5383 std::vector<int> vfree(0);
5384 solutions.AddSolution(vinfos,vfree);
5385 }
5386 }
5387 }
5388 
5389 }
5390 } while(0);
5391 if( bgotonextstatement )
5392 {
5393 bool bgotonextstatement = true;
5394 do
5395 {
5396 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5397 evalcond[1]=gconst7;
5398 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5399 {
5400 bgotonextstatement=false;
5401 {
5402 IkReal j3eval[3];
5403 IkReal x420=((-1.0)*new_r01);
5404 CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5405 if(!x422.valid){
5406 continue;
5407 }
5408 IkReal x421=((-1.0)*(x422.value));
5409 sj4=0;
5410 cj4=1.0;
5411 j4=0;
5412 sj5=gconst7;
5413 cj5=gconst8;
5414 j5=x421;
5415 new_r00=0;
5416 new_r10=0;
5417 new_r21=0;
5418 new_r22=0;
5419 IkReal gconst6=x421;
5420 IkReal gconst7=new_r11;
5421 IkReal gconst8=x420;
5422 j3eval[0]=-1.0;
5423 j3eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5424 j3eval[2]=-1.0;
5425 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5426 {
5427 {
5428 IkReal j3eval[3];
5429 IkReal x423=((-1.0)*new_r01);
5430 CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5431 if(!x425.valid){
5432 continue;
5433 }
5434 IkReal x424=((-1.0)*(x425.value));
5435 sj4=0;
5436 cj4=1.0;
5437 j4=0;
5438 sj5=gconst7;
5439 cj5=gconst8;
5440 j5=x424;
5441 new_r00=0;
5442 new_r10=0;
5443 new_r21=0;
5444 new_r22=0;
5445 IkReal gconst6=x424;
5446 IkReal gconst7=new_r11;
5447 IkReal gconst8=x423;
5448 j3eval[0]=-1.0;
5449 j3eval[1]=-1.0;
5450 j3eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5451 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5452 {
5453 {
5454 IkReal j3eval[3];
5455 IkReal x426=((-1.0)*new_r01);
5456 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5457 if(!x428.valid){
5458 continue;
5459 }
5460 IkReal x427=((-1.0)*(x428.value));
5461 sj4=0;
5462 cj4=1.0;
5463 j4=0;
5464 sj5=gconst7;
5465 cj5=gconst8;
5466 j5=x427;
5467 new_r00=0;
5468 new_r10=0;
5469 new_r21=0;
5470 new_r22=0;
5471 IkReal gconst6=x427;
5472 IkReal gconst7=new_r11;
5473 IkReal gconst8=x426;
5474 j3eval[0]=1.0;
5475 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5476 j3eval[2]=1.0;
5477 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5478 {
5479 continue; // 3 cases reached
5480 
5481 } else
5482 {
5483 {
5484 IkReal j3array[1], cj3array[1], sj3array[1];
5485 bool j3valid[1]={false};
5486 _nj3 = 1;
5487 IkReal x429=((1.0)*new_r01);
5488 CheckValue<IkReal> x430=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*x429))+((gconst7*new_r11)))),-1);
5489 if(!x430.valid){
5490 continue;
5491 }
5492 CheckValue<IkReal> x431 = IKatan2WithCheck(IkReal(((((-1.0)*(gconst7*gconst7)))+(new_r01*new_r01))),IkReal(((((-1.0)*new_r11*x429))+((gconst7*gconst8)))),IKFAST_ATAN2_MAGTHRESH);
5493 if(!x431.valid){
5494 continue;
5495 }
5496 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x430.value)))+(x431.value));
5497 sj3array[0]=IKsin(j3array[0]);
5498 cj3array[0]=IKcos(j3array[0]);
5499 if( j3array[0] > IKPI )
5500 {
5501  j3array[0]-=IK2PI;
5502 }
5503 else if( j3array[0] < -IKPI )
5504 { j3array[0]+=IK2PI;
5505 }
5506 j3valid[0] = true;
5507 for(int ij3 = 0; ij3 < 1; ++ij3)
5508 {
5509 if( !j3valid[ij3] )
5510 {
5511  continue;
5512 }
5513 _ij3[0] = ij3; _ij3[1] = -1;
5514 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5515 {
5516 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5517 {
5518  j3valid[iij3]=false; _ij3[1] = iij3; break;
5519 }
5520 }
5521 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5522 {
5523 IkReal evalcond[6];
5524 IkReal x432=IKsin(j3);
5525 IkReal x433=IKcos(j3);
5526 IkReal x434=(gconst7*x432);
5527 IkReal x435=((1.0)*x433);
5528 IkReal x436=(gconst8*x432);
5529 IkReal x437=(gconst8*x435);
5530 evalcond[0]=(((new_r01*x433))+gconst7+((new_r11*x432)));
5531 evalcond[1]=(((gconst7*x433))+x436+new_r01);
5532 evalcond[2]=((((-1.0)*x437))+x434);
5533 evalcond[3]=(((new_r01*x432))+gconst8+(((-1.0)*new_r11*x435)));
5534 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5535 evalcond[5]=((((-1.0)*x436))+(((-1.0)*gconst7*x435)));
5536 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5537 {
5538 continue;
5539 }
5540 }
5541 
5542 {
5543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5544 vinfos[0].jointtype = 1;
5545 vinfos[0].foffset = j0;
5546 vinfos[0].indices[0] = _ij0[0];
5547 vinfos[0].indices[1] = _ij0[1];
5548 vinfos[0].maxsolutions = _nj0;
5549 vinfos[1].jointtype = 1;
5550 vinfos[1].foffset = j1;
5551 vinfos[1].indices[0] = _ij1[0];
5552 vinfos[1].indices[1] = _ij1[1];
5553 vinfos[1].maxsolutions = _nj1;
5554 vinfos[2].jointtype = 1;
5555 vinfos[2].foffset = j2;
5556 vinfos[2].indices[0] = _ij2[0];
5557 vinfos[2].indices[1] = _ij2[1];
5558 vinfos[2].maxsolutions = _nj2;
5559 vinfos[3].jointtype = 1;
5560 vinfos[3].foffset = j3;
5561 vinfos[3].indices[0] = _ij3[0];
5562 vinfos[3].indices[1] = _ij3[1];
5563 vinfos[3].maxsolutions = _nj3;
5564 vinfos[4].jointtype = 1;
5565 vinfos[4].foffset = j4;
5566 vinfos[4].indices[0] = _ij4[0];
5567 vinfos[4].indices[1] = _ij4[1];
5568 vinfos[4].maxsolutions = _nj4;
5569 vinfos[5].jointtype = 1;
5570 vinfos[5].foffset = j5;
5571 vinfos[5].indices[0] = _ij5[0];
5572 vinfos[5].indices[1] = _ij5[1];
5573 vinfos[5].maxsolutions = _nj5;
5574 std::vector<int> vfree(0);
5575 solutions.AddSolution(vinfos,vfree);
5576 }
5577 }
5578 }
5579 
5580 }
5581 
5582 }
5583 
5584 } else
5585 {
5586 {
5587 IkReal j3array[1], cj3array[1], sj3array[1];
5588 bool j3valid[1]={false};
5589 _nj3 = 1;
5590 CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
5591 if(!x438.valid){
5592 continue;
5593 }
5594 CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst8*new_r01)),IkReal((gconst7*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5595 if(!x439.valid){
5596 continue;
5597 }
5598 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5599 sj3array[0]=IKsin(j3array[0]);
5600 cj3array[0]=IKcos(j3array[0]);
5601 if( j3array[0] > IKPI )
5602 {
5603  j3array[0]-=IK2PI;
5604 }
5605 else if( j3array[0] < -IKPI )
5606 { j3array[0]+=IK2PI;
5607 }
5608 j3valid[0] = true;
5609 for(int ij3 = 0; ij3 < 1; ++ij3)
5610 {
5611 if( !j3valid[ij3] )
5612 {
5613  continue;
5614 }
5615 _ij3[0] = ij3; _ij3[1] = -1;
5616 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5617 {
5618 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5619 {
5620  j3valid[iij3]=false; _ij3[1] = iij3; break;
5621 }
5622 }
5623 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5624 {
5625 IkReal evalcond[6];
5626 IkReal x440=IKsin(j3);
5627 IkReal x441=IKcos(j3);
5628 IkReal x442=(gconst7*x440);
5629 IkReal x443=((1.0)*x441);
5630 IkReal x444=(gconst8*x440);
5631 IkReal x445=(gconst8*x443);
5632 evalcond[0]=(((new_r01*x441))+gconst7+((new_r11*x440)));
5633 evalcond[1]=(((gconst7*x441))+x444+new_r01);
5634 evalcond[2]=((((-1.0)*x445))+x442);
5635 evalcond[3]=(((new_r01*x440))+gconst8+(((-1.0)*new_r11*x443)));
5636 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5637 evalcond[5]=((((-1.0)*x444))+(((-1.0)*gconst7*x443)));
5638 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5639 {
5640 continue;
5641 }
5642 }
5643 
5644 {
5645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5646 vinfos[0].jointtype = 1;
5647 vinfos[0].foffset = j0;
5648 vinfos[0].indices[0] = _ij0[0];
5649 vinfos[0].indices[1] = _ij0[1];
5650 vinfos[0].maxsolutions = _nj0;
5651 vinfos[1].jointtype = 1;
5652 vinfos[1].foffset = j1;
5653 vinfos[1].indices[0] = _ij1[0];
5654 vinfos[1].indices[1] = _ij1[1];
5655 vinfos[1].maxsolutions = _nj1;
5656 vinfos[2].jointtype = 1;
5657 vinfos[2].foffset = j2;
5658 vinfos[2].indices[0] = _ij2[0];
5659 vinfos[2].indices[1] = _ij2[1];
5660 vinfos[2].maxsolutions = _nj2;
5661 vinfos[3].jointtype = 1;
5662 vinfos[3].foffset = j3;
5663 vinfos[3].indices[0] = _ij3[0];
5664 vinfos[3].indices[1] = _ij3[1];
5665 vinfos[3].maxsolutions = _nj3;
5666 vinfos[4].jointtype = 1;
5667 vinfos[4].foffset = j4;
5668 vinfos[4].indices[0] = _ij4[0];
5669 vinfos[4].indices[1] = _ij4[1];
5670 vinfos[4].maxsolutions = _nj4;
5671 vinfos[5].jointtype = 1;
5672 vinfos[5].foffset = j5;
5673 vinfos[5].indices[0] = _ij5[0];
5674 vinfos[5].indices[1] = _ij5[1];
5675 vinfos[5].maxsolutions = _nj5;
5676 std::vector<int> vfree(0);
5677 solutions.AddSolution(vinfos,vfree);
5678 }
5679 }
5680 }
5681 
5682 }
5683 
5684 }
5685 
5686 } else
5687 {
5688 {
5689 IkReal j3array[1], cj3array[1], sj3array[1];
5690 bool j3valid[1]={false};
5691 _nj3 = 1;
5692 CheckValue<IkReal> x446=IKPowWithIntegerCheck(IKsign((((gconst8*new_r01))+(((-1.0)*gconst7*new_r11)))),-1);
5693 if(!x446.valid){
5694 continue;
5695 }
5696 CheckValue<IkReal> x447 = IKatan2WithCheck(IkReal(gconst7*gconst7),IkReal(((-1.0)*gconst7*gconst8)),IKFAST_ATAN2_MAGTHRESH);
5697 if(!x447.valid){
5698 continue;
5699 }
5700 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x446.value)))+(x447.value));
5701 sj3array[0]=IKsin(j3array[0]);
5702 cj3array[0]=IKcos(j3array[0]);
5703 if( j3array[0] > IKPI )
5704 {
5705  j3array[0]-=IK2PI;
5706 }
5707 else if( j3array[0] < -IKPI )
5708 { j3array[0]+=IK2PI;
5709 }
5710 j3valid[0] = true;
5711 for(int ij3 = 0; ij3 < 1; ++ij3)
5712 {
5713 if( !j3valid[ij3] )
5714 {
5715  continue;
5716 }
5717 _ij3[0] = ij3; _ij3[1] = -1;
5718 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5719 {
5720 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5721 {
5722  j3valid[iij3]=false; _ij3[1] = iij3; break;
5723 }
5724 }
5725 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5726 {
5727 IkReal evalcond[6];
5728 IkReal x448=IKsin(j3);
5729 IkReal x449=IKcos(j3);
5730 IkReal x450=(gconst7*x448);
5731 IkReal x451=((1.0)*x449);
5732 IkReal x452=(gconst8*x448);
5733 IkReal x453=(gconst8*x451);
5734 evalcond[0]=(((new_r01*x449))+gconst7+((new_r11*x448)));
5735 evalcond[1]=(((gconst7*x449))+x452+new_r01);
5736 evalcond[2]=((((-1.0)*x453))+x450);
5737 evalcond[3]=(((new_r01*x448))+gconst8+(((-1.0)*new_r11*x451)));
5738 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5739 evalcond[5]=((((-1.0)*x452))+(((-1.0)*gconst7*x451)));
5740 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5741 {
5742 continue;
5743 }
5744 }
5745 
5746 {
5747 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5748 vinfos[0].jointtype = 1;
5749 vinfos[0].foffset = j0;
5750 vinfos[0].indices[0] = _ij0[0];
5751 vinfos[0].indices[1] = _ij0[1];
5752 vinfos[0].maxsolutions = _nj0;
5753 vinfos[1].jointtype = 1;
5754 vinfos[1].foffset = j1;
5755 vinfos[1].indices[0] = _ij1[0];
5756 vinfos[1].indices[1] = _ij1[1];
5757 vinfos[1].maxsolutions = _nj1;
5758 vinfos[2].jointtype = 1;
5759 vinfos[2].foffset = j2;
5760 vinfos[2].indices[0] = _ij2[0];
5761 vinfos[2].indices[1] = _ij2[1];
5762 vinfos[2].maxsolutions = _nj2;
5763 vinfos[3].jointtype = 1;
5764 vinfos[3].foffset = j3;
5765 vinfos[3].indices[0] = _ij3[0];
5766 vinfos[3].indices[1] = _ij3[1];
5767 vinfos[3].maxsolutions = _nj3;
5768 vinfos[4].jointtype = 1;
5769 vinfos[4].foffset = j4;
5770 vinfos[4].indices[0] = _ij4[0];
5771 vinfos[4].indices[1] = _ij4[1];
5772 vinfos[4].maxsolutions = _nj4;
5773 vinfos[5].jointtype = 1;
5774 vinfos[5].foffset = j5;
5775 vinfos[5].indices[0] = _ij5[0];
5776 vinfos[5].indices[1] = _ij5[1];
5777 vinfos[5].maxsolutions = _nj5;
5778 std::vector<int> vfree(0);
5779 solutions.AddSolution(vinfos,vfree);
5780 }
5781 }
5782 }
5783 
5784 }
5785 
5786 }
5787 
5788 }
5789 } while(0);
5790 if( bgotonextstatement )
5791 {
5792 bool bgotonextstatement = true;
5793 do
5794 {
5795 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5796 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5797 {
5798 bgotonextstatement=false;
5799 {
5800 IkReal j3eval[1];
5801 CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5802 if(!x455.valid){
5803 continue;
5804 }
5805 IkReal x454=((-1.0)*(x455.value));
5806 sj4=0;
5807 cj4=1.0;
5808 j4=0;
5809 sj5=gconst7;
5810 cj5=gconst8;
5811 j5=x454;
5812 new_r01=0;
5813 new_r10=0;
5814 IkReal gconst6=x454;
5815 IkReal x456 = new_r11*new_r11;
5816 if(IKabs(x456)==0){
5817 continue;
5818 }
5819 IkReal gconst7=(new_r11*(pow(x456,-0.5)));
5820 IkReal gconst8=0;
5821 j3eval[0]=new_r00;
5822 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5823 {
5824 {
5825 IkReal j3eval[1];
5826 CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5827 if(!x458.valid){
5828 continue;
5829 }
5830 IkReal x457=((-1.0)*(x458.value));
5831 sj4=0;
5832 cj4=1.0;
5833 j4=0;
5834 sj5=gconst7;
5835 cj5=gconst8;
5836 j5=x457;
5837 new_r01=0;
5838 new_r10=0;
5839 IkReal gconst6=x457;
5840 IkReal x459 = new_r11*new_r11;
5841 if(IKabs(x459)==0){
5842 continue;
5843 }
5844 IkReal gconst7=(new_r11*(pow(x459,-0.5)));
5845 IkReal gconst8=0;
5846 j3eval[0]=new_r11;
5847 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5848 {
5849 {
5850 IkReal j3array[2], cj3array[2], sj3array[2];
5851 bool j3valid[2]={false};
5852 _nj3 = 2;
5853 CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst7,-1);
5854 if(!x460.valid){
5855 continue;
5856 }
5857 sj3array[0]=((-1.0)*new_r00*(x460.value));
5858 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5859 {
5860  j3valid[0] = j3valid[1] = true;
5861  j3array[0] = IKasin(sj3array[0]);
5862  cj3array[0] = IKcos(j3array[0]);
5863  sj3array[1] = sj3array[0];
5864  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5865  cj3array[1] = -cj3array[0];
5866 }
5867 else if( isnan(sj3array[0]) )
5868 {
5869  // probably any value will work
5870  j3valid[0] = true;
5871  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5872 }
5873 for(int ij3 = 0; ij3 < 2; ++ij3)
5874 {
5875 if( !j3valid[ij3] )
5876 {
5877  continue;
5878 }
5879 _ij3[0] = ij3; _ij3[1] = -1;
5880 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5881 {
5882 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5883 {
5884  j3valid[iij3]=false; _ij3[1] = iij3; break;
5885 }
5886 }
5887 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5888 {
5889 IkReal evalcond[6];
5890 IkReal x461=IKcos(j3);
5891 IkReal x462=IKsin(j3);
5892 evalcond[0]=(gconst7*x461);
5893 evalcond[1]=(new_r00*x461);
5894 evalcond[2]=((-1.0)*new_r11*x461);
5895 evalcond[3]=(((new_r00*x462))+gconst7);
5896 evalcond[4]=(((new_r11*x462))+gconst7);
5897 evalcond[5]=(((gconst7*x462))+new_r11);
5898 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5899 {
5900 continue;
5901 }
5902 }
5903 
5904 {
5905 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5906 vinfos[0].jointtype = 1;
5907 vinfos[0].foffset = j0;
5908 vinfos[0].indices[0] = _ij0[0];
5909 vinfos[0].indices[1] = _ij0[1];
5910 vinfos[0].maxsolutions = _nj0;
5911 vinfos[1].jointtype = 1;
5912 vinfos[1].foffset = j1;
5913 vinfos[1].indices[0] = _ij1[0];
5914 vinfos[1].indices[1] = _ij1[1];
5915 vinfos[1].maxsolutions = _nj1;
5916 vinfos[2].jointtype = 1;
5917 vinfos[2].foffset = j2;
5918 vinfos[2].indices[0] = _ij2[0];
5919 vinfos[2].indices[1] = _ij2[1];
5920 vinfos[2].maxsolutions = _nj2;
5921 vinfos[3].jointtype = 1;
5922 vinfos[3].foffset = j3;
5923 vinfos[3].indices[0] = _ij3[0];
5924 vinfos[3].indices[1] = _ij3[1];
5925 vinfos[3].maxsolutions = _nj3;
5926 vinfos[4].jointtype = 1;
5927 vinfos[4].foffset = j4;
5928 vinfos[4].indices[0] = _ij4[0];
5929 vinfos[4].indices[1] = _ij4[1];
5930 vinfos[4].maxsolutions = _nj4;
5931 vinfos[5].jointtype = 1;
5932 vinfos[5].foffset = j5;
5933 vinfos[5].indices[0] = _ij5[0];
5934 vinfos[5].indices[1] = _ij5[1];
5935 vinfos[5].maxsolutions = _nj5;
5936 std::vector<int> vfree(0);
5937 solutions.AddSolution(vinfos,vfree);
5938 }
5939 }
5940 }
5941 
5942 } else
5943 {
5944 {
5945 IkReal j3array[2], cj3array[2], sj3array[2];
5946 bool j3valid[2]={false};
5947 _nj3 = 2;
5948 CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
5949 if(!x463.valid){
5950 continue;
5951 }
5952 sj3array[0]=((-1.0)*gconst7*(x463.value));
5953 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5954 {
5955  j3valid[0] = j3valid[1] = true;
5956  j3array[0] = IKasin(sj3array[0]);
5957  cj3array[0] = IKcos(j3array[0]);
5958  sj3array[1] = sj3array[0];
5959  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5960  cj3array[1] = -cj3array[0];
5961 }
5962 else if( isnan(sj3array[0]) )
5963 {
5964  // probably any value will work
5965  j3valid[0] = true;
5966  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5967 }
5968 for(int ij3 = 0; ij3 < 2; ++ij3)
5969 {
5970 if( !j3valid[ij3] )
5971 {
5972  continue;
5973 }
5974 _ij3[0] = ij3; _ij3[1] = -1;
5975 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5976 {
5977 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5978 {
5979  j3valid[iij3]=false; _ij3[1] = iij3; break;
5980 }
5981 }
5982 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5983 {
5984 IkReal evalcond[6];
5985 IkReal x464=IKcos(j3);
5986 IkReal x465=IKsin(j3);
5987 IkReal x466=(gconst7*x465);
5988 evalcond[0]=(gconst7*x464);
5989 evalcond[1]=(new_r00*x464);
5990 evalcond[2]=((-1.0)*new_r11*x464);
5991 evalcond[3]=(((new_r00*x465))+gconst7);
5992 evalcond[4]=(x466+new_r00);
5993 evalcond[5]=(x466+new_r11);
5994 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5995 {
5996 continue;
5997 }
5998 }
5999 
6000 {
6001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6002 vinfos[0].jointtype = 1;
6003 vinfos[0].foffset = j0;
6004 vinfos[0].indices[0] = _ij0[0];
6005 vinfos[0].indices[1] = _ij0[1];
6006 vinfos[0].maxsolutions = _nj0;
6007 vinfos[1].jointtype = 1;
6008 vinfos[1].foffset = j1;
6009 vinfos[1].indices[0] = _ij1[0];
6010 vinfos[1].indices[1] = _ij1[1];
6011 vinfos[1].maxsolutions = _nj1;
6012 vinfos[2].jointtype = 1;
6013 vinfos[2].foffset = j2;
6014 vinfos[2].indices[0] = _ij2[0];
6015 vinfos[2].indices[1] = _ij2[1];
6016 vinfos[2].maxsolutions = _nj2;
6017 vinfos[3].jointtype = 1;
6018 vinfos[3].foffset = j3;
6019 vinfos[3].indices[0] = _ij3[0];
6020 vinfos[3].indices[1] = _ij3[1];
6021 vinfos[3].maxsolutions = _nj3;
6022 vinfos[4].jointtype = 1;
6023 vinfos[4].foffset = j4;
6024 vinfos[4].indices[0] = _ij4[0];
6025 vinfos[4].indices[1] = _ij4[1];
6026 vinfos[4].maxsolutions = _nj4;
6027 vinfos[5].jointtype = 1;
6028 vinfos[5].foffset = j5;
6029 vinfos[5].indices[0] = _ij5[0];
6030 vinfos[5].indices[1] = _ij5[1];
6031 vinfos[5].maxsolutions = _nj5;
6032 std::vector<int> vfree(0);
6033 solutions.AddSolution(vinfos,vfree);
6034 }
6035 }
6036 }
6037 
6038 }
6039 
6040 }
6041 
6042 } else
6043 {
6044 {
6045 IkReal j3array[2], cj3array[2], sj3array[2];
6046 bool j3valid[2]={false};
6047 _nj3 = 2;
6048 CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6049 if(!x467.valid){
6050 continue;
6051 }
6052 sj3array[0]=((-1.0)*gconst7*(x467.value));
6053 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6054 {
6055  j3valid[0] = j3valid[1] = true;
6056  j3array[0] = IKasin(sj3array[0]);
6057  cj3array[0] = IKcos(j3array[0]);
6058  sj3array[1] = sj3array[0];
6059  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6060  cj3array[1] = -cj3array[0];
6061 }
6062 else if( isnan(sj3array[0]) )
6063 {
6064  // probably any value will work
6065  j3valid[0] = true;
6066  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6067 }
6068 for(int ij3 = 0; ij3 < 2; ++ij3)
6069 {
6070 if( !j3valid[ij3] )
6071 {
6072  continue;
6073 }
6074 _ij3[0] = ij3; _ij3[1] = -1;
6075 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6076 {
6077 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6078 {
6079  j3valid[iij3]=false; _ij3[1] = iij3; break;
6080 }
6081 }
6082 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6083 {
6084 IkReal evalcond[6];
6085 IkReal x468=IKcos(j3);
6086 IkReal x469=IKsin(j3);
6087 IkReal x470=(gconst7*x469);
6088 evalcond[0]=(gconst7*x468);
6089 evalcond[1]=(new_r00*x468);
6090 evalcond[2]=((-1.0)*new_r11*x468);
6091 evalcond[3]=(((new_r11*x469))+gconst7);
6092 evalcond[4]=(x470+new_r00);
6093 evalcond[5]=(x470+new_r11);
6094 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6095 {
6096 continue;
6097 }
6098 }
6099 
6100 {
6101 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6102 vinfos[0].jointtype = 1;
6103 vinfos[0].foffset = j0;
6104 vinfos[0].indices[0] = _ij0[0];
6105 vinfos[0].indices[1] = _ij0[1];
6106 vinfos[0].maxsolutions = _nj0;
6107 vinfos[1].jointtype = 1;
6108 vinfos[1].foffset = j1;
6109 vinfos[1].indices[0] = _ij1[0];
6110 vinfos[1].indices[1] = _ij1[1];
6111 vinfos[1].maxsolutions = _nj1;
6112 vinfos[2].jointtype = 1;
6113 vinfos[2].foffset = j2;
6114 vinfos[2].indices[0] = _ij2[0];
6115 vinfos[2].indices[1] = _ij2[1];
6116 vinfos[2].maxsolutions = _nj2;
6117 vinfos[3].jointtype = 1;
6118 vinfos[3].foffset = j3;
6119 vinfos[3].indices[0] = _ij3[0];
6120 vinfos[3].indices[1] = _ij3[1];
6121 vinfos[3].maxsolutions = _nj3;
6122 vinfos[4].jointtype = 1;
6123 vinfos[4].foffset = j4;
6124 vinfos[4].indices[0] = _ij4[0];
6125 vinfos[4].indices[1] = _ij4[1];
6126 vinfos[4].maxsolutions = _nj4;
6127 vinfos[5].jointtype = 1;
6128 vinfos[5].foffset = j5;
6129 vinfos[5].indices[0] = _ij5[0];
6130 vinfos[5].indices[1] = _ij5[1];
6131 vinfos[5].maxsolutions = _nj5;
6132 std::vector<int> vfree(0);
6133 solutions.AddSolution(vinfos,vfree);
6134 }
6135 }
6136 }
6137 
6138 }
6139 
6140 }
6141 
6142 }
6143 } while(0);
6144 if( bgotonextstatement )
6145 {
6146 bool bgotonextstatement = true;
6147 do
6148 {
6149 evalcond[0]=IKabs(new_r11);
6150 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6151 {
6152 bgotonextstatement=false;
6153 {
6154 IkReal j3eval[1];
6155 IkReal x471=((-1.0)*new_r01);
6156 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6157 if(!x473.valid){
6158 continue;
6159 }
6160 IkReal x472=((-1.0)*(x473.value));
6161 sj4=0;
6162 cj4=1.0;
6163 j4=0;
6164 sj5=gconst7;
6165 cj5=gconst8;
6166 j5=x472;
6167 new_r11=0;
6168 IkReal gconst6=x472;
6169 IkReal gconst7=0;
6170 IkReal x474 = new_r01*new_r01;
6171 if(IKabs(x474)==0){
6172 continue;
6173 }
6174 IkReal gconst8=(x471*(pow(x474,-0.5)));
6175 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6176 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6177 {
6178 {
6179 IkReal j3eval[1];
6180 IkReal x475=((-1.0)*new_r01);
6181 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6182 if(!x477.valid){
6183 continue;
6184 }
6185 IkReal x476=((-1.0)*(x477.value));
6186 sj4=0;
6187 cj4=1.0;
6188 j4=0;
6189 sj5=gconst7;
6190 cj5=gconst8;
6191 j5=x476;
6192 new_r11=0;
6193 IkReal gconst6=x476;
6194 IkReal gconst7=0;
6195 IkReal x478 = new_r01*new_r01;
6196 if(IKabs(x478)==0){
6197 continue;
6198 }
6199 IkReal gconst8=(x475*(pow(x478,-0.5)));
6200 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6201 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6202 {
6203 {
6204 IkReal j3eval[1];
6205 IkReal x479=((-1.0)*new_r01);
6206 CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal(0),IkReal(x479),IKFAST_ATAN2_MAGTHRESH);
6207 if(!x481.valid){
6208 continue;
6209 }
6210 IkReal x480=((-1.0)*(x481.value));
6211 sj4=0;
6212 cj4=1.0;
6213 j4=0;
6214 sj5=gconst7;
6215 cj5=gconst8;
6216 j5=x480;
6217 new_r11=0;
6218 IkReal gconst6=x480;
6219 IkReal gconst7=0;
6220 IkReal x482 = new_r01*new_r01;
6221 if(IKabs(x482)==0){
6222 continue;
6223 }
6224 IkReal gconst8=(x479*(pow(x482,-0.5)));
6225 j3eval[0]=new_r01;
6226 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6227 {
6228 continue; // 3 cases reached
6229 
6230 } else
6231 {
6232 {
6233 IkReal j3array[1], cj3array[1], sj3array[1];
6234 bool j3valid[1]={false};
6235 _nj3 = 1;
6236 CheckValue<IkReal> x483=IKPowWithIntegerCheck(new_r01,-1);
6237 if(!x483.valid){
6238 continue;
6239 }
6240 CheckValue<IkReal> x484=IKPowWithIntegerCheck(gconst8,-1);
6241 if(!x484.valid){
6242 continue;
6243 }
6244 if( IKabs(((-1.0)*gconst8*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x484.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst8*(x483.value)))+IKsqr((new_r00*(x484.value)))-1) <= IKFAST_SINCOS_THRESH )
6245  continue;
6246 j3array[0]=IKatan2(((-1.0)*gconst8*(x483.value)), (new_r00*(x484.value)));
6247 sj3array[0]=IKsin(j3array[0]);
6248 cj3array[0]=IKcos(j3array[0]);
6249 if( j3array[0] > IKPI )
6250 {
6251  j3array[0]-=IK2PI;
6252 }
6253 else if( j3array[0] < -IKPI )
6254 { j3array[0]+=IK2PI;
6255 }
6256 j3valid[0] = true;
6257 for(int ij3 = 0; ij3 < 1; ++ij3)
6258 {
6259 if( !j3valid[ij3] )
6260 {
6261  continue;
6262 }
6263 _ij3[0] = ij3; _ij3[1] = -1;
6264 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6265 {
6266 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6267 {
6268  j3valid[iij3]=false; _ij3[1] = iij3; break;
6269 }
6270 }
6271 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6272 {
6273 IkReal evalcond[8];
6274 IkReal x485=IKcos(j3);
6275 IkReal x486=IKsin(j3);
6276 IkReal x487=(gconst8*x486);
6277 IkReal x488=(gconst8*x485);
6278 evalcond[0]=(new_r01*x485);
6279 evalcond[1]=((-1.0)*x488);
6280 evalcond[2]=(gconst8+((new_r01*x486)));
6281 evalcond[3]=(x487+new_r01);
6282 evalcond[4]=((((-1.0)*x488))+new_r00);
6283 evalcond[5]=((((-1.0)*x487))+new_r10);
6284 evalcond[6]=((((-1.0)*new_r10*x485))+((new_r00*x486)));
6285 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x486))+((new_r00*x485)));
6286 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6287 {
6288 continue;
6289 }
6290 }
6291 
6292 {
6293 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6294 vinfos[0].jointtype = 1;
6295 vinfos[0].foffset = j0;
6296 vinfos[0].indices[0] = _ij0[0];
6297 vinfos[0].indices[1] = _ij0[1];
6298 vinfos[0].maxsolutions = _nj0;
6299 vinfos[1].jointtype = 1;
6300 vinfos[1].foffset = j1;
6301 vinfos[1].indices[0] = _ij1[0];
6302 vinfos[1].indices[1] = _ij1[1];
6303 vinfos[1].maxsolutions = _nj1;
6304 vinfos[2].jointtype = 1;
6305 vinfos[2].foffset = j2;
6306 vinfos[2].indices[0] = _ij2[0];
6307 vinfos[2].indices[1] = _ij2[1];
6308 vinfos[2].maxsolutions = _nj2;
6309 vinfos[3].jointtype = 1;
6310 vinfos[3].foffset = j3;
6311 vinfos[3].indices[0] = _ij3[0];
6312 vinfos[3].indices[1] = _ij3[1];
6313 vinfos[3].maxsolutions = _nj3;
6314 vinfos[4].jointtype = 1;
6315 vinfos[4].foffset = j4;
6316 vinfos[4].indices[0] = _ij4[0];
6317 vinfos[4].indices[1] = _ij4[1];
6318 vinfos[4].maxsolutions = _nj4;
6319 vinfos[5].jointtype = 1;
6320 vinfos[5].foffset = j5;
6321 vinfos[5].indices[0] = _ij5[0];
6322 vinfos[5].indices[1] = _ij5[1];
6323 vinfos[5].maxsolutions = _nj5;
6324 std::vector<int> vfree(0);
6325 solutions.AddSolution(vinfos,vfree);
6326 }
6327 }
6328 }
6329 
6330 }
6331 
6332 }
6333 
6334 } else
6335 {
6336 {
6337 IkReal j3array[1], cj3array[1], sj3array[1];
6338 bool j3valid[1]={false};
6339 _nj3 = 1;
6340 CheckValue<IkReal> x489 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6341 if(!x489.valid){
6342 continue;
6343 }
6344 CheckValue<IkReal> x490=IKPowWithIntegerCheck(IKsign(gconst8),-1);
6345 if(!x490.valid){
6346 continue;
6347 }
6348 j3array[0]=((-1.5707963267949)+(x489.value)+(((1.5707963267949)*(x490.value))));
6349 sj3array[0]=IKsin(j3array[0]);
6350 cj3array[0]=IKcos(j3array[0]);
6351 if( j3array[0] > IKPI )
6352 {
6353  j3array[0]-=IK2PI;
6354 }
6355 else if( j3array[0] < -IKPI )
6356 { j3array[0]+=IK2PI;
6357 }
6358 j3valid[0] = true;
6359 for(int ij3 = 0; ij3 < 1; ++ij3)
6360 {
6361 if( !j3valid[ij3] )
6362 {
6363  continue;
6364 }
6365 _ij3[0] = ij3; _ij3[1] = -1;
6366 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6367 {
6368 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6369 {
6370  j3valid[iij3]=false; _ij3[1] = iij3; break;
6371 }
6372 }
6373 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6374 {
6375 IkReal evalcond[8];
6376 IkReal x491=IKcos(j3);
6377 IkReal x492=IKsin(j3);
6378 IkReal x493=(gconst8*x492);
6379 IkReal x494=(gconst8*x491);
6380 evalcond[0]=(new_r01*x491);
6381 evalcond[1]=((-1.0)*x494);
6382 evalcond[2]=(gconst8+((new_r01*x492)));
6383 evalcond[3]=(x493+new_r01);
6384 evalcond[4]=((((-1.0)*x494))+new_r00);
6385 evalcond[5]=((((-1.0)*x493))+new_r10);
6386 evalcond[6]=((((-1.0)*new_r10*x491))+((new_r00*x492)));
6387 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x492))+((new_r00*x491)));
6388 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6389 {
6390 continue;
6391 }
6392 }
6393 
6394 {
6395 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6396 vinfos[0].jointtype = 1;
6397 vinfos[0].foffset = j0;
6398 vinfos[0].indices[0] = _ij0[0];
6399 vinfos[0].indices[1] = _ij0[1];
6400 vinfos[0].maxsolutions = _nj0;
6401 vinfos[1].jointtype = 1;
6402 vinfos[1].foffset = j1;
6403 vinfos[1].indices[0] = _ij1[0];
6404 vinfos[1].indices[1] = _ij1[1];
6405 vinfos[1].maxsolutions = _nj1;
6406 vinfos[2].jointtype = 1;
6407 vinfos[2].foffset = j2;
6408 vinfos[2].indices[0] = _ij2[0];
6409 vinfos[2].indices[1] = _ij2[1];
6410 vinfos[2].maxsolutions = _nj2;
6411 vinfos[3].jointtype = 1;
6412 vinfos[3].foffset = j3;
6413 vinfos[3].indices[0] = _ij3[0];
6414 vinfos[3].indices[1] = _ij3[1];
6415 vinfos[3].maxsolutions = _nj3;
6416 vinfos[4].jointtype = 1;
6417 vinfos[4].foffset = j4;
6418 vinfos[4].indices[0] = _ij4[0];
6419 vinfos[4].indices[1] = _ij4[1];
6420 vinfos[4].maxsolutions = _nj4;
6421 vinfos[5].jointtype = 1;
6422 vinfos[5].foffset = j5;
6423 vinfos[5].indices[0] = _ij5[0];
6424 vinfos[5].indices[1] = _ij5[1];
6425 vinfos[5].maxsolutions = _nj5;
6426 std::vector<int> vfree(0);
6427 solutions.AddSolution(vinfos,vfree);
6428 }
6429 }
6430 }
6431 
6432 }
6433 
6434 }
6435 
6436 } else
6437 {
6438 {
6439 IkReal j3array[1], cj3array[1], sj3array[1];
6440 bool j3valid[1]={false};
6441 _nj3 = 1;
6442 CheckValue<IkReal> x495=IKPowWithIntegerCheck(IKsign(gconst8),-1);
6443 if(!x495.valid){
6444 continue;
6445 }
6446 CheckValue<IkReal> x496 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6447 if(!x496.valid){
6448 continue;
6449 }
6450 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x495.value)))+(x496.value));
6451 sj3array[0]=IKsin(j3array[0]);
6452 cj3array[0]=IKcos(j3array[0]);
6453 if( j3array[0] > IKPI )
6454 {
6455  j3array[0]-=IK2PI;
6456 }
6457 else if( j3array[0] < -IKPI )
6458 { j3array[0]+=IK2PI;
6459 }
6460 j3valid[0] = true;
6461 for(int ij3 = 0; ij3 < 1; ++ij3)
6462 {
6463 if( !j3valid[ij3] )
6464 {
6465  continue;
6466 }
6467 _ij3[0] = ij3; _ij3[1] = -1;
6468 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6469 {
6470 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6471 {
6472  j3valid[iij3]=false; _ij3[1] = iij3; break;
6473 }
6474 }
6475 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6476 {
6477 IkReal evalcond[8];
6478 IkReal x497=IKcos(j3);
6479 IkReal x498=IKsin(j3);
6480 IkReal x499=(gconst8*x498);
6481 IkReal x500=(gconst8*x497);
6482 evalcond[0]=(new_r01*x497);
6483 evalcond[1]=((-1.0)*x500);
6484 evalcond[2]=(gconst8+((new_r01*x498)));
6485 evalcond[3]=(x499+new_r01);
6486 evalcond[4]=(new_r00+(((-1.0)*x500)));
6487 evalcond[5]=((((-1.0)*x499))+new_r10);
6488 evalcond[6]=((((-1.0)*new_r10*x497))+((new_r00*x498)));
6489 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x498))+((new_r00*x497)));
6490 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6491 {
6492 continue;
6493 }
6494 }
6495 
6496 {
6497 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6498 vinfos[0].jointtype = 1;
6499 vinfos[0].foffset = j0;
6500 vinfos[0].indices[0] = _ij0[0];
6501 vinfos[0].indices[1] = _ij0[1];
6502 vinfos[0].maxsolutions = _nj0;
6503 vinfos[1].jointtype = 1;
6504 vinfos[1].foffset = j1;
6505 vinfos[1].indices[0] = _ij1[0];
6506 vinfos[1].indices[1] = _ij1[1];
6507 vinfos[1].maxsolutions = _nj1;
6508 vinfos[2].jointtype = 1;
6509 vinfos[2].foffset = j2;
6510 vinfos[2].indices[0] = _ij2[0];
6511 vinfos[2].indices[1] = _ij2[1];
6512 vinfos[2].maxsolutions = _nj2;
6513 vinfos[3].jointtype = 1;
6514 vinfos[3].foffset = j3;
6515 vinfos[3].indices[0] = _ij3[0];
6516 vinfos[3].indices[1] = _ij3[1];
6517 vinfos[3].maxsolutions = _nj3;
6518 vinfos[4].jointtype = 1;
6519 vinfos[4].foffset = j4;
6520 vinfos[4].indices[0] = _ij4[0];
6521 vinfos[4].indices[1] = _ij4[1];
6522 vinfos[4].maxsolutions = _nj4;
6523 vinfos[5].jointtype = 1;
6524 vinfos[5].foffset = j5;
6525 vinfos[5].indices[0] = _ij5[0];
6526 vinfos[5].indices[1] = _ij5[1];
6527 vinfos[5].maxsolutions = _nj5;
6528 std::vector<int> vfree(0);
6529 solutions.AddSolution(vinfos,vfree);
6530 }
6531 }
6532 }
6533 
6534 }
6535 
6536 }
6537 
6538 }
6539 } while(0);
6540 if( bgotonextstatement )
6541 {
6542 bool bgotonextstatement = true;
6543 do
6544 {
6545 if( 1 )
6546 {
6547 bgotonextstatement=false;
6548 continue; // branch miss [j3]
6549 
6550 }
6551 } while(0);
6552 if( bgotonextstatement )
6553 {
6554 }
6555 }
6556 }
6557 }
6558 }
6559 }
6560 
6561 } else
6562 {
6563 {
6564 IkReal j3array[1], cj3array[1], sj3array[1];
6565 bool j3valid[1]={false};
6566 _nj3 = 1;
6567 IkReal x501=((1.0)*new_r01);
6568 CheckValue<IkReal> x502=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*x501))+((gconst7*new_r11)))),-1);
6569 if(!x502.valid){
6570 continue;
6571 }
6572 CheckValue<IkReal> x503 = IKatan2WithCheck(IkReal(((((-1.0)*(gconst7*gconst7)))+(new_r01*new_r01))),IkReal(((((-1.0)*new_r11*x501))+((gconst7*gconst8)))),IKFAST_ATAN2_MAGTHRESH);
6573 if(!x503.valid){
6574 continue;
6575 }
6576 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x502.value)))+(x503.value));
6577 sj3array[0]=IKsin(j3array[0]);
6578 cj3array[0]=IKcos(j3array[0]);
6579 if( j3array[0] > IKPI )
6580 {
6581  j3array[0]-=IK2PI;
6582 }
6583 else if( j3array[0] < -IKPI )
6584 { j3array[0]+=IK2PI;
6585 }
6586 j3valid[0] = true;
6587 for(int ij3 = 0; ij3 < 1; ++ij3)
6588 {
6589 if( !j3valid[ij3] )
6590 {
6591  continue;
6592 }
6593 _ij3[0] = ij3; _ij3[1] = -1;
6594 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6595 {
6596 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6597 {
6598  j3valid[iij3]=false; _ij3[1] = iij3; break;
6599 }
6600 }
6601 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6602 {
6603 IkReal evalcond[8];
6604 IkReal x504=IKsin(j3);
6605 IkReal x505=IKcos(j3);
6606 IkReal x506=(gconst7*x504);
6607 IkReal x507=((1.0)*x505);
6608 IkReal x508=(gconst8*x504);
6609 IkReal x509=(gconst8*x507);
6610 evalcond[0]=(gconst7+((new_r11*x504))+((new_r01*x505)));
6611 evalcond[1]=(((gconst7*x505))+x508+new_r01);
6612 evalcond[2]=(gconst7+((new_r00*x504))+(((-1.0)*new_r10*x507)));
6613 evalcond[3]=(gconst8+(((-1.0)*new_r11*x507))+((new_r01*x504)));
6614 evalcond[4]=((((-1.0)*x509))+x506+new_r00);
6615 evalcond[5]=((((-1.0)*x509))+x506+new_r11);
6616 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x504))+((new_r00*x505)));
6617 evalcond[7]=((((-1.0)*gconst7*x507))+new_r10+(((-1.0)*x508)));
6618 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6619 {
6620 continue;
6621 }
6622 }
6623 
6624 {
6625 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6626 vinfos[0].jointtype = 1;
6627 vinfos[0].foffset = j0;
6628 vinfos[0].indices[0] = _ij0[0];
6629 vinfos[0].indices[1] = _ij0[1];
6630 vinfos[0].maxsolutions = _nj0;
6631 vinfos[1].jointtype = 1;
6632 vinfos[1].foffset = j1;
6633 vinfos[1].indices[0] = _ij1[0];
6634 vinfos[1].indices[1] = _ij1[1];
6635 vinfos[1].maxsolutions = _nj1;
6636 vinfos[2].jointtype = 1;
6637 vinfos[2].foffset = j2;
6638 vinfos[2].indices[0] = _ij2[0];
6639 vinfos[2].indices[1] = _ij2[1];
6640 vinfos[2].maxsolutions = _nj2;
6641 vinfos[3].jointtype = 1;
6642 vinfos[3].foffset = j3;
6643 vinfos[3].indices[0] = _ij3[0];
6644 vinfos[3].indices[1] = _ij3[1];
6645 vinfos[3].maxsolutions = _nj3;
6646 vinfos[4].jointtype = 1;
6647 vinfos[4].foffset = j4;
6648 vinfos[4].indices[0] = _ij4[0];
6649 vinfos[4].indices[1] = _ij4[1];
6650 vinfos[4].maxsolutions = _nj4;
6651 vinfos[5].jointtype = 1;
6652 vinfos[5].foffset = j5;
6653 vinfos[5].indices[0] = _ij5[0];
6654 vinfos[5].indices[1] = _ij5[1];
6655 vinfos[5].maxsolutions = _nj5;
6656 std::vector<int> vfree(0);
6657 solutions.AddSolution(vinfos,vfree);
6658 }
6659 }
6660 }
6661 
6662 }
6663 
6664 }
6665 
6666 } else
6667 {
6668 {
6669 IkReal j3array[1], cj3array[1], sj3array[1];
6670 bool j3valid[1]={false};
6671 _nj3 = 1;
6672 IkReal x510=((1.0)*gconst7);
6673 CheckValue<IkReal> x511=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*x510))+(((-1.0)*gconst8*new_r10)))),-1);
6674 if(!x511.valid){
6675 continue;
6676 }
6677 CheckValue<IkReal> x512 = IKatan2WithCheck(IkReal(((gconst7*gconst7)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst8*x510)))),IKFAST_ATAN2_MAGTHRESH);
6678 if(!x512.valid){
6679 continue;
6680 }
6681 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x511.value)))+(x512.value));
6682 sj3array[0]=IKsin(j3array[0]);
6683 cj3array[0]=IKcos(j3array[0]);
6684 if( j3array[0] > IKPI )
6685 {
6686  j3array[0]-=IK2PI;
6687 }
6688 else if( j3array[0] < -IKPI )
6689 { j3array[0]+=IK2PI;
6690 }
6691 j3valid[0] = true;
6692 for(int ij3 = 0; ij3 < 1; ++ij3)
6693 {
6694 if( !j3valid[ij3] )
6695 {
6696  continue;
6697 }
6698 _ij3[0] = ij3; _ij3[1] = -1;
6699 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6700 {
6701 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6702 {
6703  j3valid[iij3]=false; _ij3[1] = iij3; break;
6704 }
6705 }
6706 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6707 {
6708 IkReal evalcond[8];
6709 IkReal x513=IKsin(j3);
6710 IkReal x514=IKcos(j3);
6711 IkReal x515=(gconst7*x513);
6712 IkReal x516=((1.0)*x514);
6713 IkReal x517=(gconst8*x513);
6714 IkReal x518=(gconst8*x516);
6715 evalcond[0]=(((new_r01*x514))+((new_r11*x513))+gconst7);
6716 evalcond[1]=(((gconst7*x514))+x517+new_r01);
6717 evalcond[2]=(((new_r00*x513))+(((-1.0)*new_r10*x516))+gconst7);
6718 evalcond[3]=(((new_r01*x513))+(((-1.0)*new_r11*x516))+gconst8);
6719 evalcond[4]=(x515+new_r00+(((-1.0)*x518)));
6720 evalcond[5]=(x515+new_r11+(((-1.0)*x518)));
6721 evalcond[6]=(((new_r00*x514))+((new_r10*x513))+(((-1.0)*gconst8)));
6722 evalcond[7]=((((-1.0)*x517))+new_r10+(((-1.0)*gconst7*x516)));
6723 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6724 {
6725 continue;
6726 }
6727 }
6728 
6729 {
6730 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6731 vinfos[0].jointtype = 1;
6732 vinfos[0].foffset = j0;
6733 vinfos[0].indices[0] = _ij0[0];
6734 vinfos[0].indices[1] = _ij0[1];
6735 vinfos[0].maxsolutions = _nj0;
6736 vinfos[1].jointtype = 1;
6737 vinfos[1].foffset = j1;
6738 vinfos[1].indices[0] = _ij1[0];
6739 vinfos[1].indices[1] = _ij1[1];
6740 vinfos[1].maxsolutions = _nj1;
6741 vinfos[2].jointtype = 1;
6742 vinfos[2].foffset = j2;
6743 vinfos[2].indices[0] = _ij2[0];
6744 vinfos[2].indices[1] = _ij2[1];
6745 vinfos[2].maxsolutions = _nj2;
6746 vinfos[3].jointtype = 1;
6747 vinfos[3].foffset = j3;
6748 vinfos[3].indices[0] = _ij3[0];
6749 vinfos[3].indices[1] = _ij3[1];
6750 vinfos[3].maxsolutions = _nj3;
6751 vinfos[4].jointtype = 1;
6752 vinfos[4].foffset = j4;
6753 vinfos[4].indices[0] = _ij4[0];
6754 vinfos[4].indices[1] = _ij4[1];
6755 vinfos[4].maxsolutions = _nj4;
6756 vinfos[5].jointtype = 1;
6757 vinfos[5].foffset = j5;
6758 vinfos[5].indices[0] = _ij5[0];
6759 vinfos[5].indices[1] = _ij5[1];
6760 vinfos[5].maxsolutions = _nj5;
6761 std::vector<int> vfree(0);
6762 solutions.AddSolution(vinfos,vfree);
6763 }
6764 }
6765 }
6766 
6767 }
6768 
6769 }
6770 
6771 } else
6772 {
6773 {
6774 IkReal j3array[1], cj3array[1], sj3array[1];
6775 bool j3valid[1]={false};
6776 _nj3 = 1;
6777 IkReal x519=((1.0)*new_r11);
6778 CheckValue<IkReal> x520 = IKatan2WithCheck(IkReal((((gconst7*new_r01))+((gconst7*new_r10)))),IkReal(((((-1.0)*gconst7*x519))+((gconst7*new_r00)))),IKFAST_ATAN2_MAGTHRESH);
6779 if(!x520.valid){
6780 continue;
6781 }
6782 CheckValue<IkReal> x521=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x519))+(((-1.0)*new_r00*new_r01)))),-1);
6783 if(!x521.valid){
6784 continue;
6785 }
6786 j3array[0]=((-1.5707963267949)+(x520.value)+(((1.5707963267949)*(x521.value))));
6787 sj3array[0]=IKsin(j3array[0]);
6788 cj3array[0]=IKcos(j3array[0]);
6789 if( j3array[0] > IKPI )
6790 {
6791  j3array[0]-=IK2PI;
6792 }
6793 else if( j3array[0] < -IKPI )
6794 { j3array[0]+=IK2PI;
6795 }
6796 j3valid[0] = true;
6797 for(int ij3 = 0; ij3 < 1; ++ij3)
6798 {
6799 if( !j3valid[ij3] )
6800 {
6801  continue;
6802 }
6803 _ij3[0] = ij3; _ij3[1] = -1;
6804 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6805 {
6806 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6807 {
6808  j3valid[iij3]=false; _ij3[1] = iij3; break;
6809 }
6810 }
6811 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6812 {
6813 IkReal evalcond[8];
6814 IkReal x522=IKsin(j3);
6815 IkReal x523=IKcos(j3);
6816 IkReal x524=(gconst7*x522);
6817 IkReal x525=((1.0)*x523);
6818 IkReal x526=(gconst8*x522);
6819 IkReal x527=(gconst8*x525);
6820 evalcond[0]=(((new_r01*x523))+gconst7+((new_r11*x522)));
6821 evalcond[1]=(((gconst7*x523))+x526+new_r01);
6822 evalcond[2]=(gconst7+(((-1.0)*new_r10*x525))+((new_r00*x522)));
6823 evalcond[3]=(((new_r01*x522))+gconst8+(((-1.0)*new_r11*x525)));
6824 evalcond[4]=((((-1.0)*x527))+x524+new_r00);
6825 evalcond[5]=((((-1.0)*x527))+x524+new_r11);
6826 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x522))+((new_r00*x523)));
6827 evalcond[7]=((((-1.0)*x526))+(((-1.0)*gconst7*x525))+new_r10);
6828 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6829 {
6830 continue;
6831 }
6832 }
6833 
6834 {
6835 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6836 vinfos[0].jointtype = 1;
6837 vinfos[0].foffset = j0;
6838 vinfos[0].indices[0] = _ij0[0];
6839 vinfos[0].indices[1] = _ij0[1];
6840 vinfos[0].maxsolutions = _nj0;
6841 vinfos[1].jointtype = 1;
6842 vinfos[1].foffset = j1;
6843 vinfos[1].indices[0] = _ij1[0];
6844 vinfos[1].indices[1] = _ij1[1];
6845 vinfos[1].maxsolutions = _nj1;
6846 vinfos[2].jointtype = 1;
6847 vinfos[2].foffset = j2;
6848 vinfos[2].indices[0] = _ij2[0];
6849 vinfos[2].indices[1] = _ij2[1];
6850 vinfos[2].maxsolutions = _nj2;
6851 vinfos[3].jointtype = 1;
6852 vinfos[3].foffset = j3;
6853 vinfos[3].indices[0] = _ij3[0];
6854 vinfos[3].indices[1] = _ij3[1];
6855 vinfos[3].maxsolutions = _nj3;
6856 vinfos[4].jointtype = 1;
6857 vinfos[4].foffset = j4;
6858 vinfos[4].indices[0] = _ij4[0];
6859 vinfos[4].indices[1] = _ij4[1];
6860 vinfos[4].maxsolutions = _nj4;
6861 vinfos[5].jointtype = 1;
6862 vinfos[5].foffset = j5;
6863 vinfos[5].indices[0] = _ij5[0];
6864 vinfos[5].indices[1] = _ij5[1];
6865 vinfos[5].maxsolutions = _nj5;
6866 std::vector<int> vfree(0);
6867 solutions.AddSolution(vinfos,vfree);
6868 }
6869 }
6870 }
6871 
6872 }
6873 
6874 }
6875 
6876 }
6877 } while(0);
6878 if( bgotonextstatement )
6879 {
6880 bool bgotonextstatement = true;
6881 do
6882 {
6883 IkReal x528=((-1.0)*new_r11);
6884 IkReal x530 = ((new_r01*new_r01)+(new_r11*new_r11));
6885 if(IKabs(x530)==0){
6886 continue;
6887 }
6888 IkReal x529=pow(x530,-0.5);
6889 CheckValue<IkReal> x531 = IKatan2WithCheck(IkReal(x528),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6890 if(!x531.valid){
6891 continue;
6892 }
6893 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x531.value))));
6894 IkReal gconst10=(x528*x529);
6895 IkReal gconst11=((1.0)*new_r01*x529);
6896 CheckValue<IkReal> x532 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6897 if(!x532.valid){
6898 continue;
6899 }
6900 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x532.value)+j5)))), 6.28318530717959)));
6901 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6902 {
6903 bgotonextstatement=false;
6904 {
6905 IkReal j3eval[3];
6906 IkReal x533=((-1.0)*new_r11);
6907 CheckValue<IkReal> x536 = IKatan2WithCheck(IkReal(x533),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6908 if(!x536.valid){
6909 continue;
6910 }
6911 IkReal x534=((1.0)*(x536.value));
6912 IkReal x535=x529;
6913 sj4=0;
6914 cj4=1.0;
6915 j4=0;
6916 sj5=gconst10;
6917 cj5=gconst11;
6918 j5=((3.14159265)+(((-1.0)*x534)));
6919 IkReal gconst9=((3.14159265358979)+(((-1.0)*x534)));
6920 IkReal gconst10=(x533*x535);
6921 IkReal gconst11=((1.0)*new_r01*x535);
6922 IkReal x537=new_r11*new_r11;
6923 IkReal x538=((1.0)*new_r01);
6924 IkReal x539=((1.0)*new_r10);
6925 IkReal x540=((((-1.0)*new_r00*x538))+(((-1.0)*new_r11*x539)));
6926 IkReal x541=x529;
6927 IkReal x542=(new_r11*x541);
6928 j3eval[0]=x540;
6929 j3eval[1]=((IKabs(((((-1.0)*x539*x542))+(((-1.0)*x538*x542)))))+(IKabs((((x537*x541))+(((-1.0)*new_r00*x542))))));
6930 j3eval[2]=IKsign(x540);
6931 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
6932 {
6933 {
6934 IkReal j3eval[1];
6935 IkReal x543=((-1.0)*new_r11);
6936 CheckValue<IkReal> x546 = IKatan2WithCheck(IkReal(x543),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6937 if(!x546.valid){
6938 continue;
6939 }
6940 IkReal x544=((1.0)*(x546.value));
6941 IkReal x545=x529;
6942 sj4=0;
6943 cj4=1.0;
6944 j4=0;
6945 sj5=gconst10;
6946 cj5=gconst11;
6947 j5=((3.14159265)+(((-1.0)*x544)));
6948 IkReal gconst9=((3.14159265358979)+(((-1.0)*x544)));
6949 IkReal gconst10=(x543*x545);
6950 IkReal gconst11=((1.0)*new_r01*x545);
6951 IkReal x547=new_r11*new_r11;
6952 IkReal x548=new_r01*new_r01*new_r01;
6953 CheckValue<IkReal> x552=IKPowWithIntegerCheck(((new_r01*new_r01)+x547),-1);
6954 if(!x552.valid){
6955 continue;
6956 }
6957 IkReal x549=x552.value;
6958 IkReal x550=(x547*x549);
6959 IkReal x551=(x548*x549);
6960 j3eval[0]=((IKabs((((new_r10*x551))+x550+((new_r01*new_r10*x550)))))+(IKabs((((new_r00*new_r01*x550))+((new_r01*new_r11*x549))+((new_r00*x551))))));
6961 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6962 {
6963 {
6964 IkReal j3eval[1];
6965 IkReal x553=((-1.0)*new_r11);
6966 CheckValue<IkReal> x556 = IKatan2WithCheck(IkReal(x553),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6967 if(!x556.valid){
6968 continue;
6969 }
6970 IkReal x554=((1.0)*(x556.value));
6971 IkReal x555=x529;
6972 sj4=0;
6973 cj4=1.0;
6974 j4=0;
6975 sj5=gconst10;
6976 cj5=gconst11;
6977 j5=((3.14159265)+(((-1.0)*x554)));
6978 IkReal gconst9=((3.14159265358979)+(((-1.0)*x554)));
6979 IkReal gconst10=(x553*x555);
6980 IkReal gconst11=((1.0)*new_r01*x555);
6981 IkReal x557=new_r01*new_r01;
6982 IkReal x558=new_r11*new_r11;
6983 CheckValue<IkReal> x565=IKPowWithIntegerCheck((x558+x557),-1);
6984 if(!x565.valid){
6985 continue;
6986 }
6987 IkReal x559=x565.value;
6988 IkReal x560=(x558*x559);
6989 CheckValue<IkReal> x566=IKPowWithIntegerCheck(((((-1.0)*x557))+(((-1.0)*x558))),-1);
6990 if(!x566.valid){
6991 continue;
6992 }
6993 IkReal x561=x566.value;
6994 IkReal x562=((1.0)*x561);
6995 IkReal x563=(new_r11*x562);
6996 IkReal x564=(new_r01*x562);
6997 j3eval[0]=((IKabs((((x559*(x557*x557)))+(((-1.0)*x560))+((x557*x560)))))+(IKabs(((((-1.0)*new_r01*x563*(new_r11*new_r11)))+(((-1.0)*x563*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x563))))));
6998 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6999 {
7000 {
7001 IkReal evalcond[2];
7002 bool bgotonextstatement = true;
7003 do
7004 {
7005 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7006 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7007 {
7008 bgotonextstatement=false;
7009 {
7010 IkReal j3array[2], cj3array[2], sj3array[2];
7011 bool j3valid[2]={false};
7012 _nj3 = 2;
7013 CheckValue<IkReal> x567=IKPowWithIntegerCheck(gconst11,-1);
7014 if(!x567.valid){
7015 continue;
7016 }
7017 sj3array[0]=(new_r10*(x567.value));
7018 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7019 {
7020  j3valid[0] = j3valid[1] = true;
7021  j3array[0] = IKasin(sj3array[0]);
7022  cj3array[0] = IKcos(j3array[0]);
7023  sj3array[1] = sj3array[0];
7024  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7025  cj3array[1] = -cj3array[0];
7026 }
7027 else if( isnan(sj3array[0]) )
7028 {
7029  // probably any value will work
7030  j3valid[0] = true;
7031  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7032 }
7033 for(int ij3 = 0; ij3 < 2; ++ij3)
7034 {
7035 if( !j3valid[ij3] )
7036 {
7037  continue;
7038 }
7039 _ij3[0] = ij3; _ij3[1] = -1;
7040 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7041 {
7042 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7043 {
7044  j3valid[iij3]=false; _ij3[1] = iij3; break;
7045 }
7046 }
7047 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7048 {
7049 IkReal evalcond[6];
7050 IkReal x568=IKcos(j3);
7051 IkReal x569=IKsin(j3);
7052 IkReal x570=((-1.0)*x568);
7053 evalcond[0]=(new_r01*x568);
7054 evalcond[1]=(new_r10*x570);
7055 evalcond[2]=(gconst11*x570);
7056 evalcond[3]=(gconst11+((new_r01*x569)));
7057 evalcond[4]=(((gconst11*x569))+new_r01);
7058 evalcond[5]=((((-1.0)*gconst11))+((new_r10*x569)));
7059 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7060 {
7061 continue;
7062 }
7063 }
7064 
7065 {
7066 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7067 vinfos[0].jointtype = 1;
7068 vinfos[0].foffset = j0;
7069 vinfos[0].indices[0] = _ij0[0];
7070 vinfos[0].indices[1] = _ij0[1];
7071 vinfos[0].maxsolutions = _nj0;
7072 vinfos[1].jointtype = 1;
7073 vinfos[1].foffset = j1;
7074 vinfos[1].indices[0] = _ij1[0];
7075 vinfos[1].indices[1] = _ij1[1];
7076 vinfos[1].maxsolutions = _nj1;
7077 vinfos[2].jointtype = 1;
7078 vinfos[2].foffset = j2;
7079 vinfos[2].indices[0] = _ij2[0];
7080 vinfos[2].indices[1] = _ij2[1];
7081 vinfos[2].maxsolutions = _nj2;
7082 vinfos[3].jointtype = 1;
7083 vinfos[3].foffset = j3;
7084 vinfos[3].indices[0] = _ij3[0];
7085 vinfos[3].indices[1] = _ij3[1];
7086 vinfos[3].maxsolutions = _nj3;
7087 vinfos[4].jointtype = 1;
7088 vinfos[4].foffset = j4;
7089 vinfos[4].indices[0] = _ij4[0];
7090 vinfos[4].indices[1] = _ij4[1];
7091 vinfos[4].maxsolutions = _nj4;
7092 vinfos[5].jointtype = 1;
7093 vinfos[5].foffset = j5;
7094 vinfos[5].indices[0] = _ij5[0];
7095 vinfos[5].indices[1] = _ij5[1];
7096 vinfos[5].maxsolutions = _nj5;
7097 std::vector<int> vfree(0);
7098 solutions.AddSolution(vinfos,vfree);
7099 }
7100 }
7101 }
7102 
7103 }
7104 } while(0);
7105 if( bgotonextstatement )
7106 {
7107 bool bgotonextstatement = true;
7108 do
7109 {
7110 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7111 evalcond[1]=gconst10;
7112 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7113 {
7114 bgotonextstatement=false;
7115 {
7116 IkReal j3eval[3];
7117 IkReal x571=((-1.0)*new_r11);
7118 CheckValue<IkReal> x573 = IKatan2WithCheck(IkReal(x571),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7119 if(!x573.valid){
7120 continue;
7121 }
7122 IkReal x572=((1.0)*(x573.value));
7123 sj4=0;
7124 cj4=1.0;
7125 j4=0;
7126 sj5=gconst10;
7127 cj5=gconst11;
7128 j5=((3.14159265)+(((-1.0)*x572)));
7129 new_r00=0;
7130 new_r10=0;
7131 new_r21=0;
7132 new_r22=0;
7133 IkReal gconst9=((3.14159265358979)+(((-1.0)*x572)));
7134 IkReal gconst10=x571;
7135 IkReal gconst11=((1.0)*new_r01);
7136 j3eval[0]=1.0;
7137 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(IKabs(((1.0)*new_r01*new_r11))));
7138 j3eval[2]=1.0;
7139 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7140 {
7141 {
7142 IkReal j3eval[4];
7143 IkReal x574=((-1.0)*new_r11);
7144 CheckValue<IkReal> x576 = IKatan2WithCheck(IkReal(x574),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7145 if(!x576.valid){
7146 continue;
7147 }
7148 IkReal x575=((1.0)*(x576.value));
7149 sj4=0;
7150 cj4=1.0;
7151 j4=0;
7152 sj5=gconst10;
7153 cj5=gconst11;
7154 j5=((3.14159265)+(((-1.0)*x575)));
7155 new_r00=0;
7156 new_r10=0;
7157 new_r21=0;
7158 new_r22=0;
7159 IkReal gconst9=((3.14159265358979)+(((-1.0)*x575)));
7160 IkReal gconst10=x574;
7161 IkReal gconst11=((1.0)*new_r01);
7162 j3eval[0]=-1.0;
7163 j3eval[1]=-1.0;
7164 j3eval[2]=new_r01;
7165 j3eval[3]=1.0;
7166 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
7167 {
7168 {
7169 IkReal j3eval[3];
7170 IkReal x577=((-1.0)*new_r11);
7171 CheckValue<IkReal> x579 = IKatan2WithCheck(IkReal(x577),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7172 if(!x579.valid){
7173 continue;
7174 }
7175 IkReal x578=((1.0)*(x579.value));
7176 sj4=0;
7177 cj4=1.0;
7178 j4=0;
7179 sj5=gconst10;
7180 cj5=gconst11;
7181 j5=((3.14159265)+(((-1.0)*x578)));
7182 new_r00=0;
7183 new_r10=0;
7184 new_r21=0;
7185 new_r22=0;
7186 IkReal gconst9=((3.14159265358979)+(((-1.0)*x578)));
7187 IkReal gconst10=x577;
7188 IkReal gconst11=((1.0)*new_r01);
7189 j3eval[0]=-1.0;
7190 j3eval[1]=-1.0;
7191 j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7192 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7193 {
7194 continue; // 3 cases reached
7195 
7196 } else
7197 {
7198 {
7199 IkReal j3array[1], cj3array[1], sj3array[1];
7200 bool j3valid[1]={false};
7201 _nj3 = 1;
7202 IkReal x580=((1.0)*new_r01);
7203 CheckValue<IkReal> x581 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst10*gconst10))))),IkReal((((gconst10*gconst11))+(((-1.0)*new_r11*x580)))),IKFAST_ATAN2_MAGTHRESH);
7204 if(!x581.valid){
7205 continue;
7206 }
7207 CheckValue<IkReal> x582=IKPowWithIntegerCheck(IKsign((((gconst10*new_r11))+(((-1.0)*gconst11*x580)))),-1);
7208 if(!x582.valid){
7209 continue;
7210 }
7211 j3array[0]=((-1.5707963267949)+(x581.value)+(((1.5707963267949)*(x582.value))));
7212 sj3array[0]=IKsin(j3array[0]);
7213 cj3array[0]=IKcos(j3array[0]);
7214 if( j3array[0] > IKPI )
7215 {
7216  j3array[0]-=IK2PI;
7217 }
7218 else if( j3array[0] < -IKPI )
7219 { j3array[0]+=IK2PI;
7220 }
7221 j3valid[0] = true;
7222 for(int ij3 = 0; ij3 < 1; ++ij3)
7223 {
7224 if( !j3valid[ij3] )
7225 {
7226  continue;
7227 }
7228 _ij3[0] = ij3; _ij3[1] = -1;
7229 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7230 {
7231 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7232 {
7233  j3valid[iij3]=false; _ij3[1] = iij3; break;
7234 }
7235 }
7236 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7237 {
7238 IkReal evalcond[6];
7239 IkReal x583=IKcos(j3);
7240 IkReal x584=IKsin(j3);
7241 IkReal x585=(gconst10*x584);
7242 IkReal x586=((1.0)*x583);
7243 IkReal x587=(gconst11*x584);
7244 IkReal x588=(gconst11*x586);
7245 evalcond[0]=(gconst10+((new_r01*x583))+((new_r11*x584)));
7246 evalcond[1]=(x587+new_r01+((gconst10*x583)));
7247 evalcond[2]=(x585+(((-1.0)*x588)));
7248 evalcond[3]=(gconst11+(((-1.0)*new_r11*x586))+((new_r01*x584)));
7249 evalcond[4]=(x585+new_r11+(((-1.0)*x588)));
7250 evalcond[5]=((((-1.0)*gconst10*x586))+(((-1.0)*x587)));
7251 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7252 {
7253 continue;
7254 }
7255 }
7256 
7257 {
7258 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7259 vinfos[0].jointtype = 1;
7260 vinfos[0].foffset = j0;
7261 vinfos[0].indices[0] = _ij0[0];
7262 vinfos[0].indices[1] = _ij0[1];
7263 vinfos[0].maxsolutions = _nj0;
7264 vinfos[1].jointtype = 1;
7265 vinfos[1].foffset = j1;
7266 vinfos[1].indices[0] = _ij1[0];
7267 vinfos[1].indices[1] = _ij1[1];
7268 vinfos[1].maxsolutions = _nj1;
7269 vinfos[2].jointtype = 1;
7270 vinfos[2].foffset = j2;
7271 vinfos[2].indices[0] = _ij2[0];
7272 vinfos[2].indices[1] = _ij2[1];
7273 vinfos[2].maxsolutions = _nj2;
7274 vinfos[3].jointtype = 1;
7275 vinfos[3].foffset = j3;
7276 vinfos[3].indices[0] = _ij3[0];
7277 vinfos[3].indices[1] = _ij3[1];
7278 vinfos[3].maxsolutions = _nj3;
7279 vinfos[4].jointtype = 1;
7280 vinfos[4].foffset = j4;
7281 vinfos[4].indices[0] = _ij4[0];
7282 vinfos[4].indices[1] = _ij4[1];
7283 vinfos[4].maxsolutions = _nj4;
7284 vinfos[5].jointtype = 1;
7285 vinfos[5].foffset = j5;
7286 vinfos[5].indices[0] = _ij5[0];
7287 vinfos[5].indices[1] = _ij5[1];
7288 vinfos[5].maxsolutions = _nj5;
7289 std::vector<int> vfree(0);
7290 solutions.AddSolution(vinfos,vfree);
7291 }
7292 }
7293 }
7294 
7295 }
7296 
7297 }
7298 
7299 } else
7300 {
7301 {
7302 IkReal j3array[1], cj3array[1], sj3array[1];
7303 bool j3valid[1]={false};
7304 _nj3 = 1;
7305 CheckValue<IkReal> x589=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
7306 if(!x589.valid){
7307 continue;
7308 }
7309 CheckValue<IkReal> x590 = IKatan2WithCheck(IkReal((gconst11*new_r01)),IkReal((gconst10*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7310 if(!x590.valid){
7311 continue;
7312 }
7313 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x589.value)))+(x590.value));
7314 sj3array[0]=IKsin(j3array[0]);
7315 cj3array[0]=IKcos(j3array[0]);
7316 if( j3array[0] > IKPI )
7317 {
7318  j3array[0]-=IK2PI;
7319 }
7320 else if( j3array[0] < -IKPI )
7321 { j3array[0]+=IK2PI;
7322 }
7323 j3valid[0] = true;
7324 for(int ij3 = 0; ij3 < 1; ++ij3)
7325 {
7326 if( !j3valid[ij3] )
7327 {
7328  continue;
7329 }
7330 _ij3[0] = ij3; _ij3[1] = -1;
7331 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7332 {
7333 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7334 {
7335  j3valid[iij3]=false; _ij3[1] = iij3; break;
7336 }
7337 }
7338 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7339 {
7340 IkReal evalcond[6];
7341 IkReal x591=IKcos(j3);
7342 IkReal x592=IKsin(j3);
7343 IkReal x593=(gconst10*x592);
7344 IkReal x594=((1.0)*x591);
7345 IkReal x595=(gconst11*x592);
7346 IkReal x596=(gconst11*x594);
7347 evalcond[0]=(((new_r11*x592))+gconst10+((new_r01*x591)));
7348 evalcond[1]=(((gconst10*x591))+x595+new_r01);
7349 evalcond[2]=((((-1.0)*x596))+x593);
7350 evalcond[3]=((((-1.0)*new_r11*x594))+gconst11+((new_r01*x592)));
7351 evalcond[4]=((((-1.0)*x596))+x593+new_r11);
7352 evalcond[5]=((((-1.0)*x595))+(((-1.0)*gconst10*x594)));
7353 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7354 {
7355 continue;
7356 }
7357 }
7358 
7359 {
7360 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7361 vinfos[0].jointtype = 1;
7362 vinfos[0].foffset = j0;
7363 vinfos[0].indices[0] = _ij0[0];
7364 vinfos[0].indices[1] = _ij0[1];
7365 vinfos[0].maxsolutions = _nj0;
7366 vinfos[1].jointtype = 1;
7367 vinfos[1].foffset = j1;
7368 vinfos[1].indices[0] = _ij1[0];
7369 vinfos[1].indices[1] = _ij1[1];
7370 vinfos[1].maxsolutions = _nj1;
7371 vinfos[2].jointtype = 1;
7372 vinfos[2].foffset = j2;
7373 vinfos[2].indices[0] = _ij2[0];
7374 vinfos[2].indices[1] = _ij2[1];
7375 vinfos[2].maxsolutions = _nj2;
7376 vinfos[3].jointtype = 1;
7377 vinfos[3].foffset = j3;
7378 vinfos[3].indices[0] = _ij3[0];
7379 vinfos[3].indices[1] = _ij3[1];
7380 vinfos[3].maxsolutions = _nj3;
7381 vinfos[4].jointtype = 1;
7382 vinfos[4].foffset = j4;
7383 vinfos[4].indices[0] = _ij4[0];
7384 vinfos[4].indices[1] = _ij4[1];
7385 vinfos[4].maxsolutions = _nj4;
7386 vinfos[5].jointtype = 1;
7387 vinfos[5].foffset = j5;
7388 vinfos[5].indices[0] = _ij5[0];
7389 vinfos[5].indices[1] = _ij5[1];
7390 vinfos[5].maxsolutions = _nj5;
7391 std::vector<int> vfree(0);
7392 solutions.AddSolution(vinfos,vfree);
7393 }
7394 }
7395 }
7396 
7397 }
7398 
7399 }
7400 
7401 } else
7402 {
7403 {
7404 IkReal j3array[1], cj3array[1], sj3array[1];
7405 bool j3valid[1]={false};
7406 _nj3 = 1;
7407 CheckValue<IkReal> x597=IKPowWithIntegerCheck(IKsign((((gconst11*new_r01))+(((-1.0)*gconst10*new_r11)))),-1);
7408 if(!x597.valid){
7409 continue;
7410 }
7411 CheckValue<IkReal> x598 = IKatan2WithCheck(IkReal(gconst10*gconst10),IkReal(((-1.0)*gconst10*gconst11)),IKFAST_ATAN2_MAGTHRESH);
7412 if(!x598.valid){
7413 continue;
7414 }
7415 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x597.value)))+(x598.value));
7416 sj3array[0]=IKsin(j3array[0]);
7417 cj3array[0]=IKcos(j3array[0]);
7418 if( j3array[0] > IKPI )
7419 {
7420  j3array[0]-=IK2PI;
7421 }
7422 else if( j3array[0] < -IKPI )
7423 { j3array[0]+=IK2PI;
7424 }
7425 j3valid[0] = true;
7426 for(int ij3 = 0; ij3 < 1; ++ij3)
7427 {
7428 if( !j3valid[ij3] )
7429 {
7430  continue;
7431 }
7432 _ij3[0] = ij3; _ij3[1] = -1;
7433 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7434 {
7435 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7436 {
7437  j3valid[iij3]=false; _ij3[1] = iij3; break;
7438 }
7439 }
7440 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7441 {
7442 IkReal evalcond[6];
7443 IkReal x599=IKcos(j3);
7444 IkReal x600=IKsin(j3);
7445 IkReal x601=(gconst10*x600);
7446 IkReal x602=((1.0)*x599);
7447 IkReal x603=(gconst11*x600);
7448 IkReal x604=(gconst11*x602);
7449 evalcond[0]=(gconst10+((new_r11*x600))+((new_r01*x599)));
7450 evalcond[1]=(((gconst10*x599))+x603+new_r01);
7451 evalcond[2]=(x601+(((-1.0)*x604)));
7452 evalcond[3]=((((-1.0)*new_r11*x602))+gconst11+((new_r01*x600)));
7453 evalcond[4]=(x601+(((-1.0)*x604))+new_r11);
7454 evalcond[5]=((((-1.0)*gconst10*x602))+(((-1.0)*x603)));
7455 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7456 {
7457 continue;
7458 }
7459 }
7460 
7461 {
7462 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7463 vinfos[0].jointtype = 1;
7464 vinfos[0].foffset = j0;
7465 vinfos[0].indices[0] = _ij0[0];
7466 vinfos[0].indices[1] = _ij0[1];
7467 vinfos[0].maxsolutions = _nj0;
7468 vinfos[1].jointtype = 1;
7469 vinfos[1].foffset = j1;
7470 vinfos[1].indices[0] = _ij1[0];
7471 vinfos[1].indices[1] = _ij1[1];
7472 vinfos[1].maxsolutions = _nj1;
7473 vinfos[2].jointtype = 1;
7474 vinfos[2].foffset = j2;
7475 vinfos[2].indices[0] = _ij2[0];
7476 vinfos[2].indices[1] = _ij2[1];
7477 vinfos[2].maxsolutions = _nj2;
7478 vinfos[3].jointtype = 1;
7479 vinfos[3].foffset = j3;
7480 vinfos[3].indices[0] = _ij3[0];
7481 vinfos[3].indices[1] = _ij3[1];
7482 vinfos[3].maxsolutions = _nj3;
7483 vinfos[4].jointtype = 1;
7484 vinfos[4].foffset = j4;
7485 vinfos[4].indices[0] = _ij4[0];
7486 vinfos[4].indices[1] = _ij4[1];
7487 vinfos[4].maxsolutions = _nj4;
7488 vinfos[5].jointtype = 1;
7489 vinfos[5].foffset = j5;
7490 vinfos[5].indices[0] = _ij5[0];
7491 vinfos[5].indices[1] = _ij5[1];
7492 vinfos[5].maxsolutions = _nj5;
7493 std::vector<int> vfree(0);
7494 solutions.AddSolution(vinfos,vfree);
7495 }
7496 }
7497 }
7498 
7499 }
7500 
7501 }
7502 
7503 }
7504 } while(0);
7505 if( bgotonextstatement )
7506 {
7507 bool bgotonextstatement = true;
7508 do
7509 {
7510 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
7511 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7512 {
7513 bgotonextstatement=false;
7514 {
7515 IkReal j3eval[1];
7516 IkReal x605=((-1.0)*new_r11);
7517 CheckValue<IkReal> x607 = IKatan2WithCheck(IkReal(x605),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7518 if(!x607.valid){
7519 continue;
7520 }
7521 IkReal x606=((1.0)*(x607.value));
7522 sj4=0;
7523 cj4=1.0;
7524 j4=0;
7525 sj5=gconst10;
7526 cj5=gconst11;
7527 j5=((3.14159265)+(((-1.0)*x606)));
7528 new_r01=0;
7529 new_r10=0;
7530 IkReal gconst9=((3.14159265358979)+(((-1.0)*x606)));
7531 IkReal x608 = new_r11*new_r11;
7532 if(IKabs(x608)==0){
7533 continue;
7534 }
7535 IkReal gconst10=(x605*(pow(x608,-0.5)));
7536 IkReal gconst11=0;
7537 j3eval[0]=new_r00;
7538 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7539 {
7540 {
7541 IkReal j3eval[1];
7542 IkReal x609=((-1.0)*new_r11);
7543 CheckValue<IkReal> x611 = IKatan2WithCheck(IkReal(x609),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7544 if(!x611.valid){
7545 continue;
7546 }
7547 IkReal x610=((1.0)*(x611.value));
7548 sj4=0;
7549 cj4=1.0;
7550 j4=0;
7551 sj5=gconst10;
7552 cj5=gconst11;
7553 j5=((3.14159265)+(((-1.0)*x610)));
7554 new_r01=0;
7555 new_r10=0;
7556 IkReal gconst9=((3.14159265358979)+(((-1.0)*x610)));
7557 IkReal x612 = new_r11*new_r11;
7558 if(IKabs(x612)==0){
7559 continue;
7560 }
7561 IkReal gconst10=(x609*(pow(x612,-0.5)));
7562 IkReal gconst11=0;
7563 j3eval[0]=new_r11;
7564 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7565 {
7566 {
7567 IkReal j3array[2], cj3array[2], sj3array[2];
7568 bool j3valid[2]={false};
7569 _nj3 = 2;
7570 CheckValue<IkReal> x613=IKPowWithIntegerCheck(gconst10,-1);
7571 if(!x613.valid){
7572 continue;
7573 }
7574 sj3array[0]=((-1.0)*new_r00*(x613.value));
7575 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7576 {
7577  j3valid[0] = j3valid[1] = true;
7578  j3array[0] = IKasin(sj3array[0]);
7579  cj3array[0] = IKcos(j3array[0]);
7580  sj3array[1] = sj3array[0];
7581  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7582  cj3array[1] = -cj3array[0];
7583 }
7584 else if( isnan(sj3array[0]) )
7585 {
7586  // probably any value will work
7587  j3valid[0] = true;
7588  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7589 }
7590 for(int ij3 = 0; ij3 < 2; ++ij3)
7591 {
7592 if( !j3valid[ij3] )
7593 {
7594  continue;
7595 }
7596 _ij3[0] = ij3; _ij3[1] = -1;
7597 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7598 {
7599 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7600 {
7601  j3valid[iij3]=false; _ij3[1] = iij3; break;
7602 }
7603 }
7604 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7605 {
7606 IkReal evalcond[6];
7607 IkReal x614=IKcos(j3);
7608 IkReal x615=IKsin(j3);
7609 evalcond[0]=(gconst10*x614);
7610 evalcond[1]=(new_r00*x614);
7611 evalcond[2]=((-1.0)*new_r11*x614);
7612 evalcond[3]=(gconst10+((new_r00*x615)));
7613 evalcond[4]=(gconst10+((new_r11*x615)));
7614 evalcond[5]=(new_r11+((gconst10*x615)));
7615 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7616 {
7617 continue;
7618 }
7619 }
7620 
7621 {
7622 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7623 vinfos[0].jointtype = 1;
7624 vinfos[0].foffset = j0;
7625 vinfos[0].indices[0] = _ij0[0];
7626 vinfos[0].indices[1] = _ij0[1];
7627 vinfos[0].maxsolutions = _nj0;
7628 vinfos[1].jointtype = 1;
7629 vinfos[1].foffset = j1;
7630 vinfos[1].indices[0] = _ij1[0];
7631 vinfos[1].indices[1] = _ij1[1];
7632 vinfos[1].maxsolutions = _nj1;
7633 vinfos[2].jointtype = 1;
7634 vinfos[2].foffset = j2;
7635 vinfos[2].indices[0] = _ij2[0];
7636 vinfos[2].indices[1] = _ij2[1];
7637 vinfos[2].maxsolutions = _nj2;
7638 vinfos[3].jointtype = 1;
7639 vinfos[3].foffset = j3;
7640 vinfos[3].indices[0] = _ij3[0];
7641 vinfos[3].indices[1] = _ij3[1];
7642 vinfos[3].maxsolutions = _nj3;
7643 vinfos[4].jointtype = 1;
7644 vinfos[4].foffset = j4;
7645 vinfos[4].indices[0] = _ij4[0];
7646 vinfos[4].indices[1] = _ij4[1];
7647 vinfos[4].maxsolutions = _nj4;
7648 vinfos[5].jointtype = 1;
7649 vinfos[5].foffset = j5;
7650 vinfos[5].indices[0] = _ij5[0];
7651 vinfos[5].indices[1] = _ij5[1];
7652 vinfos[5].maxsolutions = _nj5;
7653 std::vector<int> vfree(0);
7654 solutions.AddSolution(vinfos,vfree);
7655 }
7656 }
7657 }
7658 
7659 } else
7660 {
7661 {
7662 IkReal j3array[2], cj3array[2], sj3array[2];
7663 bool j3valid[2]={false};
7664 _nj3 = 2;
7665 CheckValue<IkReal> x616=IKPowWithIntegerCheck(new_r11,-1);
7666 if(!x616.valid){
7667 continue;
7668 }
7669 sj3array[0]=((-1.0)*gconst10*(x616.value));
7670 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7671 {
7672  j3valid[0] = j3valid[1] = true;
7673  j3array[0] = IKasin(sj3array[0]);
7674  cj3array[0] = IKcos(j3array[0]);
7675  sj3array[1] = sj3array[0];
7676  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7677  cj3array[1] = -cj3array[0];
7678 }
7679 else if( isnan(sj3array[0]) )
7680 {
7681  // probably any value will work
7682  j3valid[0] = true;
7683  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7684 }
7685 for(int ij3 = 0; ij3 < 2; ++ij3)
7686 {
7687 if( !j3valid[ij3] )
7688 {
7689  continue;
7690 }
7691 _ij3[0] = ij3; _ij3[1] = -1;
7692 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7693 {
7694 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7695 {
7696  j3valid[iij3]=false; _ij3[1] = iij3; break;
7697 }
7698 }
7699 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7700 {
7701 IkReal evalcond[6];
7702 IkReal x617=IKcos(j3);
7703 IkReal x618=IKsin(j3);
7704 IkReal x619=(gconst10*x618);
7705 evalcond[0]=(gconst10*x617);
7706 evalcond[1]=(new_r00*x617);
7707 evalcond[2]=((-1.0)*new_r11*x617);
7708 evalcond[3]=(gconst10+((new_r00*x618)));
7709 evalcond[4]=(x619+new_r00);
7710 evalcond[5]=(x619+new_r11);
7711 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7712 {
7713 continue;
7714 }
7715 }
7716 
7717 {
7718 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7719 vinfos[0].jointtype = 1;
7720 vinfos[0].foffset = j0;
7721 vinfos[0].indices[0] = _ij0[0];
7722 vinfos[0].indices[1] = _ij0[1];
7723 vinfos[0].maxsolutions = _nj0;
7724 vinfos[1].jointtype = 1;
7725 vinfos[1].foffset = j1;
7726 vinfos[1].indices[0] = _ij1[0];
7727 vinfos[1].indices[1] = _ij1[1];
7728 vinfos[1].maxsolutions = _nj1;
7729 vinfos[2].jointtype = 1;
7730 vinfos[2].foffset = j2;
7731 vinfos[2].indices[0] = _ij2[0];
7732 vinfos[2].indices[1] = _ij2[1];
7733 vinfos[2].maxsolutions = _nj2;
7734 vinfos[3].jointtype = 1;
7735 vinfos[3].foffset = j3;
7736 vinfos[3].indices[0] = _ij3[0];
7737 vinfos[3].indices[1] = _ij3[1];
7738 vinfos[3].maxsolutions = _nj3;
7739 vinfos[4].jointtype = 1;
7740 vinfos[4].foffset = j4;
7741 vinfos[4].indices[0] = _ij4[0];
7742 vinfos[4].indices[1] = _ij4[1];
7743 vinfos[4].maxsolutions = _nj4;
7744 vinfos[5].jointtype = 1;
7745 vinfos[5].foffset = j5;
7746 vinfos[5].indices[0] = _ij5[0];
7747 vinfos[5].indices[1] = _ij5[1];
7748 vinfos[5].maxsolutions = _nj5;
7749 std::vector<int> vfree(0);
7750 solutions.AddSolution(vinfos,vfree);
7751 }
7752 }
7753 }
7754 
7755 }
7756 
7757 }
7758 
7759 } else
7760 {
7761 {
7762 IkReal j3array[2], cj3array[2], sj3array[2];
7763 bool j3valid[2]={false};
7764 _nj3 = 2;
7765 CheckValue<IkReal> x620=IKPowWithIntegerCheck(new_r00,-1);
7766 if(!x620.valid){
7767 continue;
7768 }
7769 sj3array[0]=((-1.0)*gconst10*(x620.value));
7770 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7771 {
7772  j3valid[0] = j3valid[1] = true;
7773  j3array[0] = IKasin(sj3array[0]);
7774  cj3array[0] = IKcos(j3array[0]);
7775  sj3array[1] = sj3array[0];
7776  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7777  cj3array[1] = -cj3array[0];
7778 }
7779 else if( isnan(sj3array[0]) )
7780 {
7781  // probably any value will work
7782  j3valid[0] = true;
7783  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7784 }
7785 for(int ij3 = 0; ij3 < 2; ++ij3)
7786 {
7787 if( !j3valid[ij3] )
7788 {
7789  continue;
7790 }
7791 _ij3[0] = ij3; _ij3[1] = -1;
7792 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7793 {
7794 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7795 {
7796  j3valid[iij3]=false; _ij3[1] = iij3; break;
7797 }
7798 }
7799 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7800 {
7801 IkReal evalcond[6];
7802 IkReal x621=IKcos(j3);
7803 IkReal x622=IKsin(j3);
7804 IkReal x623=(gconst10*x622);
7805 evalcond[0]=(gconst10*x621);
7806 evalcond[1]=(new_r00*x621);
7807 evalcond[2]=((-1.0)*new_r11*x621);
7808 evalcond[3]=(gconst10+((new_r11*x622)));
7809 evalcond[4]=(x623+new_r00);
7810 evalcond[5]=(x623+new_r11);
7811 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7812 {
7813 continue;
7814 }
7815 }
7816 
7817 {
7818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7819 vinfos[0].jointtype = 1;
7820 vinfos[0].foffset = j0;
7821 vinfos[0].indices[0] = _ij0[0];
7822 vinfos[0].indices[1] = _ij0[1];
7823 vinfos[0].maxsolutions = _nj0;
7824 vinfos[1].jointtype = 1;
7825 vinfos[1].foffset = j1;
7826 vinfos[1].indices[0] = _ij1[0];
7827 vinfos[1].indices[1] = _ij1[1];
7828 vinfos[1].maxsolutions = _nj1;
7829 vinfos[2].jointtype = 1;
7830 vinfos[2].foffset = j2;
7831 vinfos[2].indices[0] = _ij2[0];
7832 vinfos[2].indices[1] = _ij2[1];
7833 vinfos[2].maxsolutions = _nj2;
7834 vinfos[3].jointtype = 1;
7835 vinfos[3].foffset = j3;
7836 vinfos[3].indices[0] = _ij3[0];
7837 vinfos[3].indices[1] = _ij3[1];
7838 vinfos[3].maxsolutions = _nj3;
7839 vinfos[4].jointtype = 1;
7840 vinfos[4].foffset = j4;
7841 vinfos[4].indices[0] = _ij4[0];
7842 vinfos[4].indices[1] = _ij4[1];
7843 vinfos[4].maxsolutions = _nj4;
7844 vinfos[5].jointtype = 1;
7845 vinfos[5].foffset = j5;
7846 vinfos[5].indices[0] = _ij5[0];
7847 vinfos[5].indices[1] = _ij5[1];
7848 vinfos[5].maxsolutions = _nj5;
7849 std::vector<int> vfree(0);
7850 solutions.AddSolution(vinfos,vfree);
7851 }
7852 }
7853 }
7854 
7855 }
7856 
7857 }
7858 
7859 }
7860 } while(0);
7861 if( bgotonextstatement )
7862 {
7863 bool bgotonextstatement = true;
7864 do
7865 {
7866 evalcond[0]=IKabs(new_r11);
7867 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7868 {
7869 bgotonextstatement=false;
7870 {
7871 IkReal j3eval[1];
7872 CheckValue<IkReal> x625 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7873 if(!x625.valid){
7874 continue;
7875 }
7876 IkReal x624=((1.0)*(x625.value));
7877 sj4=0;
7878 cj4=1.0;
7879 j4=0;
7880 sj5=gconst10;
7881 cj5=gconst11;
7882 j5=((3.14159265)+(((-1.0)*x624)));
7883 new_r11=0;
7884 IkReal gconst9=((3.14159265358979)+(((-1.0)*x624)));
7885 IkReal gconst10=0;
7886 IkReal x626 = new_r01*new_r01;
7887 if(IKabs(x626)==0){
7888 continue;
7889 }
7890 IkReal gconst11=((1.0)*new_r01*(pow(x626,-0.5)));
7891 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7892 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7893 {
7894 {
7895 IkReal j3eval[1];
7896 CheckValue<IkReal> x628 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7897 if(!x628.valid){
7898 continue;
7899 }
7900 IkReal x627=((1.0)*(x628.value));
7901 sj4=0;
7902 cj4=1.0;
7903 j4=0;
7904 sj5=gconst10;
7905 cj5=gconst11;
7906 j5=((3.14159265)+(((-1.0)*x627)));
7907 new_r11=0;
7908 IkReal gconst9=((3.14159265358979)+(((-1.0)*x627)));
7909 IkReal gconst10=0;
7910 IkReal x629 = new_r01*new_r01;
7911 if(IKabs(x629)==0){
7912 continue;
7913 }
7914 IkReal gconst11=((1.0)*new_r01*(pow(x629,-0.5)));
7915 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
7916 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7917 {
7918 {
7919 IkReal j3eval[1];
7920 CheckValue<IkReal> x631 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7921 if(!x631.valid){
7922 continue;
7923 }
7924 IkReal x630=((1.0)*(x631.value));
7925 sj4=0;
7926 cj4=1.0;
7927 j4=0;
7928 sj5=gconst10;
7929 cj5=gconst11;
7930 j5=((3.14159265)+(((-1.0)*x630)));
7931 new_r11=0;
7932 IkReal gconst9=((3.14159265358979)+(((-1.0)*x630)));
7933 IkReal gconst10=0;
7934 IkReal x632 = new_r01*new_r01;
7935 if(IKabs(x632)==0){
7936 continue;
7937 }
7938 IkReal gconst11=((1.0)*new_r01*(pow(x632,-0.5)));
7939 j3eval[0]=new_r01;
7940 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7941 {
7942 continue; // 3 cases reached
7943 
7944 } else
7945 {
7946 {
7947 IkReal j3array[1], cj3array[1], sj3array[1];
7948 bool j3valid[1]={false};
7949 _nj3 = 1;
7950 CheckValue<IkReal> x633=IKPowWithIntegerCheck(new_r01,-1);
7951 if(!x633.valid){
7952 continue;
7953 }
7954 CheckValue<IkReal> x634=IKPowWithIntegerCheck(gconst11,-1);
7955 if(!x634.valid){
7956 continue;
7957 }
7958 if( IKabs(((-1.0)*gconst11*(x633.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x634.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst11*(x633.value)))+IKsqr((new_r00*(x634.value)))-1) <= IKFAST_SINCOS_THRESH )
7959  continue;
7960 j3array[0]=IKatan2(((-1.0)*gconst11*(x633.value)), (new_r00*(x634.value)));
7961 sj3array[0]=IKsin(j3array[0]);
7962 cj3array[0]=IKcos(j3array[0]);
7963 if( j3array[0] > IKPI )
7964 {
7965  j3array[0]-=IK2PI;
7966 }
7967 else if( j3array[0] < -IKPI )
7968 { j3array[0]+=IK2PI;
7969 }
7970 j3valid[0] = true;
7971 for(int ij3 = 0; ij3 < 1; ++ij3)
7972 {
7973 if( !j3valid[ij3] )
7974 {
7975  continue;
7976 }
7977 _ij3[0] = ij3; _ij3[1] = -1;
7978 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7979 {
7980 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7981 {
7982  j3valid[iij3]=false; _ij3[1] = iij3; break;
7983 }
7984 }
7985 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7986 {
7987 IkReal evalcond[8];
7988 IkReal x635=IKcos(j3);
7989 IkReal x636=IKsin(j3);
7990 IkReal x637=((1.0)*gconst11);
7991 IkReal x638=(gconst11*x635);
7992 evalcond[0]=(new_r01*x635);
7993 evalcond[1]=((-1.0)*x638);
7994 evalcond[2]=(gconst11+((new_r01*x636)));
7995 evalcond[3]=(((gconst11*x636))+new_r01);
7996 evalcond[4]=((((-1.0)*x635*x637))+new_r00);
7997 evalcond[5]=((((-1.0)*x636*x637))+new_r10);
7998 evalcond[6]=((((-1.0)*new_r10*x635))+((new_r00*x636)));
7999 evalcond[7]=((((-1.0)*x637))+((new_r00*x635))+((new_r10*x636)));
8000 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8001 {
8002 continue;
8003 }
8004 }
8005 
8006 {
8007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8008 vinfos[0].jointtype = 1;
8009 vinfos[0].foffset = j0;
8010 vinfos[0].indices[0] = _ij0[0];
8011 vinfos[0].indices[1] = _ij0[1];
8012 vinfos[0].maxsolutions = _nj0;
8013 vinfos[1].jointtype = 1;
8014 vinfos[1].foffset = j1;
8015 vinfos[1].indices[0] = _ij1[0];
8016 vinfos[1].indices[1] = _ij1[1];
8017 vinfos[1].maxsolutions = _nj1;
8018 vinfos[2].jointtype = 1;
8019 vinfos[2].foffset = j2;
8020 vinfos[2].indices[0] = _ij2[0];
8021 vinfos[2].indices[1] = _ij2[1];
8022 vinfos[2].maxsolutions = _nj2;
8023 vinfos[3].jointtype = 1;
8024 vinfos[3].foffset = j3;
8025 vinfos[3].indices[0] = _ij3[0];
8026 vinfos[3].indices[1] = _ij3[1];
8027 vinfos[3].maxsolutions = _nj3;
8028 vinfos[4].jointtype = 1;
8029 vinfos[4].foffset = j4;
8030 vinfos[4].indices[0] = _ij4[0];
8031 vinfos[4].indices[1] = _ij4[1];
8032 vinfos[4].maxsolutions = _nj4;
8033 vinfos[5].jointtype = 1;
8034 vinfos[5].foffset = j5;
8035 vinfos[5].indices[0] = _ij5[0];
8036 vinfos[5].indices[1] = _ij5[1];
8037 vinfos[5].maxsolutions = _nj5;
8038 std::vector<int> vfree(0);
8039 solutions.AddSolution(vinfos,vfree);
8040 }
8041 }
8042 }
8043 
8044 }
8045 
8046 }
8047 
8048 } else
8049 {
8050 {
8051 IkReal j3array[1], cj3array[1], sj3array[1];
8052 bool j3valid[1]={false};
8053 _nj3 = 1;
8054 CheckValue<IkReal> x639=IKPowWithIntegerCheck(IKsign(gconst11),-1);
8055 if(!x639.valid){
8056 continue;
8057 }
8058 CheckValue<IkReal> x640 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8059 if(!x640.valid){
8060 continue;
8061 }
8062 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x639.value)))+(x640.value));
8063 sj3array[0]=IKsin(j3array[0]);
8064 cj3array[0]=IKcos(j3array[0]);
8065 if( j3array[0] > IKPI )
8066 {
8067  j3array[0]-=IK2PI;
8068 }
8069 else if( j3array[0] < -IKPI )
8070 { j3array[0]+=IK2PI;
8071 }
8072 j3valid[0] = true;
8073 for(int ij3 = 0; ij3 < 1; ++ij3)
8074 {
8075 if( !j3valid[ij3] )
8076 {
8077  continue;
8078 }
8079 _ij3[0] = ij3; _ij3[1] = -1;
8080 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8081 {
8082 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8083 {
8084  j3valid[iij3]=false; _ij3[1] = iij3; break;
8085 }
8086 }
8087 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8088 {
8089 IkReal evalcond[8];
8090 IkReal x641=IKcos(j3);
8091 IkReal x642=IKsin(j3);
8092 IkReal x643=((1.0)*gconst11);
8093 IkReal x644=(gconst11*x641);
8094 evalcond[0]=(new_r01*x641);
8095 evalcond[1]=((-1.0)*x644);
8096 evalcond[2]=(gconst11+((new_r01*x642)));
8097 evalcond[3]=(((gconst11*x642))+new_r01);
8098 evalcond[4]=((((-1.0)*x641*x643))+new_r00);
8099 evalcond[5]=((((-1.0)*x642*x643))+new_r10);
8100 evalcond[6]=((((-1.0)*new_r10*x641))+((new_r00*x642)));
8101 evalcond[7]=((((-1.0)*x643))+((new_r10*x642))+((new_r00*x641)));
8102 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8103 {
8104 continue;
8105 }
8106 }
8107 
8108 {
8109 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8110 vinfos[0].jointtype = 1;
8111 vinfos[0].foffset = j0;
8112 vinfos[0].indices[0] = _ij0[0];
8113 vinfos[0].indices[1] = _ij0[1];
8114 vinfos[0].maxsolutions = _nj0;
8115 vinfos[1].jointtype = 1;
8116 vinfos[1].foffset = j1;
8117 vinfos[1].indices[0] = _ij1[0];
8118 vinfos[1].indices[1] = _ij1[1];
8119 vinfos[1].maxsolutions = _nj1;
8120 vinfos[2].jointtype = 1;
8121 vinfos[2].foffset = j2;
8122 vinfos[2].indices[0] = _ij2[0];
8123 vinfos[2].indices[1] = _ij2[1];
8124 vinfos[2].maxsolutions = _nj2;
8125 vinfos[3].jointtype = 1;
8126 vinfos[3].foffset = j3;
8127 vinfos[3].indices[0] = _ij3[0];
8128 vinfos[3].indices[1] = _ij3[1];
8129 vinfos[3].maxsolutions = _nj3;
8130 vinfos[4].jointtype = 1;
8131 vinfos[4].foffset = j4;
8132 vinfos[4].indices[0] = _ij4[0];
8133 vinfos[4].indices[1] = _ij4[1];
8134 vinfos[4].maxsolutions = _nj4;
8135 vinfos[5].jointtype = 1;
8136 vinfos[5].foffset = j5;
8137 vinfos[5].indices[0] = _ij5[0];
8138 vinfos[5].indices[1] = _ij5[1];
8139 vinfos[5].maxsolutions = _nj5;
8140 std::vector<int> vfree(0);
8141 solutions.AddSolution(vinfos,vfree);
8142 }
8143 }
8144 }
8145 
8146 }
8147 
8148 }
8149 
8150 } else
8151 {
8152 {
8153 IkReal j3array[1], cj3array[1], sj3array[1];
8154 bool j3valid[1]={false};
8155 _nj3 = 1;
8156 CheckValue<IkReal> x645=IKPowWithIntegerCheck(IKsign(gconst11),-1);
8157 if(!x645.valid){
8158 continue;
8159 }
8160 CheckValue<IkReal> x646 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8161 if(!x646.valid){
8162 continue;
8163 }
8164 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x645.value)))+(x646.value));
8165 sj3array[0]=IKsin(j3array[0]);
8166 cj3array[0]=IKcos(j3array[0]);
8167 if( j3array[0] > IKPI )
8168 {
8169  j3array[0]-=IK2PI;
8170 }
8171 else if( j3array[0] < -IKPI )
8172 { j3array[0]+=IK2PI;
8173 }
8174 j3valid[0] = true;
8175 for(int ij3 = 0; ij3 < 1; ++ij3)
8176 {
8177 if( !j3valid[ij3] )
8178 {
8179  continue;
8180 }
8181 _ij3[0] = ij3; _ij3[1] = -1;
8182 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8183 {
8184 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8185 {
8186  j3valid[iij3]=false; _ij3[1] = iij3; break;
8187 }
8188 }
8189 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8190 {
8191 IkReal evalcond[8];
8192 IkReal x647=IKcos(j3);
8193 IkReal x648=IKsin(j3);
8194 IkReal x649=((1.0)*gconst11);
8195 IkReal x650=(gconst11*x647);
8196 evalcond[0]=(new_r01*x647);
8197 evalcond[1]=((-1.0)*x650);
8198 evalcond[2]=(gconst11+((new_r01*x648)));
8199 evalcond[3]=(((gconst11*x648))+new_r01);
8200 evalcond[4]=((((-1.0)*x647*x649))+new_r00);
8201 evalcond[5]=((((-1.0)*x648*x649))+new_r10);
8202 evalcond[6]=((((-1.0)*new_r10*x647))+((new_r00*x648)));
8203 evalcond[7]=((((-1.0)*x649))+((new_r10*x648))+((new_r00*x647)));
8204 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8205 {
8206 continue;
8207 }
8208 }
8209 
8210 {
8211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8212 vinfos[0].jointtype = 1;
8213 vinfos[0].foffset = j0;
8214 vinfos[0].indices[0] = _ij0[0];
8215 vinfos[0].indices[1] = _ij0[1];
8216 vinfos[0].maxsolutions = _nj0;
8217 vinfos[1].jointtype = 1;
8218 vinfos[1].foffset = j1;
8219 vinfos[1].indices[0] = _ij1[0];
8220 vinfos[1].indices[1] = _ij1[1];
8221 vinfos[1].maxsolutions = _nj1;
8222 vinfos[2].jointtype = 1;
8223 vinfos[2].foffset = j2;
8224 vinfos[2].indices[0] = _ij2[0];
8225 vinfos[2].indices[1] = _ij2[1];
8226 vinfos[2].maxsolutions = _nj2;
8227 vinfos[3].jointtype = 1;
8228 vinfos[3].foffset = j3;
8229 vinfos[3].indices[0] = _ij3[0];
8230 vinfos[3].indices[1] = _ij3[1];
8231 vinfos[3].maxsolutions = _nj3;
8232 vinfos[4].jointtype = 1;
8233 vinfos[4].foffset = j4;
8234 vinfos[4].indices[0] = _ij4[0];
8235 vinfos[4].indices[1] = _ij4[1];
8236 vinfos[4].maxsolutions = _nj4;
8237 vinfos[5].jointtype = 1;
8238 vinfos[5].foffset = j5;
8239 vinfos[5].indices[0] = _ij5[0];
8240 vinfos[5].indices[1] = _ij5[1];
8241 vinfos[5].maxsolutions = _nj5;
8242 std::vector<int> vfree(0);
8243 solutions.AddSolution(vinfos,vfree);
8244 }
8245 }
8246 }
8247 
8248 }
8249 
8250 }
8251 
8252 }
8253 } while(0);
8254 if( bgotonextstatement )
8255 {
8256 bool bgotonextstatement = true;
8257 do
8258 {
8259 if( 1 )
8260 {
8261 bgotonextstatement=false;
8262 continue; // branch miss [j3]
8263 
8264 }
8265 } while(0);
8266 if( bgotonextstatement )
8267 {
8268 }
8269 }
8270 }
8271 }
8272 }
8273 }
8274 
8275 } else
8276 {
8277 {
8278 IkReal j3array[1], cj3array[1], sj3array[1];
8279 bool j3valid[1]={false};
8280 _nj3 = 1;
8281 IkReal x651=((1.0)*new_r01);
8282 CheckValue<IkReal> x652 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst10*gconst10))))),IkReal((((gconst10*gconst11))+(((-1.0)*new_r11*x651)))),IKFAST_ATAN2_MAGTHRESH);
8283 if(!x652.valid){
8284 continue;
8285 }
8286 CheckValue<IkReal> x653=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst11*x651))+((gconst10*new_r11)))),-1);
8287 if(!x653.valid){
8288 continue;
8289 }
8290 j3array[0]=((-1.5707963267949)+(x652.value)+(((1.5707963267949)*(x653.value))));
8291 sj3array[0]=IKsin(j3array[0]);
8292 cj3array[0]=IKcos(j3array[0]);
8293 if( j3array[0] > IKPI )
8294 {
8295  j3array[0]-=IK2PI;
8296 }
8297 else if( j3array[0] < -IKPI )
8298 { j3array[0]+=IK2PI;
8299 }
8300 j3valid[0] = true;
8301 for(int ij3 = 0; ij3 < 1; ++ij3)
8302 {
8303 if( !j3valid[ij3] )
8304 {
8305  continue;
8306 }
8307 _ij3[0] = ij3; _ij3[1] = -1;
8308 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8309 {
8310 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8311 {
8312  j3valid[iij3]=false; _ij3[1] = iij3; break;
8313 }
8314 }
8315 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8316 {
8317 IkReal evalcond[8];
8318 IkReal x654=IKcos(j3);
8319 IkReal x655=IKsin(j3);
8320 IkReal x656=((1.0)*gconst11);
8321 IkReal x657=(gconst10*x655);
8322 IkReal x658=((1.0)*x654);
8323 IkReal x659=(x654*x656);
8324 evalcond[0]=(gconst10+((new_r01*x654))+((new_r11*x655)));
8325 evalcond[1]=(((gconst10*x654))+((gconst11*x655))+new_r01);
8326 evalcond[2]=(gconst10+(((-1.0)*new_r10*x658))+((new_r00*x655)));
8327 evalcond[3]=(gconst11+(((-1.0)*new_r11*x658))+((new_r01*x655)));
8328 evalcond[4]=((((-1.0)*x659))+x657+new_r00);
8329 evalcond[5]=((((-1.0)*x659))+x657+new_r11);
8330 evalcond[6]=((((-1.0)*x656))+((new_r10*x655))+((new_r00*x654)));
8331 evalcond[7]=((((-1.0)*gconst10*x658))+(((-1.0)*x655*x656))+new_r10);
8332 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8333 {
8334 continue;
8335 }
8336 }
8337 
8338 {
8339 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8340 vinfos[0].jointtype = 1;
8341 vinfos[0].foffset = j0;
8342 vinfos[0].indices[0] = _ij0[0];
8343 vinfos[0].indices[1] = _ij0[1];
8344 vinfos[0].maxsolutions = _nj0;
8345 vinfos[1].jointtype = 1;
8346 vinfos[1].foffset = j1;
8347 vinfos[1].indices[0] = _ij1[0];
8348 vinfos[1].indices[1] = _ij1[1];
8349 vinfos[1].maxsolutions = _nj1;
8350 vinfos[2].jointtype = 1;
8351 vinfos[2].foffset = j2;
8352 vinfos[2].indices[0] = _ij2[0];
8353 vinfos[2].indices[1] = _ij2[1];
8354 vinfos[2].maxsolutions = _nj2;
8355 vinfos[3].jointtype = 1;
8356 vinfos[3].foffset = j3;
8357 vinfos[3].indices[0] = _ij3[0];
8358 vinfos[3].indices[1] = _ij3[1];
8359 vinfos[3].maxsolutions = _nj3;
8360 vinfos[4].jointtype = 1;
8361 vinfos[4].foffset = j4;
8362 vinfos[4].indices[0] = _ij4[0];
8363 vinfos[4].indices[1] = _ij4[1];
8364 vinfos[4].maxsolutions = _nj4;
8365 vinfos[5].jointtype = 1;
8366 vinfos[5].foffset = j5;
8367 vinfos[5].indices[0] = _ij5[0];
8368 vinfos[5].indices[1] = _ij5[1];
8369 vinfos[5].maxsolutions = _nj5;
8370 std::vector<int> vfree(0);
8371 solutions.AddSolution(vinfos,vfree);
8372 }
8373 }
8374 }
8375 
8376 }
8377 
8378 }
8379 
8380 } else
8381 {
8382 {
8383 IkReal j3array[1], cj3array[1], sj3array[1];
8384 bool j3valid[1]={false};
8385 _nj3 = 1;
8386 IkReal x660=((1.0)*gconst10);
8387 CheckValue<IkReal> x661=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*x660))+(((-1.0)*gconst11*new_r10)))),-1);
8388 if(!x661.valid){
8389 continue;
8390 }
8391 CheckValue<IkReal> x662 = IKatan2WithCheck(IkReal(((gconst10*gconst10)+((new_r01*new_r10)))),IkReal(((((-1.0)*gconst11*x660))+((new_r00*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
8392 if(!x662.valid){
8393 continue;
8394 }
8395 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x661.value)))+(x662.value));
8396 sj3array[0]=IKsin(j3array[0]);
8397 cj3array[0]=IKcos(j3array[0]);
8398 if( j3array[0] > IKPI )
8399 {
8400  j3array[0]-=IK2PI;
8401 }
8402 else if( j3array[0] < -IKPI )
8403 { j3array[0]+=IK2PI;
8404 }
8405 j3valid[0] = true;
8406 for(int ij3 = 0; ij3 < 1; ++ij3)
8407 {
8408 if( !j3valid[ij3] )
8409 {
8410  continue;
8411 }
8412 _ij3[0] = ij3; _ij3[1] = -1;
8413 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8414 {
8415 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8416 {
8417  j3valid[iij3]=false; _ij3[1] = iij3; break;
8418 }
8419 }
8420 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8421 {
8422 IkReal evalcond[8];
8423 IkReal x663=IKcos(j3);
8424 IkReal x664=IKsin(j3);
8425 IkReal x665=((1.0)*gconst11);
8426 IkReal x666=(gconst10*x664);
8427 IkReal x667=((1.0)*x663);
8428 IkReal x668=(x663*x665);
8429 evalcond[0]=(gconst10+((new_r11*x664))+((new_r01*x663)));
8430 evalcond[1]=(new_r01+((gconst10*x663))+((gconst11*x664)));
8431 evalcond[2]=(gconst10+(((-1.0)*new_r10*x667))+((new_r00*x664)));
8432 evalcond[3]=((((-1.0)*new_r11*x667))+gconst11+((new_r01*x664)));
8433 evalcond[4]=((((-1.0)*x668))+x666+new_r00);
8434 evalcond[5]=((((-1.0)*x668))+x666+new_r11);
8435 evalcond[6]=(((new_r10*x664))+(((-1.0)*x665))+((new_r00*x663)));
8436 evalcond[7]=((((-1.0)*gconst10*x667))+new_r10+(((-1.0)*x664*x665)));
8437 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8438 {
8439 continue;
8440 }
8441 }
8442 
8443 {
8444 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8445 vinfos[0].jointtype = 1;
8446 vinfos[0].foffset = j0;
8447 vinfos[0].indices[0] = _ij0[0];
8448 vinfos[0].indices[1] = _ij0[1];
8449 vinfos[0].maxsolutions = _nj0;
8450 vinfos[1].jointtype = 1;
8451 vinfos[1].foffset = j1;
8452 vinfos[1].indices[0] = _ij1[0];
8453 vinfos[1].indices[1] = _ij1[1];
8454 vinfos[1].maxsolutions = _nj1;
8455 vinfos[2].jointtype = 1;
8456 vinfos[2].foffset = j2;
8457 vinfos[2].indices[0] = _ij2[0];
8458 vinfos[2].indices[1] = _ij2[1];
8459 vinfos[2].maxsolutions = _nj2;
8460 vinfos[3].jointtype = 1;
8461 vinfos[3].foffset = j3;
8462 vinfos[3].indices[0] = _ij3[0];
8463 vinfos[3].indices[1] = _ij3[1];
8464 vinfos[3].maxsolutions = _nj3;
8465 vinfos[4].jointtype = 1;
8466 vinfos[4].foffset = j4;
8467 vinfos[4].indices[0] = _ij4[0];
8468 vinfos[4].indices[1] = _ij4[1];
8469 vinfos[4].maxsolutions = _nj4;
8470 vinfos[5].jointtype = 1;
8471 vinfos[5].foffset = j5;
8472 vinfos[5].indices[0] = _ij5[0];
8473 vinfos[5].indices[1] = _ij5[1];
8474 vinfos[5].maxsolutions = _nj5;
8475 std::vector<int> vfree(0);
8476 solutions.AddSolution(vinfos,vfree);
8477 }
8478 }
8479 }
8480 
8481 }
8482 
8483 }
8484 
8485 } else
8486 {
8487 {
8488 IkReal j3array[1], cj3array[1], sj3array[1];
8489 bool j3valid[1]={false};
8490 _nj3 = 1;
8491 IkReal x669=((1.0)*new_r11);
8492 CheckValue<IkReal> x670=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x669))+(((-1.0)*new_r00*new_r01)))),-1);
8493 if(!x670.valid){
8494 continue;
8495 }
8496 CheckValue<IkReal> x671 = IKatan2WithCheck(IkReal((((gconst10*new_r01))+((gconst10*new_r10)))),IkReal(((((-1.0)*gconst10*x669))+((gconst10*new_r00)))),IKFAST_ATAN2_MAGTHRESH);
8497 if(!x671.valid){
8498 continue;
8499 }
8500 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x670.value)))+(x671.value));
8501 sj3array[0]=IKsin(j3array[0]);
8502 cj3array[0]=IKcos(j3array[0]);
8503 if( j3array[0] > IKPI )
8504 {
8505  j3array[0]-=IK2PI;
8506 }
8507 else if( j3array[0] < -IKPI )
8508 { j3array[0]+=IK2PI;
8509 }
8510 j3valid[0] = true;
8511 for(int ij3 = 0; ij3 < 1; ++ij3)
8512 {
8513 if( !j3valid[ij3] )
8514 {
8515  continue;
8516 }
8517 _ij3[0] = ij3; _ij3[1] = -1;
8518 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8519 {
8520 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8521 {
8522  j3valid[iij3]=false; _ij3[1] = iij3; break;
8523 }
8524 }
8525 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8526 {
8527 IkReal evalcond[8];
8528 IkReal x672=IKcos(j3);
8529 IkReal x673=IKsin(j3);
8530 IkReal x674=((1.0)*gconst11);
8531 IkReal x675=(gconst10*x673);
8532 IkReal x676=((1.0)*x672);
8533 IkReal x677=(x672*x674);
8534 evalcond[0]=(((new_r11*x673))+((new_r01*x672))+gconst10);
8535 evalcond[1]=(((gconst11*x673))+((gconst10*x672))+new_r01);
8536 evalcond[2]=(((new_r00*x673))+gconst10+(((-1.0)*new_r10*x676)));
8537 evalcond[3]=(((new_r01*x673))+gconst11+(((-1.0)*new_r11*x676)));
8538 evalcond[4]=(x675+new_r00+(((-1.0)*x677)));
8539 evalcond[5]=(x675+new_r11+(((-1.0)*x677)));
8540 evalcond[6]=(((new_r00*x672))+((new_r10*x673))+(((-1.0)*x674)));
8541 evalcond[7]=(new_r10+(((-1.0)*x673*x674))+(((-1.0)*gconst10*x676)));
8542 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8543 {
8544 continue;
8545 }
8546 }
8547 
8548 {
8549 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8550 vinfos[0].jointtype = 1;
8551 vinfos[0].foffset = j0;
8552 vinfos[0].indices[0] = _ij0[0];
8553 vinfos[0].indices[1] = _ij0[1];
8554 vinfos[0].maxsolutions = _nj0;
8555 vinfos[1].jointtype = 1;
8556 vinfos[1].foffset = j1;
8557 vinfos[1].indices[0] = _ij1[0];
8558 vinfos[1].indices[1] = _ij1[1];
8559 vinfos[1].maxsolutions = _nj1;
8560 vinfos[2].jointtype = 1;
8561 vinfos[2].foffset = j2;
8562 vinfos[2].indices[0] = _ij2[0];
8563 vinfos[2].indices[1] = _ij2[1];
8564 vinfos[2].maxsolutions = _nj2;
8565 vinfos[3].jointtype = 1;
8566 vinfos[3].foffset = j3;
8567 vinfos[3].indices[0] = _ij3[0];
8568 vinfos[3].indices[1] = _ij3[1];
8569 vinfos[3].maxsolutions = _nj3;
8570 vinfos[4].jointtype = 1;
8571 vinfos[4].foffset = j4;
8572 vinfos[4].indices[0] = _ij4[0];
8573 vinfos[4].indices[1] = _ij4[1];
8574 vinfos[4].maxsolutions = _nj4;
8575 vinfos[5].jointtype = 1;
8576 vinfos[5].foffset = j5;
8577 vinfos[5].indices[0] = _ij5[0];
8578 vinfos[5].indices[1] = _ij5[1];
8579 vinfos[5].maxsolutions = _nj5;
8580 std::vector<int> vfree(0);
8581 solutions.AddSolution(vinfos,vfree);
8582 }
8583 }
8584 }
8585 
8586 }
8587 
8588 }
8589 
8590 }
8591 } while(0);
8592 if( bgotonextstatement )
8593 {
8594 bool bgotonextstatement = true;
8595 do
8596 {
8597 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8598 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8599 {
8600 bgotonextstatement=false;
8601 {
8602 IkReal j3array[1], cj3array[1], sj3array[1];
8603 bool j3valid[1]={false};
8604 _nj3 = 1;
8605 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
8606  continue;
8607 j3array[0]=IKatan2(((-1.0)*new_r01), new_r00);
8608 sj3array[0]=IKsin(j3array[0]);
8609 cj3array[0]=IKcos(j3array[0]);
8610 if( j3array[0] > IKPI )
8611 {
8612  j3array[0]-=IK2PI;
8613 }
8614 else if( j3array[0] < -IKPI )
8615 { j3array[0]+=IK2PI;
8616 }
8617 j3valid[0] = true;
8618 for(int ij3 = 0; ij3 < 1; ++ij3)
8619 {
8620 if( !j3valid[ij3] )
8621 {
8622  continue;
8623 }
8624 _ij3[0] = ij3; _ij3[1] = -1;
8625 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8626 {
8627 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8628 {
8629  j3valid[iij3]=false; _ij3[1] = iij3; break;
8630 }
8631 }
8632 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8633 {
8634 IkReal evalcond[8];
8635 IkReal x678=IKsin(j3);
8636 IkReal x679=IKcos(j3);
8637 IkReal x680=((1.0)*x679);
8638 evalcond[0]=(x678+new_r01);
8639 evalcond[1]=((((-1.0)*x680))+new_r00);
8640 evalcond[2]=((((-1.0)*x680))+new_r11);
8641 evalcond[3]=((((-1.0)*x678))+new_r10);
8642 evalcond[4]=(((new_r11*x678))+((new_r01*x679)));
8643 evalcond[5]=((((-1.0)*new_r10*x680))+((new_r00*x678)));
8644 evalcond[6]=((-1.0)+((new_r00*x679))+((new_r10*x678)));
8645 evalcond[7]=((1.0)+((new_r01*x678))+(((-1.0)*new_r11*x680)));
8646 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8647 {
8648 continue;
8649 }
8650 }
8651 
8652 {
8653 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8654 vinfos[0].jointtype = 1;
8655 vinfos[0].foffset = j0;
8656 vinfos[0].indices[0] = _ij0[0];
8657 vinfos[0].indices[1] = _ij0[1];
8658 vinfos[0].maxsolutions = _nj0;
8659 vinfos[1].jointtype = 1;
8660 vinfos[1].foffset = j1;
8661 vinfos[1].indices[0] = _ij1[0];
8662 vinfos[1].indices[1] = _ij1[1];
8663 vinfos[1].maxsolutions = _nj1;
8664 vinfos[2].jointtype = 1;
8665 vinfos[2].foffset = j2;
8666 vinfos[2].indices[0] = _ij2[0];
8667 vinfos[2].indices[1] = _ij2[1];
8668 vinfos[2].maxsolutions = _nj2;
8669 vinfos[3].jointtype = 1;
8670 vinfos[3].foffset = j3;
8671 vinfos[3].indices[0] = _ij3[0];
8672 vinfos[3].indices[1] = _ij3[1];
8673 vinfos[3].maxsolutions = _nj3;
8674 vinfos[4].jointtype = 1;
8675 vinfos[4].foffset = j4;
8676 vinfos[4].indices[0] = _ij4[0];
8677 vinfos[4].indices[1] = _ij4[1];
8678 vinfos[4].maxsolutions = _nj4;
8679 vinfos[5].jointtype = 1;
8680 vinfos[5].foffset = j5;
8681 vinfos[5].indices[0] = _ij5[0];
8682 vinfos[5].indices[1] = _ij5[1];
8683 vinfos[5].maxsolutions = _nj5;
8684 std::vector<int> vfree(0);
8685 solutions.AddSolution(vinfos,vfree);
8686 }
8687 }
8688 }
8689 
8690 }
8691 } while(0);
8692 if( bgotonextstatement )
8693 {
8694 bool bgotonextstatement = true;
8695 do
8696 {
8697 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8698 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8699 {
8700 bgotonextstatement=false;
8701 {
8702 IkReal j3array[1], cj3array[1], sj3array[1];
8703 bool j3valid[1]={false};
8704 _nj3 = 1;
8705 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
8706  continue;
8707 j3array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8708 sj3array[0]=IKsin(j3array[0]);
8709 cj3array[0]=IKcos(j3array[0]);
8710 if( j3array[0] > IKPI )
8711 {
8712  j3array[0]-=IK2PI;
8713 }
8714 else if( j3array[0] < -IKPI )
8715 { j3array[0]+=IK2PI;
8716 }
8717 j3valid[0] = true;
8718 for(int ij3 = 0; ij3 < 1; ++ij3)
8719 {
8720 if( !j3valid[ij3] )
8721 {
8722  continue;
8723 }
8724 _ij3[0] = ij3; _ij3[1] = -1;
8725 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8726 {
8727 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8728 {
8729  j3valid[iij3]=false; _ij3[1] = iij3; break;
8730 }
8731 }
8732 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8733 {
8734 IkReal evalcond[8];
8735 IkReal x681=IKcos(j3);
8736 IkReal x682=IKsin(j3);
8737 IkReal x683=((1.0)*x681);
8738 evalcond[0]=(x681+new_r00);
8739 evalcond[1]=(x681+new_r11);
8740 evalcond[2]=(x682+new_r10);
8741 evalcond[3]=(new_r01+(((-1.0)*x682)));
8742 evalcond[4]=(((new_r01*x681))+((new_r11*x682)));
8743 evalcond[5]=((((-1.0)*new_r10*x683))+((new_r00*x682)));
8744 evalcond[6]=((1.0)+((new_r00*x681))+((new_r10*x682)));
8745 evalcond[7]=((-1.0)+((new_r01*x682))+(((-1.0)*new_r11*x683)));
8746 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8747 {
8748 continue;
8749 }
8750 }
8751 
8752 {
8753 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8754 vinfos[0].jointtype = 1;
8755 vinfos[0].foffset = j0;
8756 vinfos[0].indices[0] = _ij0[0];
8757 vinfos[0].indices[1] = _ij0[1];
8758 vinfos[0].maxsolutions = _nj0;
8759 vinfos[1].jointtype = 1;
8760 vinfos[1].foffset = j1;
8761 vinfos[1].indices[0] = _ij1[0];
8762 vinfos[1].indices[1] = _ij1[1];
8763 vinfos[1].maxsolutions = _nj1;
8764 vinfos[2].jointtype = 1;
8765 vinfos[2].foffset = j2;
8766 vinfos[2].indices[0] = _ij2[0];
8767 vinfos[2].indices[1] = _ij2[1];
8768 vinfos[2].maxsolutions = _nj2;
8769 vinfos[3].jointtype = 1;
8770 vinfos[3].foffset = j3;
8771 vinfos[3].indices[0] = _ij3[0];
8772 vinfos[3].indices[1] = _ij3[1];
8773 vinfos[3].maxsolutions = _nj3;
8774 vinfos[4].jointtype = 1;
8775 vinfos[4].foffset = j4;
8776 vinfos[4].indices[0] = _ij4[0];
8777 vinfos[4].indices[1] = _ij4[1];
8778 vinfos[4].maxsolutions = _nj4;
8779 vinfos[5].jointtype = 1;
8780 vinfos[5].foffset = j5;
8781 vinfos[5].indices[0] = _ij5[0];
8782 vinfos[5].indices[1] = _ij5[1];
8783 vinfos[5].maxsolutions = _nj5;
8784 std::vector<int> vfree(0);
8785 solutions.AddSolution(vinfos,vfree);
8786 }
8787 }
8788 }
8789 
8790 }
8791 } while(0);
8792 if( bgotonextstatement )
8793 {
8794 bool bgotonextstatement = true;
8795 do
8796 {
8797 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
8798 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8799 {
8800 bgotonextstatement=false;
8801 {
8802 IkReal j3eval[3];
8803 sj4=0;
8804 cj4=1.0;
8805 j4=0;
8806 new_r11=0;
8807 new_r00=0;
8808 j3eval[0]=new_r01;
8809 j3eval[1]=IKsign(new_r01);
8810 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
8811 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
8812 {
8813 {
8814 IkReal j3eval[2];
8815 sj4=0;
8816 cj4=1.0;
8817 j4=0;
8818 new_r11=0;
8819 new_r00=0;
8820 j3eval[0]=new_r01;
8821 j3eval[1]=new_r10;
8822 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8823 {
8824 {
8825 IkReal j3eval[2];
8826 sj4=0;
8827 cj4=1.0;
8828 j4=0;
8829 new_r11=0;
8830 new_r00=0;
8831 j3eval[0]=new_r01;
8832 j3eval[1]=sj5;
8833 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8834 {
8835 {
8836 IkReal evalcond[1];
8837 bool bgotonextstatement = true;
8838 do
8839 {
8840 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8841 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8842 {
8843 bgotonextstatement=false;
8844 {
8845 IkReal j3array[2], cj3array[2], sj3array[2];
8846 bool j3valid[2]={false};
8847 _nj3 = 2;
8848 sj3array[0]=new_r10;
8849 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8850 {
8851  j3valid[0] = j3valid[1] = true;
8852  j3array[0] = IKasin(sj3array[0]);
8853  cj3array[0] = IKcos(j3array[0]);
8854  sj3array[1] = sj3array[0];
8855  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8856  cj3array[1] = -cj3array[0];
8857 }
8858 else if( isnan(sj3array[0]) )
8859 {
8860  // probably any value will work
8861  j3valid[0] = true;
8862  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8863 }
8864 for(int ij3 = 0; ij3 < 2; ++ij3)
8865 {
8866 if( !j3valid[ij3] )
8867 {
8868  continue;
8869 }
8870 _ij3[0] = ij3; _ij3[1] = -1;
8871 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8872 {
8873 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8874 {
8875  j3valid[iij3]=false; _ij3[1] = iij3; break;
8876 }
8877 }
8878 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8879 {
8880 IkReal evalcond[6];
8881 IkReal x684=IKcos(j3);
8882 IkReal x685=IKsin(j3);
8883 IkReal x686=((-1.0)*x684);
8884 evalcond[0]=(new_r01*x684);
8885 evalcond[1]=(x685+new_r01);
8886 evalcond[2]=x686;
8887 evalcond[3]=(new_r10*x686);
8888 evalcond[4]=((1.0)+((new_r01*x685)));
8889 evalcond[5]=((-1.0)+((new_r10*x685)));
8890 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8891 {
8892 continue;
8893 }
8894 }
8895 
8896 {
8897 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8898 vinfos[0].jointtype = 1;
8899 vinfos[0].foffset = j0;
8900 vinfos[0].indices[0] = _ij0[0];
8901 vinfos[0].indices[1] = _ij0[1];
8902 vinfos[0].maxsolutions = _nj0;
8903 vinfos[1].jointtype = 1;
8904 vinfos[1].foffset = j1;
8905 vinfos[1].indices[0] = _ij1[0];
8906 vinfos[1].indices[1] = _ij1[1];
8907 vinfos[1].maxsolutions = _nj1;
8908 vinfos[2].jointtype = 1;
8909 vinfos[2].foffset = j2;
8910 vinfos[2].indices[0] = _ij2[0];
8911 vinfos[2].indices[1] = _ij2[1];
8912 vinfos[2].maxsolutions = _nj2;
8913 vinfos[3].jointtype = 1;
8914 vinfos[3].foffset = j3;
8915 vinfos[3].indices[0] = _ij3[0];
8916 vinfos[3].indices[1] = _ij3[1];
8917 vinfos[3].maxsolutions = _nj3;
8918 vinfos[4].jointtype = 1;
8919 vinfos[4].foffset = j4;
8920 vinfos[4].indices[0] = _ij4[0];
8921 vinfos[4].indices[1] = _ij4[1];
8922 vinfos[4].maxsolutions = _nj4;
8923 vinfos[5].jointtype = 1;
8924 vinfos[5].foffset = j5;
8925 vinfos[5].indices[0] = _ij5[0];
8926 vinfos[5].indices[1] = _ij5[1];
8927 vinfos[5].maxsolutions = _nj5;
8928 std::vector<int> vfree(0);
8929 solutions.AddSolution(vinfos,vfree);
8930 }
8931 }
8932 }
8933 
8934 }
8935 } while(0);
8936 if( bgotonextstatement )
8937 {
8938 bool bgotonextstatement = true;
8939 do
8940 {
8941 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8942 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8943 {
8944 bgotonextstatement=false;
8945 {
8946 IkReal j3array[2], cj3array[2], sj3array[2];
8947 bool j3valid[2]={false};
8948 _nj3 = 2;
8949 sj3array[0]=new_r01;
8950 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8951 {
8952  j3valid[0] = j3valid[1] = true;
8953  j3array[0] = IKasin(sj3array[0]);
8954  cj3array[0] = IKcos(j3array[0]);
8955  sj3array[1] = sj3array[0];
8956  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8957  cj3array[1] = -cj3array[0];
8958 }
8959 else if( isnan(sj3array[0]) )
8960 {
8961  // probably any value will work
8962  j3valid[0] = true;
8963  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8964 }
8965 for(int ij3 = 0; ij3 < 2; ++ij3)
8966 {
8967 if( !j3valid[ij3] )
8968 {
8969  continue;
8970 }
8971 _ij3[0] = ij3; _ij3[1] = -1;
8972 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8973 {
8974 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8975 {
8976  j3valid[iij3]=false; _ij3[1] = iij3; break;
8977 }
8978 }
8979 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8980 {
8981 IkReal evalcond[6];
8982 IkReal x687=IKcos(j3);
8983 IkReal x688=IKsin(j3);
8984 evalcond[0]=x687;
8985 evalcond[1]=(new_r01*x687);
8986 evalcond[2]=(x688+new_r10);
8987 evalcond[3]=((-1.0)*new_r10*x687);
8988 evalcond[4]=((-1.0)+((new_r01*x688)));
8989 evalcond[5]=((1.0)+((new_r10*x688)));
8990 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8991 {
8992 continue;
8993 }
8994 }
8995 
8996 {
8997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8998 vinfos[0].jointtype = 1;
8999 vinfos[0].foffset = j0;
9000 vinfos[0].indices[0] = _ij0[0];
9001 vinfos[0].indices[1] = _ij0[1];
9002 vinfos[0].maxsolutions = _nj0;
9003 vinfos[1].jointtype = 1;
9004 vinfos[1].foffset = j1;
9005 vinfos[1].indices[0] = _ij1[0];
9006 vinfos[1].indices[1] = _ij1[1];
9007 vinfos[1].maxsolutions = _nj1;
9008 vinfos[2].jointtype = 1;
9009 vinfos[2].foffset = j2;
9010 vinfos[2].indices[0] = _ij2[0];
9011 vinfos[2].indices[1] = _ij2[1];
9012 vinfos[2].maxsolutions = _nj2;
9013 vinfos[3].jointtype = 1;
9014 vinfos[3].foffset = j3;
9015 vinfos[3].indices[0] = _ij3[0];
9016 vinfos[3].indices[1] = _ij3[1];
9017 vinfos[3].maxsolutions = _nj3;
9018 vinfos[4].jointtype = 1;
9019 vinfos[4].foffset = j4;
9020 vinfos[4].indices[0] = _ij4[0];
9021 vinfos[4].indices[1] = _ij4[1];
9022 vinfos[4].maxsolutions = _nj4;
9023 vinfos[5].jointtype = 1;
9024 vinfos[5].foffset = j5;
9025 vinfos[5].indices[0] = _ij5[0];
9026 vinfos[5].indices[1] = _ij5[1];
9027 vinfos[5].maxsolutions = _nj5;
9028 std::vector<int> vfree(0);
9029 solutions.AddSolution(vinfos,vfree);
9030 }
9031 }
9032 }
9033 
9034 }
9035 } while(0);
9036 if( bgotonextstatement )
9037 {
9038 bool bgotonextstatement = true;
9039 do
9040 {
9041 if( 1 )
9042 {
9043 bgotonextstatement=false;
9044 continue; // branch miss [j3]
9045 
9046 }
9047 } while(0);
9048 if( bgotonextstatement )
9049 {
9050 }
9051 }
9052 }
9053 }
9054 
9055 } else
9056 {
9057 {
9058 IkReal j3array[1], cj3array[1], sj3array[1];
9059 bool j3valid[1]={false};
9060 _nj3 = 1;
9061 CheckValue<IkReal> x690=IKPowWithIntegerCheck(new_r01,-1);
9062 if(!x690.valid){
9063 continue;
9064 }
9065 IkReal x689=x690.value;
9066 CheckValue<IkReal> x691=IKPowWithIntegerCheck(sj5,-1);
9067 if(!x691.valid){
9068 continue;
9069 }
9070 CheckValue<IkReal> x692=IKPowWithIntegerCheck(x689,-2);
9071 if(!x692.valid){
9072 continue;
9073 }
9074 if( IKabs(((-1.0)*cj5*x689)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x689*(x691.value)*(((((-1.0)*(x692.value)))+(cj5*cj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*x689))+IKsqr((x689*(x691.value)*(((((-1.0)*(x692.value)))+(cj5*cj5)))))-1) <= IKFAST_SINCOS_THRESH )
9075  continue;
9076 j3array[0]=IKatan2(((-1.0)*cj5*x689), (x689*(x691.value)*(((((-1.0)*(x692.value)))+(cj5*cj5)))));
9077 sj3array[0]=IKsin(j3array[0]);
9078 cj3array[0]=IKcos(j3array[0]);
9079 if( j3array[0] > IKPI )
9080 {
9081  j3array[0]-=IK2PI;
9082 }
9083 else if( j3array[0] < -IKPI )
9084 { j3array[0]+=IK2PI;
9085 }
9086 j3valid[0] = true;
9087 for(int ij3 = 0; ij3 < 1; ++ij3)
9088 {
9089 if( !j3valid[ij3] )
9090 {
9091  continue;
9092 }
9093 _ij3[0] = ij3; _ij3[1] = -1;
9094 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9095 {
9096 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9097 {
9098  j3valid[iij3]=false; _ij3[1] = iij3; break;
9099 }
9100 }
9101 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9102 {
9103 IkReal evalcond[7];
9104 IkReal x693=IKcos(j3);
9105 IkReal x694=IKsin(j3);
9106 IkReal x695=((1.0)*cj5);
9107 IkReal x696=(sj5*x693);
9108 evalcond[0]=(cj5+((new_r01*x694)));
9109 evalcond[1]=(sj5+((new_r01*x693)));
9110 evalcond[2]=(sj5+(((-1.0)*new_r10*x693)));
9111 evalcond[3]=(((new_r10*x694))+(((-1.0)*x695)));
9112 evalcond[4]=(x696+new_r01+((cj5*x694)));
9113 evalcond[5]=((((-1.0)*x693*x695))+((sj5*x694)));
9114 evalcond[6]=((((-1.0)*x694*x695))+new_r10+(((-1.0)*x696)));
9115 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9116 {
9117 continue;
9118 }
9119 }
9120 
9121 {
9122 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9123 vinfos[0].jointtype = 1;
9124 vinfos[0].foffset = j0;
9125 vinfos[0].indices[0] = _ij0[0];
9126 vinfos[0].indices[1] = _ij0[1];
9127 vinfos[0].maxsolutions = _nj0;
9128 vinfos[1].jointtype = 1;
9129 vinfos[1].foffset = j1;
9130 vinfos[1].indices[0] = _ij1[0];
9131 vinfos[1].indices[1] = _ij1[1];
9132 vinfos[1].maxsolutions = _nj1;
9133 vinfos[2].jointtype = 1;
9134 vinfos[2].foffset = j2;
9135 vinfos[2].indices[0] = _ij2[0];
9136 vinfos[2].indices[1] = _ij2[1];
9137 vinfos[2].maxsolutions = _nj2;
9138 vinfos[3].jointtype = 1;
9139 vinfos[3].foffset = j3;
9140 vinfos[3].indices[0] = _ij3[0];
9141 vinfos[3].indices[1] = _ij3[1];
9142 vinfos[3].maxsolutions = _nj3;
9143 vinfos[4].jointtype = 1;
9144 vinfos[4].foffset = j4;
9145 vinfos[4].indices[0] = _ij4[0];
9146 vinfos[4].indices[1] = _ij4[1];
9147 vinfos[4].maxsolutions = _nj4;
9148 vinfos[5].jointtype = 1;
9149 vinfos[5].foffset = j5;
9150 vinfos[5].indices[0] = _ij5[0];
9151 vinfos[5].indices[1] = _ij5[1];
9152 vinfos[5].maxsolutions = _nj5;
9153 std::vector<int> vfree(0);
9154 solutions.AddSolution(vinfos,vfree);
9155 }
9156 }
9157 }
9158 
9159 }
9160 
9161 }
9162 
9163 } else
9164 {
9165 {
9166 IkReal j3array[1], cj3array[1], sj3array[1];
9167 bool j3valid[1]={false};
9168 _nj3 = 1;
9169 CheckValue<IkReal> x697=IKPowWithIntegerCheck(new_r01,-1);
9170 if(!x697.valid){
9171 continue;
9172 }
9173 CheckValue<IkReal> x698=IKPowWithIntegerCheck(new_r10,-1);
9174 if(!x698.valid){
9175 continue;
9176 }
9177 if( IKabs(((-1.0)*cj5*(x697.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x698.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x697.value)))+IKsqr((sj5*(x698.value)))-1) <= IKFAST_SINCOS_THRESH )
9178  continue;
9179 j3array[0]=IKatan2(((-1.0)*cj5*(x697.value)), (sj5*(x698.value)));
9180 sj3array[0]=IKsin(j3array[0]);
9181 cj3array[0]=IKcos(j3array[0]);
9182 if( j3array[0] > IKPI )
9183 {
9184  j3array[0]-=IK2PI;
9185 }
9186 else if( j3array[0] < -IKPI )
9187 { j3array[0]+=IK2PI;
9188 }
9189 j3valid[0] = true;
9190 for(int ij3 = 0; ij3 < 1; ++ij3)
9191 {
9192 if( !j3valid[ij3] )
9193 {
9194  continue;
9195 }
9196 _ij3[0] = ij3; _ij3[1] = -1;
9197 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9198 {
9199 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9200 {
9201  j3valid[iij3]=false; _ij3[1] = iij3; break;
9202 }
9203 }
9204 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9205 {
9206 IkReal evalcond[7];
9207 IkReal x699=IKcos(j3);
9208 IkReal x700=IKsin(j3);
9209 IkReal x701=((1.0)*cj5);
9210 IkReal x702=(sj5*x699);
9211 evalcond[0]=(cj5+((new_r01*x700)));
9212 evalcond[1]=(sj5+((new_r01*x699)));
9213 evalcond[2]=(sj5+(((-1.0)*new_r10*x699)));
9214 evalcond[3]=((((-1.0)*x701))+((new_r10*x700)));
9215 evalcond[4]=(((cj5*x700))+x702+new_r01);
9216 evalcond[5]=(((sj5*x700))+(((-1.0)*x699*x701)));
9217 evalcond[6]=((((-1.0)*x702))+new_r10+(((-1.0)*x700*x701)));
9218 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9219 {
9220 continue;
9221 }
9222 }
9223 
9224 {
9225 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9226 vinfos[0].jointtype = 1;
9227 vinfos[0].foffset = j0;
9228 vinfos[0].indices[0] = _ij0[0];
9229 vinfos[0].indices[1] = _ij0[1];
9230 vinfos[0].maxsolutions = _nj0;
9231 vinfos[1].jointtype = 1;
9232 vinfos[1].foffset = j1;
9233 vinfos[1].indices[0] = _ij1[0];
9234 vinfos[1].indices[1] = _ij1[1];
9235 vinfos[1].maxsolutions = _nj1;
9236 vinfos[2].jointtype = 1;
9237 vinfos[2].foffset = j2;
9238 vinfos[2].indices[0] = _ij2[0];
9239 vinfos[2].indices[1] = _ij2[1];
9240 vinfos[2].maxsolutions = _nj2;
9241 vinfos[3].jointtype = 1;
9242 vinfos[3].foffset = j3;
9243 vinfos[3].indices[0] = _ij3[0];
9244 vinfos[3].indices[1] = _ij3[1];
9245 vinfos[3].maxsolutions = _nj3;
9246 vinfos[4].jointtype = 1;
9247 vinfos[4].foffset = j4;
9248 vinfos[4].indices[0] = _ij4[0];
9249 vinfos[4].indices[1] = _ij4[1];
9250 vinfos[4].maxsolutions = _nj4;
9251 vinfos[5].jointtype = 1;
9252 vinfos[5].foffset = j5;
9253 vinfos[5].indices[0] = _ij5[0];
9254 vinfos[5].indices[1] = _ij5[1];
9255 vinfos[5].maxsolutions = _nj5;
9256 std::vector<int> vfree(0);
9257 solutions.AddSolution(vinfos,vfree);
9258 }
9259 }
9260 }
9261 
9262 }
9263 
9264 }
9265 
9266 } else
9267 {
9268 {
9269 IkReal j3array[1], cj3array[1], sj3array[1];
9270 bool j3valid[1]={false};
9271 _nj3 = 1;
9272 CheckValue<IkReal> x703=IKPowWithIntegerCheck(IKsign(new_r01),-1);
9273 if(!x703.valid){
9274 continue;
9275 }
9276 CheckValue<IkReal> x704 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(((-1.0)*sj5)),IKFAST_ATAN2_MAGTHRESH);
9277 if(!x704.valid){
9278 continue;
9279 }
9280 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x703.value)))+(x704.value));
9281 sj3array[0]=IKsin(j3array[0]);
9282 cj3array[0]=IKcos(j3array[0]);
9283 if( j3array[0] > IKPI )
9284 {
9285  j3array[0]-=IK2PI;
9286 }
9287 else if( j3array[0] < -IKPI )
9288 { j3array[0]+=IK2PI;
9289 }
9290 j3valid[0] = true;
9291 for(int ij3 = 0; ij3 < 1; ++ij3)
9292 {
9293 if( !j3valid[ij3] )
9294 {
9295  continue;
9296 }
9297 _ij3[0] = ij3; _ij3[1] = -1;
9298 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9299 {
9300 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9301 {
9302  j3valid[iij3]=false; _ij3[1] = iij3; break;
9303 }
9304 }
9305 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9306 {
9307 IkReal evalcond[7];
9308 IkReal x705=IKcos(j3);
9309 IkReal x706=IKsin(j3);
9310 IkReal x707=((1.0)*cj5);
9311 IkReal x708=(sj5*x705);
9312 evalcond[0]=(cj5+((new_r01*x706)));
9313 evalcond[1]=(sj5+((new_r01*x705)));
9314 evalcond[2]=(sj5+(((-1.0)*new_r10*x705)));
9315 evalcond[3]=((((-1.0)*x707))+((new_r10*x706)));
9316 evalcond[4]=(((cj5*x706))+x708+new_r01);
9317 evalcond[5]=(((sj5*x706))+(((-1.0)*x705*x707)));
9318 evalcond[6]=((((-1.0)*x706*x707))+(((-1.0)*x708))+new_r10);
9319 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9320 {
9321 continue;
9322 }
9323 }
9324 
9325 {
9326 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9327 vinfos[0].jointtype = 1;
9328 vinfos[0].foffset = j0;
9329 vinfos[0].indices[0] = _ij0[0];
9330 vinfos[0].indices[1] = _ij0[1];
9331 vinfos[0].maxsolutions = _nj0;
9332 vinfos[1].jointtype = 1;
9333 vinfos[1].foffset = j1;
9334 vinfos[1].indices[0] = _ij1[0];
9335 vinfos[1].indices[1] = _ij1[1];
9336 vinfos[1].maxsolutions = _nj1;
9337 vinfos[2].jointtype = 1;
9338 vinfos[2].foffset = j2;
9339 vinfos[2].indices[0] = _ij2[0];
9340 vinfos[2].indices[1] = _ij2[1];
9341 vinfos[2].maxsolutions = _nj2;
9342 vinfos[3].jointtype = 1;
9343 vinfos[3].foffset = j3;
9344 vinfos[3].indices[0] = _ij3[0];
9345 vinfos[3].indices[1] = _ij3[1];
9346 vinfos[3].maxsolutions = _nj3;
9347 vinfos[4].jointtype = 1;
9348 vinfos[4].foffset = j4;
9349 vinfos[4].indices[0] = _ij4[0];
9350 vinfos[4].indices[1] = _ij4[1];
9351 vinfos[4].maxsolutions = _nj4;
9352 vinfos[5].jointtype = 1;
9353 vinfos[5].foffset = j5;
9354 vinfos[5].indices[0] = _ij5[0];
9355 vinfos[5].indices[1] = _ij5[1];
9356 vinfos[5].maxsolutions = _nj5;
9357 std::vector<int> vfree(0);
9358 solutions.AddSolution(vinfos,vfree);
9359 }
9360 }
9361 }
9362 
9363 }
9364 
9365 }
9366 
9367 }
9368 } while(0);
9369 if( bgotonextstatement )
9370 {
9371 bool bgotonextstatement = true;
9372 do
9373 {
9374 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9375 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9376 {
9377 bgotonextstatement=false;
9378 {
9379 IkReal j3eval[1];
9380 sj4=0;
9381 cj4=1.0;
9382 j4=0;
9383 new_r11=0;
9384 new_r01=0;
9385 new_r22=0;
9386 new_r20=0;
9387 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9388 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9389 {
9390 continue; // no branches [j3]
9391 
9392 } else
9393 {
9394 {
9395 IkReal j3array[2], cj3array[2], sj3array[2];
9396 bool j3valid[2]={false};
9397 _nj3 = 2;
9398 CheckValue<IkReal> x710 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9399 if(!x710.valid){
9400 continue;
9401 }
9402 IkReal x709=x710.value;
9403 j3array[0]=((-1.0)*x709);
9404 sj3array[0]=IKsin(j3array[0]);
9405 cj3array[0]=IKcos(j3array[0]);
9406 j3array[1]=((3.14159265358979)+(((-1.0)*x709)));
9407 sj3array[1]=IKsin(j3array[1]);
9408 cj3array[1]=IKcos(j3array[1]);
9409 if( j3array[0] > IKPI )
9410 {
9411  j3array[0]-=IK2PI;
9412 }
9413 else if( j3array[0] < -IKPI )
9414 { j3array[0]+=IK2PI;
9415 }
9416 j3valid[0] = true;
9417 if( j3array[1] > IKPI )
9418 {
9419  j3array[1]-=IK2PI;
9420 }
9421 else if( j3array[1] < -IKPI )
9422 { j3array[1]+=IK2PI;
9423 }
9424 j3valid[1] = true;
9425 for(int ij3 = 0; ij3 < 2; ++ij3)
9426 {
9427 if( !j3valid[ij3] )
9428 {
9429  continue;
9430 }
9431 _ij3[0] = ij3; _ij3[1] = -1;
9432 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9433 {
9434 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9435 {
9436  j3valid[iij3]=false; _ij3[1] = iij3; break;
9437 }
9438 }
9439 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9440 {
9441 IkReal evalcond[1];
9442 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
9443 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9444 {
9445 continue;
9446 }
9447 }
9448 
9449 {
9450 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9451 vinfos[0].jointtype = 1;
9452 vinfos[0].foffset = j0;
9453 vinfos[0].indices[0] = _ij0[0];
9454 vinfos[0].indices[1] = _ij0[1];
9455 vinfos[0].maxsolutions = _nj0;
9456 vinfos[1].jointtype = 1;
9457 vinfos[1].foffset = j1;
9458 vinfos[1].indices[0] = _ij1[0];
9459 vinfos[1].indices[1] = _ij1[1];
9460 vinfos[1].maxsolutions = _nj1;
9461 vinfos[2].jointtype = 1;
9462 vinfos[2].foffset = j2;
9463 vinfos[2].indices[0] = _ij2[0];
9464 vinfos[2].indices[1] = _ij2[1];
9465 vinfos[2].maxsolutions = _nj2;
9466 vinfos[3].jointtype = 1;
9467 vinfos[3].foffset = j3;
9468 vinfos[3].indices[0] = _ij3[0];
9469 vinfos[3].indices[1] = _ij3[1];
9470 vinfos[3].maxsolutions = _nj3;
9471 vinfos[4].jointtype = 1;
9472 vinfos[4].foffset = j4;
9473 vinfos[4].indices[0] = _ij4[0];
9474 vinfos[4].indices[1] = _ij4[1];
9475 vinfos[4].maxsolutions = _nj4;
9476 vinfos[5].jointtype = 1;
9477 vinfos[5].foffset = j5;
9478 vinfos[5].indices[0] = _ij5[0];
9479 vinfos[5].indices[1] = _ij5[1];
9480 vinfos[5].maxsolutions = _nj5;
9481 std::vector<int> vfree(0);
9482 solutions.AddSolution(vinfos,vfree);
9483 }
9484 }
9485 }
9486 
9487 }
9488 
9489 }
9490 
9491 }
9492 } while(0);
9493 if( bgotonextstatement )
9494 {
9495 bool bgotonextstatement = true;
9496 do
9497 {
9498 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9499 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9500 {
9501 bgotonextstatement=false;
9502 {
9503 IkReal j3eval[1];
9504 sj4=0;
9505 cj4=1.0;
9506 j4=0;
9507 new_r00=0;
9508 new_r10=0;
9509 new_r21=0;
9510 new_r22=0;
9511 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9512 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9513 {
9514 continue; // no branches [j3]
9515 
9516 } else
9517 {
9518 {
9519 IkReal j3array[2], cj3array[2], sj3array[2];
9520 bool j3valid[2]={false};
9521 _nj3 = 2;
9522 CheckValue<IkReal> x712 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9523 if(!x712.valid){
9524 continue;
9525 }
9526 IkReal x711=x712.value;
9527 j3array[0]=((-1.0)*x711);
9528 sj3array[0]=IKsin(j3array[0]);
9529 cj3array[0]=IKcos(j3array[0]);
9530 j3array[1]=((3.14159265358979)+(((-1.0)*x711)));
9531 sj3array[1]=IKsin(j3array[1]);
9532 cj3array[1]=IKcos(j3array[1]);
9533 if( j3array[0] > IKPI )
9534 {
9535  j3array[0]-=IK2PI;
9536 }
9537 else if( j3array[0] < -IKPI )
9538 { j3array[0]+=IK2PI;
9539 }
9540 j3valid[0] = true;
9541 if( j3array[1] > IKPI )
9542 {
9543  j3array[1]-=IK2PI;
9544 }
9545 else if( j3array[1] < -IKPI )
9546 { j3array[1]+=IK2PI;
9547 }
9548 j3valid[1] = true;
9549 for(int ij3 = 0; ij3 < 2; ++ij3)
9550 {
9551 if( !j3valid[ij3] )
9552 {
9553  continue;
9554 }
9555 _ij3[0] = ij3; _ij3[1] = -1;
9556 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9557 {
9558 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9559 {
9560  j3valid[iij3]=false; _ij3[1] = iij3; break;
9561 }
9562 }
9563 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9564 {
9565 IkReal evalcond[1];
9566 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
9567 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9568 {
9569 continue;
9570 }
9571 }
9572 
9573 {
9574 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9575 vinfos[0].jointtype = 1;
9576 vinfos[0].foffset = j0;
9577 vinfos[0].indices[0] = _ij0[0];
9578 vinfos[0].indices[1] = _ij0[1];
9579 vinfos[0].maxsolutions = _nj0;
9580 vinfos[1].jointtype = 1;
9581 vinfos[1].foffset = j1;
9582 vinfos[1].indices[0] = _ij1[0];
9583 vinfos[1].indices[1] = _ij1[1];
9584 vinfos[1].maxsolutions = _nj1;
9585 vinfos[2].jointtype = 1;
9586 vinfos[2].foffset = j2;
9587 vinfos[2].indices[0] = _ij2[0];
9588 vinfos[2].indices[1] = _ij2[1];
9589 vinfos[2].maxsolutions = _nj2;
9590 vinfos[3].jointtype = 1;
9591 vinfos[3].foffset = j3;
9592 vinfos[3].indices[0] = _ij3[0];
9593 vinfos[3].indices[1] = _ij3[1];
9594 vinfos[3].maxsolutions = _nj3;
9595 vinfos[4].jointtype = 1;
9596 vinfos[4].foffset = j4;
9597 vinfos[4].indices[0] = _ij4[0];
9598 vinfos[4].indices[1] = _ij4[1];
9599 vinfos[4].maxsolutions = _nj4;
9600 vinfos[5].jointtype = 1;
9601 vinfos[5].foffset = j5;
9602 vinfos[5].indices[0] = _ij5[0];
9603 vinfos[5].indices[1] = _ij5[1];
9604 vinfos[5].maxsolutions = _nj5;
9605 std::vector<int> vfree(0);
9606 solutions.AddSolution(vinfos,vfree);
9607 }
9608 }
9609 }
9610 
9611 }
9612 
9613 }
9614 
9615 }
9616 } while(0);
9617 if( bgotonextstatement )
9618 {
9619 bool bgotonextstatement = true;
9620 do
9621 {
9622 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
9623 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9624 {
9625 bgotonextstatement=false;
9626 {
9627 IkReal j3eval[3];
9628 sj4=0;
9629 cj4=1.0;
9630 j4=0;
9631 new_r01=0;
9632 new_r10=0;
9633 j3eval[0]=new_r11;
9634 j3eval[1]=IKsign(new_r11);
9635 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
9636 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9637 {
9638 {
9639 IkReal j3eval[3];
9640 sj4=0;
9641 cj4=1.0;
9642 j4=0;
9643 new_r01=0;
9644 new_r10=0;
9645 j3eval[0]=new_r00;
9646 j3eval[1]=((IKabs(cj5))+(IKabs(sj5)));
9647 j3eval[2]=IKsign(new_r00);
9648 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9649 {
9650 {
9651 IkReal j3eval[2];
9652 sj4=0;
9653 cj4=1.0;
9654 j4=0;
9655 new_r01=0;
9656 new_r10=0;
9657 j3eval[0]=new_r00;
9658 j3eval[1]=new_r11;
9659 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9660 {
9661 continue; // no branches [j3]
9662 
9663 } else
9664 {
9665 {
9666 IkReal j3array[1], cj3array[1], sj3array[1];
9667 bool j3valid[1]={false};
9668 _nj3 = 1;
9669 CheckValue<IkReal> x713=IKPowWithIntegerCheck(new_r00,-1);
9670 if(!x713.valid){
9671 continue;
9672 }
9673 CheckValue<IkReal> x714=IKPowWithIntegerCheck(new_r11,-1);
9674 if(!x714.valid){
9675 continue;
9676 }
9677 if( IKabs(((-1.0)*sj5*(x713.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x714.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x713.value)))+IKsqr((cj5*(x714.value)))-1) <= IKFAST_SINCOS_THRESH )
9678  continue;
9679 j3array[0]=IKatan2(((-1.0)*sj5*(x713.value)), (cj5*(x714.value)));
9680 sj3array[0]=IKsin(j3array[0]);
9681 cj3array[0]=IKcos(j3array[0]);
9682 if( j3array[0] > IKPI )
9683 {
9684  j3array[0]-=IK2PI;
9685 }
9686 else if( j3array[0] < -IKPI )
9687 { j3array[0]+=IK2PI;
9688 }
9689 j3valid[0] = true;
9690 for(int ij3 = 0; ij3 < 1; ++ij3)
9691 {
9692 if( !j3valid[ij3] )
9693 {
9694  continue;
9695 }
9696 _ij3[0] = ij3; _ij3[1] = -1;
9697 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9698 {
9699 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9700 {
9701  j3valid[iij3]=false; _ij3[1] = iij3; break;
9702 }
9703 }
9704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9705 {
9706 IkReal evalcond[7];
9707 IkReal x715=IKsin(j3);
9708 IkReal x716=IKcos(j3);
9709 IkReal x717=(sj5*x715);
9710 IkReal x718=((1.0)*x716);
9711 IkReal x719=(cj5*x718);
9712 evalcond[0]=(sj5+((new_r00*x715)));
9713 evalcond[1]=(sj5+((new_r11*x715)));
9714 evalcond[2]=(cj5+(((-1.0)*new_r11*x718)));
9715 evalcond[3]=(((new_r00*x716))+(((-1.0)*cj5)));
9716 evalcond[4]=(((cj5*x715))+((sj5*x716)));
9717 evalcond[5]=((((-1.0)*x719))+x717+new_r00);
9718 evalcond[6]=((((-1.0)*x719))+x717+new_r11);
9719 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9720 {
9721 continue;
9722 }
9723 }
9724 
9725 {
9726 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9727 vinfos[0].jointtype = 1;
9728 vinfos[0].foffset = j0;
9729 vinfos[0].indices[0] = _ij0[0];
9730 vinfos[0].indices[1] = _ij0[1];
9731 vinfos[0].maxsolutions = _nj0;
9732 vinfos[1].jointtype = 1;
9733 vinfos[1].foffset = j1;
9734 vinfos[1].indices[0] = _ij1[0];
9735 vinfos[1].indices[1] = _ij1[1];
9736 vinfos[1].maxsolutions = _nj1;
9737 vinfos[2].jointtype = 1;
9738 vinfos[2].foffset = j2;
9739 vinfos[2].indices[0] = _ij2[0];
9740 vinfos[2].indices[1] = _ij2[1];
9741 vinfos[2].maxsolutions = _nj2;
9742 vinfos[3].jointtype = 1;
9743 vinfos[3].foffset = j3;
9744 vinfos[3].indices[0] = _ij3[0];
9745 vinfos[3].indices[1] = _ij3[1];
9746 vinfos[3].maxsolutions = _nj3;
9747 vinfos[4].jointtype = 1;
9748 vinfos[4].foffset = j4;
9749 vinfos[4].indices[0] = _ij4[0];
9750 vinfos[4].indices[1] = _ij4[1];
9751 vinfos[4].maxsolutions = _nj4;
9752 vinfos[5].jointtype = 1;
9753 vinfos[5].foffset = j5;
9754 vinfos[5].indices[0] = _ij5[0];
9755 vinfos[5].indices[1] = _ij5[1];
9756 vinfos[5].maxsolutions = _nj5;
9757 std::vector<int> vfree(0);
9758 solutions.AddSolution(vinfos,vfree);
9759 }
9760 }
9761 }
9762 
9763 }
9764 
9765 }
9766 
9767 } else
9768 {
9769 {
9770 IkReal j3array[1], cj3array[1], sj3array[1];
9771 bool j3valid[1]={false};
9772 _nj3 = 1;
9773 CheckValue<IkReal> x720=IKPowWithIntegerCheck(IKsign(new_r00),-1);
9774 if(!x720.valid){
9775 continue;
9776 }
9777 CheckValue<IkReal> x721 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9778 if(!x721.valid){
9779 continue;
9780 }
9781 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x720.value)))+(x721.value));
9782 sj3array[0]=IKsin(j3array[0]);
9783 cj3array[0]=IKcos(j3array[0]);
9784 if( j3array[0] > IKPI )
9785 {
9786  j3array[0]-=IK2PI;
9787 }
9788 else if( j3array[0] < -IKPI )
9789 { j3array[0]+=IK2PI;
9790 }
9791 j3valid[0] = true;
9792 for(int ij3 = 0; ij3 < 1; ++ij3)
9793 {
9794 if( !j3valid[ij3] )
9795 {
9796  continue;
9797 }
9798 _ij3[0] = ij3; _ij3[1] = -1;
9799 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9800 {
9801 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9802 {
9803  j3valid[iij3]=false; _ij3[1] = iij3; break;
9804 }
9805 }
9806 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9807 {
9808 IkReal evalcond[7];
9809 IkReal x722=IKsin(j3);
9810 IkReal x723=IKcos(j3);
9811 IkReal x724=(sj5*x722);
9812 IkReal x725=((1.0)*x723);
9813 IkReal x726=(cj5*x725);
9814 evalcond[0]=(sj5+((new_r00*x722)));
9815 evalcond[1]=(sj5+((new_r11*x722)));
9816 evalcond[2]=(cj5+(((-1.0)*new_r11*x725)));
9817 evalcond[3]=(((new_r00*x723))+(((-1.0)*cj5)));
9818 evalcond[4]=(((cj5*x722))+((sj5*x723)));
9819 evalcond[5]=((((-1.0)*x726))+x724+new_r00);
9820 evalcond[6]=((((-1.0)*x726))+x724+new_r11);
9821 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9822 {
9823 continue;
9824 }
9825 }
9826 
9827 {
9828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9829 vinfos[0].jointtype = 1;
9830 vinfos[0].foffset = j0;
9831 vinfos[0].indices[0] = _ij0[0];
9832 vinfos[0].indices[1] = _ij0[1];
9833 vinfos[0].maxsolutions = _nj0;
9834 vinfos[1].jointtype = 1;
9835 vinfos[1].foffset = j1;
9836 vinfos[1].indices[0] = _ij1[0];
9837 vinfos[1].indices[1] = _ij1[1];
9838 vinfos[1].maxsolutions = _nj1;
9839 vinfos[2].jointtype = 1;
9840 vinfos[2].foffset = j2;
9841 vinfos[2].indices[0] = _ij2[0];
9842 vinfos[2].indices[1] = _ij2[1];
9843 vinfos[2].maxsolutions = _nj2;
9844 vinfos[3].jointtype = 1;
9845 vinfos[3].foffset = j3;
9846 vinfos[3].indices[0] = _ij3[0];
9847 vinfos[3].indices[1] = _ij3[1];
9848 vinfos[3].maxsolutions = _nj3;
9849 vinfos[4].jointtype = 1;
9850 vinfos[4].foffset = j4;
9851 vinfos[4].indices[0] = _ij4[0];
9852 vinfos[4].indices[1] = _ij4[1];
9853 vinfos[4].maxsolutions = _nj4;
9854 vinfos[5].jointtype = 1;
9855 vinfos[5].foffset = j5;
9856 vinfos[5].indices[0] = _ij5[0];
9857 vinfos[5].indices[1] = _ij5[1];
9858 vinfos[5].maxsolutions = _nj5;
9859 std::vector<int> vfree(0);
9860 solutions.AddSolution(vinfos,vfree);
9861 }
9862 }
9863 }
9864 
9865 }
9866 
9867 }
9868 
9869 } else
9870 {
9871 {
9872 IkReal j3array[1], cj3array[1], sj3array[1];
9873 bool j3valid[1]={false};
9874 _nj3 = 1;
9875 CheckValue<IkReal> x727=IKPowWithIntegerCheck(IKsign(new_r11),-1);
9876 if(!x727.valid){
9877 continue;
9878 }
9879 CheckValue<IkReal> x728 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9880 if(!x728.valid){
9881 continue;
9882 }
9883 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x727.value)))+(x728.value));
9884 sj3array[0]=IKsin(j3array[0]);
9885 cj3array[0]=IKcos(j3array[0]);
9886 if( j3array[0] > IKPI )
9887 {
9888  j3array[0]-=IK2PI;
9889 }
9890 else if( j3array[0] < -IKPI )
9891 { j3array[0]+=IK2PI;
9892 }
9893 j3valid[0] = true;
9894 for(int ij3 = 0; ij3 < 1; ++ij3)
9895 {
9896 if( !j3valid[ij3] )
9897 {
9898  continue;
9899 }
9900 _ij3[0] = ij3; _ij3[1] = -1;
9901 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9902 {
9903 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9904 {
9905  j3valid[iij3]=false; _ij3[1] = iij3; break;
9906 }
9907 }
9908 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9909 {
9910 IkReal evalcond[7];
9911 IkReal x729=IKsin(j3);
9912 IkReal x730=IKcos(j3);
9913 IkReal x731=(sj5*x729);
9914 IkReal x732=((1.0)*x730);
9915 IkReal x733=(cj5*x732);
9916 evalcond[0]=(sj5+((new_r00*x729)));
9917 evalcond[1]=(sj5+((new_r11*x729)));
9918 evalcond[2]=(cj5+(((-1.0)*new_r11*x732)));
9919 evalcond[3]=(((new_r00*x730))+(((-1.0)*cj5)));
9920 evalcond[4]=(((cj5*x729))+((sj5*x730)));
9921 evalcond[5]=((((-1.0)*x733))+x731+new_r00);
9922 evalcond[6]=((((-1.0)*x733))+x731+new_r11);
9923 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9924 {
9925 continue;
9926 }
9927 }
9928 
9929 {
9930 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9931 vinfos[0].jointtype = 1;
9932 vinfos[0].foffset = j0;
9933 vinfos[0].indices[0] = _ij0[0];
9934 vinfos[0].indices[1] = _ij0[1];
9935 vinfos[0].maxsolutions = _nj0;
9936 vinfos[1].jointtype = 1;
9937 vinfos[1].foffset = j1;
9938 vinfos[1].indices[0] = _ij1[0];
9939 vinfos[1].indices[1] = _ij1[1];
9940 vinfos[1].maxsolutions = _nj1;
9941 vinfos[2].jointtype = 1;
9942 vinfos[2].foffset = j2;
9943 vinfos[2].indices[0] = _ij2[0];
9944 vinfos[2].indices[1] = _ij2[1];
9945 vinfos[2].maxsolutions = _nj2;
9946 vinfos[3].jointtype = 1;
9947 vinfos[3].foffset = j3;
9948 vinfos[3].indices[0] = _ij3[0];
9949 vinfos[3].indices[1] = _ij3[1];
9950 vinfos[3].maxsolutions = _nj3;
9951 vinfos[4].jointtype = 1;
9952 vinfos[4].foffset = j4;
9953 vinfos[4].indices[0] = _ij4[0];
9954 vinfos[4].indices[1] = _ij4[1];
9955 vinfos[4].maxsolutions = _nj4;
9956 vinfos[5].jointtype = 1;
9957 vinfos[5].foffset = j5;
9958 vinfos[5].indices[0] = _ij5[0];
9959 vinfos[5].indices[1] = _ij5[1];
9960 vinfos[5].maxsolutions = _nj5;
9961 std::vector<int> vfree(0);
9962 solutions.AddSolution(vinfos,vfree);
9963 }
9964 }
9965 }
9966 
9967 }
9968 
9969 }
9970 
9971 }
9972 } while(0);
9973 if( bgotonextstatement )
9974 {
9975 bool bgotonextstatement = true;
9976 do
9977 {
9978 if( 1 )
9979 {
9980 bgotonextstatement=false;
9981 continue; // branch miss [j3]
9982 
9983 }
9984 } while(0);
9985 if( bgotonextstatement )
9986 {
9987 }
9988 }
9989 }
9990 }
9991 }
9992 }
9993 }
9994 }
9995 }
9996 }
9997 
9998 } else
9999 {
10000 {
10001 IkReal j3array[1], cj3array[1], sj3array[1];
10002 bool j3valid[1]={false};
10003 _nj3 = 1;
10004 IkReal x734=((1.0)*new_r11);
10005 CheckValue<IkReal> x735=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*x734)))),-1);
10006 if(!x735.valid){
10007 continue;
10008 }
10009 CheckValue<IkReal> x736 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((1.0)+(((-1.0)*(cj5*cj5)))+(((-1.0)*new_r00*x734)))),IKFAST_ATAN2_MAGTHRESH);
10010 if(!x736.valid){
10011 continue;
10012 }
10013 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x735.value)))+(x736.value));
10014 sj3array[0]=IKsin(j3array[0]);
10015 cj3array[0]=IKcos(j3array[0]);
10016 if( j3array[0] > IKPI )
10017 {
10018  j3array[0]-=IK2PI;
10019 }
10020 else if( j3array[0] < -IKPI )
10021 { j3array[0]+=IK2PI;
10022 }
10023 j3valid[0] = true;
10024 for(int ij3 = 0; ij3 < 1; ++ij3)
10025 {
10026 if( !j3valid[ij3] )
10027 {
10028  continue;
10029 }
10030 _ij3[0] = ij3; _ij3[1] = -1;
10031 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10032 {
10033 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10034 {
10035  j3valid[iij3]=false; _ij3[1] = iij3; break;
10036 }
10037 }
10038 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10039 {
10040 IkReal evalcond[8];
10041 IkReal x737=IKcos(j3);
10042 IkReal x738=IKsin(j3);
10043 IkReal x739=(sj5*x738);
10044 IkReal x740=(cj5*x738);
10045 IkReal x741=(sj5*x737);
10046 IkReal x742=((1.0)*x737);
10047 IkReal x743=(cj5*x742);
10048 evalcond[0]=(sj5+((new_r01*x737))+((new_r11*x738)));
10049 evalcond[1]=(x740+x741+new_r01);
10050 evalcond[2]=(sj5+((new_r00*x738))+(((-1.0)*new_r10*x742)));
10051 evalcond[3]=(cj5+((new_r01*x738))+(((-1.0)*new_r11*x742)));
10052 evalcond[4]=(x739+new_r00+(((-1.0)*x743)));
10053 evalcond[5]=(x739+new_r11+(((-1.0)*x743)));
10054 evalcond[6]=(((new_r00*x737))+((new_r10*x738))+(((-1.0)*cj5)));
10055 evalcond[7]=((((-1.0)*x740))+(((-1.0)*x741))+new_r10);
10056 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10057 {
10058 continue;
10059 }
10060 }
10061 
10062 {
10063 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10064 vinfos[0].jointtype = 1;
10065 vinfos[0].foffset = j0;
10066 vinfos[0].indices[0] = _ij0[0];
10067 vinfos[0].indices[1] = _ij0[1];
10068 vinfos[0].maxsolutions = _nj0;
10069 vinfos[1].jointtype = 1;
10070 vinfos[1].foffset = j1;
10071 vinfos[1].indices[0] = _ij1[0];
10072 vinfos[1].indices[1] = _ij1[1];
10073 vinfos[1].maxsolutions = _nj1;
10074 vinfos[2].jointtype = 1;
10075 vinfos[2].foffset = j2;
10076 vinfos[2].indices[0] = _ij2[0];
10077 vinfos[2].indices[1] = _ij2[1];
10078 vinfos[2].maxsolutions = _nj2;
10079 vinfos[3].jointtype = 1;
10080 vinfos[3].foffset = j3;
10081 vinfos[3].indices[0] = _ij3[0];
10082 vinfos[3].indices[1] = _ij3[1];
10083 vinfos[3].maxsolutions = _nj3;
10084 vinfos[4].jointtype = 1;
10085 vinfos[4].foffset = j4;
10086 vinfos[4].indices[0] = _ij4[0];
10087 vinfos[4].indices[1] = _ij4[1];
10088 vinfos[4].maxsolutions = _nj4;
10089 vinfos[5].jointtype = 1;
10090 vinfos[5].foffset = j5;
10091 vinfos[5].indices[0] = _ij5[0];
10092 vinfos[5].indices[1] = _ij5[1];
10093 vinfos[5].maxsolutions = _nj5;
10094 std::vector<int> vfree(0);
10095 solutions.AddSolution(vinfos,vfree);
10096 }
10097 }
10098 }
10099 
10100 }
10101 
10102 }
10103 
10104 } else
10105 {
10106 {
10107 IkReal j3array[1], cj3array[1], sj3array[1];
10108 bool j3valid[1]={false};
10109 _nj3 = 1;
10110 CheckValue<IkReal> x744 = IKatan2WithCheck(IkReal((((new_r11*sj5))+((cj5*new_r01)))),IkReal((((new_r01*sj5))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10111 if(!x744.valid){
10112 continue;
10113 }
10114 CheckValue<IkReal> x745=IKPowWithIntegerCheck(IKsign(((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))))),-1);
10115 if(!x745.valid){
10116 continue;
10117 }
10118 j3array[0]=((-1.5707963267949)+(x744.value)+(((1.5707963267949)*(x745.value))));
10119 sj3array[0]=IKsin(j3array[0]);
10120 cj3array[0]=IKcos(j3array[0]);
10121 if( j3array[0] > IKPI )
10122 {
10123  j3array[0]-=IK2PI;
10124 }
10125 else if( j3array[0] < -IKPI )
10126 { j3array[0]+=IK2PI;
10127 }
10128 j3valid[0] = true;
10129 for(int ij3 = 0; ij3 < 1; ++ij3)
10130 {
10131 if( !j3valid[ij3] )
10132 {
10133  continue;
10134 }
10135 _ij3[0] = ij3; _ij3[1] = -1;
10136 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10137 {
10138 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10139 {
10140  j3valid[iij3]=false; _ij3[1] = iij3; break;
10141 }
10142 }
10143 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10144 {
10145 IkReal evalcond[8];
10146 IkReal x746=IKcos(j3);
10147 IkReal x747=IKsin(j3);
10148 IkReal x748=(sj5*x747);
10149 IkReal x749=(cj5*x747);
10150 IkReal x750=(sj5*x746);
10151 IkReal x751=((1.0)*x746);
10152 IkReal x752=(cj5*x751);
10153 evalcond[0]=(sj5+((new_r01*x746))+((new_r11*x747)));
10154 evalcond[1]=(x750+x749+new_r01);
10155 evalcond[2]=(sj5+(((-1.0)*new_r10*x751))+((new_r00*x747)));
10156 evalcond[3]=(cj5+(((-1.0)*new_r11*x751))+((new_r01*x747)));
10157 evalcond[4]=(x748+new_r00+(((-1.0)*x752)));
10158 evalcond[5]=(x748+new_r11+(((-1.0)*x752)));
10159 evalcond[6]=(((new_r10*x747))+((new_r00*x746))+(((-1.0)*cj5)));
10160 evalcond[7]=((((-1.0)*x750))+(((-1.0)*x749))+new_r10);
10161 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10162 {
10163 continue;
10164 }
10165 }
10166 
10167 {
10168 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10169 vinfos[0].jointtype = 1;
10170 vinfos[0].foffset = j0;
10171 vinfos[0].indices[0] = _ij0[0];
10172 vinfos[0].indices[1] = _ij0[1];
10173 vinfos[0].maxsolutions = _nj0;
10174 vinfos[1].jointtype = 1;
10175 vinfos[1].foffset = j1;
10176 vinfos[1].indices[0] = _ij1[0];
10177 vinfos[1].indices[1] = _ij1[1];
10178 vinfos[1].maxsolutions = _nj1;
10179 vinfos[2].jointtype = 1;
10180 vinfos[2].foffset = j2;
10181 vinfos[2].indices[0] = _ij2[0];
10182 vinfos[2].indices[1] = _ij2[1];
10183 vinfos[2].maxsolutions = _nj2;
10184 vinfos[3].jointtype = 1;
10185 vinfos[3].foffset = j3;
10186 vinfos[3].indices[0] = _ij3[0];
10187 vinfos[3].indices[1] = _ij3[1];
10188 vinfos[3].maxsolutions = _nj3;
10189 vinfos[4].jointtype = 1;
10190 vinfos[4].foffset = j4;
10191 vinfos[4].indices[0] = _ij4[0];
10192 vinfos[4].indices[1] = _ij4[1];
10193 vinfos[4].maxsolutions = _nj4;
10194 vinfos[5].jointtype = 1;
10195 vinfos[5].foffset = j5;
10196 vinfos[5].indices[0] = _ij5[0];
10197 vinfos[5].indices[1] = _ij5[1];
10198 vinfos[5].maxsolutions = _nj5;
10199 std::vector<int> vfree(0);
10200 solutions.AddSolution(vinfos,vfree);
10201 }
10202 }
10203 }
10204 
10205 }
10206 
10207 }
10208 
10209 } else
10210 {
10211 {
10212 IkReal j3array[1], cj3array[1], sj3array[1];
10213 bool j3valid[1]={false};
10214 _nj3 = 1;
10215 IkReal x753=((1.0)*new_r11);
10216 CheckValue<IkReal> x754 = IKatan2WithCheck(IkReal((((new_r10*sj5))+((new_r01*sj5)))),IkReal(((((-1.0)*sj5*x753))+((new_r00*sj5)))),IKFAST_ATAN2_MAGTHRESH);
10217 if(!x754.valid){
10218 continue;
10219 }
10220 CheckValue<IkReal> x755=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x753))+(((-1.0)*new_r00*new_r01)))),-1);
10221 if(!x755.valid){
10222 continue;
10223 }
10224 j3array[0]=((-1.5707963267949)+(x754.value)+(((1.5707963267949)*(x755.value))));
10225 sj3array[0]=IKsin(j3array[0]);
10226 cj3array[0]=IKcos(j3array[0]);
10227 if( j3array[0] > IKPI )
10228 {
10229  j3array[0]-=IK2PI;
10230 }
10231 else if( j3array[0] < -IKPI )
10232 { j3array[0]+=IK2PI;
10233 }
10234 j3valid[0] = true;
10235 for(int ij3 = 0; ij3 < 1; ++ij3)
10236 {
10237 if( !j3valid[ij3] )
10238 {
10239  continue;
10240 }
10241 _ij3[0] = ij3; _ij3[1] = -1;
10242 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10243 {
10244 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10245 {
10246  j3valid[iij3]=false; _ij3[1] = iij3; break;
10247 }
10248 }
10249 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10250 {
10251 IkReal evalcond[8];
10252 IkReal x756=IKcos(j3);
10253 IkReal x757=IKsin(j3);
10254 IkReal x758=(sj5*x757);
10255 IkReal x759=(cj5*x757);
10256 IkReal x760=(sj5*x756);
10257 IkReal x761=((1.0)*x756);
10258 IkReal x762=(cj5*x761);
10259 evalcond[0]=(sj5+((new_r11*x757))+((new_r01*x756)));
10260 evalcond[1]=(x759+x760+new_r01);
10261 evalcond[2]=(sj5+(((-1.0)*new_r10*x761))+((new_r00*x757)));
10262 evalcond[3]=(cj5+(((-1.0)*new_r11*x761))+((new_r01*x757)));
10263 evalcond[4]=((((-1.0)*x762))+x758+new_r00);
10264 evalcond[5]=((((-1.0)*x762))+x758+new_r11);
10265 evalcond[6]=(((new_r00*x756))+((new_r10*x757))+(((-1.0)*cj5)));
10266 evalcond[7]=((((-1.0)*x760))+(((-1.0)*x759))+new_r10);
10267 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10268 {
10269 continue;
10270 }
10271 }
10272 
10273 {
10274 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10275 vinfos[0].jointtype = 1;
10276 vinfos[0].foffset = j0;
10277 vinfos[0].indices[0] = _ij0[0];
10278 vinfos[0].indices[1] = _ij0[1];
10279 vinfos[0].maxsolutions = _nj0;
10280 vinfos[1].jointtype = 1;
10281 vinfos[1].foffset = j1;
10282 vinfos[1].indices[0] = _ij1[0];
10283 vinfos[1].indices[1] = _ij1[1];
10284 vinfos[1].maxsolutions = _nj1;
10285 vinfos[2].jointtype = 1;
10286 vinfos[2].foffset = j2;
10287 vinfos[2].indices[0] = _ij2[0];
10288 vinfos[2].indices[1] = _ij2[1];
10289 vinfos[2].maxsolutions = _nj2;
10290 vinfos[3].jointtype = 1;
10291 vinfos[3].foffset = j3;
10292 vinfos[3].indices[0] = _ij3[0];
10293 vinfos[3].indices[1] = _ij3[1];
10294 vinfos[3].maxsolutions = _nj3;
10295 vinfos[4].jointtype = 1;
10296 vinfos[4].foffset = j4;
10297 vinfos[4].indices[0] = _ij4[0];
10298 vinfos[4].indices[1] = _ij4[1];
10299 vinfos[4].maxsolutions = _nj4;
10300 vinfos[5].jointtype = 1;
10301 vinfos[5].foffset = j5;
10302 vinfos[5].indices[0] = _ij5[0];
10303 vinfos[5].indices[1] = _ij5[1];
10304 vinfos[5].maxsolutions = _nj5;
10305 std::vector<int> vfree(0);
10306 solutions.AddSolution(vinfos,vfree);
10307 }
10308 }
10309 }
10310 
10311 }
10312 
10313 }
10314 
10315 }
10316 } while(0);
10317 if( bgotonextstatement )
10318 {
10319 bool bgotonextstatement = true;
10320 do
10321 {
10322 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10323 evalcond[1]=new_r02;
10324 evalcond[2]=new_r12;
10325 evalcond[3]=new_r21;
10326 evalcond[4]=new_r20;
10327 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
10328 {
10329 bgotonextstatement=false;
10330 {
10331 IkReal j3eval[3];
10332 sj4=0;
10333 cj4=-1.0;
10334 j4=3.14159265358979;
10335 IkReal x763=((1.0)*new_r10);
10336 IkReal x764=((((-1.0)*new_r11*x763))+(((-1.0)*new_r00*new_r01)));
10337 j3eval[0]=x764;
10338 j3eval[1]=((IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(IKabs((((cj5*new_r01))+(((-1.0)*cj5*x763))))));
10339 j3eval[2]=IKsign(x764);
10340 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10341 {
10342 {
10343 IkReal j3eval[3];
10344 sj4=0;
10345 cj4=-1.0;
10346 j4=3.14159265358979;
10347 IkReal x765=((1.0)*new_r10);
10348 IkReal x766=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x765)));
10349 j3eval[0]=x766;
10350 j3eval[1]=IKsign(x766);
10351 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((cj5*cj5)+(((-1.0)*new_r01*x765))))));
10352 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10353 {
10354 {
10355 IkReal j3eval[3];
10356 sj4=0;
10357 cj4=-1.0;
10358 j4=3.14159265358979;
10359 IkReal x767=((1.0)*new_r00);
10360 IkReal x768=(((cj5*new_r10))+(((-1.0)*sj5*x767)));
10361 j3eval[0]=x768;
10362 j3eval[1]=((IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))))+(IKabs(((((-1.0)*new_r10*x767))+((cj5*sj5))))));
10363 j3eval[2]=IKsign(x768);
10364 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10365 {
10366 {
10367 IkReal evalcond[1];
10368 bool bgotonextstatement = true;
10369 do
10370 {
10371 IkReal x769=((-1.0)*new_r00);
10372 IkReal x771 = ((new_r10*new_r10)+(new_r00*new_r00));
10373 if(IKabs(x771)==0){
10374 continue;
10375 }
10376 IkReal x770=pow(x771,-0.5);
10377 CheckValue<IkReal> x772 = IKatan2WithCheck(IkReal(new_r10),IkReal(x769),IKFAST_ATAN2_MAGTHRESH);
10378 if(!x772.valid){
10379 continue;
10380 }
10381 IkReal gconst12=((-1.0)*(x772.value));
10382 IkReal gconst13=((-1.0)*new_r10*x770);
10383 IkReal gconst14=(x769*x770);
10384 CheckValue<IkReal> x773 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10385 if(!x773.valid){
10386 continue;
10387 }
10388 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j5+(x773.value))))), 6.28318530717959)));
10389 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10390 {
10391 bgotonextstatement=false;
10392 {
10393 IkReal j3eval[3];
10394 IkReal x774=((-1.0)*new_r00);
10395 CheckValue<IkReal> x777 = IKatan2WithCheck(IkReal(new_r10),IkReal(x774),IKFAST_ATAN2_MAGTHRESH);
10396 if(!x777.valid){
10397 continue;
10398 }
10399 IkReal x775=((-1.0)*(x777.value));
10400 IkReal x776=x770;
10401 sj4=0;
10402 cj4=-1.0;
10403 j4=3.14159265358979;
10404 sj5=gconst13;
10405 cj5=gconst14;
10406 j5=x775;
10407 IkReal gconst12=x775;
10408 IkReal gconst13=((-1.0)*new_r10*x776);
10409 IkReal gconst14=(x774*x776);
10410 IkReal x778=new_r00*new_r00;
10411 IkReal x779=((1.0)*new_r11);
10412 IkReal x780=((1.0)*new_r00*new_r01);
10413 IkReal x781=((((-1.0)*x780))+(((-1.0)*new_r10*x779)));
10414 IkReal x782=x770;
10415 IkReal x783=(new_r00*x782);
10416 j3eval[0]=x781;
10417 j3eval[1]=((IKabs((((new_r10*x783))+(((-1.0)*x780*x782)))))+(IKabs(((((-1.0)*x778*x782))+(((-1.0)*x779*x783))))));
10418 j3eval[2]=IKsign(x781);
10419 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10420 {
10421 {
10422 IkReal j3eval[1];
10423 IkReal x784=((-1.0)*new_r00);
10424 CheckValue<IkReal> x787 = IKatan2WithCheck(IkReal(new_r10),IkReal(x784),IKFAST_ATAN2_MAGTHRESH);
10425 if(!x787.valid){
10426 continue;
10427 }
10428 IkReal x785=((-1.0)*(x787.value));
10429 IkReal x786=x770;
10430 sj4=0;
10431 cj4=-1.0;
10432 j4=3.14159265358979;
10433 sj5=gconst13;
10434 cj5=gconst14;
10435 j5=x785;
10436 IkReal gconst12=x785;
10437 IkReal gconst13=((-1.0)*new_r10*x786);
10438 IkReal gconst14=(x784*x786);
10439 IkReal x788=new_r10*new_r10;
10440 IkReal x789=new_r00*new_r00;
10441 CheckValue<IkReal> x792=IKPowWithIntegerCheck((x788+x789),-1);
10442 if(!x792.valid){
10443 continue;
10444 }
10445 IkReal x790=x792.value;
10446 IkReal x791=(new_r00*x790);
10447 j3eval[0]=((IKabs((((new_r01*x791*(new_r00*new_r00)))+((new_r10*x791))+((new_r01*x788*x791)))))+(IKabs(((((-1.0)*new_r01*new_r10))+((x789*x790))))));
10448 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10449 {
10450 {
10451 IkReal j3eval[1];
10452 IkReal x793=((-1.0)*new_r00);
10453 CheckValue<IkReal> x796 = IKatan2WithCheck(IkReal(new_r10),IkReal(x793),IKFAST_ATAN2_MAGTHRESH);
10454 if(!x796.valid){
10455 continue;
10456 }
10457 IkReal x794=((-1.0)*(x796.value));
10458 IkReal x795=x770;
10459 sj4=0;
10460 cj4=-1.0;
10461 j4=3.14159265358979;
10462 sj5=gconst13;
10463 cj5=gconst14;
10464 j5=x794;
10465 IkReal gconst12=x794;
10466 IkReal gconst13=((-1.0)*new_r10*x795);
10467 IkReal gconst14=(x793*x795);
10468 IkReal x797=new_r00*new_r00;
10469 IkReal x798=new_r10*new_r10;
10470 CheckValue<IkReal> x802=IKPowWithIntegerCheck((x797+x798),-1);
10471 if(!x802.valid){
10472 continue;
10473 }
10474 IkReal x799=x802.value;
10475 IkReal x800=(new_r10*x799);
10476 IkReal x801=((1.0)*x799);
10477 j3eval[0]=((IKabs(((((-1.0)*x797*x798*x801))+((x797*x799))+(((-1.0)*x801*(x798*x798))))))+(IKabs((((x800*(new_r00*new_r00*new_r00)))+((new_r00*x800*(new_r10*new_r10)))+((new_r00*x800))))));
10478 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10479 {
10480 {
10481 IkReal evalcond[3];
10482 bool bgotonextstatement = true;
10483 do
10484 {
10485 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10486 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10487 {
10488 bgotonextstatement=false;
10489 {
10490 IkReal j3eval[1];
10491 CheckValue<IkReal> x804 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10492 if(!x804.valid){
10493 continue;
10494 }
10495 IkReal x803=((-1.0)*(x804.value));
10496 sj4=0;
10497 cj4=-1.0;
10498 j4=3.14159265358979;
10499 sj5=gconst13;
10500 cj5=gconst14;
10501 j5=x803;
10502 new_r11=0;
10503 new_r00=0;
10504 IkReal gconst12=x803;
10505 IkReal x805 = new_r10*new_r10;
10506 if(IKabs(x805)==0){
10507 continue;
10508 }
10509 IkReal gconst13=((-1.0)*new_r10*(pow(x805,-0.5)));
10510 IkReal gconst14=0;
10511 j3eval[0]=new_r10;
10512 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10513 {
10514 {
10515 IkReal j3array[2], cj3array[2], sj3array[2];
10516 bool j3valid[2]={false};
10517 _nj3 = 2;
10518 CheckValue<IkReal> x806=IKPowWithIntegerCheck(gconst13,-1);
10519 if(!x806.valid){
10520 continue;
10521 }
10522 cj3array[0]=(new_r01*(x806.value));
10523 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10524 {
10525  j3valid[0] = j3valid[1] = true;
10526  j3array[0] = IKacos(cj3array[0]);
10527  sj3array[0] = IKsin(j3array[0]);
10528  cj3array[1] = cj3array[0];
10529  j3array[1] = -j3array[0];
10530  sj3array[1] = -sj3array[0];
10531 }
10532 else if( isnan(cj3array[0]) )
10533 {
10534  // probably any value will work
10535  j3valid[0] = true;
10536  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10537 }
10538 for(int ij3 = 0; ij3 < 2; ++ij3)
10539 {
10540 if( !j3valid[ij3] )
10541 {
10542  continue;
10543 }
10544 _ij3[0] = ij3; _ij3[1] = -1;
10545 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10546 {
10547 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10548 {
10549  j3valid[iij3]=false; _ij3[1] = iij3; break;
10550 }
10551 }
10552 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10553 {
10554 IkReal evalcond[6];
10555 IkReal x807=IKsin(j3);
10556 IkReal x808=IKcos(j3);
10557 IkReal x809=((1.0)*gconst13);
10558 evalcond[0]=(new_r01*x807);
10559 evalcond[1]=(new_r10*x807);
10560 evalcond[2]=(gconst13*x807);
10561 evalcond[3]=(gconst13+(((-1.0)*new_r10*x808)));
10562 evalcond[4]=((((-1.0)*x808*x809))+new_r10);
10563 evalcond[5]=(((new_r01*x808))+(((-1.0)*x809)));
10564 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10565 {
10566 continue;
10567 }
10568 }
10569 
10570 {
10571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10572 vinfos[0].jointtype = 1;
10573 vinfos[0].foffset = j0;
10574 vinfos[0].indices[0] = _ij0[0];
10575 vinfos[0].indices[1] = _ij0[1];
10576 vinfos[0].maxsolutions = _nj0;
10577 vinfos[1].jointtype = 1;
10578 vinfos[1].foffset = j1;
10579 vinfos[1].indices[0] = _ij1[0];
10580 vinfos[1].indices[1] = _ij1[1];
10581 vinfos[1].maxsolutions = _nj1;
10582 vinfos[2].jointtype = 1;
10583 vinfos[2].foffset = j2;
10584 vinfos[2].indices[0] = _ij2[0];
10585 vinfos[2].indices[1] = _ij2[1];
10586 vinfos[2].maxsolutions = _nj2;
10587 vinfos[3].jointtype = 1;
10588 vinfos[3].foffset = j3;
10589 vinfos[3].indices[0] = _ij3[0];
10590 vinfos[3].indices[1] = _ij3[1];
10591 vinfos[3].maxsolutions = _nj3;
10592 vinfos[4].jointtype = 1;
10593 vinfos[4].foffset = j4;
10594 vinfos[4].indices[0] = _ij4[0];
10595 vinfos[4].indices[1] = _ij4[1];
10596 vinfos[4].maxsolutions = _nj4;
10597 vinfos[5].jointtype = 1;
10598 vinfos[5].foffset = j5;
10599 vinfos[5].indices[0] = _ij5[0];
10600 vinfos[5].indices[1] = _ij5[1];
10601 vinfos[5].maxsolutions = _nj5;
10602 std::vector<int> vfree(0);
10603 solutions.AddSolution(vinfos,vfree);
10604 }
10605 }
10606 }
10607 
10608 } else
10609 {
10610 {
10611 IkReal j3array[2], cj3array[2], sj3array[2];
10612 bool j3valid[2]={false};
10613 _nj3 = 2;
10614 CheckValue<IkReal> x810=IKPowWithIntegerCheck(new_r10,-1);
10615 if(!x810.valid){
10616 continue;
10617 }
10618 cj3array[0]=(gconst13*(x810.value));
10619 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10620 {
10621  j3valid[0] = j3valid[1] = true;
10622  j3array[0] = IKacos(cj3array[0]);
10623  sj3array[0] = IKsin(j3array[0]);
10624  cj3array[1] = cj3array[0];
10625  j3array[1] = -j3array[0];
10626  sj3array[1] = -sj3array[0];
10627 }
10628 else if( isnan(cj3array[0]) )
10629 {
10630  // probably any value will work
10631  j3valid[0] = true;
10632  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10633 }
10634 for(int ij3 = 0; ij3 < 2; ++ij3)
10635 {
10636 if( !j3valid[ij3] )
10637 {
10638  continue;
10639 }
10640 _ij3[0] = ij3; _ij3[1] = -1;
10641 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10642 {
10643 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10644 {
10645  j3valid[iij3]=false; _ij3[1] = iij3; break;
10646 }
10647 }
10648 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10649 {
10650 IkReal evalcond[6];
10651 IkReal x811=IKsin(j3);
10652 IkReal x812=IKcos(j3);
10653 IkReal x813=((1.0)*gconst13);
10654 IkReal x814=(x812*x813);
10655 evalcond[0]=(new_r01*x811);
10656 evalcond[1]=(new_r10*x811);
10657 evalcond[2]=(gconst13*x811);
10658 evalcond[3]=((((-1.0)*x814))+new_r01);
10659 evalcond[4]=((((-1.0)*x814))+new_r10);
10660 evalcond[5]=(((new_r01*x812))+(((-1.0)*x813)));
10661 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10662 {
10663 continue;
10664 }
10665 }
10666 
10667 {
10668 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10669 vinfos[0].jointtype = 1;
10670 vinfos[0].foffset = j0;
10671 vinfos[0].indices[0] = _ij0[0];
10672 vinfos[0].indices[1] = _ij0[1];
10673 vinfos[0].maxsolutions = _nj0;
10674 vinfos[1].jointtype = 1;
10675 vinfos[1].foffset = j1;
10676 vinfos[1].indices[0] = _ij1[0];
10677 vinfos[1].indices[1] = _ij1[1];
10678 vinfos[1].maxsolutions = _nj1;
10679 vinfos[2].jointtype = 1;
10680 vinfos[2].foffset = j2;
10681 vinfos[2].indices[0] = _ij2[0];
10682 vinfos[2].indices[1] = _ij2[1];
10683 vinfos[2].maxsolutions = _nj2;
10684 vinfos[3].jointtype = 1;
10685 vinfos[3].foffset = j3;
10686 vinfos[3].indices[0] = _ij3[0];
10687 vinfos[3].indices[1] = _ij3[1];
10688 vinfos[3].maxsolutions = _nj3;
10689 vinfos[4].jointtype = 1;
10690 vinfos[4].foffset = j4;
10691 vinfos[4].indices[0] = _ij4[0];
10692 vinfos[4].indices[1] = _ij4[1];
10693 vinfos[4].maxsolutions = _nj4;
10694 vinfos[5].jointtype = 1;
10695 vinfos[5].foffset = j5;
10696 vinfos[5].indices[0] = _ij5[0];
10697 vinfos[5].indices[1] = _ij5[1];
10698 vinfos[5].maxsolutions = _nj5;
10699 std::vector<int> vfree(0);
10700 solutions.AddSolution(vinfos,vfree);
10701 }
10702 }
10703 }
10704 
10705 }
10706 
10707 }
10708 
10709 }
10710 } while(0);
10711 if( bgotonextstatement )
10712 {
10713 bool bgotonextstatement = true;
10714 do
10715 {
10716 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
10717 evalcond[1]=gconst14;
10718 evalcond[2]=gconst13;
10719 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
10720 {
10721 bgotonextstatement=false;
10722 {
10723 IkReal j3eval[3];
10724 IkReal x815=((-1.0)*new_r00);
10725 CheckValue<IkReal> x817 = IKatan2WithCheck(IkReal(new_r10),IkReal(x815),IKFAST_ATAN2_MAGTHRESH);
10726 if(!x817.valid){
10727 continue;
10728 }
10729 IkReal x816=((-1.0)*(x817.value));
10730 sj4=0;
10731 cj4=-1.0;
10732 j4=3.14159265358979;
10733 sj5=gconst13;
10734 cj5=gconst14;
10735 j5=x816;
10736 new_r11=0;
10737 new_r01=0;
10738 new_r22=0;
10739 new_r20=0;
10740 IkReal gconst12=x816;
10741 IkReal gconst13=((-1.0)*new_r10);
10742 IkReal gconst14=x815;
10743 j3eval[0]=1.0;
10744 j3eval[1]=1.0;
10745 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
10746 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10747 {
10748 {
10749 IkReal j3eval[3];
10750 IkReal x818=((-1.0)*new_r00);
10751 CheckValue<IkReal> x820 = IKatan2WithCheck(IkReal(new_r10),IkReal(x818),IKFAST_ATAN2_MAGTHRESH);
10752 if(!x820.valid){
10753 continue;
10754 }
10755 IkReal x819=((-1.0)*(x820.value));
10756 sj4=0;
10757 cj4=-1.0;
10758 j4=3.14159265358979;
10759 sj5=gconst13;
10760 cj5=gconst14;
10761 j5=x819;
10762 new_r11=0;
10763 new_r01=0;
10764 new_r22=0;
10765 new_r20=0;
10766 IkReal gconst12=x819;
10767 IkReal gconst13=((-1.0)*new_r10);
10768 IkReal gconst14=x818;
10769 j3eval[0]=-1.0;
10770 j3eval[1]=-1.0;
10771 j3eval[2]=((IKabs(((-1.0)+(new_r10*new_r10))))+(IKabs((new_r00*new_r10))));
10772 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10773 {
10774 {
10775 IkReal j3eval[3];
10776 IkReal x821=((-1.0)*new_r00);
10777 CheckValue<IkReal> x823 = IKatan2WithCheck(IkReal(new_r10),IkReal(x821),IKFAST_ATAN2_MAGTHRESH);
10778 if(!x823.valid){
10779 continue;
10780 }
10781 IkReal x822=((-1.0)*(x823.value));
10782 sj4=0;
10783 cj4=-1.0;
10784 j4=3.14159265358979;
10785 sj5=gconst13;
10786 cj5=gconst14;
10787 j5=x822;
10788 new_r11=0;
10789 new_r01=0;
10790 new_r22=0;
10791 new_r20=0;
10792 IkReal gconst12=x822;
10793 IkReal gconst13=((-1.0)*new_r10);
10794 IkReal gconst14=x821;
10795 j3eval[0]=1.0;
10796 j3eval[1]=((((0.5)*(IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
10797 j3eval[2]=1.0;
10798 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10799 {
10800 continue; // 3 cases reached
10801 
10802 } else
10803 {
10804 {
10805 IkReal j3array[1], cj3array[1], sj3array[1];
10806 bool j3valid[1]={false};
10807 _nj3 = 1;
10808 CheckValue<IkReal> x824 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r10)))),IkReal(((gconst14*gconst14)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
10809 if(!x824.valid){
10810 continue;
10811 }
10812 CheckValue<IkReal> x825=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
10813 if(!x825.valid){
10814 continue;
10815 }
10816 j3array[0]=((-1.5707963267949)+(x824.value)+(((1.5707963267949)*(x825.value))));
10817 sj3array[0]=IKsin(j3array[0]);
10818 cj3array[0]=IKcos(j3array[0]);
10819 if( j3array[0] > IKPI )
10820 {
10821  j3array[0]-=IK2PI;
10822 }
10823 else if( j3array[0] < -IKPI )
10824 { j3array[0]+=IK2PI;
10825 }
10826 j3valid[0] = true;
10827 for(int ij3 = 0; ij3 < 1; ++ij3)
10828 {
10829 if( !j3valid[ij3] )
10830 {
10831  continue;
10832 }
10833 _ij3[0] = ij3; _ij3[1] = -1;
10834 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10835 {
10836 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10837 {
10838  j3valid[iij3]=false; _ij3[1] = iij3; break;
10839 }
10840 }
10841 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10842 {
10843 IkReal evalcond[6];
10844 IkReal x826=IKsin(j3);
10845 IkReal x827=IKcos(j3);
10846 IkReal x828=((1.0)*gconst13);
10847 IkReal x829=(gconst14*x826);
10848 IkReal x830=(gconst14*x827);
10849 IkReal x831=(x827*x828);
10850 evalcond[0]=((((-1.0)*x831))+x829);
10851 evalcond[1]=(((new_r10*x826))+gconst14+((new_r00*x827)));
10852 evalcond[2]=(((gconst13*x826))+new_r00+x830);
10853 evalcond[3]=((((-1.0)*new_r10*x827))+gconst13+((new_r00*x826)));
10854 evalcond[4]=((((-1.0)*x826*x828))+(((-1.0)*x830)));
10855 evalcond[5]=((((-1.0)*x831))+new_r10+x829);
10856 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10857 {
10858 continue;
10859 }
10860 }
10861 
10862 {
10863 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10864 vinfos[0].jointtype = 1;
10865 vinfos[0].foffset = j0;
10866 vinfos[0].indices[0] = _ij0[0];
10867 vinfos[0].indices[1] = _ij0[1];
10868 vinfos[0].maxsolutions = _nj0;
10869 vinfos[1].jointtype = 1;
10870 vinfos[1].foffset = j1;
10871 vinfos[1].indices[0] = _ij1[0];
10872 vinfos[1].indices[1] = _ij1[1];
10873 vinfos[1].maxsolutions = _nj1;
10874 vinfos[2].jointtype = 1;
10875 vinfos[2].foffset = j2;
10876 vinfos[2].indices[0] = _ij2[0];
10877 vinfos[2].indices[1] = _ij2[1];
10878 vinfos[2].maxsolutions = _nj2;
10879 vinfos[3].jointtype = 1;
10880 vinfos[3].foffset = j3;
10881 vinfos[3].indices[0] = _ij3[0];
10882 vinfos[3].indices[1] = _ij3[1];
10883 vinfos[3].maxsolutions = _nj3;
10884 vinfos[4].jointtype = 1;
10885 vinfos[4].foffset = j4;
10886 vinfos[4].indices[0] = _ij4[0];
10887 vinfos[4].indices[1] = _ij4[1];
10888 vinfos[4].maxsolutions = _nj4;
10889 vinfos[5].jointtype = 1;
10890 vinfos[5].foffset = j5;
10891 vinfos[5].indices[0] = _ij5[0];
10892 vinfos[5].indices[1] = _ij5[1];
10893 vinfos[5].maxsolutions = _nj5;
10894 std::vector<int> vfree(0);
10895 solutions.AddSolution(vinfos,vfree);
10896 }
10897 }
10898 }
10899 
10900 }
10901 
10902 }
10903 
10904 } else
10905 {
10906 {
10907 IkReal j3array[1], cj3array[1], sj3array[1];
10908 bool j3valid[1]={false};
10909 _nj3 = 1;
10910 CheckValue<IkReal> x832=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst14*gconst14)))+(((-1.0)*(gconst13*gconst13))))),-1);
10911 if(!x832.valid){
10912 continue;
10913 }
10914 CheckValue<IkReal> x833 = IKatan2WithCheck(IkReal((gconst13*new_r00)),IkReal((gconst14*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10915 if(!x833.valid){
10916 continue;
10917 }
10918 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x832.value)))+(x833.value));
10919 sj3array[0]=IKsin(j3array[0]);
10920 cj3array[0]=IKcos(j3array[0]);
10921 if( j3array[0] > IKPI )
10922 {
10923  j3array[0]-=IK2PI;
10924 }
10925 else if( j3array[0] < -IKPI )
10926 { j3array[0]+=IK2PI;
10927 }
10928 j3valid[0] = true;
10929 for(int ij3 = 0; ij3 < 1; ++ij3)
10930 {
10931 if( !j3valid[ij3] )
10932 {
10933  continue;
10934 }
10935 _ij3[0] = ij3; _ij3[1] = -1;
10936 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10937 {
10938 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10939 {
10940  j3valid[iij3]=false; _ij3[1] = iij3; break;
10941 }
10942 }
10943 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10944 {
10945 IkReal evalcond[6];
10946 IkReal x834=IKsin(j3);
10947 IkReal x835=IKcos(j3);
10948 IkReal x836=((1.0)*gconst13);
10949 IkReal x837=(gconst14*x834);
10950 IkReal x838=(gconst14*x835);
10951 IkReal x839=(x835*x836);
10952 evalcond[0]=((((-1.0)*x839))+x837);
10953 evalcond[1]=(gconst14+((new_r00*x835))+((new_r10*x834)));
10954 evalcond[2]=(((gconst13*x834))+new_r00+x838);
10955 evalcond[3]=(gconst13+((new_r00*x834))+(((-1.0)*new_r10*x835)));
10956 evalcond[4]=((((-1.0)*x834*x836))+(((-1.0)*x838)));
10957 evalcond[5]=((((-1.0)*x839))+new_r10+x837);
10958 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10959 {
10960 continue;
10961 }
10962 }
10963 
10964 {
10965 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10966 vinfos[0].jointtype = 1;
10967 vinfos[0].foffset = j0;
10968 vinfos[0].indices[0] = _ij0[0];
10969 vinfos[0].indices[1] = _ij0[1];
10970 vinfos[0].maxsolutions = _nj0;
10971 vinfos[1].jointtype = 1;
10972 vinfos[1].foffset = j1;
10973 vinfos[1].indices[0] = _ij1[0];
10974 vinfos[1].indices[1] = _ij1[1];
10975 vinfos[1].maxsolutions = _nj1;
10976 vinfos[2].jointtype = 1;
10977 vinfos[2].foffset = j2;
10978 vinfos[2].indices[0] = _ij2[0];
10979 vinfos[2].indices[1] = _ij2[1];
10980 vinfos[2].maxsolutions = _nj2;
10981 vinfos[3].jointtype = 1;
10982 vinfos[3].foffset = j3;
10983 vinfos[3].indices[0] = _ij3[0];
10984 vinfos[3].indices[1] = _ij3[1];
10985 vinfos[3].maxsolutions = _nj3;
10986 vinfos[4].jointtype = 1;
10987 vinfos[4].foffset = j4;
10988 vinfos[4].indices[0] = _ij4[0];
10989 vinfos[4].indices[1] = _ij4[1];
10990 vinfos[4].maxsolutions = _nj4;
10991 vinfos[5].jointtype = 1;
10992 vinfos[5].foffset = j5;
10993 vinfos[5].indices[0] = _ij5[0];
10994 vinfos[5].indices[1] = _ij5[1];
10995 vinfos[5].maxsolutions = _nj5;
10996 std::vector<int> vfree(0);
10997 solutions.AddSolution(vinfos,vfree);
10998 }
10999 }
11000 }
11001 
11002 }
11003 
11004 }
11005 
11006 } else
11007 {
11008 {
11009 IkReal j3array[1], cj3array[1], sj3array[1];
11010 bool j3valid[1]={false};
11011 _nj3 = 1;
11012 CheckValue<IkReal> x840=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
11013 if(!x840.valid){
11014 continue;
11015 }
11016 CheckValue<IkReal> x841 = IKatan2WithCheck(IkReal((gconst13*gconst14)),IkReal(gconst14*gconst14),IKFAST_ATAN2_MAGTHRESH);
11017 if(!x841.valid){
11018 continue;
11019 }
11020 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x840.value)))+(x841.value));
11021 sj3array[0]=IKsin(j3array[0]);
11022 cj3array[0]=IKcos(j3array[0]);
11023 if( j3array[0] > IKPI )
11024 {
11025  j3array[0]-=IK2PI;
11026 }
11027 else if( j3array[0] < -IKPI )
11028 { j3array[0]+=IK2PI;
11029 }
11030 j3valid[0] = true;
11031 for(int ij3 = 0; ij3 < 1; ++ij3)
11032 {
11033 if( !j3valid[ij3] )
11034 {
11035  continue;
11036 }
11037 _ij3[0] = ij3; _ij3[1] = -1;
11038 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11039 {
11040 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11041 {
11042  j3valid[iij3]=false; _ij3[1] = iij3; break;
11043 }
11044 }
11045 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11046 {
11047 IkReal evalcond[6];
11048 IkReal x842=IKsin(j3);
11049 IkReal x843=IKcos(j3);
11050 IkReal x844=((1.0)*gconst13);
11051 IkReal x845=(gconst14*x842);
11052 IkReal x846=(gconst14*x843);
11053 IkReal x847=(x843*x844);
11054 evalcond[0]=((((-1.0)*x847))+x845);
11055 evalcond[1]=(((new_r10*x842))+((new_r00*x843))+gconst14);
11056 evalcond[2]=(((gconst13*x842))+new_r00+x846);
11057 evalcond[3]=(((new_r00*x842))+gconst13+(((-1.0)*new_r10*x843)));
11058 evalcond[4]=((((-1.0)*x842*x844))+(((-1.0)*x846)));
11059 evalcond[5]=((((-1.0)*x847))+new_r10+x845);
11060 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11061 {
11062 continue;
11063 }
11064 }
11065 
11066 {
11067 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11068 vinfos[0].jointtype = 1;
11069 vinfos[0].foffset = j0;
11070 vinfos[0].indices[0] = _ij0[0];
11071 vinfos[0].indices[1] = _ij0[1];
11072 vinfos[0].maxsolutions = _nj0;
11073 vinfos[1].jointtype = 1;
11074 vinfos[1].foffset = j1;
11075 vinfos[1].indices[0] = _ij1[0];
11076 vinfos[1].indices[1] = _ij1[1];
11077 vinfos[1].maxsolutions = _nj1;
11078 vinfos[2].jointtype = 1;
11079 vinfos[2].foffset = j2;
11080 vinfos[2].indices[0] = _ij2[0];
11081 vinfos[2].indices[1] = _ij2[1];
11082 vinfos[2].maxsolutions = _nj2;
11083 vinfos[3].jointtype = 1;
11084 vinfos[3].foffset = j3;
11085 vinfos[3].indices[0] = _ij3[0];
11086 vinfos[3].indices[1] = _ij3[1];
11087 vinfos[3].maxsolutions = _nj3;
11088 vinfos[4].jointtype = 1;
11089 vinfos[4].foffset = j4;
11090 vinfos[4].indices[0] = _ij4[0];
11091 vinfos[4].indices[1] = _ij4[1];
11092 vinfos[4].maxsolutions = _nj4;
11093 vinfos[5].jointtype = 1;
11094 vinfos[5].foffset = j5;
11095 vinfos[5].indices[0] = _ij5[0];
11096 vinfos[5].indices[1] = _ij5[1];
11097 vinfos[5].maxsolutions = _nj5;
11098 std::vector<int> vfree(0);
11099 solutions.AddSolution(vinfos,vfree);
11100 }
11101 }
11102 }
11103 
11104 }
11105 
11106 }
11107 
11108 }
11109 } while(0);
11110 if( bgotonextstatement )
11111 {
11112 bool bgotonextstatement = true;
11113 do
11114 {
11115 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
11116 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11117 {
11118 bgotonextstatement=false;
11119 {
11120 IkReal j3eval[1];
11121 IkReal x848=((-1.0)*new_r00);
11122 CheckValue<IkReal> x850 = IKatan2WithCheck(IkReal(0),IkReal(x848),IKFAST_ATAN2_MAGTHRESH);
11123 if(!x850.valid){
11124 continue;
11125 }
11126 IkReal x849=((-1.0)*(x850.value));
11127 sj4=0;
11128 cj4=-1.0;
11129 j4=3.14159265358979;
11130 sj5=gconst13;
11131 cj5=gconst14;
11132 j5=x849;
11133 new_r01=0;
11134 new_r10=0;
11135 IkReal gconst12=x849;
11136 IkReal gconst13=0;
11137 IkReal x851 = new_r00*new_r00;
11138 if(IKabs(x851)==0){
11139 continue;
11140 }
11141 IkReal gconst14=(x848*(pow(x851,-0.5)));
11142 j3eval[0]=new_r11;
11143 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11144 {
11145 {
11146 IkReal j3array[2], cj3array[2], sj3array[2];
11147 bool j3valid[2]={false};
11148 _nj3 = 2;
11149 CheckValue<IkReal> x852=IKPowWithIntegerCheck(gconst14,-1);
11150 if(!x852.valid){
11151 continue;
11152 }
11153 cj3array[0]=(new_r11*(x852.value));
11154 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11155 {
11156  j3valid[0] = j3valid[1] = true;
11157  j3array[0] = IKacos(cj3array[0]);
11158  sj3array[0] = IKsin(j3array[0]);
11159  cj3array[1] = cj3array[0];
11160  j3array[1] = -j3array[0];
11161  sj3array[1] = -sj3array[0];
11162 }
11163 else if( isnan(cj3array[0]) )
11164 {
11165  // probably any value will work
11166  j3valid[0] = true;
11167  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11168 }
11169 for(int ij3 = 0; ij3 < 2; ++ij3)
11170 {
11171 if( !j3valid[ij3] )
11172 {
11173  continue;
11174 }
11175 _ij3[0] = ij3; _ij3[1] = -1;
11176 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11177 {
11178 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11179 {
11180  j3valid[iij3]=false; _ij3[1] = iij3; break;
11181 }
11182 }
11183 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11184 {
11185 IkReal evalcond[6];
11186 IkReal x853=IKsin(j3);
11187 IkReal x854=IKcos(j3);
11188 evalcond[0]=(new_r00*x853);
11189 evalcond[1]=(new_r11*x853);
11190 evalcond[2]=(gconst14*x853);
11191 evalcond[3]=(gconst14+((new_r00*x854)));
11192 evalcond[4]=(new_r00+((gconst14*x854)));
11193 evalcond[5]=(gconst14+(((-1.0)*new_r11*x854)));
11194 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11195 {
11196 continue;
11197 }
11198 }
11199 
11200 {
11201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11202 vinfos[0].jointtype = 1;
11203 vinfos[0].foffset = j0;
11204 vinfos[0].indices[0] = _ij0[0];
11205 vinfos[0].indices[1] = _ij0[1];
11206 vinfos[0].maxsolutions = _nj0;
11207 vinfos[1].jointtype = 1;
11208 vinfos[1].foffset = j1;
11209 vinfos[1].indices[0] = _ij1[0];
11210 vinfos[1].indices[1] = _ij1[1];
11211 vinfos[1].maxsolutions = _nj1;
11212 vinfos[2].jointtype = 1;
11213 vinfos[2].foffset = j2;
11214 vinfos[2].indices[0] = _ij2[0];
11215 vinfos[2].indices[1] = _ij2[1];
11216 vinfos[2].maxsolutions = _nj2;
11217 vinfos[3].jointtype = 1;
11218 vinfos[3].foffset = j3;
11219 vinfos[3].indices[0] = _ij3[0];
11220 vinfos[3].indices[1] = _ij3[1];
11221 vinfos[3].maxsolutions = _nj3;
11222 vinfos[4].jointtype = 1;
11223 vinfos[4].foffset = j4;
11224 vinfos[4].indices[0] = _ij4[0];
11225 vinfos[4].indices[1] = _ij4[1];
11226 vinfos[4].maxsolutions = _nj4;
11227 vinfos[5].jointtype = 1;
11228 vinfos[5].foffset = j5;
11229 vinfos[5].indices[0] = _ij5[0];
11230 vinfos[5].indices[1] = _ij5[1];
11231 vinfos[5].maxsolutions = _nj5;
11232 std::vector<int> vfree(0);
11233 solutions.AddSolution(vinfos,vfree);
11234 }
11235 }
11236 }
11237 
11238 } else
11239 {
11240 {
11241 IkReal j3array[2], cj3array[2], sj3array[2];
11242 bool j3valid[2]={false};
11243 _nj3 = 2;
11244 CheckValue<IkReal> x855=IKPowWithIntegerCheck(new_r11,-1);
11245 if(!x855.valid){
11246 continue;
11247 }
11248 cj3array[0]=(gconst14*(x855.value));
11249 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11250 {
11251  j3valid[0] = j3valid[1] = true;
11252  j3array[0] = IKacos(cj3array[0]);
11253  sj3array[0] = IKsin(j3array[0]);
11254  cj3array[1] = cj3array[0];
11255  j3array[1] = -j3array[0];
11256  sj3array[1] = -sj3array[0];
11257 }
11258 else if( isnan(cj3array[0]) )
11259 {
11260  // probably any value will work
11261  j3valid[0] = true;
11262  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11263 }
11264 for(int ij3 = 0; ij3 < 2; ++ij3)
11265 {
11266 if( !j3valid[ij3] )
11267 {
11268  continue;
11269 }
11270 _ij3[0] = ij3; _ij3[1] = -1;
11271 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11272 {
11273 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11274 {
11275  j3valid[iij3]=false; _ij3[1] = iij3; break;
11276 }
11277 }
11278 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11279 {
11280 IkReal evalcond[6];
11281 IkReal x856=IKsin(j3);
11282 IkReal x857=IKcos(j3);
11283 IkReal x858=(gconst14*x857);
11284 evalcond[0]=(new_r00*x856);
11285 evalcond[1]=(new_r11*x856);
11286 evalcond[2]=(gconst14*x856);
11287 evalcond[3]=(gconst14+((new_r00*x857)));
11288 evalcond[4]=(new_r00+x858);
11289 evalcond[5]=((((-1.0)*x858))+new_r11);
11290 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11291 {
11292 continue;
11293 }
11294 }
11295 
11296 {
11297 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11298 vinfos[0].jointtype = 1;
11299 vinfos[0].foffset = j0;
11300 vinfos[0].indices[0] = _ij0[0];
11301 vinfos[0].indices[1] = _ij0[1];
11302 vinfos[0].maxsolutions = _nj0;
11303 vinfos[1].jointtype = 1;
11304 vinfos[1].foffset = j1;
11305 vinfos[1].indices[0] = _ij1[0];
11306 vinfos[1].indices[1] = _ij1[1];
11307 vinfos[1].maxsolutions = _nj1;
11308 vinfos[2].jointtype = 1;
11309 vinfos[2].foffset = j2;
11310 vinfos[2].indices[0] = _ij2[0];
11311 vinfos[2].indices[1] = _ij2[1];
11312 vinfos[2].maxsolutions = _nj2;
11313 vinfos[3].jointtype = 1;
11314 vinfos[3].foffset = j3;
11315 vinfos[3].indices[0] = _ij3[0];
11316 vinfos[3].indices[1] = _ij3[1];
11317 vinfos[3].maxsolutions = _nj3;
11318 vinfos[4].jointtype = 1;
11319 vinfos[4].foffset = j4;
11320 vinfos[4].indices[0] = _ij4[0];
11321 vinfos[4].indices[1] = _ij4[1];
11322 vinfos[4].maxsolutions = _nj4;
11323 vinfos[5].jointtype = 1;
11324 vinfos[5].foffset = j5;
11325 vinfos[5].indices[0] = _ij5[0];
11326 vinfos[5].indices[1] = _ij5[1];
11327 vinfos[5].maxsolutions = _nj5;
11328 std::vector<int> vfree(0);
11329 solutions.AddSolution(vinfos,vfree);
11330 }
11331 }
11332 }
11333 
11334 }
11335 
11336 }
11337 
11338 }
11339 } while(0);
11340 if( bgotonextstatement )
11341 {
11342 bool bgotonextstatement = true;
11343 do
11344 {
11345 evalcond[0]=IKabs(new_r00);
11346 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11347 {
11348 bgotonextstatement=false;
11349 {
11350 IkReal j3eval[1];
11351 CheckValue<IkReal> x860 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11352 if(!x860.valid){
11353 continue;
11354 }
11355 IkReal x859=((-1.0)*(x860.value));
11356 sj4=0;
11357 cj4=-1.0;
11358 j4=3.14159265358979;
11359 sj5=gconst13;
11360 cj5=gconst14;
11361 j5=x859;
11362 new_r00=0;
11363 IkReal gconst12=x859;
11364 IkReal x861 = new_r10*new_r10;
11365 if(IKabs(x861)==0){
11366 continue;
11367 }
11368 IkReal gconst13=((-1.0)*new_r10*(pow(x861,-0.5)));
11369 IkReal gconst14=0;
11370 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11371 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11372 {
11373 {
11374 IkReal j3eval[1];
11375 CheckValue<IkReal> x863 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11376 if(!x863.valid){
11377 continue;
11378 }
11379 IkReal x862=((-1.0)*(x863.value));
11380 sj4=0;
11381 cj4=-1.0;
11382 j4=3.14159265358979;
11383 sj5=gconst13;
11384 cj5=gconst14;
11385 j5=x862;
11386 new_r00=0;
11387 IkReal gconst12=x862;
11388 IkReal x864 = new_r10*new_r10;
11389 if(IKabs(x864)==0){
11390 continue;
11391 }
11392 IkReal gconst13=((-1.0)*new_r10*(pow(x864,-0.5)));
11393 IkReal gconst14=0;
11394 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11395 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11396 {
11397 {
11398 IkReal j3eval[1];
11399 CheckValue<IkReal> x866 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11400 if(!x866.valid){
11401 continue;
11402 }
11403 IkReal x865=((-1.0)*(x866.value));
11404 sj4=0;
11405 cj4=-1.0;
11406 j4=3.14159265358979;
11407 sj5=gconst13;
11408 cj5=gconst14;
11409 j5=x865;
11410 new_r00=0;
11411 IkReal gconst12=x865;
11412 IkReal x867 = new_r10*new_r10;
11413 if(IKabs(x867)==0){
11414 continue;
11415 }
11416 IkReal gconst13=((-1.0)*new_r10*(pow(x867,-0.5)));
11417 IkReal gconst14=0;
11418 j3eval[0]=new_r10;
11419 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11420 {
11421 continue; // 3 cases reached
11422 
11423 } else
11424 {
11425 {
11426 IkReal j3array[1], cj3array[1], sj3array[1];
11427 bool j3valid[1]={false};
11428 _nj3 = 1;
11429 CheckValue<IkReal> x868=IKPowWithIntegerCheck(gconst13,-1);
11430 if(!x868.valid){
11431 continue;
11432 }
11433 CheckValue<IkReal> x869=IKPowWithIntegerCheck(new_r10,-1);
11434 if(!x869.valid){
11435 continue;
11436 }
11437 if( IKabs((new_r11*(x868.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst13*(x869.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x868.value)))+IKsqr((gconst13*(x869.value)))-1) <= IKFAST_SINCOS_THRESH )
11438  continue;
11439 j3array[0]=IKatan2((new_r11*(x868.value)), (gconst13*(x869.value)));
11440 sj3array[0]=IKsin(j3array[0]);
11441 cj3array[0]=IKcos(j3array[0]);
11442 if( j3array[0] > IKPI )
11443 {
11444  j3array[0]-=IK2PI;
11445 }
11446 else if( j3array[0] < -IKPI )
11447 { j3array[0]+=IK2PI;
11448 }
11449 j3valid[0] = true;
11450 for(int ij3 = 0; ij3 < 1; ++ij3)
11451 {
11452 if( !j3valid[ij3] )
11453 {
11454  continue;
11455 }
11456 _ij3[0] = ij3; _ij3[1] = -1;
11457 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11458 {
11459 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11460 {
11461  j3valid[iij3]=false; _ij3[1] = iij3; break;
11462 }
11463 }
11464 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11465 {
11466 IkReal evalcond[8];
11467 IkReal x870=IKsin(j3);
11468 IkReal x871=IKcos(j3);
11469 IkReal x872=((1.0)*x871);
11470 IkReal x873=(gconst13*x870);
11471 IkReal x874=(gconst13*x872);
11472 evalcond[0]=(new_r10*x870);
11473 evalcond[1]=x873;
11474 evalcond[2]=(gconst13+(((-1.0)*new_r10*x872)));
11475 evalcond[3]=(new_r01+(((-1.0)*x874)));
11476 evalcond[4]=((((-1.0)*x873))+new_r11);
11477 evalcond[5]=(new_r10+(((-1.0)*x874)));
11478 evalcond[6]=(((new_r01*x870))+(((-1.0)*new_r11*x872)));
11479 evalcond[7]=(((new_r01*x871))+((new_r11*x870))+(((-1.0)*gconst13)));
11480 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11481 {
11482 continue;
11483 }
11484 }
11485 
11486 {
11487 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11488 vinfos[0].jointtype = 1;
11489 vinfos[0].foffset = j0;
11490 vinfos[0].indices[0] = _ij0[0];
11491 vinfos[0].indices[1] = _ij0[1];
11492 vinfos[0].maxsolutions = _nj0;
11493 vinfos[1].jointtype = 1;
11494 vinfos[1].foffset = j1;
11495 vinfos[1].indices[0] = _ij1[0];
11496 vinfos[1].indices[1] = _ij1[1];
11497 vinfos[1].maxsolutions = _nj1;
11498 vinfos[2].jointtype = 1;
11499 vinfos[2].foffset = j2;
11500 vinfos[2].indices[0] = _ij2[0];
11501 vinfos[2].indices[1] = _ij2[1];
11502 vinfos[2].maxsolutions = _nj2;
11503 vinfos[3].jointtype = 1;
11504 vinfos[3].foffset = j3;
11505 vinfos[3].indices[0] = _ij3[0];
11506 vinfos[3].indices[1] = _ij3[1];
11507 vinfos[3].maxsolutions = _nj3;
11508 vinfos[4].jointtype = 1;
11509 vinfos[4].foffset = j4;
11510 vinfos[4].indices[0] = _ij4[0];
11511 vinfos[4].indices[1] = _ij4[1];
11512 vinfos[4].maxsolutions = _nj4;
11513 vinfos[5].jointtype = 1;
11514 vinfos[5].foffset = j5;
11515 vinfos[5].indices[0] = _ij5[0];
11516 vinfos[5].indices[1] = _ij5[1];
11517 vinfos[5].maxsolutions = _nj5;
11518 std::vector<int> vfree(0);
11519 solutions.AddSolution(vinfos,vfree);
11520 }
11521 }
11522 }
11523 
11524 }
11525 
11526 }
11527 
11528 } else
11529 {
11530 {
11531 IkReal j3array[1], cj3array[1], sj3array[1];
11532 bool j3valid[1]={false};
11533 _nj3 = 1;
11534 CheckValue<IkReal> x875 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11535 if(!x875.valid){
11536 continue;
11537 }
11538 CheckValue<IkReal> x876=IKPowWithIntegerCheck(IKsign(gconst13),-1);
11539 if(!x876.valid){
11540 continue;
11541 }
11542 j3array[0]=((-1.5707963267949)+(x875.value)+(((1.5707963267949)*(x876.value))));
11543 sj3array[0]=IKsin(j3array[0]);
11544 cj3array[0]=IKcos(j3array[0]);
11545 if( j3array[0] > IKPI )
11546 {
11547  j3array[0]-=IK2PI;
11548 }
11549 else if( j3array[0] < -IKPI )
11550 { j3array[0]+=IK2PI;
11551 }
11552 j3valid[0] = true;
11553 for(int ij3 = 0; ij3 < 1; ++ij3)
11554 {
11555 if( !j3valid[ij3] )
11556 {
11557  continue;
11558 }
11559 _ij3[0] = ij3; _ij3[1] = -1;
11560 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11561 {
11562 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11563 {
11564  j3valid[iij3]=false; _ij3[1] = iij3; break;
11565 }
11566 }
11567 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11568 {
11569 IkReal evalcond[8];
11570 IkReal x877=IKsin(j3);
11571 IkReal x878=IKcos(j3);
11572 IkReal x879=((1.0)*x878);
11573 IkReal x880=(gconst13*x877);
11574 IkReal x881=(gconst13*x879);
11575 evalcond[0]=(new_r10*x877);
11576 evalcond[1]=x880;
11577 evalcond[2]=(gconst13+(((-1.0)*new_r10*x879)));
11578 evalcond[3]=((((-1.0)*x881))+new_r01);
11579 evalcond[4]=((((-1.0)*x880))+new_r11);
11580 evalcond[5]=((((-1.0)*x881))+new_r10);
11581 evalcond[6]=(((new_r01*x877))+(((-1.0)*new_r11*x879)));
11582 evalcond[7]=(((new_r01*x878))+((new_r11*x877))+(((-1.0)*gconst13)));
11583 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11584 {
11585 continue;
11586 }
11587 }
11588 
11589 {
11590 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11591 vinfos[0].jointtype = 1;
11592 vinfos[0].foffset = j0;
11593 vinfos[0].indices[0] = _ij0[0];
11594 vinfos[0].indices[1] = _ij0[1];
11595 vinfos[0].maxsolutions = _nj0;
11596 vinfos[1].jointtype = 1;
11597 vinfos[1].foffset = j1;
11598 vinfos[1].indices[0] = _ij1[0];
11599 vinfos[1].indices[1] = _ij1[1];
11600 vinfos[1].maxsolutions = _nj1;
11601 vinfos[2].jointtype = 1;
11602 vinfos[2].foffset = j2;
11603 vinfos[2].indices[0] = _ij2[0];
11604 vinfos[2].indices[1] = _ij2[1];
11605 vinfos[2].maxsolutions = _nj2;
11606 vinfos[3].jointtype = 1;
11607 vinfos[3].foffset = j3;
11608 vinfos[3].indices[0] = _ij3[0];
11609 vinfos[3].indices[1] = _ij3[1];
11610 vinfos[3].maxsolutions = _nj3;
11611 vinfos[4].jointtype = 1;
11612 vinfos[4].foffset = j4;
11613 vinfos[4].indices[0] = _ij4[0];
11614 vinfos[4].indices[1] = _ij4[1];
11615 vinfos[4].maxsolutions = _nj4;
11616 vinfos[5].jointtype = 1;
11617 vinfos[5].foffset = j5;
11618 vinfos[5].indices[0] = _ij5[0];
11619 vinfos[5].indices[1] = _ij5[1];
11620 vinfos[5].maxsolutions = _nj5;
11621 std::vector<int> vfree(0);
11622 solutions.AddSolution(vinfos,vfree);
11623 }
11624 }
11625 }
11626 
11627 }
11628 
11629 }
11630 
11631 } else
11632 {
11633 {
11634 IkReal j3array[1], cj3array[1], sj3array[1];
11635 bool j3valid[1]={false};
11636 _nj3 = 1;
11637 CheckValue<IkReal> x882 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
11638 if(!x882.valid){
11639 continue;
11640 }
11641 CheckValue<IkReal> x883=IKPowWithIntegerCheck(IKsign(gconst13),-1);
11642 if(!x883.valid){
11643 continue;
11644 }
11645 j3array[0]=((-1.5707963267949)+(x882.value)+(((1.5707963267949)*(x883.value))));
11646 sj3array[0]=IKsin(j3array[0]);
11647 cj3array[0]=IKcos(j3array[0]);
11648 if( j3array[0] > IKPI )
11649 {
11650  j3array[0]-=IK2PI;
11651 }
11652 else if( j3array[0] < -IKPI )
11653 { j3array[0]+=IK2PI;
11654 }
11655 j3valid[0] = true;
11656 for(int ij3 = 0; ij3 < 1; ++ij3)
11657 {
11658 if( !j3valid[ij3] )
11659 {
11660  continue;
11661 }
11662 _ij3[0] = ij3; _ij3[1] = -1;
11663 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11664 {
11665 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11666 {
11667  j3valid[iij3]=false; _ij3[1] = iij3; break;
11668 }
11669 }
11670 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11671 {
11672 IkReal evalcond[8];
11673 IkReal x884=IKsin(j3);
11674 IkReal x885=IKcos(j3);
11675 IkReal x886=((1.0)*x885);
11676 IkReal x887=(gconst13*x884);
11677 IkReal x888=(gconst13*x886);
11678 evalcond[0]=(new_r10*x884);
11679 evalcond[1]=x887;
11680 evalcond[2]=((((-1.0)*new_r10*x886))+gconst13);
11681 evalcond[3]=((((-1.0)*x888))+new_r01);
11682 evalcond[4]=((((-1.0)*x887))+new_r11);
11683 evalcond[5]=((((-1.0)*x888))+new_r10);
11684 evalcond[6]=(((new_r01*x884))+(((-1.0)*new_r11*x886)));
11685 evalcond[7]=(((new_r01*x885))+((new_r11*x884))+(((-1.0)*gconst13)));
11686 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11687 {
11688 continue;
11689 }
11690 }
11691 
11692 {
11693 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11694 vinfos[0].jointtype = 1;
11695 vinfos[0].foffset = j0;
11696 vinfos[0].indices[0] = _ij0[0];
11697 vinfos[0].indices[1] = _ij0[1];
11698 vinfos[0].maxsolutions = _nj0;
11699 vinfos[1].jointtype = 1;
11700 vinfos[1].foffset = j1;
11701 vinfos[1].indices[0] = _ij1[0];
11702 vinfos[1].indices[1] = _ij1[1];
11703 vinfos[1].maxsolutions = _nj1;
11704 vinfos[2].jointtype = 1;
11705 vinfos[2].foffset = j2;
11706 vinfos[2].indices[0] = _ij2[0];
11707 vinfos[2].indices[1] = _ij2[1];
11708 vinfos[2].maxsolutions = _nj2;
11709 vinfos[3].jointtype = 1;
11710 vinfos[3].foffset = j3;
11711 vinfos[3].indices[0] = _ij3[0];
11712 vinfos[3].indices[1] = _ij3[1];
11713 vinfos[3].maxsolutions = _nj3;
11714 vinfos[4].jointtype = 1;
11715 vinfos[4].foffset = j4;
11716 vinfos[4].indices[0] = _ij4[0];
11717 vinfos[4].indices[1] = _ij4[1];
11718 vinfos[4].maxsolutions = _nj4;
11719 vinfos[5].jointtype = 1;
11720 vinfos[5].foffset = j5;
11721 vinfos[5].indices[0] = _ij5[0];
11722 vinfos[5].indices[1] = _ij5[1];
11723 vinfos[5].maxsolutions = _nj5;
11724 std::vector<int> vfree(0);
11725 solutions.AddSolution(vinfos,vfree);
11726 }
11727 }
11728 }
11729 
11730 }
11731 
11732 }
11733 
11734 }
11735 } while(0);
11736 if( bgotonextstatement )
11737 {
11738 bool bgotonextstatement = true;
11739 do
11740 {
11741 if( 1 )
11742 {
11743 bgotonextstatement=false;
11744 continue; // branch miss [j3]
11745 
11746 }
11747 } while(0);
11748 if( bgotonextstatement )
11749 {
11750 }
11751 }
11752 }
11753 }
11754 }
11755 }
11756 
11757 } else
11758 {
11759 {
11760 IkReal j3array[1], cj3array[1], sj3array[1];
11761 bool j3valid[1]={false};
11762 _nj3 = 1;
11763 CheckValue<IkReal> x889 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r10)))),IkReal(((gconst14*gconst14)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
11764 if(!x889.valid){
11765 continue;
11766 }
11767 CheckValue<IkReal> x890=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
11768 if(!x890.valid){
11769 continue;
11770 }
11771 j3array[0]=((-1.5707963267949)+(x889.value)+(((1.5707963267949)*(x890.value))));
11772 sj3array[0]=IKsin(j3array[0]);
11773 cj3array[0]=IKcos(j3array[0]);
11774 if( j3array[0] > IKPI )
11775 {
11776  j3array[0]-=IK2PI;
11777 }
11778 else if( j3array[0] < -IKPI )
11779 { j3array[0]+=IK2PI;
11780 }
11781 j3valid[0] = true;
11782 for(int ij3 = 0; ij3 < 1; ++ij3)
11783 {
11784 if( !j3valid[ij3] )
11785 {
11786  continue;
11787 }
11788 _ij3[0] = ij3; _ij3[1] = -1;
11789 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11790 {
11791 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11792 {
11793  j3valid[iij3]=false; _ij3[1] = iij3; break;
11794 }
11795 }
11796 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11797 {
11798 IkReal evalcond[8];
11799 IkReal x891=IKsin(j3);
11800 IkReal x892=IKcos(j3);
11801 IkReal x893=(gconst14*x891);
11802 IkReal x894=((1.0)*x892);
11803 IkReal x895=(gconst13*x891);
11804 IkReal x896=(gconst13*x894);
11805 evalcond[0]=(((new_r00*x892))+((new_r10*x891))+gconst14);
11806 evalcond[1]=(((gconst14*x892))+new_r00+x895);
11807 evalcond[2]=(((new_r00*x891))+(((-1.0)*new_r10*x894))+gconst13);
11808 evalcond[3]=(((new_r01*x891))+(((-1.0)*new_r11*x894))+gconst14);
11809 evalcond[4]=((((-1.0)*x896))+new_r01+x893);
11810 evalcond[5]=((((-1.0)*x896))+new_r10+x893);
11811 evalcond[6]=(((new_r11*x891))+((new_r01*x892))+(((-1.0)*gconst13)));
11812 evalcond[7]=((((-1.0)*x895))+new_r11+(((-1.0)*gconst14*x894)));
11813 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11814 {
11815 continue;
11816 }
11817 }
11818 
11819 {
11820 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11821 vinfos[0].jointtype = 1;
11822 vinfos[0].foffset = j0;
11823 vinfos[0].indices[0] = _ij0[0];
11824 vinfos[0].indices[1] = _ij0[1];
11825 vinfos[0].maxsolutions = _nj0;
11826 vinfos[1].jointtype = 1;
11827 vinfos[1].foffset = j1;
11828 vinfos[1].indices[0] = _ij1[0];
11829 vinfos[1].indices[1] = _ij1[1];
11830 vinfos[1].maxsolutions = _nj1;
11831 vinfos[2].jointtype = 1;
11832 vinfos[2].foffset = j2;
11833 vinfos[2].indices[0] = _ij2[0];
11834 vinfos[2].indices[1] = _ij2[1];
11835 vinfos[2].maxsolutions = _nj2;
11836 vinfos[3].jointtype = 1;
11837 vinfos[3].foffset = j3;
11838 vinfos[3].indices[0] = _ij3[0];
11839 vinfos[3].indices[1] = _ij3[1];
11840 vinfos[3].maxsolutions = _nj3;
11841 vinfos[4].jointtype = 1;
11842 vinfos[4].foffset = j4;
11843 vinfos[4].indices[0] = _ij4[0];
11844 vinfos[4].indices[1] = _ij4[1];
11845 vinfos[4].maxsolutions = _nj4;
11846 vinfos[5].jointtype = 1;
11847 vinfos[5].foffset = j5;
11848 vinfos[5].indices[0] = _ij5[0];
11849 vinfos[5].indices[1] = _ij5[1];
11850 vinfos[5].maxsolutions = _nj5;
11851 std::vector<int> vfree(0);
11852 solutions.AddSolution(vinfos,vfree);
11853 }
11854 }
11855 }
11856 
11857 }
11858 
11859 }
11860 
11861 } else
11862 {
11863 {
11864 IkReal j3array[1], cj3array[1], sj3array[1];
11865 bool j3valid[1]={false};
11866 _nj3 = 1;
11867 IkReal x897=((1.0)*new_r10);
11868 CheckValue<IkReal> x898=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*x897))+(((-1.0)*gconst14*new_r00)))),-1);
11869 if(!x898.valid){
11870 continue;
11871 }
11872 CheckValue<IkReal> x899 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r01)))),IkReal(((gconst14*gconst14)+(((-1.0)*new_r01*x897)))),IKFAST_ATAN2_MAGTHRESH);
11873 if(!x899.valid){
11874 continue;
11875 }
11876 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x898.value)))+(x899.value));
11877 sj3array[0]=IKsin(j3array[0]);
11878 cj3array[0]=IKcos(j3array[0]);
11879 if( j3array[0] > IKPI )
11880 {
11881  j3array[0]-=IK2PI;
11882 }
11883 else if( j3array[0] < -IKPI )
11884 { j3array[0]+=IK2PI;
11885 }
11886 j3valid[0] = true;
11887 for(int ij3 = 0; ij3 < 1; ++ij3)
11888 {
11889 if( !j3valid[ij3] )
11890 {
11891  continue;
11892 }
11893 _ij3[0] = ij3; _ij3[1] = -1;
11894 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11895 {
11896 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11897 {
11898  j3valid[iij3]=false; _ij3[1] = iij3; break;
11899 }
11900 }
11901 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11902 {
11903 IkReal evalcond[8];
11904 IkReal x900=IKsin(j3);
11905 IkReal x901=IKcos(j3);
11906 IkReal x902=(gconst14*x900);
11907 IkReal x903=((1.0)*x901);
11908 IkReal x904=(gconst13*x900);
11909 IkReal x905=(gconst13*x903);
11910 evalcond[0]=(gconst14+((new_r10*x900))+((new_r00*x901)));
11911 evalcond[1]=(new_r00+x904+((gconst14*x901)));
11912 evalcond[2]=(gconst13+((new_r00*x900))+(((-1.0)*new_r10*x903)));
11913 evalcond[3]=((((-1.0)*new_r11*x903))+gconst14+((new_r01*x900)));
11914 evalcond[4]=((((-1.0)*x905))+new_r01+x902);
11915 evalcond[5]=((((-1.0)*x905))+new_r10+x902);
11916 evalcond[6]=(((new_r11*x900))+((new_r01*x901))+(((-1.0)*gconst13)));
11917 evalcond[7]=((((-1.0)*gconst14*x903))+(((-1.0)*x904))+new_r11);
11918 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11919 {
11920 continue;
11921 }
11922 }
11923 
11924 {
11925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11926 vinfos[0].jointtype = 1;
11927 vinfos[0].foffset = j0;
11928 vinfos[0].indices[0] = _ij0[0];
11929 vinfos[0].indices[1] = _ij0[1];
11930 vinfos[0].maxsolutions = _nj0;
11931 vinfos[1].jointtype = 1;
11932 vinfos[1].foffset = j1;
11933 vinfos[1].indices[0] = _ij1[0];
11934 vinfos[1].indices[1] = _ij1[1];
11935 vinfos[1].maxsolutions = _nj1;
11936 vinfos[2].jointtype = 1;
11937 vinfos[2].foffset = j2;
11938 vinfos[2].indices[0] = _ij2[0];
11939 vinfos[2].indices[1] = _ij2[1];
11940 vinfos[2].maxsolutions = _nj2;
11941 vinfos[3].jointtype = 1;
11942 vinfos[3].foffset = j3;
11943 vinfos[3].indices[0] = _ij3[0];
11944 vinfos[3].indices[1] = _ij3[1];
11945 vinfos[3].maxsolutions = _nj3;
11946 vinfos[4].jointtype = 1;
11947 vinfos[4].foffset = j4;
11948 vinfos[4].indices[0] = _ij4[0];
11949 vinfos[4].indices[1] = _ij4[1];
11950 vinfos[4].maxsolutions = _nj4;
11951 vinfos[5].jointtype = 1;
11952 vinfos[5].foffset = j5;
11953 vinfos[5].indices[0] = _ij5[0];
11954 vinfos[5].indices[1] = _ij5[1];
11955 vinfos[5].maxsolutions = _nj5;
11956 std::vector<int> vfree(0);
11957 solutions.AddSolution(vinfos,vfree);
11958 }
11959 }
11960 }
11961 
11962 }
11963 
11964 }
11965 
11966 } else
11967 {
11968 {
11969 IkReal j3array[1], cj3array[1], sj3array[1];
11970 bool j3valid[1]={false};
11971 _nj3 = 1;
11972 IkReal x906=((1.0)*new_r10);
11973 CheckValue<IkReal> x907 = IKatan2WithCheck(IkReal((((gconst14*new_r00))+((gconst14*new_r11)))),IkReal(((((-1.0)*gconst14*x906))+((gconst14*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
11974 if(!x907.valid){
11975 continue;
11976 }
11977 CheckValue<IkReal> x908=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x906))+(((-1.0)*new_r00*new_r01)))),-1);
11978 if(!x908.valid){
11979 continue;
11980 }
11981 j3array[0]=((-1.5707963267949)+(x907.value)+(((1.5707963267949)*(x908.value))));
11982 sj3array[0]=IKsin(j3array[0]);
11983 cj3array[0]=IKcos(j3array[0]);
11984 if( j3array[0] > IKPI )
11985 {
11986  j3array[0]-=IK2PI;
11987 }
11988 else if( j3array[0] < -IKPI )
11989 { j3array[0]+=IK2PI;
11990 }
11991 j3valid[0] = true;
11992 for(int ij3 = 0; ij3 < 1; ++ij3)
11993 {
11994 if( !j3valid[ij3] )
11995 {
11996  continue;
11997 }
11998 _ij3[0] = ij3; _ij3[1] = -1;
11999 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12000 {
12001 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12002 {
12003  j3valid[iij3]=false; _ij3[1] = iij3; break;
12004 }
12005 }
12006 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12007 {
12008 IkReal evalcond[8];
12009 IkReal x909=IKsin(j3);
12010 IkReal x910=IKcos(j3);
12011 IkReal x911=(gconst14*x909);
12012 IkReal x912=((1.0)*x910);
12013 IkReal x913=(gconst13*x909);
12014 IkReal x914=(gconst13*x912);
12015 evalcond[0]=(gconst14+((new_r10*x909))+((new_r00*x910)));
12016 evalcond[1]=(((gconst14*x910))+new_r00+x913);
12017 evalcond[2]=(gconst13+((new_r00*x909))+(((-1.0)*new_r10*x912)));
12018 evalcond[3]=(gconst14+((new_r01*x909))+(((-1.0)*new_r11*x912)));
12019 evalcond[4]=((((-1.0)*x914))+new_r01+x911);
12020 evalcond[5]=((((-1.0)*x914))+new_r10+x911);
12021 evalcond[6]=(((new_r11*x909))+((new_r01*x910))+(((-1.0)*gconst13)));
12022 evalcond[7]=((((-1.0)*gconst14*x912))+(((-1.0)*x913))+new_r11);
12023 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12024 {
12025 continue;
12026 }
12027 }
12028 
12029 {
12030 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12031 vinfos[0].jointtype = 1;
12032 vinfos[0].foffset = j0;
12033 vinfos[0].indices[0] = _ij0[0];
12034 vinfos[0].indices[1] = _ij0[1];
12035 vinfos[0].maxsolutions = _nj0;
12036 vinfos[1].jointtype = 1;
12037 vinfos[1].foffset = j1;
12038 vinfos[1].indices[0] = _ij1[0];
12039 vinfos[1].indices[1] = _ij1[1];
12040 vinfos[1].maxsolutions = _nj1;
12041 vinfos[2].jointtype = 1;
12042 vinfos[2].foffset = j2;
12043 vinfos[2].indices[0] = _ij2[0];
12044 vinfos[2].indices[1] = _ij2[1];
12045 vinfos[2].maxsolutions = _nj2;
12046 vinfos[3].jointtype = 1;
12047 vinfos[3].foffset = j3;
12048 vinfos[3].indices[0] = _ij3[0];
12049 vinfos[3].indices[1] = _ij3[1];
12050 vinfos[3].maxsolutions = _nj3;
12051 vinfos[4].jointtype = 1;
12052 vinfos[4].foffset = j4;
12053 vinfos[4].indices[0] = _ij4[0];
12054 vinfos[4].indices[1] = _ij4[1];
12055 vinfos[4].maxsolutions = _nj4;
12056 vinfos[5].jointtype = 1;
12057 vinfos[5].foffset = j5;
12058 vinfos[5].indices[0] = _ij5[0];
12059 vinfos[5].indices[1] = _ij5[1];
12060 vinfos[5].maxsolutions = _nj5;
12061 std::vector<int> vfree(0);
12062 solutions.AddSolution(vinfos,vfree);
12063 }
12064 }
12065 }
12066 
12067 }
12068 
12069 }
12070 
12071 }
12072 } while(0);
12073 if( bgotonextstatement )
12074 {
12075 bool bgotonextstatement = true;
12076 do
12077 {
12078 IkReal x917 = ((new_r10*new_r10)+(new_r00*new_r00));
12079 if(IKabs(x917)==0){
12080 continue;
12081 }
12082 IkReal x915=pow(x917,-0.5);
12083 IkReal x916=((1.0)*x915);
12084 CheckValue<IkReal> x918 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12085 if(!x918.valid){
12086 continue;
12087 }
12088 IkReal gconst15=((3.14159265358979)+(((-1.0)*(x918.value))));
12089 IkReal gconst16=(new_r10*x916);
12090 IkReal gconst17=(new_r00*x916);
12091 CheckValue<IkReal> x919 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12092 if(!x919.valid){
12093 continue;
12094 }
12095 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5+(x919.value))))), 6.28318530717959)));
12096 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12097 {
12098 bgotonextstatement=false;
12099 {
12100 IkReal j3eval[3];
12101 CheckValue<IkReal> x923 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12102 if(!x923.valid){
12103 continue;
12104 }
12105 IkReal x920=((1.0)*(x923.value));
12106 IkReal x921=x915;
12107 IkReal x922=((1.0)*x921);
12108 sj4=0;
12109 cj4=-1.0;
12110 j4=3.14159265358979;
12111 sj5=gconst16;
12112 cj5=gconst17;
12113 j5=((3.14159265)+(((-1.0)*x920)));
12114 IkReal gconst15=((3.14159265358979)+(((-1.0)*x920)));
12115 IkReal gconst16=(new_r10*x922);
12116 IkReal gconst17=(new_r00*x922);
12117 IkReal x924=new_r00*new_r00;
12118 IkReal x925=((1.0)*new_r00);
12119 IkReal x926=((((-1.0)*new_r01*x925))+(((-1.0)*new_r10*new_r11)));
12120 IkReal x927=x915;
12121 IkReal x928=(new_r00*x927);
12122 j3eval[0]=x926;
12123 j3eval[1]=((IKabs(((((-1.0)*new_r10*x925*x927))+((new_r01*x928)))))+(IKabs((((x924*x927))+((new_r11*x928))))));
12124 j3eval[2]=IKsign(x926);
12125 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12126 {
12127 {
12128 IkReal j3eval[1];
12129 CheckValue<IkReal> x932 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12130 if(!x932.valid){
12131 continue;
12132 }
12133 IkReal x929=((1.0)*(x932.value));
12134 IkReal x930=x915;
12135 IkReal x931=((1.0)*x930);
12136 sj4=0;
12137 cj4=-1.0;
12138 j4=3.14159265358979;
12139 sj5=gconst16;
12140 cj5=gconst17;
12141 j5=((3.14159265)+(((-1.0)*x929)));
12142 IkReal gconst15=((3.14159265358979)+(((-1.0)*x929)));
12143 IkReal gconst16=(new_r10*x931);
12144 IkReal gconst17=(new_r00*x931);
12145 IkReal x933=new_r10*new_r10;
12146 IkReal x934=new_r00*new_r00;
12147 IkReal x935=((1.0)*new_r01);
12148 CheckValue<IkReal> x939=IKPowWithIntegerCheck((x934+x933),-1);
12149 if(!x939.valid){
12150 continue;
12151 }
12152 IkReal x936=x939.value;
12153 IkReal x937=(new_r10*x936);
12154 IkReal x938=(new_r01*x936);
12155 j3eval[0]=((IKabs(((((-1.0)*x935*x937*(new_r10*new_r10)))+(((-1.0)*x934*x935*x937))+((x934*x936)))))+(IKabs((((x938*(new_r00*new_r00*new_r00)))+((new_r00*x933*x938))+((new_r00*x937))))));
12156 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12157 {
12158 {
12159 IkReal j3eval[1];
12160 CheckValue<IkReal> x943 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12161 if(!x943.valid){
12162 continue;
12163 }
12164 IkReal x940=((1.0)*(x943.value));
12165 IkReal x941=x915;
12166 IkReal x942=((1.0)*x941);
12167 sj4=0;
12168 cj4=-1.0;
12169 j4=3.14159265358979;
12170 sj5=gconst16;
12171 cj5=gconst17;
12172 j5=((3.14159265)+(((-1.0)*x940)));
12173 IkReal gconst15=((3.14159265358979)+(((-1.0)*x940)));
12174 IkReal gconst16=(new_r10*x942);
12175 IkReal gconst17=(new_r00*x942);
12176 IkReal x944=new_r00*new_r00;
12177 IkReal x945=new_r10*new_r10;
12178 CheckValue<IkReal> x949=IKPowWithIntegerCheck((x944+x945),-1);
12179 if(!x949.valid){
12180 continue;
12181 }
12182 IkReal x946=x949.value;
12183 IkReal x947=(new_r10*x946);
12184 IkReal x948=((1.0)*x946);
12185 j3eval[0]=((IKabs((((new_r00*x947))+((x947*(new_r00*new_r00*new_r00)))+((new_r00*x947*(new_r10*new_r10))))))+(IKabs(((((-1.0)*x944*x945*x948))+((x944*x946))+(((-1.0)*x948*(x945*x945)))))));
12186 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12187 {
12188 {
12189 IkReal evalcond[3];
12190 bool bgotonextstatement = true;
12191 do
12192 {
12193 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12194 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12195 {
12196 bgotonextstatement=false;
12197 {
12198 IkReal j3eval[1];
12199 CheckValue<IkReal> x951 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12200 if(!x951.valid){
12201 continue;
12202 }
12203 IkReal x950=((1.0)*(x951.value));
12204 sj4=0;
12205 cj4=-1.0;
12206 j4=3.14159265358979;
12207 sj5=gconst16;
12208 cj5=gconst17;
12209 j5=((3.14159265)+(((-1.0)*x950)));
12210 new_r11=0;
12211 new_r00=0;
12212 IkReal gconst15=((3.14159265358979)+(((-1.0)*x950)));
12213 IkReal x952 = new_r10*new_r10;
12214 if(IKabs(x952)==0){
12215 continue;
12216 }
12217 IkReal gconst16=((1.0)*new_r10*(pow(x952,-0.5)));
12218 IkReal gconst17=0;
12219 j3eval[0]=new_r10;
12220 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12221 {
12222 {
12223 IkReal j3array[2], cj3array[2], sj3array[2];
12224 bool j3valid[2]={false};
12225 _nj3 = 2;
12226 CheckValue<IkReal> x953=IKPowWithIntegerCheck(gconst16,-1);
12227 if(!x953.valid){
12228 continue;
12229 }
12230 cj3array[0]=(new_r01*(x953.value));
12231 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12232 {
12233  j3valid[0] = j3valid[1] = true;
12234  j3array[0] = IKacos(cj3array[0]);
12235  sj3array[0] = IKsin(j3array[0]);
12236  cj3array[1] = cj3array[0];
12237  j3array[1] = -j3array[0];
12238  sj3array[1] = -sj3array[0];
12239 }
12240 else if( isnan(cj3array[0]) )
12241 {
12242  // probably any value will work
12243  j3valid[0] = true;
12244  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12245 }
12246 for(int ij3 = 0; ij3 < 2; ++ij3)
12247 {
12248 if( !j3valid[ij3] )
12249 {
12250  continue;
12251 }
12252 _ij3[0] = ij3; _ij3[1] = -1;
12253 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12254 {
12255 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12256 {
12257  j3valid[iij3]=false; _ij3[1] = iij3; break;
12258 }
12259 }
12260 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12261 {
12262 IkReal evalcond[6];
12263 IkReal x954=IKsin(j3);
12264 IkReal x955=IKcos(j3);
12265 IkReal x956=((1.0)*gconst16);
12266 evalcond[0]=(new_r01*x954);
12267 evalcond[1]=(new_r10*x954);
12268 evalcond[2]=(gconst16*x954);
12269 evalcond[3]=(gconst16+(((-1.0)*new_r10*x955)));
12270 evalcond[4]=(new_r10+(((-1.0)*x955*x956)));
12271 evalcond[5]=(((new_r01*x955))+(((-1.0)*x956)));
12272 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12273 {
12274 continue;
12275 }
12276 }
12277 
12278 {
12279 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12280 vinfos[0].jointtype = 1;
12281 vinfos[0].foffset = j0;
12282 vinfos[0].indices[0] = _ij0[0];
12283 vinfos[0].indices[1] = _ij0[1];
12284 vinfos[0].maxsolutions = _nj0;
12285 vinfos[1].jointtype = 1;
12286 vinfos[1].foffset = j1;
12287 vinfos[1].indices[0] = _ij1[0];
12288 vinfos[1].indices[1] = _ij1[1];
12289 vinfos[1].maxsolutions = _nj1;
12290 vinfos[2].jointtype = 1;
12291 vinfos[2].foffset = j2;
12292 vinfos[2].indices[0] = _ij2[0];
12293 vinfos[2].indices[1] = _ij2[1];
12294 vinfos[2].maxsolutions = _nj2;
12295 vinfos[3].jointtype = 1;
12296 vinfos[3].foffset = j3;
12297 vinfos[3].indices[0] = _ij3[0];
12298 vinfos[3].indices[1] = _ij3[1];
12299 vinfos[3].maxsolutions = _nj3;
12300 vinfos[4].jointtype = 1;
12301 vinfos[4].foffset = j4;
12302 vinfos[4].indices[0] = _ij4[0];
12303 vinfos[4].indices[1] = _ij4[1];
12304 vinfos[4].maxsolutions = _nj4;
12305 vinfos[5].jointtype = 1;
12306 vinfos[5].foffset = j5;
12307 vinfos[5].indices[0] = _ij5[0];
12308 vinfos[5].indices[1] = _ij5[1];
12309 vinfos[5].maxsolutions = _nj5;
12310 std::vector<int> vfree(0);
12311 solutions.AddSolution(vinfos,vfree);
12312 }
12313 }
12314 }
12315 
12316 } else
12317 {
12318 {
12319 IkReal j3array[2], cj3array[2], sj3array[2];
12320 bool j3valid[2]={false};
12321 _nj3 = 2;
12322 CheckValue<IkReal> x957=IKPowWithIntegerCheck(new_r10,-1);
12323 if(!x957.valid){
12324 continue;
12325 }
12326 cj3array[0]=(gconst16*(x957.value));
12327 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12328 {
12329  j3valid[0] = j3valid[1] = true;
12330  j3array[0] = IKacos(cj3array[0]);
12331  sj3array[0] = IKsin(j3array[0]);
12332  cj3array[1] = cj3array[0];
12333  j3array[1] = -j3array[0];
12334  sj3array[1] = -sj3array[0];
12335 }
12336 else if( isnan(cj3array[0]) )
12337 {
12338  // probably any value will work
12339  j3valid[0] = true;
12340  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12341 }
12342 for(int ij3 = 0; ij3 < 2; ++ij3)
12343 {
12344 if( !j3valid[ij3] )
12345 {
12346  continue;
12347 }
12348 _ij3[0] = ij3; _ij3[1] = -1;
12349 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12350 {
12351 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12352 {
12353  j3valid[iij3]=false; _ij3[1] = iij3; break;
12354 }
12355 }
12356 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12357 {
12358 IkReal evalcond[6];
12359 IkReal x958=IKsin(j3);
12360 IkReal x959=IKcos(j3);
12361 IkReal x960=((1.0)*gconst16);
12362 IkReal x961=(x959*x960);
12363 evalcond[0]=(new_r01*x958);
12364 evalcond[1]=(new_r10*x958);
12365 evalcond[2]=(gconst16*x958);
12366 evalcond[3]=(new_r01+(((-1.0)*x961)));
12367 evalcond[4]=(new_r10+(((-1.0)*x961)));
12368 evalcond[5]=(((new_r01*x959))+(((-1.0)*x960)));
12369 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12370 {
12371 continue;
12372 }
12373 }
12374 
12375 {
12376 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12377 vinfos[0].jointtype = 1;
12378 vinfos[0].foffset = j0;
12379 vinfos[0].indices[0] = _ij0[0];
12380 vinfos[0].indices[1] = _ij0[1];
12381 vinfos[0].maxsolutions = _nj0;
12382 vinfos[1].jointtype = 1;
12383 vinfos[1].foffset = j1;
12384 vinfos[1].indices[0] = _ij1[0];
12385 vinfos[1].indices[1] = _ij1[1];
12386 vinfos[1].maxsolutions = _nj1;
12387 vinfos[2].jointtype = 1;
12388 vinfos[2].foffset = j2;
12389 vinfos[2].indices[0] = _ij2[0];
12390 vinfos[2].indices[1] = _ij2[1];
12391 vinfos[2].maxsolutions = _nj2;
12392 vinfos[3].jointtype = 1;
12393 vinfos[3].foffset = j3;
12394 vinfos[3].indices[0] = _ij3[0];
12395 vinfos[3].indices[1] = _ij3[1];
12396 vinfos[3].maxsolutions = _nj3;
12397 vinfos[4].jointtype = 1;
12398 vinfos[4].foffset = j4;
12399 vinfos[4].indices[0] = _ij4[0];
12400 vinfos[4].indices[1] = _ij4[1];
12401 vinfos[4].maxsolutions = _nj4;
12402 vinfos[5].jointtype = 1;
12403 vinfos[5].foffset = j5;
12404 vinfos[5].indices[0] = _ij5[0];
12405 vinfos[5].indices[1] = _ij5[1];
12406 vinfos[5].maxsolutions = _nj5;
12407 std::vector<int> vfree(0);
12408 solutions.AddSolution(vinfos,vfree);
12409 }
12410 }
12411 }
12412 
12413 }
12414 
12415 }
12416 
12417 }
12418 } while(0);
12419 if( bgotonextstatement )
12420 {
12421 bool bgotonextstatement = true;
12422 do
12423 {
12424 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
12425 evalcond[1]=gconst17;
12426 evalcond[2]=gconst16;
12427 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
12428 {
12429 bgotonextstatement=false;
12430 {
12431 IkReal j3eval[3];
12432 CheckValue<IkReal> x963 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12433 if(!x963.valid){
12434 continue;
12435 }
12436 IkReal x962=((1.0)*(x963.value));
12437 sj4=0;
12438 cj4=-1.0;
12439 j4=3.14159265358979;
12440 sj5=gconst16;
12441 cj5=gconst17;
12442 j5=((3.14159265)+(((-1.0)*x962)));
12443 new_r11=0;
12444 new_r01=0;
12445 new_r22=0;
12446 new_r20=0;
12447 IkReal gconst15=((3.14159265358979)+(((-1.0)*x962)));
12448 IkReal gconst16=((1.0)*new_r10);
12449 IkReal gconst17=((1.0)*new_r00);
12450 j3eval[0]=-1.0;
12451 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12452 j3eval[2]=-1.0;
12453 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12454 {
12455 {
12456 IkReal j3eval[3];
12457 CheckValue<IkReal> x965 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12458 if(!x965.valid){
12459 continue;
12460 }
12461 IkReal x964=((1.0)*(x965.value));
12462 sj4=0;
12463 cj4=-1.0;
12464 j4=3.14159265358979;
12465 sj5=gconst16;
12466 cj5=gconst17;
12467 j5=((3.14159265)+(((-1.0)*x964)));
12468 new_r11=0;
12469 new_r01=0;
12470 new_r22=0;
12471 new_r20=0;
12472 IkReal gconst15=((3.14159265358979)+(((-1.0)*x964)));
12473 IkReal gconst16=((1.0)*new_r10);
12474 IkReal gconst17=((1.0)*new_r00);
12475 j3eval[0]=-1.0;
12476 j3eval[1]=-1.0;
12477 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12478 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12479 {
12480 {
12481 IkReal j3eval[3];
12482 CheckValue<IkReal> x967 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12483 if(!x967.valid){
12484 continue;
12485 }
12486 IkReal x966=((1.0)*(x967.value));
12487 sj4=0;
12488 cj4=-1.0;
12489 j4=3.14159265358979;
12490 sj5=gconst16;
12491 cj5=gconst17;
12492 j5=((3.14159265)+(((-1.0)*x966)));
12493 new_r11=0;
12494 new_r01=0;
12495 new_r22=0;
12496 new_r20=0;
12497 IkReal gconst15=((3.14159265358979)+(((-1.0)*x966)));
12498 IkReal gconst16=((1.0)*new_r10);
12499 IkReal gconst17=((1.0)*new_r00);
12500 j3eval[0]=-1.0;
12501 j3eval[1]=((IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
12502 j3eval[2]=-1.0;
12503 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12504 {
12505 continue; // 3 cases reached
12506 
12507 } else
12508 {
12509 {
12510 IkReal j3array[1], cj3array[1], sj3array[1];
12511 bool j3valid[1]={false};
12512 _nj3 = 1;
12513 CheckValue<IkReal> x968 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst16*gconst17)))),IkReal(((gconst17*gconst17)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
12514 if(!x968.valid){
12515 continue;
12516 }
12517 CheckValue<IkReal> x969=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
12518 if(!x969.valid){
12519 continue;
12520 }
12521 j3array[0]=((-1.5707963267949)+(x968.value)+(((1.5707963267949)*(x969.value))));
12522 sj3array[0]=IKsin(j3array[0]);
12523 cj3array[0]=IKcos(j3array[0]);
12524 if( j3array[0] > IKPI )
12525 {
12526  j3array[0]-=IK2PI;
12527 }
12528 else if( j3array[0] < -IKPI )
12529 { j3array[0]+=IK2PI;
12530 }
12531 j3valid[0] = true;
12532 for(int ij3 = 0; ij3 < 1; ++ij3)
12533 {
12534 if( !j3valid[ij3] )
12535 {
12536  continue;
12537 }
12538 _ij3[0] = ij3; _ij3[1] = -1;
12539 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12540 {
12541 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12542 {
12543  j3valid[iij3]=false; _ij3[1] = iij3; break;
12544 }
12545 }
12546 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12547 {
12548 IkReal evalcond[6];
12549 IkReal x970=IKsin(j3);
12550 IkReal x971=IKcos(j3);
12551 IkReal x972=(gconst17*x970);
12552 IkReal x973=(gconst16*x970);
12553 IkReal x974=(gconst17*x971);
12554 IkReal x975=((1.0)*x971);
12555 IkReal x976=(gconst16*x975);
12556 evalcond[0]=(x972+(((-1.0)*x976)));
12557 evalcond[1]=(gconst17+((new_r10*x970))+((new_r00*x971)));
12558 evalcond[2]=(new_r00+x973+x974);
12559 evalcond[3]=(gconst16+(((-1.0)*new_r10*x975))+((new_r00*x970)));
12560 evalcond[4]=((((-1.0)*x974))+(((-1.0)*x973)));
12561 evalcond[5]=(new_r10+x972+(((-1.0)*x976)));
12562 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12563 {
12564 continue;
12565 }
12566 }
12567 
12568 {
12569 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12570 vinfos[0].jointtype = 1;
12571 vinfos[0].foffset = j0;
12572 vinfos[0].indices[0] = _ij0[0];
12573 vinfos[0].indices[1] = _ij0[1];
12574 vinfos[0].maxsolutions = _nj0;
12575 vinfos[1].jointtype = 1;
12576 vinfos[1].foffset = j1;
12577 vinfos[1].indices[0] = _ij1[0];
12578 vinfos[1].indices[1] = _ij1[1];
12579 vinfos[1].maxsolutions = _nj1;
12580 vinfos[2].jointtype = 1;
12581 vinfos[2].foffset = j2;
12582 vinfos[2].indices[0] = _ij2[0];
12583 vinfos[2].indices[1] = _ij2[1];
12584 vinfos[2].maxsolutions = _nj2;
12585 vinfos[3].jointtype = 1;
12586 vinfos[3].foffset = j3;
12587 vinfos[3].indices[0] = _ij3[0];
12588 vinfos[3].indices[1] = _ij3[1];
12589 vinfos[3].maxsolutions = _nj3;
12590 vinfos[4].jointtype = 1;
12591 vinfos[4].foffset = j4;
12592 vinfos[4].indices[0] = _ij4[0];
12593 vinfos[4].indices[1] = _ij4[1];
12594 vinfos[4].maxsolutions = _nj4;
12595 vinfos[5].jointtype = 1;
12596 vinfos[5].foffset = j5;
12597 vinfos[5].indices[0] = _ij5[0];
12598 vinfos[5].indices[1] = _ij5[1];
12599 vinfos[5].maxsolutions = _nj5;
12600 std::vector<int> vfree(0);
12601 solutions.AddSolution(vinfos,vfree);
12602 }
12603 }
12604 }
12605 
12606 }
12607 
12608 }
12609 
12610 } else
12611 {
12612 {
12613 IkReal j3array[1], cj3array[1], sj3array[1];
12614 bool j3valid[1]={false};
12615 _nj3 = 1;
12616 CheckValue<IkReal> x977=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst17*gconst17)))+(((-1.0)*(gconst16*gconst16))))),-1);
12617 if(!x977.valid){
12618 continue;
12619 }
12620 CheckValue<IkReal> x978 = IKatan2WithCheck(IkReal((gconst16*new_r00)),IkReal((gconst17*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12621 if(!x978.valid){
12622 continue;
12623 }
12624 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x977.value)))+(x978.value));
12625 sj3array[0]=IKsin(j3array[0]);
12626 cj3array[0]=IKcos(j3array[0]);
12627 if( j3array[0] > IKPI )
12628 {
12629  j3array[0]-=IK2PI;
12630 }
12631 else if( j3array[0] < -IKPI )
12632 { j3array[0]+=IK2PI;
12633 }
12634 j3valid[0] = true;
12635 for(int ij3 = 0; ij3 < 1; ++ij3)
12636 {
12637 if( !j3valid[ij3] )
12638 {
12639  continue;
12640 }
12641 _ij3[0] = ij3; _ij3[1] = -1;
12642 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12643 {
12644 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12645 {
12646  j3valid[iij3]=false; _ij3[1] = iij3; break;
12647 }
12648 }
12649 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12650 {
12651 IkReal evalcond[6];
12652 IkReal x979=IKsin(j3);
12653 IkReal x980=IKcos(j3);
12654 IkReal x981=(gconst17*x979);
12655 IkReal x982=(gconst16*x979);
12656 IkReal x983=(gconst17*x980);
12657 IkReal x984=((1.0)*x980);
12658 IkReal x985=(gconst16*x984);
12659 evalcond[0]=((((-1.0)*x985))+x981);
12660 evalcond[1]=(((new_r00*x980))+gconst17+((new_r10*x979)));
12661 evalcond[2]=(new_r00+x982+x983);
12662 evalcond[3]=(gconst16+(((-1.0)*new_r10*x984))+((new_r00*x979)));
12663 evalcond[4]=((((-1.0)*x983))+(((-1.0)*x982)));
12664 evalcond[5]=((((-1.0)*x985))+new_r10+x981);
12665 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12666 {
12667 continue;
12668 }
12669 }
12670 
12671 {
12672 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12673 vinfos[0].jointtype = 1;
12674 vinfos[0].foffset = j0;
12675 vinfos[0].indices[0] = _ij0[0];
12676 vinfos[0].indices[1] = _ij0[1];
12677 vinfos[0].maxsolutions = _nj0;
12678 vinfos[1].jointtype = 1;
12679 vinfos[1].foffset = j1;
12680 vinfos[1].indices[0] = _ij1[0];
12681 vinfos[1].indices[1] = _ij1[1];
12682 vinfos[1].maxsolutions = _nj1;
12683 vinfos[2].jointtype = 1;
12684 vinfos[2].foffset = j2;
12685 vinfos[2].indices[0] = _ij2[0];
12686 vinfos[2].indices[1] = _ij2[1];
12687 vinfos[2].maxsolutions = _nj2;
12688 vinfos[3].jointtype = 1;
12689 vinfos[3].foffset = j3;
12690 vinfos[3].indices[0] = _ij3[0];
12691 vinfos[3].indices[1] = _ij3[1];
12692 vinfos[3].maxsolutions = _nj3;
12693 vinfos[4].jointtype = 1;
12694 vinfos[4].foffset = j4;
12695 vinfos[4].indices[0] = _ij4[0];
12696 vinfos[4].indices[1] = _ij4[1];
12697 vinfos[4].maxsolutions = _nj4;
12698 vinfos[5].jointtype = 1;
12699 vinfos[5].foffset = j5;
12700 vinfos[5].indices[0] = _ij5[0];
12701 vinfos[5].indices[1] = _ij5[1];
12702 vinfos[5].maxsolutions = _nj5;
12703 std::vector<int> vfree(0);
12704 solutions.AddSolution(vinfos,vfree);
12705 }
12706 }
12707 }
12708 
12709 }
12710 
12711 }
12712 
12713 } else
12714 {
12715 {
12716 IkReal j3array[1], cj3array[1], sj3array[1];
12717 bool j3valid[1]={false};
12718 _nj3 = 1;
12719 CheckValue<IkReal> x986 = IKatan2WithCheck(IkReal((gconst16*gconst17)),IkReal(gconst17*gconst17),IKFAST_ATAN2_MAGTHRESH);
12720 if(!x986.valid){
12721 continue;
12722 }
12723 CheckValue<IkReal> x987=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
12724 if(!x987.valid){
12725 continue;
12726 }
12727 j3array[0]=((-1.5707963267949)+(x986.value)+(((1.5707963267949)*(x987.value))));
12728 sj3array[0]=IKsin(j3array[0]);
12729 cj3array[0]=IKcos(j3array[0]);
12730 if( j3array[0] > IKPI )
12731 {
12732  j3array[0]-=IK2PI;
12733 }
12734 else if( j3array[0] < -IKPI )
12735 { j3array[0]+=IK2PI;
12736 }
12737 j3valid[0] = true;
12738 for(int ij3 = 0; ij3 < 1; ++ij3)
12739 {
12740 if( !j3valid[ij3] )
12741 {
12742  continue;
12743 }
12744 _ij3[0] = ij3; _ij3[1] = -1;
12745 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12746 {
12747 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12748 {
12749  j3valid[iij3]=false; _ij3[1] = iij3; break;
12750 }
12751 }
12752 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12753 {
12754 IkReal evalcond[6];
12755 IkReal x988=IKsin(j3);
12756 IkReal x989=IKcos(j3);
12757 IkReal x990=(gconst17*x988);
12758 IkReal x991=(gconst16*x988);
12759 IkReal x992=(gconst17*x989);
12760 IkReal x993=((1.0)*x989);
12761 IkReal x994=(gconst16*x993);
12762 evalcond[0]=((((-1.0)*x994))+x990);
12763 evalcond[1]=(((new_r00*x989))+gconst17+((new_r10*x988)));
12764 evalcond[2]=(new_r00+x992+x991);
12765 evalcond[3]=(((new_r00*x988))+(((-1.0)*new_r10*x993))+gconst16);
12766 evalcond[4]=((((-1.0)*x992))+(((-1.0)*x991)));
12767 evalcond[5]=((((-1.0)*x994))+new_r10+x990);
12768 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12769 {
12770 continue;
12771 }
12772 }
12773 
12774 {
12775 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12776 vinfos[0].jointtype = 1;
12777 vinfos[0].foffset = j0;
12778 vinfos[0].indices[0] = _ij0[0];
12779 vinfos[0].indices[1] = _ij0[1];
12780 vinfos[0].maxsolutions = _nj0;
12781 vinfos[1].jointtype = 1;
12782 vinfos[1].foffset = j1;
12783 vinfos[1].indices[0] = _ij1[0];
12784 vinfos[1].indices[1] = _ij1[1];
12785 vinfos[1].maxsolutions = _nj1;
12786 vinfos[2].jointtype = 1;
12787 vinfos[2].foffset = j2;
12788 vinfos[2].indices[0] = _ij2[0];
12789 vinfos[2].indices[1] = _ij2[1];
12790 vinfos[2].maxsolutions = _nj2;
12791 vinfos[3].jointtype = 1;
12792 vinfos[3].foffset = j3;
12793 vinfos[3].indices[0] = _ij3[0];
12794 vinfos[3].indices[1] = _ij3[1];
12795 vinfos[3].maxsolutions = _nj3;
12796 vinfos[4].jointtype = 1;
12797 vinfos[4].foffset = j4;
12798 vinfos[4].indices[0] = _ij4[0];
12799 vinfos[4].indices[1] = _ij4[1];
12800 vinfos[4].maxsolutions = _nj4;
12801 vinfos[5].jointtype = 1;
12802 vinfos[5].foffset = j5;
12803 vinfos[5].indices[0] = _ij5[0];
12804 vinfos[5].indices[1] = _ij5[1];
12805 vinfos[5].maxsolutions = _nj5;
12806 std::vector<int> vfree(0);
12807 solutions.AddSolution(vinfos,vfree);
12808 }
12809 }
12810 }
12811 
12812 }
12813 
12814 }
12815 
12816 }
12817 } while(0);
12818 if( bgotonextstatement )
12819 {
12820 bool bgotonextstatement = true;
12821 do
12822 {
12823 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12824 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12825 {
12826 bgotonextstatement=false;
12827 {
12828 IkReal j3eval[1];
12829 CheckValue<IkReal> x996 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12830 if(!x996.valid){
12831 continue;
12832 }
12833 IkReal x995=((1.0)*(x996.value));
12834 sj4=0;
12835 cj4=-1.0;
12836 j4=3.14159265358979;
12837 sj5=gconst16;
12838 cj5=gconst17;
12839 j5=((3.14159265)+(((-1.0)*x995)));
12840 new_r01=0;
12841 new_r10=0;
12842 IkReal gconst15=((3.14159265358979)+(((-1.0)*x995)));
12843 IkReal gconst16=0;
12844 IkReal x997 = new_r00*new_r00;
12845 if(IKabs(x997)==0){
12846 continue;
12847 }
12848 IkReal gconst17=((1.0)*new_r00*(pow(x997,-0.5)));
12849 j3eval[0]=new_r11;
12850 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12851 {
12852 {
12853 IkReal j3array[2], cj3array[2], sj3array[2];
12854 bool j3valid[2]={false};
12855 _nj3 = 2;
12856 CheckValue<IkReal> x998=IKPowWithIntegerCheck(gconst17,-1);
12857 if(!x998.valid){
12858 continue;
12859 }
12860 cj3array[0]=(new_r11*(x998.value));
12861 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12862 {
12863  j3valid[0] = j3valid[1] = true;
12864  j3array[0] = IKacos(cj3array[0]);
12865  sj3array[0] = IKsin(j3array[0]);
12866  cj3array[1] = cj3array[0];
12867  j3array[1] = -j3array[0];
12868  sj3array[1] = -sj3array[0];
12869 }
12870 else if( isnan(cj3array[0]) )
12871 {
12872  // probably any value will work
12873  j3valid[0] = true;
12874  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12875 }
12876 for(int ij3 = 0; ij3 < 2; ++ij3)
12877 {
12878 if( !j3valid[ij3] )
12879 {
12880  continue;
12881 }
12882 _ij3[0] = ij3; _ij3[1] = -1;
12883 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12884 {
12885 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12886 {
12887  j3valid[iij3]=false; _ij3[1] = iij3; break;
12888 }
12889 }
12890 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12891 {
12892 IkReal evalcond[6];
12893 IkReal x999=IKsin(j3);
12894 IkReal x1000=IKcos(j3);
12895 evalcond[0]=(new_r00*x999);
12896 evalcond[1]=(new_r11*x999);
12897 evalcond[2]=(gconst17*x999);
12898 evalcond[3]=(gconst17+((new_r00*x1000)));
12899 evalcond[4]=(((gconst17*x1000))+new_r00);
12900 evalcond[5]=((((-1.0)*new_r11*x1000))+gconst17);
12901 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12902 {
12903 continue;
12904 }
12905 }
12906 
12907 {
12908 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12909 vinfos[0].jointtype = 1;
12910 vinfos[0].foffset = j0;
12911 vinfos[0].indices[0] = _ij0[0];
12912 vinfos[0].indices[1] = _ij0[1];
12913 vinfos[0].maxsolutions = _nj0;
12914 vinfos[1].jointtype = 1;
12915 vinfos[1].foffset = j1;
12916 vinfos[1].indices[0] = _ij1[0];
12917 vinfos[1].indices[1] = _ij1[1];
12918 vinfos[1].maxsolutions = _nj1;
12919 vinfos[2].jointtype = 1;
12920 vinfos[2].foffset = j2;
12921 vinfos[2].indices[0] = _ij2[0];
12922 vinfos[2].indices[1] = _ij2[1];
12923 vinfos[2].maxsolutions = _nj2;
12924 vinfos[3].jointtype = 1;
12925 vinfos[3].foffset = j3;
12926 vinfos[3].indices[0] = _ij3[0];
12927 vinfos[3].indices[1] = _ij3[1];
12928 vinfos[3].maxsolutions = _nj3;
12929 vinfos[4].jointtype = 1;
12930 vinfos[4].foffset = j4;
12931 vinfos[4].indices[0] = _ij4[0];
12932 vinfos[4].indices[1] = _ij4[1];
12933 vinfos[4].maxsolutions = _nj4;
12934 vinfos[5].jointtype = 1;
12935 vinfos[5].foffset = j5;
12936 vinfos[5].indices[0] = _ij5[0];
12937 vinfos[5].indices[1] = _ij5[1];
12938 vinfos[5].maxsolutions = _nj5;
12939 std::vector<int> vfree(0);
12940 solutions.AddSolution(vinfos,vfree);
12941 }
12942 }
12943 }
12944 
12945 } else
12946 {
12947 {
12948 IkReal j3array[2], cj3array[2], sj3array[2];
12949 bool j3valid[2]={false};
12950 _nj3 = 2;
12951 CheckValue<IkReal> x1001=IKPowWithIntegerCheck(new_r11,-1);
12952 if(!x1001.valid){
12953 continue;
12954 }
12955 cj3array[0]=(gconst17*(x1001.value));
12956 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12957 {
12958  j3valid[0] = j3valid[1] = true;
12959  j3array[0] = IKacos(cj3array[0]);
12960  sj3array[0] = IKsin(j3array[0]);
12961  cj3array[1] = cj3array[0];
12962  j3array[1] = -j3array[0];
12963  sj3array[1] = -sj3array[0];
12964 }
12965 else if( isnan(cj3array[0]) )
12966 {
12967  // probably any value will work
12968  j3valid[0] = true;
12969  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12970 }
12971 for(int ij3 = 0; ij3 < 2; ++ij3)
12972 {
12973 if( !j3valid[ij3] )
12974 {
12975  continue;
12976 }
12977 _ij3[0] = ij3; _ij3[1] = -1;
12978 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12979 {
12980 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12981 {
12982  j3valid[iij3]=false; _ij3[1] = iij3; break;
12983 }
12984 }
12985 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12986 {
12987 IkReal evalcond[6];
12988 IkReal x1002=IKsin(j3);
12989 IkReal x1003=IKcos(j3);
12990 IkReal x1004=(gconst17*x1003);
12991 evalcond[0]=(new_r00*x1002);
12992 evalcond[1]=(new_r11*x1002);
12993 evalcond[2]=(gconst17*x1002);
12994 evalcond[3]=(gconst17+((new_r00*x1003)));
12995 evalcond[4]=(x1004+new_r00);
12996 evalcond[5]=((((-1.0)*x1004))+new_r11);
12997 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12998 {
12999 continue;
13000 }
13001 }
13002 
13003 {
13004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13005 vinfos[0].jointtype = 1;
13006 vinfos[0].foffset = j0;
13007 vinfos[0].indices[0] = _ij0[0];
13008 vinfos[0].indices[1] = _ij0[1];
13009 vinfos[0].maxsolutions = _nj0;
13010 vinfos[1].jointtype = 1;
13011 vinfos[1].foffset = j1;
13012 vinfos[1].indices[0] = _ij1[0];
13013 vinfos[1].indices[1] = _ij1[1];
13014 vinfos[1].maxsolutions = _nj1;
13015 vinfos[2].jointtype = 1;
13016 vinfos[2].foffset = j2;
13017 vinfos[2].indices[0] = _ij2[0];
13018 vinfos[2].indices[1] = _ij2[1];
13019 vinfos[2].maxsolutions = _nj2;
13020 vinfos[3].jointtype = 1;
13021 vinfos[3].foffset = j3;
13022 vinfos[3].indices[0] = _ij3[0];
13023 vinfos[3].indices[1] = _ij3[1];
13024 vinfos[3].maxsolutions = _nj3;
13025 vinfos[4].jointtype = 1;
13026 vinfos[4].foffset = j4;
13027 vinfos[4].indices[0] = _ij4[0];
13028 vinfos[4].indices[1] = _ij4[1];
13029 vinfos[4].maxsolutions = _nj4;
13030 vinfos[5].jointtype = 1;
13031 vinfos[5].foffset = j5;
13032 vinfos[5].indices[0] = _ij5[0];
13033 vinfos[5].indices[1] = _ij5[1];
13034 vinfos[5].maxsolutions = _nj5;
13035 std::vector<int> vfree(0);
13036 solutions.AddSolution(vinfos,vfree);
13037 }
13038 }
13039 }
13040 
13041 }
13042 
13043 }
13044 
13045 }
13046 } while(0);
13047 if( bgotonextstatement )
13048 {
13049 bool bgotonextstatement = true;
13050 do
13051 {
13052 evalcond[0]=IKabs(new_r00);
13053 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13054 {
13055 bgotonextstatement=false;
13056 {
13057 IkReal j3eval[1];
13058 CheckValue<IkReal> x1006 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13059 if(!x1006.valid){
13060 continue;
13061 }
13062 IkReal x1005=((1.0)*(x1006.value));
13063 sj4=0;
13064 cj4=-1.0;
13065 j4=3.14159265358979;
13066 sj5=gconst16;
13067 cj5=gconst17;
13068 j5=((3.14159265)+(((-1.0)*x1005)));
13069 new_r00=0;
13070 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1005)));
13071 IkReal x1007 = new_r10*new_r10;
13072 if(IKabs(x1007)==0){
13073 continue;
13074 }
13075 IkReal gconst16=((1.0)*new_r10*(pow(x1007,-0.5)));
13076 IkReal gconst17=0;
13077 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13078 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13079 {
13080 {
13081 IkReal j3eval[1];
13082 CheckValue<IkReal> x1009 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13083 if(!x1009.valid){
13084 continue;
13085 }
13086 IkReal x1008=((1.0)*(x1009.value));
13087 sj4=0;
13088 cj4=-1.0;
13089 j4=3.14159265358979;
13090 sj5=gconst16;
13091 cj5=gconst17;
13092 j5=((3.14159265)+(((-1.0)*x1008)));
13093 new_r00=0;
13094 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1008)));
13095 IkReal x1010 = new_r10*new_r10;
13096 if(IKabs(x1010)==0){
13097 continue;
13098 }
13099 IkReal gconst16=((1.0)*new_r10*(pow(x1010,-0.5)));
13100 IkReal gconst17=0;
13101 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13102 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13103 {
13104 {
13105 IkReal j3eval[1];
13106 CheckValue<IkReal> x1012 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13107 if(!x1012.valid){
13108 continue;
13109 }
13110 IkReal x1011=((1.0)*(x1012.value));
13111 sj4=0;
13112 cj4=-1.0;
13113 j4=3.14159265358979;
13114 sj5=gconst16;
13115 cj5=gconst17;
13116 j5=((3.14159265)+(((-1.0)*x1011)));
13117 new_r00=0;
13118 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1011)));
13119 IkReal x1013 = new_r10*new_r10;
13120 if(IKabs(x1013)==0){
13121 continue;
13122 }
13123 IkReal gconst16=((1.0)*new_r10*(pow(x1013,-0.5)));
13124 IkReal gconst17=0;
13125 j3eval[0]=new_r10;
13126 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13127 {
13128 continue; // 3 cases reached
13129 
13130 } else
13131 {
13132 {
13133 IkReal j3array[1], cj3array[1], sj3array[1];
13134 bool j3valid[1]={false};
13135 _nj3 = 1;
13136 CheckValue<IkReal> x1014=IKPowWithIntegerCheck(gconst16,-1);
13137 if(!x1014.valid){
13138 continue;
13139 }
13140 CheckValue<IkReal> x1015=IKPowWithIntegerCheck(new_r10,-1);
13141 if(!x1015.valid){
13142 continue;
13143 }
13144 if( IKabs((new_r11*(x1014.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst16*(x1015.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x1014.value)))+IKsqr((gconst16*(x1015.value)))-1) <= IKFAST_SINCOS_THRESH )
13145  continue;
13146 j3array[0]=IKatan2((new_r11*(x1014.value)), (gconst16*(x1015.value)));
13147 sj3array[0]=IKsin(j3array[0]);
13148 cj3array[0]=IKcos(j3array[0]);
13149 if( j3array[0] > IKPI )
13150 {
13151  j3array[0]-=IK2PI;
13152 }
13153 else if( j3array[0] < -IKPI )
13154 { j3array[0]+=IK2PI;
13155 }
13156 j3valid[0] = true;
13157 for(int ij3 = 0; ij3 < 1; ++ij3)
13158 {
13159 if( !j3valid[ij3] )
13160 {
13161  continue;
13162 }
13163 _ij3[0] = ij3; _ij3[1] = -1;
13164 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13165 {
13166 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13167 {
13168  j3valid[iij3]=false; _ij3[1] = iij3; break;
13169 }
13170 }
13171 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13172 {
13173 IkReal evalcond[8];
13174 IkReal x1016=IKsin(j3);
13175 IkReal x1017=IKcos(j3);
13176 IkReal x1018=((1.0)*gconst16);
13177 IkReal x1019=((1.0)*x1017);
13178 IkReal x1020=(x1017*x1018);
13179 evalcond[0]=(new_r10*x1016);
13180 evalcond[1]=(gconst16*x1016);
13181 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1019)));
13182 evalcond[3]=((((-1.0)*x1020))+new_r01);
13183 evalcond[4]=((((-1.0)*x1016*x1018))+new_r11);
13184 evalcond[5]=((((-1.0)*x1020))+new_r10);
13185 evalcond[6]=((((-1.0)*new_r11*x1019))+((new_r01*x1016)));
13186 evalcond[7]=(((new_r11*x1016))+(((-1.0)*x1018))+((new_r01*x1017)));
13187 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13188 {
13189 continue;
13190 }
13191 }
13192 
13193 {
13194 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13195 vinfos[0].jointtype = 1;
13196 vinfos[0].foffset = j0;
13197 vinfos[0].indices[0] = _ij0[0];
13198 vinfos[0].indices[1] = _ij0[1];
13199 vinfos[0].maxsolutions = _nj0;
13200 vinfos[1].jointtype = 1;
13201 vinfos[1].foffset = j1;
13202 vinfos[1].indices[0] = _ij1[0];
13203 vinfos[1].indices[1] = _ij1[1];
13204 vinfos[1].maxsolutions = _nj1;
13205 vinfos[2].jointtype = 1;
13206 vinfos[2].foffset = j2;
13207 vinfos[2].indices[0] = _ij2[0];
13208 vinfos[2].indices[1] = _ij2[1];
13209 vinfos[2].maxsolutions = _nj2;
13210 vinfos[3].jointtype = 1;
13211 vinfos[3].foffset = j3;
13212 vinfos[3].indices[0] = _ij3[0];
13213 vinfos[3].indices[1] = _ij3[1];
13214 vinfos[3].maxsolutions = _nj3;
13215 vinfos[4].jointtype = 1;
13216 vinfos[4].foffset = j4;
13217 vinfos[4].indices[0] = _ij4[0];
13218 vinfos[4].indices[1] = _ij4[1];
13219 vinfos[4].maxsolutions = _nj4;
13220 vinfos[5].jointtype = 1;
13221 vinfos[5].foffset = j5;
13222 vinfos[5].indices[0] = _ij5[0];
13223 vinfos[5].indices[1] = _ij5[1];
13224 vinfos[5].maxsolutions = _nj5;
13225 std::vector<int> vfree(0);
13226 solutions.AddSolution(vinfos,vfree);
13227 }
13228 }
13229 }
13230 
13231 }
13232 
13233 }
13234 
13235 } else
13236 {
13237 {
13238 IkReal j3array[1], cj3array[1], sj3array[1];
13239 bool j3valid[1]={false};
13240 _nj3 = 1;
13241 CheckValue<IkReal> x1021=IKPowWithIntegerCheck(IKsign(gconst16),-1);
13242 if(!x1021.valid){
13243 continue;
13244 }
13245 CheckValue<IkReal> x1022 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13246 if(!x1022.valid){
13247 continue;
13248 }
13249 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1021.value)))+(x1022.value));
13250 sj3array[0]=IKsin(j3array[0]);
13251 cj3array[0]=IKcos(j3array[0]);
13252 if( j3array[0] > IKPI )
13253 {
13254  j3array[0]-=IK2PI;
13255 }
13256 else if( j3array[0] < -IKPI )
13257 { j3array[0]+=IK2PI;
13258 }
13259 j3valid[0] = true;
13260 for(int ij3 = 0; ij3 < 1; ++ij3)
13261 {
13262 if( !j3valid[ij3] )
13263 {
13264  continue;
13265 }
13266 _ij3[0] = ij3; _ij3[1] = -1;
13267 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13268 {
13269 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13270 {
13271  j3valid[iij3]=false; _ij3[1] = iij3; break;
13272 }
13273 }
13274 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13275 {
13276 IkReal evalcond[8];
13277 IkReal x1023=IKsin(j3);
13278 IkReal x1024=IKcos(j3);
13279 IkReal x1025=((1.0)*gconst16);
13280 IkReal x1026=((1.0)*x1024);
13281 IkReal x1027=(x1024*x1025);
13282 evalcond[0]=(new_r10*x1023);
13283 evalcond[1]=(gconst16*x1023);
13284 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1026)));
13285 evalcond[3]=((((-1.0)*x1027))+new_r01);
13286 evalcond[4]=((((-1.0)*x1023*x1025))+new_r11);
13287 evalcond[5]=((((-1.0)*x1027))+new_r10);
13288 evalcond[6]=((((-1.0)*new_r11*x1026))+((new_r01*x1023)));
13289 evalcond[7]=(((new_r11*x1023))+(((-1.0)*x1025))+((new_r01*x1024)));
13290 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13291 {
13292 continue;
13293 }
13294 }
13295 
13296 {
13297 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13298 vinfos[0].jointtype = 1;
13299 vinfos[0].foffset = j0;
13300 vinfos[0].indices[0] = _ij0[0];
13301 vinfos[0].indices[1] = _ij0[1];
13302 vinfos[0].maxsolutions = _nj0;
13303 vinfos[1].jointtype = 1;
13304 vinfos[1].foffset = j1;
13305 vinfos[1].indices[0] = _ij1[0];
13306 vinfos[1].indices[1] = _ij1[1];
13307 vinfos[1].maxsolutions = _nj1;
13308 vinfos[2].jointtype = 1;
13309 vinfos[2].foffset = j2;
13310 vinfos[2].indices[0] = _ij2[0];
13311 vinfos[2].indices[1] = _ij2[1];
13312 vinfos[2].maxsolutions = _nj2;
13313 vinfos[3].jointtype = 1;
13314 vinfos[3].foffset = j3;
13315 vinfos[3].indices[0] = _ij3[0];
13316 vinfos[3].indices[1] = _ij3[1];
13317 vinfos[3].maxsolutions = _nj3;
13318 vinfos[4].jointtype = 1;
13319 vinfos[4].foffset = j4;
13320 vinfos[4].indices[0] = _ij4[0];
13321 vinfos[4].indices[1] = _ij4[1];
13322 vinfos[4].maxsolutions = _nj4;
13323 vinfos[5].jointtype = 1;
13324 vinfos[5].foffset = j5;
13325 vinfos[5].indices[0] = _ij5[0];
13326 vinfos[5].indices[1] = _ij5[1];
13327 vinfos[5].maxsolutions = _nj5;
13328 std::vector<int> vfree(0);
13329 solutions.AddSolution(vinfos,vfree);
13330 }
13331 }
13332 }
13333 
13334 }
13335 
13336 }
13337 
13338 } else
13339 {
13340 {
13341 IkReal j3array[1], cj3array[1], sj3array[1];
13342 bool j3valid[1]={false};
13343 _nj3 = 1;
13344 CheckValue<IkReal> x1028=IKPowWithIntegerCheck(IKsign(gconst16),-1);
13345 if(!x1028.valid){
13346 continue;
13347 }
13348 CheckValue<IkReal> x1029 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
13349 if(!x1029.valid){
13350 continue;
13351 }
13352 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1028.value)))+(x1029.value));
13353 sj3array[0]=IKsin(j3array[0]);
13354 cj3array[0]=IKcos(j3array[0]);
13355 if( j3array[0] > IKPI )
13356 {
13357  j3array[0]-=IK2PI;
13358 }
13359 else if( j3array[0] < -IKPI )
13360 { j3array[0]+=IK2PI;
13361 }
13362 j3valid[0] = true;
13363 for(int ij3 = 0; ij3 < 1; ++ij3)
13364 {
13365 if( !j3valid[ij3] )
13366 {
13367  continue;
13368 }
13369 _ij3[0] = ij3; _ij3[1] = -1;
13370 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13371 {
13372 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13373 {
13374  j3valid[iij3]=false; _ij3[1] = iij3; break;
13375 }
13376 }
13377 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13378 {
13379 IkReal evalcond[8];
13380 IkReal x1030=IKsin(j3);
13381 IkReal x1031=IKcos(j3);
13382 IkReal x1032=((1.0)*gconst16);
13383 IkReal x1033=((1.0)*x1031);
13384 IkReal x1034=(x1031*x1032);
13385 evalcond[0]=(new_r10*x1030);
13386 evalcond[1]=(gconst16*x1030);
13387 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1033)));
13388 evalcond[3]=((((-1.0)*x1034))+new_r01);
13389 evalcond[4]=((((-1.0)*x1030*x1032))+new_r11);
13390 evalcond[5]=((((-1.0)*x1034))+new_r10);
13391 evalcond[6]=((((-1.0)*new_r11*x1033))+((new_r01*x1030)));
13392 evalcond[7]=((((-1.0)*x1032))+((new_r11*x1030))+((new_r01*x1031)));
13393 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13394 {
13395 continue;
13396 }
13397 }
13398 
13399 {
13400 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13401 vinfos[0].jointtype = 1;
13402 vinfos[0].foffset = j0;
13403 vinfos[0].indices[0] = _ij0[0];
13404 vinfos[0].indices[1] = _ij0[1];
13405 vinfos[0].maxsolutions = _nj0;
13406 vinfos[1].jointtype = 1;
13407 vinfos[1].foffset = j1;
13408 vinfos[1].indices[0] = _ij1[0];
13409 vinfos[1].indices[1] = _ij1[1];
13410 vinfos[1].maxsolutions = _nj1;
13411 vinfos[2].jointtype = 1;
13412 vinfos[2].foffset = j2;
13413 vinfos[2].indices[0] = _ij2[0];
13414 vinfos[2].indices[1] = _ij2[1];
13415 vinfos[2].maxsolutions = _nj2;
13416 vinfos[3].jointtype = 1;
13417 vinfos[3].foffset = j3;
13418 vinfos[3].indices[0] = _ij3[0];
13419 vinfos[3].indices[1] = _ij3[1];
13420 vinfos[3].maxsolutions = _nj3;
13421 vinfos[4].jointtype = 1;
13422 vinfos[4].foffset = j4;
13423 vinfos[4].indices[0] = _ij4[0];
13424 vinfos[4].indices[1] = _ij4[1];
13425 vinfos[4].maxsolutions = _nj4;
13426 vinfos[5].jointtype = 1;
13427 vinfos[5].foffset = j5;
13428 vinfos[5].indices[0] = _ij5[0];
13429 vinfos[5].indices[1] = _ij5[1];
13430 vinfos[5].maxsolutions = _nj5;
13431 std::vector<int> vfree(0);
13432 solutions.AddSolution(vinfos,vfree);
13433 }
13434 }
13435 }
13436 
13437 }
13438 
13439 }
13440 
13441 }
13442 } while(0);
13443 if( bgotonextstatement )
13444 {
13445 bool bgotonextstatement = true;
13446 do
13447 {
13448 if( 1 )
13449 {
13450 bgotonextstatement=false;
13451 continue; // branch miss [j3]
13452 
13453 }
13454 } while(0);
13455 if( bgotonextstatement )
13456 {
13457 }
13458 }
13459 }
13460 }
13461 }
13462 }
13463 
13464 } else
13465 {
13466 {
13467 IkReal j3array[1], cj3array[1], sj3array[1];
13468 bool j3valid[1]={false};
13469 _nj3 = 1;
13470 CheckValue<IkReal> x1035 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst16*gconst17)))),IkReal(((gconst17*gconst17)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
13471 if(!x1035.valid){
13472 continue;
13473 }
13474 CheckValue<IkReal> x1036=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
13475 if(!x1036.valid){
13476 continue;
13477 }
13478 j3array[0]=((-1.5707963267949)+(x1035.value)+(((1.5707963267949)*(x1036.value))));
13479 sj3array[0]=IKsin(j3array[0]);
13480 cj3array[0]=IKcos(j3array[0]);
13481 if( j3array[0] > IKPI )
13482 {
13483  j3array[0]-=IK2PI;
13484 }
13485 else if( j3array[0] < -IKPI )
13486 { j3array[0]+=IK2PI;
13487 }
13488 j3valid[0] = true;
13489 for(int ij3 = 0; ij3 < 1; ++ij3)
13490 {
13491 if( !j3valid[ij3] )
13492 {
13493  continue;
13494 }
13495 _ij3[0] = ij3; _ij3[1] = -1;
13496 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13497 {
13498 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13499 {
13500  j3valid[iij3]=false; _ij3[1] = iij3; break;
13501 }
13502 }
13503 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13504 {
13505 IkReal evalcond[8];
13506 IkReal x1037=IKsin(j3);
13507 IkReal x1038=IKcos(j3);
13508 IkReal x1039=((1.0)*gconst16);
13509 IkReal x1040=(gconst17*x1037);
13510 IkReal x1041=(gconst17*x1038);
13511 IkReal x1042=((1.0)*x1038);
13512 IkReal x1043=(x1038*x1039);
13513 evalcond[0]=(gconst17+((new_r10*x1037))+((new_r00*x1038)));
13514 evalcond[1]=(x1041+((gconst16*x1037))+new_r00);
13515 evalcond[2]=(gconst16+((new_r00*x1037))+(((-1.0)*new_r10*x1042)));
13516 evalcond[3]=(gconst17+((new_r01*x1037))+(((-1.0)*new_r11*x1042)));
13517 evalcond[4]=(x1040+new_r01+(((-1.0)*x1043)));
13518 evalcond[5]=(x1040+new_r10+(((-1.0)*x1043)));
13519 evalcond[6]=((((-1.0)*x1039))+((new_r11*x1037))+((new_r01*x1038)));
13520 evalcond[7]=((((-1.0)*x1037*x1039))+(((-1.0)*x1041))+new_r11);
13521 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13522 {
13523 continue;
13524 }
13525 }
13526 
13527 {
13528 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13529 vinfos[0].jointtype = 1;
13530 vinfos[0].foffset = j0;
13531 vinfos[0].indices[0] = _ij0[0];
13532 vinfos[0].indices[1] = _ij0[1];
13533 vinfos[0].maxsolutions = _nj0;
13534 vinfos[1].jointtype = 1;
13535 vinfos[1].foffset = j1;
13536 vinfos[1].indices[0] = _ij1[0];
13537 vinfos[1].indices[1] = _ij1[1];
13538 vinfos[1].maxsolutions = _nj1;
13539 vinfos[2].jointtype = 1;
13540 vinfos[2].foffset = j2;
13541 vinfos[2].indices[0] = _ij2[0];
13542 vinfos[2].indices[1] = _ij2[1];
13543 vinfos[2].maxsolutions = _nj2;
13544 vinfos[3].jointtype = 1;
13545 vinfos[3].foffset = j3;
13546 vinfos[3].indices[0] = _ij3[0];
13547 vinfos[3].indices[1] = _ij3[1];
13548 vinfos[3].maxsolutions = _nj3;
13549 vinfos[4].jointtype = 1;
13550 vinfos[4].foffset = j4;
13551 vinfos[4].indices[0] = _ij4[0];
13552 vinfos[4].indices[1] = _ij4[1];
13553 vinfos[4].maxsolutions = _nj4;
13554 vinfos[5].jointtype = 1;
13555 vinfos[5].foffset = j5;
13556 vinfos[5].indices[0] = _ij5[0];
13557 vinfos[5].indices[1] = _ij5[1];
13558 vinfos[5].maxsolutions = _nj5;
13559 std::vector<int> vfree(0);
13560 solutions.AddSolution(vinfos,vfree);
13561 }
13562 }
13563 }
13564 
13565 }
13566 
13567 }
13568 
13569 } else
13570 {
13571 {
13572 IkReal j3array[1], cj3array[1], sj3array[1];
13573 bool j3valid[1]={false};
13574 _nj3 = 1;
13575 IkReal x1044=((1.0)*new_r10);
13576 CheckValue<IkReal> x1045 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((gconst16*gconst17)))),IkReal(((((-1.0)*new_r01*x1044))+(gconst17*gconst17))),IKFAST_ATAN2_MAGTHRESH);
13577 if(!x1045.valid){
13578 continue;
13579 }
13580 CheckValue<IkReal> x1046=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*x1044))+(((-1.0)*gconst17*new_r00)))),-1);
13581 if(!x1046.valid){
13582 continue;
13583 }
13584 j3array[0]=((-1.5707963267949)+(x1045.value)+(((1.5707963267949)*(x1046.value))));
13585 sj3array[0]=IKsin(j3array[0]);
13586 cj3array[0]=IKcos(j3array[0]);
13587 if( j3array[0] > IKPI )
13588 {
13589  j3array[0]-=IK2PI;
13590 }
13591 else if( j3array[0] < -IKPI )
13592 { j3array[0]+=IK2PI;
13593 }
13594 j3valid[0] = true;
13595 for(int ij3 = 0; ij3 < 1; ++ij3)
13596 {
13597 if( !j3valid[ij3] )
13598 {
13599  continue;
13600 }
13601 _ij3[0] = ij3; _ij3[1] = -1;
13602 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13603 {
13604 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13605 {
13606  j3valid[iij3]=false; _ij3[1] = iij3; break;
13607 }
13608 }
13609 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13610 {
13611 IkReal evalcond[8];
13612 IkReal x1047=IKsin(j3);
13613 IkReal x1048=IKcos(j3);
13614 IkReal x1049=((1.0)*gconst16);
13615 IkReal x1050=(gconst17*x1047);
13616 IkReal x1051=(gconst17*x1048);
13617 IkReal x1052=((1.0)*x1048);
13618 IkReal x1053=(x1048*x1049);
13619 evalcond[0]=(gconst17+((new_r00*x1048))+((new_r10*x1047)));
13620 evalcond[1]=(x1051+new_r00+((gconst16*x1047)));
13621 evalcond[2]=(gconst16+((new_r00*x1047))+(((-1.0)*new_r10*x1052)));
13622 evalcond[3]=(gconst17+((new_r01*x1047))+(((-1.0)*new_r11*x1052)));
13623 evalcond[4]=((((-1.0)*x1053))+x1050+new_r01);
13624 evalcond[5]=((((-1.0)*x1053))+x1050+new_r10);
13625 evalcond[6]=(((new_r11*x1047))+((new_r01*x1048))+(((-1.0)*x1049)));
13626 evalcond[7]=((((-1.0)*x1051))+new_r11+(((-1.0)*x1047*x1049)));
13627 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13628 {
13629 continue;
13630 }
13631 }
13632 
13633 {
13634 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13635 vinfos[0].jointtype = 1;
13636 vinfos[0].foffset = j0;
13637 vinfos[0].indices[0] = _ij0[0];
13638 vinfos[0].indices[1] = _ij0[1];
13639 vinfos[0].maxsolutions = _nj0;
13640 vinfos[1].jointtype = 1;
13641 vinfos[1].foffset = j1;
13642 vinfos[1].indices[0] = _ij1[0];
13643 vinfos[1].indices[1] = _ij1[1];
13644 vinfos[1].maxsolutions = _nj1;
13645 vinfos[2].jointtype = 1;
13646 vinfos[2].foffset = j2;
13647 vinfos[2].indices[0] = _ij2[0];
13648 vinfos[2].indices[1] = _ij2[1];
13649 vinfos[2].maxsolutions = _nj2;
13650 vinfos[3].jointtype = 1;
13651 vinfos[3].foffset = j3;
13652 vinfos[3].indices[0] = _ij3[0];
13653 vinfos[3].indices[1] = _ij3[1];
13654 vinfos[3].maxsolutions = _nj3;
13655 vinfos[4].jointtype = 1;
13656 vinfos[4].foffset = j4;
13657 vinfos[4].indices[0] = _ij4[0];
13658 vinfos[4].indices[1] = _ij4[1];
13659 vinfos[4].maxsolutions = _nj4;
13660 vinfos[5].jointtype = 1;
13661 vinfos[5].foffset = j5;
13662 vinfos[5].indices[0] = _ij5[0];
13663 vinfos[5].indices[1] = _ij5[1];
13664 vinfos[5].maxsolutions = _nj5;
13665 std::vector<int> vfree(0);
13666 solutions.AddSolution(vinfos,vfree);
13667 }
13668 }
13669 }
13670 
13671 }
13672 
13673 }
13674 
13675 } else
13676 {
13677 {
13678 IkReal j3array[1], cj3array[1], sj3array[1];
13679 bool j3valid[1]={false};
13680 _nj3 = 1;
13681 IkReal x1054=((1.0)*new_r10);
13682 CheckValue<IkReal> x1055=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1054)))),-1);
13683 if(!x1055.valid){
13684 continue;
13685 }
13686 CheckValue<IkReal> x1056 = IKatan2WithCheck(IkReal((((gconst17*new_r11))+((gconst17*new_r00)))),IkReal(((((-1.0)*gconst17*x1054))+((gconst17*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
13687 if(!x1056.valid){
13688 continue;
13689 }
13690 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1055.value)))+(x1056.value));
13691 sj3array[0]=IKsin(j3array[0]);
13692 cj3array[0]=IKcos(j3array[0]);
13693 if( j3array[0] > IKPI )
13694 {
13695  j3array[0]-=IK2PI;
13696 }
13697 else if( j3array[0] < -IKPI )
13698 { j3array[0]+=IK2PI;
13699 }
13700 j3valid[0] = true;
13701 for(int ij3 = 0; ij3 < 1; ++ij3)
13702 {
13703 if( !j3valid[ij3] )
13704 {
13705  continue;
13706 }
13707 _ij3[0] = ij3; _ij3[1] = -1;
13708 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13709 {
13710 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13711 {
13712  j3valid[iij3]=false; _ij3[1] = iij3; break;
13713 }
13714 }
13715 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13716 {
13717 IkReal evalcond[8];
13718 IkReal x1057=IKsin(j3);
13719 IkReal x1058=IKcos(j3);
13720 IkReal x1059=((1.0)*gconst16);
13721 IkReal x1060=(gconst17*x1057);
13722 IkReal x1061=(gconst17*x1058);
13723 IkReal x1062=((1.0)*x1058);
13724 IkReal x1063=(x1058*x1059);
13725 evalcond[0]=(gconst17+((new_r00*x1058))+((new_r10*x1057)));
13726 evalcond[1]=(x1061+new_r00+((gconst16*x1057)));
13727 evalcond[2]=((((-1.0)*new_r10*x1062))+gconst16+((new_r00*x1057)));
13728 evalcond[3]=(gconst17+((new_r01*x1057))+(((-1.0)*new_r11*x1062)));
13729 evalcond[4]=(x1060+new_r01+(((-1.0)*x1063)));
13730 evalcond[5]=(x1060+new_r10+(((-1.0)*x1063)));
13731 evalcond[6]=((((-1.0)*x1059))+((new_r01*x1058))+((new_r11*x1057)));
13732 evalcond[7]=((((-1.0)*x1057*x1059))+new_r11+(((-1.0)*x1061)));
13733 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13734 {
13735 continue;
13736 }
13737 }
13738 
13739 {
13740 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13741 vinfos[0].jointtype = 1;
13742 vinfos[0].foffset = j0;
13743 vinfos[0].indices[0] = _ij0[0];
13744 vinfos[0].indices[1] = _ij0[1];
13745 vinfos[0].maxsolutions = _nj0;
13746 vinfos[1].jointtype = 1;
13747 vinfos[1].foffset = j1;
13748 vinfos[1].indices[0] = _ij1[0];
13749 vinfos[1].indices[1] = _ij1[1];
13750 vinfos[1].maxsolutions = _nj1;
13751 vinfos[2].jointtype = 1;
13752 vinfos[2].foffset = j2;
13753 vinfos[2].indices[0] = _ij2[0];
13754 vinfos[2].indices[1] = _ij2[1];
13755 vinfos[2].maxsolutions = _nj2;
13756 vinfos[3].jointtype = 1;
13757 vinfos[3].foffset = j3;
13758 vinfos[3].indices[0] = _ij3[0];
13759 vinfos[3].indices[1] = _ij3[1];
13760 vinfos[3].maxsolutions = _nj3;
13761 vinfos[4].jointtype = 1;
13762 vinfos[4].foffset = j4;
13763 vinfos[4].indices[0] = _ij4[0];
13764 vinfos[4].indices[1] = _ij4[1];
13765 vinfos[4].maxsolutions = _nj4;
13766 vinfos[5].jointtype = 1;
13767 vinfos[5].foffset = j5;
13768 vinfos[5].indices[0] = _ij5[0];
13769 vinfos[5].indices[1] = _ij5[1];
13770 vinfos[5].maxsolutions = _nj5;
13771 std::vector<int> vfree(0);
13772 solutions.AddSolution(vinfos,vfree);
13773 }
13774 }
13775 }
13776 
13777 }
13778 
13779 }
13780 
13781 }
13782 } while(0);
13783 if( bgotonextstatement )
13784 {
13785 bool bgotonextstatement = true;
13786 do
13787 {
13788 IkReal x1064=((-1.0)*new_r10);
13789 IkReal x1066 = ((new_r10*new_r10)+(new_r00*new_r00));
13790 if(IKabs(x1066)==0){
13791 continue;
13792 }
13793 IkReal x1065=pow(x1066,-0.5);
13794 CheckValue<IkReal> x1067 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1064),IKFAST_ATAN2_MAGTHRESH);
13795 if(!x1067.valid){
13796 continue;
13797 }
13798 IkReal gconst18=((-1.0)*(x1067.value));
13799 IkReal gconst19=(new_r00*x1065);
13800 IkReal gconst20=(x1064*x1065);
13801 CheckValue<IkReal> x1068 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
13802 if(!x1068.valid){
13803 continue;
13804 }
13805 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x1068.value)+j5)))), 6.28318530717959)));
13806 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13807 {
13808 bgotonextstatement=false;
13809 {
13810 IkReal j3eval[3];
13811 IkReal x1069=((-1.0)*new_r10);
13812 CheckValue<IkReal> x1072 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1069),IKFAST_ATAN2_MAGTHRESH);
13813 if(!x1072.valid){
13814 continue;
13815 }
13816 IkReal x1070=((-1.0)*(x1072.value));
13817 IkReal x1071=x1065;
13818 sj4=0;
13819 cj4=-1.0;
13820 j4=3.14159265358979;
13821 sj5=gconst19;
13822 cj5=gconst20;
13823 j5=x1070;
13824 IkReal gconst18=x1070;
13825 IkReal gconst19=(new_r00*x1071);
13826 IkReal gconst20=(x1069*x1071);
13827 IkReal x1073=new_r10*new_r10;
13828 IkReal x1074=((1.0)*new_r00);
13829 IkReal x1075=((1.0)*new_r10*new_r11);
13830 IkReal x1076=((((-1.0)*new_r01*x1074))+(((-1.0)*x1075)));
13831 IkReal x1077=x1065;
13832 IkReal x1078=(new_r10*x1077);
13833 j3eval[0]=x1076;
13834 j3eval[1]=((IKabs(((((-1.0)*x1075*x1077))+(((-1.0)*x1074*x1078)))))+(IKabs((((x1073*x1077))+(((-1.0)*new_r01*x1078))))));
13835 j3eval[2]=IKsign(x1076);
13836 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
13837 {
13838 {
13839 IkReal j3eval[1];
13840 IkReal x1079=((-1.0)*new_r10);
13841 CheckValue<IkReal> x1082 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1079),IKFAST_ATAN2_MAGTHRESH);
13842 if(!x1082.valid){
13843 continue;
13844 }
13845 IkReal x1080=((-1.0)*(x1082.value));
13846 IkReal x1081=x1065;
13847 sj4=0;
13848 cj4=-1.0;
13849 j4=3.14159265358979;
13850 sj5=gconst19;
13851 cj5=gconst20;
13852 j5=x1080;
13853 IkReal gconst18=x1080;
13854 IkReal gconst19=(new_r00*x1081);
13855 IkReal gconst20=(x1079*x1081);
13856 IkReal x1083=new_r10*new_r10;
13857 CheckValue<IkReal> x1086=IKPowWithIntegerCheck((x1083+(new_r00*new_r00)),-1);
13858 if(!x1086.valid){
13859 continue;
13860 }
13861 IkReal x1084=x1086.value;
13862 IkReal x1085=(new_r00*x1084);
13863 j3eval[0]=((IKabs((((new_r00*new_r11))+((x1083*x1084)))))+(IKabs((((new_r01*x1083*x1085))+((new_r10*x1085))+((new_r01*x1085*(new_r00*new_r00)))))));
13864 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13865 {
13866 {
13867 IkReal j3eval[1];
13868 IkReal x1087=((-1.0)*new_r10);
13869 CheckValue<IkReal> x1090 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1087),IKFAST_ATAN2_MAGTHRESH);
13870 if(!x1090.valid){
13871 continue;
13872 }
13873 IkReal x1088=((-1.0)*(x1090.value));
13874 IkReal x1089=x1065;
13875 sj4=0;
13876 cj4=-1.0;
13877 j4=3.14159265358979;
13878 sj5=gconst19;
13879 cj5=gconst20;
13880 j5=x1088;
13881 IkReal gconst18=x1088;
13882 IkReal gconst19=(new_r00*x1089);
13883 IkReal gconst20=(x1087*x1089);
13884 IkReal x1091=new_r10*new_r10;
13885 IkReal x1092=new_r00*new_r00;
13886 CheckValue<IkReal> x1096=IKPowWithIntegerCheck((x1091+x1092),-1);
13887 if(!x1096.valid){
13888 continue;
13889 }
13890 IkReal x1093=x1096.value;
13891 IkReal x1094=(new_r10*x1093);
13892 IkReal x1095=(x1091*x1093);
13893 j3eval[0]=((IKabs((x1095+(((-1.0)*x1093*(x1092*x1092)))+(((-1.0)*x1092*x1095)))))+(IKabs((((new_r00*x1094))+((x1094*(new_r00*new_r00*new_r00)))+((new_r00*x1094*(new_r10*new_r10)))))));
13894 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13895 {
13896 {
13897 IkReal evalcond[3];
13898 bool bgotonextstatement = true;
13899 do
13900 {
13901 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
13902 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13903 {
13904 bgotonextstatement=false;
13905 {
13906 IkReal j3eval[1];
13907 IkReal x1097=((-1.0)*new_r10);
13908 CheckValue<IkReal> x1099 = IKatan2WithCheck(IkReal(0),IkReal(x1097),IKFAST_ATAN2_MAGTHRESH);
13909 if(!x1099.valid){
13910 continue;
13911 }
13912 IkReal x1098=((-1.0)*(x1099.value));
13913 sj4=0;
13914 cj4=-1.0;
13915 j4=3.14159265358979;
13916 sj5=gconst19;
13917 cj5=gconst20;
13918 j5=x1098;
13919 new_r11=0;
13920 new_r00=0;
13921 IkReal gconst18=x1098;
13922 IkReal gconst19=0;
13923 IkReal x1100 = new_r10*new_r10;
13924 if(IKabs(x1100)==0){
13925 continue;
13926 }
13927 IkReal gconst20=(x1097*(pow(x1100,-0.5)));
13928 j3eval[0]=new_r01;
13929 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13930 {
13931 {
13932 IkReal j3array[2], cj3array[2], sj3array[2];
13933 bool j3valid[2]={false};
13934 _nj3 = 2;
13935 CheckValue<IkReal> x1101=IKPowWithIntegerCheck(gconst20,-1);
13936 if(!x1101.valid){
13937 continue;
13938 }
13939 sj3array[0]=((-1.0)*new_r01*(x1101.value));
13940 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
13941 {
13942  j3valid[0] = j3valid[1] = true;
13943  j3array[0] = IKasin(sj3array[0]);
13944  cj3array[0] = IKcos(j3array[0]);
13945  sj3array[1] = sj3array[0];
13946  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
13947  cj3array[1] = -cj3array[0];
13948 }
13949 else if( isnan(sj3array[0]) )
13950 {
13951  // probably any value will work
13952  j3valid[0] = true;
13953  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13954 }
13955 for(int ij3 = 0; ij3 < 2; ++ij3)
13956 {
13957 if( !j3valid[ij3] )
13958 {
13959  continue;
13960 }
13961 _ij3[0] = ij3; _ij3[1] = -1;
13962 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
13963 {
13964 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13965 {
13966  j3valid[iij3]=false; _ij3[1] = iij3; break;
13967 }
13968 }
13969 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13970 {
13971 IkReal evalcond[6];
13972 IkReal x1102=IKcos(j3);
13973 IkReal x1103=IKsin(j3);
13974 evalcond[0]=(new_r01*x1102);
13975 evalcond[1]=(gconst20*x1102);
13976 evalcond[2]=((-1.0)*new_r10*x1102);
13977 evalcond[3]=(gconst20+((new_r01*x1103)));
13978 evalcond[4]=(gconst20+((new_r10*x1103)));
13979 evalcond[5]=(((gconst20*x1103))+new_r10);
13980 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
13981 {
13982 continue;
13983 }
13984 }
13985 
13986 {
13987 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13988 vinfos[0].jointtype = 1;
13989 vinfos[0].foffset = j0;
13990 vinfos[0].indices[0] = _ij0[0];
13991 vinfos[0].indices[1] = _ij0[1];
13992 vinfos[0].maxsolutions = _nj0;
13993 vinfos[1].jointtype = 1;
13994 vinfos[1].foffset = j1;
13995 vinfos[1].indices[0] = _ij1[0];
13996 vinfos[1].indices[1] = _ij1[1];
13997 vinfos[1].maxsolutions = _nj1;
13998 vinfos[2].jointtype = 1;
13999 vinfos[2].foffset = j2;
14000 vinfos[2].indices[0] = _ij2[0];
14001 vinfos[2].indices[1] = _ij2[1];
14002 vinfos[2].maxsolutions = _nj2;
14003 vinfos[3].jointtype = 1;
14004 vinfos[3].foffset = j3;
14005 vinfos[3].indices[0] = _ij3[0];
14006 vinfos[3].indices[1] = _ij3[1];
14007 vinfos[3].maxsolutions = _nj3;
14008 vinfos[4].jointtype = 1;
14009 vinfos[4].foffset = j4;
14010 vinfos[4].indices[0] = _ij4[0];
14011 vinfos[4].indices[1] = _ij4[1];
14012 vinfos[4].maxsolutions = _nj4;
14013 vinfos[5].jointtype = 1;
14014 vinfos[5].foffset = j5;
14015 vinfos[5].indices[0] = _ij5[0];
14016 vinfos[5].indices[1] = _ij5[1];
14017 vinfos[5].maxsolutions = _nj5;
14018 std::vector<int> vfree(0);
14019 solutions.AddSolution(vinfos,vfree);
14020 }
14021 }
14022 }
14023 
14024 } else
14025 {
14026 {
14027 IkReal j3array[2], cj3array[2], sj3array[2];
14028 bool j3valid[2]={false};
14029 _nj3 = 2;
14030 CheckValue<IkReal> x1104=IKPowWithIntegerCheck(new_r01,-1);
14031 if(!x1104.valid){
14032 continue;
14033 }
14034 sj3array[0]=((-1.0)*gconst20*(x1104.value));
14035 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14036 {
14037  j3valid[0] = j3valid[1] = true;
14038  j3array[0] = IKasin(sj3array[0]);
14039  cj3array[0] = IKcos(j3array[0]);
14040  sj3array[1] = sj3array[0];
14041  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14042  cj3array[1] = -cj3array[0];
14043 }
14044 else if( isnan(sj3array[0]) )
14045 {
14046  // probably any value will work
14047  j3valid[0] = true;
14048  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14049 }
14050 for(int ij3 = 0; ij3 < 2; ++ij3)
14051 {
14052 if( !j3valid[ij3] )
14053 {
14054  continue;
14055 }
14056 _ij3[0] = ij3; _ij3[1] = -1;
14057 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14058 {
14059 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14060 {
14061  j3valid[iij3]=false; _ij3[1] = iij3; break;
14062 }
14063 }
14064 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14065 {
14066 IkReal evalcond[6];
14067 IkReal x1105=IKcos(j3);
14068 IkReal x1106=IKsin(j3);
14069 IkReal x1107=(gconst20*x1106);
14070 evalcond[0]=(new_r01*x1105);
14071 evalcond[1]=(gconst20*x1105);
14072 evalcond[2]=((-1.0)*new_r10*x1105);
14073 evalcond[3]=(x1107+new_r01);
14074 evalcond[4]=(gconst20+((new_r10*x1106)));
14075 evalcond[5]=(x1107+new_r10);
14076 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14077 {
14078 continue;
14079 }
14080 }
14081 
14082 {
14083 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14084 vinfos[0].jointtype = 1;
14085 vinfos[0].foffset = j0;
14086 vinfos[0].indices[0] = _ij0[0];
14087 vinfos[0].indices[1] = _ij0[1];
14088 vinfos[0].maxsolutions = _nj0;
14089 vinfos[1].jointtype = 1;
14090 vinfos[1].foffset = j1;
14091 vinfos[1].indices[0] = _ij1[0];
14092 vinfos[1].indices[1] = _ij1[1];
14093 vinfos[1].maxsolutions = _nj1;
14094 vinfos[2].jointtype = 1;
14095 vinfos[2].foffset = j2;
14096 vinfos[2].indices[0] = _ij2[0];
14097 vinfos[2].indices[1] = _ij2[1];
14098 vinfos[2].maxsolutions = _nj2;
14099 vinfos[3].jointtype = 1;
14100 vinfos[3].foffset = j3;
14101 vinfos[3].indices[0] = _ij3[0];
14102 vinfos[3].indices[1] = _ij3[1];
14103 vinfos[3].maxsolutions = _nj3;
14104 vinfos[4].jointtype = 1;
14105 vinfos[4].foffset = j4;
14106 vinfos[4].indices[0] = _ij4[0];
14107 vinfos[4].indices[1] = _ij4[1];
14108 vinfos[4].maxsolutions = _nj4;
14109 vinfos[5].jointtype = 1;
14110 vinfos[5].foffset = j5;
14111 vinfos[5].indices[0] = _ij5[0];
14112 vinfos[5].indices[1] = _ij5[1];
14113 vinfos[5].maxsolutions = _nj5;
14114 std::vector<int> vfree(0);
14115 solutions.AddSolution(vinfos,vfree);
14116 }
14117 }
14118 }
14119 
14120 }
14121 
14122 }
14123 
14124 }
14125 } while(0);
14126 if( bgotonextstatement )
14127 {
14128 bool bgotonextstatement = true;
14129 do
14130 {
14131 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14132 evalcond[1]=gconst20;
14133 evalcond[2]=gconst19;
14134 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
14135 {
14136 bgotonextstatement=false;
14137 {
14138 IkReal j3eval[3];
14139 IkReal x1108=((-1.0)*new_r10);
14140 CheckValue<IkReal> x1110 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1108),IKFAST_ATAN2_MAGTHRESH);
14141 if(!x1110.valid){
14142 continue;
14143 }
14144 IkReal x1109=((-1.0)*(x1110.value));
14145 sj4=0;
14146 cj4=-1.0;
14147 j4=3.14159265358979;
14148 sj5=gconst19;
14149 cj5=gconst20;
14150 j5=x1109;
14151 new_r11=0;
14152 new_r01=0;
14153 new_r22=0;
14154 new_r20=0;
14155 IkReal gconst18=x1109;
14156 IkReal gconst19=new_r00;
14157 IkReal gconst20=x1108;
14158 j3eval[0]=-1.0;
14159 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14160 j3eval[2]=-1.0;
14161 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14162 {
14163 {
14164 IkReal j3eval[3];
14165 IkReal x1111=((-1.0)*new_r10);
14166 CheckValue<IkReal> x1113 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1111),IKFAST_ATAN2_MAGTHRESH);
14167 if(!x1113.valid){
14168 continue;
14169 }
14170 IkReal x1112=((-1.0)*(x1113.value));
14171 sj4=0;
14172 cj4=-1.0;
14173 j4=3.14159265358979;
14174 sj5=gconst19;
14175 cj5=gconst20;
14176 j5=x1112;
14177 new_r11=0;
14178 new_r01=0;
14179 new_r22=0;
14180 new_r20=0;
14181 IkReal gconst18=x1112;
14182 IkReal gconst19=new_r00;
14183 IkReal gconst20=x1111;
14184 j3eval[0]=-1.0;
14185 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14186 j3eval[2]=-1.0;
14187 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14188 {
14189 {
14190 IkReal j3eval[3];
14191 IkReal x1114=((-1.0)*new_r10);
14192 CheckValue<IkReal> x1116 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1114),IKFAST_ATAN2_MAGTHRESH);
14193 if(!x1116.valid){
14194 continue;
14195 }
14196 IkReal x1115=((-1.0)*(x1116.value));
14197 sj4=0;
14198 cj4=-1.0;
14199 j4=3.14159265358979;
14200 sj5=gconst19;
14201 cj5=gconst20;
14202 j5=x1115;
14203 new_r11=0;
14204 new_r01=0;
14205 new_r22=0;
14206 new_r20=0;
14207 IkReal gconst18=x1115;
14208 IkReal gconst19=new_r00;
14209 IkReal gconst20=x1114;
14210 j3eval[0]=1.0;
14211 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
14212 j3eval[2]=1.0;
14213 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14214 {
14215 continue; // 3 cases reached
14216 
14217 } else
14218 {
14219 {
14220 IkReal j3array[1], cj3array[1], sj3array[1];
14221 bool j3valid[1]={false};
14222 _nj3 = 1;
14223 IkReal x1117=((1.0)*gconst20);
14224 CheckValue<IkReal> x1118=IKPowWithIntegerCheck(IKsign((((gconst19*new_r00))+(((-1.0)*new_r10*x1117)))),-1);
14225 if(!x1118.valid){
14226 continue;
14227 }
14228 CheckValue<IkReal> x1119 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst20*gconst20))),IkReal(((((-1.0)*gconst19*x1117))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
14229 if(!x1119.valid){
14230 continue;
14231 }
14232 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1118.value)))+(x1119.value));
14233 sj3array[0]=IKsin(j3array[0]);
14234 cj3array[0]=IKcos(j3array[0]);
14235 if( j3array[0] > IKPI )
14236 {
14237  j3array[0]-=IK2PI;
14238 }
14239 else if( j3array[0] < -IKPI )
14240 { j3array[0]+=IK2PI;
14241 }
14242 j3valid[0] = true;
14243 for(int ij3 = 0; ij3 < 1; ++ij3)
14244 {
14245 if( !j3valid[ij3] )
14246 {
14247  continue;
14248 }
14249 _ij3[0] = ij3; _ij3[1] = -1;
14250 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14251 {
14252 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14253 {
14254  j3valid[iij3]=false; _ij3[1] = iij3; break;
14255 }
14256 }
14257 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14258 {
14259 IkReal evalcond[6];
14260 IkReal x1120=IKsin(j3);
14261 IkReal x1121=IKcos(j3);
14262 IkReal x1122=(gconst20*x1120);
14263 IkReal x1123=(gconst19*x1120);
14264 IkReal x1124=(gconst20*x1121);
14265 IkReal x1125=((1.0)*x1121);
14266 IkReal x1126=(gconst19*x1125);
14267 evalcond[0]=(x1122+(((-1.0)*x1126)));
14268 evalcond[1]=(gconst20+((new_r10*x1120))+((new_r00*x1121)));
14269 evalcond[2]=(x1123+x1124+new_r00);
14270 evalcond[3]=((((-1.0)*new_r10*x1125))+gconst19+((new_r00*x1120)));
14271 evalcond[4]=((((-1.0)*x1124))+(((-1.0)*x1123)));
14272 evalcond[5]=(x1122+(((-1.0)*x1126))+new_r10);
14273 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14274 {
14275 continue;
14276 }
14277 }
14278 
14279 {
14280 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14281 vinfos[0].jointtype = 1;
14282 vinfos[0].foffset = j0;
14283 vinfos[0].indices[0] = _ij0[0];
14284 vinfos[0].indices[1] = _ij0[1];
14285 vinfos[0].maxsolutions = _nj0;
14286 vinfos[1].jointtype = 1;
14287 vinfos[1].foffset = j1;
14288 vinfos[1].indices[0] = _ij1[0];
14289 vinfos[1].indices[1] = _ij1[1];
14290 vinfos[1].maxsolutions = _nj1;
14291 vinfos[2].jointtype = 1;
14292 vinfos[2].foffset = j2;
14293 vinfos[2].indices[0] = _ij2[0];
14294 vinfos[2].indices[1] = _ij2[1];
14295 vinfos[2].maxsolutions = _nj2;
14296 vinfos[3].jointtype = 1;
14297 vinfos[3].foffset = j3;
14298 vinfos[3].indices[0] = _ij3[0];
14299 vinfos[3].indices[1] = _ij3[1];
14300 vinfos[3].maxsolutions = _nj3;
14301 vinfos[4].jointtype = 1;
14302 vinfos[4].foffset = j4;
14303 vinfos[4].indices[0] = _ij4[0];
14304 vinfos[4].indices[1] = _ij4[1];
14305 vinfos[4].maxsolutions = _nj4;
14306 vinfos[5].jointtype = 1;
14307 vinfos[5].foffset = j5;
14308 vinfos[5].indices[0] = _ij5[0];
14309 vinfos[5].indices[1] = _ij5[1];
14310 vinfos[5].maxsolutions = _nj5;
14311 std::vector<int> vfree(0);
14312 solutions.AddSolution(vinfos,vfree);
14313 }
14314 }
14315 }
14316 
14317 }
14318 
14319 }
14320 
14321 } else
14322 {
14323 {
14324 IkReal j3array[1], cj3array[1], sj3array[1];
14325 bool j3valid[1]={false};
14326 _nj3 = 1;
14327 CheckValue<IkReal> x1127 = IKatan2WithCheck(IkReal((gconst19*new_r00)),IkReal((gconst20*new_r00)),IKFAST_ATAN2_MAGTHRESH);
14328 if(!x1127.valid){
14329 continue;
14330 }
14331 CheckValue<IkReal> x1128=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst20*gconst20)))+(((-1.0)*(gconst19*gconst19))))),-1);
14332 if(!x1128.valid){
14333 continue;
14334 }
14335 j3array[0]=((-1.5707963267949)+(x1127.value)+(((1.5707963267949)*(x1128.value))));
14336 sj3array[0]=IKsin(j3array[0]);
14337 cj3array[0]=IKcos(j3array[0]);
14338 if( j3array[0] > IKPI )
14339 {
14340  j3array[0]-=IK2PI;
14341 }
14342 else if( j3array[0] < -IKPI )
14343 { j3array[0]+=IK2PI;
14344 }
14345 j3valid[0] = true;
14346 for(int ij3 = 0; ij3 < 1; ++ij3)
14347 {
14348 if( !j3valid[ij3] )
14349 {
14350  continue;
14351 }
14352 _ij3[0] = ij3; _ij3[1] = -1;
14353 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14354 {
14355 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14356 {
14357  j3valid[iij3]=false; _ij3[1] = iij3; break;
14358 }
14359 }
14360 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14361 {
14362 IkReal evalcond[6];
14363 IkReal x1129=IKsin(j3);
14364 IkReal x1130=IKcos(j3);
14365 IkReal x1131=(gconst20*x1129);
14366 IkReal x1132=(gconst19*x1129);
14367 IkReal x1133=(gconst20*x1130);
14368 IkReal x1134=((1.0)*x1130);
14369 IkReal x1135=(gconst19*x1134);
14370 evalcond[0]=(x1131+(((-1.0)*x1135)));
14371 evalcond[1]=(gconst20+((new_r10*x1129))+((new_r00*x1130)));
14372 evalcond[2]=(x1133+x1132+new_r00);
14373 evalcond[3]=(gconst19+(((-1.0)*new_r10*x1134))+((new_r00*x1129)));
14374 evalcond[4]=((((-1.0)*x1132))+(((-1.0)*x1133)));
14375 evalcond[5]=(x1131+(((-1.0)*x1135))+new_r10);
14376 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14377 {
14378 continue;
14379 }
14380 }
14381 
14382 {
14383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14384 vinfos[0].jointtype = 1;
14385 vinfos[0].foffset = j0;
14386 vinfos[0].indices[0] = _ij0[0];
14387 vinfos[0].indices[1] = _ij0[1];
14388 vinfos[0].maxsolutions = _nj0;
14389 vinfos[1].jointtype = 1;
14390 vinfos[1].foffset = j1;
14391 vinfos[1].indices[0] = _ij1[0];
14392 vinfos[1].indices[1] = _ij1[1];
14393 vinfos[1].maxsolutions = _nj1;
14394 vinfos[2].jointtype = 1;
14395 vinfos[2].foffset = j2;
14396 vinfos[2].indices[0] = _ij2[0];
14397 vinfos[2].indices[1] = _ij2[1];
14398 vinfos[2].maxsolutions = _nj2;
14399 vinfos[3].jointtype = 1;
14400 vinfos[3].foffset = j3;
14401 vinfos[3].indices[0] = _ij3[0];
14402 vinfos[3].indices[1] = _ij3[1];
14403 vinfos[3].maxsolutions = _nj3;
14404 vinfos[4].jointtype = 1;
14405 vinfos[4].foffset = j4;
14406 vinfos[4].indices[0] = _ij4[0];
14407 vinfos[4].indices[1] = _ij4[1];
14408 vinfos[4].maxsolutions = _nj4;
14409 vinfos[5].jointtype = 1;
14410 vinfos[5].foffset = j5;
14411 vinfos[5].indices[0] = _ij5[0];
14412 vinfos[5].indices[1] = _ij5[1];
14413 vinfos[5].maxsolutions = _nj5;
14414 std::vector<int> vfree(0);
14415 solutions.AddSolution(vinfos,vfree);
14416 }
14417 }
14418 }
14419 
14420 }
14421 
14422 }
14423 
14424 } else
14425 {
14426 {
14427 IkReal j3array[1], cj3array[1], sj3array[1];
14428 bool j3valid[1]={false};
14429 _nj3 = 1;
14430 CheckValue<IkReal> x1136=IKPowWithIntegerCheck(IKsign((((gconst20*new_r10))+(((-1.0)*gconst19*new_r00)))),-1);
14431 if(!x1136.valid){
14432 continue;
14433 }
14434 CheckValue<IkReal> x1137 = IKatan2WithCheck(IkReal(gconst19*gconst19),IkReal((gconst19*gconst20)),IKFAST_ATAN2_MAGTHRESH);
14435 if(!x1137.valid){
14436 continue;
14437 }
14438 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1136.value)))+(x1137.value));
14439 sj3array[0]=IKsin(j3array[0]);
14440 cj3array[0]=IKcos(j3array[0]);
14441 if( j3array[0] > IKPI )
14442 {
14443  j3array[0]-=IK2PI;
14444 }
14445 else if( j3array[0] < -IKPI )
14446 { j3array[0]+=IK2PI;
14447 }
14448 j3valid[0] = true;
14449 for(int ij3 = 0; ij3 < 1; ++ij3)
14450 {
14451 if( !j3valid[ij3] )
14452 {
14453  continue;
14454 }
14455 _ij3[0] = ij3; _ij3[1] = -1;
14456 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14457 {
14458 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14459 {
14460  j3valid[iij3]=false; _ij3[1] = iij3; break;
14461 }
14462 }
14463 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14464 {
14465 IkReal evalcond[6];
14466 IkReal x1138=IKsin(j3);
14467 IkReal x1139=IKcos(j3);
14468 IkReal x1140=(gconst20*x1138);
14469 IkReal x1141=(gconst19*x1138);
14470 IkReal x1142=(gconst20*x1139);
14471 IkReal x1143=((1.0)*x1139);
14472 IkReal x1144=(gconst19*x1143);
14473 evalcond[0]=(x1140+(((-1.0)*x1144)));
14474 evalcond[1]=(gconst20+((new_r00*x1139))+((new_r10*x1138)));
14475 evalcond[2]=(x1142+x1141+new_r00);
14476 evalcond[3]=(gconst19+((new_r00*x1138))+(((-1.0)*new_r10*x1143)));
14477 evalcond[4]=((((-1.0)*x1141))+(((-1.0)*x1142)));
14478 evalcond[5]=(x1140+(((-1.0)*x1144))+new_r10);
14479 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14480 {
14481 continue;
14482 }
14483 }
14484 
14485 {
14486 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14487 vinfos[0].jointtype = 1;
14488 vinfos[0].foffset = j0;
14489 vinfos[0].indices[0] = _ij0[0];
14490 vinfos[0].indices[1] = _ij0[1];
14491 vinfos[0].maxsolutions = _nj0;
14492 vinfos[1].jointtype = 1;
14493 vinfos[1].foffset = j1;
14494 vinfos[1].indices[0] = _ij1[0];
14495 vinfos[1].indices[1] = _ij1[1];
14496 vinfos[1].maxsolutions = _nj1;
14497 vinfos[2].jointtype = 1;
14498 vinfos[2].foffset = j2;
14499 vinfos[2].indices[0] = _ij2[0];
14500 vinfos[2].indices[1] = _ij2[1];
14501 vinfos[2].maxsolutions = _nj2;
14502 vinfos[3].jointtype = 1;
14503 vinfos[3].foffset = j3;
14504 vinfos[3].indices[0] = _ij3[0];
14505 vinfos[3].indices[1] = _ij3[1];
14506 vinfos[3].maxsolutions = _nj3;
14507 vinfos[4].jointtype = 1;
14508 vinfos[4].foffset = j4;
14509 vinfos[4].indices[0] = _ij4[0];
14510 vinfos[4].indices[1] = _ij4[1];
14511 vinfos[4].maxsolutions = _nj4;
14512 vinfos[5].jointtype = 1;
14513 vinfos[5].foffset = j5;
14514 vinfos[5].indices[0] = _ij5[0];
14515 vinfos[5].indices[1] = _ij5[1];
14516 vinfos[5].maxsolutions = _nj5;
14517 std::vector<int> vfree(0);
14518 solutions.AddSolution(vinfos,vfree);
14519 }
14520 }
14521 }
14522 
14523 }
14524 
14525 }
14526 
14527 }
14528 } while(0);
14529 if( bgotonextstatement )
14530 {
14531 bool bgotonextstatement = true;
14532 do
14533 {
14534 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
14535 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14536 {
14537 bgotonextstatement=false;
14538 {
14539 IkReal j3array[2], cj3array[2], sj3array[2];
14540 bool j3valid[2]={false};
14541 _nj3 = 2;
14542 CheckValue<IkReal> x1145=IKPowWithIntegerCheck(gconst19,-1);
14543 if(!x1145.valid){
14544 continue;
14545 }
14546 sj3array[0]=(new_r11*(x1145.value));
14547 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14548 {
14549  j3valid[0] = j3valid[1] = true;
14550  j3array[0] = IKasin(sj3array[0]);
14551  cj3array[0] = IKcos(j3array[0]);
14552  sj3array[1] = sj3array[0];
14553  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14554  cj3array[1] = -cj3array[0];
14555 }
14556 else if( isnan(sj3array[0]) )
14557 {
14558  // probably any value will work
14559  j3valid[0] = true;
14560  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14561 }
14562 for(int ij3 = 0; ij3 < 2; ++ij3)
14563 {
14564 if( !j3valid[ij3] )
14565 {
14566  continue;
14567 }
14568 _ij3[0] = ij3; _ij3[1] = -1;
14569 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14570 {
14571 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14572 {
14573  j3valid[iij3]=false; _ij3[1] = iij3; break;
14574 }
14575 }
14576 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14577 {
14578 IkReal evalcond[6];
14579 IkReal x1146=IKcos(j3);
14580 IkReal x1147=IKsin(j3);
14581 IkReal x1148=((-1.0)*x1146);
14582 evalcond[0]=(new_r00*x1146);
14583 evalcond[1]=(new_r11*x1148);
14584 evalcond[2]=(gconst19*x1148);
14585 evalcond[3]=(((new_r00*x1147))+gconst19);
14586 evalcond[4]=(((gconst19*x1147))+new_r00);
14587 evalcond[5]=((((-1.0)*gconst19))+((new_r11*x1147)));
14588 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14589 {
14590 continue;
14591 }
14592 }
14593 
14594 {
14595 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14596 vinfos[0].jointtype = 1;
14597 vinfos[0].foffset = j0;
14598 vinfos[0].indices[0] = _ij0[0];
14599 vinfos[0].indices[1] = _ij0[1];
14600 vinfos[0].maxsolutions = _nj0;
14601 vinfos[1].jointtype = 1;
14602 vinfos[1].foffset = j1;
14603 vinfos[1].indices[0] = _ij1[0];
14604 vinfos[1].indices[1] = _ij1[1];
14605 vinfos[1].maxsolutions = _nj1;
14606 vinfos[2].jointtype = 1;
14607 vinfos[2].foffset = j2;
14608 vinfos[2].indices[0] = _ij2[0];
14609 vinfos[2].indices[1] = _ij2[1];
14610 vinfos[2].maxsolutions = _nj2;
14611 vinfos[3].jointtype = 1;
14612 vinfos[3].foffset = j3;
14613 vinfos[3].indices[0] = _ij3[0];
14614 vinfos[3].indices[1] = _ij3[1];
14615 vinfos[3].maxsolutions = _nj3;
14616 vinfos[4].jointtype = 1;
14617 vinfos[4].foffset = j4;
14618 vinfos[4].indices[0] = _ij4[0];
14619 vinfos[4].indices[1] = _ij4[1];
14620 vinfos[4].maxsolutions = _nj4;
14621 vinfos[5].jointtype = 1;
14622 vinfos[5].foffset = j5;
14623 vinfos[5].indices[0] = _ij5[0];
14624 vinfos[5].indices[1] = _ij5[1];
14625 vinfos[5].maxsolutions = _nj5;
14626 std::vector<int> vfree(0);
14627 solutions.AddSolution(vinfos,vfree);
14628 }
14629 }
14630 }
14631 
14632 }
14633 } while(0);
14634 if( bgotonextstatement )
14635 {
14636 bool bgotonextstatement = true;
14637 do
14638 {
14639 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
14640 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14641 {
14642 bgotonextstatement=false;
14643 {
14644 IkReal j3eval[1];
14645 CheckValue<IkReal> x1150 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14646 if(!x1150.valid){
14647 continue;
14648 }
14649 IkReal x1149=((-1.0)*(x1150.value));
14650 sj4=0;
14651 cj4=-1.0;
14652 j4=3.14159265358979;
14653 sj5=gconst19;
14654 cj5=gconst20;
14655 j5=x1149;
14656 new_r11=0;
14657 new_r10=0;
14658 new_r22=0;
14659 new_r02=0;
14660 IkReal gconst18=x1149;
14661 IkReal x1151 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14662 if(IKabs(x1151)==0){
14663 continue;
14664 }
14665 IkReal gconst19=(new_r00*(pow(x1151,-0.5)));
14666 IkReal gconst20=0;
14667 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
14668 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14669 {
14670 {
14671 IkReal j3eval[1];
14672 CheckValue<IkReal> x1153 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14673 if(!x1153.valid){
14674 continue;
14675 }
14676 IkReal x1152=((-1.0)*(x1153.value));
14677 sj4=0;
14678 cj4=-1.0;
14679 j4=3.14159265358979;
14680 sj5=gconst19;
14681 cj5=gconst20;
14682 j5=x1152;
14683 new_r11=0;
14684 new_r10=0;
14685 new_r22=0;
14686 new_r02=0;
14687 IkReal gconst18=x1152;
14688 IkReal x1154 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14689 if(IKabs(x1154)==0){
14690 continue;
14691 }
14692 IkReal gconst19=(new_r00*(pow(x1154,-0.5)));
14693 IkReal gconst20=0;
14694 j3eval[0]=new_r00;
14695 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14696 {
14697 {
14698 IkReal j3eval[2];
14699 CheckValue<IkReal> x1156 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14700 if(!x1156.valid){
14701 continue;
14702 }
14703 IkReal x1155=((-1.0)*(x1156.value));
14704 sj4=0;
14705 cj4=-1.0;
14706 j4=3.14159265358979;
14707 sj5=gconst19;
14708 cj5=gconst20;
14709 j5=x1155;
14710 new_r11=0;
14711 new_r10=0;
14712 new_r22=0;
14713 new_r02=0;
14714 IkReal gconst18=x1155;
14715 IkReal x1157 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14716 if(IKabs(x1157)==0){
14717 continue;
14718 }
14719 IkReal gconst19=(new_r00*(pow(x1157,-0.5)));
14720 IkReal gconst20=0;
14721 j3eval[0]=new_r00;
14722 j3eval[1]=new_r01;
14723 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
14724 {
14725 continue; // 3 cases reached
14726 
14727 } else
14728 {
14729 {
14730 IkReal j3array[1], cj3array[1], sj3array[1];
14731 bool j3valid[1]={false};
14732 _nj3 = 1;
14733 CheckValue<IkReal> x1158=IKPowWithIntegerCheck(new_r00,-1);
14734 if(!x1158.valid){
14735 continue;
14736 }
14737 CheckValue<IkReal> x1159=IKPowWithIntegerCheck(new_r01,-1);
14738 if(!x1159.valid){
14739 continue;
14740 }
14741 if( IKabs(((-1.0)*gconst19*(x1158.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst19*(x1159.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1158.value)))+IKsqr((gconst19*(x1159.value)))-1) <= IKFAST_SINCOS_THRESH )
14742  continue;
14743 j3array[0]=IKatan2(((-1.0)*gconst19*(x1158.value)), (gconst19*(x1159.value)));
14744 sj3array[0]=IKsin(j3array[0]);
14745 cj3array[0]=IKcos(j3array[0]);
14746 if( j3array[0] > IKPI )
14747 {
14748  j3array[0]-=IK2PI;
14749 }
14750 else if( j3array[0] < -IKPI )
14751 { j3array[0]+=IK2PI;
14752 }
14753 j3valid[0] = true;
14754 for(int ij3 = 0; ij3 < 1; ++ij3)
14755 {
14756 if( !j3valid[ij3] )
14757 {
14758  continue;
14759 }
14760 _ij3[0] = ij3; _ij3[1] = -1;
14761 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14762 {
14763 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14764 {
14765  j3valid[iij3]=false; _ij3[1] = iij3; break;
14766 }
14767 }
14768 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14769 {
14770 IkReal evalcond[8];
14771 IkReal x1160=IKsin(j3);
14772 IkReal x1161=IKcos(j3);
14773 IkReal x1162=((1.0)*gconst19);
14774 IkReal x1163=(gconst19*x1160);
14775 evalcond[0]=(new_r01*x1160);
14776 evalcond[1]=(new_r00*x1161);
14777 evalcond[2]=((-1.0)*x1163);
14778 evalcond[3]=((-1.0)*gconst19*x1161);
14779 evalcond[4]=(gconst19+((new_r00*x1160)));
14780 evalcond[5]=(x1163+new_r00);
14781 evalcond[6]=(new_r01+(((-1.0)*x1161*x1162)));
14782 evalcond[7]=(((new_r01*x1161))+(((-1.0)*x1162)));
14783 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14784 {
14785 continue;
14786 }
14787 }
14788 
14789 {
14790 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14791 vinfos[0].jointtype = 1;
14792 vinfos[0].foffset = j0;
14793 vinfos[0].indices[0] = _ij0[0];
14794 vinfos[0].indices[1] = _ij0[1];
14795 vinfos[0].maxsolutions = _nj0;
14796 vinfos[1].jointtype = 1;
14797 vinfos[1].foffset = j1;
14798 vinfos[1].indices[0] = _ij1[0];
14799 vinfos[1].indices[1] = _ij1[1];
14800 vinfos[1].maxsolutions = _nj1;
14801 vinfos[2].jointtype = 1;
14802 vinfos[2].foffset = j2;
14803 vinfos[2].indices[0] = _ij2[0];
14804 vinfos[2].indices[1] = _ij2[1];
14805 vinfos[2].maxsolutions = _nj2;
14806 vinfos[3].jointtype = 1;
14807 vinfos[3].foffset = j3;
14808 vinfos[3].indices[0] = _ij3[0];
14809 vinfos[3].indices[1] = _ij3[1];
14810 vinfos[3].maxsolutions = _nj3;
14811 vinfos[4].jointtype = 1;
14812 vinfos[4].foffset = j4;
14813 vinfos[4].indices[0] = _ij4[0];
14814 vinfos[4].indices[1] = _ij4[1];
14815 vinfos[4].maxsolutions = _nj4;
14816 vinfos[5].jointtype = 1;
14817 vinfos[5].foffset = j5;
14818 vinfos[5].indices[0] = _ij5[0];
14819 vinfos[5].indices[1] = _ij5[1];
14820 vinfos[5].maxsolutions = _nj5;
14821 std::vector<int> vfree(0);
14822 solutions.AddSolution(vinfos,vfree);
14823 }
14824 }
14825 }
14826 
14827 }
14828 
14829 }
14830 
14831 } else
14832 {
14833 {
14834 IkReal j3array[1], cj3array[1], sj3array[1];
14835 bool j3valid[1]={false};
14836 _nj3 = 1;
14837 CheckValue<IkReal> x1164=IKPowWithIntegerCheck(new_r00,-1);
14838 if(!x1164.valid){
14839 continue;
14840 }
14841 CheckValue<IkReal> x1165=IKPowWithIntegerCheck(gconst19,-1);
14842 if(!x1165.valid){
14843 continue;
14844 }
14845 if( IKabs(((-1.0)*gconst19*(x1164.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1165.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1164.value)))+IKsqr((new_r01*(x1165.value)))-1) <= IKFAST_SINCOS_THRESH )
14846  continue;
14847 j3array[0]=IKatan2(((-1.0)*gconst19*(x1164.value)), (new_r01*(x1165.value)));
14848 sj3array[0]=IKsin(j3array[0]);
14849 cj3array[0]=IKcos(j3array[0]);
14850 if( j3array[0] > IKPI )
14851 {
14852  j3array[0]-=IK2PI;
14853 }
14854 else if( j3array[0] < -IKPI )
14855 { j3array[0]+=IK2PI;
14856 }
14857 j3valid[0] = true;
14858 for(int ij3 = 0; ij3 < 1; ++ij3)
14859 {
14860 if( !j3valid[ij3] )
14861 {
14862  continue;
14863 }
14864 _ij3[0] = ij3; _ij3[1] = -1;
14865 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14866 {
14867 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14868 {
14869  j3valid[iij3]=false; _ij3[1] = iij3; break;
14870 }
14871 }
14872 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14873 {
14874 IkReal evalcond[8];
14875 IkReal x1166=IKsin(j3);
14876 IkReal x1167=IKcos(j3);
14877 IkReal x1168=((1.0)*gconst19);
14878 IkReal x1169=(gconst19*x1166);
14879 evalcond[0]=(new_r01*x1166);
14880 evalcond[1]=(new_r00*x1167);
14881 evalcond[2]=((-1.0)*x1169);
14882 evalcond[3]=((-1.0)*gconst19*x1167);
14883 evalcond[4]=(gconst19+((new_r00*x1166)));
14884 evalcond[5]=(x1169+new_r00);
14885 evalcond[6]=((((-1.0)*x1167*x1168))+new_r01);
14886 evalcond[7]=(((new_r01*x1167))+(((-1.0)*x1168)));
14887 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14888 {
14889 continue;
14890 }
14891 }
14892 
14893 {
14894 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14895 vinfos[0].jointtype = 1;
14896 vinfos[0].foffset = j0;
14897 vinfos[0].indices[0] = _ij0[0];
14898 vinfos[0].indices[1] = _ij0[1];
14899 vinfos[0].maxsolutions = _nj0;
14900 vinfos[1].jointtype = 1;
14901 vinfos[1].foffset = j1;
14902 vinfos[1].indices[0] = _ij1[0];
14903 vinfos[1].indices[1] = _ij1[1];
14904 vinfos[1].maxsolutions = _nj1;
14905 vinfos[2].jointtype = 1;
14906 vinfos[2].foffset = j2;
14907 vinfos[2].indices[0] = _ij2[0];
14908 vinfos[2].indices[1] = _ij2[1];
14909 vinfos[2].maxsolutions = _nj2;
14910 vinfos[3].jointtype = 1;
14911 vinfos[3].foffset = j3;
14912 vinfos[3].indices[0] = _ij3[0];
14913 vinfos[3].indices[1] = _ij3[1];
14914 vinfos[3].maxsolutions = _nj3;
14915 vinfos[4].jointtype = 1;
14916 vinfos[4].foffset = j4;
14917 vinfos[4].indices[0] = _ij4[0];
14918 vinfos[4].indices[1] = _ij4[1];
14919 vinfos[4].maxsolutions = _nj4;
14920 vinfos[5].jointtype = 1;
14921 vinfos[5].foffset = j5;
14922 vinfos[5].indices[0] = _ij5[0];
14923 vinfos[5].indices[1] = _ij5[1];
14924 vinfos[5].maxsolutions = _nj5;
14925 std::vector<int> vfree(0);
14926 solutions.AddSolution(vinfos,vfree);
14927 }
14928 }
14929 }
14930 
14931 }
14932 
14933 }
14934 
14935 } else
14936 {
14937 {
14938 IkReal j3array[1], cj3array[1], sj3array[1];
14939 bool j3valid[1]={false};
14940 _nj3 = 1;
14941 CheckValue<IkReal> x1170=IKPowWithIntegerCheck(IKsign(gconst19),-1);
14942 if(!x1170.valid){
14943 continue;
14944 }
14945 CheckValue<IkReal> x1171 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
14946 if(!x1171.valid){
14947 continue;
14948 }
14949 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1170.value)))+(x1171.value));
14950 sj3array[0]=IKsin(j3array[0]);
14951 cj3array[0]=IKcos(j3array[0]);
14952 if( j3array[0] > IKPI )
14953 {
14954  j3array[0]-=IK2PI;
14955 }
14956 else if( j3array[0] < -IKPI )
14957 { j3array[0]+=IK2PI;
14958 }
14959 j3valid[0] = true;
14960 for(int ij3 = 0; ij3 < 1; ++ij3)
14961 {
14962 if( !j3valid[ij3] )
14963 {
14964  continue;
14965 }
14966 _ij3[0] = ij3; _ij3[1] = -1;
14967 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14968 {
14969 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14970 {
14971  j3valid[iij3]=false; _ij3[1] = iij3; break;
14972 }
14973 }
14974 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14975 {
14976 IkReal evalcond[8];
14977 IkReal x1172=IKsin(j3);
14978 IkReal x1173=IKcos(j3);
14979 IkReal x1174=((1.0)*gconst19);
14980 IkReal x1175=(gconst19*x1172);
14981 evalcond[0]=(new_r01*x1172);
14982 evalcond[1]=(new_r00*x1173);
14983 evalcond[2]=((-1.0)*x1175);
14984 evalcond[3]=((-1.0)*gconst19*x1173);
14985 evalcond[4]=(gconst19+((new_r00*x1172)));
14986 evalcond[5]=(x1175+new_r00);
14987 evalcond[6]=((((-1.0)*x1173*x1174))+new_r01);
14988 evalcond[7]=((((-1.0)*x1174))+((new_r01*x1173)));
14989 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14990 {
14991 continue;
14992 }
14993 }
14994 
14995 {
14996 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14997 vinfos[0].jointtype = 1;
14998 vinfos[0].foffset = j0;
14999 vinfos[0].indices[0] = _ij0[0];
15000 vinfos[0].indices[1] = _ij0[1];
15001 vinfos[0].maxsolutions = _nj0;
15002 vinfos[1].jointtype = 1;
15003 vinfos[1].foffset = j1;
15004 vinfos[1].indices[0] = _ij1[0];
15005 vinfos[1].indices[1] = _ij1[1];
15006 vinfos[1].maxsolutions = _nj1;
15007 vinfos[2].jointtype = 1;
15008 vinfos[2].foffset = j2;
15009 vinfos[2].indices[0] = _ij2[0];
15010 vinfos[2].indices[1] = _ij2[1];
15011 vinfos[2].maxsolutions = _nj2;
15012 vinfos[3].jointtype = 1;
15013 vinfos[3].foffset = j3;
15014 vinfos[3].indices[0] = _ij3[0];
15015 vinfos[3].indices[1] = _ij3[1];
15016 vinfos[3].maxsolutions = _nj3;
15017 vinfos[4].jointtype = 1;
15018 vinfos[4].foffset = j4;
15019 vinfos[4].indices[0] = _ij4[0];
15020 vinfos[4].indices[1] = _ij4[1];
15021 vinfos[4].maxsolutions = _nj4;
15022 vinfos[5].jointtype = 1;
15023 vinfos[5].foffset = j5;
15024 vinfos[5].indices[0] = _ij5[0];
15025 vinfos[5].indices[1] = _ij5[1];
15026 vinfos[5].maxsolutions = _nj5;
15027 std::vector<int> vfree(0);
15028 solutions.AddSolution(vinfos,vfree);
15029 }
15030 }
15031 }
15032 
15033 }
15034 
15035 }
15036 
15037 }
15038 } while(0);
15039 if( bgotonextstatement )
15040 {
15041 bool bgotonextstatement = true;
15042 do
15043 {
15044 evalcond[0]=IKabs(new_r10);
15045 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15046 {
15047 bgotonextstatement=false;
15048 {
15049 IkReal j3eval[1];
15050 CheckValue<IkReal> x1177 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15051 if(!x1177.valid){
15052 continue;
15053 }
15054 IkReal x1176=((-1.0)*(x1177.value));
15055 sj4=0;
15056 cj4=-1.0;
15057 j4=3.14159265358979;
15058 sj5=gconst19;
15059 cj5=gconst20;
15060 j5=x1176;
15061 new_r10=0;
15062 IkReal gconst18=x1176;
15063 IkReal x1178 = new_r00*new_r00;
15064 if(IKabs(x1178)==0){
15065 continue;
15066 }
15067 IkReal gconst19=(new_r00*(pow(x1178,-0.5)));
15068 IkReal gconst20=0;
15069 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
15070 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15071 {
15072 {
15073 IkReal j3eval[1];
15074 CheckValue<IkReal> x1180 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15075 if(!x1180.valid){
15076 continue;
15077 }
15078 IkReal x1179=((-1.0)*(x1180.value));
15079 sj4=0;
15080 cj4=-1.0;
15081 j4=3.14159265358979;
15082 sj5=gconst19;
15083 cj5=gconst20;
15084 j5=x1179;
15085 new_r10=0;
15086 IkReal gconst18=x1179;
15087 IkReal x1181 = new_r00*new_r00;
15088 if(IKabs(x1181)==0){
15089 continue;
15090 }
15091 IkReal gconst19=(new_r00*(pow(x1181,-0.5)));
15092 IkReal gconst20=0;
15093 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
15094 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15095 {
15096 {
15097 IkReal j3eval[1];
15098 CheckValue<IkReal> x1183 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15099 if(!x1183.valid){
15100 continue;
15101 }
15102 IkReal x1182=((-1.0)*(x1183.value));
15103 sj4=0;
15104 cj4=-1.0;
15105 j4=3.14159265358979;
15106 sj5=gconst19;
15107 cj5=gconst20;
15108 j5=x1182;
15109 new_r10=0;
15110 IkReal gconst18=x1182;
15111 IkReal x1184 = new_r00*new_r00;
15112 if(IKabs(x1184)==0){
15113 continue;
15114 }
15115 IkReal gconst19=(new_r00*(pow(x1184,-0.5)));
15116 IkReal gconst20=0;
15117 j3eval[0]=new_r00;
15118 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15119 {
15120 continue; // 3 cases reached
15121 
15122 } else
15123 {
15124 {
15125 IkReal j3array[1], cj3array[1], sj3array[1];
15126 bool j3valid[1]={false};
15127 _nj3 = 1;
15128 CheckValue<IkReal> x1185=IKPowWithIntegerCheck(new_r00,-1);
15129 if(!x1185.valid){
15130 continue;
15131 }
15132 CheckValue<IkReal> x1186=IKPowWithIntegerCheck(gconst19,-1);
15133 if(!x1186.valid){
15134 continue;
15135 }
15136 if( IKabs(((-1.0)*gconst19*(x1185.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1186.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1185.value)))+IKsqr((new_r01*(x1186.value)))-1) <= IKFAST_SINCOS_THRESH )
15137  continue;
15138 j3array[0]=IKatan2(((-1.0)*gconst19*(x1185.value)), (new_r01*(x1186.value)));
15139 sj3array[0]=IKsin(j3array[0]);
15140 cj3array[0]=IKcos(j3array[0]);
15141 if( j3array[0] > IKPI )
15142 {
15143  j3array[0]-=IK2PI;
15144 }
15145 else if( j3array[0] < -IKPI )
15146 { j3array[0]+=IK2PI;
15147 }
15148 j3valid[0] = true;
15149 for(int ij3 = 0; ij3 < 1; ++ij3)
15150 {
15151 if( !j3valid[ij3] )
15152 {
15153  continue;
15154 }
15155 _ij3[0] = ij3; _ij3[1] = -1;
15156 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15157 {
15158 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15159 {
15160  j3valid[iij3]=false; _ij3[1] = iij3; break;
15161 }
15162 }
15163 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15164 {
15165 IkReal evalcond[8];
15166 IkReal x1187=IKcos(j3);
15167 IkReal x1188=IKsin(j3);
15168 IkReal x1189=(gconst19*x1188);
15169 IkReal x1190=((1.0)*x1187);
15170 evalcond[0]=(new_r00*x1187);
15171 evalcond[1]=((-1.0)*gconst19*x1187);
15172 evalcond[2]=(gconst19+((new_r00*x1188)));
15173 evalcond[3]=(x1189+new_r00);
15174 evalcond[4]=((((-1.0)*gconst19*x1190))+new_r01);
15175 evalcond[5]=(new_r11+(((-1.0)*x1189)));
15176 evalcond[6]=((((-1.0)*new_r11*x1190))+((new_r01*x1188)));
15177 evalcond[7]=(((new_r01*x1187))+(((-1.0)*gconst19))+((new_r11*x1188)));
15178 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15179 {
15180 continue;
15181 }
15182 }
15183 
15184 {
15185 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15186 vinfos[0].jointtype = 1;
15187 vinfos[0].foffset = j0;
15188 vinfos[0].indices[0] = _ij0[0];
15189 vinfos[0].indices[1] = _ij0[1];
15190 vinfos[0].maxsolutions = _nj0;
15191 vinfos[1].jointtype = 1;
15192 vinfos[1].foffset = j1;
15193 vinfos[1].indices[0] = _ij1[0];
15194 vinfos[1].indices[1] = _ij1[1];
15195 vinfos[1].maxsolutions = _nj1;
15196 vinfos[2].jointtype = 1;
15197 vinfos[2].foffset = j2;
15198 vinfos[2].indices[0] = _ij2[0];
15199 vinfos[2].indices[1] = _ij2[1];
15200 vinfos[2].maxsolutions = _nj2;
15201 vinfos[3].jointtype = 1;
15202 vinfos[3].foffset = j3;
15203 vinfos[3].indices[0] = _ij3[0];
15204 vinfos[3].indices[1] = _ij3[1];
15205 vinfos[3].maxsolutions = _nj3;
15206 vinfos[4].jointtype = 1;
15207 vinfos[4].foffset = j4;
15208 vinfos[4].indices[0] = _ij4[0];
15209 vinfos[4].indices[1] = _ij4[1];
15210 vinfos[4].maxsolutions = _nj4;
15211 vinfos[5].jointtype = 1;
15212 vinfos[5].foffset = j5;
15213 vinfos[5].indices[0] = _ij5[0];
15214 vinfos[5].indices[1] = _ij5[1];
15215 vinfos[5].maxsolutions = _nj5;
15216 std::vector<int> vfree(0);
15217 solutions.AddSolution(vinfos,vfree);
15218 }
15219 }
15220 }
15221 
15222 }
15223 
15224 }
15225 
15226 } else
15227 {
15228 {
15229 IkReal j3array[1], cj3array[1], sj3array[1];
15230 bool j3valid[1]={false};
15231 _nj3 = 1;
15232 CheckValue<IkReal> x1191=IKPowWithIntegerCheck(IKsign(gconst19),-1);
15233 if(!x1191.valid){
15234 continue;
15235 }
15236 CheckValue<IkReal> x1192 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15237 if(!x1192.valid){
15238 continue;
15239 }
15240 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1191.value)))+(x1192.value));
15241 sj3array[0]=IKsin(j3array[0]);
15242 cj3array[0]=IKcos(j3array[0]);
15243 if( j3array[0] > IKPI )
15244 {
15245  j3array[0]-=IK2PI;
15246 }
15247 else if( j3array[0] < -IKPI )
15248 { j3array[0]+=IK2PI;
15249 }
15250 j3valid[0] = true;
15251 for(int ij3 = 0; ij3 < 1; ++ij3)
15252 {
15253 if( !j3valid[ij3] )
15254 {
15255  continue;
15256 }
15257 _ij3[0] = ij3; _ij3[1] = -1;
15258 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15259 {
15260 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15261 {
15262  j3valid[iij3]=false; _ij3[1] = iij3; break;
15263 }
15264 }
15265 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15266 {
15267 IkReal evalcond[8];
15268 IkReal x1193=IKcos(j3);
15269 IkReal x1194=IKsin(j3);
15270 IkReal x1195=(gconst19*x1194);
15271 IkReal x1196=((1.0)*x1193);
15272 evalcond[0]=(new_r00*x1193);
15273 evalcond[1]=((-1.0)*gconst19*x1193);
15274 evalcond[2]=(gconst19+((new_r00*x1194)));
15275 evalcond[3]=(x1195+new_r00);
15276 evalcond[4]=((((-1.0)*gconst19*x1196))+new_r01);
15277 evalcond[5]=(new_r11+(((-1.0)*x1195)));
15278 evalcond[6]=((((-1.0)*new_r11*x1196))+((new_r01*x1194)));
15279 evalcond[7]=(((new_r11*x1194))+(((-1.0)*gconst19))+((new_r01*x1193)));
15280 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15281 {
15282 continue;
15283 }
15284 }
15285 
15286 {
15287 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15288 vinfos[0].jointtype = 1;
15289 vinfos[0].foffset = j0;
15290 vinfos[0].indices[0] = _ij0[0];
15291 vinfos[0].indices[1] = _ij0[1];
15292 vinfos[0].maxsolutions = _nj0;
15293 vinfos[1].jointtype = 1;
15294 vinfos[1].foffset = j1;
15295 vinfos[1].indices[0] = _ij1[0];
15296 vinfos[1].indices[1] = _ij1[1];
15297 vinfos[1].maxsolutions = _nj1;
15298 vinfos[2].jointtype = 1;
15299 vinfos[2].foffset = j2;
15300 vinfos[2].indices[0] = _ij2[0];
15301 vinfos[2].indices[1] = _ij2[1];
15302 vinfos[2].maxsolutions = _nj2;
15303 vinfos[3].jointtype = 1;
15304 vinfos[3].foffset = j3;
15305 vinfos[3].indices[0] = _ij3[0];
15306 vinfos[3].indices[1] = _ij3[1];
15307 vinfos[3].maxsolutions = _nj3;
15308 vinfos[4].jointtype = 1;
15309 vinfos[4].foffset = j4;
15310 vinfos[4].indices[0] = _ij4[0];
15311 vinfos[4].indices[1] = _ij4[1];
15312 vinfos[4].maxsolutions = _nj4;
15313 vinfos[5].jointtype = 1;
15314 vinfos[5].foffset = j5;
15315 vinfos[5].indices[0] = _ij5[0];
15316 vinfos[5].indices[1] = _ij5[1];
15317 vinfos[5].maxsolutions = _nj5;
15318 std::vector<int> vfree(0);
15319 solutions.AddSolution(vinfos,vfree);
15320 }
15321 }
15322 }
15323 
15324 }
15325 
15326 }
15327 
15328 } else
15329 {
15330 {
15331 IkReal j3array[1], cj3array[1], sj3array[1];
15332 bool j3valid[1]={false};
15333 _nj3 = 1;
15334 CheckValue<IkReal> x1197=IKPowWithIntegerCheck(IKsign(gconst19),-1);
15335 if(!x1197.valid){
15336 continue;
15337 }
15338 CheckValue<IkReal> x1198 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15339 if(!x1198.valid){
15340 continue;
15341 }
15342 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1197.value)))+(x1198.value));
15343 sj3array[0]=IKsin(j3array[0]);
15344 cj3array[0]=IKcos(j3array[0]);
15345 if( j3array[0] > IKPI )
15346 {
15347  j3array[0]-=IK2PI;
15348 }
15349 else if( j3array[0] < -IKPI )
15350 { j3array[0]+=IK2PI;
15351 }
15352 j3valid[0] = true;
15353 for(int ij3 = 0; ij3 < 1; ++ij3)
15354 {
15355 if( !j3valid[ij3] )
15356 {
15357  continue;
15358 }
15359 _ij3[0] = ij3; _ij3[1] = -1;
15360 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15361 {
15362 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15363 {
15364  j3valid[iij3]=false; _ij3[1] = iij3; break;
15365 }
15366 }
15367 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15368 {
15369 IkReal evalcond[8];
15370 IkReal x1199=IKcos(j3);
15371 IkReal x1200=IKsin(j3);
15372 IkReal x1201=(gconst19*x1200);
15373 IkReal x1202=((1.0)*x1199);
15374 evalcond[0]=(new_r00*x1199);
15375 evalcond[1]=((-1.0)*gconst19*x1199);
15376 evalcond[2]=(((new_r00*x1200))+gconst19);
15377 evalcond[3]=(x1201+new_r00);
15378 evalcond[4]=((((-1.0)*gconst19*x1202))+new_r01);
15379 evalcond[5]=((((-1.0)*x1201))+new_r11);
15380 evalcond[6]=(((new_r01*x1200))+(((-1.0)*new_r11*x1202)));
15381 evalcond[7]=(((new_r11*x1200))+(((-1.0)*gconst19))+((new_r01*x1199)));
15382 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15383 {
15384 continue;
15385 }
15386 }
15387 
15388 {
15389 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15390 vinfos[0].jointtype = 1;
15391 vinfos[0].foffset = j0;
15392 vinfos[0].indices[0] = _ij0[0];
15393 vinfos[0].indices[1] = _ij0[1];
15394 vinfos[0].maxsolutions = _nj0;
15395 vinfos[1].jointtype = 1;
15396 vinfos[1].foffset = j1;
15397 vinfos[1].indices[0] = _ij1[0];
15398 vinfos[1].indices[1] = _ij1[1];
15399 vinfos[1].maxsolutions = _nj1;
15400 vinfos[2].jointtype = 1;
15401 vinfos[2].foffset = j2;
15402 vinfos[2].indices[0] = _ij2[0];
15403 vinfos[2].indices[1] = _ij2[1];
15404 vinfos[2].maxsolutions = _nj2;
15405 vinfos[3].jointtype = 1;
15406 vinfos[3].foffset = j3;
15407 vinfos[3].indices[0] = _ij3[0];
15408 vinfos[3].indices[1] = _ij3[1];
15409 vinfos[3].maxsolutions = _nj3;
15410 vinfos[4].jointtype = 1;
15411 vinfos[4].foffset = j4;
15412 vinfos[4].indices[0] = _ij4[0];
15413 vinfos[4].indices[1] = _ij4[1];
15414 vinfos[4].maxsolutions = _nj4;
15415 vinfos[5].jointtype = 1;
15416 vinfos[5].foffset = j5;
15417 vinfos[5].indices[0] = _ij5[0];
15418 vinfos[5].indices[1] = _ij5[1];
15419 vinfos[5].maxsolutions = _nj5;
15420 std::vector<int> vfree(0);
15421 solutions.AddSolution(vinfos,vfree);
15422 }
15423 }
15424 }
15425 
15426 }
15427 
15428 }
15429 
15430 }
15431 } while(0);
15432 if( bgotonextstatement )
15433 {
15434 bool bgotonextstatement = true;
15435 do
15436 {
15437 if( 1 )
15438 {
15439 bgotonextstatement=false;
15440 continue; // branch miss [j3]
15441 
15442 }
15443 } while(0);
15444 if( bgotonextstatement )
15445 {
15446 }
15447 }
15448 }
15449 }
15450 }
15451 }
15452 }
15453 
15454 } else
15455 {
15456 {
15457 IkReal j3array[1], cj3array[1], sj3array[1];
15458 bool j3valid[1]={false};
15459 _nj3 = 1;
15460 IkReal x1203=((1.0)*gconst20);
15461 CheckValue<IkReal> x1204=IKPowWithIntegerCheck(IKsign((((gconst19*new_r00))+(((-1.0)*new_r10*x1203)))),-1);
15462 if(!x1204.valid){
15463 continue;
15464 }
15465 CheckValue<IkReal> x1205 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst20*gconst20))),IkReal(((((-1.0)*gconst19*x1203))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
15466 if(!x1205.valid){
15467 continue;
15468 }
15469 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1204.value)))+(x1205.value));
15470 sj3array[0]=IKsin(j3array[0]);
15471 cj3array[0]=IKcos(j3array[0]);
15472 if( j3array[0] > IKPI )
15473 {
15474  j3array[0]-=IK2PI;
15475 }
15476 else if( j3array[0] < -IKPI )
15477 { j3array[0]+=IK2PI;
15478 }
15479 j3valid[0] = true;
15480 for(int ij3 = 0; ij3 < 1; ++ij3)
15481 {
15482 if( !j3valid[ij3] )
15483 {
15484  continue;
15485 }
15486 _ij3[0] = ij3; _ij3[1] = -1;
15487 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15488 {
15489 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15490 {
15491  j3valid[iij3]=false; _ij3[1] = iij3; break;
15492 }
15493 }
15494 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15495 {
15496 IkReal evalcond[8];
15497 IkReal x1206=IKcos(j3);
15498 IkReal x1207=IKsin(j3);
15499 IkReal x1208=(gconst20*x1207);
15500 IkReal x1209=((1.0)*x1206);
15501 IkReal x1210=(gconst19*x1207);
15502 IkReal x1211=(gconst19*x1209);
15503 evalcond[0]=(((new_r00*x1206))+((new_r10*x1207))+gconst20);
15504 evalcond[1]=(((gconst20*x1206))+x1210+new_r00);
15505 evalcond[2]=(((new_r00*x1207))+gconst19+(((-1.0)*new_r10*x1209)));
15506 evalcond[3]=(((new_r01*x1207))+(((-1.0)*new_r11*x1209))+gconst20);
15507 evalcond[4]=(x1208+new_r01+(((-1.0)*x1211)));
15508 evalcond[5]=(x1208+new_r10+(((-1.0)*x1211)));
15509 evalcond[6]=(((new_r11*x1207))+((new_r01*x1206))+(((-1.0)*gconst19)));
15510 evalcond[7]=(new_r11+(((-1.0)*gconst20*x1209))+(((-1.0)*x1210)));
15511 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15512 {
15513 continue;
15514 }
15515 }
15516 
15517 {
15518 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15519 vinfos[0].jointtype = 1;
15520 vinfos[0].foffset = j0;
15521 vinfos[0].indices[0] = _ij0[0];
15522 vinfos[0].indices[1] = _ij0[1];
15523 vinfos[0].maxsolutions = _nj0;
15524 vinfos[1].jointtype = 1;
15525 vinfos[1].foffset = j1;
15526 vinfos[1].indices[0] = _ij1[0];
15527 vinfos[1].indices[1] = _ij1[1];
15528 vinfos[1].maxsolutions = _nj1;
15529 vinfos[2].jointtype = 1;
15530 vinfos[2].foffset = j2;
15531 vinfos[2].indices[0] = _ij2[0];
15532 vinfos[2].indices[1] = _ij2[1];
15533 vinfos[2].maxsolutions = _nj2;
15534 vinfos[3].jointtype = 1;
15535 vinfos[3].foffset = j3;
15536 vinfos[3].indices[0] = _ij3[0];
15537 vinfos[3].indices[1] = _ij3[1];
15538 vinfos[3].maxsolutions = _nj3;
15539 vinfos[4].jointtype = 1;
15540 vinfos[4].foffset = j4;
15541 vinfos[4].indices[0] = _ij4[0];
15542 vinfos[4].indices[1] = _ij4[1];
15543 vinfos[4].maxsolutions = _nj4;
15544 vinfos[5].jointtype = 1;
15545 vinfos[5].foffset = j5;
15546 vinfos[5].indices[0] = _ij5[0];
15547 vinfos[5].indices[1] = _ij5[1];
15548 vinfos[5].maxsolutions = _nj5;
15549 std::vector<int> vfree(0);
15550 solutions.AddSolution(vinfos,vfree);
15551 }
15552 }
15553 }
15554 
15555 }
15556 
15557 }
15558 
15559 } else
15560 {
15561 {
15562 IkReal j3array[1], cj3array[1], sj3array[1];
15563 bool j3valid[1]={false};
15564 _nj3 = 1;
15565 IkReal x1212=((1.0)*gconst19);
15566 CheckValue<IkReal> x1213 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst20*gconst20))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst20*x1212)))),IKFAST_ATAN2_MAGTHRESH);
15567 if(!x1213.valid){
15568 continue;
15569 }
15570 CheckValue<IkReal> x1214=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst20*new_r01))+(((-1.0)*new_r11*x1212)))),-1);
15571 if(!x1214.valid){
15572 continue;
15573 }
15574 j3array[0]=((-1.5707963267949)+(x1213.value)+(((1.5707963267949)*(x1214.value))));
15575 sj3array[0]=IKsin(j3array[0]);
15576 cj3array[0]=IKcos(j3array[0]);
15577 if( j3array[0] > IKPI )
15578 {
15579  j3array[0]-=IK2PI;
15580 }
15581 else if( j3array[0] < -IKPI )
15582 { j3array[0]+=IK2PI;
15583 }
15584 j3valid[0] = true;
15585 for(int ij3 = 0; ij3 < 1; ++ij3)
15586 {
15587 if( !j3valid[ij3] )
15588 {
15589  continue;
15590 }
15591 _ij3[0] = ij3; _ij3[1] = -1;
15592 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15593 {
15594 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15595 {
15596  j3valid[iij3]=false; _ij3[1] = iij3; break;
15597 }
15598 }
15599 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15600 {
15601 IkReal evalcond[8];
15602 IkReal x1215=IKcos(j3);
15603 IkReal x1216=IKsin(j3);
15604 IkReal x1217=(gconst20*x1216);
15605 IkReal x1218=((1.0)*x1215);
15606 IkReal x1219=(gconst19*x1216);
15607 IkReal x1220=(gconst19*x1218);
15608 evalcond[0]=(((new_r10*x1216))+gconst20+((new_r00*x1215)));
15609 evalcond[1]=(x1219+((gconst20*x1215))+new_r00);
15610 evalcond[2]=(gconst19+((new_r00*x1216))+(((-1.0)*new_r10*x1218)));
15611 evalcond[3]=(gconst20+((new_r01*x1216))+(((-1.0)*new_r11*x1218)));
15612 evalcond[4]=(x1217+(((-1.0)*x1220))+new_r01);
15613 evalcond[5]=(x1217+(((-1.0)*x1220))+new_r10);
15614 evalcond[6]=(((new_r11*x1216))+((new_r01*x1215))+(((-1.0)*gconst19)));
15615 evalcond[7]=((((-1.0)*gconst20*x1218))+new_r11+(((-1.0)*x1219)));
15616 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15617 {
15618 continue;
15619 }
15620 }
15621 
15622 {
15623 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15624 vinfos[0].jointtype = 1;
15625 vinfos[0].foffset = j0;
15626 vinfos[0].indices[0] = _ij0[0];
15627 vinfos[0].indices[1] = _ij0[1];
15628 vinfos[0].maxsolutions = _nj0;
15629 vinfos[1].jointtype = 1;
15630 vinfos[1].foffset = j1;
15631 vinfos[1].indices[0] = _ij1[0];
15632 vinfos[1].indices[1] = _ij1[1];
15633 vinfos[1].maxsolutions = _nj1;
15634 vinfos[2].jointtype = 1;
15635 vinfos[2].foffset = j2;
15636 vinfos[2].indices[0] = _ij2[0];
15637 vinfos[2].indices[1] = _ij2[1];
15638 vinfos[2].maxsolutions = _nj2;
15639 vinfos[3].jointtype = 1;
15640 vinfos[3].foffset = j3;
15641 vinfos[3].indices[0] = _ij3[0];
15642 vinfos[3].indices[1] = _ij3[1];
15643 vinfos[3].maxsolutions = _nj3;
15644 vinfos[4].jointtype = 1;
15645 vinfos[4].foffset = j4;
15646 vinfos[4].indices[0] = _ij4[0];
15647 vinfos[4].indices[1] = _ij4[1];
15648 vinfos[4].maxsolutions = _nj4;
15649 vinfos[5].jointtype = 1;
15650 vinfos[5].foffset = j5;
15651 vinfos[5].indices[0] = _ij5[0];
15652 vinfos[5].indices[1] = _ij5[1];
15653 vinfos[5].maxsolutions = _nj5;
15654 std::vector<int> vfree(0);
15655 solutions.AddSolution(vinfos,vfree);
15656 }
15657 }
15658 }
15659 
15660 }
15661 
15662 }
15663 
15664 } else
15665 {
15666 {
15667 IkReal j3array[1], cj3array[1], sj3array[1];
15668 bool j3valid[1]={false};
15669 _nj3 = 1;
15670 IkReal x1221=((1.0)*new_r10);
15671 CheckValue<IkReal> x1222=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1221)))),-1);
15672 if(!x1222.valid){
15673 continue;
15674 }
15675 CheckValue<IkReal> x1223 = IKatan2WithCheck(IkReal((((gconst20*new_r00))+((gconst20*new_r11)))),IkReal((((gconst20*new_r01))+(((-1.0)*gconst20*x1221)))),IKFAST_ATAN2_MAGTHRESH);
15676 if(!x1223.valid){
15677 continue;
15678 }
15679 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1222.value)))+(x1223.value));
15680 sj3array[0]=IKsin(j3array[0]);
15681 cj3array[0]=IKcos(j3array[0]);
15682 if( j3array[0] > IKPI )
15683 {
15684  j3array[0]-=IK2PI;
15685 }
15686 else if( j3array[0] < -IKPI )
15687 { j3array[0]+=IK2PI;
15688 }
15689 j3valid[0] = true;
15690 for(int ij3 = 0; ij3 < 1; ++ij3)
15691 {
15692 if( !j3valid[ij3] )
15693 {
15694  continue;
15695 }
15696 _ij3[0] = ij3; _ij3[1] = -1;
15697 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15698 {
15699 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15700 {
15701  j3valid[iij3]=false; _ij3[1] = iij3; break;
15702 }
15703 }
15704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15705 {
15706 IkReal evalcond[8];
15707 IkReal x1224=IKcos(j3);
15708 IkReal x1225=IKsin(j3);
15709 IkReal x1226=(gconst20*x1225);
15710 IkReal x1227=((1.0)*x1224);
15711 IkReal x1228=(gconst19*x1225);
15712 IkReal x1229=(gconst19*x1227);
15713 evalcond[0]=(gconst20+((new_r00*x1224))+((new_r10*x1225)));
15714 evalcond[1]=(x1228+((gconst20*x1224))+new_r00);
15715 evalcond[2]=(gconst19+((new_r00*x1225))+(((-1.0)*new_r10*x1227)));
15716 evalcond[3]=(gconst20+((new_r01*x1225))+(((-1.0)*new_r11*x1227)));
15717 evalcond[4]=(x1226+(((-1.0)*x1229))+new_r01);
15718 evalcond[5]=(x1226+(((-1.0)*x1229))+new_r10);
15719 evalcond[6]=(((new_r11*x1225))+((new_r01*x1224))+(((-1.0)*gconst19)));
15720 evalcond[7]=((((-1.0)*x1228))+(((-1.0)*gconst20*x1227))+new_r11);
15721 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15722 {
15723 continue;
15724 }
15725 }
15726 
15727 {
15728 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15729 vinfos[0].jointtype = 1;
15730 vinfos[0].foffset = j0;
15731 vinfos[0].indices[0] = _ij0[0];
15732 vinfos[0].indices[1] = _ij0[1];
15733 vinfos[0].maxsolutions = _nj0;
15734 vinfos[1].jointtype = 1;
15735 vinfos[1].foffset = j1;
15736 vinfos[1].indices[0] = _ij1[0];
15737 vinfos[1].indices[1] = _ij1[1];
15738 vinfos[1].maxsolutions = _nj1;
15739 vinfos[2].jointtype = 1;
15740 vinfos[2].foffset = j2;
15741 vinfos[2].indices[0] = _ij2[0];
15742 vinfos[2].indices[1] = _ij2[1];
15743 vinfos[2].maxsolutions = _nj2;
15744 vinfos[3].jointtype = 1;
15745 vinfos[3].foffset = j3;
15746 vinfos[3].indices[0] = _ij3[0];
15747 vinfos[3].indices[1] = _ij3[1];
15748 vinfos[3].maxsolutions = _nj3;
15749 vinfos[4].jointtype = 1;
15750 vinfos[4].foffset = j4;
15751 vinfos[4].indices[0] = _ij4[0];
15752 vinfos[4].indices[1] = _ij4[1];
15753 vinfos[4].maxsolutions = _nj4;
15754 vinfos[5].jointtype = 1;
15755 vinfos[5].foffset = j5;
15756 vinfos[5].indices[0] = _ij5[0];
15757 vinfos[5].indices[1] = _ij5[1];
15758 vinfos[5].maxsolutions = _nj5;
15759 std::vector<int> vfree(0);
15760 solutions.AddSolution(vinfos,vfree);
15761 }
15762 }
15763 }
15764 
15765 }
15766 
15767 }
15768 
15769 }
15770 } while(0);
15771 if( bgotonextstatement )
15772 {
15773 bool bgotonextstatement = true;
15774 do
15775 {
15776 IkReal x1230=((-1.0)*new_r00);
15777 IkReal x1232 = ((new_r10*new_r10)+(new_r00*new_r00));
15778 if(IKabs(x1232)==0){
15779 continue;
15780 }
15781 IkReal x1231=pow(x1232,-0.5);
15782 CheckValue<IkReal> x1233 = IKatan2WithCheck(IkReal(x1230),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15783 if(!x1233.valid){
15784 continue;
15785 }
15786 IkReal gconst21=((3.14159265358979)+(((-1.0)*(x1233.value))));
15787 IkReal gconst22=(x1230*x1231);
15788 IkReal gconst23=((1.0)*new_r10*x1231);
15789 CheckValue<IkReal> x1234 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15790 if(!x1234.valid){
15791 continue;
15792 }
15793 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x1234.value)+j5)))), 6.28318530717959)));
15794 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15795 {
15796 bgotonextstatement=false;
15797 {
15798 IkReal j3eval[3];
15799 IkReal x1235=((-1.0)*new_r00);
15800 CheckValue<IkReal> x1238 = IKatan2WithCheck(IkReal(x1235),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15801 if(!x1238.valid){
15802 continue;
15803 }
15804 IkReal x1236=((1.0)*(x1238.value));
15805 IkReal x1237=x1231;
15806 sj4=0;
15807 cj4=-1.0;
15808 j4=3.14159265358979;
15809 sj5=gconst22;
15810 cj5=gconst23;
15811 j5=((3.14159265)+(((-1.0)*x1236)));
15812 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1236)));
15813 IkReal gconst22=(x1235*x1237);
15814 IkReal gconst23=((1.0)*new_r10*x1237);
15815 IkReal x1239=new_r10*new_r10;
15816 IkReal x1240=(new_r10*new_r11);
15817 IkReal x1241=((((-1.0)*x1240))+(((-1.0)*new_r00*new_r01)));
15818 IkReal x1242=x1231;
15819 IkReal x1243=(new_r10*x1242);
15820 j3eval[0]=x1241;
15821 j3eval[1]=((IKabs((((x1240*x1242))+((new_r00*x1243)))))+(IKabs(((((-1.0)*x1239*x1242))+((new_r01*x1243))))));
15822 j3eval[2]=IKsign(x1241);
15823 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
15824 {
15825 {
15826 IkReal j3eval[1];
15827 IkReal x1244=((-1.0)*new_r00);
15828 CheckValue<IkReal> x1247 = IKatan2WithCheck(IkReal(x1244),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15829 if(!x1247.valid){
15830 continue;
15831 }
15832 IkReal x1245=((1.0)*(x1247.value));
15833 IkReal x1246=x1231;
15834 sj4=0;
15835 cj4=-1.0;
15836 j4=3.14159265358979;
15837 sj5=gconst22;
15838 cj5=gconst23;
15839 j5=((3.14159265)+(((-1.0)*x1245)));
15840 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1245)));
15841 IkReal gconst22=(x1244*x1246);
15842 IkReal gconst23=((1.0)*new_r10*x1246);
15843 IkReal x1248=new_r10*new_r10;
15844 IkReal x1249=new_r00*new_r00*new_r00;
15845 CheckValue<IkReal> x1253=IKPowWithIntegerCheck((x1248+(new_r00*new_r00)),-1);
15846 if(!x1253.valid){
15847 continue;
15848 }
15849 IkReal x1250=x1253.value;
15850 IkReal x1251=(x1248*x1250);
15851 IkReal x1252=(x1249*x1250);
15852 j3eval[0]=((IKabs((x1251+((new_r11*x1252))+((new_r00*new_r11*x1251)))))+(IKabs((((new_r00*new_r10*x1250))+((new_r01*x1252))+((new_r00*new_r01*x1251))))));
15853 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15854 {
15855 {
15856 IkReal j3eval[1];
15857 IkReal x1254=((-1.0)*new_r00);
15858 CheckValue<IkReal> x1257 = IKatan2WithCheck(IkReal(x1254),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15859 if(!x1257.valid){
15860 continue;
15861 }
15862 IkReal x1255=((1.0)*(x1257.value));
15863 IkReal x1256=x1231;
15864 sj4=0;
15865 cj4=-1.0;
15866 j4=3.14159265358979;
15867 sj5=gconst22;
15868 cj5=gconst23;
15869 j5=((3.14159265)+(((-1.0)*x1255)));
15870 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1255)));
15871 IkReal gconst22=(x1254*x1256);
15872 IkReal gconst23=((1.0)*new_r10*x1256);
15873 IkReal x1258=new_r10*new_r10;
15874 IkReal x1259=new_r00*new_r00;
15875 CheckValue<IkReal> x1263=IKPowWithIntegerCheck((x1258+x1259),-1);
15876 if(!x1263.valid){
15877 continue;
15878 }
15879 IkReal x1260=x1263.value;
15880 IkReal x1261=(new_r10*x1260);
15881 IkReal x1262=(x1258*x1260);
15882 j3eval[0]=((IKabs((x1262+(((-1.0)*x1260*(x1259*x1259)))+(((-1.0)*x1259*x1262)))))+(IKabs((((new_r00*x1261))+((x1261*(new_r00*new_r00*new_r00)))+((new_r00*x1261*(new_r10*new_r10)))))));
15883 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15884 {
15885 {
15886 IkReal evalcond[3];
15887 bool bgotonextstatement = true;
15888 do
15889 {
15890 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
15891 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15892 {
15893 bgotonextstatement=false;
15894 {
15895 IkReal j3eval[1];
15896 CheckValue<IkReal> x1265 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15897 if(!x1265.valid){
15898 continue;
15899 }
15900 IkReal x1264=((1.0)*(x1265.value));
15901 sj4=0;
15902 cj4=-1.0;
15903 j4=3.14159265358979;
15904 sj5=gconst22;
15905 cj5=gconst23;
15906 j5=((3.14159265)+(((-1.0)*x1264)));
15907 new_r11=0;
15908 new_r00=0;
15909 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1264)));
15910 IkReal gconst22=0;
15911 IkReal x1266 = new_r10*new_r10;
15912 if(IKabs(x1266)==0){
15913 continue;
15914 }
15915 IkReal gconst23=((1.0)*new_r10*(pow(x1266,-0.5)));
15916 j3eval[0]=new_r01;
15917 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15918 {
15919 {
15920 IkReal j3array[2], cj3array[2], sj3array[2];
15921 bool j3valid[2]={false};
15922 _nj3 = 2;
15923 CheckValue<IkReal> x1267=IKPowWithIntegerCheck(gconst23,-1);
15924 if(!x1267.valid){
15925 continue;
15926 }
15927 sj3array[0]=((-1.0)*new_r01*(x1267.value));
15928 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
15929 {
15930  j3valid[0] = j3valid[1] = true;
15931  j3array[0] = IKasin(sj3array[0]);
15932  cj3array[0] = IKcos(j3array[0]);
15933  sj3array[1] = sj3array[0];
15934  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
15935  cj3array[1] = -cj3array[0];
15936 }
15937 else if( isnan(sj3array[0]) )
15938 {
15939  // probably any value will work
15940  j3valid[0] = true;
15941  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
15942 }
15943 for(int ij3 = 0; ij3 < 2; ++ij3)
15944 {
15945 if( !j3valid[ij3] )
15946 {
15947  continue;
15948 }
15949 _ij3[0] = ij3; _ij3[1] = -1;
15950 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
15951 {
15952 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15953 {
15954  j3valid[iij3]=false; _ij3[1] = iij3; break;
15955 }
15956 }
15957 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15958 {
15959 IkReal evalcond[6];
15960 IkReal x1268=IKcos(j3);
15961 IkReal x1269=IKsin(j3);
15962 evalcond[0]=(new_r01*x1268);
15963 evalcond[1]=(gconst23*x1268);
15964 evalcond[2]=((-1.0)*new_r10*x1268);
15965 evalcond[3]=(((new_r01*x1269))+gconst23);
15966 evalcond[4]=(gconst23+((new_r10*x1269)));
15967 evalcond[5]=(((gconst23*x1269))+new_r10);
15968 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
15969 {
15970 continue;
15971 }
15972 }
15973 
15974 {
15975 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15976 vinfos[0].jointtype = 1;
15977 vinfos[0].foffset = j0;
15978 vinfos[0].indices[0] = _ij0[0];
15979 vinfos[0].indices[1] = _ij0[1];
15980 vinfos[0].maxsolutions = _nj0;
15981 vinfos[1].jointtype = 1;
15982 vinfos[1].foffset = j1;
15983 vinfos[1].indices[0] = _ij1[0];
15984 vinfos[1].indices[1] = _ij1[1];
15985 vinfos[1].maxsolutions = _nj1;
15986 vinfos[2].jointtype = 1;
15987 vinfos[2].foffset = j2;
15988 vinfos[2].indices[0] = _ij2[0];
15989 vinfos[2].indices[1] = _ij2[1];
15990 vinfos[2].maxsolutions = _nj2;
15991 vinfos[3].jointtype = 1;
15992 vinfos[3].foffset = j3;
15993 vinfos[3].indices[0] = _ij3[0];
15994 vinfos[3].indices[1] = _ij3[1];
15995 vinfos[3].maxsolutions = _nj3;
15996 vinfos[4].jointtype = 1;
15997 vinfos[4].foffset = j4;
15998 vinfos[4].indices[0] = _ij4[0];
15999 vinfos[4].indices[1] = _ij4[1];
16000 vinfos[4].maxsolutions = _nj4;
16001 vinfos[5].jointtype = 1;
16002 vinfos[5].foffset = j5;
16003 vinfos[5].indices[0] = _ij5[0];
16004 vinfos[5].indices[1] = _ij5[1];
16005 vinfos[5].maxsolutions = _nj5;
16006 std::vector<int> vfree(0);
16007 solutions.AddSolution(vinfos,vfree);
16008 }
16009 }
16010 }
16011 
16012 } else
16013 {
16014 {
16015 IkReal j3array[2], cj3array[2], sj3array[2];
16016 bool j3valid[2]={false};
16017 _nj3 = 2;
16018 CheckValue<IkReal> x1270=IKPowWithIntegerCheck(new_r01,-1);
16019 if(!x1270.valid){
16020 continue;
16021 }
16022 sj3array[0]=((-1.0)*gconst23*(x1270.value));
16023 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16024 {
16025  j3valid[0] = j3valid[1] = true;
16026  j3array[0] = IKasin(sj3array[0]);
16027  cj3array[0] = IKcos(j3array[0]);
16028  sj3array[1] = sj3array[0];
16029  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16030  cj3array[1] = -cj3array[0];
16031 }
16032 else if( isnan(sj3array[0]) )
16033 {
16034  // probably any value will work
16035  j3valid[0] = true;
16036  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16037 }
16038 for(int ij3 = 0; ij3 < 2; ++ij3)
16039 {
16040 if( !j3valid[ij3] )
16041 {
16042  continue;
16043 }
16044 _ij3[0] = ij3; _ij3[1] = -1;
16045 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16046 {
16047 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16048 {
16049  j3valid[iij3]=false; _ij3[1] = iij3; break;
16050 }
16051 }
16052 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16053 {
16054 IkReal evalcond[6];
16055 IkReal x1271=IKcos(j3);
16056 IkReal x1272=IKsin(j3);
16057 IkReal x1273=(gconst23*x1272);
16058 evalcond[0]=(new_r01*x1271);
16059 evalcond[1]=(gconst23*x1271);
16060 evalcond[2]=((-1.0)*new_r10*x1271);
16061 evalcond[3]=(x1273+new_r01);
16062 evalcond[4]=(gconst23+((new_r10*x1272)));
16063 evalcond[5]=(x1273+new_r10);
16064 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16065 {
16066 continue;
16067 }
16068 }
16069 
16070 {
16071 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16072 vinfos[0].jointtype = 1;
16073 vinfos[0].foffset = j0;
16074 vinfos[0].indices[0] = _ij0[0];
16075 vinfos[0].indices[1] = _ij0[1];
16076 vinfos[0].maxsolutions = _nj0;
16077 vinfos[1].jointtype = 1;
16078 vinfos[1].foffset = j1;
16079 vinfos[1].indices[0] = _ij1[0];
16080 vinfos[1].indices[1] = _ij1[1];
16081 vinfos[1].maxsolutions = _nj1;
16082 vinfos[2].jointtype = 1;
16083 vinfos[2].foffset = j2;
16084 vinfos[2].indices[0] = _ij2[0];
16085 vinfos[2].indices[1] = _ij2[1];
16086 vinfos[2].maxsolutions = _nj2;
16087 vinfos[3].jointtype = 1;
16088 vinfos[3].foffset = j3;
16089 vinfos[3].indices[0] = _ij3[0];
16090 vinfos[3].indices[1] = _ij3[1];
16091 vinfos[3].maxsolutions = _nj3;
16092 vinfos[4].jointtype = 1;
16093 vinfos[4].foffset = j4;
16094 vinfos[4].indices[0] = _ij4[0];
16095 vinfos[4].indices[1] = _ij4[1];
16096 vinfos[4].maxsolutions = _nj4;
16097 vinfos[5].jointtype = 1;
16098 vinfos[5].foffset = j5;
16099 vinfos[5].indices[0] = _ij5[0];
16100 vinfos[5].indices[1] = _ij5[1];
16101 vinfos[5].maxsolutions = _nj5;
16102 std::vector<int> vfree(0);
16103 solutions.AddSolution(vinfos,vfree);
16104 }
16105 }
16106 }
16107 
16108 }
16109 
16110 }
16111 
16112 }
16113 } while(0);
16114 if( bgotonextstatement )
16115 {
16116 bool bgotonextstatement = true;
16117 do
16118 {
16119 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
16120 evalcond[1]=gconst23;
16121 evalcond[2]=gconst22;
16122 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
16123 {
16124 bgotonextstatement=false;
16125 {
16126 IkReal j3eval[3];
16127 IkReal x1274=((-1.0)*new_r00);
16128 CheckValue<IkReal> x1276 = IKatan2WithCheck(IkReal(x1274),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16129 if(!x1276.valid){
16130 continue;
16131 }
16132 IkReal x1275=((1.0)*(x1276.value));
16133 sj4=0;
16134 cj4=-1.0;
16135 j4=3.14159265358979;
16136 sj5=gconst22;
16137 cj5=gconst23;
16138 j5=((3.14159265)+(((-1.0)*x1275)));
16139 new_r11=0;
16140 new_r01=0;
16141 new_r22=0;
16142 new_r20=0;
16143 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1275)));
16144 IkReal gconst22=x1274;
16145 IkReal gconst23=((1.0)*new_r10);
16146 j3eval[0]=1.0;
16147 j3eval[1]=1.0;
16148 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
16149 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16150 {
16151 {
16152 IkReal j3eval[3];
16153 IkReal x1277=((-1.0)*new_r00);
16154 CheckValue<IkReal> x1279 = IKatan2WithCheck(IkReal(x1277),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16155 if(!x1279.valid){
16156 continue;
16157 }
16158 IkReal x1278=((1.0)*(x1279.value));
16159 sj4=0;
16160 cj4=-1.0;
16161 j4=3.14159265358979;
16162 sj5=gconst22;
16163 cj5=gconst23;
16164 j5=((3.14159265)+(((-1.0)*x1278)));
16165 new_r11=0;
16166 new_r01=0;
16167 new_r22=0;
16168 new_r20=0;
16169 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1278)));
16170 IkReal gconst22=x1277;
16171 IkReal gconst23=((1.0)*new_r10);
16172 j3eval[0]=-1.0;
16173 j3eval[1]=((IKabs(((1.0)*new_r00*new_r10)))+(IKabs(((-1.0)+(new_r10*new_r10)))));
16174 j3eval[2]=-1.0;
16175 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16176 {
16177 {
16178 IkReal j3eval[3];
16179 IkReal x1280=((-1.0)*new_r00);
16180 CheckValue<IkReal> x1282 = IKatan2WithCheck(IkReal(x1280),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16181 if(!x1282.valid){
16182 continue;
16183 }
16184 IkReal x1281=((1.0)*(x1282.value));
16185 sj4=0;
16186 cj4=-1.0;
16187 j4=3.14159265358979;
16188 sj5=gconst22;
16189 cj5=gconst23;
16190 j5=((3.14159265)+(((-1.0)*x1281)));
16191 new_r11=0;
16192 new_r01=0;
16193 new_r22=0;
16194 new_r20=0;
16195 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1281)));
16196 IkReal gconst22=x1280;
16197 IkReal gconst23=((1.0)*new_r10);
16198 j3eval[0]=-1.0;
16199 j3eval[1]=((IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
16200 j3eval[2]=-1.0;
16201 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16202 {
16203 continue; // 3 cases reached
16204 
16205 } else
16206 {
16207 {
16208 IkReal j3array[1], cj3array[1], sj3array[1];
16209 bool j3valid[1]={false};
16210 _nj3 = 1;
16211 IkReal x1283=((1.0)*gconst23);
16212 CheckValue<IkReal> x1284 = IKatan2WithCheck(IkReal(((gconst23*gconst23)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst22*x1283)))),IKFAST_ATAN2_MAGTHRESH);
16213 if(!x1284.valid){
16214 continue;
16215 }
16216 CheckValue<IkReal> x1285=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x1283))+((gconst22*new_r00)))),-1);
16217 if(!x1285.valid){
16218 continue;
16219 }
16220 j3array[0]=((-1.5707963267949)+(x1284.value)+(((1.5707963267949)*(x1285.value))));
16221 sj3array[0]=IKsin(j3array[0]);
16222 cj3array[0]=IKcos(j3array[0]);
16223 if( j3array[0] > IKPI )
16224 {
16225  j3array[0]-=IK2PI;
16226 }
16227 else if( j3array[0] < -IKPI )
16228 { j3array[0]+=IK2PI;
16229 }
16230 j3valid[0] = true;
16231 for(int ij3 = 0; ij3 < 1; ++ij3)
16232 {
16233 if( !j3valid[ij3] )
16234 {
16235  continue;
16236 }
16237 _ij3[0] = ij3; _ij3[1] = -1;
16238 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16239 {
16240 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16241 {
16242  j3valid[iij3]=false; _ij3[1] = iij3; break;
16243 }
16244 }
16245 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16246 {
16247 IkReal evalcond[6];
16248 IkReal x1286=IKsin(j3);
16249 IkReal x1287=IKcos(j3);
16250 IkReal x1288=(gconst23*x1286);
16251 IkReal x1289=(gconst22*x1286);
16252 IkReal x1290=(gconst23*x1287);
16253 IkReal x1291=((1.0)*x1287);
16254 IkReal x1292=(gconst22*x1291);
16255 evalcond[0]=(x1288+(((-1.0)*x1292)));
16256 evalcond[1]=(((new_r10*x1286))+gconst23+((new_r00*x1287)));
16257 evalcond[2]=(x1289+x1290+new_r00);
16258 evalcond[3]=((((-1.0)*new_r10*x1291))+gconst22+((new_r00*x1286)));
16259 evalcond[4]=((((-1.0)*x1289))+(((-1.0)*x1290)));
16260 evalcond[5]=(x1288+(((-1.0)*x1292))+new_r10);
16261 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16262 {
16263 continue;
16264 }
16265 }
16266 
16267 {
16268 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16269 vinfos[0].jointtype = 1;
16270 vinfos[0].foffset = j0;
16271 vinfos[0].indices[0] = _ij0[0];
16272 vinfos[0].indices[1] = _ij0[1];
16273 vinfos[0].maxsolutions = _nj0;
16274 vinfos[1].jointtype = 1;
16275 vinfos[1].foffset = j1;
16276 vinfos[1].indices[0] = _ij1[0];
16277 vinfos[1].indices[1] = _ij1[1];
16278 vinfos[1].maxsolutions = _nj1;
16279 vinfos[2].jointtype = 1;
16280 vinfos[2].foffset = j2;
16281 vinfos[2].indices[0] = _ij2[0];
16282 vinfos[2].indices[1] = _ij2[1];
16283 vinfos[2].maxsolutions = _nj2;
16284 vinfos[3].jointtype = 1;
16285 vinfos[3].foffset = j3;
16286 vinfos[3].indices[0] = _ij3[0];
16287 vinfos[3].indices[1] = _ij3[1];
16288 vinfos[3].maxsolutions = _nj3;
16289 vinfos[4].jointtype = 1;
16290 vinfos[4].foffset = j4;
16291 vinfos[4].indices[0] = _ij4[0];
16292 vinfos[4].indices[1] = _ij4[1];
16293 vinfos[4].maxsolutions = _nj4;
16294 vinfos[5].jointtype = 1;
16295 vinfos[5].foffset = j5;
16296 vinfos[5].indices[0] = _ij5[0];
16297 vinfos[5].indices[1] = _ij5[1];
16298 vinfos[5].maxsolutions = _nj5;
16299 std::vector<int> vfree(0);
16300 solutions.AddSolution(vinfos,vfree);
16301 }
16302 }
16303 }
16304 
16305 }
16306 
16307 }
16308 
16309 } else
16310 {
16311 {
16312 IkReal j3array[1], cj3array[1], sj3array[1];
16313 bool j3valid[1]={false};
16314 _nj3 = 1;
16315 CheckValue<IkReal> x1293=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst22*gconst22)))+(((-1.0)*(gconst23*gconst23))))),-1);
16316 if(!x1293.valid){
16317 continue;
16318 }
16319 CheckValue<IkReal> x1294 = IKatan2WithCheck(IkReal((gconst22*new_r00)),IkReal((gconst23*new_r00)),IKFAST_ATAN2_MAGTHRESH);
16320 if(!x1294.valid){
16321 continue;
16322 }
16323 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1293.value)))+(x1294.value));
16324 sj3array[0]=IKsin(j3array[0]);
16325 cj3array[0]=IKcos(j3array[0]);
16326 if( j3array[0] > IKPI )
16327 {
16328  j3array[0]-=IK2PI;
16329 }
16330 else if( j3array[0] < -IKPI )
16331 { j3array[0]+=IK2PI;
16332 }
16333 j3valid[0] = true;
16334 for(int ij3 = 0; ij3 < 1; ++ij3)
16335 {
16336 if( !j3valid[ij3] )
16337 {
16338  continue;
16339 }
16340 _ij3[0] = ij3; _ij3[1] = -1;
16341 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16342 {
16343 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16344 {
16345  j3valid[iij3]=false; _ij3[1] = iij3; break;
16346 }
16347 }
16348 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16349 {
16350 IkReal evalcond[6];
16351 IkReal x1295=IKsin(j3);
16352 IkReal x1296=IKcos(j3);
16353 IkReal x1297=(gconst23*x1295);
16354 IkReal x1298=(gconst22*x1295);
16355 IkReal x1299=(gconst23*x1296);
16356 IkReal x1300=((1.0)*x1296);
16357 IkReal x1301=(gconst22*x1300);
16358 evalcond[0]=(x1297+(((-1.0)*x1301)));
16359 evalcond[1]=(gconst23+((new_r10*x1295))+((new_r00*x1296)));
16360 evalcond[2]=(x1298+x1299+new_r00);
16361 evalcond[3]=((((-1.0)*new_r10*x1300))+gconst22+((new_r00*x1295)));
16362 evalcond[4]=((((-1.0)*x1298))+(((-1.0)*x1299)));
16363 evalcond[5]=(x1297+(((-1.0)*x1301))+new_r10);
16364 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16365 {
16366 continue;
16367 }
16368 }
16369 
16370 {
16371 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16372 vinfos[0].jointtype = 1;
16373 vinfos[0].foffset = j0;
16374 vinfos[0].indices[0] = _ij0[0];
16375 vinfos[0].indices[1] = _ij0[1];
16376 vinfos[0].maxsolutions = _nj0;
16377 vinfos[1].jointtype = 1;
16378 vinfos[1].foffset = j1;
16379 vinfos[1].indices[0] = _ij1[0];
16380 vinfos[1].indices[1] = _ij1[1];
16381 vinfos[1].maxsolutions = _nj1;
16382 vinfos[2].jointtype = 1;
16383 vinfos[2].foffset = j2;
16384 vinfos[2].indices[0] = _ij2[0];
16385 vinfos[2].indices[1] = _ij2[1];
16386 vinfos[2].maxsolutions = _nj2;
16387 vinfos[3].jointtype = 1;
16388 vinfos[3].foffset = j3;
16389 vinfos[3].indices[0] = _ij3[0];
16390 vinfos[3].indices[1] = _ij3[1];
16391 vinfos[3].maxsolutions = _nj3;
16392 vinfos[4].jointtype = 1;
16393 vinfos[4].foffset = j4;
16394 vinfos[4].indices[0] = _ij4[0];
16395 vinfos[4].indices[1] = _ij4[1];
16396 vinfos[4].maxsolutions = _nj4;
16397 vinfos[5].jointtype = 1;
16398 vinfos[5].foffset = j5;
16399 vinfos[5].indices[0] = _ij5[0];
16400 vinfos[5].indices[1] = _ij5[1];
16401 vinfos[5].maxsolutions = _nj5;
16402 std::vector<int> vfree(0);
16403 solutions.AddSolution(vinfos,vfree);
16404 }
16405 }
16406 }
16407 
16408 }
16409 
16410 }
16411 
16412 } else
16413 {
16414 {
16415 IkReal j3array[1], cj3array[1], sj3array[1];
16416 bool j3valid[1]={false};
16417 _nj3 = 1;
16418 CheckValue<IkReal> x1302=IKPowWithIntegerCheck(IKsign((((gconst23*new_r10))+(((-1.0)*gconst22*new_r00)))),-1);
16419 if(!x1302.valid){
16420 continue;
16421 }
16422 CheckValue<IkReal> x1303 = IKatan2WithCheck(IkReal(gconst22*gconst22),IkReal((gconst22*gconst23)),IKFAST_ATAN2_MAGTHRESH);
16423 if(!x1303.valid){
16424 continue;
16425 }
16426 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1302.value)))+(x1303.value));
16427 sj3array[0]=IKsin(j3array[0]);
16428 cj3array[0]=IKcos(j3array[0]);
16429 if( j3array[0] > IKPI )
16430 {
16431  j3array[0]-=IK2PI;
16432 }
16433 else if( j3array[0] < -IKPI )
16434 { j3array[0]+=IK2PI;
16435 }
16436 j3valid[0] = true;
16437 for(int ij3 = 0; ij3 < 1; ++ij3)
16438 {
16439 if( !j3valid[ij3] )
16440 {
16441  continue;
16442 }
16443 _ij3[0] = ij3; _ij3[1] = -1;
16444 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16445 {
16446 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16447 {
16448  j3valid[iij3]=false; _ij3[1] = iij3; break;
16449 }
16450 }
16451 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16452 {
16453 IkReal evalcond[6];
16454 IkReal x1304=IKsin(j3);
16455 IkReal x1305=IKcos(j3);
16456 IkReal x1306=(gconst23*x1304);
16457 IkReal x1307=(gconst22*x1304);
16458 IkReal x1308=(gconst23*x1305);
16459 IkReal x1309=((1.0)*x1305);
16460 IkReal x1310=(gconst22*x1309);
16461 evalcond[0]=(x1306+(((-1.0)*x1310)));
16462 evalcond[1]=(gconst23+((new_r10*x1304))+((new_r00*x1305)));
16463 evalcond[2]=(x1307+x1308+new_r00);
16464 evalcond[3]=((((-1.0)*new_r10*x1309))+gconst22+((new_r00*x1304)));
16465 evalcond[4]=((((-1.0)*x1308))+(((-1.0)*x1307)));
16466 evalcond[5]=(x1306+(((-1.0)*x1310))+new_r10);
16467 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16468 {
16469 continue;
16470 }
16471 }
16472 
16473 {
16474 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16475 vinfos[0].jointtype = 1;
16476 vinfos[0].foffset = j0;
16477 vinfos[0].indices[0] = _ij0[0];
16478 vinfos[0].indices[1] = _ij0[1];
16479 vinfos[0].maxsolutions = _nj0;
16480 vinfos[1].jointtype = 1;
16481 vinfos[1].foffset = j1;
16482 vinfos[1].indices[0] = _ij1[0];
16483 vinfos[1].indices[1] = _ij1[1];
16484 vinfos[1].maxsolutions = _nj1;
16485 vinfos[2].jointtype = 1;
16486 vinfos[2].foffset = j2;
16487 vinfos[2].indices[0] = _ij2[0];
16488 vinfos[2].indices[1] = _ij2[1];
16489 vinfos[2].maxsolutions = _nj2;
16490 vinfos[3].jointtype = 1;
16491 vinfos[3].foffset = j3;
16492 vinfos[3].indices[0] = _ij3[0];
16493 vinfos[3].indices[1] = _ij3[1];
16494 vinfos[3].maxsolutions = _nj3;
16495 vinfos[4].jointtype = 1;
16496 vinfos[4].foffset = j4;
16497 vinfos[4].indices[0] = _ij4[0];
16498 vinfos[4].indices[1] = _ij4[1];
16499 vinfos[4].maxsolutions = _nj4;
16500 vinfos[5].jointtype = 1;
16501 vinfos[5].foffset = j5;
16502 vinfos[5].indices[0] = _ij5[0];
16503 vinfos[5].indices[1] = _ij5[1];
16504 vinfos[5].maxsolutions = _nj5;
16505 std::vector<int> vfree(0);
16506 solutions.AddSolution(vinfos,vfree);
16507 }
16508 }
16509 }
16510 
16511 }
16512 
16513 }
16514 
16515 }
16516 } while(0);
16517 if( bgotonextstatement )
16518 {
16519 bool bgotonextstatement = true;
16520 do
16521 {
16522 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
16523 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16524 {
16525 bgotonextstatement=false;
16526 {
16527 IkReal j3array[2], cj3array[2], sj3array[2];
16528 bool j3valid[2]={false};
16529 _nj3 = 2;
16530 CheckValue<IkReal> x1311=IKPowWithIntegerCheck(gconst22,-1);
16531 if(!x1311.valid){
16532 continue;
16533 }
16534 sj3array[0]=(new_r11*(x1311.value));
16535 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16536 {
16537  j3valid[0] = j3valid[1] = true;
16538  j3array[0] = IKasin(sj3array[0]);
16539  cj3array[0] = IKcos(j3array[0]);
16540  sj3array[1] = sj3array[0];
16541  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16542  cj3array[1] = -cj3array[0];
16543 }
16544 else if( isnan(sj3array[0]) )
16545 {
16546  // probably any value will work
16547  j3valid[0] = true;
16548  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16549 }
16550 for(int ij3 = 0; ij3 < 2; ++ij3)
16551 {
16552 if( !j3valid[ij3] )
16553 {
16554  continue;
16555 }
16556 _ij3[0] = ij3; _ij3[1] = -1;
16557 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16558 {
16559 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16560 {
16561  j3valid[iij3]=false; _ij3[1] = iij3; break;
16562 }
16563 }
16564 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16565 {
16566 IkReal evalcond[6];
16567 IkReal x1312=IKcos(j3);
16568 IkReal x1313=IKsin(j3);
16569 IkReal x1314=((-1.0)*x1312);
16570 evalcond[0]=(new_r00*x1312);
16571 evalcond[1]=(new_r11*x1314);
16572 evalcond[2]=(gconst22*x1314);
16573 evalcond[3]=(((new_r00*x1313))+gconst22);
16574 evalcond[4]=(((gconst22*x1313))+new_r00);
16575 evalcond[5]=(((new_r11*x1313))+(((-1.0)*gconst22)));
16576 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16577 {
16578 continue;
16579 }
16580 }
16581 
16582 {
16583 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16584 vinfos[0].jointtype = 1;
16585 vinfos[0].foffset = j0;
16586 vinfos[0].indices[0] = _ij0[0];
16587 vinfos[0].indices[1] = _ij0[1];
16588 vinfos[0].maxsolutions = _nj0;
16589 vinfos[1].jointtype = 1;
16590 vinfos[1].foffset = j1;
16591 vinfos[1].indices[0] = _ij1[0];
16592 vinfos[1].indices[1] = _ij1[1];
16593 vinfos[1].maxsolutions = _nj1;
16594 vinfos[2].jointtype = 1;
16595 vinfos[2].foffset = j2;
16596 vinfos[2].indices[0] = _ij2[0];
16597 vinfos[2].indices[1] = _ij2[1];
16598 vinfos[2].maxsolutions = _nj2;
16599 vinfos[3].jointtype = 1;
16600 vinfos[3].foffset = j3;
16601 vinfos[3].indices[0] = _ij3[0];
16602 vinfos[3].indices[1] = _ij3[1];
16603 vinfos[3].maxsolutions = _nj3;
16604 vinfos[4].jointtype = 1;
16605 vinfos[4].foffset = j4;
16606 vinfos[4].indices[0] = _ij4[0];
16607 vinfos[4].indices[1] = _ij4[1];
16608 vinfos[4].maxsolutions = _nj4;
16609 vinfos[5].jointtype = 1;
16610 vinfos[5].foffset = j5;
16611 vinfos[5].indices[0] = _ij5[0];
16612 vinfos[5].indices[1] = _ij5[1];
16613 vinfos[5].maxsolutions = _nj5;
16614 std::vector<int> vfree(0);
16615 solutions.AddSolution(vinfos,vfree);
16616 }
16617 }
16618 }
16619 
16620 }
16621 } while(0);
16622 if( bgotonextstatement )
16623 {
16624 bool bgotonextstatement = true;
16625 do
16626 {
16627 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
16628 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16629 {
16630 bgotonextstatement=false;
16631 {
16632 IkReal j3eval[1];
16633 IkReal x1315=((-1.0)*new_r00);
16634 CheckValue<IkReal> x1317 = IKatan2WithCheck(IkReal(x1315),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16635 if(!x1317.valid){
16636 continue;
16637 }
16638 IkReal x1316=((1.0)*(x1317.value));
16639 sj4=0;
16640 cj4=-1.0;
16641 j4=3.14159265358979;
16642 sj5=gconst22;
16643 cj5=gconst23;
16644 j5=((3.14159265)+(((-1.0)*x1316)));
16645 new_r11=0;
16646 new_r10=0;
16647 new_r22=0;
16648 new_r02=0;
16649 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1316)));
16650 IkReal x1318 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16651 if(IKabs(x1318)==0){
16652 continue;
16653 }
16654 IkReal gconst22=(x1315*(pow(x1318,-0.5)));
16655 IkReal gconst23=0;
16656 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
16657 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16658 {
16659 {
16660 IkReal j3eval[1];
16661 IkReal x1319=((-1.0)*new_r00);
16662 CheckValue<IkReal> x1321 = IKatan2WithCheck(IkReal(x1319),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16663 if(!x1321.valid){
16664 continue;
16665 }
16666 IkReal x1320=((1.0)*(x1321.value));
16667 sj4=0;
16668 cj4=-1.0;
16669 j4=3.14159265358979;
16670 sj5=gconst22;
16671 cj5=gconst23;
16672 j5=((3.14159265)+(((-1.0)*x1320)));
16673 new_r11=0;
16674 new_r10=0;
16675 new_r22=0;
16676 new_r02=0;
16677 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1320)));
16678 IkReal x1322 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16679 if(IKabs(x1322)==0){
16680 continue;
16681 }
16682 IkReal gconst22=(x1319*(pow(x1322,-0.5)));
16683 IkReal gconst23=0;
16684 j3eval[0]=new_r00;
16685 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16686 {
16687 {
16688 IkReal j3eval[2];
16689 IkReal x1323=((-1.0)*new_r00);
16690 CheckValue<IkReal> x1325 = IKatan2WithCheck(IkReal(x1323),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16691 if(!x1325.valid){
16692 continue;
16693 }
16694 IkReal x1324=((1.0)*(x1325.value));
16695 sj4=0;
16696 cj4=-1.0;
16697 j4=3.14159265358979;
16698 sj5=gconst22;
16699 cj5=gconst23;
16700 j5=((3.14159265)+(((-1.0)*x1324)));
16701 new_r11=0;
16702 new_r10=0;
16703 new_r22=0;
16704 new_r02=0;
16705 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1324)));
16706 IkReal x1326 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16707 if(IKabs(x1326)==0){
16708 continue;
16709 }
16710 IkReal gconst22=(x1323*(pow(x1326,-0.5)));
16711 IkReal gconst23=0;
16712 j3eval[0]=new_r00;
16713 j3eval[1]=new_r01;
16714 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
16715 {
16716 continue; // 3 cases reached
16717 
16718 } else
16719 {
16720 {
16721 IkReal j3array[1], cj3array[1], sj3array[1];
16722 bool j3valid[1]={false};
16723 _nj3 = 1;
16724 CheckValue<IkReal> x1327=IKPowWithIntegerCheck(new_r00,-1);
16725 if(!x1327.valid){
16726 continue;
16727 }
16728 CheckValue<IkReal> x1328=IKPowWithIntegerCheck(new_r01,-1);
16729 if(!x1328.valid){
16730 continue;
16731 }
16732 if( IKabs(((-1.0)*gconst22*(x1327.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst22*(x1328.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1327.value)))+IKsqr((gconst22*(x1328.value)))-1) <= IKFAST_SINCOS_THRESH )
16733  continue;
16734 j3array[0]=IKatan2(((-1.0)*gconst22*(x1327.value)), (gconst22*(x1328.value)));
16735 sj3array[0]=IKsin(j3array[0]);
16736 cj3array[0]=IKcos(j3array[0]);
16737 if( j3array[0] > IKPI )
16738 {
16739  j3array[0]-=IK2PI;
16740 }
16741 else if( j3array[0] < -IKPI )
16742 { j3array[0]+=IK2PI;
16743 }
16744 j3valid[0] = true;
16745 for(int ij3 = 0; ij3 < 1; ++ij3)
16746 {
16747 if( !j3valid[ij3] )
16748 {
16749  continue;
16750 }
16751 _ij3[0] = ij3; _ij3[1] = -1;
16752 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16753 {
16754 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16755 {
16756  j3valid[iij3]=false; _ij3[1] = iij3; break;
16757 }
16758 }
16759 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16760 {
16761 IkReal evalcond[8];
16762 IkReal x1329=IKsin(j3);
16763 IkReal x1330=IKcos(j3);
16764 IkReal x1331=(gconst22*x1330);
16765 IkReal x1332=(gconst22*x1329);
16766 evalcond[0]=(new_r01*x1329);
16767 evalcond[1]=(new_r00*x1330);
16768 evalcond[2]=((-1.0)*x1332);
16769 evalcond[3]=((-1.0)*x1331);
16770 evalcond[4]=(((new_r00*x1329))+gconst22);
16771 evalcond[5]=(x1332+new_r00);
16772 evalcond[6]=((((-1.0)*x1331))+new_r01);
16773 evalcond[7]=(((new_r01*x1330))+(((-1.0)*gconst22)));
16774 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16775 {
16776 continue;
16777 }
16778 }
16779 
16780 {
16781 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16782 vinfos[0].jointtype = 1;
16783 vinfos[0].foffset = j0;
16784 vinfos[0].indices[0] = _ij0[0];
16785 vinfos[0].indices[1] = _ij0[1];
16786 vinfos[0].maxsolutions = _nj0;
16787 vinfos[1].jointtype = 1;
16788 vinfos[1].foffset = j1;
16789 vinfos[1].indices[0] = _ij1[0];
16790 vinfos[1].indices[1] = _ij1[1];
16791 vinfos[1].maxsolutions = _nj1;
16792 vinfos[2].jointtype = 1;
16793 vinfos[2].foffset = j2;
16794 vinfos[2].indices[0] = _ij2[0];
16795 vinfos[2].indices[1] = _ij2[1];
16796 vinfos[2].maxsolutions = _nj2;
16797 vinfos[3].jointtype = 1;
16798 vinfos[3].foffset = j3;
16799 vinfos[3].indices[0] = _ij3[0];
16800 vinfos[3].indices[1] = _ij3[1];
16801 vinfos[3].maxsolutions = _nj3;
16802 vinfos[4].jointtype = 1;
16803 vinfos[4].foffset = j4;
16804 vinfos[4].indices[0] = _ij4[0];
16805 vinfos[4].indices[1] = _ij4[1];
16806 vinfos[4].maxsolutions = _nj4;
16807 vinfos[5].jointtype = 1;
16808 vinfos[5].foffset = j5;
16809 vinfos[5].indices[0] = _ij5[0];
16810 vinfos[5].indices[1] = _ij5[1];
16811 vinfos[5].maxsolutions = _nj5;
16812 std::vector<int> vfree(0);
16813 solutions.AddSolution(vinfos,vfree);
16814 }
16815 }
16816 }
16817 
16818 }
16819 
16820 }
16821 
16822 } else
16823 {
16824 {
16825 IkReal j3array[1], cj3array[1], sj3array[1];
16826 bool j3valid[1]={false};
16827 _nj3 = 1;
16828 CheckValue<IkReal> x1333=IKPowWithIntegerCheck(new_r00,-1);
16829 if(!x1333.valid){
16830 continue;
16831 }
16832 CheckValue<IkReal> x1334=IKPowWithIntegerCheck(gconst22,-1);
16833 if(!x1334.valid){
16834 continue;
16835 }
16836 if( IKabs(((-1.0)*gconst22*(x1333.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1334.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1333.value)))+IKsqr((new_r01*(x1334.value)))-1) <= IKFAST_SINCOS_THRESH )
16837  continue;
16838 j3array[0]=IKatan2(((-1.0)*gconst22*(x1333.value)), (new_r01*(x1334.value)));
16839 sj3array[0]=IKsin(j3array[0]);
16840 cj3array[0]=IKcos(j3array[0]);
16841 if( j3array[0] > IKPI )
16842 {
16843  j3array[0]-=IK2PI;
16844 }
16845 else if( j3array[0] < -IKPI )
16846 { j3array[0]+=IK2PI;
16847 }
16848 j3valid[0] = true;
16849 for(int ij3 = 0; ij3 < 1; ++ij3)
16850 {
16851 if( !j3valid[ij3] )
16852 {
16853  continue;
16854 }
16855 _ij3[0] = ij3; _ij3[1] = -1;
16856 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16857 {
16858 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16859 {
16860  j3valid[iij3]=false; _ij3[1] = iij3; break;
16861 }
16862 }
16863 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16864 {
16865 IkReal evalcond[8];
16866 IkReal x1335=IKsin(j3);
16867 IkReal x1336=IKcos(j3);
16868 IkReal x1337=(gconst22*x1336);
16869 IkReal x1338=(gconst22*x1335);
16870 evalcond[0]=(new_r01*x1335);
16871 evalcond[1]=(new_r00*x1336);
16872 evalcond[2]=((-1.0)*x1338);
16873 evalcond[3]=((-1.0)*x1337);
16874 evalcond[4]=(gconst22+((new_r00*x1335)));
16875 evalcond[5]=(x1338+new_r00);
16876 evalcond[6]=((((-1.0)*x1337))+new_r01);
16877 evalcond[7]=(((new_r01*x1336))+(((-1.0)*gconst22)));
16878 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16879 {
16880 continue;
16881 }
16882 }
16883 
16884 {
16885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16886 vinfos[0].jointtype = 1;
16887 vinfos[0].foffset = j0;
16888 vinfos[0].indices[0] = _ij0[0];
16889 vinfos[0].indices[1] = _ij0[1];
16890 vinfos[0].maxsolutions = _nj0;
16891 vinfos[1].jointtype = 1;
16892 vinfos[1].foffset = j1;
16893 vinfos[1].indices[0] = _ij1[0];
16894 vinfos[1].indices[1] = _ij1[1];
16895 vinfos[1].maxsolutions = _nj1;
16896 vinfos[2].jointtype = 1;
16897 vinfos[2].foffset = j2;
16898 vinfos[2].indices[0] = _ij2[0];
16899 vinfos[2].indices[1] = _ij2[1];
16900 vinfos[2].maxsolutions = _nj2;
16901 vinfos[3].jointtype = 1;
16902 vinfos[3].foffset = j3;
16903 vinfos[3].indices[0] = _ij3[0];
16904 vinfos[3].indices[1] = _ij3[1];
16905 vinfos[3].maxsolutions = _nj3;
16906 vinfos[4].jointtype = 1;
16907 vinfos[4].foffset = j4;
16908 vinfos[4].indices[0] = _ij4[0];
16909 vinfos[4].indices[1] = _ij4[1];
16910 vinfos[4].maxsolutions = _nj4;
16911 vinfos[5].jointtype = 1;
16912 vinfos[5].foffset = j5;
16913 vinfos[5].indices[0] = _ij5[0];
16914 vinfos[5].indices[1] = _ij5[1];
16915 vinfos[5].maxsolutions = _nj5;
16916 std::vector<int> vfree(0);
16917 solutions.AddSolution(vinfos,vfree);
16918 }
16919 }
16920 }
16921 
16922 }
16923 
16924 }
16925 
16926 } else
16927 {
16928 {
16929 IkReal j3array[1], cj3array[1], sj3array[1];
16930 bool j3valid[1]={false};
16931 _nj3 = 1;
16932 CheckValue<IkReal> x1339=IKPowWithIntegerCheck(IKsign(gconst22),-1);
16933 if(!x1339.valid){
16934 continue;
16935 }
16936 CheckValue<IkReal> x1340 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
16937 if(!x1340.valid){
16938 continue;
16939 }
16940 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1339.value)))+(x1340.value));
16941 sj3array[0]=IKsin(j3array[0]);
16942 cj3array[0]=IKcos(j3array[0]);
16943 if( j3array[0] > IKPI )
16944 {
16945  j3array[0]-=IK2PI;
16946 }
16947 else if( j3array[0] < -IKPI )
16948 { j3array[0]+=IK2PI;
16949 }
16950 j3valid[0] = true;
16951 for(int ij3 = 0; ij3 < 1; ++ij3)
16952 {
16953 if( !j3valid[ij3] )
16954 {
16955  continue;
16956 }
16957 _ij3[0] = ij3; _ij3[1] = -1;
16958 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16959 {
16960 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16961 {
16962  j3valid[iij3]=false; _ij3[1] = iij3; break;
16963 }
16964 }
16965 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16966 {
16967 IkReal evalcond[8];
16968 IkReal x1341=IKsin(j3);
16969 IkReal x1342=IKcos(j3);
16970 IkReal x1343=(gconst22*x1342);
16971 IkReal x1344=(gconst22*x1341);
16972 evalcond[0]=(new_r01*x1341);
16973 evalcond[1]=(new_r00*x1342);
16974 evalcond[2]=((-1.0)*x1344);
16975 evalcond[3]=((-1.0)*x1343);
16976 evalcond[4]=(gconst22+((new_r00*x1341)));
16977 evalcond[5]=(x1344+new_r00);
16978 evalcond[6]=(new_r01+(((-1.0)*x1343)));
16979 evalcond[7]=(((new_r01*x1342))+(((-1.0)*gconst22)));
16980 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16981 {
16982 continue;
16983 }
16984 }
16985 
16986 {
16987 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16988 vinfos[0].jointtype = 1;
16989 vinfos[0].foffset = j0;
16990 vinfos[0].indices[0] = _ij0[0];
16991 vinfos[0].indices[1] = _ij0[1];
16992 vinfos[0].maxsolutions = _nj0;
16993 vinfos[1].jointtype = 1;
16994 vinfos[1].foffset = j1;
16995 vinfos[1].indices[0] = _ij1[0];
16996 vinfos[1].indices[1] = _ij1[1];
16997 vinfos[1].maxsolutions = _nj1;
16998 vinfos[2].jointtype = 1;
16999 vinfos[2].foffset = j2;
17000 vinfos[2].indices[0] = _ij2[0];
17001 vinfos[2].indices[1] = _ij2[1];
17002 vinfos[2].maxsolutions = _nj2;
17003 vinfos[3].jointtype = 1;
17004 vinfos[3].foffset = j3;
17005 vinfos[3].indices[0] = _ij3[0];
17006 vinfos[3].indices[1] = _ij3[1];
17007 vinfos[3].maxsolutions = _nj3;
17008 vinfos[4].jointtype = 1;
17009 vinfos[4].foffset = j4;
17010 vinfos[4].indices[0] = _ij4[0];
17011 vinfos[4].indices[1] = _ij4[1];
17012 vinfos[4].maxsolutions = _nj4;
17013 vinfos[5].jointtype = 1;
17014 vinfos[5].foffset = j5;
17015 vinfos[5].indices[0] = _ij5[0];
17016 vinfos[5].indices[1] = _ij5[1];
17017 vinfos[5].maxsolutions = _nj5;
17018 std::vector<int> vfree(0);
17019 solutions.AddSolution(vinfos,vfree);
17020 }
17021 }
17022 }
17023 
17024 }
17025 
17026 }
17027 
17028 }
17029 } while(0);
17030 if( bgotonextstatement )
17031 {
17032 bool bgotonextstatement = true;
17033 do
17034 {
17035 evalcond[0]=IKabs(new_r10);
17036 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17037 {
17038 bgotonextstatement=false;
17039 {
17040 IkReal j3eval[1];
17041 IkReal x1345=((-1.0)*new_r00);
17042 CheckValue<IkReal> x1347 = IKatan2WithCheck(IkReal(x1345),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17043 if(!x1347.valid){
17044 continue;
17045 }
17046 IkReal x1346=((1.0)*(x1347.value));
17047 sj4=0;
17048 cj4=-1.0;
17049 j4=3.14159265358979;
17050 sj5=gconst22;
17051 cj5=gconst23;
17052 j5=((3.14159265)+(((-1.0)*x1346)));
17053 new_r10=0;
17054 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1346)));
17055 IkReal x1348 = new_r00*new_r00;
17056 if(IKabs(x1348)==0){
17057 continue;
17058 }
17059 IkReal gconst22=(x1345*(pow(x1348,-0.5)));
17060 IkReal gconst23=0;
17061 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
17062 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17063 {
17064 {
17065 IkReal j3eval[1];
17066 IkReal x1349=((-1.0)*new_r00);
17067 CheckValue<IkReal> x1351 = IKatan2WithCheck(IkReal(x1349),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17068 if(!x1351.valid){
17069 continue;
17070 }
17071 IkReal x1350=((1.0)*(x1351.value));
17072 sj4=0;
17073 cj4=-1.0;
17074 j4=3.14159265358979;
17075 sj5=gconst22;
17076 cj5=gconst23;
17077 j5=((3.14159265)+(((-1.0)*x1350)));
17078 new_r10=0;
17079 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1350)));
17080 IkReal x1352 = new_r00*new_r00;
17081 if(IKabs(x1352)==0){
17082 continue;
17083 }
17084 IkReal gconst22=(x1349*(pow(x1352,-0.5)));
17085 IkReal gconst23=0;
17086 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
17087 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17088 {
17089 {
17090 IkReal j3eval[1];
17091 IkReal x1353=((-1.0)*new_r00);
17092 CheckValue<IkReal> x1355 = IKatan2WithCheck(IkReal(x1353),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17093 if(!x1355.valid){
17094 continue;
17095 }
17096 IkReal x1354=((1.0)*(x1355.value));
17097 sj4=0;
17098 cj4=-1.0;
17099 j4=3.14159265358979;
17100 sj5=gconst22;
17101 cj5=gconst23;
17102 j5=((3.14159265)+(((-1.0)*x1354)));
17103 new_r10=0;
17104 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1354)));
17105 IkReal x1356 = new_r00*new_r00;
17106 if(IKabs(x1356)==0){
17107 continue;
17108 }
17109 IkReal gconst22=(x1353*(pow(x1356,-0.5)));
17110 IkReal gconst23=0;
17111 j3eval[0]=new_r00;
17112 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17113 {
17114 continue; // 3 cases reached
17115 
17116 } else
17117 {
17118 {
17119 IkReal j3array[1], cj3array[1], sj3array[1];
17120 bool j3valid[1]={false};
17121 _nj3 = 1;
17122 CheckValue<IkReal> x1357=IKPowWithIntegerCheck(new_r00,-1);
17123 if(!x1357.valid){
17124 continue;
17125 }
17126 CheckValue<IkReal> x1358=IKPowWithIntegerCheck(gconst22,-1);
17127 if(!x1358.valid){
17128 continue;
17129 }
17130 if( IKabs(((-1.0)*gconst22*(x1357.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1358.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1357.value)))+IKsqr((new_r01*(x1358.value)))-1) <= IKFAST_SINCOS_THRESH )
17131  continue;
17132 j3array[0]=IKatan2(((-1.0)*gconst22*(x1357.value)), (new_r01*(x1358.value)));
17133 sj3array[0]=IKsin(j3array[0]);
17134 cj3array[0]=IKcos(j3array[0]);
17135 if( j3array[0] > IKPI )
17136 {
17137  j3array[0]-=IK2PI;
17138 }
17139 else if( j3array[0] < -IKPI )
17140 { j3array[0]+=IK2PI;
17141 }
17142 j3valid[0] = true;
17143 for(int ij3 = 0; ij3 < 1; ++ij3)
17144 {
17145 if( !j3valid[ij3] )
17146 {
17147  continue;
17148 }
17149 _ij3[0] = ij3; _ij3[1] = -1;
17150 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17151 {
17152 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17153 {
17154  j3valid[iij3]=false; _ij3[1] = iij3; break;
17155 }
17156 }
17157 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17158 {
17159 IkReal evalcond[8];
17160 IkReal x1359=IKcos(j3);
17161 IkReal x1360=IKsin(j3);
17162 IkReal x1361=(gconst22*x1360);
17163 IkReal x1362=((1.0)*x1359);
17164 evalcond[0]=(new_r00*x1359);
17165 evalcond[1]=((-1.0)*gconst22*x1359);
17166 evalcond[2]=(gconst22+((new_r00*x1360)));
17167 evalcond[3]=(x1361+new_r00);
17168 evalcond[4]=((((-1.0)*gconst22*x1362))+new_r01);
17169 evalcond[5]=((((-1.0)*x1361))+new_r11);
17170 evalcond[6]=((((-1.0)*new_r11*x1362))+((new_r01*x1360)));
17171 evalcond[7]=(((new_r01*x1359))+(((-1.0)*gconst22))+((new_r11*x1360)));
17172 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17173 {
17174 continue;
17175 }
17176 }
17177 
17178 {
17179 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17180 vinfos[0].jointtype = 1;
17181 vinfos[0].foffset = j0;
17182 vinfos[0].indices[0] = _ij0[0];
17183 vinfos[0].indices[1] = _ij0[1];
17184 vinfos[0].maxsolutions = _nj0;
17185 vinfos[1].jointtype = 1;
17186 vinfos[1].foffset = j1;
17187 vinfos[1].indices[0] = _ij1[0];
17188 vinfos[1].indices[1] = _ij1[1];
17189 vinfos[1].maxsolutions = _nj1;
17190 vinfos[2].jointtype = 1;
17191 vinfos[2].foffset = j2;
17192 vinfos[2].indices[0] = _ij2[0];
17193 vinfos[2].indices[1] = _ij2[1];
17194 vinfos[2].maxsolutions = _nj2;
17195 vinfos[3].jointtype = 1;
17196 vinfos[3].foffset = j3;
17197 vinfos[3].indices[0] = _ij3[0];
17198 vinfos[3].indices[1] = _ij3[1];
17199 vinfos[3].maxsolutions = _nj3;
17200 vinfos[4].jointtype = 1;
17201 vinfos[4].foffset = j4;
17202 vinfos[4].indices[0] = _ij4[0];
17203 vinfos[4].indices[1] = _ij4[1];
17204 vinfos[4].maxsolutions = _nj4;
17205 vinfos[5].jointtype = 1;
17206 vinfos[5].foffset = j5;
17207 vinfos[5].indices[0] = _ij5[0];
17208 vinfos[5].indices[1] = _ij5[1];
17209 vinfos[5].maxsolutions = _nj5;
17210 std::vector<int> vfree(0);
17211 solutions.AddSolution(vinfos,vfree);
17212 }
17213 }
17214 }
17215 
17216 }
17217 
17218 }
17219 
17220 } else
17221 {
17222 {
17223 IkReal j3array[1], cj3array[1], sj3array[1];
17224 bool j3valid[1]={false};
17225 _nj3 = 1;
17226 CheckValue<IkReal> x1363=IKPowWithIntegerCheck(IKsign(gconst22),-1);
17227 if(!x1363.valid){
17228 continue;
17229 }
17230 CheckValue<IkReal> x1364 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17231 if(!x1364.valid){
17232 continue;
17233 }
17234 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1363.value)))+(x1364.value));
17235 sj3array[0]=IKsin(j3array[0]);
17236 cj3array[0]=IKcos(j3array[0]);
17237 if( j3array[0] > IKPI )
17238 {
17239  j3array[0]-=IK2PI;
17240 }
17241 else if( j3array[0] < -IKPI )
17242 { j3array[0]+=IK2PI;
17243 }
17244 j3valid[0] = true;
17245 for(int ij3 = 0; ij3 < 1; ++ij3)
17246 {
17247 if( !j3valid[ij3] )
17248 {
17249  continue;
17250 }
17251 _ij3[0] = ij3; _ij3[1] = -1;
17252 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17253 {
17254 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17255 {
17256  j3valid[iij3]=false; _ij3[1] = iij3; break;
17257 }
17258 }
17259 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17260 {
17261 IkReal evalcond[8];
17262 IkReal x1365=IKcos(j3);
17263 IkReal x1366=IKsin(j3);
17264 IkReal x1367=(gconst22*x1366);
17265 IkReal x1368=((1.0)*x1365);
17266 evalcond[0]=(new_r00*x1365);
17267 evalcond[1]=((-1.0)*gconst22*x1365);
17268 evalcond[2]=(gconst22+((new_r00*x1366)));
17269 evalcond[3]=(x1367+new_r00);
17270 evalcond[4]=((((-1.0)*gconst22*x1368))+new_r01);
17271 evalcond[5]=((((-1.0)*x1367))+new_r11);
17272 evalcond[6]=((((-1.0)*new_r11*x1368))+((new_r01*x1366)));
17273 evalcond[7]=((((-1.0)*gconst22))+((new_r01*x1365))+((new_r11*x1366)));
17274 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17275 {
17276 continue;
17277 }
17278 }
17279 
17280 {
17281 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17282 vinfos[0].jointtype = 1;
17283 vinfos[0].foffset = j0;
17284 vinfos[0].indices[0] = _ij0[0];
17285 vinfos[0].indices[1] = _ij0[1];
17286 vinfos[0].maxsolutions = _nj0;
17287 vinfos[1].jointtype = 1;
17288 vinfos[1].foffset = j1;
17289 vinfos[1].indices[0] = _ij1[0];
17290 vinfos[1].indices[1] = _ij1[1];
17291 vinfos[1].maxsolutions = _nj1;
17292 vinfos[2].jointtype = 1;
17293 vinfos[2].foffset = j2;
17294 vinfos[2].indices[0] = _ij2[0];
17295 vinfos[2].indices[1] = _ij2[1];
17296 vinfos[2].maxsolutions = _nj2;
17297 vinfos[3].jointtype = 1;
17298 vinfos[3].foffset = j3;
17299 vinfos[3].indices[0] = _ij3[0];
17300 vinfos[3].indices[1] = _ij3[1];
17301 vinfos[3].maxsolutions = _nj3;
17302 vinfos[4].jointtype = 1;
17303 vinfos[4].foffset = j4;
17304 vinfos[4].indices[0] = _ij4[0];
17305 vinfos[4].indices[1] = _ij4[1];
17306 vinfos[4].maxsolutions = _nj4;
17307 vinfos[5].jointtype = 1;
17308 vinfos[5].foffset = j5;
17309 vinfos[5].indices[0] = _ij5[0];
17310 vinfos[5].indices[1] = _ij5[1];
17311 vinfos[5].maxsolutions = _nj5;
17312 std::vector<int> vfree(0);
17313 solutions.AddSolution(vinfos,vfree);
17314 }
17315 }
17316 }
17317 
17318 }
17319 
17320 }
17321 
17322 } else
17323 {
17324 {
17325 IkReal j3array[1], cj3array[1], sj3array[1];
17326 bool j3valid[1]={false};
17327 _nj3 = 1;
17328 CheckValue<IkReal> x1369=IKPowWithIntegerCheck(IKsign(gconst22),-1);
17329 if(!x1369.valid){
17330 continue;
17331 }
17332 CheckValue<IkReal> x1370 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17333 if(!x1370.valid){
17334 continue;
17335 }
17336 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1369.value)))+(x1370.value));
17337 sj3array[0]=IKsin(j3array[0]);
17338 cj3array[0]=IKcos(j3array[0]);
17339 if( j3array[0] > IKPI )
17340 {
17341  j3array[0]-=IK2PI;
17342 }
17343 else if( j3array[0] < -IKPI )
17344 { j3array[0]+=IK2PI;
17345 }
17346 j3valid[0] = true;
17347 for(int ij3 = 0; ij3 < 1; ++ij3)
17348 {
17349 if( !j3valid[ij3] )
17350 {
17351  continue;
17352 }
17353 _ij3[0] = ij3; _ij3[1] = -1;
17354 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17355 {
17356 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17357 {
17358  j3valid[iij3]=false; _ij3[1] = iij3; break;
17359 }
17360 }
17361 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17362 {
17363 IkReal evalcond[8];
17364 IkReal x1371=IKcos(j3);
17365 IkReal x1372=IKsin(j3);
17366 IkReal x1373=(gconst22*x1372);
17367 IkReal x1374=((1.0)*x1371);
17368 evalcond[0]=(new_r00*x1371);
17369 evalcond[1]=((-1.0)*gconst22*x1371);
17370 evalcond[2]=(gconst22+((new_r00*x1372)));
17371 evalcond[3]=(x1373+new_r00);
17372 evalcond[4]=(new_r01+(((-1.0)*gconst22*x1374)));
17373 evalcond[5]=((((-1.0)*x1373))+new_r11);
17374 evalcond[6]=((((-1.0)*new_r11*x1374))+((new_r01*x1372)));
17375 evalcond[7]=(((new_r11*x1372))+(((-1.0)*gconst22))+((new_r01*x1371)));
17376 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17377 {
17378 continue;
17379 }
17380 }
17381 
17382 {
17383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17384 vinfos[0].jointtype = 1;
17385 vinfos[0].foffset = j0;
17386 vinfos[0].indices[0] = _ij0[0];
17387 vinfos[0].indices[1] = _ij0[1];
17388 vinfos[0].maxsolutions = _nj0;
17389 vinfos[1].jointtype = 1;
17390 vinfos[1].foffset = j1;
17391 vinfos[1].indices[0] = _ij1[0];
17392 vinfos[1].indices[1] = _ij1[1];
17393 vinfos[1].maxsolutions = _nj1;
17394 vinfos[2].jointtype = 1;
17395 vinfos[2].foffset = j2;
17396 vinfos[2].indices[0] = _ij2[0];
17397 vinfos[2].indices[1] = _ij2[1];
17398 vinfos[2].maxsolutions = _nj2;
17399 vinfos[3].jointtype = 1;
17400 vinfos[3].foffset = j3;
17401 vinfos[3].indices[0] = _ij3[0];
17402 vinfos[3].indices[1] = _ij3[1];
17403 vinfos[3].maxsolutions = _nj3;
17404 vinfos[4].jointtype = 1;
17405 vinfos[4].foffset = j4;
17406 vinfos[4].indices[0] = _ij4[0];
17407 vinfos[4].indices[1] = _ij4[1];
17408 vinfos[4].maxsolutions = _nj4;
17409 vinfos[5].jointtype = 1;
17410 vinfos[5].foffset = j5;
17411 vinfos[5].indices[0] = _ij5[0];
17412 vinfos[5].indices[1] = _ij5[1];
17413 vinfos[5].maxsolutions = _nj5;
17414 std::vector<int> vfree(0);
17415 solutions.AddSolution(vinfos,vfree);
17416 }
17417 }
17418 }
17419 
17420 }
17421 
17422 }
17423 
17424 }
17425 } while(0);
17426 if( bgotonextstatement )
17427 {
17428 bool bgotonextstatement = true;
17429 do
17430 {
17431 if( 1 )
17432 {
17433 bgotonextstatement=false;
17434 continue; // branch miss [j3]
17435 
17436 }
17437 } while(0);
17438 if( bgotonextstatement )
17439 {
17440 }
17441 }
17442 }
17443 }
17444 }
17445 }
17446 }
17447 
17448 } else
17449 {
17450 {
17451 IkReal j3array[1], cj3array[1], sj3array[1];
17452 bool j3valid[1]={false};
17453 _nj3 = 1;
17454 IkReal x1375=((1.0)*gconst23);
17455 CheckValue<IkReal> x1376 = IKatan2WithCheck(IkReal(((gconst23*gconst23)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst22*x1375)))),IKFAST_ATAN2_MAGTHRESH);
17456 if(!x1376.valid){
17457 continue;
17458 }
17459 CheckValue<IkReal> x1377=IKPowWithIntegerCheck(IKsign((((gconst22*new_r00))+(((-1.0)*new_r10*x1375)))),-1);
17460 if(!x1377.valid){
17461 continue;
17462 }
17463 j3array[0]=((-1.5707963267949)+(x1376.value)+(((1.5707963267949)*(x1377.value))));
17464 sj3array[0]=IKsin(j3array[0]);
17465 cj3array[0]=IKcos(j3array[0]);
17466 if( j3array[0] > IKPI )
17467 {
17468  j3array[0]-=IK2PI;
17469 }
17470 else if( j3array[0] < -IKPI )
17471 { j3array[0]+=IK2PI;
17472 }
17473 j3valid[0] = true;
17474 for(int ij3 = 0; ij3 < 1; ++ij3)
17475 {
17476 if( !j3valid[ij3] )
17477 {
17478  continue;
17479 }
17480 _ij3[0] = ij3; _ij3[1] = -1;
17481 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17482 {
17483 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17484 {
17485  j3valid[iij3]=false; _ij3[1] = iij3; break;
17486 }
17487 }
17488 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17489 {
17490 IkReal evalcond[8];
17491 IkReal x1378=IKsin(j3);
17492 IkReal x1379=IKcos(j3);
17493 IkReal x1380=((1.0)*gconst22);
17494 IkReal x1381=(gconst23*x1378);
17495 IkReal x1382=((1.0)*x1379);
17496 IkReal x1383=(x1379*x1380);
17497 evalcond[0]=(gconst23+((new_r10*x1378))+((new_r00*x1379)));
17498 evalcond[1]=(((gconst23*x1379))+((gconst22*x1378))+new_r00);
17499 evalcond[2]=(gconst22+(((-1.0)*new_r10*x1382))+((new_r00*x1378)));
17500 evalcond[3]=(gconst23+(((-1.0)*new_r11*x1382))+((new_r01*x1378)));
17501 evalcond[4]=((((-1.0)*x1383))+x1381+new_r01);
17502 evalcond[5]=((((-1.0)*x1383))+x1381+new_r10);
17503 evalcond[6]=((((-1.0)*x1380))+((new_r11*x1378))+((new_r01*x1379)));
17504 evalcond[7]=((((-1.0)*gconst23*x1382))+new_r11+(((-1.0)*x1378*x1380)));
17505 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17506 {
17507 continue;
17508 }
17509 }
17510 
17511 {
17512 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17513 vinfos[0].jointtype = 1;
17514 vinfos[0].foffset = j0;
17515 vinfos[0].indices[0] = _ij0[0];
17516 vinfos[0].indices[1] = _ij0[1];
17517 vinfos[0].maxsolutions = _nj0;
17518 vinfos[1].jointtype = 1;
17519 vinfos[1].foffset = j1;
17520 vinfos[1].indices[0] = _ij1[0];
17521 vinfos[1].indices[1] = _ij1[1];
17522 vinfos[1].maxsolutions = _nj1;
17523 vinfos[2].jointtype = 1;
17524 vinfos[2].foffset = j2;
17525 vinfos[2].indices[0] = _ij2[0];
17526 vinfos[2].indices[1] = _ij2[1];
17527 vinfos[2].maxsolutions = _nj2;
17528 vinfos[3].jointtype = 1;
17529 vinfos[3].foffset = j3;
17530 vinfos[3].indices[0] = _ij3[0];
17531 vinfos[3].indices[1] = _ij3[1];
17532 vinfos[3].maxsolutions = _nj3;
17533 vinfos[4].jointtype = 1;
17534 vinfos[4].foffset = j4;
17535 vinfos[4].indices[0] = _ij4[0];
17536 vinfos[4].indices[1] = _ij4[1];
17537 vinfos[4].maxsolutions = _nj4;
17538 vinfos[5].jointtype = 1;
17539 vinfos[5].foffset = j5;
17540 vinfos[5].indices[0] = _ij5[0];
17541 vinfos[5].indices[1] = _ij5[1];
17542 vinfos[5].maxsolutions = _nj5;
17543 std::vector<int> vfree(0);
17544 solutions.AddSolution(vinfos,vfree);
17545 }
17546 }
17547 }
17548 
17549 }
17550 
17551 }
17552 
17553 } else
17554 {
17555 {
17556 IkReal j3array[1], cj3array[1], sj3array[1];
17557 bool j3valid[1]={false};
17558 _nj3 = 1;
17559 IkReal x1384=((1.0)*gconst22);
17560 CheckValue<IkReal> x1385=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst23*new_r01))+(((-1.0)*new_r11*x1384)))),-1);
17561 if(!x1385.valid){
17562 continue;
17563 }
17564 CheckValue<IkReal> x1386 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst23*gconst23))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst23*x1384)))),IKFAST_ATAN2_MAGTHRESH);
17565 if(!x1386.valid){
17566 continue;
17567 }
17568 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.value)))+(x1386.value));
17569 sj3array[0]=IKsin(j3array[0]);
17570 cj3array[0]=IKcos(j3array[0]);
17571 if( j3array[0] > IKPI )
17572 {
17573  j3array[0]-=IK2PI;
17574 }
17575 else if( j3array[0] < -IKPI )
17576 { j3array[0]+=IK2PI;
17577 }
17578 j3valid[0] = true;
17579 for(int ij3 = 0; ij3 < 1; ++ij3)
17580 {
17581 if( !j3valid[ij3] )
17582 {
17583  continue;
17584 }
17585 _ij3[0] = ij3; _ij3[1] = -1;
17586 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17587 {
17588 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17589 {
17590  j3valid[iij3]=false; _ij3[1] = iij3; break;
17591 }
17592 }
17593 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17594 {
17595 IkReal evalcond[8];
17596 IkReal x1387=IKsin(j3);
17597 IkReal x1388=IKcos(j3);
17598 IkReal x1389=((1.0)*gconst22);
17599 IkReal x1390=(gconst23*x1387);
17600 IkReal x1391=((1.0)*x1388);
17601 IkReal x1392=(x1388*x1389);
17602 evalcond[0]=(((new_r10*x1387))+gconst23+((new_r00*x1388)));
17603 evalcond[1]=(((gconst22*x1387))+((gconst23*x1388))+new_r00);
17604 evalcond[2]=(gconst22+((new_r00*x1387))+(((-1.0)*new_r10*x1391)));
17605 evalcond[3]=(gconst23+((new_r01*x1387))+(((-1.0)*new_r11*x1391)));
17606 evalcond[4]=(x1390+new_r01+(((-1.0)*x1392)));
17607 evalcond[5]=(x1390+new_r10+(((-1.0)*x1392)));
17608 evalcond[6]=((((-1.0)*x1389))+((new_r11*x1387))+((new_r01*x1388)));
17609 evalcond[7]=((((-1.0)*gconst23*x1391))+new_r11+(((-1.0)*x1387*x1389)));
17610 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17611 {
17612 continue;
17613 }
17614 }
17615 
17616 {
17617 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17618 vinfos[0].jointtype = 1;
17619 vinfos[0].foffset = j0;
17620 vinfos[0].indices[0] = _ij0[0];
17621 vinfos[0].indices[1] = _ij0[1];
17622 vinfos[0].maxsolutions = _nj0;
17623 vinfos[1].jointtype = 1;
17624 vinfos[1].foffset = j1;
17625 vinfos[1].indices[0] = _ij1[0];
17626 vinfos[1].indices[1] = _ij1[1];
17627 vinfos[1].maxsolutions = _nj1;
17628 vinfos[2].jointtype = 1;
17629 vinfos[2].foffset = j2;
17630 vinfos[2].indices[0] = _ij2[0];
17631 vinfos[2].indices[1] = _ij2[1];
17632 vinfos[2].maxsolutions = _nj2;
17633 vinfos[3].jointtype = 1;
17634 vinfos[3].foffset = j3;
17635 vinfos[3].indices[0] = _ij3[0];
17636 vinfos[3].indices[1] = _ij3[1];
17637 vinfos[3].maxsolutions = _nj3;
17638 vinfos[4].jointtype = 1;
17639 vinfos[4].foffset = j4;
17640 vinfos[4].indices[0] = _ij4[0];
17641 vinfos[4].indices[1] = _ij4[1];
17642 vinfos[4].maxsolutions = _nj4;
17643 vinfos[5].jointtype = 1;
17644 vinfos[5].foffset = j5;
17645 vinfos[5].indices[0] = _ij5[0];
17646 vinfos[5].indices[1] = _ij5[1];
17647 vinfos[5].maxsolutions = _nj5;
17648 std::vector<int> vfree(0);
17649 solutions.AddSolution(vinfos,vfree);
17650 }
17651 }
17652 }
17653 
17654 }
17655 
17656 }
17657 
17658 } else
17659 {
17660 {
17661 IkReal j3array[1], cj3array[1], sj3array[1];
17662 bool j3valid[1]={false};
17663 _nj3 = 1;
17664 IkReal x1393=((1.0)*new_r10);
17665 CheckValue<IkReal> x1394 = IKatan2WithCheck(IkReal((((gconst23*new_r11))+((gconst23*new_r00)))),IkReal((((gconst23*new_r01))+(((-1.0)*gconst23*x1393)))),IKFAST_ATAN2_MAGTHRESH);
17666 if(!x1394.valid){
17667 continue;
17668 }
17669 CheckValue<IkReal> x1395=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x1393))+(((-1.0)*new_r00*new_r01)))),-1);
17670 if(!x1395.valid){
17671 continue;
17672 }
17673 j3array[0]=((-1.5707963267949)+(x1394.value)+(((1.5707963267949)*(x1395.value))));
17674 sj3array[0]=IKsin(j3array[0]);
17675 cj3array[0]=IKcos(j3array[0]);
17676 if( j3array[0] > IKPI )
17677 {
17678  j3array[0]-=IK2PI;
17679 }
17680 else if( j3array[0] < -IKPI )
17681 { j3array[0]+=IK2PI;
17682 }
17683 j3valid[0] = true;
17684 for(int ij3 = 0; ij3 < 1; ++ij3)
17685 {
17686 if( !j3valid[ij3] )
17687 {
17688  continue;
17689 }
17690 _ij3[0] = ij3; _ij3[1] = -1;
17691 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17692 {
17693 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17694 {
17695  j3valid[iij3]=false; _ij3[1] = iij3; break;
17696 }
17697 }
17698 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17699 {
17700 IkReal evalcond[8];
17701 IkReal x1396=IKsin(j3);
17702 IkReal x1397=IKcos(j3);
17703 IkReal x1398=((1.0)*gconst22);
17704 IkReal x1399=(gconst23*x1396);
17705 IkReal x1400=((1.0)*x1397);
17706 IkReal x1401=(x1397*x1398);
17707 evalcond[0]=(((new_r10*x1396))+gconst23+((new_r00*x1397)));
17708 evalcond[1]=(((gconst22*x1396))+((gconst23*x1397))+new_r00);
17709 evalcond[2]=(gconst22+((new_r00*x1396))+(((-1.0)*new_r10*x1400)));
17710 evalcond[3]=(gconst23+((new_r01*x1396))+(((-1.0)*new_r11*x1400)));
17711 evalcond[4]=(x1399+(((-1.0)*x1401))+new_r01);
17712 evalcond[5]=(x1399+(((-1.0)*x1401))+new_r10);
17713 evalcond[6]=(((new_r11*x1396))+((new_r01*x1397))+(((-1.0)*x1398)));
17714 evalcond[7]=((((-1.0)*x1396*x1398))+(((-1.0)*gconst23*x1400))+new_r11);
17715 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17716 {
17717 continue;
17718 }
17719 }
17720 
17721 {
17722 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17723 vinfos[0].jointtype = 1;
17724 vinfos[0].foffset = j0;
17725 vinfos[0].indices[0] = _ij0[0];
17726 vinfos[0].indices[1] = _ij0[1];
17727 vinfos[0].maxsolutions = _nj0;
17728 vinfos[1].jointtype = 1;
17729 vinfos[1].foffset = j1;
17730 vinfos[1].indices[0] = _ij1[0];
17731 vinfos[1].indices[1] = _ij1[1];
17732 vinfos[1].maxsolutions = _nj1;
17733 vinfos[2].jointtype = 1;
17734 vinfos[2].foffset = j2;
17735 vinfos[2].indices[0] = _ij2[0];
17736 vinfos[2].indices[1] = _ij2[1];
17737 vinfos[2].maxsolutions = _nj2;
17738 vinfos[3].jointtype = 1;
17739 vinfos[3].foffset = j3;
17740 vinfos[3].indices[0] = _ij3[0];
17741 vinfos[3].indices[1] = _ij3[1];
17742 vinfos[3].maxsolutions = _nj3;
17743 vinfos[4].jointtype = 1;
17744 vinfos[4].foffset = j4;
17745 vinfos[4].indices[0] = _ij4[0];
17746 vinfos[4].indices[1] = _ij4[1];
17747 vinfos[4].maxsolutions = _nj4;
17748 vinfos[5].jointtype = 1;
17749 vinfos[5].foffset = j5;
17750 vinfos[5].indices[0] = _ij5[0];
17751 vinfos[5].indices[1] = _ij5[1];
17752 vinfos[5].maxsolutions = _nj5;
17753 std::vector<int> vfree(0);
17754 solutions.AddSolution(vinfos,vfree);
17755 }
17756 }
17757 }
17758 
17759 }
17760 
17761 }
17762 
17763 }
17764 } while(0);
17765 if( bgotonextstatement )
17766 {
17767 bool bgotonextstatement = true;
17768 do
17769 {
17770 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17771 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17772 {
17773 bgotonextstatement=false;
17774 {
17775 IkReal j3array[1], cj3array[1], sj3array[1];
17776 bool j3valid[1]={false};
17777 _nj3 = 1;
17778 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
17779  continue;
17780 j3array[0]=IKatan2(((-1.0)*new_r00), new_r01);
17781 sj3array[0]=IKsin(j3array[0]);
17782 cj3array[0]=IKcos(j3array[0]);
17783 if( j3array[0] > IKPI )
17784 {
17785  j3array[0]-=IK2PI;
17786 }
17787 else if( j3array[0] < -IKPI )
17788 { j3array[0]+=IK2PI;
17789 }
17790 j3valid[0] = true;
17791 for(int ij3 = 0; ij3 < 1; ++ij3)
17792 {
17793 if( !j3valid[ij3] )
17794 {
17795  continue;
17796 }
17797 _ij3[0] = ij3; _ij3[1] = -1;
17798 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17799 {
17800 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17801 {
17802  j3valid[iij3]=false; _ij3[1] = iij3; break;
17803 }
17804 }
17805 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17806 {
17807 IkReal evalcond[8];
17808 IkReal x1402=IKsin(j3);
17809 IkReal x1403=IKcos(j3);
17810 IkReal x1404=((1.0)*x1403);
17811 evalcond[0]=(x1402+new_r00);
17812 evalcond[1]=((((-1.0)*x1404))+new_r01);
17813 evalcond[2]=((((-1.0)*x1402))+new_r11);
17814 evalcond[3]=((((-1.0)*x1404))+new_r10);
17815 evalcond[4]=(((new_r10*x1402))+((new_r00*x1403)));
17816 evalcond[5]=(((new_r01*x1402))+(((-1.0)*new_r11*x1404)));
17817 evalcond[6]=((-1.0)+((new_r01*x1403))+((new_r11*x1402)));
17818 evalcond[7]=((1.0)+((new_r00*x1402))+(((-1.0)*new_r10*x1404)));
17819 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17820 {
17821 continue;
17822 }
17823 }
17824 
17825 {
17826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17827 vinfos[0].jointtype = 1;
17828 vinfos[0].foffset = j0;
17829 vinfos[0].indices[0] = _ij0[0];
17830 vinfos[0].indices[1] = _ij0[1];
17831 vinfos[0].maxsolutions = _nj0;
17832 vinfos[1].jointtype = 1;
17833 vinfos[1].foffset = j1;
17834 vinfos[1].indices[0] = _ij1[0];
17835 vinfos[1].indices[1] = _ij1[1];
17836 vinfos[1].maxsolutions = _nj1;
17837 vinfos[2].jointtype = 1;
17838 vinfos[2].foffset = j2;
17839 vinfos[2].indices[0] = _ij2[0];
17840 vinfos[2].indices[1] = _ij2[1];
17841 vinfos[2].maxsolutions = _nj2;
17842 vinfos[3].jointtype = 1;
17843 vinfos[3].foffset = j3;
17844 vinfos[3].indices[0] = _ij3[0];
17845 vinfos[3].indices[1] = _ij3[1];
17846 vinfos[3].maxsolutions = _nj3;
17847 vinfos[4].jointtype = 1;
17848 vinfos[4].foffset = j4;
17849 vinfos[4].indices[0] = _ij4[0];
17850 vinfos[4].indices[1] = _ij4[1];
17851 vinfos[4].maxsolutions = _nj4;
17852 vinfos[5].jointtype = 1;
17853 vinfos[5].foffset = j5;
17854 vinfos[5].indices[0] = _ij5[0];
17855 vinfos[5].indices[1] = _ij5[1];
17856 vinfos[5].maxsolutions = _nj5;
17857 std::vector<int> vfree(0);
17858 solutions.AddSolution(vinfos,vfree);
17859 }
17860 }
17861 }
17862 
17863 }
17864 } while(0);
17865 if( bgotonextstatement )
17866 {
17867 bool bgotonextstatement = true;
17868 do
17869 {
17870 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17871 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17872 {
17873 bgotonextstatement=false;
17874 {
17875 IkReal j3array[1], cj3array[1], sj3array[1];
17876 bool j3valid[1]={false};
17877 _nj3 = 1;
17878 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
17879  continue;
17880 j3array[0]=IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17881 sj3array[0]=IKsin(j3array[0]);
17882 cj3array[0]=IKcos(j3array[0]);
17883 if( j3array[0] > IKPI )
17884 {
17885  j3array[0]-=IK2PI;
17886 }
17887 else if( j3array[0] < -IKPI )
17888 { j3array[0]+=IK2PI;
17889 }
17890 j3valid[0] = true;
17891 for(int ij3 = 0; ij3 < 1; ++ij3)
17892 {
17893 if( !j3valid[ij3] )
17894 {
17895  continue;
17896 }
17897 _ij3[0] = ij3; _ij3[1] = -1;
17898 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17899 {
17900 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17901 {
17902  j3valid[iij3]=false; _ij3[1] = iij3; break;
17903 }
17904 }
17905 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17906 {
17907 IkReal evalcond[8];
17908 IkReal x1405=IKcos(j3);
17909 IkReal x1406=IKsin(j3);
17910 IkReal x1407=((1.0)*x1405);
17911 evalcond[0]=(x1405+new_r01);
17912 evalcond[1]=(x1406+new_r11);
17913 evalcond[2]=(x1405+new_r10);
17914 evalcond[3]=((((-1.0)*x1406))+new_r00);
17915 evalcond[4]=(((new_r10*x1406))+((new_r00*x1405)));
17916 evalcond[5]=(((new_r01*x1406))+(((-1.0)*new_r11*x1407)));
17917 evalcond[6]=((1.0)+((new_r01*x1405))+((new_r11*x1406)));
17918 evalcond[7]=((-1.0)+((new_r00*x1406))+(((-1.0)*new_r10*x1407)));
17919 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17920 {
17921 continue;
17922 }
17923 }
17924 
17925 {
17926 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17927 vinfos[0].jointtype = 1;
17928 vinfos[0].foffset = j0;
17929 vinfos[0].indices[0] = _ij0[0];
17930 vinfos[0].indices[1] = _ij0[1];
17931 vinfos[0].maxsolutions = _nj0;
17932 vinfos[1].jointtype = 1;
17933 vinfos[1].foffset = j1;
17934 vinfos[1].indices[0] = _ij1[0];
17935 vinfos[1].indices[1] = _ij1[1];
17936 vinfos[1].maxsolutions = _nj1;
17937 vinfos[2].jointtype = 1;
17938 vinfos[2].foffset = j2;
17939 vinfos[2].indices[0] = _ij2[0];
17940 vinfos[2].indices[1] = _ij2[1];
17941 vinfos[2].maxsolutions = _nj2;
17942 vinfos[3].jointtype = 1;
17943 vinfos[3].foffset = j3;
17944 vinfos[3].indices[0] = _ij3[0];
17945 vinfos[3].indices[1] = _ij3[1];
17946 vinfos[3].maxsolutions = _nj3;
17947 vinfos[4].jointtype = 1;
17948 vinfos[4].foffset = j4;
17949 vinfos[4].indices[0] = _ij4[0];
17950 vinfos[4].indices[1] = _ij4[1];
17951 vinfos[4].maxsolutions = _nj4;
17952 vinfos[5].jointtype = 1;
17953 vinfos[5].foffset = j5;
17954 vinfos[5].indices[0] = _ij5[0];
17955 vinfos[5].indices[1] = _ij5[1];
17956 vinfos[5].maxsolutions = _nj5;
17957 std::vector<int> vfree(0);
17958 solutions.AddSolution(vinfos,vfree);
17959 }
17960 }
17961 }
17962 
17963 }
17964 } while(0);
17965 if( bgotonextstatement )
17966 {
17967 bool bgotonextstatement = true;
17968 do
17969 {
17970 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
17971 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17972 {
17973 bgotonextstatement=false;
17974 {
17975 IkReal j3eval[3];
17976 sj4=0;
17977 cj4=-1.0;
17978 j4=3.14159265358979;
17979 new_r11=0;
17980 new_r00=0;
17981 j3eval[0]=new_r10;
17982 j3eval[1]=IKsign(new_r10);
17983 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
17984 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
17985 {
17986 {
17987 IkReal j3eval[3];
17988 sj4=0;
17989 cj4=-1.0;
17990 j4=3.14159265358979;
17991 new_r11=0;
17992 new_r00=0;
17993 j3eval[0]=new_r01;
17994 j3eval[1]=IKsign(new_r01);
17995 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
17996 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
17997 {
17998 {
17999 IkReal j3eval[2];
18000 sj4=0;
18001 cj4=-1.0;
18002 j4=3.14159265358979;
18003 new_r11=0;
18004 new_r00=0;
18005 j3eval[0]=new_r01;
18006 j3eval[1]=new_r10;
18007 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18008 {
18009 continue; // no branches [j3]
18010 
18011 } else
18012 {
18013 {
18014 IkReal j3array[1], cj3array[1], sj3array[1];
18015 bool j3valid[1]={false};
18016 _nj3 = 1;
18017 CheckValue<IkReal> x1408=IKPowWithIntegerCheck(new_r01,-1);
18018 if(!x1408.valid){
18019 continue;
18020 }
18021 CheckValue<IkReal> x1409=IKPowWithIntegerCheck(new_r10,-1);
18022 if(!x1409.valid){
18023 continue;
18024 }
18025 if( IKabs(((-1.0)*cj5*(x1408.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x1409.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x1408.value)))+IKsqr((sj5*(x1409.value)))-1) <= IKFAST_SINCOS_THRESH )
18026  continue;
18027 j3array[0]=IKatan2(((-1.0)*cj5*(x1408.value)), (sj5*(x1409.value)));
18028 sj3array[0]=IKsin(j3array[0]);
18029 cj3array[0]=IKcos(j3array[0]);
18030 if( j3array[0] > IKPI )
18031 {
18032  j3array[0]-=IK2PI;
18033 }
18034 else if( j3array[0] < -IKPI )
18035 { j3array[0]+=IK2PI;
18036 }
18037 j3valid[0] = true;
18038 for(int ij3 = 0; ij3 < 1; ++ij3)
18039 {
18040 if( !j3valid[ij3] )
18041 {
18042  continue;
18043 }
18044 _ij3[0] = ij3; _ij3[1] = -1;
18045 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18046 {
18047 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18048 {
18049  j3valid[iij3]=false; _ij3[1] = iij3; break;
18050 }
18051 }
18052 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18053 {
18054 IkReal evalcond[7];
18055 IkReal x1410=IKsin(j3);
18056 IkReal x1411=IKcos(j3);
18057 IkReal x1412=((1.0)*sj5);
18058 IkReal x1413=(cj5*x1410);
18059 IkReal x1414=(x1411*x1412);
18060 evalcond[0]=(cj5+((new_r01*x1410)));
18061 evalcond[1]=(cj5+((new_r10*x1410)));
18062 evalcond[2]=(sj5+(((-1.0)*new_r10*x1411)));
18063 evalcond[3]=(((new_r01*x1411))+(((-1.0)*x1412)));
18064 evalcond[4]=(((sj5*x1410))+((cj5*x1411)));
18065 evalcond[5]=((((-1.0)*x1414))+x1413+new_r01);
18066 evalcond[6]=((((-1.0)*x1414))+x1413+new_r10);
18067 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18068 {
18069 continue;
18070 }
18071 }
18072 
18073 {
18074 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18075 vinfos[0].jointtype = 1;
18076 vinfos[0].foffset = j0;
18077 vinfos[0].indices[0] = _ij0[0];
18078 vinfos[0].indices[1] = _ij0[1];
18079 vinfos[0].maxsolutions = _nj0;
18080 vinfos[1].jointtype = 1;
18081 vinfos[1].foffset = j1;
18082 vinfos[1].indices[0] = _ij1[0];
18083 vinfos[1].indices[1] = _ij1[1];
18084 vinfos[1].maxsolutions = _nj1;
18085 vinfos[2].jointtype = 1;
18086 vinfos[2].foffset = j2;
18087 vinfos[2].indices[0] = _ij2[0];
18088 vinfos[2].indices[1] = _ij2[1];
18089 vinfos[2].maxsolutions = _nj2;
18090 vinfos[3].jointtype = 1;
18091 vinfos[3].foffset = j3;
18092 vinfos[3].indices[0] = _ij3[0];
18093 vinfos[3].indices[1] = _ij3[1];
18094 vinfos[3].maxsolutions = _nj3;
18095 vinfos[4].jointtype = 1;
18096 vinfos[4].foffset = j4;
18097 vinfos[4].indices[0] = _ij4[0];
18098 vinfos[4].indices[1] = _ij4[1];
18099 vinfos[4].maxsolutions = _nj4;
18100 vinfos[5].jointtype = 1;
18101 vinfos[5].foffset = j5;
18102 vinfos[5].indices[0] = _ij5[0];
18103 vinfos[5].indices[1] = _ij5[1];
18104 vinfos[5].maxsolutions = _nj5;
18105 std::vector<int> vfree(0);
18106 solutions.AddSolution(vinfos,vfree);
18107 }
18108 }
18109 }
18110 
18111 }
18112 
18113 }
18114 
18115 } else
18116 {
18117 {
18118 IkReal j3array[1], cj3array[1], sj3array[1];
18119 bool j3valid[1]={false};
18120 _nj3 = 1;
18121 CheckValue<IkReal> x1415=IKPowWithIntegerCheck(IKsign(new_r01),-1);
18122 if(!x1415.valid){
18123 continue;
18124 }
18125 CheckValue<IkReal> x1416 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18126 if(!x1416.valid){
18127 continue;
18128 }
18129 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1415.value)))+(x1416.value));
18130 sj3array[0]=IKsin(j3array[0]);
18131 cj3array[0]=IKcos(j3array[0]);
18132 if( j3array[0] > IKPI )
18133 {
18134  j3array[0]-=IK2PI;
18135 }
18136 else if( j3array[0] < -IKPI )
18137 { j3array[0]+=IK2PI;
18138 }
18139 j3valid[0] = true;
18140 for(int ij3 = 0; ij3 < 1; ++ij3)
18141 {
18142 if( !j3valid[ij3] )
18143 {
18144  continue;
18145 }
18146 _ij3[0] = ij3; _ij3[1] = -1;
18147 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18148 {
18149 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18150 {
18151  j3valid[iij3]=false; _ij3[1] = iij3; break;
18152 }
18153 }
18154 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18155 {
18156 IkReal evalcond[7];
18157 IkReal x1417=IKsin(j3);
18158 IkReal x1418=IKcos(j3);
18159 IkReal x1419=((1.0)*sj5);
18160 IkReal x1420=(cj5*x1417);
18161 IkReal x1421=(x1418*x1419);
18162 evalcond[0]=(cj5+((new_r01*x1417)));
18163 evalcond[1]=(cj5+((new_r10*x1417)));
18164 evalcond[2]=(sj5+(((-1.0)*new_r10*x1418)));
18165 evalcond[3]=(((new_r01*x1418))+(((-1.0)*x1419)));
18166 evalcond[4]=(((sj5*x1417))+((cj5*x1418)));
18167 evalcond[5]=(x1420+new_r01+(((-1.0)*x1421)));
18168 evalcond[6]=(x1420+new_r10+(((-1.0)*x1421)));
18169 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18170 {
18171 continue;
18172 }
18173 }
18174 
18175 {
18176 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18177 vinfos[0].jointtype = 1;
18178 vinfos[0].foffset = j0;
18179 vinfos[0].indices[0] = _ij0[0];
18180 vinfos[0].indices[1] = _ij0[1];
18181 vinfos[0].maxsolutions = _nj0;
18182 vinfos[1].jointtype = 1;
18183 vinfos[1].foffset = j1;
18184 vinfos[1].indices[0] = _ij1[0];
18185 vinfos[1].indices[1] = _ij1[1];
18186 vinfos[1].maxsolutions = _nj1;
18187 vinfos[2].jointtype = 1;
18188 vinfos[2].foffset = j2;
18189 vinfos[2].indices[0] = _ij2[0];
18190 vinfos[2].indices[1] = _ij2[1];
18191 vinfos[2].maxsolutions = _nj2;
18192 vinfos[3].jointtype = 1;
18193 vinfos[3].foffset = j3;
18194 vinfos[3].indices[0] = _ij3[0];
18195 vinfos[3].indices[1] = _ij3[1];
18196 vinfos[3].maxsolutions = _nj3;
18197 vinfos[4].jointtype = 1;
18198 vinfos[4].foffset = j4;
18199 vinfos[4].indices[0] = _ij4[0];
18200 vinfos[4].indices[1] = _ij4[1];
18201 vinfos[4].maxsolutions = _nj4;
18202 vinfos[5].jointtype = 1;
18203 vinfos[5].foffset = j5;
18204 vinfos[5].indices[0] = _ij5[0];
18205 vinfos[5].indices[1] = _ij5[1];
18206 vinfos[5].maxsolutions = _nj5;
18207 std::vector<int> vfree(0);
18208 solutions.AddSolution(vinfos,vfree);
18209 }
18210 }
18211 }
18212 
18213 }
18214 
18215 }
18216 
18217 } else
18218 {
18219 {
18220 IkReal j3array[1], cj3array[1], sj3array[1];
18221 bool j3valid[1]={false};
18222 _nj3 = 1;
18223 CheckValue<IkReal> x1422=IKPowWithIntegerCheck(IKsign(new_r10),-1);
18224 if(!x1422.valid){
18225 continue;
18226 }
18227 CheckValue<IkReal> x1423 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18228 if(!x1423.valid){
18229 continue;
18230 }
18231 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1422.value)))+(x1423.value));
18232 sj3array[0]=IKsin(j3array[0]);
18233 cj3array[0]=IKcos(j3array[0]);
18234 if( j3array[0] > IKPI )
18235 {
18236  j3array[0]-=IK2PI;
18237 }
18238 else if( j3array[0] < -IKPI )
18239 { j3array[0]+=IK2PI;
18240 }
18241 j3valid[0] = true;
18242 for(int ij3 = 0; ij3 < 1; ++ij3)
18243 {
18244 if( !j3valid[ij3] )
18245 {
18246  continue;
18247 }
18248 _ij3[0] = ij3; _ij3[1] = -1;
18249 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18250 {
18251 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18252 {
18253  j3valid[iij3]=false; _ij3[1] = iij3; break;
18254 }
18255 }
18256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18257 {
18258 IkReal evalcond[7];
18259 IkReal x1424=IKsin(j3);
18260 IkReal x1425=IKcos(j3);
18261 IkReal x1426=((1.0)*sj5);
18262 IkReal x1427=(cj5*x1424);
18263 IkReal x1428=(x1425*x1426);
18264 evalcond[0]=(cj5+((new_r01*x1424)));
18265 evalcond[1]=(cj5+((new_r10*x1424)));
18266 evalcond[2]=(sj5+(((-1.0)*new_r10*x1425)));
18267 evalcond[3]=(((new_r01*x1425))+(((-1.0)*x1426)));
18268 evalcond[4]=(((sj5*x1424))+((cj5*x1425)));
18269 evalcond[5]=(x1427+new_r01+(((-1.0)*x1428)));
18270 evalcond[6]=(x1427+new_r10+(((-1.0)*x1428)));
18271 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18272 {
18273 continue;
18274 }
18275 }
18276 
18277 {
18278 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18279 vinfos[0].jointtype = 1;
18280 vinfos[0].foffset = j0;
18281 vinfos[0].indices[0] = _ij0[0];
18282 vinfos[0].indices[1] = _ij0[1];
18283 vinfos[0].maxsolutions = _nj0;
18284 vinfos[1].jointtype = 1;
18285 vinfos[1].foffset = j1;
18286 vinfos[1].indices[0] = _ij1[0];
18287 vinfos[1].indices[1] = _ij1[1];
18288 vinfos[1].maxsolutions = _nj1;
18289 vinfos[2].jointtype = 1;
18290 vinfos[2].foffset = j2;
18291 vinfos[2].indices[0] = _ij2[0];
18292 vinfos[2].indices[1] = _ij2[1];
18293 vinfos[2].maxsolutions = _nj2;
18294 vinfos[3].jointtype = 1;
18295 vinfos[3].foffset = j3;
18296 vinfos[3].indices[0] = _ij3[0];
18297 vinfos[3].indices[1] = _ij3[1];
18298 vinfos[3].maxsolutions = _nj3;
18299 vinfos[4].jointtype = 1;
18300 vinfos[4].foffset = j4;
18301 vinfos[4].indices[0] = _ij4[0];
18302 vinfos[4].indices[1] = _ij4[1];
18303 vinfos[4].maxsolutions = _nj4;
18304 vinfos[5].jointtype = 1;
18305 vinfos[5].foffset = j5;
18306 vinfos[5].indices[0] = _ij5[0];
18307 vinfos[5].indices[1] = _ij5[1];
18308 vinfos[5].maxsolutions = _nj5;
18309 std::vector<int> vfree(0);
18310 solutions.AddSolution(vinfos,vfree);
18311 }
18312 }
18313 }
18314 
18315 }
18316 
18317 }
18318 
18319 }
18320 } while(0);
18321 if( bgotonextstatement )
18322 {
18323 bool bgotonextstatement = true;
18324 do
18325 {
18326 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18327 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18328 {
18329 bgotonextstatement=false;
18330 {
18331 IkReal j3eval[1];
18332 sj4=0;
18333 cj4=-1.0;
18334 j4=3.14159265358979;
18335 new_r11=0;
18336 new_r01=0;
18337 new_r22=0;
18338 new_r20=0;
18339 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18340 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18341 {
18342 continue; // no branches [j3]
18343 
18344 } else
18345 {
18346 {
18347 IkReal j3array[2], cj3array[2], sj3array[2];
18348 bool j3valid[2]={false};
18349 _nj3 = 2;
18350 CheckValue<IkReal> x1430 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
18351 if(!x1430.valid){
18352 continue;
18353 }
18354 IkReal x1429=x1430.value;
18355 j3array[0]=((-1.0)*x1429);
18356 sj3array[0]=IKsin(j3array[0]);
18357 cj3array[0]=IKcos(j3array[0]);
18358 j3array[1]=((3.14159265358979)+(((-1.0)*x1429)));
18359 sj3array[1]=IKsin(j3array[1]);
18360 cj3array[1]=IKcos(j3array[1]);
18361 if( j3array[0] > IKPI )
18362 {
18363  j3array[0]-=IK2PI;
18364 }
18365 else if( j3array[0] < -IKPI )
18366 { j3array[0]+=IK2PI;
18367 }
18368 j3valid[0] = true;
18369 if( j3array[1] > IKPI )
18370 {
18371  j3array[1]-=IK2PI;
18372 }
18373 else if( j3array[1] < -IKPI )
18374 { j3array[1]+=IK2PI;
18375 }
18376 j3valid[1] = true;
18377 for(int ij3 = 0; ij3 < 2; ++ij3)
18378 {
18379 if( !j3valid[ij3] )
18380 {
18381  continue;
18382 }
18383 _ij3[0] = ij3; _ij3[1] = -1;
18384 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18385 {
18386 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18387 {
18388  j3valid[iij3]=false; _ij3[1] = iij3; break;
18389 }
18390 }
18391 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18392 {
18393 IkReal evalcond[1];
18394 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
18395 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18396 {
18397 continue;
18398 }
18399 }
18400 
18401 {
18402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18403 vinfos[0].jointtype = 1;
18404 vinfos[0].foffset = j0;
18405 vinfos[0].indices[0] = _ij0[0];
18406 vinfos[0].indices[1] = _ij0[1];
18407 vinfos[0].maxsolutions = _nj0;
18408 vinfos[1].jointtype = 1;
18409 vinfos[1].foffset = j1;
18410 vinfos[1].indices[0] = _ij1[0];
18411 vinfos[1].indices[1] = _ij1[1];
18412 vinfos[1].maxsolutions = _nj1;
18413 vinfos[2].jointtype = 1;
18414 vinfos[2].foffset = j2;
18415 vinfos[2].indices[0] = _ij2[0];
18416 vinfos[2].indices[1] = _ij2[1];
18417 vinfos[2].maxsolutions = _nj2;
18418 vinfos[3].jointtype = 1;
18419 vinfos[3].foffset = j3;
18420 vinfos[3].indices[0] = _ij3[0];
18421 vinfos[3].indices[1] = _ij3[1];
18422 vinfos[3].maxsolutions = _nj3;
18423 vinfos[4].jointtype = 1;
18424 vinfos[4].foffset = j4;
18425 vinfos[4].indices[0] = _ij4[0];
18426 vinfos[4].indices[1] = _ij4[1];
18427 vinfos[4].maxsolutions = _nj4;
18428 vinfos[5].jointtype = 1;
18429 vinfos[5].foffset = j5;
18430 vinfos[5].indices[0] = _ij5[0];
18431 vinfos[5].indices[1] = _ij5[1];
18432 vinfos[5].maxsolutions = _nj5;
18433 std::vector<int> vfree(0);
18434 solutions.AddSolution(vinfos,vfree);
18435 }
18436 }
18437 }
18438 
18439 }
18440 
18441 }
18442 
18443 }
18444 } while(0);
18445 if( bgotonextstatement )
18446 {
18447 bool bgotonextstatement = true;
18448 do
18449 {
18450 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18451 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18452 {
18453 bgotonextstatement=false;
18454 {
18455 IkReal j3eval[1];
18456 sj4=0;
18457 cj4=-1.0;
18458 j4=3.14159265358979;
18459 new_r00=0;
18460 new_r10=0;
18461 new_r21=0;
18462 new_r22=0;
18463 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18464 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18465 {
18466 continue; // no branches [j3]
18467 
18468 } else
18469 {
18470 {
18471 IkReal j3array[2], cj3array[2], sj3array[2];
18472 bool j3valid[2]={false};
18473 _nj3 = 2;
18474 CheckValue<IkReal> x1432 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
18475 if(!x1432.valid){
18476 continue;
18477 }
18478 IkReal x1431=x1432.value;
18479 j3array[0]=((-1.0)*x1431);
18480 sj3array[0]=IKsin(j3array[0]);
18481 cj3array[0]=IKcos(j3array[0]);
18482 j3array[1]=((3.14159265358979)+(((-1.0)*x1431)));
18483 sj3array[1]=IKsin(j3array[1]);
18484 cj3array[1]=IKcos(j3array[1]);
18485 if( j3array[0] > IKPI )
18486 {
18487  j3array[0]-=IK2PI;
18488 }
18489 else if( j3array[0] < -IKPI )
18490 { j3array[0]+=IK2PI;
18491 }
18492 j3valid[0] = true;
18493 if( j3array[1] > IKPI )
18494 {
18495  j3array[1]-=IK2PI;
18496 }
18497 else if( j3array[1] < -IKPI )
18498 { j3array[1]+=IK2PI;
18499 }
18500 j3valid[1] = true;
18501 for(int ij3 = 0; ij3 < 2; ++ij3)
18502 {
18503 if( !j3valid[ij3] )
18504 {
18505  continue;
18506 }
18507 _ij3[0] = ij3; _ij3[1] = -1;
18508 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18509 {
18510 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18511 {
18512  j3valid[iij3]=false; _ij3[1] = iij3; break;
18513 }
18514 }
18515 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18516 {
18517 IkReal evalcond[1];
18518 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
18519 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18520 {
18521 continue;
18522 }
18523 }
18524 
18525 {
18526 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18527 vinfos[0].jointtype = 1;
18528 vinfos[0].foffset = j0;
18529 vinfos[0].indices[0] = _ij0[0];
18530 vinfos[0].indices[1] = _ij0[1];
18531 vinfos[0].maxsolutions = _nj0;
18532 vinfos[1].jointtype = 1;
18533 vinfos[1].foffset = j1;
18534 vinfos[1].indices[0] = _ij1[0];
18535 vinfos[1].indices[1] = _ij1[1];
18536 vinfos[1].maxsolutions = _nj1;
18537 vinfos[2].jointtype = 1;
18538 vinfos[2].foffset = j2;
18539 vinfos[2].indices[0] = _ij2[0];
18540 vinfos[2].indices[1] = _ij2[1];
18541 vinfos[2].maxsolutions = _nj2;
18542 vinfos[3].jointtype = 1;
18543 vinfos[3].foffset = j3;
18544 vinfos[3].indices[0] = _ij3[0];
18545 vinfos[3].indices[1] = _ij3[1];
18546 vinfos[3].maxsolutions = _nj3;
18547 vinfos[4].jointtype = 1;
18548 vinfos[4].foffset = j4;
18549 vinfos[4].indices[0] = _ij4[0];
18550 vinfos[4].indices[1] = _ij4[1];
18551 vinfos[4].maxsolutions = _nj4;
18552 vinfos[5].jointtype = 1;
18553 vinfos[5].foffset = j5;
18554 vinfos[5].indices[0] = _ij5[0];
18555 vinfos[5].indices[1] = _ij5[1];
18556 vinfos[5].maxsolutions = _nj5;
18557 std::vector<int> vfree(0);
18558 solutions.AddSolution(vinfos,vfree);
18559 }
18560 }
18561 }
18562 
18563 }
18564 
18565 }
18566 
18567 }
18568 } while(0);
18569 if( bgotonextstatement )
18570 {
18571 bool bgotonextstatement = true;
18572 do
18573 {
18574 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
18575 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18576 {
18577 bgotonextstatement=false;
18578 {
18579 IkReal j3eval[3];
18580 sj4=0;
18581 cj4=-1.0;
18582 j4=3.14159265358979;
18583 new_r01=0;
18584 new_r10=0;
18585 j3eval[0]=new_r00;
18586 j3eval[1]=IKsign(new_r00);
18587 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18588 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18589 {
18590 {
18591 IkReal j3eval[2];
18592 sj4=0;
18593 cj4=-1.0;
18594 j4=3.14159265358979;
18595 new_r01=0;
18596 new_r10=0;
18597 j3eval[0]=new_r00;
18598 j3eval[1]=new_r11;
18599 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18600 {
18601 continue; // no branches [j3]
18602 
18603 } else
18604 {
18605 {
18606 IkReal j3array[1], cj3array[1], sj3array[1];
18607 bool j3valid[1]={false};
18608 _nj3 = 1;
18609 CheckValue<IkReal> x1433=IKPowWithIntegerCheck(new_r00,-1);
18610 if(!x1433.valid){
18611 continue;
18612 }
18613 CheckValue<IkReal> x1434=IKPowWithIntegerCheck(new_r11,-1);
18614 if(!x1434.valid){
18615 continue;
18616 }
18617 if( IKabs(((-1.0)*sj5*(x1433.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x1434.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x1433.value)))+IKsqr((cj5*(x1434.value)))-1) <= IKFAST_SINCOS_THRESH )
18618  continue;
18619 j3array[0]=IKatan2(((-1.0)*sj5*(x1433.value)), (cj5*(x1434.value)));
18620 sj3array[0]=IKsin(j3array[0]);
18621 cj3array[0]=IKcos(j3array[0]);
18622 if( j3array[0] > IKPI )
18623 {
18624  j3array[0]-=IK2PI;
18625 }
18626 else if( j3array[0] < -IKPI )
18627 { j3array[0]+=IK2PI;
18628 }
18629 j3valid[0] = true;
18630 for(int ij3 = 0; ij3 < 1; ++ij3)
18631 {
18632 if( !j3valid[ij3] )
18633 {
18634  continue;
18635 }
18636 _ij3[0] = ij3; _ij3[1] = -1;
18637 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18638 {
18639 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18640 {
18641  j3valid[iij3]=false; _ij3[1] = iij3; break;
18642 }
18643 }
18644 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18645 {
18646 IkReal evalcond[7];
18647 IkReal x1435=IKcos(j3);
18648 IkReal x1436=IKsin(j3);
18649 IkReal x1437=((1.0)*sj5);
18650 IkReal x1438=(cj5*x1435);
18651 IkReal x1439=((1.0)*x1435);
18652 evalcond[0]=(sj5+((new_r00*x1436)));
18653 evalcond[1]=(cj5+((new_r00*x1435)));
18654 evalcond[2]=(cj5+(((-1.0)*new_r11*x1439)));
18655 evalcond[3]=(((new_r11*x1436))+(((-1.0)*x1437)));
18656 evalcond[4]=((((-1.0)*x1435*x1437))+((cj5*x1436)));
18657 evalcond[5]=(((sj5*x1436))+x1438+new_r00);
18658 evalcond[6]=((((-1.0)*x1436*x1437))+(((-1.0)*x1438))+new_r11);
18659 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18660 {
18661 continue;
18662 }
18663 }
18664 
18665 {
18666 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18667 vinfos[0].jointtype = 1;
18668 vinfos[0].foffset = j0;
18669 vinfos[0].indices[0] = _ij0[0];
18670 vinfos[0].indices[1] = _ij0[1];
18671 vinfos[0].maxsolutions = _nj0;
18672 vinfos[1].jointtype = 1;
18673 vinfos[1].foffset = j1;
18674 vinfos[1].indices[0] = _ij1[0];
18675 vinfos[1].indices[1] = _ij1[1];
18676 vinfos[1].maxsolutions = _nj1;
18677 vinfos[2].jointtype = 1;
18678 vinfos[2].foffset = j2;
18679 vinfos[2].indices[0] = _ij2[0];
18680 vinfos[2].indices[1] = _ij2[1];
18681 vinfos[2].maxsolutions = _nj2;
18682 vinfos[3].jointtype = 1;
18683 vinfos[3].foffset = j3;
18684 vinfos[3].indices[0] = _ij3[0];
18685 vinfos[3].indices[1] = _ij3[1];
18686 vinfos[3].maxsolutions = _nj3;
18687 vinfos[4].jointtype = 1;
18688 vinfos[4].foffset = j4;
18689 vinfos[4].indices[0] = _ij4[0];
18690 vinfos[4].indices[1] = _ij4[1];
18691 vinfos[4].maxsolutions = _nj4;
18692 vinfos[5].jointtype = 1;
18693 vinfos[5].foffset = j5;
18694 vinfos[5].indices[0] = _ij5[0];
18695 vinfos[5].indices[1] = _ij5[1];
18696 vinfos[5].maxsolutions = _nj5;
18697 std::vector<int> vfree(0);
18698 solutions.AddSolution(vinfos,vfree);
18699 }
18700 }
18701 }
18702 
18703 }
18704 
18705 }
18706 
18707 } else
18708 {
18709 {
18710 IkReal j3array[1], cj3array[1], sj3array[1];
18711 bool j3valid[1]={false};
18712 _nj3 = 1;
18713 CheckValue<IkReal> x1440 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(((-1.0)*cj5)),IKFAST_ATAN2_MAGTHRESH);
18714 if(!x1440.valid){
18715 continue;
18716 }
18717 CheckValue<IkReal> x1441=IKPowWithIntegerCheck(IKsign(new_r00),-1);
18718 if(!x1441.valid){
18719 continue;
18720 }
18721 j3array[0]=((-1.5707963267949)+(x1440.value)+(((1.5707963267949)*(x1441.value))));
18722 sj3array[0]=IKsin(j3array[0]);
18723 cj3array[0]=IKcos(j3array[0]);
18724 if( j3array[0] > IKPI )
18725 {
18726  j3array[0]-=IK2PI;
18727 }
18728 else if( j3array[0] < -IKPI )
18729 { j3array[0]+=IK2PI;
18730 }
18731 j3valid[0] = true;
18732 for(int ij3 = 0; ij3 < 1; ++ij3)
18733 {
18734 if( !j3valid[ij3] )
18735 {
18736  continue;
18737 }
18738 _ij3[0] = ij3; _ij3[1] = -1;
18739 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18740 {
18741 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18742 {
18743  j3valid[iij3]=false; _ij3[1] = iij3; break;
18744 }
18745 }
18746 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18747 {
18748 IkReal evalcond[7];
18749 IkReal x1442=IKcos(j3);
18750 IkReal x1443=IKsin(j3);
18751 IkReal x1444=((1.0)*sj5);
18752 IkReal x1445=(cj5*x1442);
18753 IkReal x1446=((1.0)*x1442);
18754 evalcond[0]=(sj5+((new_r00*x1443)));
18755 evalcond[1]=(cj5+((new_r00*x1442)));
18756 evalcond[2]=(cj5+(((-1.0)*new_r11*x1446)));
18757 evalcond[3]=(((new_r11*x1443))+(((-1.0)*x1444)));
18758 evalcond[4]=((((-1.0)*x1442*x1444))+((cj5*x1443)));
18759 evalcond[5]=(((sj5*x1443))+x1445+new_r00);
18760 evalcond[6]=((((-1.0)*x1445))+new_r11+(((-1.0)*x1443*x1444)));
18761 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18762 {
18763 continue;
18764 }
18765 }
18766 
18767 {
18768 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18769 vinfos[0].jointtype = 1;
18770 vinfos[0].foffset = j0;
18771 vinfos[0].indices[0] = _ij0[0];
18772 vinfos[0].indices[1] = _ij0[1];
18773 vinfos[0].maxsolutions = _nj0;
18774 vinfos[1].jointtype = 1;
18775 vinfos[1].foffset = j1;
18776 vinfos[1].indices[0] = _ij1[0];
18777 vinfos[1].indices[1] = _ij1[1];
18778 vinfos[1].maxsolutions = _nj1;
18779 vinfos[2].jointtype = 1;
18780 vinfos[2].foffset = j2;
18781 vinfos[2].indices[0] = _ij2[0];
18782 vinfos[2].indices[1] = _ij2[1];
18783 vinfos[2].maxsolutions = _nj2;
18784 vinfos[3].jointtype = 1;
18785 vinfos[3].foffset = j3;
18786 vinfos[3].indices[0] = _ij3[0];
18787 vinfos[3].indices[1] = _ij3[1];
18788 vinfos[3].maxsolutions = _nj3;
18789 vinfos[4].jointtype = 1;
18790 vinfos[4].foffset = j4;
18791 vinfos[4].indices[0] = _ij4[0];
18792 vinfos[4].indices[1] = _ij4[1];
18793 vinfos[4].maxsolutions = _nj4;
18794 vinfos[5].jointtype = 1;
18795 vinfos[5].foffset = j5;
18796 vinfos[5].indices[0] = _ij5[0];
18797 vinfos[5].indices[1] = _ij5[1];
18798 vinfos[5].maxsolutions = _nj5;
18799 std::vector<int> vfree(0);
18800 solutions.AddSolution(vinfos,vfree);
18801 }
18802 }
18803 }
18804 
18805 }
18806 
18807 }
18808 
18809 }
18810 } while(0);
18811 if( bgotonextstatement )
18812 {
18813 bool bgotonextstatement = true;
18814 do
18815 {
18816 if( 1 )
18817 {
18818 bgotonextstatement=false;
18819 continue; // branch miss [j3]
18820 
18821 }
18822 } while(0);
18823 if( bgotonextstatement )
18824 {
18825 }
18826 }
18827 }
18828 }
18829 }
18830 }
18831 }
18832 }
18833 }
18834 }
18835 }
18836 }
18837 
18838 } else
18839 {
18840 {
18841 IkReal j3array[1], cj3array[1], sj3array[1];
18842 bool j3valid[1]={false};
18843 _nj3 = 1;
18844 IkReal x1447=((1.0)*new_r00);
18845 CheckValue<IkReal> x1448 = IKatan2WithCheck(IkReal(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))),IkReal((((cj5*sj5))+(((-1.0)*new_r10*x1447)))),IKFAST_ATAN2_MAGTHRESH);
18846 if(!x1448.valid){
18847 continue;
18848 }
18849 CheckValue<IkReal> x1449=IKPowWithIntegerCheck(IKsign(((((-1.0)*sj5*x1447))+((cj5*new_r10)))),-1);
18850 if(!x1449.valid){
18851 continue;
18852 }
18853 j3array[0]=((-1.5707963267949)+(x1448.value)+(((1.5707963267949)*(x1449.value))));
18854 sj3array[0]=IKsin(j3array[0]);
18855 cj3array[0]=IKcos(j3array[0]);
18856 if( j3array[0] > IKPI )
18857 {
18858  j3array[0]-=IK2PI;
18859 }
18860 else if( j3array[0] < -IKPI )
18861 { j3array[0]+=IK2PI;
18862 }
18863 j3valid[0] = true;
18864 for(int ij3 = 0; ij3 < 1; ++ij3)
18865 {
18866 if( !j3valid[ij3] )
18867 {
18868  continue;
18869 }
18870 _ij3[0] = ij3; _ij3[1] = -1;
18871 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18872 {
18873 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18874 {
18875  j3valid[iij3]=false; _ij3[1] = iij3; break;
18876 }
18877 }
18878 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18879 {
18880 IkReal evalcond[8];
18881 IkReal x1450=IKsin(j3);
18882 IkReal x1451=IKcos(j3);
18883 IkReal x1452=((1.0)*sj5);
18884 IkReal x1453=(cj5*x1450);
18885 IkReal x1454=((1.0)*x1451);
18886 IkReal x1455=(x1451*x1452);
18887 evalcond[0]=(cj5+((new_r00*x1451))+((new_r10*x1450)));
18888 evalcond[1]=(((cj5*x1451))+((sj5*x1450))+new_r00);
18889 evalcond[2]=((((-1.0)*new_r10*x1454))+sj5+((new_r00*x1450)));
18890 evalcond[3]=(cj5+((new_r01*x1450))+(((-1.0)*new_r11*x1454)));
18891 evalcond[4]=((((-1.0)*x1455))+x1453+new_r01);
18892 evalcond[5]=((((-1.0)*x1455))+x1453+new_r10);
18893 evalcond[6]=((((-1.0)*x1452))+((new_r01*x1451))+((new_r11*x1450)));
18894 evalcond[7]=((((-1.0)*cj5*x1454))+new_r11+(((-1.0)*x1450*x1452)));
18895 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18896 {
18897 continue;
18898 }
18899 }
18900 
18901 {
18902 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18903 vinfos[0].jointtype = 1;
18904 vinfos[0].foffset = j0;
18905 vinfos[0].indices[0] = _ij0[0];
18906 vinfos[0].indices[1] = _ij0[1];
18907 vinfos[0].maxsolutions = _nj0;
18908 vinfos[1].jointtype = 1;
18909 vinfos[1].foffset = j1;
18910 vinfos[1].indices[0] = _ij1[0];
18911 vinfos[1].indices[1] = _ij1[1];
18912 vinfos[1].maxsolutions = _nj1;
18913 vinfos[2].jointtype = 1;
18914 vinfos[2].foffset = j2;
18915 vinfos[2].indices[0] = _ij2[0];
18916 vinfos[2].indices[1] = _ij2[1];
18917 vinfos[2].maxsolutions = _nj2;
18918 vinfos[3].jointtype = 1;
18919 vinfos[3].foffset = j3;
18920 vinfos[3].indices[0] = _ij3[0];
18921 vinfos[3].indices[1] = _ij3[1];
18922 vinfos[3].maxsolutions = _nj3;
18923 vinfos[4].jointtype = 1;
18924 vinfos[4].foffset = j4;
18925 vinfos[4].indices[0] = _ij4[0];
18926 vinfos[4].indices[1] = _ij4[1];
18927 vinfos[4].maxsolutions = _nj4;
18928 vinfos[5].jointtype = 1;
18929 vinfos[5].foffset = j5;
18930 vinfos[5].indices[0] = _ij5[0];
18931 vinfos[5].indices[1] = _ij5[1];
18932 vinfos[5].maxsolutions = _nj5;
18933 std::vector<int> vfree(0);
18934 solutions.AddSolution(vinfos,vfree);
18935 }
18936 }
18937 }
18938 
18939 }
18940 
18941 }
18942 
18943 } else
18944 {
18945 {
18946 IkReal j3array[1], cj3array[1], sj3array[1];
18947 bool j3valid[1]={false};
18948 _nj3 = 1;
18949 IkReal x1456=((1.0)*new_r10);
18950 CheckValue<IkReal> x1457=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x1456)))),-1);
18951 if(!x1457.valid){
18952 continue;
18953 }
18954 CheckValue<IkReal> x1458 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((cj5*cj5)+(((-1.0)*new_r01*x1456)))),IKFAST_ATAN2_MAGTHRESH);
18955 if(!x1458.valid){
18956 continue;
18957 }
18958 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1457.value)))+(x1458.value));
18959 sj3array[0]=IKsin(j3array[0]);
18960 cj3array[0]=IKcos(j3array[0]);
18961 if( j3array[0] > IKPI )
18962 {
18963  j3array[0]-=IK2PI;
18964 }
18965 else if( j3array[0] < -IKPI )
18966 { j3array[0]+=IK2PI;
18967 }
18968 j3valid[0] = true;
18969 for(int ij3 = 0; ij3 < 1; ++ij3)
18970 {
18971 if( !j3valid[ij3] )
18972 {
18973  continue;
18974 }
18975 _ij3[0] = ij3; _ij3[1] = -1;
18976 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18977 {
18978 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18979 {
18980  j3valid[iij3]=false; _ij3[1] = iij3; break;
18981 }
18982 }
18983 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18984 {
18985 IkReal evalcond[8];
18986 IkReal x1459=IKsin(j3);
18987 IkReal x1460=IKcos(j3);
18988 IkReal x1461=((1.0)*sj5);
18989 IkReal x1462=(cj5*x1459);
18990 IkReal x1463=((1.0)*x1460);
18991 IkReal x1464=(x1460*x1461);
18992 evalcond[0]=(((new_r00*x1460))+cj5+((new_r10*x1459)));
18993 evalcond[1]=(((sj5*x1459))+((cj5*x1460))+new_r00);
18994 evalcond[2]=((((-1.0)*new_r10*x1463))+sj5+((new_r00*x1459)));
18995 evalcond[3]=(cj5+((new_r01*x1459))+(((-1.0)*new_r11*x1463)));
18996 evalcond[4]=((((-1.0)*x1464))+x1462+new_r01);
18997 evalcond[5]=((((-1.0)*x1464))+x1462+new_r10);
18998 evalcond[6]=(((new_r01*x1460))+(((-1.0)*x1461))+((new_r11*x1459)));
18999 evalcond[7]=((((-1.0)*cj5*x1463))+(((-1.0)*x1459*x1461))+new_r11);
19000 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19001 {
19002 continue;
19003 }
19004 }
19005 
19006 {
19007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19008 vinfos[0].jointtype = 1;
19009 vinfos[0].foffset = j0;
19010 vinfos[0].indices[0] = _ij0[0];
19011 vinfos[0].indices[1] = _ij0[1];
19012 vinfos[0].maxsolutions = _nj0;
19013 vinfos[1].jointtype = 1;
19014 vinfos[1].foffset = j1;
19015 vinfos[1].indices[0] = _ij1[0];
19016 vinfos[1].indices[1] = _ij1[1];
19017 vinfos[1].maxsolutions = _nj1;
19018 vinfos[2].jointtype = 1;
19019 vinfos[2].foffset = j2;
19020 vinfos[2].indices[0] = _ij2[0];
19021 vinfos[2].indices[1] = _ij2[1];
19022 vinfos[2].maxsolutions = _nj2;
19023 vinfos[3].jointtype = 1;
19024 vinfos[3].foffset = j3;
19025 vinfos[3].indices[0] = _ij3[0];
19026 vinfos[3].indices[1] = _ij3[1];
19027 vinfos[3].maxsolutions = _nj3;
19028 vinfos[4].jointtype = 1;
19029 vinfos[4].foffset = j4;
19030 vinfos[4].indices[0] = _ij4[0];
19031 vinfos[4].indices[1] = _ij4[1];
19032 vinfos[4].maxsolutions = _nj4;
19033 vinfos[5].jointtype = 1;
19034 vinfos[5].foffset = j5;
19035 vinfos[5].indices[0] = _ij5[0];
19036 vinfos[5].indices[1] = _ij5[1];
19037 vinfos[5].maxsolutions = _nj5;
19038 std::vector<int> vfree(0);
19039 solutions.AddSolution(vinfos,vfree);
19040 }
19041 }
19042 }
19043 
19044 }
19045 
19046 }
19047 
19048 } else
19049 {
19050 {
19051 IkReal j3array[1], cj3array[1], sj3array[1];
19052 bool j3valid[1]={false};
19053 _nj3 = 1;
19054 IkReal x1465=((1.0)*new_r10);
19055 CheckValue<IkReal> x1466=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1465)))),-1);
19056 if(!x1466.valid){
19057 continue;
19058 }
19059 CheckValue<IkReal> x1467 = IKatan2WithCheck(IkReal((((cj5*new_r11))+((cj5*new_r00)))),IkReal(((((-1.0)*cj5*x1465))+((cj5*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
19060 if(!x1467.valid){
19061 continue;
19062 }
19063 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1466.value)))+(x1467.value));
19064 sj3array[0]=IKsin(j3array[0]);
19065 cj3array[0]=IKcos(j3array[0]);
19066 if( j3array[0] > IKPI )
19067 {
19068  j3array[0]-=IK2PI;
19069 }
19070 else if( j3array[0] < -IKPI )
19071 { j3array[0]+=IK2PI;
19072 }
19073 j3valid[0] = true;
19074 for(int ij3 = 0; ij3 < 1; ++ij3)
19075 {
19076 if( !j3valid[ij3] )
19077 {
19078  continue;
19079 }
19080 _ij3[0] = ij3; _ij3[1] = -1;
19081 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19082 {
19083 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19084 {
19085  j3valid[iij3]=false; _ij3[1] = iij3; break;
19086 }
19087 }
19088 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19089 {
19090 IkReal evalcond[8];
19091 IkReal x1468=IKsin(j3);
19092 IkReal x1469=IKcos(j3);
19093 IkReal x1470=((1.0)*sj5);
19094 IkReal x1471=(cj5*x1468);
19095 IkReal x1472=((1.0)*x1469);
19096 IkReal x1473=(x1469*x1470);
19097 evalcond[0]=(((new_r00*x1469))+cj5+((new_r10*x1468)));
19098 evalcond[1]=(((cj5*x1469))+new_r00+((sj5*x1468)));
19099 evalcond[2]=(((new_r00*x1468))+sj5+(((-1.0)*new_r10*x1472)));
19100 evalcond[3]=(((new_r01*x1468))+cj5+(((-1.0)*new_r11*x1472)));
19101 evalcond[4]=(x1471+(((-1.0)*x1473))+new_r01);
19102 evalcond[5]=(x1471+(((-1.0)*x1473))+new_r10);
19103 evalcond[6]=(((new_r01*x1469))+((new_r11*x1468))+(((-1.0)*x1470)));
19104 evalcond[7]=((((-1.0)*cj5*x1472))+(((-1.0)*x1468*x1470))+new_r11);
19105 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19106 {
19107 continue;
19108 }
19109 }
19110 
19111 {
19112 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19113 vinfos[0].jointtype = 1;
19114 vinfos[0].foffset = j0;
19115 vinfos[0].indices[0] = _ij0[0];
19116 vinfos[0].indices[1] = _ij0[1];
19117 vinfos[0].maxsolutions = _nj0;
19118 vinfos[1].jointtype = 1;
19119 vinfos[1].foffset = j1;
19120 vinfos[1].indices[0] = _ij1[0];
19121 vinfos[1].indices[1] = _ij1[1];
19122 vinfos[1].maxsolutions = _nj1;
19123 vinfos[2].jointtype = 1;
19124 vinfos[2].foffset = j2;
19125 vinfos[2].indices[0] = _ij2[0];
19126 vinfos[2].indices[1] = _ij2[1];
19127 vinfos[2].maxsolutions = _nj2;
19128 vinfos[3].jointtype = 1;
19129 vinfos[3].foffset = j3;
19130 vinfos[3].indices[0] = _ij3[0];
19131 vinfos[3].indices[1] = _ij3[1];
19132 vinfos[3].maxsolutions = _nj3;
19133 vinfos[4].jointtype = 1;
19134 vinfos[4].foffset = j4;
19135 vinfos[4].indices[0] = _ij4[0];
19136 vinfos[4].indices[1] = _ij4[1];
19137 vinfos[4].maxsolutions = _nj4;
19138 vinfos[5].jointtype = 1;
19139 vinfos[5].foffset = j5;
19140 vinfos[5].indices[0] = _ij5[0];
19141 vinfos[5].indices[1] = _ij5[1];
19142 vinfos[5].maxsolutions = _nj5;
19143 std::vector<int> vfree(0);
19144 solutions.AddSolution(vinfos,vfree);
19145 }
19146 }
19147 }
19148 
19149 }
19150 
19151 }
19152 
19153 }
19154 } while(0);
19155 if( bgotonextstatement )
19156 {
19157 bool bgotonextstatement = true;
19158 do
19159 {
19160 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
19161 if( IKabs(evalcond[0]) < 0.0000050000000000 )
19162 {
19163 bgotonextstatement=false;
19164 {
19165 IkReal j3eval[1];
19166 new_r02=0;
19167 new_r12=0;
19168 new_r20=0;
19169 new_r21=0;
19170 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
19171 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19172 {
19173 {
19174 IkReal j3eval[1];
19175 new_r02=0;
19176 new_r12=0;
19177 new_r20=0;
19178 new_r21=0;
19179 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
19180 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19181 {
19182 {
19183 IkReal j3eval[1];
19184 new_r02=0;
19185 new_r12=0;
19186 new_r20=0;
19187 new_r21=0;
19188 j3eval[0]=((IKabs((new_r11*new_r22)))+(IKabs((new_r01*new_r22))));
19189 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19190 {
19191 continue; // no branches [j3]
19192 
19193 } else
19194 {
19195 {
19196 IkReal j3array[2], cj3array[2], sj3array[2];
19197 bool j3valid[2]={false};
19198 _nj3 = 2;
19199 CheckValue<IkReal> x1475 = IKatan2WithCheck(IkReal((new_r01*new_r22)),IkReal((new_r11*new_r22)),IKFAST_ATAN2_MAGTHRESH);
19200 if(!x1475.valid){
19201 continue;
19202 }
19203 IkReal x1474=x1475.value;
19204 j3array[0]=((-1.0)*x1474);
19205 sj3array[0]=IKsin(j3array[0]);
19206 cj3array[0]=IKcos(j3array[0]);
19207 j3array[1]=((3.14159265358979)+(((-1.0)*x1474)));
19208 sj3array[1]=IKsin(j3array[1]);
19209 cj3array[1]=IKcos(j3array[1]);
19210 if( j3array[0] > IKPI )
19211 {
19212  j3array[0]-=IK2PI;
19213 }
19214 else if( j3array[0] < -IKPI )
19215 { j3array[0]+=IK2PI;
19216 }
19217 j3valid[0] = true;
19218 if( j3array[1] > IKPI )
19219 {
19220  j3array[1]-=IK2PI;
19221 }
19222 else if( j3array[1] < -IKPI )
19223 { j3array[1]+=IK2PI;
19224 }
19225 j3valid[1] = true;
19226 for(int ij3 = 0; ij3 < 2; ++ij3)
19227 {
19228 if( !j3valid[ij3] )
19229 {
19230  continue;
19231 }
19232 _ij3[0] = ij3; _ij3[1] = -1;
19233 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19234 {
19235 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19236 {
19237  j3valid[iij3]=false; _ij3[1] = iij3; break;
19238 }
19239 }
19240 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19241 {
19242 IkReal evalcond[5];
19243 IkReal x1476=IKsin(j3);
19244 IkReal x1477=IKcos(j3);
19245 IkReal x1478=(new_r00*x1477);
19246 IkReal x1479=((1.0)*x1477);
19247 IkReal x1480=(new_r10*x1476);
19248 evalcond[0]=(((new_r11*x1476))+((new_r01*x1477)));
19249 evalcond[1]=(x1478+x1480);
19250 evalcond[2]=(((new_r00*x1476))+(((-1.0)*new_r10*x1479)));
19251 evalcond[3]=((((-1.0)*new_r11*x1479))+((new_r01*x1476)));
19252 evalcond[4]=(((new_r22*x1480))+((new_r22*x1478)));
19253 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19254 {
19255 continue;
19256 }
19257 }
19258 
19259 {
19260 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19261 vinfos[0].jointtype = 1;
19262 vinfos[0].foffset = j0;
19263 vinfos[0].indices[0] = _ij0[0];
19264 vinfos[0].indices[1] = _ij0[1];
19265 vinfos[0].maxsolutions = _nj0;
19266 vinfos[1].jointtype = 1;
19267 vinfos[1].foffset = j1;
19268 vinfos[1].indices[0] = _ij1[0];
19269 vinfos[1].indices[1] = _ij1[1];
19270 vinfos[1].maxsolutions = _nj1;
19271 vinfos[2].jointtype = 1;
19272 vinfos[2].foffset = j2;
19273 vinfos[2].indices[0] = _ij2[0];
19274 vinfos[2].indices[1] = _ij2[1];
19275 vinfos[2].maxsolutions = _nj2;
19276 vinfos[3].jointtype = 1;
19277 vinfos[3].foffset = j3;
19278 vinfos[3].indices[0] = _ij3[0];
19279 vinfos[3].indices[1] = _ij3[1];
19280 vinfos[3].maxsolutions = _nj3;
19281 vinfos[4].jointtype = 1;
19282 vinfos[4].foffset = j4;
19283 vinfos[4].indices[0] = _ij4[0];
19284 vinfos[4].indices[1] = _ij4[1];
19285 vinfos[4].maxsolutions = _nj4;
19286 vinfos[5].jointtype = 1;
19287 vinfos[5].foffset = j5;
19288 vinfos[5].indices[0] = _ij5[0];
19289 vinfos[5].indices[1] = _ij5[1];
19290 vinfos[5].maxsolutions = _nj5;
19291 std::vector<int> vfree(0);
19292 solutions.AddSolution(vinfos,vfree);
19293 }
19294 }
19295 }
19296 
19297 }
19298 
19299 }
19300 
19301 } else
19302 {
19303 {
19304 IkReal j3array[2], cj3array[2], sj3array[2];
19305 bool j3valid[2]={false};
19306 _nj3 = 2;
19307 CheckValue<IkReal> x1482 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
19308 if(!x1482.valid){
19309 continue;
19310 }
19311 IkReal x1481=x1482.value;
19312 j3array[0]=((-1.0)*x1481);
19313 sj3array[0]=IKsin(j3array[0]);
19314 cj3array[0]=IKcos(j3array[0]);
19315 j3array[1]=((3.14159265358979)+(((-1.0)*x1481)));
19316 sj3array[1]=IKsin(j3array[1]);
19317 cj3array[1]=IKcos(j3array[1]);
19318 if( j3array[0] > IKPI )
19319 {
19320  j3array[0]-=IK2PI;
19321 }
19322 else if( j3array[0] < -IKPI )
19323 { j3array[0]+=IK2PI;
19324 }
19325 j3valid[0] = true;
19326 if( j3array[1] > IKPI )
19327 {
19328  j3array[1]-=IK2PI;
19329 }
19330 else if( j3array[1] < -IKPI )
19331 { j3array[1]+=IK2PI;
19332 }
19333 j3valid[1] = true;
19334 for(int ij3 = 0; ij3 < 2; ++ij3)
19335 {
19336 if( !j3valid[ij3] )
19337 {
19338  continue;
19339 }
19340 _ij3[0] = ij3; _ij3[1] = -1;
19341 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19342 {
19343 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19344 {
19345  j3valid[iij3]=false; _ij3[1] = iij3; break;
19346 }
19347 }
19348 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19349 {
19350 IkReal evalcond[5];
19351 IkReal x1483=IKsin(j3);
19352 IkReal x1484=IKcos(j3);
19353 IkReal x1485=(new_r22*x1484);
19354 IkReal x1486=(new_r22*x1483);
19355 IkReal x1487=((1.0)*x1484);
19356 evalcond[0]=(((new_r11*x1483))+((new_r01*x1484)));
19357 evalcond[1]=((((-1.0)*new_r10*x1487))+((new_r00*x1483)));
19358 evalcond[2]=((((-1.0)*new_r11*x1487))+((new_r01*x1483)));
19359 evalcond[3]=(((new_r11*x1486))+((new_r01*x1485)));
19360 evalcond[4]=(((new_r00*x1485))+((new_r10*x1486)));
19361 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19362 {
19363 continue;
19364 }
19365 }
19366 
19367 {
19368 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19369 vinfos[0].jointtype = 1;
19370 vinfos[0].foffset = j0;
19371 vinfos[0].indices[0] = _ij0[0];
19372 vinfos[0].indices[1] = _ij0[1];
19373 vinfos[0].maxsolutions = _nj0;
19374 vinfos[1].jointtype = 1;
19375 vinfos[1].foffset = j1;
19376 vinfos[1].indices[0] = _ij1[0];
19377 vinfos[1].indices[1] = _ij1[1];
19378 vinfos[1].maxsolutions = _nj1;
19379 vinfos[2].jointtype = 1;
19380 vinfos[2].foffset = j2;
19381 vinfos[2].indices[0] = _ij2[0];
19382 vinfos[2].indices[1] = _ij2[1];
19383 vinfos[2].maxsolutions = _nj2;
19384 vinfos[3].jointtype = 1;
19385 vinfos[3].foffset = j3;
19386 vinfos[3].indices[0] = _ij3[0];
19387 vinfos[3].indices[1] = _ij3[1];
19388 vinfos[3].maxsolutions = _nj3;
19389 vinfos[4].jointtype = 1;
19390 vinfos[4].foffset = j4;
19391 vinfos[4].indices[0] = _ij4[0];
19392 vinfos[4].indices[1] = _ij4[1];
19393 vinfos[4].maxsolutions = _nj4;
19394 vinfos[5].jointtype = 1;
19395 vinfos[5].foffset = j5;
19396 vinfos[5].indices[0] = _ij5[0];
19397 vinfos[5].indices[1] = _ij5[1];
19398 vinfos[5].maxsolutions = _nj5;
19399 std::vector<int> vfree(0);
19400 solutions.AddSolution(vinfos,vfree);
19401 }
19402 }
19403 }
19404 
19405 }
19406 
19407 }
19408 
19409 } else
19410 {
19411 {
19412 IkReal j3array[2], cj3array[2], sj3array[2];
19413 bool j3valid[2]={false};
19414 _nj3 = 2;
19415 CheckValue<IkReal> x1489 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
19416 if(!x1489.valid){
19417 continue;
19418 }
19419 IkReal x1488=x1489.value;
19420 j3array[0]=((-1.0)*x1488);
19421 sj3array[0]=IKsin(j3array[0]);
19422 cj3array[0]=IKcos(j3array[0]);
19423 j3array[1]=((3.14159265358979)+(((-1.0)*x1488)));
19424 sj3array[1]=IKsin(j3array[1]);
19425 cj3array[1]=IKcos(j3array[1]);
19426 if( j3array[0] > IKPI )
19427 {
19428  j3array[0]-=IK2PI;
19429 }
19430 else if( j3array[0] < -IKPI )
19431 { j3array[0]+=IK2PI;
19432 }
19433 j3valid[0] = true;
19434 if( j3array[1] > IKPI )
19435 {
19436  j3array[1]-=IK2PI;
19437 }
19438 else if( j3array[1] < -IKPI )
19439 { j3array[1]+=IK2PI;
19440 }
19441 j3valid[1] = true;
19442 for(int ij3 = 0; ij3 < 2; ++ij3)
19443 {
19444 if( !j3valid[ij3] )
19445 {
19446  continue;
19447 }
19448 _ij3[0] = ij3; _ij3[1] = -1;
19449 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19450 {
19451 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19452 {
19453  j3valid[iij3]=false; _ij3[1] = iij3; break;
19454 }
19455 }
19456 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19457 {
19458 IkReal evalcond[5];
19459 IkReal x1490=IKsin(j3);
19460 IkReal x1491=IKcos(j3);
19461 IkReal x1492=(new_r22*x1491);
19462 IkReal x1493=(new_r22*x1490);
19463 IkReal x1494=((1.0)*x1491);
19464 evalcond[0]=(((new_r00*x1491))+((new_r10*x1490)));
19465 evalcond[1]=((((-1.0)*new_r10*x1494))+((new_r00*x1490)));
19466 evalcond[2]=(((new_r01*x1490))+(((-1.0)*new_r11*x1494)));
19467 evalcond[3]=(((new_r01*x1492))+((new_r11*x1493)));
19468 evalcond[4]=(((new_r00*x1492))+((new_r10*x1493)));
19469 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19470 {
19471 continue;
19472 }
19473 }
19474 
19475 {
19476 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19477 vinfos[0].jointtype = 1;
19478 vinfos[0].foffset = j0;
19479 vinfos[0].indices[0] = _ij0[0];
19480 vinfos[0].indices[1] = _ij0[1];
19481 vinfos[0].maxsolutions = _nj0;
19482 vinfos[1].jointtype = 1;
19483 vinfos[1].foffset = j1;
19484 vinfos[1].indices[0] = _ij1[0];
19485 vinfos[1].indices[1] = _ij1[1];
19486 vinfos[1].maxsolutions = _nj1;
19487 vinfos[2].jointtype = 1;
19488 vinfos[2].foffset = j2;
19489 vinfos[2].indices[0] = _ij2[0];
19490 vinfos[2].indices[1] = _ij2[1];
19491 vinfos[2].maxsolutions = _nj2;
19492 vinfos[3].jointtype = 1;
19493 vinfos[3].foffset = j3;
19494 vinfos[3].indices[0] = _ij3[0];
19495 vinfos[3].indices[1] = _ij3[1];
19496 vinfos[3].maxsolutions = _nj3;
19497 vinfos[4].jointtype = 1;
19498 vinfos[4].foffset = j4;
19499 vinfos[4].indices[0] = _ij4[0];
19500 vinfos[4].indices[1] = _ij4[1];
19501 vinfos[4].maxsolutions = _nj4;
19502 vinfos[5].jointtype = 1;
19503 vinfos[5].foffset = j5;
19504 vinfos[5].indices[0] = _ij5[0];
19505 vinfos[5].indices[1] = _ij5[1];
19506 vinfos[5].maxsolutions = _nj5;
19507 std::vector<int> vfree(0);
19508 solutions.AddSolution(vinfos,vfree);
19509 }
19510 }
19511 }
19512 
19513 }
19514 
19515 }
19516 
19517 }
19518 } while(0);
19519 if( bgotonextstatement )
19520 {
19521 bool bgotonextstatement = true;
19522 do
19523 {
19524 if( 1 )
19525 {
19526 bgotonextstatement=false;
19527 continue; // branch miss [j3]
19528 
19529 }
19530 } while(0);
19531 if( bgotonextstatement )
19532 {
19533 }
19534 }
19535 }
19536 }
19537 }
19538 
19539 } else
19540 {
19541 {
19542 IkReal j3array[1], cj3array[1], sj3array[1];
19543 bool j3valid[1]={false};
19544 _nj3 = 1;
19545 CheckValue<IkReal> x1496=IKPowWithIntegerCheck(sj4,-1);
19546 if(!x1496.valid){
19547 continue;
19548 }
19549 IkReal x1495=x1496.value;
19550 CheckValue<IkReal> x1497=IKPowWithIntegerCheck(new_r12,-1);
19551 if(!x1497.valid){
19552 continue;
19553 }
19554 if( IKabs((x1495*(x1497.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x1495)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x1495*(x1497.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x1495))-1) <= IKFAST_SINCOS_THRESH )
19555  continue;
19556 j3array[0]=IKatan2((x1495*(x1497.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1495));
19557 sj3array[0]=IKsin(j3array[0]);
19558 cj3array[0]=IKcos(j3array[0]);
19559 if( j3array[0] > IKPI )
19560 {
19561  j3array[0]-=IK2PI;
19562 }
19563 else if( j3array[0] < -IKPI )
19564 { j3array[0]+=IK2PI;
19565 }
19566 j3valid[0] = true;
19567 for(int ij3 = 0; ij3 < 1; ++ij3)
19568 {
19569 if( !j3valid[ij3] )
19570 {
19571  continue;
19572 }
19573 _ij3[0] = ij3; _ij3[1] = -1;
19574 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19575 {
19576 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19577 {
19578  j3valid[iij3]=false; _ij3[1] = iij3; break;
19579 }
19580 }
19581 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19582 {
19583 IkReal evalcond[18];
19584 IkReal x1498=IKcos(j3);
19585 IkReal x1499=IKsin(j3);
19586 IkReal x1500=((1.0)*cj4);
19587 IkReal x1501=((1.0)*cj5);
19588 IkReal x1502=(cj4*x1498);
19589 IkReal x1503=(sj4*x1499);
19590 IkReal x1504=((1.0)*x1498);
19591 IkReal x1505=(sj4*x1498);
19592 IkReal x1506=(cj4*x1499);
19593 IkReal x1507=(cj5*x1499);
19594 evalcond[0]=(x1505+new_r02);
19595 evalcond[1]=(x1503+new_r12);
19596 evalcond[2]=((((-1.0)*new_r02*x1499))+((new_r12*x1498)));
19597 evalcond[3]=(sj4+((new_r02*x1498))+((new_r12*x1499)));
19598 evalcond[4]=(sj5+(((-1.0)*new_r10*x1504))+((new_r00*x1499)));
19599 evalcond[5]=((((-1.0)*new_r11*x1504))+cj5+((new_r01*x1499)));
19600 evalcond[6]=(((sj5*x1502))+x1507+new_r01);
19601 evalcond[7]=(((new_r01*x1498))+((cj4*sj5))+((new_r11*x1499)));
19602 evalcond[8]=(((sj5*x1499))+(((-1.0)*cj5*x1498*x1500))+new_r00);
19603 evalcond[9]=((((-1.0)*x1498*x1501))+((sj5*x1506))+new_r11);
19604 evalcond[10]=((((-1.0)*cj5*x1500))+((new_r00*x1498))+((new_r10*x1499)));
19605 evalcond[11]=((((-1.0)*sj5*x1504))+(((-1.0)*x1500*x1507))+new_r10);
19606 evalcond[12]=(((new_r02*x1502))+((new_r22*sj4))+((new_r12*x1506)));
19607 evalcond[13]=(((new_r10*x1503))+(((-1.0)*new_r20*x1500))+((new_r00*x1505)));
19608 evalcond[14]=((((-1.0)*new_r21*x1500))+((new_r01*x1505))+((new_r11*x1503)));
19609 evalcond[15]=(sj5+((new_r01*x1502))+((new_r21*sj4))+((new_r11*x1506)));
19610 evalcond[16]=((1.0)+(((-1.0)*new_r22*x1500))+((new_r02*x1505))+((new_r12*x1503)));
19611 evalcond[17]=(((new_r10*x1506))+(((-1.0)*x1501))+((new_r20*sj4))+((new_r00*x1502)));
19612 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
19613 {
19614 continue;
19615 }
19616 }
19617 
19618 {
19619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19620 vinfos[0].jointtype = 1;
19621 vinfos[0].foffset = j0;
19622 vinfos[0].indices[0] = _ij0[0];
19623 vinfos[0].indices[1] = _ij0[1];
19624 vinfos[0].maxsolutions = _nj0;
19625 vinfos[1].jointtype = 1;
19626 vinfos[1].foffset = j1;
19627 vinfos[1].indices[0] = _ij1[0];
19628 vinfos[1].indices[1] = _ij1[1];
19629 vinfos[1].maxsolutions = _nj1;
19630 vinfos[2].jointtype = 1;
19631 vinfos[2].foffset = j2;
19632 vinfos[2].indices[0] = _ij2[0];
19633 vinfos[2].indices[1] = _ij2[1];
19634 vinfos[2].maxsolutions = _nj2;
19635 vinfos[3].jointtype = 1;
19636 vinfos[3].foffset = j3;
19637 vinfos[3].indices[0] = _ij3[0];
19638 vinfos[3].indices[1] = _ij3[1];
19639 vinfos[3].maxsolutions = _nj3;
19640 vinfos[4].jointtype = 1;
19641 vinfos[4].foffset = j4;
19642 vinfos[4].indices[0] = _ij4[0];
19643 vinfos[4].indices[1] = _ij4[1];
19644 vinfos[4].maxsolutions = _nj4;
19645 vinfos[5].jointtype = 1;
19646 vinfos[5].foffset = j5;
19647 vinfos[5].indices[0] = _ij5[0];
19648 vinfos[5].indices[1] = _ij5[1];
19649 vinfos[5].maxsolutions = _nj5;
19650 std::vector<int> vfree(0);
19651 solutions.AddSolution(vinfos,vfree);
19652 }
19653 }
19654 }
19655 
19656 }
19657 
19658 }
19659 
19660 } else
19661 {
19662 {
19663 IkReal j3array[1], cj3array[1], sj3array[1];
19664 bool j3valid[1]={false};
19665 _nj3 = 1;
19666 CheckValue<IkReal> x1508=IKPowWithIntegerCheck(IKsign(sj4),-1);
19667 if(!x1508.valid){
19668 continue;
19669 }
19670 CheckValue<IkReal> x1509 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
19671 if(!x1509.valid){
19672 continue;
19673 }
19674 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1508.value)))+(x1509.value));
19675 sj3array[0]=IKsin(j3array[0]);
19676 cj3array[0]=IKcos(j3array[0]);
19677 if( j3array[0] > IKPI )
19678 {
19679  j3array[0]-=IK2PI;
19680 }
19681 else if( j3array[0] < -IKPI )
19682 { j3array[0]+=IK2PI;
19683 }
19684 j3valid[0] = true;
19685 for(int ij3 = 0; ij3 < 1; ++ij3)
19686 {
19687 if( !j3valid[ij3] )
19688 {
19689  continue;
19690 }
19691 _ij3[0] = ij3; _ij3[1] = -1;
19692 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19693 {
19694 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19695 {
19696  j3valid[iij3]=false; _ij3[1] = iij3; break;
19697 }
19698 }
19699 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19700 {
19701 IkReal evalcond[18];
19702 IkReal x1510=IKcos(j3);
19703 IkReal x1511=IKsin(j3);
19704 IkReal x1512=((1.0)*cj4);
19705 IkReal x1513=((1.0)*cj5);
19706 IkReal x1514=(cj4*x1510);
19707 IkReal x1515=(sj4*x1511);
19708 IkReal x1516=((1.0)*x1510);
19709 IkReal x1517=(sj4*x1510);
19710 IkReal x1518=(cj4*x1511);
19711 IkReal x1519=(cj5*x1511);
19712 evalcond[0]=(x1517+new_r02);
19713 evalcond[1]=(x1515+new_r12);
19714 evalcond[2]=((((-1.0)*new_r02*x1511))+((new_r12*x1510)));
19715 evalcond[3]=(sj4+((new_r02*x1510))+((new_r12*x1511)));
19716 evalcond[4]=(sj5+(((-1.0)*new_r10*x1516))+((new_r00*x1511)));
19717 evalcond[5]=((((-1.0)*new_r11*x1516))+cj5+((new_r01*x1511)));
19718 evalcond[6]=(((sj5*x1514))+x1519+new_r01);
19719 evalcond[7]=(((cj4*sj5))+((new_r01*x1510))+((new_r11*x1511)));
19720 evalcond[8]=((((-1.0)*cj5*x1510*x1512))+((sj5*x1511))+new_r00);
19721 evalcond[9]=(((sj5*x1518))+(((-1.0)*x1510*x1513))+new_r11);
19722 evalcond[10]=(((new_r10*x1511))+(((-1.0)*cj5*x1512))+((new_r00*x1510)));
19723 evalcond[11]=((((-1.0)*sj5*x1516))+(((-1.0)*x1512*x1519))+new_r10);
19724 evalcond[12]=(((new_r02*x1514))+((new_r22*sj4))+((new_r12*x1518)));
19725 evalcond[13]=(((new_r10*x1515))+(((-1.0)*new_r20*x1512))+((new_r00*x1517)));
19726 evalcond[14]=((((-1.0)*new_r21*x1512))+((new_r01*x1517))+((new_r11*x1515)));
19727 evalcond[15]=(sj5+((new_r01*x1514))+((new_r21*sj4))+((new_r11*x1518)));
19728 evalcond[16]=((1.0)+(((-1.0)*new_r22*x1512))+((new_r02*x1517))+((new_r12*x1515)));
19729 evalcond[17]=(((new_r10*x1518))+(((-1.0)*x1513))+((new_r20*sj4))+((new_r00*x1514)));
19730 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
19731 {
19732 continue;
19733 }
19734 }
19735 
19736 {
19737 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19738 vinfos[0].jointtype = 1;
19739 vinfos[0].foffset = j0;
19740 vinfos[0].indices[0] = _ij0[0];
19741 vinfos[0].indices[1] = _ij0[1];
19742 vinfos[0].maxsolutions = _nj0;
19743 vinfos[1].jointtype = 1;
19744 vinfos[1].foffset = j1;
19745 vinfos[1].indices[0] = _ij1[0];
19746 vinfos[1].indices[1] = _ij1[1];
19747 vinfos[1].maxsolutions = _nj1;
19748 vinfos[2].jointtype = 1;
19749 vinfos[2].foffset = j2;
19750 vinfos[2].indices[0] = _ij2[0];
19751 vinfos[2].indices[1] = _ij2[1];
19752 vinfos[2].maxsolutions = _nj2;
19753 vinfos[3].jointtype = 1;
19754 vinfos[3].foffset = j3;
19755 vinfos[3].indices[0] = _ij3[0];
19756 vinfos[3].indices[1] = _ij3[1];
19757 vinfos[3].maxsolutions = _nj3;
19758 vinfos[4].jointtype = 1;
19759 vinfos[4].foffset = j4;
19760 vinfos[4].indices[0] = _ij4[0];
19761 vinfos[4].indices[1] = _ij4[1];
19762 vinfos[4].maxsolutions = _nj4;
19763 vinfos[5].jointtype = 1;
19764 vinfos[5].foffset = j5;
19765 vinfos[5].indices[0] = _ij5[0];
19766 vinfos[5].indices[1] = _ij5[1];
19767 vinfos[5].maxsolutions = _nj5;
19768 std::vector<int> vfree(0);
19769 solutions.AddSolution(vinfos,vfree);
19770 }
19771 }
19772 }
19773 
19774 }
19775 
19776 }
19777 }
19778 }
19779 
19780 }
19781 
19782 }
19783 }
19784 }
19785 }
19786 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
19787 {
19788  using std::complex;
19789  if( rawcoeffs[0] == 0 ) {
19790  // solve with one reduced degree
19791  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
19792  return;
19793  }
19794  IKFAST_ASSERT(rawcoeffs[0] != 0);
19795  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19796  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19797  complex<IkReal> coeffs[3];
19798  const int maxsteps = 110;
19799  for(int i = 0; i < 3; ++i) {
19800  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19801  }
19802  complex<IkReal> roots[3];
19803  IkReal err[3];
19804  roots[0] = complex<IkReal>(1,0);
19805  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19806  err[0] = 1.0;
19807  err[1] = 1.0;
19808  for(int i = 2; i < 3; ++i) {
19809  roots[i] = roots[i-1]*roots[1];
19810  err[i] = 1.0;
19811  }
19812  for(int step = 0; step < maxsteps; ++step) {
19813  bool changed = false;
19814  for(int i = 0; i < 3; ++i) {
19815  if ( err[i] >= tol ) {
19816  changed = true;
19817  // evaluate
19818  complex<IkReal> x = roots[i] + coeffs[0];
19819  for(int j = 1; j < 3; ++j) {
19820  x = roots[i] * x + coeffs[j];
19821  }
19822  for(int j = 0; j < 3; ++j) {
19823  if( i != j ) {
19824  if( roots[i] != roots[j] ) {
19825  x /= (roots[i] - roots[j]);
19826  }
19827  }
19828  }
19829  roots[i] -= x;
19830  err[i] = abs(x);
19831  }
19832  }
19833  if( !changed ) {
19834  break;
19835  }
19836  }
19837 
19838  numroots = 0;
19839  bool visited[3] = {false};
19840  for(int i = 0; i < 3; ++i) {
19841  if( !visited[i] ) {
19842  // might be a multiple root, in which case it will have more error than the other roots
19843  // find any neighboring roots, and take the average
19844  complex<IkReal> newroot=roots[i];
19845  int n = 1;
19846  for(int j = i+1; j < 3; ++j) {
19847  // care about error in real much more than imaginary
19848  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19849  newroot += roots[j];
19850  n += 1;
19851  visited[j] = true;
19852  }
19853  }
19854  if( n > 1 ) {
19855  newroot /= n;
19856  }
19857  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
19858  if( IKabs(imag(newroot)) < tolsqrt ) {
19859  rawroots[numroots++] = real(newroot);
19860  }
19861  }
19862  }
19863 }
19864 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
19865  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19866  if( det < 0 ) {
19867  numroots=0;
19868  }
19869  else if( det == 0 ) {
19870  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19871  numroots = 1;
19872  }
19873  else {
19874  det = IKsqrt(det);
19875  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19876  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
19877  numroots = 2;
19878  }
19879 }
19880 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
19881 {
19882  using std::complex;
19883  if( rawcoeffs[0] == 0 ) {
19884  // solve with one reduced degree
19885  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19886  return;
19887  }
19888  IKFAST_ASSERT(rawcoeffs[0] != 0);
19889  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19890  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19891  complex<IkReal> coeffs[4];
19892  const int maxsteps = 110;
19893  for(int i = 0; i < 4; ++i) {
19894  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19895  }
19896  complex<IkReal> roots[4];
19897  IkReal err[4];
19898  roots[0] = complex<IkReal>(1,0);
19899  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19900  err[0] = 1.0;
19901  err[1] = 1.0;
19902  for(int i = 2; i < 4; ++i) {
19903  roots[i] = roots[i-1]*roots[1];
19904  err[i] = 1.0;
19905  }
19906  for(int step = 0; step < maxsteps; ++step) {
19907  bool changed = false;
19908  for(int i = 0; i < 4; ++i) {
19909  if ( err[i] >= tol ) {
19910  changed = true;
19911  // evaluate
19912  complex<IkReal> x = roots[i] + coeffs[0];
19913  for(int j = 1; j < 4; ++j) {
19914  x = roots[i] * x + coeffs[j];
19915  }
19916  for(int j = 0; j < 4; ++j) {
19917  if( i != j ) {
19918  if( roots[i] != roots[j] ) {
19919  x /= (roots[i] - roots[j]);
19920  }
19921  }
19922  }
19923  roots[i] -= x;
19924  err[i] = abs(x);
19925  }
19926  }
19927  if( !changed ) {
19928  break;
19929  }
19930  }
19931 
19932  numroots = 0;
19933  bool visited[4] = {false};
19934  for(int i = 0; i < 4; ++i) {
19935  if( !visited[i] ) {
19936  // might be a multiple root, in which case it will have more error than the other roots
19937  // find any neighboring roots, and take the average
19938  complex<IkReal> newroot=roots[i];
19939  int n = 1;
19940  for(int j = i+1; j < 4; ++j) {
19941  // care about error in real much more than imaginary
19942  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19943  newroot += roots[j];
19944  n += 1;
19945  visited[j] = true;
19946  }
19947  }
19948  if( n > 1 ) {
19949  newroot /= n;
19950  }
19951  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
19952  if( IKabs(imag(newroot)) < tolsqrt ) {
19953  rawroots[numroots++] = real(newroot);
19954  }
19955  }
19956  }
19957 }
19958 };
19959 
19960 
19963 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
19964 IKSolver solver;
19965 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
19966 }
19967 
19968 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
19969 IKSolver solver;
19970 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
19971 }
19972 
19973 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - fanuc_lrmate200id7l (4198bf8a131c845c474e8ef89919d50f)>"; }
19974 
19975 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
19976 
19977 #ifdef IKFAST_NAMESPACE
19978 } // end namespace
19979 #endif
19980 
19981 #ifndef IKFAST_NO_MAIN
19982 #include <stdio.h>
19983 #include <stdlib.h>
19984 #ifdef IKFAST_NAMESPACE
19985 using namespace IKFAST_NAMESPACE;
19986 #endif
19987 int main(int argc, char** argv)
19988 {
19989  if( argc != 12+GetNumFreeParameters()+1 ) {
19990  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
19991  "Returns the ik solutions given the transformation of the end effector specified by\n"
19992  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
19993  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
19994  return 1;
19995  }
19996 
19997  IkSolutionList<IkReal> solutions;
19998  std::vector<IkReal> vfree(GetNumFreeParameters());
19999  IkReal eerot[9],eetrans[3];
20000  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
20001  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
20002  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20003  for(std::size_t i = 0; i < vfree.size(); ++i)
20004  vfree[i] = atof(argv[13+i]);
20005  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
20006 
20007  if( !bSuccess ) {
20008  fprintf(stderr,"Failed to get ik solution\n");
20009  return -1;
20010  }
20011 
20012  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
20013  std::vector<IkReal> solvalues(GetNumJoints());
20014  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
20015  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
20016  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
20017  std::vector<IkReal> vsolfree(sol.GetFree().size());
20018  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
20019  for( std::size_t j = 0; j < solvalues.size(); ++j)
20020  printf("%.15f, ", solvalues[j]);
20021  printf("\n");
20022  }
20023  return 0;
20024 }
20025 
20026 #endif
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:362
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19975
dgeev_
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)
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: lrmate200id7l_kinematics/include/ikfast.h:283
IKacos
float IKacos(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:154
IKsign
float IKsign(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:225
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19973
IKasin
float IKasin(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:121
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: lrmate200id7l_kinematics/include/ikfast.h:294
IK2PI
#define IK2PI
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:59
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: lrmate200id7l_kinematics/include/ikfast.h:43
main
int main(int argc, char **argv)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19987
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: lrmate200id7l_kinematics/include/ikfast.h:70
IKcos
float IKcos(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:170
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:819
IKtan
float IKtan(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:172
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:107
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:299
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:49
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19786
IKPI
#define IKPI
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:60
IKsin
float IKsin(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:168
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:117
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19963
IKfmod
float IKfmod(float x, float y)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:137
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetIkType
IKFAST_API int GetIkType()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:367
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:112
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:361
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKPI_2
#define IKPI_2
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:61
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:376
pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19880
step
unsigned int step
dgetrs_
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)
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:246
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:363
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:211
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
IKatan2
float IKatan2(float fy, float fx)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:179
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: lrmate200id7l_kinematics/include/ikfast.h:273
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:25
IKlog
float IKlog(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:97
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:176
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
IKabs
float IKabs(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:91
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19864
x
double x
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:102
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:19968
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:365
IKsqr
float IKsqr(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:94
IKsqrt
float IKsqrt(float f)
Definition: fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp:174

fanuc_lrmate200id_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Tue Mar 23 2021 02:10:01