moveit2
The MoveIt Motion Planning Framework for ROS 2.
prbt_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
25 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
26 using namespace ikfast;
27 
28 // check if the included ikfast version matches what this file was compiled with
29 static_assert(IKFAST_VERSION==61); // version found in ikfast.h
30 
31 #include <cmath>
32 #include <vector>
33 #include <limits>
34 #include <algorithm>
35 #include <complex>
36 
37 #ifndef IKFAST_ASSERT
38 #include <stdexcept>
39 #include <sstream>
40 #include <iostream>
41 
42 #ifdef _MSC_VER
43 #ifndef __PRETTY_FUNCTION__
44 #define __PRETTY_FUNCTION__ __FUNCDNAME__
45 #endif
46 #endif
47 
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __func__
50 #endif
51 
52 #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()); } }
53 
54 #endif
55 
56 #if defined(_MSC_VER)
57 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
58 #else
59 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
60 #endif
61 
62 #define IK2PI ((IkReal)6.28318530717959)
63 #define IKPI ((IkReal)3.14159265358979)
64 #define IKPI_2 ((IkReal)1.57079632679490)
65 
66 #ifdef _MSC_VER
67 #ifndef isnan
68 #define isnan _isnan
69 #endif
70 #ifndef isinf
71 #define isinf _isinf
72 #endif
73 //#ifndef isfinite
74 //#define isfinite _isfinite
75 //#endif
76 #endif // _MSC_VER
77 
78 using namespace std; // necessary to get std math routines
79 
80 #ifdef IKFAST_NAMESPACE
81 namespace IKFAST_NAMESPACE {
82 #endif
83 
84 inline float IKabs(float f) { return fabsf(f); }
85 inline double IKabs(double f) { return fabs(f); }
86 
87 inline float IKsqr(float f) { return f*f; }
88 inline double IKsqr(double f) { return f*f; }
89 
90 inline float IKlog(float f) { return logf(f); }
91 inline double IKlog(double f) { return log(f); }
92 
93 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
94 #ifndef IKFAST_SINCOS_THRESH
95 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
96 #endif
97 
98 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
99 #ifndef IKFAST_ATAN2_MAGTHRESH
100 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
101 #endif
102 
103 // minimum distance of separate solutions
104 #ifndef IKFAST_SOLUTION_THRESH
105 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
106 #endif
107 
108 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
109 #ifndef IKFAST_EVALCOND_THRESH
110 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
111 #endif
112 
113 
114 inline float IKasin(float f)
115 {
116 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
117 if( f <= -1 ) return float(-IKPI_2);
118 else if( f >= 1 ) return float(IKPI_2);
119 return asinf(f);
120 }
121 inline double IKasin(double 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 -IKPI_2;
125 else if( f >= 1 ) return IKPI_2;
126 return asin(f);
127 }
128 
129 // return positive value in [0,y)
130 inline float IKfmod(float x, float y)
131 {
132  while(x < 0) {
133  x += y;
134  }
135  return fmodf(x,y);
136 }
137 
138 // return positive value in [0,y)
139 inline double IKfmod(double x, double y)
140 {
141  while(x < 0) {
142  x += y;
143  }
144  return fmod(x,y);
145 }
146 
147 inline float IKacos(float f)
148 {
149 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
150 if( f <= -1 ) return float(IKPI);
151 else if( f >= 1 ) return float(0);
152 return acosf(f);
153 }
154 inline double IKacos(double 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 IKPI;
158 else if( f >= 1 ) return 0;
159 return acos(f);
160 }
161 inline float IKsin(float f) { return sinf(f); }
162 inline double IKsin(double f) { return sin(f); }
163 inline float IKcos(float f) { return cosf(f); }
164 inline double IKcos(double f) { return cos(f); }
165 inline float IKtan(float f) { return tanf(f); }
166 inline double IKtan(double f) { return tan(f); }
167 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
168 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
169 inline float IKatan2Simple(float fy, float fx) {
170  return atan2f(fy,fx);
171 }
172 inline float IKatan2(float fy, float fx) {
173  if( isnan(fy) ) {
174  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
175  return float(IKPI_2);
176  }
177  else if( isnan(fx) ) {
178  return 0;
179  }
180  return atan2f(fy,fx);
181 }
182 inline double IKatan2Simple(double fy, double fx) {
183  return atan2(fy,fx);
184 }
185 inline double IKatan2(double fy, double fx) {
186  if( isnan(fy) ) {
187  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
188  return IKPI_2;
189  }
190  else if( isnan(fx) ) {
191  return 0;
192  }
193  return atan2(fy,fx);
194 }
195 
196 template <typename T>
197 struct CheckValue
198 {
199  T value;
200  bool valid;
201 };
202 
203 template <typename T>
204 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T /* unused */)
205 {
206  CheckValue<T> ret;
207  ret.valid = false;
208  ret.value = 0;
209  if( !isnan(fy) && !isnan(fx) ) {
210  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
211  ret.value = IKatan2Simple(fy,fx);
212  ret.valid = true;
213  }
214  }
215  return ret;
216 }
217 
218 inline float IKsign(float f) {
219  if( f > 0 ) {
220  return float(1);
221  }
222  else if( f < 0 ) {
223  return float(-1);
224  }
225  return 0;
226 }
227 
228 inline double IKsign(double f) {
229  if( f > 0 ) {
230  return 1.0;
231  }
232  else if( f < 0 ) {
233  return -1.0;
234  }
235  return 0;
236 }
237 
238 template <typename T>
239 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
240 {
241  CheckValue<T> ret;
242  ret.valid = true;
243  if( n == 0 ) {
244  ret.value = 1.0;
245  return ret;
246  }
247  else if( n == 1 )
248  {
249  ret.value = f;
250  return ret;
251  }
252  else if( n < 0 )
253  {
254  if( f == 0 )
255  {
256  ret.valid = false;
257  ret.value = (T)1.0e30;
258  return ret;
259  }
260  if( n == -1 ) {
261  ret.value = T(1.0)/f;
262  return ret;
263  }
264  }
265 
266  int num = n > 0 ? n : -n;
267  if( num == 2 ) {
268  ret.value = f*f;
269  }
270  else if( num == 3 ) {
271  ret.value = f*f*f;
272  }
273  else {
274  ret.value = 1.0;
275  while(num>0) {
276  if( num & 1 ) {
277  ret.value *= f;
278  }
279  num >>= 1;
280  f *= f;
281  }
282  }
283 
284  if( n < 0 ) {
285  ret.value = T(1.0)/ret.value;
286  }
287  return ret;
288 }
289 
292 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
293 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46;
294 x0=IKcos(j[0]);
295 x1=IKcos(j[1]);
296 x2=IKcos(j[2]);
297 x3=IKsin(j[1]);
298 x4=IKsin(j[2]);
299 x5=IKsin(j[3]);
300 x6=IKcos(j[3]);
301 x7=IKsin(j[0]);
302 x8=IKcos(j[5]);
303 x9=IKsin(j[5]);
304 x10=IKsin(j[4]);
305 x11=IKcos(j[4]);
306 x12=((1.0)*x7);
307 x13=((1.0)*x6);
308 x14=((0.307)*x7);
309 x15=((0.084)*x3);
310 x16=((0.084)*x7);
311 x17=((1.0)*x0);
312 x18=((1.0)*x10);
313 x19=((0.35)*x3);
314 x20=((0.084)*x6);
315 x21=((0.307)*x0);
316 x22=((0.084)*x0);
317 x23=(x1*x2);
318 x24=(x1*x4);
319 x25=((-1.0)*x6);
320 x26=(x2*x3);
321 x27=(x5*x7);
322 x28=(x3*x4);
323 x29=(x10*x6);
324 x30=(x17*x5);
325 x31=((1.0)*x26);
326 x32=((((-1.0)*x31))+x24);
327 x33=((((-1.0)*x24))+x31);
328 x34=((((1.0)*x23))+(((1.0)*x28)));
329 x35=(x11*x32);
330 x36=(x33*x5);
331 x37=(x17*(((((-1.0)*x24))+x26)));
332 x38=(x12*(((((-1.0)*x24))+x26)));
333 x39=(x17*(((((-1.0)*x23))+(((-1.0)*x28)))));
334 x40=(x12*(((((-1.0)*x23))+(((-1.0)*x28)))));
335 x41=(x10*x38);
336 x42=(x39*x5);
337 x43=(x39*x6);
338 x44=(((x0*x6))+((x40*x5)));
339 x45=(x30+((x25*x40)));
340 x46=(x11*x45);
341 eerot[0]=(((x9*(((((-1.0)*x12*x6))+x42))))+((x8*((((x10*x37))+((x11*(((((-1.0)*x27))+((x25*x39)))))))))));
342 eerot[1]=((((-1.0)*x9*((((x18*x37))+(((1.0)*x11*(((((-1.0)*x12*x5))+(((-1.0)*x13*x39))))))))))+((x8*((x42+((x25*x7)))))));
343 eerot[2]=(((x10*((x43+x27))))+((x11*x37)));
344 IkReal x47=((1.0)*x24);
345 eetrans[0]=(((x21*x26))+((x0*x19))+((x11*(((((-1.0)*x22*x47))+((x0*x15*x2))))))+(((-1.0)*x21*x47))+((x10*((((x20*x39))+((x16*x5)))))));
346 eerot[3]=(((x44*x9))+((x8*((x46+x41)))));
347 eerot[4]=(((x9*(((((-1.0)*x41))+(((-1.0)*x46))))))+((x44*x8)));
348 eerot[5]=(((x10*(((((-1.0)*x30))+((x40*x6))))))+((x11*x38)));
349 IkReal x48=((1.0)*x24);
350 eetrans[1]=(((x19*x7))+((x14*x26))+((x10*((((x20*x40))+(((-1.0)*x22*x5))))))+((x11*(((((-1.0)*x16*x48))+((x15*x2*x7))))))+(((-1.0)*x14*x48)));
351 eerot[6]=(((x8*((((x10*x34))+((x35*x6))))))+((x36*x9)));
352 eerot[7]=(((x9*(((((-1.0)*x18*x34))+(((-1.0)*x13*x35))))))+((x36*x8)));
353 eerot[8]=(((x29*x33))+((x11*x34)));
354 eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11*(((((0.084)*x23))+((x15*x4))))))+(((0.307)*x23))+(((0.307)*x28)));
355 }
356 
357 IKFAST_API int GetNumFreeParameters() { return 0; }
358 IKFAST_API int* GetFreeParameters() { return nullptr; }
359 IKFAST_API int GetNumJoints() { return 6; }
360 
361 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
362 
363 IKFAST_API int GetIkType() { return 0x67000001; }
364 
365 class IKSolver {
366 public:
367 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;
368 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
369 
370 IkReal j100, cj100, sj100;
371 unsigned char _ij100[2], _nj100;
372  bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* /* unused */, IkSolutionListBase<IkReal>& solutions) {
373 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;
374 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
375  solutions.Clear();
376 r00 = eerot[0*3+0];
377 r01 = eerot[0*3+1];
378 r02 = eerot[0*3+2];
379 r10 = eerot[1*3+0];
380 r11 = eerot[1*3+1];
381 r12 = eerot[1*3+2];
382 r20 = eerot[2*3+0];
383 r21 = eerot[2*3+1];
384 r22 = eerot[2*3+2];
385 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
386 
387 new_r00=r00;
388 new_r01=r01;
389 new_r02=r02;
390 new_px=(px+(((-0.084)*r02)));
391 new_r10=r10;
392 new_r11=r11;
393 new_r12=r12;
394 new_py=(py+(((-0.084)*r12)));
395 new_r20=r20;
396 new_r21=r21;
397 new_r22=r22;
398 new_pz=((-0.2604)+pz+(((-0.084)*r22)));
399 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;
400 IkReal x49=((1.0)*px);
401 IkReal x50=((1.0)*pz);
402 IkReal x51=((1.0)*py);
403 pp=((px*px)+(py*py)+(pz*pz));
404 npx=(((px*r00))+((py*r10))+((pz*r20)));
405 npy=(((px*r01))+((py*r11))+((pz*r21)));
406 npz=(((px*r02))+((py*r12))+((pz*r22)));
407 rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
408 rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
409 rxp0_2=((((-1.0)*r10*x49))+((py*r00)));
410 rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
411 rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
412 rxp1_2=((((-1.0)*r11*x49))+((py*r01)));
413 rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
414 rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
415 rxp2_2=((((-1.0)*r12*x49))+((py*r02)));
416 {
417 IkReal j2array[2], cj2array[2], sj2array[2];
418 bool j2valid[2]={false};
419 _nj2 = 2;
420 cj2array[0]=((-1.00860400186133)+(((4.65332712889716)*pp)));
421 if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
422 {
423  j2valid[0] = j2valid[1] = true;
424  j2array[0] = IKacos(cj2array[0]);
425  sj2array[0] = IKsin(j2array[0]);
426  cj2array[1] = cj2array[0];
427  j2array[1] = -j2array[0];
428  sj2array[1] = -sj2array[0];
429 }
430 else if( isnan(cj2array[0]) )
431 {
432  // probably any value will work
433  j2valid[0] = true;
434  cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
435 }
436 for(int ij2 = 0; ij2 < 2; ++ij2)
437 {
438 if( !j2valid[ij2] )
439 {
440  continue;
441 }
442 _ij2[0] = ij2; _ij2[1] = -1;
443 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
444 {
445 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
446 {
447  j2valid[iij2]=false; _ij2[1] = iij2; break;
448 }
449 }
450 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
451 
452 {
453 IkReal j0eval[1];
454 j0eval[0]=((IKabs(px))+(IKabs(py)));
455 if( IKabs(j0eval[0]) < 0.0000010000000000 )
456 {
457 {
458 IkReal j1eval[2];
459 j1eval[0]=((IKabs(sj2))+(((3.25732899022801)*(IKabs(((0.35)+(((0.307)*cj2))))))));
460 j1eval[1]=((1.29974853844603)+(sj2*sj2)+(cj2*cj2)+(((2.28013029315961)*cj2)));
461 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
462 {
463 continue; // no branches [j0, j1]
464 
465 } else
466 {
467 {
468 IkReal j1array[2], cj1array[2], sj1array[2];
469 bool j1valid[2]={false};
470 _nj1 = 2;
471 IkReal x52=((0.35)+(((0.307)*cj2)));
472 CheckValue<IkReal> x55 = IKatan2WithCheck(IkReal(x52),IkReal(((0.307)*sj2)),IKFAST_ATAN2_MAGTHRESH);
473 if(!x55.valid){
474 continue;
475 }
476 IkReal x53=((1.0)*(x55.value));
477 if((((((0.094249)*(sj2*sj2)))+(x52*x52))) < -0.00001)
478 continue;
479 CheckValue<IkReal> x56=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.094249)*(sj2*sj2)))+(x52*x52)))),-1);
480 if(!x56.valid){
481 continue;
482 }
483 if( ((pz*(x56.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x56.value))) > 1+IKFAST_SINCOS_THRESH )
484  continue;
485 IkReal x54=IKasin((pz*(x56.value)));
486 j1array[0]=(x54+(((-1.0)*x53)));
487 sj1array[0]=IKsin(j1array[0]);
488 cj1array[0]=IKcos(j1array[0]);
489 j1array[1]=((3.14159265358979)+(((-1.0)*x53))+(((-1.0)*x54)));
490 sj1array[1]=IKsin(j1array[1]);
491 cj1array[1]=IKcos(j1array[1]);
492 if( j1array[0] > IKPI )
493 {
494  j1array[0]-=IK2PI;
495 }
496 else if( j1array[0] < -IKPI )
497 { j1array[0]+=IK2PI;
498 }
499 j1valid[0] = true;
500 if( j1array[1] > IKPI )
501 {
502  j1array[1]-=IK2PI;
503 }
504 else if( j1array[1] < -IKPI )
505 { j1array[1]+=IK2PI;
506 }
507 j1valid[1] = true;
508 for(int ij1 = 0; ij1 < 2; ++ij1)
509 {
510 if( !j1valid[ij1] )
511 {
512  continue;
513 }
514 _ij1[0] = ij1; _ij1[1] = -1;
515 for(int iij1 = ij1+1; iij1 < 2; ++iij1)
516 {
517 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
518 {
519  j1valid[iij1]=false; _ij1[1] = iij1; break;
520 }
521 }
522 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
523 
524 // The remainder of this scope is removed as it does not return a solution.
525 // This is the branch (IKabs(px))+(IKabs(py)), from the kinematics we know,
526 // that j0 is free. So we simply choose 4 values and pass directly to the
527 // rotationfunction (orientation computation is unchanged).
528 _nj0=4;
529 for(int ij0=1; ij0 < _nj0; ++ij0)
530 {
531 j0=(IkReal)(ij0 - 1) * (3.14159265358979) / 2.; // -pi/2, 0, pi/2, pi
532 cj0=IKcos(j0);
533 sj0=IKsin(j0);
534 _ij0[0] = ij0;
535 rotationfunction0(solutions);
536 }
537 
538 }
539 }
540 
541 }
542 
543 }
544 
545 } else
546 {
547 {
548 IkReal j0array[2], cj0array[2], sj0array[2];
549 bool j0valid[2]={false};
550 _nj0 = 2;
551 CheckValue<IkReal> x1691 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
552 if(!x1691.valid){
553 continue;
554 }
555 IkReal x1690=x1691.value;
556 j0array[0]=((-1.0)*x1690);
557 sj0array[0]=IKsin(j0array[0]);
558 cj0array[0]=IKcos(j0array[0]);
559 j0array[1]=((3.14159265358979)+(((-1.0)*x1690)));
560 sj0array[1]=IKsin(j0array[1]);
561 cj0array[1]=IKcos(j0array[1]);
562 if( j0array[0] > IKPI )
563 {
564  j0array[0]-=IK2PI;
565 }
566 else if( j0array[0] < -IKPI )
567 { j0array[0]+=IK2PI;
568 }
569 j0valid[0] = true;
570 if( j0array[1] > IKPI )
571 {
572  j0array[1]-=IK2PI;
573 }
574 else if( j0array[1] < -IKPI )
575 { j0array[1]+=IK2PI;
576 }
577 j0valid[1] = true;
578 for(int ij0 = 0; ij0 < 2; ++ij0)
579 {
580 if( !j0valid[ij0] )
581 {
582  continue;
583 }
584 _ij0[0] = ij0; _ij0[1] = -1;
585 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
586 {
587 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
588 {
589  j0valid[iij0]=false; _ij0[1] = iij0; break;
590 }
591 }
592 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
593 
594 {
595 IkReal j1eval[3];
596 IkReal x1692=((307000.0)*cj2);
597 IkReal x1693=(cj0*px);
598 IkReal x1694=((307000.0)*sj2);
599 IkReal x1695=(py*sj0);
600 j1eval[0]=((-1.00860400186133)+(((-1.0)*cj2)));
601 j1eval[1]=((IKabs(((((-1.0)*pz*x1692))+(((-350000.0)*pz))+((x1693*x1694))+((x1694*x1695)))))+(IKabs(((((-1.0)*x1692*x1693))+(((-1.0)*x1692*x1695))+(((-1.0)*pz*x1694))+(((-350000.0)*x1693))+(((-350000.0)*x1695))))));
602 j1eval[2]=IKsign(((-216749.0)+(((-214900.0)*cj2))));
603 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
604 {
605 {
606 IkReal j1eval[3];
607 IkReal x1696=(py*sj0);
608 IkReal x1697=((1000.0)*pz);
609 IkReal x1698=(pz*sj2);
610 IkReal x1699=(cj0*px);
611 IkReal x1700=(cj2*x1699);
612 j1eval[0]=((((-1.0)*x1700))+x1698+(((-1.0)*cj2*x1696))+(((-1.1400651465798)*x1696))+(((-1.1400651465798)*x1699)));
613 j1eval[1]=((IKabs(((((-1.0)*x1696*x1697))+(((-1.0)*x1697*x1699))+(((94.249)*cj2*sj2))+(((107.45)*sj2)))))+(IKabs(((-122.5)+(((-94.249)*(cj2*cj2)))+(((-214.9)*cj2))+((pz*x1697))))));
614 j1eval[2]=IKsign(((((-307.0)*x1700))+(((-350.0)*x1699))+(((-350.0)*x1696))+(((-307.0)*cj2*x1696))+(((307.0)*x1698))));
615 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
616 {
617 {
618 IkReal j1eval[3];
619 IkReal x1701=(cj0*px);
620 IkReal x1702=((1.0)*cj2);
621 IkReal x1703=((7000.0)*pz);
622 IkReal x1704=(py*sj0);
623 IkReal x1705=((2149.0)*cj2);
624 IkReal x1706=(pz*sj2);
625 IkReal x1707=((3070.0)*pp);
626 j1eval[0]=((((-1.1400651465798)*x1704))+(((-1.1400651465798)*x1701))+(((-1.0)*x1702*x1704))+x1706+(((-1.0)*x1701*x1702)));
627 j1eval[1]=((IKabs(((-98.8785)+(((-86.73057)*cj2))+(((-1.0)*cj2*x1707))+(((-3500.0)*pp))+((pz*x1703)))))+(IKabs(((((-1.0)*x1703*x1704))+((sj2*x1707))+(((-1.0)*x1701*x1703))+(((86.73057)*sj2))))));
628 j1eval[2]=IKsign(((((2149.0)*x1706))+(((-2450.0)*x1704))+(((-2450.0)*x1701))+(((-1.0)*x1704*x1705))+(((-1.0)*x1701*x1705))));
629 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
630 {
631 continue; // no branches [j1]
632 
633 } else
634 {
635 {
636 IkReal j1array[1], cj1array[1], sj1array[1];
637 bool j1valid[1]={false};
638 _nj1 = 1;
639 IkReal x1708=((7000.0)*pz);
640 IkReal x1709=(cj0*px);
641 IkReal x1710=(py*sj0);
642 IkReal x1711=((2149.0)*cj2);
643 IkReal x1712=((3070.0)*pp);
644 CheckValue<IkReal> x1713 = IKatan2WithCheck(IkReal(((-98.8785)+(((-86.73057)*cj2))+(((-3500.0)*pp))+(((-1.0)*cj2*x1712))+((pz*x1708)))),IkReal(((((-1.0)*x1708*x1709))+(((-1.0)*x1708*x1710))+((sj2*x1712))+(((86.73057)*sj2)))),IKFAST_ATAN2_MAGTHRESH);
645 if(!x1713.valid){
646 continue;
647 }
648 CheckValue<IkReal> x1714=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1710*x1711))+(((-1.0)*x1709*x1711))+(((-2450.0)*x1709))+(((-2450.0)*x1710))+(((2149.0)*pz*sj2)))),-1);
649 if(!x1714.valid){
650 continue;
651 }
652 j1array[0]=((-1.5707963267949)+(x1713.value)+(((1.5707963267949)*(x1714.value))));
653 sj1array[0]=IKsin(j1array[0]);
654 cj1array[0]=IKcos(j1array[0]);
655 if( j1array[0] > IKPI )
656 {
657  j1array[0]-=IK2PI;
658 }
659 else if( j1array[0] < -IKPI )
660 { j1array[0]+=IK2PI;
661 }
662 j1valid[0] = true;
663 for(int ij1 = 0; ij1 < 1; ++ij1)
664 {
665 if( !j1valid[ij1] )
666 {
667  continue;
668 }
669 _ij1[0] = ij1; _ij1[1] = -1;
670 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
671 {
672 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
673 {
674  j1valid[iij1]=false; _ij1[1] = iij1; break;
675 }
676 }
677 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
678 {
679 IkReal evalcond[5];
680 IkReal x1715=IKcos(j1);
681 IkReal x1716=IKsin(j1);
682 IkReal x1717=(cj0*px);
683 IkReal x1718=(py*sj0);
684 IkReal x1719=((0.307)*x1715);
685 IkReal x1720=((1.0)*x1718);
686 IkReal x1721=(pz*x1715);
687 IkReal x1722=((0.7)*x1716);
688 IkReal x1723=((0.307)*x1716);
689 evalcond[0]=((((-1.0)*pz))+((cj2*x1719))+(((0.35)*x1715))+((sj2*x1723)));
690 evalcond[1]=((((-1.0)*x1715*x1720))+(((-1.0)*x1715*x1717))+(((-0.307)*sj2))+((pz*x1716)));
691 evalcond[2]=((((-0.35)*x1716))+x1717+x1718+(((-1.0)*cj2*x1723))+((sj2*x1719)));
692 evalcond[3]=((-0.028251)+((x1718*x1722))+((x1717*x1722))+(((-1.0)*pp))+(((0.7)*x1721)));
693 evalcond[4]=((0.35)+(((-1.0)*x1716*x1717))+(((-1.0)*x1721))+(((-1.0)*x1716*x1720))+(((0.307)*cj2)));
694 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 )
695 {
696 continue;
697 }
698 }
699 
700 rotationfunction0(solutions);
701 }
702 }
703 
704 }
705 
706 }
707 
708 } else
709 {
710 {
711 IkReal j1array[1], cj1array[1], sj1array[1];
712 bool j1valid[1]={false};
713 _nj1 = 1;
714 IkReal x1724=((307.0)*cj2);
715 IkReal x1725=(cj0*px);
716 IkReal x1726=(py*sj0);
717 IkReal x1727=((1000.0)*pz);
718 CheckValue<IkReal> x1728 = IKatan2WithCheck(IkReal(((-122.5)+(((-94.249)*(cj2*cj2)))+((pz*x1727))+(((-214.9)*cj2)))),IkReal(((((-1.0)*x1726*x1727))+(((94.249)*cj2*sj2))+(((107.45)*sj2))+(((-1.0)*x1725*x1727)))),IKFAST_ATAN2_MAGTHRESH);
719 if(!x1728.valid){
720 continue;
721 }
722 CheckValue<IkReal> x1729=IKPowWithIntegerCheck(IKsign(((((-350.0)*x1726))+(((-350.0)*x1725))+(((307.0)*pz*sj2))+(((-1.0)*x1724*x1725))+(((-1.0)*x1724*x1726)))),-1);
723 if(!x1729.valid){
724 continue;
725 }
726 j1array[0]=((-1.5707963267949)+(x1728.value)+(((1.5707963267949)*(x1729.value))));
727 sj1array[0]=IKsin(j1array[0]);
728 cj1array[0]=IKcos(j1array[0]);
729 if( j1array[0] > IKPI )
730 {
731  j1array[0]-=IK2PI;
732 }
733 else if( j1array[0] < -IKPI )
734 { j1array[0]+=IK2PI;
735 }
736 j1valid[0] = true;
737 for(int ij1 = 0; ij1 < 1; ++ij1)
738 {
739 if( !j1valid[ij1] )
740 {
741  continue;
742 }
743 _ij1[0] = ij1; _ij1[1] = -1;
744 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
745 {
746 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
747 {
748  j1valid[iij1]=false; _ij1[1] = iij1; break;
749 }
750 }
751 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
752 {
753 IkReal evalcond[5];
754 IkReal x1730=IKcos(j1);
755 IkReal x1731=IKsin(j1);
756 IkReal x1732=(cj0*px);
757 IkReal x1733=(py*sj0);
758 IkReal x1734=((0.307)*x1730);
759 IkReal x1735=((1.0)*x1733);
760 IkReal x1736=(pz*x1730);
761 IkReal x1737=((0.7)*x1731);
762 IkReal x1738=((0.307)*x1731);
763 evalcond[0]=(((cj2*x1734))+(((0.35)*x1730))+(((-1.0)*pz))+((sj2*x1738)));
764 evalcond[1]=(((pz*x1731))+(((-0.307)*sj2))+(((-1.0)*x1730*x1735))+(((-1.0)*x1730*x1732)));
765 evalcond[2]=(x1733+x1732+(((-1.0)*cj2*x1738))+(((-0.35)*x1731))+((sj2*x1734)));
766 evalcond[3]=((-0.028251)+(((-1.0)*pp))+(((0.7)*x1736))+((x1732*x1737))+((x1733*x1737)));
767 evalcond[4]=((0.35)+(((-1.0)*x1731*x1735))+(((-1.0)*x1731*x1732))+(((-1.0)*x1736))+(((0.307)*cj2)));
768 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 )
769 {
770 continue;
771 }
772 }
773 
774 rotationfunction0(solutions);
775 }
776 }
777 
778 }
779 
780 }
781 
782 } else
783 {
784 {
785 IkReal j1array[1], cj1array[1], sj1array[1];
786 bool j1valid[1]={false};
787 _nj1 = 1;
788 IkReal x1739=((307000.0)*cj2);
789 IkReal x1740=(cj0*px);
790 IkReal x1741=((307000.0)*sj2);
791 IkReal x1742=(py*sj0);
792 CheckValue<IkReal> x1743=IKPowWithIntegerCheck(IKsign(((-216749.0)+(((-214900.0)*cj2)))),-1);
793 if(!x1743.valid){
794 continue;
795 }
796 CheckValue<IkReal> x1744 = IKatan2WithCheck(IkReal(((((-350000.0)*x1742))+(((-350000.0)*x1740))+(((-1.0)*x1739*x1740))+(((-1.0)*x1739*x1742))+(((-1.0)*pz*x1741)))),IkReal(((((-1.0)*pz*x1739))+(((-350000.0)*pz))+((x1740*x1741))+((x1741*x1742)))),IKFAST_ATAN2_MAGTHRESH);
797 if(!x1744.valid){
798 continue;
799 }
800 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1743.value)))+(x1744.value));
801 sj1array[0]=IKsin(j1array[0]);
802 cj1array[0]=IKcos(j1array[0]);
803 if( j1array[0] > IKPI )
804 {
805  j1array[0]-=IK2PI;
806 }
807 else if( j1array[0] < -IKPI )
808 { j1array[0]+=IK2PI;
809 }
810 j1valid[0] = true;
811 for(int ij1 = 0; ij1 < 1; ++ij1)
812 {
813 if( !j1valid[ij1] )
814 {
815  continue;
816 }
817 _ij1[0] = ij1; _ij1[1] = -1;
818 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
819 {
820 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
821 {
822  j1valid[iij1]=false; _ij1[1] = iij1; break;
823 }
824 }
825 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
826 {
827 IkReal evalcond[5];
828 IkReal x1745=IKcos(j1);
829 IkReal x1746=IKsin(j1);
830 IkReal x1747=(cj0*px);
831 IkReal x1748=(py*sj0);
832 IkReal x1749=((0.307)*x1745);
833 IkReal x1750=((1.0)*x1748);
834 IkReal x1751=(pz*x1745);
835 IkReal x1752=((0.7)*x1746);
836 IkReal x1753=((0.307)*x1746);
837 evalcond[0]=((((0.35)*x1745))+((cj2*x1749))+(((-1.0)*pz))+((sj2*x1753)));
838 evalcond[1]=(((pz*x1746))+(((-0.307)*sj2))+(((-1.0)*x1745*x1750))+(((-1.0)*x1745*x1747)));
839 evalcond[2]=(x1748+x1747+(((-1.0)*cj2*x1753))+((sj2*x1749))+(((-0.35)*x1746)));
840 evalcond[3]=((-0.028251)+((x1747*x1752))+(((-1.0)*pp))+((x1748*x1752))+(((0.7)*x1751)));
841 evalcond[4]=((0.35)+(((-1.0)*x1746*x1747))+(((-1.0)*x1746*x1750))+(((-1.0)*x1751))+(((0.307)*cj2)));
842 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 )
843 {
844 continue;
845 }
846 }
847 
848 rotationfunction0(solutions);
849 }
850 }
851 
852 }
853 
854 }
855 }
856 }
857 
858 }
859 
860 }
861 }
862 }
863 }
864 return solutions.GetNumSolutions()>0;
865 }
866 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
867 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
868 IkReal x70=(r11*sj0);
869 IkReal x71=(cj0*r02);
870 IkReal x72=(r10*sj0);
871 IkReal x73=((1.0)*sj0);
872 IkReal x74=(cj2*sj1);
873 IkReal x75=(cj1*sj2);
874 IkReal x76=(r12*sj0);
875 IkReal x77=(((sj1*sj2))+((cj1*cj2)));
876 IkReal x78=(x75+(((-1.0)*x74)));
877 IkReal x79=(x74+(((-1.0)*x75)));
878 IkReal x80=(sj0*x79);
879 IkReal x81=(cj0*x79);
880 IkReal x82=(cj0*x77);
881 new_r00=(((r00*x82))+((x72*x77))+((r20*x78)));
882 new_r01=(((r21*x78))+((r01*x82))+((x70*x77)));
883 new_r02=(((r22*x78))+((x76*x77))+((x71*x77)));
884 new_r10=((((-1.0)*r00*x73))+((cj0*r10)));
885 new_r11=((((-1.0)*r01*x73))+((cj0*r11)));
886 new_r12=((((-1.0)*r02*x73))+((cj0*r12)));
887 new_r20=(((r00*x81))+((x72*x79))+((r20*x77)));
888 new_r21=(((r21*x77))+((r01*x81))+((x70*x79)));
889 new_r22=(((r22*x77))+((x76*x79))+((x71*x79)));
890 {
891 IkReal j4array[2], cj4array[2], sj4array[2];
892 bool j4valid[2]={false};
893 _nj4 = 2;
894 cj4array[0]=new_r22;
895 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
896 {
897  j4valid[0] = j4valid[1] = true;
898  j4array[0] = IKacos(cj4array[0]);
899  sj4array[0] = IKsin(j4array[0]);
900  cj4array[1] = cj4array[0];
901  j4array[1] = -j4array[0];
902  sj4array[1] = -sj4array[0];
903 }
904 else if( isnan(cj4array[0]) )
905 {
906  // probably any value will work
907  j4valid[0] = true;
908  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
909 }
910 for(int ij4 = 0; ij4 < 2; ++ij4)
911 {
912 if( !j4valid[ij4] )
913 {
914  continue;
915 }
916 _ij4[0] = ij4; _ij4[1] = -1;
917 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
918 {
919 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
920 {
921  j4valid[iij4]=false; _ij4[1] = iij4; break;
922 }
923 }
924 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
925 
926 {
927 IkReal j5eval[3];
928 j5eval[0]=sj4;
929 j5eval[1]=IKsign(sj4);
930 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
931 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
932 {
933 {
934 IkReal j3eval[3];
935 j3eval[0]=sj4;
936 j3eval[1]=IKsign(sj4);
937 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
938 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
939 {
940 {
941 IkReal j3eval[2];
942 j3eval[0]=new_r12;
943 j3eval[1]=sj4;
944 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
945 {
946 {
947 IkReal evalcond[5];
948 bool bgotonextstatement = true;
949 do
950 {
951 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
952 evalcond[1]=new_r02;
953 evalcond[2]=new_r12;
954 evalcond[3]=new_r21;
955 evalcond[4]=new_r20;
956 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 )
957 {
958 bgotonextstatement=false;
959 IkReal j5mul = 1;
960 j5=0;
961 j3mul=-1.0;
962 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 )
963  continue;
964 j3=IKatan2(((-1.0)*new_r01), new_r00);
965 {
966 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
967 vinfos[0].jointtype = 1;
968 vinfos[0].foffset = j0;
969 vinfos[0].indices[0] = _ij0[0];
970 vinfos[0].indices[1] = _ij0[1];
971 vinfos[0].maxsolutions = _nj0;
972 vinfos[1].jointtype = 1;
973 vinfos[1].foffset = j1;
974 vinfos[1].indices[0] = _ij1[0];
975 vinfos[1].indices[1] = _ij1[1];
976 vinfos[1].maxsolutions = _nj1;
977 vinfos[2].jointtype = 1;
978 vinfos[2].foffset = j2;
979 vinfos[2].indices[0] = _ij2[0];
980 vinfos[2].indices[1] = _ij2[1];
981 vinfos[2].maxsolutions = _nj2;
982 vinfos[3].jointtype = 1;
983 vinfos[3].foffset = j3;
984 vinfos[3].fmul = j3mul;
985 vinfos[3].freeind = 0;
986 vinfos[3].maxsolutions = 0;
987 vinfos[4].jointtype = 1;
988 vinfos[4].foffset = j4;
989 vinfos[4].indices[0] = _ij4[0];
990 vinfos[4].indices[1] = _ij4[1];
991 vinfos[4].maxsolutions = _nj4;
992 vinfos[5].jointtype = 1;
993 vinfos[5].foffset = j5;
994 vinfos[5].fmul = j5mul;
995 vinfos[5].freeind = 0;
996 vinfos[5].maxsolutions = 0;
997 std::vector<int> vfree(1);
998 vfree[0] = 5;
999 solutions.AddSolution(vinfos,vfree);
1000 }
1001 
1002 }
1003 } while(0);
1004 if( bgotonextstatement )
1005 {
1006 bool bgotonextstatement = true;
1007 do
1008 {
1009 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
1010 evalcond[1]=new_r02;
1011 evalcond[2]=new_r12;
1012 evalcond[3]=new_r21;
1013 evalcond[4]=new_r20;
1014 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 )
1015 {
1016 bgotonextstatement=false;
1017 IkReal j5mul = 1;
1018 j5=0;
1019 j3mul=1.0;
1020 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 )
1021  continue;
1022 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
1023 {
1024 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1025 vinfos[0].jointtype = 1;
1026 vinfos[0].foffset = j0;
1027 vinfos[0].indices[0] = _ij0[0];
1028 vinfos[0].indices[1] = _ij0[1];
1029 vinfos[0].maxsolutions = _nj0;
1030 vinfos[1].jointtype = 1;
1031 vinfos[1].foffset = j1;
1032 vinfos[1].indices[0] = _ij1[0];
1033 vinfos[1].indices[1] = _ij1[1];
1034 vinfos[1].maxsolutions = _nj1;
1035 vinfos[2].jointtype = 1;
1036 vinfos[2].foffset = j2;
1037 vinfos[2].indices[0] = _ij2[0];
1038 vinfos[2].indices[1] = _ij2[1];
1039 vinfos[2].maxsolutions = _nj2;
1040 vinfos[3].jointtype = 1;
1041 vinfos[3].foffset = j3;
1042 vinfos[3].fmul = j3mul;
1043 vinfos[3].freeind = 0;
1044 vinfos[3].maxsolutions = 0;
1045 vinfos[4].jointtype = 1;
1046 vinfos[4].foffset = j4;
1047 vinfos[4].indices[0] = _ij4[0];
1048 vinfos[4].indices[1] = _ij4[1];
1049 vinfos[4].maxsolutions = _nj4;
1050 vinfos[5].jointtype = 1;
1051 vinfos[5].foffset = j5;
1052 vinfos[5].fmul = j5mul;
1053 vinfos[5].freeind = 0;
1054 vinfos[5].maxsolutions = 0;
1055 std::vector<int> vfree(1);
1056 vfree[0] = 5;
1057 solutions.AddSolution(vinfos,vfree);
1058 }
1059 
1060 }
1061 } while(0);
1062 if( bgotonextstatement )
1063 {
1064 bool bgotonextstatement = true;
1065 do
1066 {
1067 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1068 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1069 {
1070 bgotonextstatement=false;
1071 {
1072 IkReal j3eval[1];
1073 new_r21=0;
1074 new_r20=0;
1075 new_r02=0;
1076 new_r12=0;
1077 IkReal x83=new_r22*new_r22;
1078 IkReal x84=((16.0)*new_r10);
1079 IkReal x85=((16.0)*new_r01);
1080 IkReal x86=((16.0)*new_r22);
1081 IkReal x87=((8.0)*new_r11);
1082 IkReal x88=((8.0)*new_r00);
1083 IkReal x89=(x83*x84);
1084 IkReal x90=(x83*x85);
1085 j3eval[0]=((IKabs((x89+(((-1.0)*x84)))))+(IKabs((((new_r22*x88))+(((-1.0)*x83*x87)))))+(IKabs(((((16.0)*new_r00))+((new_r11*x86))+(((-32.0)*new_r00*x83)))))+(IKabs((x85+(((-1.0)*x90)))))+(IKabs((((new_r22*x87))+(((-1.0)*x88)))))+(IKabs(((((16.0)*new_r11*x83))+((new_r00*x86))+(((-32.0)*new_r11)))))+(IKabs((x90+(((-1.0)*x85)))))+(IKabs((x84+(((-1.0)*x89))))));
1086 if( IKabs(j3eval[0]) < 0.0000000100000000 )
1087 {
1088 continue; // no branches [j3, j5]
1089 
1090 } else
1091 {
1092 IkReal op[4+1], zeror[4];
1093 int numroots;
1094 IkReal j3evalpoly[1];
1095 IkReal x91=new_r22*new_r22;
1096 IkReal x92=((16.0)*new_r10);
1097 IkReal x93=(new_r11*new_r22);
1098 IkReal x94=(x91*x92);
1099 IkReal x95=((((8.0)*x93))+(((-8.0)*new_r00)));
1100 op[0]=x95;
1101 op[1]=(x92+(((-1.0)*x94)));
1102 op[2]=((((16.0)*x93))+(((16.0)*new_r00))+(((-32.0)*new_r00*x91)));
1103 op[3]=(x94+(((-1.0)*x92)));
1104 op[4]=x95;
1105 polyroots4(op,zeror,numroots);
1106 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1107 int numsolutions = 0;
1108 for(int ij3 = 0; ij3 < numroots; ++ij3)
1109 {
1110 IkReal htj3 = zeror[ij3];
1111 tempj3array[0]=((2.0)*(atan(htj3)));
1112 for(int kj3 = 0; kj3 < 1; ++kj3)
1113 {
1114 j3array[numsolutions] = tempj3array[kj3];
1115 if( j3array[numsolutions] > IKPI )
1116 {
1117  j3array[numsolutions]-=IK2PI;
1118 }
1119 else if( j3array[numsolutions] < -IKPI )
1120 {
1121  j3array[numsolutions]+=IK2PI;
1122 }
1123 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1124 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1125 numsolutions++;
1126 }
1127 }
1128 bool j3valid[4]={true,true,true,true};
1129 _nj3 = 4;
1130 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1131  {
1132 if( !j3valid[ij3] )
1133 {
1134  continue;
1135 }
1136  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1137 htj3 = IKtan(j3/2);
1138 
1139 IkReal x96=((16.0)*new_r01);
1140 IkReal x97=new_r22*new_r22;
1141 IkReal x98=(new_r00*new_r22);
1142 IkReal x99=((8.0)*x98);
1143 IkReal x100=(new_r11*x97);
1144 IkReal x101=(x96*x97);
1145 IkReal x102=((8.0)*x100);
1146 j3evalpoly[0]=((((-1.0)*x102))+(((htj3*htj3*htj3)*(((((-1.0)*x101))+x96))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x102))+x99))))+((htj3*((x101+(((-1.0)*x96))))))+(((htj3*htj3)*(((((16.0)*x98))+(((16.0)*x100))+(((-32.0)*new_r11))))))+x99);
1147 if( IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1148 {
1149  continue;
1150 }
1151 _ij3[0] = ij3; _ij3[1] = -1;
1152 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1153 {
1154 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1155 {
1156  j3valid[iij3]=false; _ij3[1] = iij3; break;
1157 }
1158 }
1159 {
1160 IkReal j5eval[3];
1161 new_r21=0;
1162 new_r20=0;
1163 new_r02=0;
1164 new_r12=0;
1165 IkReal x103=cj3*cj3;
1166 IkReal x104=(cj3*new_r22);
1167 IkReal x105=((-1.0)+x103+(((-1.0)*x103*(new_r22*new_r22))));
1168 j5eval[0]=x105;
1169 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x104)))))+(IKabs((((new_r00*sj3))+((new_r01*x104))))));
1170 j5eval[2]=IKsign(x105);
1171 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1172 {
1173 {
1174 IkReal j5eval[1];
1175 new_r21=0;
1176 new_r20=0;
1177 new_r02=0;
1178 new_r12=0;
1179 j5eval[0]=new_r22;
1180 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1181 {
1182 {
1183 IkReal j5eval[1];
1184 new_r21=0;
1185 new_r20=0;
1186 new_r02=0;
1187 new_r12=0;
1188 j5eval[0]=sj3;
1189 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1190 {
1191 {
1192 IkReal evalcond[1];
1193 bool bgotonextstatement = true;
1194 do
1195 {
1196 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1197 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1198 {
1199 bgotonextstatement=false;
1200 {
1201 IkReal j5array[1], cj5array[1], sj5array[1];
1202 bool j5valid[1]={false};
1203 _nj5 = 1;
1204 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1205  continue;
1206 j5array[0]=IKatan2(new_r10, new_r11);
1207 sj5array[0]=IKsin(j5array[0]);
1208 cj5array[0]=IKcos(j5array[0]);
1209 if( j5array[0] > IKPI )
1210 {
1211  j5array[0]-=IK2PI;
1212 }
1213 else if( j5array[0] < -IKPI )
1214 { j5array[0]+=IK2PI;
1215 }
1216 j5valid[0] = true;
1217 for(int ij5 = 0; ij5 < 1; ++ij5)
1218 {
1219 if( !j5valid[ij5] )
1220 {
1221  continue;
1222 }
1223 _ij5[0] = ij5; _ij5[1] = -1;
1224 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1225 {
1226 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1227 {
1228  j5valid[iij5]=false; _ij5[1] = iij5; break;
1229 }
1230 }
1231 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1232 {
1233 IkReal evalcond[4];
1234 IkReal x106=IKsin(j5);
1235 IkReal x107=IKcos(j5);
1236 evalcond[0]=x107;
1237 evalcond[1]=((-1.0)*x106);
1238 evalcond[2]=(x106+(((-1.0)*new_r10)));
1239 evalcond[3]=(x107+(((-1.0)*new_r11)));
1240 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 )
1241 {
1242 continue;
1243 }
1244 }
1245 
1246 {
1247 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1248 vinfos[0].jointtype = 1;
1249 vinfos[0].foffset = j0;
1250 vinfos[0].indices[0] = _ij0[0];
1251 vinfos[0].indices[1] = _ij0[1];
1252 vinfos[0].maxsolutions = _nj0;
1253 vinfos[1].jointtype = 1;
1254 vinfos[1].foffset = j1;
1255 vinfos[1].indices[0] = _ij1[0];
1256 vinfos[1].indices[1] = _ij1[1];
1257 vinfos[1].maxsolutions = _nj1;
1258 vinfos[2].jointtype = 1;
1259 vinfos[2].foffset = j2;
1260 vinfos[2].indices[0] = _ij2[0];
1261 vinfos[2].indices[1] = _ij2[1];
1262 vinfos[2].maxsolutions = _nj2;
1263 vinfos[3].jointtype = 1;
1264 vinfos[3].foffset = j3;
1265 vinfos[3].indices[0] = _ij3[0];
1266 vinfos[3].indices[1] = _ij3[1];
1267 vinfos[3].maxsolutions = _nj3;
1268 vinfos[4].jointtype = 1;
1269 vinfos[4].foffset = j4;
1270 vinfos[4].indices[0] = _ij4[0];
1271 vinfos[4].indices[1] = _ij4[1];
1272 vinfos[4].maxsolutions = _nj4;
1273 vinfos[5].jointtype = 1;
1274 vinfos[5].foffset = j5;
1275 vinfos[5].indices[0] = _ij5[0];
1276 vinfos[5].indices[1] = _ij5[1];
1277 vinfos[5].maxsolutions = _nj5;
1278 std::vector<int> vfree(0);
1279 solutions.AddSolution(vinfos,vfree);
1280 }
1281 }
1282 }
1283 
1284 }
1285 } while(0);
1286 if( bgotonextstatement )
1287 {
1288 bool bgotonextstatement = true;
1289 do
1290 {
1291 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1292 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1293 {
1294 bgotonextstatement=false;
1295 {
1296 IkReal j5array[1], cj5array[1], sj5array[1];
1297 bool j5valid[1]={false};
1298 _nj5 = 1;
1299 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 )
1300  continue;
1301 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1302 sj5array[0]=IKsin(j5array[0]);
1303 cj5array[0]=IKcos(j5array[0]);
1304 if( j5array[0] > IKPI )
1305 {
1306  j5array[0]-=IK2PI;
1307 }
1308 else if( j5array[0] < -IKPI )
1309 { j5array[0]+=IK2PI;
1310 }
1311 j5valid[0] = true;
1312 for(int ij5 = 0; ij5 < 1; ++ij5)
1313 {
1314 if( !j5valid[ij5] )
1315 {
1316  continue;
1317 }
1318 _ij5[0] = ij5; _ij5[1] = -1;
1319 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1320 {
1321 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1322 {
1323  j5valid[iij5]=false; _ij5[1] = iij5; break;
1324 }
1325 }
1326 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1327 {
1328 IkReal evalcond[4];
1329 IkReal x108=IKcos(j5);
1330 IkReal x109=IKsin(j5);
1331 evalcond[0]=x108;
1332 evalcond[1]=(x109+new_r10);
1333 evalcond[2]=(x108+new_r11);
1334 evalcond[3]=((-1.0)*x109);
1335 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 )
1336 {
1337 continue;
1338 }
1339 }
1340 
1341 {
1342 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1343 vinfos[0].jointtype = 1;
1344 vinfos[0].foffset = j0;
1345 vinfos[0].indices[0] = _ij0[0];
1346 vinfos[0].indices[1] = _ij0[1];
1347 vinfos[0].maxsolutions = _nj0;
1348 vinfos[1].jointtype = 1;
1349 vinfos[1].foffset = j1;
1350 vinfos[1].indices[0] = _ij1[0];
1351 vinfos[1].indices[1] = _ij1[1];
1352 vinfos[1].maxsolutions = _nj1;
1353 vinfos[2].jointtype = 1;
1354 vinfos[2].foffset = j2;
1355 vinfos[2].indices[0] = _ij2[0];
1356 vinfos[2].indices[1] = _ij2[1];
1357 vinfos[2].maxsolutions = _nj2;
1358 vinfos[3].jointtype = 1;
1359 vinfos[3].foffset = j3;
1360 vinfos[3].indices[0] = _ij3[0];
1361 vinfos[3].indices[1] = _ij3[1];
1362 vinfos[3].maxsolutions = _nj3;
1363 vinfos[4].jointtype = 1;
1364 vinfos[4].foffset = j4;
1365 vinfos[4].indices[0] = _ij4[0];
1366 vinfos[4].indices[1] = _ij4[1];
1367 vinfos[4].maxsolutions = _nj4;
1368 vinfos[5].jointtype = 1;
1369 vinfos[5].foffset = j5;
1370 vinfos[5].indices[0] = _ij5[0];
1371 vinfos[5].indices[1] = _ij5[1];
1372 vinfos[5].maxsolutions = _nj5;
1373 std::vector<int> vfree(0);
1374 solutions.AddSolution(vinfos,vfree);
1375 }
1376 }
1377 }
1378 
1379 }
1380 } while(0);
1381 if( bgotonextstatement )
1382 {
1383 bool bgotonextstatement = true;
1384 do
1385 {
1386 CheckValue<IkReal> x110=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1387 if(!x110.valid){
1388 continue;
1389 }
1390 if((x110.value) < -0.00001)
1391 continue;
1392 IkReal gconst18=((-1.0)*(IKsqrt(x110.value)));
1393 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1394 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1395 {
1396 bgotonextstatement=false;
1397 {
1398 IkReal j5eval[1];
1399 new_r21=0;
1400 new_r20=0;
1401 new_r02=0;
1402 new_r12=0;
1403 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1404 continue;
1405 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))));
1406 cj3=gconst18;
1407 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1408  continue;
1409 j3=IKacos(gconst18);
1410 CheckValue<IkReal> x111=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1411 if(!x111.valid){
1412 continue;
1413 }
1414 if((x111.value) < -0.00001)
1415 continue;
1416 IkReal gconst18=((-1.0)*(IKsqrt(x111.value)));
1417 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1418 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1419 {
1420 {
1421 IkReal j5array[1], cj5array[1], sj5array[1];
1422 bool j5valid[1]={false};
1423 _nj5 = 1;
1424 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1425 continue;
1426 CheckValue<IkReal> x112=IKPowWithIntegerCheck(gconst18,-1);
1427 if(!x112.valid){
1428 continue;
1429 }
1430 if( IKabs((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x112.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x112.value)))-1) <= IKFAST_SINCOS_THRESH )
1431  continue;
1432 j5array[0]=IKatan2((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x112.value)));
1433 sj5array[0]=IKsin(j5array[0]);
1434 cj5array[0]=IKcos(j5array[0]);
1435 if( j5array[0] > IKPI )
1436 {
1437  j5array[0]-=IK2PI;
1438 }
1439 else if( j5array[0] < -IKPI )
1440 { j5array[0]+=IK2PI;
1441 }
1442 j5valid[0] = true;
1443 for(int ij5 = 0; ij5 < 1; ++ij5)
1444 {
1445 if( !j5valid[ij5] )
1446 {
1447  continue;
1448 }
1449 _ij5[0] = ij5; _ij5[1] = -1;
1450 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1451 {
1452 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1453 {
1454  j5valid[iij5]=false; _ij5[1] = iij5; break;
1455 }
1456 }
1457 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1458 {
1459 IkReal evalcond[8];
1460 IkReal x113=IKcos(j5);
1461 IkReal x114=IKsin(j5);
1462 IkReal x115=((1.0)*gconst18);
1463 if((((1.0)+(((-1.0)*gconst18*x115)))) < -0.00001)
1464 continue;
1465 IkReal x116=IKsqrt(((1.0)+(((-1.0)*gconst18*x115))));
1466 evalcond[0]=x113;
1467 evalcond[1]=((-1.0)*x114);
1468 evalcond[2]=((((-1.0)*x113*x115))+new_r11);
1469 evalcond[3]=((((-1.0)*x114*x115))+new_r10);
1470 evalcond[4]=(((x113*x116))+new_r01);
1471 evalcond[5]=(((x114*x116))+new_r00);
1472 evalcond[6]=((((-1.0)*new_r10*x115))+x114+((new_r00*x116)));
1473 evalcond[7]=((((-1.0)*new_r11*x115))+x113+((new_r01*x116)));
1474 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 )
1475 {
1476 continue;
1477 }
1478 }
1479 
1480 {
1481 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1482 vinfos[0].jointtype = 1;
1483 vinfos[0].foffset = j0;
1484 vinfos[0].indices[0] = _ij0[0];
1485 vinfos[0].indices[1] = _ij0[1];
1486 vinfos[0].maxsolutions = _nj0;
1487 vinfos[1].jointtype = 1;
1488 vinfos[1].foffset = j1;
1489 vinfos[1].indices[0] = _ij1[0];
1490 vinfos[1].indices[1] = _ij1[1];
1491 vinfos[1].maxsolutions = _nj1;
1492 vinfos[2].jointtype = 1;
1493 vinfos[2].foffset = j2;
1494 vinfos[2].indices[0] = _ij2[0];
1495 vinfos[2].indices[1] = _ij2[1];
1496 vinfos[2].maxsolutions = _nj2;
1497 vinfos[3].jointtype = 1;
1498 vinfos[3].foffset = j3;
1499 vinfos[3].indices[0] = _ij3[0];
1500 vinfos[3].indices[1] = _ij3[1];
1501 vinfos[3].maxsolutions = _nj3;
1502 vinfos[4].jointtype = 1;
1503 vinfos[4].foffset = j4;
1504 vinfos[4].indices[0] = _ij4[0];
1505 vinfos[4].indices[1] = _ij4[1];
1506 vinfos[4].maxsolutions = _nj4;
1507 vinfos[5].jointtype = 1;
1508 vinfos[5].foffset = j5;
1509 vinfos[5].indices[0] = _ij5[0];
1510 vinfos[5].indices[1] = _ij5[1];
1511 vinfos[5].maxsolutions = _nj5;
1512 std::vector<int> vfree(0);
1513 solutions.AddSolution(vinfos,vfree);
1514 }
1515 }
1516 }
1517 
1518 } else
1519 {
1520 {
1521 IkReal j5array[1], cj5array[1], sj5array[1];
1522 bool j5valid[1]={false};
1523 _nj5 = 1;
1524 CheckValue<IkReal> x117=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1525 if(!x117.valid){
1526 continue;
1527 }
1528 CheckValue<IkReal> x118 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1529 if(!x118.valid){
1530 continue;
1531 }
1532 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.value)))+(x118.value));
1533 sj5array[0]=IKsin(j5array[0]);
1534 cj5array[0]=IKcos(j5array[0]);
1535 if( j5array[0] > IKPI )
1536 {
1537  j5array[0]-=IK2PI;
1538 }
1539 else if( j5array[0] < -IKPI )
1540 { j5array[0]+=IK2PI;
1541 }
1542 j5valid[0] = true;
1543 for(int ij5 = 0; ij5 < 1; ++ij5)
1544 {
1545 if( !j5valid[ij5] )
1546 {
1547  continue;
1548 }
1549 _ij5[0] = ij5; _ij5[1] = -1;
1550 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1551 {
1552 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1553 {
1554  j5valid[iij5]=false; _ij5[1] = iij5; break;
1555 }
1556 }
1557 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1558 {
1559 IkReal evalcond[8];
1560 IkReal x119=IKcos(j5);
1561 IkReal x120=IKsin(j5);
1562 IkReal x121=((1.0)*gconst18);
1563 if((((1.0)+(((-1.0)*gconst18*x121)))) < -0.00001)
1564 continue;
1565 IkReal x122=IKsqrt(((1.0)+(((-1.0)*gconst18*x121))));
1566 evalcond[0]=x119;
1567 evalcond[1]=((-1.0)*x120);
1568 evalcond[2]=((((-1.0)*x119*x121))+new_r11);
1569 evalcond[3]=((((-1.0)*x120*x121))+new_r10);
1570 evalcond[4]=(new_r01+((x119*x122)));
1571 evalcond[5]=(((x120*x122))+new_r00);
1572 evalcond[6]=((((-1.0)*new_r10*x121))+((new_r00*x122))+x120);
1573 evalcond[7]=(((new_r01*x122))+x119+(((-1.0)*new_r11*x121)));
1574 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 )
1575 {
1576 continue;
1577 }
1578 }
1579 
1580 {
1581 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1582 vinfos[0].jointtype = 1;
1583 vinfos[0].foffset = j0;
1584 vinfos[0].indices[0] = _ij0[0];
1585 vinfos[0].indices[1] = _ij0[1];
1586 vinfos[0].maxsolutions = _nj0;
1587 vinfos[1].jointtype = 1;
1588 vinfos[1].foffset = j1;
1589 vinfos[1].indices[0] = _ij1[0];
1590 vinfos[1].indices[1] = _ij1[1];
1591 vinfos[1].maxsolutions = _nj1;
1592 vinfos[2].jointtype = 1;
1593 vinfos[2].foffset = j2;
1594 vinfos[2].indices[0] = _ij2[0];
1595 vinfos[2].indices[1] = _ij2[1];
1596 vinfos[2].maxsolutions = _nj2;
1597 vinfos[3].jointtype = 1;
1598 vinfos[3].foffset = j3;
1599 vinfos[3].indices[0] = _ij3[0];
1600 vinfos[3].indices[1] = _ij3[1];
1601 vinfos[3].maxsolutions = _nj3;
1602 vinfos[4].jointtype = 1;
1603 vinfos[4].foffset = j4;
1604 vinfos[4].indices[0] = _ij4[0];
1605 vinfos[4].indices[1] = _ij4[1];
1606 vinfos[4].maxsolutions = _nj4;
1607 vinfos[5].jointtype = 1;
1608 vinfos[5].foffset = j5;
1609 vinfos[5].indices[0] = _ij5[0];
1610 vinfos[5].indices[1] = _ij5[1];
1611 vinfos[5].maxsolutions = _nj5;
1612 std::vector<int> vfree(0);
1613 solutions.AddSolution(vinfos,vfree);
1614 }
1615 }
1616 }
1617 
1618 }
1619 
1620 }
1621 
1622 }
1623 } while(0);
1624 if( bgotonextstatement )
1625 {
1626 bool bgotonextstatement = true;
1627 do
1628 {
1629 CheckValue<IkReal> x123=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1630 if(!x123.valid){
1631 continue;
1632 }
1633 if((x123.value) < -0.00001)
1634 continue;
1635 IkReal gconst18=((-1.0)*(IKsqrt(x123.value)));
1636 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1637 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1638 {
1639 bgotonextstatement=false;
1640 {
1641 IkReal j5eval[1];
1642 new_r21=0;
1643 new_r20=0;
1644 new_r02=0;
1645 new_r12=0;
1646 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1647 continue;
1648 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))));
1649 cj3=gconst18;
1650 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1651  continue;
1652 j3=((-1.0)*(IKacos(gconst18)));
1653 CheckValue<IkReal> x124=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1654 if(!x124.valid){
1655 continue;
1656 }
1657 if((x124.value) < -0.00001)
1658 continue;
1659 IkReal gconst18=((-1.0)*(IKsqrt(x124.value)));
1660 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1661 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1662 {
1663 {
1664 IkReal j5array[1], cj5array[1], sj5array[1];
1665 bool j5valid[1]={false};
1666 _nj5 = 1;
1667 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1668 continue;
1669 CheckValue<IkReal> x125=IKPowWithIntegerCheck(gconst18,-1);
1670 if(!x125.valid){
1671 continue;
1672 }
1673 if( IKabs((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x125.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x125.value)))-1) <= IKFAST_SINCOS_THRESH )
1674  continue;
1675 j5array[0]=IKatan2((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x125.value)));
1676 sj5array[0]=IKsin(j5array[0]);
1677 cj5array[0]=IKcos(j5array[0]);
1678 if( j5array[0] > IKPI )
1679 {
1680  j5array[0]-=IK2PI;
1681 }
1682 else if( j5array[0] < -IKPI )
1683 { j5array[0]+=IK2PI;
1684 }
1685 j5valid[0] = true;
1686 for(int ij5 = 0; ij5 < 1; ++ij5)
1687 {
1688 if( !j5valid[ij5] )
1689 {
1690  continue;
1691 }
1692 _ij5[0] = ij5; _ij5[1] = -1;
1693 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1694 {
1695 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1696 {
1697  j5valid[iij5]=false; _ij5[1] = iij5; break;
1698 }
1699 }
1700 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1701 {
1702 IkReal evalcond[8];
1703 IkReal x126=IKcos(j5);
1704 IkReal x127=IKsin(j5);
1705 IkReal x128=((1.0)*gconst18);
1706 if((((1.0)+(((-1.0)*gconst18*x128)))) < -0.00001)
1707 continue;
1708 IkReal x129=IKsqrt(((1.0)+(((-1.0)*gconst18*x128))));
1709 IkReal x130=((1.0)*x129);
1710 evalcond[0]=x126;
1711 evalcond[1]=((-1.0)*x127);
1712 evalcond[2]=((((-1.0)*x126*x128))+new_r11);
1713 evalcond[3]=(new_r10+(((-1.0)*x127*x128)));
1714 evalcond[4]=((((-1.0)*x126*x130))+new_r01);
1715 evalcond[5]=(new_r00+(((-1.0)*x127*x130)));
1716 evalcond[6]=((((-1.0)*new_r10*x128))+(((-1.0)*new_r00*x130))+x127);
1717 evalcond[7]=((((-1.0)*new_r01*x130))+x126+(((-1.0)*new_r11*x128)));
1718 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 )
1719 {
1720 continue;
1721 }
1722 }
1723 
1724 {
1725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1726 vinfos[0].jointtype = 1;
1727 vinfos[0].foffset = j0;
1728 vinfos[0].indices[0] = _ij0[0];
1729 vinfos[0].indices[1] = _ij0[1];
1730 vinfos[0].maxsolutions = _nj0;
1731 vinfos[1].jointtype = 1;
1732 vinfos[1].foffset = j1;
1733 vinfos[1].indices[0] = _ij1[0];
1734 vinfos[1].indices[1] = _ij1[1];
1735 vinfos[1].maxsolutions = _nj1;
1736 vinfos[2].jointtype = 1;
1737 vinfos[2].foffset = j2;
1738 vinfos[2].indices[0] = _ij2[0];
1739 vinfos[2].indices[1] = _ij2[1];
1740 vinfos[2].maxsolutions = _nj2;
1741 vinfos[3].jointtype = 1;
1742 vinfos[3].foffset = j3;
1743 vinfos[3].indices[0] = _ij3[0];
1744 vinfos[3].indices[1] = _ij3[1];
1745 vinfos[3].maxsolutions = _nj3;
1746 vinfos[4].jointtype = 1;
1747 vinfos[4].foffset = j4;
1748 vinfos[4].indices[0] = _ij4[0];
1749 vinfos[4].indices[1] = _ij4[1];
1750 vinfos[4].maxsolutions = _nj4;
1751 vinfos[5].jointtype = 1;
1752 vinfos[5].foffset = j5;
1753 vinfos[5].indices[0] = _ij5[0];
1754 vinfos[5].indices[1] = _ij5[1];
1755 vinfos[5].maxsolutions = _nj5;
1756 std::vector<int> vfree(0);
1757 solutions.AddSolution(vinfos,vfree);
1758 }
1759 }
1760 }
1761 
1762 } else
1763 {
1764 {
1765 IkReal j5array[1], cj5array[1], sj5array[1];
1766 bool j5valid[1]={false};
1767 _nj5 = 1;
1768 CheckValue<IkReal> x131=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1769 if(!x131.valid){
1770 continue;
1771 }
1772 CheckValue<IkReal> x132 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1773 if(!x132.valid){
1774 continue;
1775 }
1776 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x131.value)))+(x132.value));
1777 sj5array[0]=IKsin(j5array[0]);
1778 cj5array[0]=IKcos(j5array[0]);
1779 if( j5array[0] > IKPI )
1780 {
1781  j5array[0]-=IK2PI;
1782 }
1783 else if( j5array[0] < -IKPI )
1784 { j5array[0]+=IK2PI;
1785 }
1786 j5valid[0] = true;
1787 for(int ij5 = 0; ij5 < 1; ++ij5)
1788 {
1789 if( !j5valid[ij5] )
1790 {
1791  continue;
1792 }
1793 _ij5[0] = ij5; _ij5[1] = -1;
1794 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1795 {
1796 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1797 {
1798  j5valid[iij5]=false; _ij5[1] = iij5; break;
1799 }
1800 }
1801 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1802 {
1803 IkReal evalcond[8];
1804 IkReal x133=IKcos(j5);
1805 IkReal x134=IKsin(j5);
1806 IkReal x135=((1.0)*gconst18);
1807 if((((1.0)+(((-1.0)*gconst18*x135)))) < -0.00001)
1808 continue;
1809 IkReal x136=IKsqrt(((1.0)+(((-1.0)*gconst18*x135))));
1810 IkReal x137=((1.0)*x136);
1811 evalcond[0]=x133;
1812 evalcond[1]=((-1.0)*x134);
1813 evalcond[2]=((((-1.0)*x133*x135))+new_r11);
1814 evalcond[3]=((((-1.0)*x134*x135))+new_r10);
1815 evalcond[4]=((((-1.0)*x133*x137))+new_r01);
1816 evalcond[5]=((((-1.0)*x134*x137))+new_r00);
1817 evalcond[6]=((((-1.0)*new_r10*x135))+(((-1.0)*new_r00*x137))+x134);
1818 evalcond[7]=((((-1.0)*new_r11*x135))+(((-1.0)*new_r01*x137))+x133);
1819 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 )
1820 {
1821 continue;
1822 }
1823 }
1824 
1825 {
1826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1827 vinfos[0].jointtype = 1;
1828 vinfos[0].foffset = j0;
1829 vinfos[0].indices[0] = _ij0[0];
1830 vinfos[0].indices[1] = _ij0[1];
1831 vinfos[0].maxsolutions = _nj0;
1832 vinfos[1].jointtype = 1;
1833 vinfos[1].foffset = j1;
1834 vinfos[1].indices[0] = _ij1[0];
1835 vinfos[1].indices[1] = _ij1[1];
1836 vinfos[1].maxsolutions = _nj1;
1837 vinfos[2].jointtype = 1;
1838 vinfos[2].foffset = j2;
1839 vinfos[2].indices[0] = _ij2[0];
1840 vinfos[2].indices[1] = _ij2[1];
1841 vinfos[2].maxsolutions = _nj2;
1842 vinfos[3].jointtype = 1;
1843 vinfos[3].foffset = j3;
1844 vinfos[3].indices[0] = _ij3[0];
1845 vinfos[3].indices[1] = _ij3[1];
1846 vinfos[3].maxsolutions = _nj3;
1847 vinfos[4].jointtype = 1;
1848 vinfos[4].foffset = j4;
1849 vinfos[4].indices[0] = _ij4[0];
1850 vinfos[4].indices[1] = _ij4[1];
1851 vinfos[4].maxsolutions = _nj4;
1852 vinfos[5].jointtype = 1;
1853 vinfos[5].foffset = j5;
1854 vinfos[5].indices[0] = _ij5[0];
1855 vinfos[5].indices[1] = _ij5[1];
1856 vinfos[5].maxsolutions = _nj5;
1857 std::vector<int> vfree(0);
1858 solutions.AddSolution(vinfos,vfree);
1859 }
1860 }
1861 }
1862 
1863 }
1864 
1865 }
1866 
1867 }
1868 } while(0);
1869 if( bgotonextstatement )
1870 {
1871 bool bgotonextstatement = true;
1872 do
1873 {
1874 CheckValue<IkReal> x138=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1875 if(!x138.valid){
1876 continue;
1877 }
1878 if((x138.value) < -0.00001)
1879 continue;
1880 IkReal gconst19=IKsqrt(x138.value);
1881 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
1882 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1883 {
1884 bgotonextstatement=false;
1885 {
1886 IkReal j5eval[1];
1887 new_r21=0;
1888 new_r20=0;
1889 new_r02=0;
1890 new_r12=0;
1891 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1892 continue;
1893 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))));
1894 cj3=gconst19;
1895 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
1896  continue;
1897 j3=IKacos(gconst19);
1898 CheckValue<IkReal> x139=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1899 if(!x139.valid){
1900 continue;
1901 }
1902 if((x139.value) < -0.00001)
1903 continue;
1904 IkReal gconst19=IKsqrt(x139.value);
1905 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1906 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1907 {
1908 {
1909 IkReal j5array[1], cj5array[1], sj5array[1];
1910 bool j5valid[1]={false};
1911 _nj5 = 1;
1912 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1913 continue;
1914 CheckValue<IkReal> x140=IKPowWithIntegerCheck(gconst19,-1);
1915 if(!x140.valid){
1916 continue;
1917 }
1918 if( IKabs((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x140.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))))+IKsqr((new_r11*(x140.value)))-1) <= IKFAST_SINCOS_THRESH )
1919  continue;
1920 j5array[0]=IKatan2((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))), (new_r11*(x140.value)));
1921 sj5array[0]=IKsin(j5array[0]);
1922 cj5array[0]=IKcos(j5array[0]);
1923 if( j5array[0] > IKPI )
1924 {
1925  j5array[0]-=IK2PI;
1926 }
1927 else if( j5array[0] < -IKPI )
1928 { j5array[0]+=IK2PI;
1929 }
1930 j5valid[0] = true;
1931 for(int ij5 = 0; ij5 < 1; ++ij5)
1932 {
1933 if( !j5valid[ij5] )
1934 {
1935  continue;
1936 }
1937 _ij5[0] = ij5; _ij5[1] = -1;
1938 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1939 {
1940 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1941 {
1942  j5valid[iij5]=false; _ij5[1] = iij5; break;
1943 }
1944 }
1945 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1946 {
1947 IkReal evalcond[8];
1948 IkReal x141=IKcos(j5);
1949 IkReal x142=IKsin(j5);
1950 IkReal x143=((1.0)*gconst19);
1951 if((((1.0)+(((-1.0)*gconst19*x143)))) < -0.00001)
1952 continue;
1953 IkReal x144=IKsqrt(((1.0)+(((-1.0)*gconst19*x143))));
1954 evalcond[0]=x141;
1955 evalcond[1]=((-1.0)*x142);
1956 evalcond[2]=((((-1.0)*x141*x143))+new_r11);
1957 evalcond[3]=((((-1.0)*x142*x143))+new_r10);
1958 evalcond[4]=(((x141*x144))+new_r01);
1959 evalcond[5]=(((x142*x144))+new_r00);
1960 evalcond[6]=(((new_r00*x144))+x142+(((-1.0)*new_r10*x143)));
1961 evalcond[7]=(((new_r01*x144))+(((-1.0)*new_r11*x143))+x141);
1962 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 )
1963 {
1964 continue;
1965 }
1966 }
1967 
1968 {
1969 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1970 vinfos[0].jointtype = 1;
1971 vinfos[0].foffset = j0;
1972 vinfos[0].indices[0] = _ij0[0];
1973 vinfos[0].indices[1] = _ij0[1];
1974 vinfos[0].maxsolutions = _nj0;
1975 vinfos[1].jointtype = 1;
1976 vinfos[1].foffset = j1;
1977 vinfos[1].indices[0] = _ij1[0];
1978 vinfos[1].indices[1] = _ij1[1];
1979 vinfos[1].maxsolutions = _nj1;
1980 vinfos[2].jointtype = 1;
1981 vinfos[2].foffset = j2;
1982 vinfos[2].indices[0] = _ij2[0];
1983 vinfos[2].indices[1] = _ij2[1];
1984 vinfos[2].maxsolutions = _nj2;
1985 vinfos[3].jointtype = 1;
1986 vinfos[3].foffset = j3;
1987 vinfos[3].indices[0] = _ij3[0];
1988 vinfos[3].indices[1] = _ij3[1];
1989 vinfos[3].maxsolutions = _nj3;
1990 vinfos[4].jointtype = 1;
1991 vinfos[4].foffset = j4;
1992 vinfos[4].indices[0] = _ij4[0];
1993 vinfos[4].indices[1] = _ij4[1];
1994 vinfos[4].maxsolutions = _nj4;
1995 vinfos[5].jointtype = 1;
1996 vinfos[5].foffset = j5;
1997 vinfos[5].indices[0] = _ij5[0];
1998 vinfos[5].indices[1] = _ij5[1];
1999 vinfos[5].maxsolutions = _nj5;
2000 std::vector<int> vfree(0);
2001 solutions.AddSolution(vinfos,vfree);
2002 }
2003 }
2004 }
2005 
2006 } else
2007 {
2008 {
2009 IkReal j5array[1], cj5array[1], sj5array[1];
2010 bool j5valid[1]={false};
2011 _nj5 = 1;
2012 CheckValue<IkReal> x145=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2013 if(!x145.valid){
2014 continue;
2015 }
2016 CheckValue<IkReal> x146 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2017 if(!x146.valid){
2018 continue;
2019 }
2020 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x145.value)))+(x146.value));
2021 sj5array[0]=IKsin(j5array[0]);
2022 cj5array[0]=IKcos(j5array[0]);
2023 if( j5array[0] > IKPI )
2024 {
2025  j5array[0]-=IK2PI;
2026 }
2027 else if( j5array[0] < -IKPI )
2028 { j5array[0]+=IK2PI;
2029 }
2030 j5valid[0] = true;
2031 for(int ij5 = 0; ij5 < 1; ++ij5)
2032 {
2033 if( !j5valid[ij5] )
2034 {
2035  continue;
2036 }
2037 _ij5[0] = ij5; _ij5[1] = -1;
2038 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2039 {
2040 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2041 {
2042  j5valid[iij5]=false; _ij5[1] = iij5; break;
2043 }
2044 }
2045 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2046 {
2047 IkReal evalcond[8];
2048 IkReal x147=IKcos(j5);
2049 IkReal x148=IKsin(j5);
2050 IkReal x149=((1.0)*gconst19);
2051 if((((1.0)+(((-1.0)*gconst19*x149)))) < -0.00001)
2052 continue;
2053 IkReal x150=IKsqrt(((1.0)+(((-1.0)*gconst19*x149))));
2054 evalcond[0]=x147;
2055 evalcond[1]=((-1.0)*x148);
2056 evalcond[2]=((((-1.0)*x147*x149))+new_r11);
2057 evalcond[3]=((((-1.0)*x148*x149))+new_r10);
2058 evalcond[4]=(((x147*x150))+new_r01);
2059 evalcond[5]=(((x148*x150))+new_r00);
2060 evalcond[6]=(((new_r00*x150))+x148+(((-1.0)*new_r10*x149)));
2061 evalcond[7]=(((new_r01*x150))+(((-1.0)*new_r11*x149))+x147);
2062 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 )
2063 {
2064 continue;
2065 }
2066 }
2067 
2068 {
2069 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2070 vinfos[0].jointtype = 1;
2071 vinfos[0].foffset = j0;
2072 vinfos[0].indices[0] = _ij0[0];
2073 vinfos[0].indices[1] = _ij0[1];
2074 vinfos[0].maxsolutions = _nj0;
2075 vinfos[1].jointtype = 1;
2076 vinfos[1].foffset = j1;
2077 vinfos[1].indices[0] = _ij1[0];
2078 vinfos[1].indices[1] = _ij1[1];
2079 vinfos[1].maxsolutions = _nj1;
2080 vinfos[2].jointtype = 1;
2081 vinfos[2].foffset = j2;
2082 vinfos[2].indices[0] = _ij2[0];
2083 vinfos[2].indices[1] = _ij2[1];
2084 vinfos[2].maxsolutions = _nj2;
2085 vinfos[3].jointtype = 1;
2086 vinfos[3].foffset = j3;
2087 vinfos[3].indices[0] = _ij3[0];
2088 vinfos[3].indices[1] = _ij3[1];
2089 vinfos[3].maxsolutions = _nj3;
2090 vinfos[4].jointtype = 1;
2091 vinfos[4].foffset = j4;
2092 vinfos[4].indices[0] = _ij4[0];
2093 vinfos[4].indices[1] = _ij4[1];
2094 vinfos[4].maxsolutions = _nj4;
2095 vinfos[5].jointtype = 1;
2096 vinfos[5].foffset = j5;
2097 vinfos[5].indices[0] = _ij5[0];
2098 vinfos[5].indices[1] = _ij5[1];
2099 vinfos[5].maxsolutions = _nj5;
2100 std::vector<int> vfree(0);
2101 solutions.AddSolution(vinfos,vfree);
2102 }
2103 }
2104 }
2105 
2106 }
2107 
2108 }
2109 
2110 }
2111 } while(0);
2112 if( bgotonextstatement )
2113 {
2114 bool bgotonextstatement = true;
2115 do
2116 {
2117 CheckValue<IkReal> x151=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2118 if(!x151.valid){
2119 continue;
2120 }
2121 if((x151.value) < -0.00001)
2122 continue;
2123 IkReal gconst19=IKsqrt(x151.value);
2124 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
2125 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2126 {
2127 bgotonextstatement=false;
2128 {
2129 IkReal j5eval[1];
2130 new_r21=0;
2131 new_r20=0;
2132 new_r02=0;
2133 new_r12=0;
2134 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2135 continue;
2136 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))));
2137 cj3=gconst19;
2138 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
2139  continue;
2140 j3=((-1.0)*(IKacos(gconst19)));
2141 CheckValue<IkReal> x152=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2142 if(!x152.valid){
2143 continue;
2144 }
2145 if((x152.value) < -0.00001)
2146 continue;
2147 IkReal gconst19=IKsqrt(x152.value);
2148 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2149 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2150 {
2151 {
2152 IkReal j5array[1], cj5array[1], sj5array[1];
2153 bool j5valid[1]={false};
2154 _nj5 = 1;
2155 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2156 continue;
2157 CheckValue<IkReal> x153=IKPowWithIntegerCheck(gconst19,-1);
2158 if(!x153.valid){
2159 continue;
2160 }
2161 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x153.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))))+IKsqr((new_r11*(x153.value)))-1) <= IKFAST_SINCOS_THRESH )
2162  continue;
2163 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))), (new_r11*(x153.value)));
2164 sj5array[0]=IKsin(j5array[0]);
2165 cj5array[0]=IKcos(j5array[0]);
2166 if( j5array[0] > IKPI )
2167 {
2168  j5array[0]-=IK2PI;
2169 }
2170 else if( j5array[0] < -IKPI )
2171 { j5array[0]+=IK2PI;
2172 }
2173 j5valid[0] = true;
2174 for(int ij5 = 0; ij5 < 1; ++ij5)
2175 {
2176 if( !j5valid[ij5] )
2177 {
2178  continue;
2179 }
2180 _ij5[0] = ij5; _ij5[1] = -1;
2181 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2182 {
2183 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2184 {
2185  j5valid[iij5]=false; _ij5[1] = iij5; break;
2186 }
2187 }
2188 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2189 {
2190 IkReal evalcond[8];
2191 IkReal x154=IKcos(j5);
2192 IkReal x155=IKsin(j5);
2193 IkReal x156=((1.0)*gconst19);
2194 if((((1.0)+(((-1.0)*gconst19*x156)))) < -0.00001)
2195 continue;
2196 IkReal x157=IKsqrt(((1.0)+(((-1.0)*gconst19*x156))));
2197 IkReal x158=((1.0)*x157);
2198 evalcond[0]=x154;
2199 evalcond[1]=((-1.0)*x155);
2200 evalcond[2]=((((-1.0)*x154*x156))+new_r11);
2201 evalcond[3]=((((-1.0)*x155*x156))+new_r10);
2202 evalcond[4]=((((-1.0)*x154*x158))+new_r01);
2203 evalcond[5]=((((-1.0)*x155*x158))+new_r00);
2204 evalcond[6]=(x155+(((-1.0)*new_r10*x156))+(((-1.0)*new_r00*x158)));
2205 evalcond[7]=(x154+(((-1.0)*new_r11*x156))+(((-1.0)*new_r01*x158)));
2206 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 )
2207 {
2208 continue;
2209 }
2210 }
2211 
2212 {
2213 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2214 vinfos[0].jointtype = 1;
2215 vinfos[0].foffset = j0;
2216 vinfos[0].indices[0] = _ij0[0];
2217 vinfos[0].indices[1] = _ij0[1];
2218 vinfos[0].maxsolutions = _nj0;
2219 vinfos[1].jointtype = 1;
2220 vinfos[1].foffset = j1;
2221 vinfos[1].indices[0] = _ij1[0];
2222 vinfos[1].indices[1] = _ij1[1];
2223 vinfos[1].maxsolutions = _nj1;
2224 vinfos[2].jointtype = 1;
2225 vinfos[2].foffset = j2;
2226 vinfos[2].indices[0] = _ij2[0];
2227 vinfos[2].indices[1] = _ij2[1];
2228 vinfos[2].maxsolutions = _nj2;
2229 vinfos[3].jointtype = 1;
2230 vinfos[3].foffset = j3;
2231 vinfos[3].indices[0] = _ij3[0];
2232 vinfos[3].indices[1] = _ij3[1];
2233 vinfos[3].maxsolutions = _nj3;
2234 vinfos[4].jointtype = 1;
2235 vinfos[4].foffset = j4;
2236 vinfos[4].indices[0] = _ij4[0];
2237 vinfos[4].indices[1] = _ij4[1];
2238 vinfos[4].maxsolutions = _nj4;
2239 vinfos[5].jointtype = 1;
2240 vinfos[5].foffset = j5;
2241 vinfos[5].indices[0] = _ij5[0];
2242 vinfos[5].indices[1] = _ij5[1];
2243 vinfos[5].maxsolutions = _nj5;
2244 std::vector<int> vfree(0);
2245 solutions.AddSolution(vinfos,vfree);
2246 }
2247 }
2248 }
2249 
2250 } else
2251 {
2252 {
2253 IkReal j5array[1], cj5array[1], sj5array[1];
2254 bool j5valid[1]={false};
2255 _nj5 = 1;
2256 CheckValue<IkReal> x159=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2257 if(!x159.valid){
2258 continue;
2259 }
2260 CheckValue<IkReal> x160 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2261 if(!x160.valid){
2262 continue;
2263 }
2264 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x159.value)))+(x160.value));
2265 sj5array[0]=IKsin(j5array[0]);
2266 cj5array[0]=IKcos(j5array[0]);
2267 if( j5array[0] > IKPI )
2268 {
2269  j5array[0]-=IK2PI;
2270 }
2271 else if( j5array[0] < -IKPI )
2272 { j5array[0]+=IK2PI;
2273 }
2274 j5valid[0] = true;
2275 for(int ij5 = 0; ij5 < 1; ++ij5)
2276 {
2277 if( !j5valid[ij5] )
2278 {
2279  continue;
2280 }
2281 _ij5[0] = ij5; _ij5[1] = -1;
2282 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2283 {
2284 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2285 {
2286  j5valid[iij5]=false; _ij5[1] = iij5; break;
2287 }
2288 }
2289 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2290 {
2291 IkReal evalcond[8];
2292 IkReal x161=IKcos(j5);
2293 IkReal x162=IKsin(j5);
2294 IkReal x163=((1.0)*gconst19);
2295 if((((1.0)+(((-1.0)*gconst19*x163)))) < -0.00001)
2296 continue;
2297 IkReal x164=IKsqrt(((1.0)+(((-1.0)*gconst19*x163))));
2298 IkReal x165=((1.0)*x164);
2299 evalcond[0]=x161;
2300 evalcond[1]=((-1.0)*x162);
2301 evalcond[2]=((((-1.0)*x161*x163))+new_r11);
2302 evalcond[3]=((((-1.0)*x162*x163))+new_r10);
2303 evalcond[4]=((((-1.0)*x161*x165))+new_r01);
2304 evalcond[5]=((((-1.0)*x162*x165))+new_r00);
2305 evalcond[6]=((((-1.0)*new_r00*x165))+(((-1.0)*new_r10*x163))+x162);
2306 evalcond[7]=((((-1.0)*new_r11*x163))+x161+(((-1.0)*new_r01*x165)));
2307 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 )
2308 {
2309 continue;
2310 }
2311 }
2312 
2313 {
2314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2315 vinfos[0].jointtype = 1;
2316 vinfos[0].foffset = j0;
2317 vinfos[0].indices[0] = _ij0[0];
2318 vinfos[0].indices[1] = _ij0[1];
2319 vinfos[0].maxsolutions = _nj0;
2320 vinfos[1].jointtype = 1;
2321 vinfos[1].foffset = j1;
2322 vinfos[1].indices[0] = _ij1[0];
2323 vinfos[1].indices[1] = _ij1[1];
2324 vinfos[1].maxsolutions = _nj1;
2325 vinfos[2].jointtype = 1;
2326 vinfos[2].foffset = j2;
2327 vinfos[2].indices[0] = _ij2[0];
2328 vinfos[2].indices[1] = _ij2[1];
2329 vinfos[2].maxsolutions = _nj2;
2330 vinfos[3].jointtype = 1;
2331 vinfos[3].foffset = j3;
2332 vinfos[3].indices[0] = _ij3[0];
2333 vinfos[3].indices[1] = _ij3[1];
2334 vinfos[3].maxsolutions = _nj3;
2335 vinfos[4].jointtype = 1;
2336 vinfos[4].foffset = j4;
2337 vinfos[4].indices[0] = _ij4[0];
2338 vinfos[4].indices[1] = _ij4[1];
2339 vinfos[4].maxsolutions = _nj4;
2340 vinfos[5].jointtype = 1;
2341 vinfos[5].foffset = j5;
2342 vinfos[5].indices[0] = _ij5[0];
2343 vinfos[5].indices[1] = _ij5[1];
2344 vinfos[5].maxsolutions = _nj5;
2345 std::vector<int> vfree(0);
2346 solutions.AddSolution(vinfos,vfree);
2347 }
2348 }
2349 }
2350 
2351 }
2352 
2353 }
2354 
2355 }
2356 } while(0);
2357 if( bgotonextstatement )
2358 {
2359 bool bgotonextstatement = true;
2360 do
2361 {
2362 if( 1 )
2363 {
2364 bgotonextstatement=false;
2365 continue; // branch miss [j5]
2366 
2367 }
2368 } while(0);
2369 if( bgotonextstatement )
2370 {
2371 }
2372 }
2373 }
2374 }
2375 }
2376 }
2377 }
2378 }
2379 
2380 } else
2381 {
2382 {
2383 IkReal j5array[1], cj5array[1], sj5array[1];
2384 bool j5valid[1]={false};
2385 _nj5 = 1;
2386 IkReal x166=(new_r00*sj3);
2387 CheckValue<IkReal> x167=IKPowWithIntegerCheck(sj3,-1);
2388 if(!x167.valid){
2389 continue;
2390 }
2391 if( IKabs(((((-1.0)*x166))+((cj3*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x166))+((cj3*new_r10))))+IKsqr(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2392  continue;
2393 j5array[0]=IKatan2(((((-1.0)*x166))+((cj3*new_r10))), ((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2394 sj5array[0]=IKsin(j5array[0]);
2395 cj5array[0]=IKcos(j5array[0]);
2396 if( j5array[0] > IKPI )
2397 {
2398  j5array[0]-=IK2PI;
2399 }
2400 else if( j5array[0] < -IKPI )
2401 { j5array[0]+=IK2PI;
2402 }
2403 j5valid[0] = true;
2404 for(int ij5 = 0; ij5 < 1; ++ij5)
2405 {
2406 if( !j5valid[ij5] )
2407 {
2408  continue;
2409 }
2410 _ij5[0] = ij5; _ij5[1] = -1;
2411 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2412 {
2413 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2414 {
2415  j5valid[iij5]=false; _ij5[1] = iij5; break;
2416 }
2417 }
2418 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2419 {
2420 IkReal evalcond[10];
2421 IkReal x168=IKsin(j5);
2422 IkReal x169=IKcos(j5);
2423 IkReal x170=((1.0)*new_r11);
2424 IkReal x171=(new_r22*sj3);
2425 IkReal x172=((1.0)*new_r10);
2426 IkReal x173=((1.0)*cj3*new_r22);
2427 IkReal x174=(sj3*x168);
2428 IkReal x175=(new_r22*x168);
2429 IkReal x176=((1.0)*x169);
2430 IkReal x177=((1.0)*x168);
2431 evalcond[0]=(((new_r00*sj3))+x168+(((-1.0)*cj3*x172)));
2432 evalcond[1]=(((new_r01*sj3))+x169+(((-1.0)*cj3*x170)));
2433 evalcond[2]=(((new_r11*sj3))+x175+((cj3*new_r01)));
2434 evalcond[3]=(((sj3*x169))+((cj3*x175))+new_r01);
2435 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x176))+((cj3*new_r00)));
2436 evalcond[5]=(x174+new_r00+(((-1.0)*x169*x173)));
2437 evalcond[6]=(((x168*x171))+(((-1.0)*cj3*x176))+new_r11);
2438 evalcond[7]=(x169+(((-1.0)*x171*x172))+(((-1.0)*new_r00*x173)));
2439 evalcond[8]=((((-1.0)*cj3*x177))+(((-1.0)*x171*x176))+new_r10);
2440 evalcond[9]=((((-1.0)*x177))+(((-1.0)*x170*x171))+(((-1.0)*new_r01*x173)));
2441 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 )
2442 {
2443 continue;
2444 }
2445 }
2446 
2447 {
2448 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2449 vinfos[0].jointtype = 1;
2450 vinfos[0].foffset = j0;
2451 vinfos[0].indices[0] = _ij0[0];
2452 vinfos[0].indices[1] = _ij0[1];
2453 vinfos[0].maxsolutions = _nj0;
2454 vinfos[1].jointtype = 1;
2455 vinfos[1].foffset = j1;
2456 vinfos[1].indices[0] = _ij1[0];
2457 vinfos[1].indices[1] = _ij1[1];
2458 vinfos[1].maxsolutions = _nj1;
2459 vinfos[2].jointtype = 1;
2460 vinfos[2].foffset = j2;
2461 vinfos[2].indices[0] = _ij2[0];
2462 vinfos[2].indices[1] = _ij2[1];
2463 vinfos[2].maxsolutions = _nj2;
2464 vinfos[3].jointtype = 1;
2465 vinfos[3].foffset = j3;
2466 vinfos[3].indices[0] = _ij3[0];
2467 vinfos[3].indices[1] = _ij3[1];
2468 vinfos[3].maxsolutions = _nj3;
2469 vinfos[4].jointtype = 1;
2470 vinfos[4].foffset = j4;
2471 vinfos[4].indices[0] = _ij4[0];
2472 vinfos[4].indices[1] = _ij4[1];
2473 vinfos[4].maxsolutions = _nj4;
2474 vinfos[5].jointtype = 1;
2475 vinfos[5].foffset = j5;
2476 vinfos[5].indices[0] = _ij5[0];
2477 vinfos[5].indices[1] = _ij5[1];
2478 vinfos[5].maxsolutions = _nj5;
2479 std::vector<int> vfree(0);
2480 solutions.AddSolution(vinfos,vfree);
2481 }
2482 }
2483 }
2484 
2485 }
2486 
2487 }
2488 
2489 } else
2490 {
2491 {
2492 IkReal j5array[1], cj5array[1], sj5array[1];
2493 bool j5valid[1]={false};
2494 _nj5 = 1;
2495 IkReal x178=((1.0)*new_r01);
2496 CheckValue<IkReal> x179=IKPowWithIntegerCheck(new_r22,-1);
2497 if(!x179.valid){
2498 continue;
2499 }
2500 if( IKabs(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r11))+(((-1.0)*sj3*x178)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))))+IKsqr((((cj3*new_r11))+(((-1.0)*sj3*x178))))-1) <= IKFAST_SINCOS_THRESH )
2501  continue;
2502 j5array[0]=IKatan2(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))), (((cj3*new_r11))+(((-1.0)*sj3*x178))));
2503 sj5array[0]=IKsin(j5array[0]);
2504 cj5array[0]=IKcos(j5array[0]);
2505 if( j5array[0] > IKPI )
2506 {
2507  j5array[0]-=IK2PI;
2508 }
2509 else if( j5array[0] < -IKPI )
2510 { j5array[0]+=IK2PI;
2511 }
2512 j5valid[0] = true;
2513 for(int ij5 = 0; ij5 < 1; ++ij5)
2514 {
2515 if( !j5valid[ij5] )
2516 {
2517  continue;
2518 }
2519 _ij5[0] = ij5; _ij5[1] = -1;
2520 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2521 {
2522 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2523 {
2524  j5valid[iij5]=false; _ij5[1] = iij5; break;
2525 }
2526 }
2527 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2528 {
2529 IkReal evalcond[10];
2530 IkReal x180=IKsin(j5);
2531 IkReal x181=IKcos(j5);
2532 IkReal x182=((1.0)*new_r11);
2533 IkReal x183=(new_r22*sj3);
2534 IkReal x184=((1.0)*new_r10);
2535 IkReal x185=((1.0)*cj3*new_r22);
2536 IkReal x186=(sj3*x180);
2537 IkReal x187=(new_r22*x180);
2538 IkReal x188=((1.0)*x181);
2539 IkReal x189=((1.0)*x180);
2540 evalcond[0]=(((new_r00*sj3))+x180+(((-1.0)*cj3*x184)));
2541 evalcond[1]=(((new_r01*sj3))+x181+(((-1.0)*cj3*x182)));
2542 evalcond[2]=(((new_r11*sj3))+x187+((cj3*new_r01)));
2543 evalcond[3]=(((sj3*x181))+((cj3*x187))+new_r01);
2544 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x188))+((cj3*new_r00)));
2545 evalcond[5]=(x186+new_r00+(((-1.0)*x181*x185)));
2546 evalcond[6]=(((x180*x183))+new_r11+(((-1.0)*cj3*x188)));
2547 evalcond[7]=(x181+(((-1.0)*new_r00*x185))+(((-1.0)*x183*x184)));
2548 evalcond[8]=(new_r10+(((-1.0)*x183*x188))+(((-1.0)*cj3*x189)));
2549 evalcond[9]=((((-1.0)*x189))+(((-1.0)*new_r01*x185))+(((-1.0)*x182*x183)));
2550 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 )
2551 {
2552 continue;
2553 }
2554 }
2555 
2556 {
2557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2558 vinfos[0].jointtype = 1;
2559 vinfos[0].foffset = j0;
2560 vinfos[0].indices[0] = _ij0[0];
2561 vinfos[0].indices[1] = _ij0[1];
2562 vinfos[0].maxsolutions = _nj0;
2563 vinfos[1].jointtype = 1;
2564 vinfos[1].foffset = j1;
2565 vinfos[1].indices[0] = _ij1[0];
2566 vinfos[1].indices[1] = _ij1[1];
2567 vinfos[1].maxsolutions = _nj1;
2568 vinfos[2].jointtype = 1;
2569 vinfos[2].foffset = j2;
2570 vinfos[2].indices[0] = _ij2[0];
2571 vinfos[2].indices[1] = _ij2[1];
2572 vinfos[2].maxsolutions = _nj2;
2573 vinfos[3].jointtype = 1;
2574 vinfos[3].foffset = j3;
2575 vinfos[3].indices[0] = _ij3[0];
2576 vinfos[3].indices[1] = _ij3[1];
2577 vinfos[3].maxsolutions = _nj3;
2578 vinfos[4].jointtype = 1;
2579 vinfos[4].foffset = j4;
2580 vinfos[4].indices[0] = _ij4[0];
2581 vinfos[4].indices[1] = _ij4[1];
2582 vinfos[4].maxsolutions = _nj4;
2583 vinfos[5].jointtype = 1;
2584 vinfos[5].foffset = j5;
2585 vinfos[5].indices[0] = _ij5[0];
2586 vinfos[5].indices[1] = _ij5[1];
2587 vinfos[5].maxsolutions = _nj5;
2588 std::vector<int> vfree(0);
2589 solutions.AddSolution(vinfos,vfree);
2590 }
2591 }
2592 }
2593 
2594 }
2595 
2596 }
2597 
2598 } else
2599 {
2600 {
2601 IkReal j5array[1], cj5array[1], sj5array[1];
2602 bool j5valid[1]={false};
2603 _nj5 = 1;
2604 IkReal x190=cj3*cj3;
2605 IkReal x191=(cj3*new_r22);
2606 CheckValue<IkReal> x192=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x190*(new_r22*new_r22)))+x190)),-1);
2607 if(!x192.valid){
2608 continue;
2609 }
2610 CheckValue<IkReal> x193 = IKatan2WithCheck(IkReal((((new_r01*x191))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x191)))),IKFAST_ATAN2_MAGTHRESH);
2611 if(!x193.valid){
2612 continue;
2613 }
2614 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.value)))+(x193.value));
2615 sj5array[0]=IKsin(j5array[0]);
2616 cj5array[0]=IKcos(j5array[0]);
2617 if( j5array[0] > IKPI )
2618 {
2619  j5array[0]-=IK2PI;
2620 }
2621 else if( j5array[0] < -IKPI )
2622 { j5array[0]+=IK2PI;
2623 }
2624 j5valid[0] = true;
2625 for(int ij5 = 0; ij5 < 1; ++ij5)
2626 {
2627 if( !j5valid[ij5] )
2628 {
2629  continue;
2630 }
2631 _ij5[0] = ij5; _ij5[1] = -1;
2632 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2633 {
2634 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2635 {
2636  j5valid[iij5]=false; _ij5[1] = iij5; break;
2637 }
2638 }
2639 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2640 {
2641 IkReal evalcond[10];
2642 IkReal x194=IKsin(j5);
2643 IkReal x195=IKcos(j5);
2644 IkReal x196=((1.0)*new_r11);
2645 IkReal x197=(new_r22*sj3);
2646 IkReal x198=((1.0)*new_r10);
2647 IkReal x199=((1.0)*cj3*new_r22);
2648 IkReal x200=(sj3*x194);
2649 IkReal x201=(new_r22*x194);
2650 IkReal x202=((1.0)*x195);
2651 IkReal x203=((1.0)*x194);
2652 evalcond[0]=(((new_r00*sj3))+x194+(((-1.0)*cj3*x198)));
2653 evalcond[1]=(((new_r01*sj3))+x195+(((-1.0)*cj3*x196)));
2654 evalcond[2]=(((new_r11*sj3))+x201+((cj3*new_r01)));
2655 evalcond[3]=(((sj3*x195))+((cj3*x201))+new_r01);
2656 evalcond[4]=((((-1.0)*new_r22*x202))+((new_r10*sj3))+((cj3*new_r00)));
2657 evalcond[5]=((((-1.0)*x195*x199))+x200+new_r00);
2658 evalcond[6]=(((x194*x197))+new_r11+(((-1.0)*cj3*x202)));
2659 evalcond[7]=((((-1.0)*x197*x198))+x195+(((-1.0)*new_r00*x199)));
2660 evalcond[8]=((((-1.0)*x197*x202))+new_r10+(((-1.0)*cj3*x203)));
2661 evalcond[9]=((((-1.0)*x196*x197))+(((-1.0)*x203))+(((-1.0)*new_r01*x199)));
2662 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 )
2663 {
2664 continue;
2665 }
2666 }
2667 
2668 {
2669 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2670 vinfos[0].jointtype = 1;
2671 vinfos[0].foffset = j0;
2672 vinfos[0].indices[0] = _ij0[0];
2673 vinfos[0].indices[1] = _ij0[1];
2674 vinfos[0].maxsolutions = _nj0;
2675 vinfos[1].jointtype = 1;
2676 vinfos[1].foffset = j1;
2677 vinfos[1].indices[0] = _ij1[0];
2678 vinfos[1].indices[1] = _ij1[1];
2679 vinfos[1].maxsolutions = _nj1;
2680 vinfos[2].jointtype = 1;
2681 vinfos[2].foffset = j2;
2682 vinfos[2].indices[0] = _ij2[0];
2683 vinfos[2].indices[1] = _ij2[1];
2684 vinfos[2].maxsolutions = _nj2;
2685 vinfos[3].jointtype = 1;
2686 vinfos[3].foffset = j3;
2687 vinfos[3].indices[0] = _ij3[0];
2688 vinfos[3].indices[1] = _ij3[1];
2689 vinfos[3].maxsolutions = _nj3;
2690 vinfos[4].jointtype = 1;
2691 vinfos[4].foffset = j4;
2692 vinfos[4].indices[0] = _ij4[0];
2693 vinfos[4].indices[1] = _ij4[1];
2694 vinfos[4].maxsolutions = _nj4;
2695 vinfos[5].jointtype = 1;
2696 vinfos[5].foffset = j5;
2697 vinfos[5].indices[0] = _ij5[0];
2698 vinfos[5].indices[1] = _ij5[1];
2699 vinfos[5].maxsolutions = _nj5;
2700 std::vector<int> vfree(0);
2701 solutions.AddSolution(vinfos,vfree);
2702 }
2703 }
2704 }
2705 
2706 }
2707 
2708 }
2709  }
2710 
2711 }
2712 
2713 }
2714 
2715 }
2716 } while(0);
2717 if( bgotonextstatement )
2718 {
2719 bool bgotonextstatement = true;
2720 do
2721 {
2722 if( 1 )
2723 {
2724 bgotonextstatement=false;
2725 continue; // branch miss [j3, j5]
2726 
2727 }
2728 } while(0);
2729 if( bgotonextstatement )
2730 {
2731 }
2732 }
2733 }
2734 }
2735 }
2736 
2737 } else
2738 {
2739 {
2740 IkReal j3array[1], cj3array[1], sj3array[1];
2741 bool j3valid[1]={false};
2742 _nj3 = 1;
2743 CheckValue<IkReal> x205=IKPowWithIntegerCheck(sj4,-1);
2744 if(!x205.valid){
2745 continue;
2746 }
2747 IkReal x204=x205.value;
2748 CheckValue<IkReal> x206=IKPowWithIntegerCheck(new_r12,-1);
2749 if(!x206.valid){
2750 continue;
2751 }
2752 if( IKabs((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x204)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x204))-1) <= IKFAST_SINCOS_THRESH )
2753  continue;
2754 j3array[0]=IKatan2((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x204));
2755 sj3array[0]=IKsin(j3array[0]);
2756 cj3array[0]=IKcos(j3array[0]);
2757 if( j3array[0] > IKPI )
2758 {
2759  j3array[0]-=IK2PI;
2760 }
2761 else if( j3array[0] < -IKPI )
2762 { j3array[0]+=IK2PI;
2763 }
2764 j3valid[0] = true;
2765 for(int ij3 = 0; ij3 < 1; ++ij3)
2766 {
2767 if( !j3valid[ij3] )
2768 {
2769  continue;
2770 }
2771 _ij3[0] = ij3; _ij3[1] = -1;
2772 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2773 {
2774 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2775 {
2776  j3valid[iij3]=false; _ij3[1] = iij3; break;
2777 }
2778 }
2779 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2780 {
2781 IkReal evalcond[8];
2782 IkReal x207=IKcos(j3);
2783 IkReal x208=IKsin(j3);
2784 IkReal x209=((1.0)*cj4);
2785 IkReal x210=(sj4*x208);
2786 IkReal x211=(new_r12*x208);
2787 IkReal x212=(sj4*x207);
2788 IkReal x213=(new_r02*x207);
2789 evalcond[0]=(x212+new_r02);
2790 evalcond[1]=(x210+new_r12);
2791 evalcond[2]=((((-1.0)*new_r02*x208))+((new_r12*x207)));
2792 evalcond[3]=(sj4+x211+x213);
2793 evalcond[4]=((((-1.0)*new_r20*x209))+((new_r00*x212))+((new_r10*x210)));
2794 evalcond[5]=((((-1.0)*new_r21*x209))+((new_r01*x212))+((new_r11*x210)));
2795 evalcond[6]=((1.0)+(((-1.0)*new_r22*x209))+((new_r02*x212))+((new_r12*x210)));
2796 evalcond[7]=((((-1.0)*new_r22*sj4))+(((-1.0)*x209*x213))+(((-1.0)*x209*x211)));
2797 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 )
2798 {
2799 continue;
2800 }
2801 }
2802 
2803 {
2804 IkReal j5eval[3];
2805 j5eval[0]=sj4;
2806 j5eval[1]=IKsign(sj4);
2807 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2808 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2809 {
2810 {
2811 IkReal j5eval[2];
2812 j5eval[0]=sj4;
2813 j5eval[1]=sj3;
2814 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2815 {
2816 {
2817 IkReal j5eval[2];
2818 j5eval[0]=sj4;
2819 j5eval[1]=cj3;
2820 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2821 {
2822 {
2823 IkReal evalcond[5];
2824 bool bgotonextstatement = true;
2825 do
2826 {
2827 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2828 evalcond[1]=new_r02;
2829 evalcond[2]=new_r12;
2830 evalcond[3]=new_r21;
2831 evalcond[4]=new_r20;
2832 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 )
2833 {
2834 bgotonextstatement=false;
2835 {
2836 IkReal j5array[1], cj5array[1], sj5array[1];
2837 bool j5valid[1]={false};
2838 _nj5 = 1;
2839 IkReal x214=((1.0)*new_r01);
2840 if( IKabs(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x214))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x214))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2841  continue;
2842 j5array[0]=IKatan2(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x214))+((cj3*new_r00))));
2843 sj5array[0]=IKsin(j5array[0]);
2844 cj5array[0]=IKcos(j5array[0]);
2845 if( j5array[0] > IKPI )
2846 {
2847  j5array[0]-=IK2PI;
2848 }
2849 else if( j5array[0] < -IKPI )
2850 { j5array[0]+=IK2PI;
2851 }
2852 j5valid[0] = true;
2853 for(int ij5 = 0; ij5 < 1; ++ij5)
2854 {
2855 if( !j5valid[ij5] )
2856 {
2857  continue;
2858 }
2859 _ij5[0] = ij5; _ij5[1] = -1;
2860 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2861 {
2862 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2863 {
2864  j5valid[iij5]=false; _ij5[1] = iij5; break;
2865 }
2866 }
2867 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2868 {
2869 IkReal evalcond[8];
2870 IkReal x215=IKsin(j5);
2871 IkReal x216=IKcos(j5);
2872 IkReal x217=((1.0)*cj3);
2873 IkReal x218=(sj3*x215);
2874 IkReal x219=((1.0)*x216);
2875 IkReal x220=(x216*x217);
2876 evalcond[0]=(((new_r11*sj3))+x215+((cj3*new_r01)));
2877 evalcond[1]=(((new_r00*sj3))+(((-1.0)*new_r10*x217))+x215);
2878 evalcond[2]=((((-1.0)*new_r11*x217))+((new_r01*sj3))+x216);
2879 evalcond[3]=(((sj3*x216))+((cj3*x215))+new_r01);
2880 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x219)));
2881 evalcond[5]=(x218+new_r00+(((-1.0)*x220)));
2882 evalcond[6]=(x218+new_r11+(((-1.0)*x220)));
2883 evalcond[7]=((((-1.0)*sj3*x219))+new_r10+(((-1.0)*x215*x217)));
2884 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 )
2885 {
2886 continue;
2887 }
2888 }
2889 
2890 {
2891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2892 vinfos[0].jointtype = 1;
2893 vinfos[0].foffset = j0;
2894 vinfos[0].indices[0] = _ij0[0];
2895 vinfos[0].indices[1] = _ij0[1];
2896 vinfos[0].maxsolutions = _nj0;
2897 vinfos[1].jointtype = 1;
2898 vinfos[1].foffset = j1;
2899 vinfos[1].indices[0] = _ij1[0];
2900 vinfos[1].indices[1] = _ij1[1];
2901 vinfos[1].maxsolutions = _nj1;
2902 vinfos[2].jointtype = 1;
2903 vinfos[2].foffset = j2;
2904 vinfos[2].indices[0] = _ij2[0];
2905 vinfos[2].indices[1] = _ij2[1];
2906 vinfos[2].maxsolutions = _nj2;
2907 vinfos[3].jointtype = 1;
2908 vinfos[3].foffset = j3;
2909 vinfos[3].indices[0] = _ij3[0];
2910 vinfos[3].indices[1] = _ij3[1];
2911 vinfos[3].maxsolutions = _nj3;
2912 vinfos[4].jointtype = 1;
2913 vinfos[4].foffset = j4;
2914 vinfos[4].indices[0] = _ij4[0];
2915 vinfos[4].indices[1] = _ij4[1];
2916 vinfos[4].maxsolutions = _nj4;
2917 vinfos[5].jointtype = 1;
2918 vinfos[5].foffset = j5;
2919 vinfos[5].indices[0] = _ij5[0];
2920 vinfos[5].indices[1] = _ij5[1];
2921 vinfos[5].maxsolutions = _nj5;
2922 std::vector<int> vfree(0);
2923 solutions.AddSolution(vinfos,vfree);
2924 }
2925 }
2926 }
2927 
2928 }
2929 } while(0);
2930 if( bgotonextstatement )
2931 {
2932 bool bgotonextstatement = true;
2933 do
2934 {
2935 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2936 evalcond[1]=new_r02;
2937 evalcond[2]=new_r12;
2938 evalcond[3]=new_r21;
2939 evalcond[4]=new_r20;
2940 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 )
2941 {
2942 bgotonextstatement=false;
2943 {
2944 IkReal j5array[1], cj5array[1], sj5array[1];
2945 bool j5valid[1]={false};
2946 _nj5 = 1;
2947 IkReal x221=((1.0)*sj3);
2948 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x221)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x221))))+IKsqr(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2949  continue;
2950 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x221))), ((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))));
2951 sj5array[0]=IKsin(j5array[0]);
2952 cj5array[0]=IKcos(j5array[0]);
2953 if( j5array[0] > IKPI )
2954 {
2955  j5array[0]-=IK2PI;
2956 }
2957 else if( j5array[0] < -IKPI )
2958 { j5array[0]+=IK2PI;
2959 }
2960 j5valid[0] = true;
2961 for(int ij5 = 0; ij5 < 1; ++ij5)
2962 {
2963 if( !j5valid[ij5] )
2964 {
2965  continue;
2966 }
2967 _ij5[0] = ij5; _ij5[1] = -1;
2968 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2969 {
2970 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2971 {
2972  j5valid[iij5]=false; _ij5[1] = iij5; break;
2973 }
2974 }
2975 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2976 {
2977 IkReal evalcond[8];
2978 IkReal x222=IKcos(j5);
2979 IkReal x223=IKsin(j5);
2980 IkReal x224=((1.0)*cj3);
2981 IkReal x225=(sj3*x222);
2982 IkReal x226=((1.0)*x223);
2983 IkReal x227=(x223*x224);
2984 evalcond[0]=(((new_r10*sj3))+x222+((cj3*new_r00)));
2985 evalcond[1]=((((-1.0)*new_r10*x224))+((new_r00*sj3))+x223);
2986 evalcond[2]=((((-1.0)*new_r11*x224))+((new_r01*sj3))+x222);
2987 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x226)));
2988 evalcond[4]=(((cj3*x222))+((sj3*x223))+new_r00);
2989 evalcond[5]=(x225+new_r01+(((-1.0)*x227)));
2990 evalcond[6]=(x225+new_r10+(((-1.0)*x227)));
2991 evalcond[7]=((((-1.0)*x222*x224))+(((-1.0)*sj3*x226))+new_r11);
2992 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 )
2993 {
2994 continue;
2995 }
2996 }
2997 
2998 {
2999 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3000 vinfos[0].jointtype = 1;
3001 vinfos[0].foffset = j0;
3002 vinfos[0].indices[0] = _ij0[0];
3003 vinfos[0].indices[1] = _ij0[1];
3004 vinfos[0].maxsolutions = _nj0;
3005 vinfos[1].jointtype = 1;
3006 vinfos[1].foffset = j1;
3007 vinfos[1].indices[0] = _ij1[0];
3008 vinfos[1].indices[1] = _ij1[1];
3009 vinfos[1].maxsolutions = _nj1;
3010 vinfos[2].jointtype = 1;
3011 vinfos[2].foffset = j2;
3012 vinfos[2].indices[0] = _ij2[0];
3013 vinfos[2].indices[1] = _ij2[1];
3014 vinfos[2].maxsolutions = _nj2;
3015 vinfos[3].jointtype = 1;
3016 vinfos[3].foffset = j3;
3017 vinfos[3].indices[0] = _ij3[0];
3018 vinfos[3].indices[1] = _ij3[1];
3019 vinfos[3].maxsolutions = _nj3;
3020 vinfos[4].jointtype = 1;
3021 vinfos[4].foffset = j4;
3022 vinfos[4].indices[0] = _ij4[0];
3023 vinfos[4].indices[1] = _ij4[1];
3024 vinfos[4].maxsolutions = _nj4;
3025 vinfos[5].jointtype = 1;
3026 vinfos[5].foffset = j5;
3027 vinfos[5].indices[0] = _ij5[0];
3028 vinfos[5].indices[1] = _ij5[1];
3029 vinfos[5].maxsolutions = _nj5;
3030 std::vector<int> vfree(0);
3031 solutions.AddSolution(vinfos,vfree);
3032 }
3033 }
3034 }
3035 
3036 }
3037 } while(0);
3038 if( bgotonextstatement )
3039 {
3040 bool bgotonextstatement = true;
3041 do
3042 {
3043 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3044 evalcond[1]=new_r02;
3045 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3046 {
3047 bgotonextstatement=false;
3048 {
3049 IkReal j5array[1], cj5array[1], sj5array[1];
3050 bool j5valid[1]={false};
3051 _nj5 = 1;
3052 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 )
3053  continue;
3054 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3055 sj5array[0]=IKsin(j5array[0]);
3056 cj5array[0]=IKcos(j5array[0]);
3057 if( j5array[0] > IKPI )
3058 {
3059  j5array[0]-=IK2PI;
3060 }
3061 else if( j5array[0] < -IKPI )
3062 { j5array[0]+=IK2PI;
3063 }
3064 j5valid[0] = true;
3065 for(int ij5 = 0; ij5 < 1; ++ij5)
3066 {
3067 if( !j5valid[ij5] )
3068 {
3069  continue;
3070 }
3071 _ij5[0] = ij5; _ij5[1] = -1;
3072 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3073 {
3074 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3075 {
3076  j5valid[iij5]=false; _ij5[1] = iij5; break;
3077 }
3078 }
3079 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3080 {
3081 IkReal evalcond[8];
3082 IkReal x228=IKsin(j5);
3083 IkReal x229=IKcos(j5);
3084 IkReal x230=((1.0)*cj4);
3085 IkReal x231=((1.0)*sj4);
3086 evalcond[0]=(x228+new_r00);
3087 evalcond[1]=(x229+new_r01);
3088 evalcond[2]=(((sj4*x228))+new_r21);
3089 evalcond[3]=(((cj4*x228))+new_r11);
3090 evalcond[4]=(new_r20+(((-1.0)*x229*x231)));
3091 evalcond[5]=(new_r10+(((-1.0)*x229*x230)));
3092 evalcond[6]=((((-1.0)*new_r20*x231))+x229+(((-1.0)*new_r10*x230)));
3093 evalcond[7]=((((-1.0)*new_r21*x231))+(((-1.0)*new_r11*x230))+(((-1.0)*x228)));
3094 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 )
3095 {
3096 continue;
3097 }
3098 }
3099 
3100 {
3101 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3102 vinfos[0].jointtype = 1;
3103 vinfos[0].foffset = j0;
3104 vinfos[0].indices[0] = _ij0[0];
3105 vinfos[0].indices[1] = _ij0[1];
3106 vinfos[0].maxsolutions = _nj0;
3107 vinfos[1].jointtype = 1;
3108 vinfos[1].foffset = j1;
3109 vinfos[1].indices[0] = _ij1[0];
3110 vinfos[1].indices[1] = _ij1[1];
3111 vinfos[1].maxsolutions = _nj1;
3112 vinfos[2].jointtype = 1;
3113 vinfos[2].foffset = j2;
3114 vinfos[2].indices[0] = _ij2[0];
3115 vinfos[2].indices[1] = _ij2[1];
3116 vinfos[2].maxsolutions = _nj2;
3117 vinfos[3].jointtype = 1;
3118 vinfos[3].foffset = j3;
3119 vinfos[3].indices[0] = _ij3[0];
3120 vinfos[3].indices[1] = _ij3[1];
3121 vinfos[3].maxsolutions = _nj3;
3122 vinfos[4].jointtype = 1;
3123 vinfos[4].foffset = j4;
3124 vinfos[4].indices[0] = _ij4[0];
3125 vinfos[4].indices[1] = _ij4[1];
3126 vinfos[4].maxsolutions = _nj4;
3127 vinfos[5].jointtype = 1;
3128 vinfos[5].foffset = j5;
3129 vinfos[5].indices[0] = _ij5[0];
3130 vinfos[5].indices[1] = _ij5[1];
3131 vinfos[5].maxsolutions = _nj5;
3132 std::vector<int> vfree(0);
3133 solutions.AddSolution(vinfos,vfree);
3134 }
3135 }
3136 }
3137 
3138 }
3139 } while(0);
3140 if( bgotonextstatement )
3141 {
3142 bool bgotonextstatement = true;
3143 do
3144 {
3145 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3146 evalcond[1]=new_r02;
3147 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3148 {
3149 bgotonextstatement=false;
3150 {
3151 IkReal j5array[1], cj5array[1], sj5array[1];
3152 bool j5valid[1]={false};
3153 _nj5 = 1;
3154 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3155  continue;
3156 j5array[0]=IKatan2(new_r00, new_r01);
3157 sj5array[0]=IKsin(j5array[0]);
3158 cj5array[0]=IKcos(j5array[0]);
3159 if( j5array[0] > IKPI )
3160 {
3161  j5array[0]-=IK2PI;
3162 }
3163 else if( j5array[0] < -IKPI )
3164 { j5array[0]+=IK2PI;
3165 }
3166 j5valid[0] = true;
3167 for(int ij5 = 0; ij5 < 1; ++ij5)
3168 {
3169 if( !j5valid[ij5] )
3170 {
3171  continue;
3172 }
3173 _ij5[0] = ij5; _ij5[1] = -1;
3174 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3175 {
3176 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3177 {
3178  j5valid[iij5]=false; _ij5[1] = iij5; break;
3179 }
3180 }
3181 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3182 {
3183 IkReal evalcond[8];
3184 IkReal x232=IKsin(j5);
3185 IkReal x233=IKcos(j5);
3186 IkReal x234=((1.0)*sj4);
3187 IkReal x235=((1.0)*x233);
3188 evalcond[0]=(((sj4*x232))+new_r21);
3189 evalcond[1]=(x232+(((-1.0)*new_r00)));
3190 evalcond[2]=(x233+(((-1.0)*new_r01)));
3191 evalcond[3]=((((-1.0)*x233*x234))+new_r20);
3192 evalcond[4]=(((cj4*x232))+(((-1.0)*new_r11)));
3193 evalcond[5]=((((-1.0)*cj4*x235))+(((-1.0)*new_r10)));
3194 evalcond[6]=((((-1.0)*new_r20*x234))+((cj4*new_r10))+x233);
3195 evalcond[7]=((((-1.0)*new_r21*x234))+((cj4*new_r11))+(((-1.0)*x232)));
3196 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 )
3197 {
3198 continue;
3199 }
3200 }
3201 
3202 {
3203 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3204 vinfos[0].jointtype = 1;
3205 vinfos[0].foffset = j0;
3206 vinfos[0].indices[0] = _ij0[0];
3207 vinfos[0].indices[1] = _ij0[1];
3208 vinfos[0].maxsolutions = _nj0;
3209 vinfos[1].jointtype = 1;
3210 vinfos[1].foffset = j1;
3211 vinfos[1].indices[0] = _ij1[0];
3212 vinfos[1].indices[1] = _ij1[1];
3213 vinfos[1].maxsolutions = _nj1;
3214 vinfos[2].jointtype = 1;
3215 vinfos[2].foffset = j2;
3216 vinfos[2].indices[0] = _ij2[0];
3217 vinfos[2].indices[1] = _ij2[1];
3218 vinfos[2].maxsolutions = _nj2;
3219 vinfos[3].jointtype = 1;
3220 vinfos[3].foffset = j3;
3221 vinfos[3].indices[0] = _ij3[0];
3222 vinfos[3].indices[1] = _ij3[1];
3223 vinfos[3].maxsolutions = _nj3;
3224 vinfos[4].jointtype = 1;
3225 vinfos[4].foffset = j4;
3226 vinfos[4].indices[0] = _ij4[0];
3227 vinfos[4].indices[1] = _ij4[1];
3228 vinfos[4].maxsolutions = _nj4;
3229 vinfos[5].jointtype = 1;
3230 vinfos[5].foffset = j5;
3231 vinfos[5].indices[0] = _ij5[0];
3232 vinfos[5].indices[1] = _ij5[1];
3233 vinfos[5].maxsolutions = _nj5;
3234 std::vector<int> vfree(0);
3235 solutions.AddSolution(vinfos,vfree);
3236 }
3237 }
3238 }
3239 
3240 }
3241 } while(0);
3242 if( bgotonextstatement )
3243 {
3244 bool bgotonextstatement = true;
3245 do
3246 {
3247 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3248 evalcond[1]=new_r12;
3249 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3250 {
3251 bgotonextstatement=false;
3252 {
3253 IkReal j5array[1], cj5array[1], sj5array[1];
3254 bool j5valid[1]={false};
3255 _nj5 = 1;
3256 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3257  continue;
3258 j5array[0]=IKatan2(new_r10, new_r11);
3259 sj5array[0]=IKsin(j5array[0]);
3260 cj5array[0]=IKcos(j5array[0]);
3261 if( j5array[0] > IKPI )
3262 {
3263  j5array[0]-=IK2PI;
3264 }
3265 else if( j5array[0] < -IKPI )
3266 { j5array[0]+=IK2PI;
3267 }
3268 j5valid[0] = true;
3269 for(int ij5 = 0; ij5 < 1; ++ij5)
3270 {
3271 if( !j5valid[ij5] )
3272 {
3273  continue;
3274 }
3275 _ij5[0] = ij5; _ij5[1] = -1;
3276 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3277 {
3278 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3279 {
3280  j5valid[iij5]=false; _ij5[1] = iij5; break;
3281 }
3282 }
3283 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3284 {
3285 IkReal evalcond[8];
3286 IkReal x236=IKcos(j5);
3287 IkReal x237=IKsin(j5);
3288 IkReal x238=((1.0)*cj4);
3289 IkReal x239=((1.0)*sj4);
3290 IkReal x240=((1.0)*x237);
3291 evalcond[0]=(((new_r02*x236))+new_r20);
3292 evalcond[1]=(x237+(((-1.0)*new_r10)));
3293 evalcond[2]=(x236+(((-1.0)*new_r11)));
3294 evalcond[3]=(((cj4*x237))+new_r01);
3295 evalcond[4]=((((-1.0)*new_r02*x240))+new_r21);
3296 evalcond[5]=((((-1.0)*x236*x238))+new_r00);
3297 evalcond[6]=((((-1.0)*new_r20*x239))+x236+(((-1.0)*new_r00*x238)));
3298 evalcond[7]=((((-1.0)*new_r21*x239))+(((-1.0)*x240))+(((-1.0)*new_r01*x238)));
3299 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 )
3300 {
3301 continue;
3302 }
3303 }
3304 
3305 {
3306 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3307 vinfos[0].jointtype = 1;
3308 vinfos[0].foffset = j0;
3309 vinfos[0].indices[0] = _ij0[0];
3310 vinfos[0].indices[1] = _ij0[1];
3311 vinfos[0].maxsolutions = _nj0;
3312 vinfos[1].jointtype = 1;
3313 vinfos[1].foffset = j1;
3314 vinfos[1].indices[0] = _ij1[0];
3315 vinfos[1].indices[1] = _ij1[1];
3316 vinfos[1].maxsolutions = _nj1;
3317 vinfos[2].jointtype = 1;
3318 vinfos[2].foffset = j2;
3319 vinfos[2].indices[0] = _ij2[0];
3320 vinfos[2].indices[1] = _ij2[1];
3321 vinfos[2].maxsolutions = _nj2;
3322 vinfos[3].jointtype = 1;
3323 vinfos[3].foffset = j3;
3324 vinfos[3].indices[0] = _ij3[0];
3325 vinfos[3].indices[1] = _ij3[1];
3326 vinfos[3].maxsolutions = _nj3;
3327 vinfos[4].jointtype = 1;
3328 vinfos[4].foffset = j4;
3329 vinfos[4].indices[0] = _ij4[0];
3330 vinfos[4].indices[1] = _ij4[1];
3331 vinfos[4].maxsolutions = _nj4;
3332 vinfos[5].jointtype = 1;
3333 vinfos[5].foffset = j5;
3334 vinfos[5].indices[0] = _ij5[0];
3335 vinfos[5].indices[1] = _ij5[1];
3336 vinfos[5].maxsolutions = _nj5;
3337 std::vector<int> vfree(0);
3338 solutions.AddSolution(vinfos,vfree);
3339 }
3340 }
3341 }
3342 
3343 }
3344 } while(0);
3345 if( bgotonextstatement )
3346 {
3347 bool bgotonextstatement = true;
3348 do
3349 {
3350 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3351 evalcond[1]=new_r12;
3352 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3353 {
3354 bgotonextstatement=false;
3355 {
3356 IkReal j5array[1], cj5array[1], sj5array[1];
3357 bool j5valid[1]={false};
3358 _nj5 = 1;
3359 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 )
3360  continue;
3361 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3362 sj5array[0]=IKsin(j5array[0]);
3363 cj5array[0]=IKcos(j5array[0]);
3364 if( j5array[0] > IKPI )
3365 {
3366  j5array[0]-=IK2PI;
3367 }
3368 else if( j5array[0] < -IKPI )
3369 { j5array[0]+=IK2PI;
3370 }
3371 j5valid[0] = true;
3372 for(int ij5 = 0; ij5 < 1; ++ij5)
3373 {
3374 if( !j5valid[ij5] )
3375 {
3376  continue;
3377 }
3378 _ij5[0] = ij5; _ij5[1] = -1;
3379 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3380 {
3381 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3382 {
3383  j5valid[iij5]=false; _ij5[1] = iij5; break;
3384 }
3385 }
3386 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3387 {
3388 IkReal evalcond[8];
3389 IkReal x241=IKsin(j5);
3390 IkReal x242=IKcos(j5);
3391 IkReal x243=((1.0)*sj4);
3392 IkReal x244=((1.0)*x242);
3393 evalcond[0]=(x241+new_r10);
3394 evalcond[1]=(x242+new_r11);
3395 evalcond[2]=(new_r21+((new_r02*x241)));
3396 evalcond[3]=((((-1.0)*new_r02*x244))+new_r20);
3397 evalcond[4]=(((cj4*x241))+(((-1.0)*new_r01)));
3398 evalcond[5]=((((-1.0)*cj4*x244))+(((-1.0)*new_r00)));
3399 evalcond[6]=(((cj4*new_r00))+x242+(((-1.0)*new_r20*x243)));
3400 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x241))+(((-1.0)*new_r21*x243)));
3401 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 )
3402 {
3403 continue;
3404 }
3405 }
3406 
3407 {
3408 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3409 vinfos[0].jointtype = 1;
3410 vinfos[0].foffset = j0;
3411 vinfos[0].indices[0] = _ij0[0];
3412 vinfos[0].indices[1] = _ij0[1];
3413 vinfos[0].maxsolutions = _nj0;
3414 vinfos[1].jointtype = 1;
3415 vinfos[1].foffset = j1;
3416 vinfos[1].indices[0] = _ij1[0];
3417 vinfos[1].indices[1] = _ij1[1];
3418 vinfos[1].maxsolutions = _nj1;
3419 vinfos[2].jointtype = 1;
3420 vinfos[2].foffset = j2;
3421 vinfos[2].indices[0] = _ij2[0];
3422 vinfos[2].indices[1] = _ij2[1];
3423 vinfos[2].maxsolutions = _nj2;
3424 vinfos[3].jointtype = 1;
3425 vinfos[3].foffset = j3;
3426 vinfos[3].indices[0] = _ij3[0];
3427 vinfos[3].indices[1] = _ij3[1];
3428 vinfos[3].maxsolutions = _nj3;
3429 vinfos[4].jointtype = 1;
3430 vinfos[4].foffset = j4;
3431 vinfos[4].indices[0] = _ij4[0];
3432 vinfos[4].indices[1] = _ij4[1];
3433 vinfos[4].maxsolutions = _nj4;
3434 vinfos[5].jointtype = 1;
3435 vinfos[5].foffset = j5;
3436 vinfos[5].indices[0] = _ij5[0];
3437 vinfos[5].indices[1] = _ij5[1];
3438 vinfos[5].maxsolutions = _nj5;
3439 std::vector<int> vfree(0);
3440 solutions.AddSolution(vinfos,vfree);
3441 }
3442 }
3443 }
3444 
3445 }
3446 } while(0);
3447 if( bgotonextstatement )
3448 {
3449 bool bgotonextstatement = true;
3450 do
3451 {
3452 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3453 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3454 {
3455 bgotonextstatement=false;
3456 {
3457 IkReal j5eval[1];
3458 new_r21=0;
3459 new_r20=0;
3460 new_r02=0;
3461 new_r12=0;
3462 j5eval[0]=1.0;
3463 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3464 {
3465 continue; // no branches [j5]
3466 
3467 } else
3468 {
3469 IkReal op[2+1], zeror[2];
3470 int numroots;
3471 op[0]=-1.0;
3472 op[1]=0;
3473 op[2]=1.0;
3474 polyroots2(op,zeror,numroots);
3475 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3476 int numsolutions = 0;
3477 for(int ij5 = 0; ij5 < numroots; ++ij5)
3478 {
3479 IkReal htj5 = zeror[ij5];
3480 tempj5array[0]=((2.0)*(atan(htj5)));
3481 for(int kj5 = 0; kj5 < 1; ++kj5)
3482 {
3483 j5array[numsolutions] = tempj5array[kj5];
3484 if( j5array[numsolutions] > IKPI )
3485 {
3486  j5array[numsolutions]-=IK2PI;
3487 }
3488 else if( j5array[numsolutions] < -IKPI )
3489 {
3490  j5array[numsolutions]+=IK2PI;
3491 }
3492 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3493 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3494 numsolutions++;
3495 }
3496 }
3497 bool j5valid[2]={true,true};
3498 _nj5 = 2;
3499 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3500  {
3501 if( !j5valid[ij5] )
3502 {
3503  continue;
3504 }
3505  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3506 htj5 = IKtan(j5/2);
3507 
3508 _ij5[0] = ij5; _ij5[1] = -1;
3509 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3510 {
3511 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3512 {
3513  j5valid[iij5]=false; _ij5[1] = iij5; break;
3514 }
3515 }
3516 {
3517 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3518 vinfos[0].jointtype = 1;
3519 vinfos[0].foffset = j0;
3520 vinfos[0].indices[0] = _ij0[0];
3521 vinfos[0].indices[1] = _ij0[1];
3522 vinfos[0].maxsolutions = _nj0;
3523 vinfos[1].jointtype = 1;
3524 vinfos[1].foffset = j1;
3525 vinfos[1].indices[0] = _ij1[0];
3526 vinfos[1].indices[1] = _ij1[1];
3527 vinfos[1].maxsolutions = _nj1;
3528 vinfos[2].jointtype = 1;
3529 vinfos[2].foffset = j2;
3530 vinfos[2].indices[0] = _ij2[0];
3531 vinfos[2].indices[1] = _ij2[1];
3532 vinfos[2].maxsolutions = _nj2;
3533 vinfos[3].jointtype = 1;
3534 vinfos[3].foffset = j3;
3535 vinfos[3].indices[0] = _ij3[0];
3536 vinfos[3].indices[1] = _ij3[1];
3537 vinfos[3].maxsolutions = _nj3;
3538 vinfos[4].jointtype = 1;
3539 vinfos[4].foffset = j4;
3540 vinfos[4].indices[0] = _ij4[0];
3541 vinfos[4].indices[1] = _ij4[1];
3542 vinfos[4].maxsolutions = _nj4;
3543 vinfos[5].jointtype = 1;
3544 vinfos[5].foffset = j5;
3545 vinfos[5].indices[0] = _ij5[0];
3546 vinfos[5].indices[1] = _ij5[1];
3547 vinfos[5].maxsolutions = _nj5;
3548 std::vector<int> vfree(0);
3549 solutions.AddSolution(vinfos,vfree);
3550 }
3551  }
3552 
3553 }
3554 
3555 }
3556 
3557 }
3558 } while(0);
3559 if( bgotonextstatement )
3560 {
3561 bool bgotonextstatement = true;
3562 do
3563 {
3564 if( 1 )
3565 {
3566 bgotonextstatement=false;
3567 continue; // branch miss [j5]
3568 
3569 }
3570 } while(0);
3571 if( bgotonextstatement )
3572 {
3573 }
3574 }
3575 }
3576 }
3577 }
3578 }
3579 }
3580 }
3581 }
3582 
3583 } else
3584 {
3585 {
3586 IkReal j5array[1], cj5array[1], sj5array[1];
3587 bool j5valid[1]={false};
3588 _nj5 = 1;
3589 CheckValue<IkReal> x246=IKPowWithIntegerCheck(sj4,-1);
3590 if(!x246.valid){
3591 continue;
3592 }
3593 IkReal x245=x246.value;
3594 CheckValue<IkReal> x247=IKPowWithIntegerCheck(cj3,-1);
3595 if(!x247.valid){
3596 continue;
3597 }
3598 if( IKabs(((-1.0)*new_r21*x245)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x245))+IKsqr((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3599  continue;
3600 j5array[0]=IKatan2(((-1.0)*new_r21*x245), (x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3601 sj5array[0]=IKsin(j5array[0]);
3602 cj5array[0]=IKcos(j5array[0]);
3603 if( j5array[0] > IKPI )
3604 {
3605  j5array[0]-=IK2PI;
3606 }
3607 else if( j5array[0] < -IKPI )
3608 { j5array[0]+=IK2PI;
3609 }
3610 j5valid[0] = true;
3611 for(int ij5 = 0; ij5 < 1; ++ij5)
3612 {
3613 if( !j5valid[ij5] )
3614 {
3615  continue;
3616 }
3617 _ij5[0] = ij5; _ij5[1] = -1;
3618 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3619 {
3620 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3621 {
3622  j5valid[iij5]=false; _ij5[1] = iij5; break;
3623 }
3624 }
3625 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3626 {
3627 IkReal evalcond[12];
3628 IkReal x248=IKsin(j5);
3629 IkReal x249=IKcos(j5);
3630 IkReal x250=(cj3*new_r00);
3631 IkReal x251=(cj3*cj4);
3632 IkReal x252=((1.0)*cj3);
3633 IkReal x253=(new_r11*sj3);
3634 IkReal x254=((1.0)*cj4);
3635 IkReal x255=(new_r10*sj3);
3636 IkReal x256=((1.0)*sj4);
3637 IkReal x257=(sj3*x248);
3638 IkReal x258=((1.0)*x249);
3639 IkReal x259=(sj3*x249);
3640 evalcond[0]=(new_r21+((sj4*x248)));
3641 evalcond[1]=((((-1.0)*x249*x256))+new_r20);
3642 evalcond[2]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x248);
3643 evalcond[3]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x249);
3644 evalcond[4]=(((cj4*x248))+x253+((cj3*new_r01)));
3645 evalcond[5]=(((x248*x251))+x259+new_r01);
3646 evalcond[6]=(x255+x250+(((-1.0)*x249*x254)));
3647 evalcond[7]=((((-1.0)*x251*x258))+x257+new_r00);
3648 evalcond[8]=((((-1.0)*x249*x252))+new_r11+((cj4*x257)));
3649 evalcond[9]=((((-1.0)*x254*x259))+(((-1.0)*x248*x252))+new_r10);
3650 evalcond[10]=((((-1.0)*x250*x254))+(((-1.0)*x254*x255))+x249+(((-1.0)*new_r20*x256)));
3651 evalcond[11]=((((-1.0)*new_r21*x256))+(((-1.0)*x248))+(((-1.0)*x253*x254))+(((-1.0)*new_r01*x251)));
3652 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 )
3653 {
3654 continue;
3655 }
3656 }
3657 
3658 {
3659 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3660 vinfos[0].jointtype = 1;
3661 vinfos[0].foffset = j0;
3662 vinfos[0].indices[0] = _ij0[0];
3663 vinfos[0].indices[1] = _ij0[1];
3664 vinfos[0].maxsolutions = _nj0;
3665 vinfos[1].jointtype = 1;
3666 vinfos[1].foffset = j1;
3667 vinfos[1].indices[0] = _ij1[0];
3668 vinfos[1].indices[1] = _ij1[1];
3669 vinfos[1].maxsolutions = _nj1;
3670 vinfos[2].jointtype = 1;
3671 vinfos[2].foffset = j2;
3672 vinfos[2].indices[0] = _ij2[0];
3673 vinfos[2].indices[1] = _ij2[1];
3674 vinfos[2].maxsolutions = _nj2;
3675 vinfos[3].jointtype = 1;
3676 vinfos[3].foffset = j3;
3677 vinfos[3].indices[0] = _ij3[0];
3678 vinfos[3].indices[1] = _ij3[1];
3679 vinfos[3].maxsolutions = _nj3;
3680 vinfos[4].jointtype = 1;
3681 vinfos[4].foffset = j4;
3682 vinfos[4].indices[0] = _ij4[0];
3683 vinfos[4].indices[1] = _ij4[1];
3684 vinfos[4].maxsolutions = _nj4;
3685 vinfos[5].jointtype = 1;
3686 vinfos[5].foffset = j5;
3687 vinfos[5].indices[0] = _ij5[0];
3688 vinfos[5].indices[1] = _ij5[1];
3689 vinfos[5].maxsolutions = _nj5;
3690 std::vector<int> vfree(0);
3691 solutions.AddSolution(vinfos,vfree);
3692 }
3693 }
3694 }
3695 
3696 }
3697 
3698 }
3699 
3700 } else
3701 {
3702 {
3703 IkReal j5array[1], cj5array[1], sj5array[1];
3704 bool j5valid[1]={false};
3705 _nj5 = 1;
3706 CheckValue<IkReal> x261=IKPowWithIntegerCheck(sj4,-1);
3707 if(!x261.valid){
3708 continue;
3709 }
3710 IkReal x260=x261.value;
3711 CheckValue<IkReal> x262=IKPowWithIntegerCheck(sj3,-1);
3712 if(!x262.valid){
3713 continue;
3714 }
3715 if( IKabs(((-1.0)*new_r21*x260)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x260))+IKsqr((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3716  continue;
3717 j5array[0]=IKatan2(((-1.0)*new_r21*x260), (x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3718 sj5array[0]=IKsin(j5array[0]);
3719 cj5array[0]=IKcos(j5array[0]);
3720 if( j5array[0] > IKPI )
3721 {
3722  j5array[0]-=IK2PI;
3723 }
3724 else if( j5array[0] < -IKPI )
3725 { j5array[0]+=IK2PI;
3726 }
3727 j5valid[0] = true;
3728 for(int ij5 = 0; ij5 < 1; ++ij5)
3729 {
3730 if( !j5valid[ij5] )
3731 {
3732  continue;
3733 }
3734 _ij5[0] = ij5; _ij5[1] = -1;
3735 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3736 {
3737 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3738 {
3739  j5valid[iij5]=false; _ij5[1] = iij5; break;
3740 }
3741 }
3742 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3743 {
3744 IkReal evalcond[12];
3745 IkReal x263=IKsin(j5);
3746 IkReal x264=IKcos(j5);
3747 IkReal x265=(cj3*new_r00);
3748 IkReal x266=(cj3*cj4);
3749 IkReal x267=((1.0)*cj3);
3750 IkReal x268=(new_r11*sj3);
3751 IkReal x269=((1.0)*cj4);
3752 IkReal x270=(new_r10*sj3);
3753 IkReal x271=((1.0)*sj4);
3754 IkReal x272=(sj3*x263);
3755 IkReal x273=((1.0)*x264);
3756 IkReal x274=(sj3*x264);
3757 evalcond[0]=(new_r21+((sj4*x263)));
3758 evalcond[1]=(new_r20+(((-1.0)*x264*x271)));
3759 evalcond[2]=(((new_r00*sj3))+x263+(((-1.0)*new_r10*x267)));
3760 evalcond[3]=(((new_r01*sj3))+x264+(((-1.0)*new_r11*x267)));
3761 evalcond[4]=(((cj4*x263))+x268+((cj3*new_r01)));
3762 evalcond[5]=(((x263*x266))+x274+new_r01);
3763 evalcond[6]=(x265+x270+(((-1.0)*x264*x269)));
3764 evalcond[7]=(x272+(((-1.0)*x266*x273))+new_r00);
3765 evalcond[8]=(((cj4*x272))+new_r11+(((-1.0)*x264*x267)));
3766 evalcond[9]=((((-1.0)*x263*x267))+(((-1.0)*x269*x274))+new_r10);
3767 evalcond[10]=((((-1.0)*x269*x270))+x264+(((-1.0)*new_r20*x271))+(((-1.0)*x265*x269)));
3768 evalcond[11]=((((-1.0)*x263))+(((-1.0)*new_r01*x266))+(((-1.0)*new_r21*x271))+(((-1.0)*x268*x269)));
3769 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 )
3770 {
3771 continue;
3772 }
3773 }
3774 
3775 {
3776 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3777 vinfos[0].jointtype = 1;
3778 vinfos[0].foffset = j0;
3779 vinfos[0].indices[0] = _ij0[0];
3780 vinfos[0].indices[1] = _ij0[1];
3781 vinfos[0].maxsolutions = _nj0;
3782 vinfos[1].jointtype = 1;
3783 vinfos[1].foffset = j1;
3784 vinfos[1].indices[0] = _ij1[0];
3785 vinfos[1].indices[1] = _ij1[1];
3786 vinfos[1].maxsolutions = _nj1;
3787 vinfos[2].jointtype = 1;
3788 vinfos[2].foffset = j2;
3789 vinfos[2].indices[0] = _ij2[0];
3790 vinfos[2].indices[1] = _ij2[1];
3791 vinfos[2].maxsolutions = _nj2;
3792 vinfos[3].jointtype = 1;
3793 vinfos[3].foffset = j3;
3794 vinfos[3].indices[0] = _ij3[0];
3795 vinfos[3].indices[1] = _ij3[1];
3796 vinfos[3].maxsolutions = _nj3;
3797 vinfos[4].jointtype = 1;
3798 vinfos[4].foffset = j4;
3799 vinfos[4].indices[0] = _ij4[0];
3800 vinfos[4].indices[1] = _ij4[1];
3801 vinfos[4].maxsolutions = _nj4;
3802 vinfos[5].jointtype = 1;
3803 vinfos[5].foffset = j5;
3804 vinfos[5].indices[0] = _ij5[0];
3805 vinfos[5].indices[1] = _ij5[1];
3806 vinfos[5].maxsolutions = _nj5;
3807 std::vector<int> vfree(0);
3808 solutions.AddSolution(vinfos,vfree);
3809 }
3810 }
3811 }
3812 
3813 }
3814 
3815 }
3816 
3817 } else
3818 {
3819 {
3820 IkReal j5array[1], cj5array[1], sj5array[1];
3821 bool j5valid[1]={false};
3822 _nj5 = 1;
3823 CheckValue<IkReal> x275 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3824 if(!x275.valid){
3825 continue;
3826 }
3827 CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(sj4),-1);
3828 if(!x276.valid){
3829 continue;
3830 }
3831 j5array[0]=((-1.5707963267949)+(x275.value)+(((1.5707963267949)*(x276.value))));
3832 sj5array[0]=IKsin(j5array[0]);
3833 cj5array[0]=IKcos(j5array[0]);
3834 if( j5array[0] > IKPI )
3835 {
3836  j5array[0]-=IK2PI;
3837 }
3838 else if( j5array[0] < -IKPI )
3839 { j5array[0]+=IK2PI;
3840 }
3841 j5valid[0] = true;
3842 for(int ij5 = 0; ij5 < 1; ++ij5)
3843 {
3844 if( !j5valid[ij5] )
3845 {
3846  continue;
3847 }
3848 _ij5[0] = ij5; _ij5[1] = -1;
3849 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3850 {
3851 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3852 {
3853  j5valid[iij5]=false; _ij5[1] = iij5; break;
3854 }
3855 }
3856 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3857 {
3858 IkReal evalcond[12];
3859 IkReal x277=IKsin(j5);
3860 IkReal x278=IKcos(j5);
3861 IkReal x279=(cj3*new_r00);
3862 IkReal x280=(cj3*cj4);
3863 IkReal x281=((1.0)*cj3);
3864 IkReal x282=(new_r11*sj3);
3865 IkReal x283=((1.0)*cj4);
3866 IkReal x284=(new_r10*sj3);
3867 IkReal x285=((1.0)*sj4);
3868 IkReal x286=(sj3*x277);
3869 IkReal x287=((1.0)*x278);
3870 IkReal x288=(sj3*x278);
3871 evalcond[0]=(new_r21+((sj4*x277)));
3872 evalcond[1]=(new_r20+(((-1.0)*x278*x285)));
3873 evalcond[2]=(((new_r00*sj3))+x277+(((-1.0)*new_r10*x281)));
3874 evalcond[3]=(((new_r01*sj3))+x278+(((-1.0)*new_r11*x281)));
3875 evalcond[4]=(((cj4*x277))+x282+((cj3*new_r01)));
3876 evalcond[5]=(x288+new_r01+((x277*x280)));
3877 evalcond[6]=(x279+x284+(((-1.0)*x278*x283)));
3878 evalcond[7]=((((-1.0)*x280*x287))+x286+new_r00);
3879 evalcond[8]=(new_r11+((cj4*x286))+(((-1.0)*x278*x281)));
3880 evalcond[9]=((((-1.0)*x277*x281))+new_r10+(((-1.0)*x283*x288)));
3881 evalcond[10]=(x278+(((-1.0)*new_r20*x285))+(((-1.0)*x279*x283))+(((-1.0)*x283*x284)));
3882 evalcond[11]=((((-1.0)*x277))+(((-1.0)*x282*x283))+(((-1.0)*new_r01*x280))+(((-1.0)*new_r21*x285)));
3883 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 )
3884 {
3885 continue;
3886 }
3887 }
3888 
3889 {
3890 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3891 vinfos[0].jointtype = 1;
3892 vinfos[0].foffset = j0;
3893 vinfos[0].indices[0] = _ij0[0];
3894 vinfos[0].indices[1] = _ij0[1];
3895 vinfos[0].maxsolutions = _nj0;
3896 vinfos[1].jointtype = 1;
3897 vinfos[1].foffset = j1;
3898 vinfos[1].indices[0] = _ij1[0];
3899 vinfos[1].indices[1] = _ij1[1];
3900 vinfos[1].maxsolutions = _nj1;
3901 vinfos[2].jointtype = 1;
3902 vinfos[2].foffset = j2;
3903 vinfos[2].indices[0] = _ij2[0];
3904 vinfos[2].indices[1] = _ij2[1];
3905 vinfos[2].maxsolutions = _nj2;
3906 vinfos[3].jointtype = 1;
3907 vinfos[3].foffset = j3;
3908 vinfos[3].indices[0] = _ij3[0];
3909 vinfos[3].indices[1] = _ij3[1];
3910 vinfos[3].maxsolutions = _nj3;
3911 vinfos[4].jointtype = 1;
3912 vinfos[4].foffset = j4;
3913 vinfos[4].indices[0] = _ij4[0];
3914 vinfos[4].indices[1] = _ij4[1];
3915 vinfos[4].maxsolutions = _nj4;
3916 vinfos[5].jointtype = 1;
3917 vinfos[5].foffset = j5;
3918 vinfos[5].indices[0] = _ij5[0];
3919 vinfos[5].indices[1] = _ij5[1];
3920 vinfos[5].maxsolutions = _nj5;
3921 std::vector<int> vfree(0);
3922 solutions.AddSolution(vinfos,vfree);
3923 }
3924 }
3925 }
3926 
3927 }
3928 
3929 }
3930 }
3931 }
3932 
3933 }
3934 
3935 }
3936 
3937 } else
3938 {
3939 {
3940 IkReal j3array[1], cj3array[1], sj3array[1];
3941 bool j3valid[1]={false};
3942 _nj3 = 1;
3943 CheckValue<IkReal> x289=IKPowWithIntegerCheck(IKsign(sj4),-1);
3944 if(!x289.valid){
3945 continue;
3946 }
3947 CheckValue<IkReal> x290 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3948 if(!x290.valid){
3949 continue;
3950 }
3951 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x289.value)))+(x290.value));
3952 sj3array[0]=IKsin(j3array[0]);
3953 cj3array[0]=IKcos(j3array[0]);
3954 if( j3array[0] > IKPI )
3955 {
3956  j3array[0]-=IK2PI;
3957 }
3958 else if( j3array[0] < -IKPI )
3959 { j3array[0]+=IK2PI;
3960 }
3961 j3valid[0] = true;
3962 for(int ij3 = 0; ij3 < 1; ++ij3)
3963 {
3964 if( !j3valid[ij3] )
3965 {
3966  continue;
3967 }
3968 _ij3[0] = ij3; _ij3[1] = -1;
3969 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3970 {
3971 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3972 {
3973  j3valid[iij3]=false; _ij3[1] = iij3; break;
3974 }
3975 }
3976 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3977 {
3978 IkReal evalcond[8];
3979 IkReal x291=IKcos(j3);
3980 IkReal x292=IKsin(j3);
3981 IkReal x293=((1.0)*cj4);
3982 IkReal x294=(sj4*x292);
3983 IkReal x295=(new_r12*x292);
3984 IkReal x296=(sj4*x291);
3985 IkReal x297=(new_r02*x291);
3986 evalcond[0]=(x296+new_r02);
3987 evalcond[1]=(x294+new_r12);
3988 evalcond[2]=(((new_r12*x291))+(((-1.0)*new_r02*x292)));
3989 evalcond[3]=(sj4+x295+x297);
3990 evalcond[4]=((((-1.0)*new_r20*x293))+((new_r10*x294))+((new_r00*x296)));
3991 evalcond[5]=((((-1.0)*new_r21*x293))+((new_r11*x294))+((new_r01*x296)));
3992 evalcond[6]=((1.0)+((new_r02*x296))+((new_r12*x294))+(((-1.0)*new_r22*x293)));
3993 evalcond[7]=((((-1.0)*x293*x297))+(((-1.0)*x293*x295))+(((-1.0)*new_r22*sj4)));
3994 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 )
3995 {
3996 continue;
3997 }
3998 }
3999 
4000 {
4001 IkReal j5eval[3];
4002 j5eval[0]=sj4;
4003 j5eval[1]=IKsign(sj4);
4004 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4005 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4006 {
4007 {
4008 IkReal j5eval[2];
4009 j5eval[0]=sj4;
4010 j5eval[1]=sj3;
4011 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4012 {
4013 {
4014 IkReal j5eval[2];
4015 j5eval[0]=sj4;
4016 j5eval[1]=cj3;
4017 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4018 {
4019 {
4020 IkReal evalcond[5];
4021 bool bgotonextstatement = true;
4022 do
4023 {
4024 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4025 evalcond[1]=new_r02;
4026 evalcond[2]=new_r12;
4027 evalcond[3]=new_r21;
4028 evalcond[4]=new_r20;
4029 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 )
4030 {
4031 bgotonextstatement=false;
4032 {
4033 IkReal j5array[1], cj5array[1], sj5array[1];
4034 bool j5valid[1]={false};
4035 _nj5 = 1;
4036 IkReal x298=((1.0)*new_r01);
4037 if( IKabs(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x298))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x298))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
4038  continue;
4039 j5array[0]=IKatan2(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x298))+((cj3*new_r00))));
4040 sj5array[0]=IKsin(j5array[0]);
4041 cj5array[0]=IKcos(j5array[0]);
4042 if( j5array[0] > IKPI )
4043 {
4044  j5array[0]-=IK2PI;
4045 }
4046 else if( j5array[0] < -IKPI )
4047 { j5array[0]+=IK2PI;
4048 }
4049 j5valid[0] = true;
4050 for(int ij5 = 0; ij5 < 1; ++ij5)
4051 {
4052 if( !j5valid[ij5] )
4053 {
4054  continue;
4055 }
4056 _ij5[0] = ij5; _ij5[1] = -1;
4057 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4058 {
4059 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4060 {
4061  j5valid[iij5]=false; _ij5[1] = iij5; break;
4062 }
4063 }
4064 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4065 {
4066 IkReal evalcond[8];
4067 IkReal x299=IKsin(j5);
4068 IkReal x300=IKcos(j5);
4069 IkReal x301=((1.0)*cj3);
4070 IkReal x302=(sj3*x299);
4071 IkReal x303=((1.0)*x300);
4072 IkReal x304=(x300*x301);
4073 evalcond[0]=(((new_r11*sj3))+x299+((cj3*new_r01)));
4074 evalcond[1]=(((new_r00*sj3))+x299+(((-1.0)*new_r10*x301)));
4075 evalcond[2]=(((new_r01*sj3))+x300+(((-1.0)*new_r11*x301)));
4076 evalcond[3]=(((cj3*x299))+((sj3*x300))+new_r01);
4077 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x303)));
4078 evalcond[5]=(x302+new_r00+(((-1.0)*x304)));
4079 evalcond[6]=(x302+new_r11+(((-1.0)*x304)));
4080 evalcond[7]=((((-1.0)*sj3*x303))+(((-1.0)*x299*x301))+new_r10);
4081 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 )
4082 {
4083 continue;
4084 }
4085 }
4086 
4087 {
4088 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4089 vinfos[0].jointtype = 1;
4090 vinfos[0].foffset = j0;
4091 vinfos[0].indices[0] = _ij0[0];
4092 vinfos[0].indices[1] = _ij0[1];
4093 vinfos[0].maxsolutions = _nj0;
4094 vinfos[1].jointtype = 1;
4095 vinfos[1].foffset = j1;
4096 vinfos[1].indices[0] = _ij1[0];
4097 vinfos[1].indices[1] = _ij1[1];
4098 vinfos[1].maxsolutions = _nj1;
4099 vinfos[2].jointtype = 1;
4100 vinfos[2].foffset = j2;
4101 vinfos[2].indices[0] = _ij2[0];
4102 vinfos[2].indices[1] = _ij2[1];
4103 vinfos[2].maxsolutions = _nj2;
4104 vinfos[3].jointtype = 1;
4105 vinfos[3].foffset = j3;
4106 vinfos[3].indices[0] = _ij3[0];
4107 vinfos[3].indices[1] = _ij3[1];
4108 vinfos[3].maxsolutions = _nj3;
4109 vinfos[4].jointtype = 1;
4110 vinfos[4].foffset = j4;
4111 vinfos[4].indices[0] = _ij4[0];
4112 vinfos[4].indices[1] = _ij4[1];
4113 vinfos[4].maxsolutions = _nj4;
4114 vinfos[5].jointtype = 1;
4115 vinfos[5].foffset = j5;
4116 vinfos[5].indices[0] = _ij5[0];
4117 vinfos[5].indices[1] = _ij5[1];
4118 vinfos[5].maxsolutions = _nj5;
4119 std::vector<int> vfree(0);
4120 solutions.AddSolution(vinfos,vfree);
4121 }
4122 }
4123 }
4124 
4125 }
4126 } while(0);
4127 if( bgotonextstatement )
4128 {
4129 bool bgotonextstatement = true;
4130 do
4131 {
4132 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4133 evalcond[1]=new_r02;
4134 evalcond[2]=new_r12;
4135 evalcond[3]=new_r21;
4136 evalcond[4]=new_r20;
4137 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 )
4138 {
4139 bgotonextstatement=false;
4140 {
4141 IkReal j5array[1], cj5array[1], sj5array[1];
4142 bool j5valid[1]={false};
4143 _nj5 = 1;
4144 IkReal x305=((1.0)*sj3);
4145 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x305))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))))-1) <= IKFAST_SINCOS_THRESH )
4146  continue;
4147 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x305))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))));
4148 sj5array[0]=IKsin(j5array[0]);
4149 cj5array[0]=IKcos(j5array[0]);
4150 if( j5array[0] > IKPI )
4151 {
4152  j5array[0]-=IK2PI;
4153 }
4154 else if( j5array[0] < -IKPI )
4155 { j5array[0]+=IK2PI;
4156 }
4157 j5valid[0] = true;
4158 for(int ij5 = 0; ij5 < 1; ++ij5)
4159 {
4160 if( !j5valid[ij5] )
4161 {
4162  continue;
4163 }
4164 _ij5[0] = ij5; _ij5[1] = -1;
4165 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4166 {
4167 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4168 {
4169  j5valid[iij5]=false; _ij5[1] = iij5; break;
4170 }
4171 }
4172 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4173 {
4174 IkReal evalcond[8];
4175 IkReal x306=IKcos(j5);
4176 IkReal x307=IKsin(j5);
4177 IkReal x308=((1.0)*cj3);
4178 IkReal x309=(sj3*x306);
4179 IkReal x310=((1.0)*x307);
4180 IkReal x311=(x307*x308);
4181 evalcond[0]=(((new_r10*sj3))+x306+((cj3*new_r00)));
4182 evalcond[1]=(((new_r00*sj3))+x307+(((-1.0)*new_r10*x308)));
4183 evalcond[2]=(((new_r01*sj3))+x306+(((-1.0)*new_r11*x308)));
4184 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x310))+((cj3*new_r01)));
4185 evalcond[4]=(((sj3*x307))+((cj3*x306))+new_r00);
4186 evalcond[5]=(x309+(((-1.0)*x311))+new_r01);
4187 evalcond[6]=(x309+(((-1.0)*x311))+new_r10);
4188 evalcond[7]=((((-1.0)*sj3*x310))+(((-1.0)*x306*x308))+new_r11);
4189 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 )
4190 {
4191 continue;
4192 }
4193 }
4194 
4195 {
4196 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4197 vinfos[0].jointtype = 1;
4198 vinfos[0].foffset = j0;
4199 vinfos[0].indices[0] = _ij0[0];
4200 vinfos[0].indices[1] = _ij0[1];
4201 vinfos[0].maxsolutions = _nj0;
4202 vinfos[1].jointtype = 1;
4203 vinfos[1].foffset = j1;
4204 vinfos[1].indices[0] = _ij1[0];
4205 vinfos[1].indices[1] = _ij1[1];
4206 vinfos[1].maxsolutions = _nj1;
4207 vinfos[2].jointtype = 1;
4208 vinfos[2].foffset = j2;
4209 vinfos[2].indices[0] = _ij2[0];
4210 vinfos[2].indices[1] = _ij2[1];
4211 vinfos[2].maxsolutions = _nj2;
4212 vinfos[3].jointtype = 1;
4213 vinfos[3].foffset = j3;
4214 vinfos[3].indices[0] = _ij3[0];
4215 vinfos[3].indices[1] = _ij3[1];
4216 vinfos[3].maxsolutions = _nj3;
4217 vinfos[4].jointtype = 1;
4218 vinfos[4].foffset = j4;
4219 vinfos[4].indices[0] = _ij4[0];
4220 vinfos[4].indices[1] = _ij4[1];
4221 vinfos[4].maxsolutions = _nj4;
4222 vinfos[5].jointtype = 1;
4223 vinfos[5].foffset = j5;
4224 vinfos[5].indices[0] = _ij5[0];
4225 vinfos[5].indices[1] = _ij5[1];
4226 vinfos[5].maxsolutions = _nj5;
4227 std::vector<int> vfree(0);
4228 solutions.AddSolution(vinfos,vfree);
4229 }
4230 }
4231 }
4232 
4233 }
4234 } while(0);
4235 if( bgotonextstatement )
4236 {
4237 bool bgotonextstatement = true;
4238 do
4239 {
4240 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4241 evalcond[1]=new_r02;
4242 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4243 {
4244 bgotonextstatement=false;
4245 {
4246 IkReal j5array[1], cj5array[1], sj5array[1];
4247 bool j5valid[1]={false};
4248 _nj5 = 1;
4249 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 )
4250  continue;
4251 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4252 sj5array[0]=IKsin(j5array[0]);
4253 cj5array[0]=IKcos(j5array[0]);
4254 if( j5array[0] > IKPI )
4255 {
4256  j5array[0]-=IK2PI;
4257 }
4258 else if( j5array[0] < -IKPI )
4259 { j5array[0]+=IK2PI;
4260 }
4261 j5valid[0] = true;
4262 for(int ij5 = 0; ij5 < 1; ++ij5)
4263 {
4264 if( !j5valid[ij5] )
4265 {
4266  continue;
4267 }
4268 _ij5[0] = ij5; _ij5[1] = -1;
4269 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4270 {
4271 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4272 {
4273  j5valid[iij5]=false; _ij5[1] = iij5; break;
4274 }
4275 }
4276 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4277 {
4278 IkReal evalcond[8];
4279 IkReal x312=IKsin(j5);
4280 IkReal x313=IKcos(j5);
4281 IkReal x314=((1.0)*cj4);
4282 IkReal x315=((1.0)*sj4);
4283 evalcond[0]=(x312+new_r00);
4284 evalcond[1]=(x313+new_r01);
4285 evalcond[2]=(((sj4*x312))+new_r21);
4286 evalcond[3]=(((cj4*x312))+new_r11);
4287 evalcond[4]=((((-1.0)*x313*x315))+new_r20);
4288 evalcond[5]=((((-1.0)*x313*x314))+new_r10);
4289 evalcond[6]=((((-1.0)*new_r20*x315))+x313+(((-1.0)*new_r10*x314)));
4290 evalcond[7]=((((-1.0)*new_r21*x315))+(((-1.0)*new_r11*x314))+(((-1.0)*x312)));
4291 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 )
4292 {
4293 continue;
4294 }
4295 }
4296 
4297 {
4298 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4299 vinfos[0].jointtype = 1;
4300 vinfos[0].foffset = j0;
4301 vinfos[0].indices[0] = _ij0[0];
4302 vinfos[0].indices[1] = _ij0[1];
4303 vinfos[0].maxsolutions = _nj0;
4304 vinfos[1].jointtype = 1;
4305 vinfos[1].foffset = j1;
4306 vinfos[1].indices[0] = _ij1[0];
4307 vinfos[1].indices[1] = _ij1[1];
4308 vinfos[1].maxsolutions = _nj1;
4309 vinfos[2].jointtype = 1;
4310 vinfos[2].foffset = j2;
4311 vinfos[2].indices[0] = _ij2[0];
4312 vinfos[2].indices[1] = _ij2[1];
4313 vinfos[2].maxsolutions = _nj2;
4314 vinfos[3].jointtype = 1;
4315 vinfos[3].foffset = j3;
4316 vinfos[3].indices[0] = _ij3[0];
4317 vinfos[3].indices[1] = _ij3[1];
4318 vinfos[3].maxsolutions = _nj3;
4319 vinfos[4].jointtype = 1;
4320 vinfos[4].foffset = j4;
4321 vinfos[4].indices[0] = _ij4[0];
4322 vinfos[4].indices[1] = _ij4[1];
4323 vinfos[4].maxsolutions = _nj4;
4324 vinfos[5].jointtype = 1;
4325 vinfos[5].foffset = j5;
4326 vinfos[5].indices[0] = _ij5[0];
4327 vinfos[5].indices[1] = _ij5[1];
4328 vinfos[5].maxsolutions = _nj5;
4329 std::vector<int> vfree(0);
4330 solutions.AddSolution(vinfos,vfree);
4331 }
4332 }
4333 }
4334 
4335 }
4336 } while(0);
4337 if( bgotonextstatement )
4338 {
4339 bool bgotonextstatement = true;
4340 do
4341 {
4342 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4343 evalcond[1]=new_r02;
4344 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4345 {
4346 bgotonextstatement=false;
4347 {
4348 IkReal j5array[1], cj5array[1], sj5array[1];
4349 bool j5valid[1]={false};
4350 _nj5 = 1;
4351 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4352  continue;
4353 j5array[0]=IKatan2(new_r00, new_r01);
4354 sj5array[0]=IKsin(j5array[0]);
4355 cj5array[0]=IKcos(j5array[0]);
4356 if( j5array[0] > IKPI )
4357 {
4358  j5array[0]-=IK2PI;
4359 }
4360 else if( j5array[0] < -IKPI )
4361 { j5array[0]+=IK2PI;
4362 }
4363 j5valid[0] = true;
4364 for(int ij5 = 0; ij5 < 1; ++ij5)
4365 {
4366 if( !j5valid[ij5] )
4367 {
4368  continue;
4369 }
4370 _ij5[0] = ij5; _ij5[1] = -1;
4371 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4372 {
4373 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4374 {
4375  j5valid[iij5]=false; _ij5[1] = iij5; break;
4376 }
4377 }
4378 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4379 {
4380 IkReal evalcond[8];
4381 IkReal x316=IKsin(j5);
4382 IkReal x317=IKcos(j5);
4383 IkReal x318=((1.0)*sj4);
4384 IkReal x319=((1.0)*x317);
4385 evalcond[0]=(((sj4*x316))+new_r21);
4386 evalcond[1]=(x316+(((-1.0)*new_r00)));
4387 evalcond[2]=(x317+(((-1.0)*new_r01)));
4388 evalcond[3]=(new_r20+(((-1.0)*x317*x318)));
4389 evalcond[4]=(((cj4*x316))+(((-1.0)*new_r11)));
4390 evalcond[5]=((((-1.0)*cj4*x319))+(((-1.0)*new_r10)));
4391 evalcond[6]=((((-1.0)*new_r20*x318))+((cj4*new_r10))+x317);
4392 evalcond[7]=((((-1.0)*new_r21*x318))+((cj4*new_r11))+(((-1.0)*x316)));
4393 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 )
4394 {
4395 continue;
4396 }
4397 }
4398 
4399 {
4400 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4401 vinfos[0].jointtype = 1;
4402 vinfos[0].foffset = j0;
4403 vinfos[0].indices[0] = _ij0[0];
4404 vinfos[0].indices[1] = _ij0[1];
4405 vinfos[0].maxsolutions = _nj0;
4406 vinfos[1].jointtype = 1;
4407 vinfos[1].foffset = j1;
4408 vinfos[1].indices[0] = _ij1[0];
4409 vinfos[1].indices[1] = _ij1[1];
4410 vinfos[1].maxsolutions = _nj1;
4411 vinfos[2].jointtype = 1;
4412 vinfos[2].foffset = j2;
4413 vinfos[2].indices[0] = _ij2[0];
4414 vinfos[2].indices[1] = _ij2[1];
4415 vinfos[2].maxsolutions = _nj2;
4416 vinfos[3].jointtype = 1;
4417 vinfos[3].foffset = j3;
4418 vinfos[3].indices[0] = _ij3[0];
4419 vinfos[3].indices[1] = _ij3[1];
4420 vinfos[3].maxsolutions = _nj3;
4421 vinfos[4].jointtype = 1;
4422 vinfos[4].foffset = j4;
4423 vinfos[4].indices[0] = _ij4[0];
4424 vinfos[4].indices[1] = _ij4[1];
4425 vinfos[4].maxsolutions = _nj4;
4426 vinfos[5].jointtype = 1;
4427 vinfos[5].foffset = j5;
4428 vinfos[5].indices[0] = _ij5[0];
4429 vinfos[5].indices[1] = _ij5[1];
4430 vinfos[5].maxsolutions = _nj5;
4431 std::vector<int> vfree(0);
4432 solutions.AddSolution(vinfos,vfree);
4433 }
4434 }
4435 }
4436 
4437 }
4438 } while(0);
4439 if( bgotonextstatement )
4440 {
4441 bool bgotonextstatement = true;
4442 do
4443 {
4444 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4445 evalcond[1]=new_r12;
4446 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4447 {
4448 bgotonextstatement=false;
4449 {
4450 IkReal j5array[1], cj5array[1], sj5array[1];
4451 bool j5valid[1]={false};
4452 _nj5 = 1;
4453 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4454  continue;
4455 j5array[0]=IKatan2(new_r10, new_r11);
4456 sj5array[0]=IKsin(j5array[0]);
4457 cj5array[0]=IKcos(j5array[0]);
4458 if( j5array[0] > IKPI )
4459 {
4460  j5array[0]-=IK2PI;
4461 }
4462 else if( j5array[0] < -IKPI )
4463 { j5array[0]+=IK2PI;
4464 }
4465 j5valid[0] = true;
4466 for(int ij5 = 0; ij5 < 1; ++ij5)
4467 {
4468 if( !j5valid[ij5] )
4469 {
4470  continue;
4471 }
4472 _ij5[0] = ij5; _ij5[1] = -1;
4473 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4474 {
4475 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4476 {
4477  j5valid[iij5]=false; _ij5[1] = iij5; break;
4478 }
4479 }
4480 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4481 {
4482 IkReal evalcond[8];
4483 IkReal x320=IKcos(j5);
4484 IkReal x321=IKsin(j5);
4485 IkReal x322=((1.0)*cj4);
4486 IkReal x323=((1.0)*sj4);
4487 IkReal x324=((1.0)*x321);
4488 evalcond[0]=(new_r20+((new_r02*x320)));
4489 evalcond[1]=(x321+(((-1.0)*new_r10)));
4490 evalcond[2]=(x320+(((-1.0)*new_r11)));
4491 evalcond[3]=(((cj4*x321))+new_r01);
4492 evalcond[4]=((((-1.0)*new_r02*x324))+new_r21);
4493 evalcond[5]=((((-1.0)*x320*x322))+new_r00);
4494 evalcond[6]=(x320+(((-1.0)*new_r00*x322))+(((-1.0)*new_r20*x323)));
4495 evalcond[7]=((((-1.0)*x324))+(((-1.0)*new_r01*x322))+(((-1.0)*new_r21*x323)));
4496 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 )
4497 {
4498 continue;
4499 }
4500 }
4501 
4502 {
4503 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4504 vinfos[0].jointtype = 1;
4505 vinfos[0].foffset = j0;
4506 vinfos[0].indices[0] = _ij0[0];
4507 vinfos[0].indices[1] = _ij0[1];
4508 vinfos[0].maxsolutions = _nj0;
4509 vinfos[1].jointtype = 1;
4510 vinfos[1].foffset = j1;
4511 vinfos[1].indices[0] = _ij1[0];
4512 vinfos[1].indices[1] = _ij1[1];
4513 vinfos[1].maxsolutions = _nj1;
4514 vinfos[2].jointtype = 1;
4515 vinfos[2].foffset = j2;
4516 vinfos[2].indices[0] = _ij2[0];
4517 vinfos[2].indices[1] = _ij2[1];
4518 vinfos[2].maxsolutions = _nj2;
4519 vinfos[3].jointtype = 1;
4520 vinfos[3].foffset = j3;
4521 vinfos[3].indices[0] = _ij3[0];
4522 vinfos[3].indices[1] = _ij3[1];
4523 vinfos[3].maxsolutions = _nj3;
4524 vinfos[4].jointtype = 1;
4525 vinfos[4].foffset = j4;
4526 vinfos[4].indices[0] = _ij4[0];
4527 vinfos[4].indices[1] = _ij4[1];
4528 vinfos[4].maxsolutions = _nj4;
4529 vinfos[5].jointtype = 1;
4530 vinfos[5].foffset = j5;
4531 vinfos[5].indices[0] = _ij5[0];
4532 vinfos[5].indices[1] = _ij5[1];
4533 vinfos[5].maxsolutions = _nj5;
4534 std::vector<int> vfree(0);
4535 solutions.AddSolution(vinfos,vfree);
4536 }
4537 }
4538 }
4539 
4540 }
4541 } while(0);
4542 if( bgotonextstatement )
4543 {
4544 bool bgotonextstatement = true;
4545 do
4546 {
4547 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4548 evalcond[1]=new_r12;
4549 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4550 {
4551 bgotonextstatement=false;
4552 {
4553 IkReal j5array[1], cj5array[1], sj5array[1];
4554 bool j5valid[1]={false};
4555 _nj5 = 1;
4556 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 )
4557  continue;
4558 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4559 sj5array[0]=IKsin(j5array[0]);
4560 cj5array[0]=IKcos(j5array[0]);
4561 if( j5array[0] > IKPI )
4562 {
4563  j5array[0]-=IK2PI;
4564 }
4565 else if( j5array[0] < -IKPI )
4566 { j5array[0]+=IK2PI;
4567 }
4568 j5valid[0] = true;
4569 for(int ij5 = 0; ij5 < 1; ++ij5)
4570 {
4571 if( !j5valid[ij5] )
4572 {
4573  continue;
4574 }
4575 _ij5[0] = ij5; _ij5[1] = -1;
4576 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4577 {
4578 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4579 {
4580  j5valid[iij5]=false; _ij5[1] = iij5; break;
4581 }
4582 }
4583 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4584 {
4585 IkReal evalcond[8];
4586 IkReal x325=IKsin(j5);
4587 IkReal x326=IKcos(j5);
4588 IkReal x327=((1.0)*sj4);
4589 IkReal x328=((1.0)*x326);
4590 evalcond[0]=(x325+new_r10);
4591 evalcond[1]=(x326+new_r11);
4592 evalcond[2]=(new_r21+((new_r02*x325)));
4593 evalcond[3]=((((-1.0)*new_r02*x328))+new_r20);
4594 evalcond[4]=(((cj4*x325))+(((-1.0)*new_r01)));
4595 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x328)));
4596 evalcond[6]=(((cj4*new_r00))+x326+(((-1.0)*new_r20*x327)));
4597 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x325))+(((-1.0)*new_r21*x327)));
4598 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 )
4599 {
4600 continue;
4601 }
4602 }
4603 
4604 {
4605 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4606 vinfos[0].jointtype = 1;
4607 vinfos[0].foffset = j0;
4608 vinfos[0].indices[0] = _ij0[0];
4609 vinfos[0].indices[1] = _ij0[1];
4610 vinfos[0].maxsolutions = _nj0;
4611 vinfos[1].jointtype = 1;
4612 vinfos[1].foffset = j1;
4613 vinfos[1].indices[0] = _ij1[0];
4614 vinfos[1].indices[1] = _ij1[1];
4615 vinfos[1].maxsolutions = _nj1;
4616 vinfos[2].jointtype = 1;
4617 vinfos[2].foffset = j2;
4618 vinfos[2].indices[0] = _ij2[0];
4619 vinfos[2].indices[1] = _ij2[1];
4620 vinfos[2].maxsolutions = _nj2;
4621 vinfos[3].jointtype = 1;
4622 vinfos[3].foffset = j3;
4623 vinfos[3].indices[0] = _ij3[0];
4624 vinfos[3].indices[1] = _ij3[1];
4625 vinfos[3].maxsolutions = _nj3;
4626 vinfos[4].jointtype = 1;
4627 vinfos[4].foffset = j4;
4628 vinfos[4].indices[0] = _ij4[0];
4629 vinfos[4].indices[1] = _ij4[1];
4630 vinfos[4].maxsolutions = _nj4;
4631 vinfos[5].jointtype = 1;
4632 vinfos[5].foffset = j5;
4633 vinfos[5].indices[0] = _ij5[0];
4634 vinfos[5].indices[1] = _ij5[1];
4635 vinfos[5].maxsolutions = _nj5;
4636 std::vector<int> vfree(0);
4637 solutions.AddSolution(vinfos,vfree);
4638 }
4639 }
4640 }
4641 
4642 }
4643 } while(0);
4644 if( bgotonextstatement )
4645 {
4646 bool bgotonextstatement = true;
4647 do
4648 {
4649 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4650 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4651 {
4652 bgotonextstatement=false;
4653 {
4654 IkReal j5eval[1];
4655 new_r21=0;
4656 new_r20=0;
4657 new_r02=0;
4658 new_r12=0;
4659 j5eval[0]=1.0;
4660 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4661 {
4662 continue; // no branches [j5]
4663 
4664 } else
4665 {
4666 IkReal op[2+1], zeror[2];
4667 int numroots;
4668 op[0]=-1.0;
4669 op[1]=0;
4670 op[2]=1.0;
4671 polyroots2(op,zeror,numroots);
4672 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4673 int numsolutions = 0;
4674 for(int ij5 = 0; ij5 < numroots; ++ij5)
4675 {
4676 IkReal htj5 = zeror[ij5];
4677 tempj5array[0]=((2.0)*(atan(htj5)));
4678 for(int kj5 = 0; kj5 < 1; ++kj5)
4679 {
4680 j5array[numsolutions] = tempj5array[kj5];
4681 if( j5array[numsolutions] > IKPI )
4682 {
4683  j5array[numsolutions]-=IK2PI;
4684 }
4685 else if( j5array[numsolutions] < -IKPI )
4686 {
4687  j5array[numsolutions]+=IK2PI;
4688 }
4689 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4690 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4691 numsolutions++;
4692 }
4693 }
4694 bool j5valid[2]={true,true};
4695 _nj5 = 2;
4696 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4697  {
4698 if( !j5valid[ij5] )
4699 {
4700  continue;
4701 }
4702  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4703 htj5 = IKtan(j5/2);
4704 
4705 _ij5[0] = ij5; _ij5[1] = -1;
4706 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4707 {
4708 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4709 {
4710  j5valid[iij5]=false; _ij5[1] = iij5; break;
4711 }
4712 }
4713 {
4714 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4715 vinfos[0].jointtype = 1;
4716 vinfos[0].foffset = j0;
4717 vinfos[0].indices[0] = _ij0[0];
4718 vinfos[0].indices[1] = _ij0[1];
4719 vinfos[0].maxsolutions = _nj0;
4720 vinfos[1].jointtype = 1;
4721 vinfos[1].foffset = j1;
4722 vinfos[1].indices[0] = _ij1[0];
4723 vinfos[1].indices[1] = _ij1[1];
4724 vinfos[1].maxsolutions = _nj1;
4725 vinfos[2].jointtype = 1;
4726 vinfos[2].foffset = j2;
4727 vinfos[2].indices[0] = _ij2[0];
4728 vinfos[2].indices[1] = _ij2[1];
4729 vinfos[2].maxsolutions = _nj2;
4730 vinfos[3].jointtype = 1;
4731 vinfos[3].foffset = j3;
4732 vinfos[3].indices[0] = _ij3[0];
4733 vinfos[3].indices[1] = _ij3[1];
4734 vinfos[3].maxsolutions = _nj3;
4735 vinfos[4].jointtype = 1;
4736 vinfos[4].foffset = j4;
4737 vinfos[4].indices[0] = _ij4[0];
4738 vinfos[4].indices[1] = _ij4[1];
4739 vinfos[4].maxsolutions = _nj4;
4740 vinfos[5].jointtype = 1;
4741 vinfos[5].foffset = j5;
4742 vinfos[5].indices[0] = _ij5[0];
4743 vinfos[5].indices[1] = _ij5[1];
4744 vinfos[5].maxsolutions = _nj5;
4745 std::vector<int> vfree(0);
4746 solutions.AddSolution(vinfos,vfree);
4747 }
4748  }
4749 
4750 }
4751 
4752 }
4753 
4754 }
4755 } while(0);
4756 if( bgotonextstatement )
4757 {
4758 bool bgotonextstatement = true;
4759 do
4760 {
4761 if( 1 )
4762 {
4763 bgotonextstatement=false;
4764 continue; // branch miss [j5]
4765 
4766 }
4767 } while(0);
4768 if( bgotonextstatement )
4769 {
4770 }
4771 }
4772 }
4773 }
4774 }
4775 }
4776 }
4777 }
4778 }
4779 
4780 } else
4781 {
4782 {
4783 IkReal j5array[1], cj5array[1], sj5array[1];
4784 bool j5valid[1]={false};
4785 _nj5 = 1;
4786 CheckValue<IkReal> x330=IKPowWithIntegerCheck(sj4,-1);
4787 if(!x330.valid){
4788 continue;
4789 }
4790 IkReal x329=x330.value;
4791 CheckValue<IkReal> x331=IKPowWithIntegerCheck(cj3,-1);
4792 if(!x331.valid){
4793 continue;
4794 }
4795 if( IKabs(((-1.0)*new_r21*x329)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x329))+IKsqr((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4796  continue;
4797 j5array[0]=IKatan2(((-1.0)*new_r21*x329), (x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4798 sj5array[0]=IKsin(j5array[0]);
4799 cj5array[0]=IKcos(j5array[0]);
4800 if( j5array[0] > IKPI )
4801 {
4802  j5array[0]-=IK2PI;
4803 }
4804 else if( j5array[0] < -IKPI )
4805 { j5array[0]+=IK2PI;
4806 }
4807 j5valid[0] = true;
4808 for(int ij5 = 0; ij5 < 1; ++ij5)
4809 {
4810 if( !j5valid[ij5] )
4811 {
4812  continue;
4813 }
4814 _ij5[0] = ij5; _ij5[1] = -1;
4815 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4816 {
4817 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4818 {
4819  j5valid[iij5]=false; _ij5[1] = iij5; break;
4820 }
4821 }
4822 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4823 {
4824 IkReal evalcond[12];
4825 IkReal x332=IKsin(j5);
4826 IkReal x333=IKcos(j5);
4827 IkReal x334=(cj3*new_r00);
4828 IkReal x335=(cj3*cj4);
4829 IkReal x336=((1.0)*cj3);
4830 IkReal x337=(new_r11*sj3);
4831 IkReal x338=((1.0)*cj4);
4832 IkReal x339=(new_r10*sj3);
4833 IkReal x340=((1.0)*sj4);
4834 IkReal x341=(sj3*x332);
4835 IkReal x342=((1.0)*x333);
4836 IkReal x343=(sj3*x333);
4837 evalcond[0]=(((sj4*x332))+new_r21);
4838 evalcond[1]=(new_r20+(((-1.0)*x333*x340)));
4839 evalcond[2]=((((-1.0)*new_r10*x336))+((new_r00*sj3))+x332);
4840 evalcond[3]=((((-1.0)*new_r11*x336))+((new_r01*sj3))+x333);
4841 evalcond[4]=(x337+((cj4*x332))+((cj3*new_r01)));
4842 evalcond[5]=(((x332*x335))+x343+new_r01);
4843 evalcond[6]=((((-1.0)*x333*x338))+x339+x334);
4844 evalcond[7]=((((-1.0)*x335*x342))+x341+new_r00);
4845 evalcond[8]=((((-1.0)*x333*x336))+((cj4*x341))+new_r11);
4846 evalcond[9]=((((-1.0)*x332*x336))+new_r10+(((-1.0)*x338*x343)));
4847 evalcond[10]=((((-1.0)*x338*x339))+x333+(((-1.0)*new_r20*x340))+(((-1.0)*x334*x338)));
4848 evalcond[11]=((((-1.0)*x337*x338))+(((-1.0)*new_r01*x335))+(((-1.0)*x332))+(((-1.0)*new_r21*x340)));
4849 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 )
4850 {
4851 continue;
4852 }
4853 }
4854 
4855 {
4856 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4857 vinfos[0].jointtype = 1;
4858 vinfos[0].foffset = j0;
4859 vinfos[0].indices[0] = _ij0[0];
4860 vinfos[0].indices[1] = _ij0[1];
4861 vinfos[0].maxsolutions = _nj0;
4862 vinfos[1].jointtype = 1;
4863 vinfos[1].foffset = j1;
4864 vinfos[1].indices[0] = _ij1[0];
4865 vinfos[1].indices[1] = _ij1[1];
4866 vinfos[1].maxsolutions = _nj1;
4867 vinfos[2].jointtype = 1;
4868 vinfos[2].foffset = j2;
4869 vinfos[2].indices[0] = _ij2[0];
4870 vinfos[2].indices[1] = _ij2[1];
4871 vinfos[2].maxsolutions = _nj2;
4872 vinfos[3].jointtype = 1;
4873 vinfos[3].foffset = j3;
4874 vinfos[3].indices[0] = _ij3[0];
4875 vinfos[3].indices[1] = _ij3[1];
4876 vinfos[3].maxsolutions = _nj3;
4877 vinfos[4].jointtype = 1;
4878 vinfos[4].foffset = j4;
4879 vinfos[4].indices[0] = _ij4[0];
4880 vinfos[4].indices[1] = _ij4[1];
4881 vinfos[4].maxsolutions = _nj4;
4882 vinfos[5].jointtype = 1;
4883 vinfos[5].foffset = j5;
4884 vinfos[5].indices[0] = _ij5[0];
4885 vinfos[5].indices[1] = _ij5[1];
4886 vinfos[5].maxsolutions = _nj5;
4887 std::vector<int> vfree(0);
4888 solutions.AddSolution(vinfos,vfree);
4889 }
4890 }
4891 }
4892 
4893 }
4894 
4895 }
4896 
4897 } else
4898 {
4899 {
4900 IkReal j5array[1], cj5array[1], sj5array[1];
4901 bool j5valid[1]={false};
4902 _nj5 = 1;
4903 CheckValue<IkReal> x345=IKPowWithIntegerCheck(sj4,-1);
4904 if(!x345.valid){
4905 continue;
4906 }
4907 IkReal x344=x345.value;
4908 CheckValue<IkReal> x346=IKPowWithIntegerCheck(sj3,-1);
4909 if(!x346.valid){
4910 continue;
4911 }
4912 if( IKabs(((-1.0)*new_r21*x344)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x344))+IKsqr((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4913  continue;
4914 j5array[0]=IKatan2(((-1.0)*new_r21*x344), (x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4915 sj5array[0]=IKsin(j5array[0]);
4916 cj5array[0]=IKcos(j5array[0]);
4917 if( j5array[0] > IKPI )
4918 {
4919  j5array[0]-=IK2PI;
4920 }
4921 else if( j5array[0] < -IKPI )
4922 { j5array[0]+=IK2PI;
4923 }
4924 j5valid[0] = true;
4925 for(int ij5 = 0; ij5 < 1; ++ij5)
4926 {
4927 if( !j5valid[ij5] )
4928 {
4929  continue;
4930 }
4931 _ij5[0] = ij5; _ij5[1] = -1;
4932 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4933 {
4934 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4935 {
4936  j5valid[iij5]=false; _ij5[1] = iij5; break;
4937 }
4938 }
4939 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4940 {
4941 IkReal evalcond[12];
4942 IkReal x347=IKsin(j5);
4943 IkReal x348=IKcos(j5);
4944 IkReal x349=(cj3*new_r00);
4945 IkReal x350=(cj3*cj4);
4946 IkReal x351=((1.0)*cj3);
4947 IkReal x352=(new_r11*sj3);
4948 IkReal x353=((1.0)*cj4);
4949 IkReal x354=(new_r10*sj3);
4950 IkReal x355=((1.0)*sj4);
4951 IkReal x356=(sj3*x347);
4952 IkReal x357=((1.0)*x348);
4953 IkReal x358=(sj3*x348);
4954 evalcond[0]=(((sj4*x347))+new_r21);
4955 evalcond[1]=((((-1.0)*x348*x355))+new_r20);
4956 evalcond[2]=(((new_r00*sj3))+x347+(((-1.0)*new_r10*x351)));
4957 evalcond[3]=(((new_r01*sj3))+x348+(((-1.0)*new_r11*x351)));
4958 evalcond[4]=(x352+((cj4*x347))+((cj3*new_r01)));
4959 evalcond[5]=(x358+((x347*x350))+new_r01);
4960 evalcond[6]=((((-1.0)*x348*x353))+x354+x349);
4961 evalcond[7]=(x356+new_r00+(((-1.0)*x350*x357)));
4962 evalcond[8]=((((-1.0)*x348*x351))+((cj4*x356))+new_r11);
4963 evalcond[9]=((((-1.0)*x347*x351))+(((-1.0)*x353*x358))+new_r10);
4964 evalcond[10]=((((-1.0)*x349*x353))+x348+(((-1.0)*x353*x354))+(((-1.0)*new_r20*x355)));
4965 evalcond[11]=((((-1.0)*new_r01*x350))+(((-1.0)*new_r21*x355))+(((-1.0)*x352*x353))+(((-1.0)*x347)));
4966 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 )
4967 {
4968 continue;
4969 }
4970 }
4971 
4972 {
4973 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4974 vinfos[0].jointtype = 1;
4975 vinfos[0].foffset = j0;
4976 vinfos[0].indices[0] = _ij0[0];
4977 vinfos[0].indices[1] = _ij0[1];
4978 vinfos[0].maxsolutions = _nj0;
4979 vinfos[1].jointtype = 1;
4980 vinfos[1].foffset = j1;
4981 vinfos[1].indices[0] = _ij1[0];
4982 vinfos[1].indices[1] = _ij1[1];
4983 vinfos[1].maxsolutions = _nj1;
4984 vinfos[2].jointtype = 1;
4985 vinfos[2].foffset = j2;
4986 vinfos[2].indices[0] = _ij2[0];
4987 vinfos[2].indices[1] = _ij2[1];
4988 vinfos[2].maxsolutions = _nj2;
4989 vinfos[3].jointtype = 1;
4990 vinfos[3].foffset = j3;
4991 vinfos[3].indices[0] = _ij3[0];
4992 vinfos[3].indices[1] = _ij3[1];
4993 vinfos[3].maxsolutions = _nj3;
4994 vinfos[4].jointtype = 1;
4995 vinfos[4].foffset = j4;
4996 vinfos[4].indices[0] = _ij4[0];
4997 vinfos[4].indices[1] = _ij4[1];
4998 vinfos[4].maxsolutions = _nj4;
4999 vinfos[5].jointtype = 1;
5000 vinfos[5].foffset = j5;
5001 vinfos[5].indices[0] = _ij5[0];
5002 vinfos[5].indices[1] = _ij5[1];
5003 vinfos[5].maxsolutions = _nj5;
5004 std::vector<int> vfree(0);
5005 solutions.AddSolution(vinfos,vfree);
5006 }
5007 }
5008 }
5009 
5010 }
5011 
5012 }
5013 
5014 } else
5015 {
5016 {
5017 IkReal j5array[1], cj5array[1], sj5array[1];
5018 bool j5valid[1]={false};
5019 _nj5 = 1;
5020 CheckValue<IkReal> x359 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5021 if(!x359.valid){
5022 continue;
5023 }
5024 CheckValue<IkReal> x360=IKPowWithIntegerCheck(IKsign(sj4),-1);
5025 if(!x360.valid){
5026 continue;
5027 }
5028 j5array[0]=((-1.5707963267949)+(x359.value)+(((1.5707963267949)*(x360.value))));
5029 sj5array[0]=IKsin(j5array[0]);
5030 cj5array[0]=IKcos(j5array[0]);
5031 if( j5array[0] > IKPI )
5032 {
5033  j5array[0]-=IK2PI;
5034 }
5035 else if( j5array[0] < -IKPI )
5036 { j5array[0]+=IK2PI;
5037 }
5038 j5valid[0] = true;
5039 for(int ij5 = 0; ij5 < 1; ++ij5)
5040 {
5041 if( !j5valid[ij5] )
5042 {
5043  continue;
5044 }
5045 _ij5[0] = ij5; _ij5[1] = -1;
5046 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5047 {
5048 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5049 {
5050  j5valid[iij5]=false; _ij5[1] = iij5; break;
5051 }
5052 }
5053 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5054 {
5055 IkReal evalcond[12];
5056 IkReal x361=IKsin(j5);
5057 IkReal x362=IKcos(j5);
5058 IkReal x363=(cj3*new_r00);
5059 IkReal x364=(cj3*cj4);
5060 IkReal x365=((1.0)*cj3);
5061 IkReal x366=(new_r11*sj3);
5062 IkReal x367=((1.0)*cj4);
5063 IkReal x368=(new_r10*sj3);
5064 IkReal x369=((1.0)*sj4);
5065 IkReal x370=(sj3*x361);
5066 IkReal x371=((1.0)*x362);
5067 IkReal x372=(sj3*x362);
5068 evalcond[0]=(((sj4*x361))+new_r21);
5069 evalcond[1]=((((-1.0)*x362*x369))+new_r20);
5070 evalcond[2]=(((new_r00*sj3))+x361+(((-1.0)*new_r10*x365)));
5071 evalcond[3]=(((new_r01*sj3))+x362+(((-1.0)*new_r11*x365)));
5072 evalcond[4]=(((cj4*x361))+x366+((cj3*new_r01)));
5073 evalcond[5]=(((x361*x364))+x372+new_r01);
5074 evalcond[6]=((((-1.0)*x362*x367))+x368+x363);
5075 evalcond[7]=(x370+new_r00+(((-1.0)*x364*x371)));
5076 evalcond[8]=((((-1.0)*x362*x365))+((cj4*x370))+new_r11);
5077 evalcond[9]=((((-1.0)*x361*x365))+(((-1.0)*x367*x372))+new_r10);
5078 evalcond[10]=((((-1.0)*x363*x367))+(((-1.0)*new_r20*x369))+(((-1.0)*x367*x368))+x362);
5079 evalcond[11]=((((-1.0)*x361))+(((-1.0)*x366*x367))+(((-1.0)*new_r01*x364))+(((-1.0)*new_r21*x369)));
5080 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 )
5081 {
5082 continue;
5083 }
5084 }
5085 
5086 {
5087 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5088 vinfos[0].jointtype = 1;
5089 vinfos[0].foffset = j0;
5090 vinfos[0].indices[0] = _ij0[0];
5091 vinfos[0].indices[1] = _ij0[1];
5092 vinfos[0].maxsolutions = _nj0;
5093 vinfos[1].jointtype = 1;
5094 vinfos[1].foffset = j1;
5095 vinfos[1].indices[0] = _ij1[0];
5096 vinfos[1].indices[1] = _ij1[1];
5097 vinfos[1].maxsolutions = _nj1;
5098 vinfos[2].jointtype = 1;
5099 vinfos[2].foffset = j2;
5100 vinfos[2].indices[0] = _ij2[0];
5101 vinfos[2].indices[1] = _ij2[1];
5102 vinfos[2].maxsolutions = _nj2;
5103 vinfos[3].jointtype = 1;
5104 vinfos[3].foffset = j3;
5105 vinfos[3].indices[0] = _ij3[0];
5106 vinfos[3].indices[1] = _ij3[1];
5107 vinfos[3].maxsolutions = _nj3;
5108 vinfos[4].jointtype = 1;
5109 vinfos[4].foffset = j4;
5110 vinfos[4].indices[0] = _ij4[0];
5111 vinfos[4].indices[1] = _ij4[1];
5112 vinfos[4].maxsolutions = _nj4;
5113 vinfos[5].jointtype = 1;
5114 vinfos[5].foffset = j5;
5115 vinfos[5].indices[0] = _ij5[0];
5116 vinfos[5].indices[1] = _ij5[1];
5117 vinfos[5].maxsolutions = _nj5;
5118 std::vector<int> vfree(0);
5119 solutions.AddSolution(vinfos,vfree);
5120 }
5121 }
5122 }
5123 
5124 }
5125 
5126 }
5127 }
5128 }
5129 
5130 }
5131 
5132 }
5133 
5134 } else
5135 {
5136 {
5137 IkReal j5array[1], cj5array[1], sj5array[1];
5138 bool j5valid[1]={false};
5139 _nj5 = 1;
5140 CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5141 if(!x373.valid){
5142 continue;
5143 }
5144 CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5145 if(!x374.valid){
5146 continue;
5147 }
5148 j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5149 sj5array[0]=IKsin(j5array[0]);
5150 cj5array[0]=IKcos(j5array[0]);
5151 if( j5array[0] > IKPI )
5152 {
5153  j5array[0]-=IK2PI;
5154 }
5155 else if( j5array[0] < -IKPI )
5156 { j5array[0]+=IK2PI;
5157 }
5158 j5valid[0] = true;
5159 for(int ij5 = 0; ij5 < 1; ++ij5)
5160 {
5161 if( !j5valid[ij5] )
5162 {
5163  continue;
5164 }
5165 _ij5[0] = ij5; _ij5[1] = -1;
5166 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5167 {
5168 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5169 {
5170  j5valid[iij5]=false; _ij5[1] = iij5; break;
5171 }
5172 }
5173 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5174 {
5175 IkReal evalcond[2];
5176 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5177 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5178 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5179 {
5180 continue;
5181 }
5182 }
5183 
5184 {
5185 IkReal j3eval[3];
5186 j3eval[0]=sj4;
5187 j3eval[1]=IKsign(sj4);
5188 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5189 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5190 {
5191 {
5192 IkReal j3eval[2];
5193 j3eval[0]=new_r12;
5194 j3eval[1]=sj4;
5195 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5196 {
5197 {
5198 IkReal evalcond[5];
5199 bool bgotonextstatement = true;
5200 do
5201 {
5202 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5203 evalcond[1]=new_r02;
5204 evalcond[2]=new_r12;
5205 evalcond[3]=new_r21;
5206 evalcond[4]=new_r20;
5207 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 )
5208 {
5209 bgotonextstatement=false;
5210 {
5211 IkReal j3eval[3];
5212 sj4=0;
5213 cj4=1.0;
5214 j4=0;
5215 IkReal x375=((1.0)*new_r11);
5216 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5217 j3eval[0]=x376;
5218 j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5219 j3eval[2]=IKsign(x376);
5220 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5221 {
5222 {
5223 IkReal j3eval[3];
5224 sj4=0;
5225 cj4=1.0;
5226 j4=0;
5227 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5228 j3eval[0]=x377;
5229 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5230 j3eval[2]=IKsign(x377);
5231 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5232 {
5233 {
5234 IkReal j3eval[3];
5235 sj4=0;
5236 cj4=1.0;
5237 j4=0;
5238 IkReal x378=((1.0)*new_r11);
5239 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5240 j3eval[0]=x379;
5241 j3eval[1]=IKsign(x379);
5242 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5243 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5244 {
5245 {
5246 IkReal evalcond[1];
5247 bool bgotonextstatement = true;
5248 do
5249 {
5250 IkReal x380=((-1.0)*new_r01);
5251 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5252 if(IKabs(x382)==0){
5253 continue;
5254 }
5255 IkReal x381=pow(x382,-0.5);
5256 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5257 if(!x383.valid){
5258 continue;
5259 }
5260 IkReal gconst0=((-1.0)*(x383.value));
5261 IkReal gconst1=(new_r11*x381);
5262 IkReal gconst2=(x380*x381);
5263 CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5264 if(!x384.valid){
5265 continue;
5266 }
5267 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5268 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5269 {
5270 bgotonextstatement=false;
5271 {
5272 IkReal j3eval[3];
5273 IkReal x385=((-1.0)*new_r01);
5274 CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5275 if(!x388.valid){
5276 continue;
5277 }
5278 IkReal x386=((-1.0)*(x388.value));
5279 IkReal x387=x381;
5280 sj4=0;
5281 cj4=1.0;
5282 j4=0;
5283 sj5=gconst1;
5284 cj5=gconst2;
5285 j5=x386;
5286 IkReal gconst0=x386;
5287 IkReal gconst1=(new_r11*x387);
5288 IkReal gconst2=(x385*x387);
5289 IkReal x389=new_r11*new_r11;
5290 IkReal x390=(new_r10*new_r11);
5291 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5292 IkReal x392=x381;
5293 IkReal x393=(new_r11*x392);
5294 j3eval[0]=x391;
5295 j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5296 j3eval[2]=IKsign(x391);
5297 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5298 {
5299 {
5300 IkReal j3eval[1];
5301 IkReal x394=((-1.0)*new_r01);
5302 CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5303 if(!x397.valid){
5304 continue;
5305 }
5306 IkReal x395=((-1.0)*(x397.value));
5307 IkReal x396=x381;
5308 sj4=0;
5309 cj4=1.0;
5310 j4=0;
5311 sj5=gconst1;
5312 cj5=gconst2;
5313 j5=x395;
5314 IkReal gconst0=x395;
5315 IkReal gconst1=(new_r11*x396);
5316 IkReal gconst2=(x394*x396);
5317 IkReal x398=new_r11*new_r11;
5318 CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5319 if(!x401.valid){
5320 continue;
5321 }
5322 IkReal x399=x401.value;
5323 IkReal x400=(x398*x399);
5324 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))))));
5325 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5326 {
5327 {
5328 IkReal j3eval[1];
5329 IkReal x402=((-1.0)*new_r01);
5330 CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5331 if(!x405.valid){
5332 continue;
5333 }
5334 IkReal x403=((-1.0)*(x405.value));
5335 IkReal x404=x381;
5336 sj4=0;
5337 cj4=1.0;
5338 j4=0;
5339 sj5=gconst1;
5340 cj5=gconst2;
5341 j5=x403;
5342 IkReal gconst0=x403;
5343 IkReal gconst1=(new_r11*x404);
5344 IkReal gconst2=(x402*x404);
5345 IkReal x406=new_r01*new_r01;
5346 IkReal x407=new_r11*new_r11;
5347 CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5348 if(!x414.valid){
5349 continue;
5350 }
5351 IkReal x408=x414.value;
5352 IkReal x409=(x407*x408);
5353 CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5354 if(!x415.valid){
5355 continue;
5356 }
5357 IkReal x410=x415.value;
5358 IkReal x411=((1.0)*x410);
5359 IkReal x412=(new_r11*x411);
5360 IkReal x413=(new_r01*x411);
5361 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))))));
5362 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5363 {
5364 {
5365 IkReal evalcond[3];
5366 bool bgotonextstatement = true;
5367 do
5368 {
5369 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5370 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5371 {
5372 bgotonextstatement=false;
5373 {
5374 IkReal j3array[2], cj3array[2], sj3array[2];
5375 bool j3valid[2]={false};
5376 _nj3 = 2;
5377 CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst2,-1);
5378 if(!x416.valid){
5379 continue;
5380 }
5381 sj3array[0]=(new_r10*(x416.value));
5382 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5383 {
5384  j3valid[0] = j3valid[1] = true;
5385  j3array[0] = IKasin(sj3array[0]);
5386  cj3array[0] = IKcos(j3array[0]);
5387  sj3array[1] = sj3array[0];
5388  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5389  cj3array[1] = -cj3array[0];
5390 }
5391 else if( isnan(sj3array[0]) )
5392 {
5393  // probably any value will work
5394  j3valid[0] = true;
5395  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5396 }
5397 for(int ij3 = 0; ij3 < 2; ++ij3)
5398 {
5399 if( !j3valid[ij3] )
5400 {
5401  continue;
5402 }
5403 _ij3[0] = ij3; _ij3[1] = -1;
5404 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5405 {
5406 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5407 {
5408  j3valid[iij3]=false; _ij3[1] = iij3; break;
5409 }
5410 }
5411 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5412 {
5413 IkReal evalcond[6];
5414 IkReal x417=IKcos(j3);
5415 IkReal x418=IKsin(j3);
5416 IkReal x419=((-1.0)*x417);
5417 evalcond[0]=(new_r01*x417);
5418 evalcond[1]=(new_r10*x419);
5419 evalcond[2]=(gconst2*x419);
5420 evalcond[3]=(gconst2+((new_r01*x418)));
5421 evalcond[4]=(((gconst2*x418))+new_r01);
5422 evalcond[5]=((((-1.0)*gconst2))+((new_r10*x418)));
5423 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 )
5424 {
5425 continue;
5426 }
5427 }
5428 
5429 {
5430 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5431 vinfos[0].jointtype = 1;
5432 vinfos[0].foffset = j0;
5433 vinfos[0].indices[0] = _ij0[0];
5434 vinfos[0].indices[1] = _ij0[1];
5435 vinfos[0].maxsolutions = _nj0;
5436 vinfos[1].jointtype = 1;
5437 vinfos[1].foffset = j1;
5438 vinfos[1].indices[0] = _ij1[0];
5439 vinfos[1].indices[1] = _ij1[1];
5440 vinfos[1].maxsolutions = _nj1;
5441 vinfos[2].jointtype = 1;
5442 vinfos[2].foffset = j2;
5443 vinfos[2].indices[0] = _ij2[0];
5444 vinfos[2].indices[1] = _ij2[1];
5445 vinfos[2].maxsolutions = _nj2;
5446 vinfos[3].jointtype = 1;
5447 vinfos[3].foffset = j3;
5448 vinfos[3].indices[0] = _ij3[0];
5449 vinfos[3].indices[1] = _ij3[1];
5450 vinfos[3].maxsolutions = _nj3;
5451 vinfos[4].jointtype = 1;
5452 vinfos[4].foffset = j4;
5453 vinfos[4].indices[0] = _ij4[0];
5454 vinfos[4].indices[1] = _ij4[1];
5455 vinfos[4].maxsolutions = _nj4;
5456 vinfos[5].jointtype = 1;
5457 vinfos[5].foffset = j5;
5458 vinfos[5].indices[0] = _ij5[0];
5459 vinfos[5].indices[1] = _ij5[1];
5460 vinfos[5].maxsolutions = _nj5;
5461 std::vector<int> vfree(0);
5462 solutions.AddSolution(vinfos,vfree);
5463 }
5464 }
5465 }
5466 
5467 }
5468 } while(0);
5469 if( bgotonextstatement )
5470 {
5471 bool bgotonextstatement = true;
5472 do
5473 {
5474 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5475 evalcond[1]=gconst1;
5476 evalcond[2]=gconst2;
5477 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
5478 {
5479 bgotonextstatement=false;
5480 {
5481 IkReal j3eval[3];
5482 IkReal x420=((-1.0)*new_r01);
5483 CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5484 if(!x422.valid){
5485 continue;
5486 }
5487 IkReal x421=((-1.0)*(x422.value));
5488 sj4=0;
5489 cj4=1.0;
5490 j4=0;
5491 sj5=gconst1;
5492 cj5=gconst2;
5493 j5=x421;
5494 new_r00=0;
5495 new_r10=0;
5496 new_r21=0;
5497 new_r22=0;
5498 IkReal gconst0=x421;
5499 IkReal gconst1=new_r11;
5500 IkReal gconst2=x420;
5501 j3eval[0]=-1.0;
5502 j3eval[1]=-1.0;
5503 j3eval[2]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5504 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5505 {
5506 {
5507 IkReal j3eval[3];
5508 IkReal x423=((-1.0)*new_r01);
5509 CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5510 if(!x425.valid){
5511 continue;
5512 }
5513 IkReal x424=((-1.0)*(x425.value));
5514 sj4=0;
5515 cj4=1.0;
5516 j4=0;
5517 sj5=gconst1;
5518 cj5=gconst2;
5519 j5=x424;
5520 new_r00=0;
5521 new_r10=0;
5522 new_r21=0;
5523 new_r22=0;
5524 IkReal gconst0=x424;
5525 IkReal gconst1=new_r11;
5526 IkReal gconst2=x423;
5527 j3eval[0]=-1.0;
5528 j3eval[1]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5529 j3eval[2]=-1.0;
5530 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5531 {
5532 {
5533 IkReal j3eval[3];
5534 IkReal x426=((-1.0)*new_r01);
5535 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5536 if(!x428.valid){
5537 continue;
5538 }
5539 IkReal x427=((-1.0)*(x428.value));
5540 sj4=0;
5541 cj4=1.0;
5542 j4=0;
5543 sj5=gconst1;
5544 cj5=gconst2;
5545 j5=x427;
5546 new_r00=0;
5547 new_r10=0;
5548 new_r21=0;
5549 new_r22=0;
5550 IkReal gconst0=x427;
5551 IkReal gconst1=new_r11;
5552 IkReal gconst2=x426;
5553 j3eval[0]=1.0;
5554 j3eval[1]=1.0;
5555 j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5556 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5557 {
5558 continue; // 3 cases reached
5559 
5560 } else
5561 {
5562 {
5563 IkReal j3array[1], cj3array[1], sj3array[1];
5564 bool j3valid[1]={false};
5565 _nj3 = 1;
5566 IkReal x429=((1.0)*new_r01);
5567 CheckValue<IkReal> x430 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x429))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
5568 if(!x430.valid){
5569 continue;
5570 }
5571 CheckValue<IkReal> x431=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x429)))),-1);
5572 if(!x431.valid){
5573 continue;
5574 }
5575 j3array[0]=((-1.5707963267949)+(x430.value)+(((1.5707963267949)*(x431.value))));
5576 sj3array[0]=IKsin(j3array[0]);
5577 cj3array[0]=IKcos(j3array[0]);
5578 if( j3array[0] > IKPI )
5579 {
5580  j3array[0]-=IK2PI;
5581 }
5582 else if( j3array[0] < -IKPI )
5583 { j3array[0]+=IK2PI;
5584 }
5585 j3valid[0] = true;
5586 for(int ij3 = 0; ij3 < 1; ++ij3)
5587 {
5588 if( !j3valid[ij3] )
5589 {
5590  continue;
5591 }
5592 _ij3[0] = ij3; _ij3[1] = -1;
5593 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5594 {
5595 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5596 {
5597  j3valid[iij3]=false; _ij3[1] = iij3; break;
5598 }
5599 }
5600 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5601 {
5602 IkReal evalcond[6];
5603 IkReal x432=IKcos(j3);
5604 IkReal x433=IKsin(j3);
5605 IkReal x434=(gconst1*x433);
5606 IkReal x435=(gconst2*x433);
5607 IkReal x436=((1.0)*x432);
5608 IkReal x437=(gconst2*x436);
5609 evalcond[0]=(((new_r01*x432))+gconst1+((new_r11*x433)));
5610 evalcond[1]=(((gconst1*x432))+x435+new_r01);
5611 evalcond[2]=((((-1.0)*x437))+x434);
5612 evalcond[3]=(((new_r01*x433))+gconst2+(((-1.0)*new_r11*x436)));
5613 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5614 evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst1*x436)));
5615 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 )
5616 {
5617 continue;
5618 }
5619 }
5620 
5621 {
5622 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5623 vinfos[0].jointtype = 1;
5624 vinfos[0].foffset = j0;
5625 vinfos[0].indices[0] = _ij0[0];
5626 vinfos[0].indices[1] = _ij0[1];
5627 vinfos[0].maxsolutions = _nj0;
5628 vinfos[1].jointtype = 1;
5629 vinfos[1].foffset = j1;
5630 vinfos[1].indices[0] = _ij1[0];
5631 vinfos[1].indices[1] = _ij1[1];
5632 vinfos[1].maxsolutions = _nj1;
5633 vinfos[2].jointtype = 1;
5634 vinfos[2].foffset = j2;
5635 vinfos[2].indices[0] = _ij2[0];
5636 vinfos[2].indices[1] = _ij2[1];
5637 vinfos[2].maxsolutions = _nj2;
5638 vinfos[3].jointtype = 1;
5639 vinfos[3].foffset = j3;
5640 vinfos[3].indices[0] = _ij3[0];
5641 vinfos[3].indices[1] = _ij3[1];
5642 vinfos[3].maxsolutions = _nj3;
5643 vinfos[4].jointtype = 1;
5644 vinfos[4].foffset = j4;
5645 vinfos[4].indices[0] = _ij4[0];
5646 vinfos[4].indices[1] = _ij4[1];
5647 vinfos[4].maxsolutions = _nj4;
5648 vinfos[5].jointtype = 1;
5649 vinfos[5].foffset = j5;
5650 vinfos[5].indices[0] = _ij5[0];
5651 vinfos[5].indices[1] = _ij5[1];
5652 vinfos[5].maxsolutions = _nj5;
5653 std::vector<int> vfree(0);
5654 solutions.AddSolution(vinfos,vfree);
5655 }
5656 }
5657 }
5658 
5659 }
5660 
5661 }
5662 
5663 } else
5664 {
5665 {
5666 IkReal j3array[1], cj3array[1], sj3array[1];
5667 bool j3valid[1]={false};
5668 _nj3 = 1;
5669 CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst2*gconst2)))+(((-1.0)*(gconst1*gconst1))))),-1);
5670 if(!x438.valid){
5671 continue;
5672 }
5673 CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst2*new_r01)),IkReal((gconst1*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5674 if(!x439.valid){
5675 continue;
5676 }
5677 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5678 sj3array[0]=IKsin(j3array[0]);
5679 cj3array[0]=IKcos(j3array[0]);
5680 if( j3array[0] > IKPI )
5681 {
5682  j3array[0]-=IK2PI;
5683 }
5684 else if( j3array[0] < -IKPI )
5685 { j3array[0]+=IK2PI;
5686 }
5687 j3valid[0] = true;
5688 for(int ij3 = 0; ij3 < 1; ++ij3)
5689 {
5690 if( !j3valid[ij3] )
5691 {
5692  continue;
5693 }
5694 _ij3[0] = ij3; _ij3[1] = -1;
5695 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5696 {
5697 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5698 {
5699  j3valid[iij3]=false; _ij3[1] = iij3; break;
5700 }
5701 }
5702 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5703 {
5704 IkReal evalcond[6];
5705 IkReal x440=IKcos(j3);
5706 IkReal x441=IKsin(j3);
5707 IkReal x442=(gconst1*x441);
5708 IkReal x443=(gconst2*x441);
5709 IkReal x444=((1.0)*x440);
5710 IkReal x445=(gconst2*x444);
5711 evalcond[0]=(((new_r01*x440))+gconst1+((new_r11*x441)));
5712 evalcond[1]=(((gconst1*x440))+x443+new_r01);
5713 evalcond[2]=((((-1.0)*x445))+x442);
5714 evalcond[3]=(((new_r01*x441))+gconst2+(((-1.0)*new_r11*x444)));
5715 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5716 evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst1*x444)));
5717 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 )
5718 {
5719 continue;
5720 }
5721 }
5722 
5723 {
5724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5725 vinfos[0].jointtype = 1;
5726 vinfos[0].foffset = j0;
5727 vinfos[0].indices[0] = _ij0[0];
5728 vinfos[0].indices[1] = _ij0[1];
5729 vinfos[0].maxsolutions = _nj0;
5730 vinfos[1].jointtype = 1;
5731 vinfos[1].foffset = j1;
5732 vinfos[1].indices[0] = _ij1[0];
5733 vinfos[1].indices[1] = _ij1[1];
5734 vinfos[1].maxsolutions = _nj1;
5735 vinfos[2].jointtype = 1;
5736 vinfos[2].foffset = j2;
5737 vinfos[2].indices[0] = _ij2[0];
5738 vinfos[2].indices[1] = _ij2[1];
5739 vinfos[2].maxsolutions = _nj2;
5740 vinfos[3].jointtype = 1;
5741 vinfos[3].foffset = j3;
5742 vinfos[3].indices[0] = _ij3[0];
5743 vinfos[3].indices[1] = _ij3[1];
5744 vinfos[3].maxsolutions = _nj3;
5745 vinfos[4].jointtype = 1;
5746 vinfos[4].foffset = j4;
5747 vinfos[4].indices[0] = _ij4[0];
5748 vinfos[4].indices[1] = _ij4[1];
5749 vinfos[4].maxsolutions = _nj4;
5750 vinfos[5].jointtype = 1;
5751 vinfos[5].foffset = j5;
5752 vinfos[5].indices[0] = _ij5[0];
5753 vinfos[5].indices[1] = _ij5[1];
5754 vinfos[5].maxsolutions = _nj5;
5755 std::vector<int> vfree(0);
5756 solutions.AddSolution(vinfos,vfree);
5757 }
5758 }
5759 }
5760 
5761 }
5762 
5763 }
5764 
5765 } else
5766 {
5767 {
5768 IkReal j3array[1], cj3array[1], sj3array[1];
5769 bool j3valid[1]={false};
5770 _nj3 = 1;
5771 CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(gconst1*gconst1),IkReal(((-1.0)*gconst1*gconst2)),IKFAST_ATAN2_MAGTHRESH);
5772 if(!x446.valid){
5773 continue;
5774 }
5775 CheckValue<IkReal> x447=IKPowWithIntegerCheck(IKsign((((gconst2*new_r01))+(((-1.0)*gconst1*new_r11)))),-1);
5776 if(!x447.valid){
5777 continue;
5778 }
5779 j3array[0]=((-1.5707963267949)+(x446.value)+(((1.5707963267949)*(x447.value))));
5780 sj3array[0]=IKsin(j3array[0]);
5781 cj3array[0]=IKcos(j3array[0]);
5782 if( j3array[0] > IKPI )
5783 {
5784  j3array[0]-=IK2PI;
5785 }
5786 else if( j3array[0] < -IKPI )
5787 { j3array[0]+=IK2PI;
5788 }
5789 j3valid[0] = true;
5790 for(int ij3 = 0; ij3 < 1; ++ij3)
5791 {
5792 if( !j3valid[ij3] )
5793 {
5794  continue;
5795 }
5796 _ij3[0] = ij3; _ij3[1] = -1;
5797 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5798 {
5799 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5800 {
5801  j3valid[iij3]=false; _ij3[1] = iij3; break;
5802 }
5803 }
5804 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5805 {
5806 IkReal evalcond[6];
5807 IkReal x448=IKcos(j3);
5808 IkReal x449=IKsin(j3);
5809 IkReal x450=(gconst1*x449);
5810 IkReal x451=(gconst2*x449);
5811 IkReal x452=((1.0)*x448);
5812 IkReal x453=(gconst2*x452);
5813 evalcond[0]=(((new_r01*x448))+gconst1+((new_r11*x449)));
5814 evalcond[1]=(((gconst1*x448))+x451+new_r01);
5815 evalcond[2]=((((-1.0)*x453))+x450);
5816 evalcond[3]=(((new_r01*x449))+gconst2+(((-1.0)*new_r11*x452)));
5817 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5818 evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst1*x452)));
5819 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 )
5820 {
5821 continue;
5822 }
5823 }
5824 
5825 {
5826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5827 vinfos[0].jointtype = 1;
5828 vinfos[0].foffset = j0;
5829 vinfos[0].indices[0] = _ij0[0];
5830 vinfos[0].indices[1] = _ij0[1];
5831 vinfos[0].maxsolutions = _nj0;
5832 vinfos[1].jointtype = 1;
5833 vinfos[1].foffset = j1;
5834 vinfos[1].indices[0] = _ij1[0];
5835 vinfos[1].indices[1] = _ij1[1];
5836 vinfos[1].maxsolutions = _nj1;
5837 vinfos[2].jointtype = 1;
5838 vinfos[2].foffset = j2;
5839 vinfos[2].indices[0] = _ij2[0];
5840 vinfos[2].indices[1] = _ij2[1];
5841 vinfos[2].maxsolutions = _nj2;
5842 vinfos[3].jointtype = 1;
5843 vinfos[3].foffset = j3;
5844 vinfos[3].indices[0] = _ij3[0];
5845 vinfos[3].indices[1] = _ij3[1];
5846 vinfos[3].maxsolutions = _nj3;
5847 vinfos[4].jointtype = 1;
5848 vinfos[4].foffset = j4;
5849 vinfos[4].indices[0] = _ij4[0];
5850 vinfos[4].indices[1] = _ij4[1];
5851 vinfos[4].maxsolutions = _nj4;
5852 vinfos[5].jointtype = 1;
5853 vinfos[5].foffset = j5;
5854 vinfos[5].indices[0] = _ij5[0];
5855 vinfos[5].indices[1] = _ij5[1];
5856 vinfos[5].maxsolutions = _nj5;
5857 std::vector<int> vfree(0);
5858 solutions.AddSolution(vinfos,vfree);
5859 }
5860 }
5861 }
5862 
5863 }
5864 
5865 }
5866 
5867 }
5868 } while(0);
5869 if( bgotonextstatement )
5870 {
5871 bool bgotonextstatement = true;
5872 do
5873 {
5874 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5875 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5876 {
5877 bgotonextstatement=false;
5878 {
5879 IkReal j3eval[1];
5880 CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5881 if(!x455.valid){
5882 continue;
5883 }
5884 IkReal x454=((-1.0)*(x455.value));
5885 sj4=0;
5886 cj4=1.0;
5887 j4=0;
5888 sj5=gconst1;
5889 cj5=gconst2;
5890 j5=x454;
5891 new_r01=0;
5892 new_r10=0;
5893 IkReal gconst0=x454;
5894 IkReal x456 = new_r11*new_r11;
5895 if(IKabs(x456)==0){
5896 continue;
5897 }
5898 IkReal gconst1=(new_r11*(pow(x456,-0.5)));
5899 IkReal gconst2=0;
5900 j3eval[0]=new_r00;
5901 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5902 {
5903 {
5904 IkReal j3eval[1];
5905 CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5906 if(!x458.valid){
5907 continue;
5908 }
5909 IkReal x457=((-1.0)*(x458.value));
5910 sj4=0;
5911 cj4=1.0;
5912 j4=0;
5913 sj5=gconst1;
5914 cj5=gconst2;
5915 j5=x457;
5916 new_r01=0;
5917 new_r10=0;
5918 IkReal gconst0=x457;
5919 IkReal x459 = new_r11*new_r11;
5920 if(IKabs(x459)==0){
5921 continue;
5922 }
5923 IkReal gconst1=(new_r11*(pow(x459,-0.5)));
5924 IkReal gconst2=0;
5925 j3eval[0]=new_r11;
5926 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5927 {
5928 {
5929 IkReal j3array[2], cj3array[2], sj3array[2];
5930 bool j3valid[2]={false};
5931 _nj3 = 2;
5932 CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst1,-1);
5933 if(!x460.valid){
5934 continue;
5935 }
5936 sj3array[0]=((-1.0)*new_r00*(x460.value));
5937 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5938 {
5939  j3valid[0] = j3valid[1] = true;
5940  j3array[0] = IKasin(sj3array[0]);
5941  cj3array[0] = IKcos(j3array[0]);
5942  sj3array[1] = sj3array[0];
5943  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5944  cj3array[1] = -cj3array[0];
5945 }
5946 else if( isnan(sj3array[0]) )
5947 {
5948  // probably any value will work
5949  j3valid[0] = true;
5950  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5951 }
5952 for(int ij3 = 0; ij3 < 2; ++ij3)
5953 {
5954 if( !j3valid[ij3] )
5955 {
5956  continue;
5957 }
5958 _ij3[0] = ij3; _ij3[1] = -1;
5959 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5960 {
5961 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5962 {
5963  j3valid[iij3]=false; _ij3[1] = iij3; break;
5964 }
5965 }
5966 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5967 {
5968 IkReal evalcond[6];
5969 IkReal x461=IKcos(j3);
5970 IkReal x462=IKsin(j3);
5971 evalcond[0]=(gconst1*x461);
5972 evalcond[1]=(new_r00*x461);
5973 evalcond[2]=((-1.0)*new_r11*x461);
5974 evalcond[3]=(((new_r00*x462))+gconst1);
5975 evalcond[4]=(((new_r11*x462))+gconst1);
5976 evalcond[5]=(((gconst1*x462))+new_r11);
5977 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 )
5978 {
5979 continue;
5980 }
5981 }
5982 
5983 {
5984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5985 vinfos[0].jointtype = 1;
5986 vinfos[0].foffset = j0;
5987 vinfos[0].indices[0] = _ij0[0];
5988 vinfos[0].indices[1] = _ij0[1];
5989 vinfos[0].maxsolutions = _nj0;
5990 vinfos[1].jointtype = 1;
5991 vinfos[1].foffset = j1;
5992 vinfos[1].indices[0] = _ij1[0];
5993 vinfos[1].indices[1] = _ij1[1];
5994 vinfos[1].maxsolutions = _nj1;
5995 vinfos[2].jointtype = 1;
5996 vinfos[2].foffset = j2;
5997 vinfos[2].indices[0] = _ij2[0];
5998 vinfos[2].indices[1] = _ij2[1];
5999 vinfos[2].maxsolutions = _nj2;
6000 vinfos[3].jointtype = 1;
6001 vinfos[3].foffset = j3;
6002 vinfos[3].indices[0] = _ij3[0];
6003 vinfos[3].indices[1] = _ij3[1];
6004 vinfos[3].maxsolutions = _nj3;
6005 vinfos[4].jointtype = 1;
6006 vinfos[4].foffset = j4;
6007 vinfos[4].indices[0] = _ij4[0];
6008 vinfos[4].indices[1] = _ij4[1];
6009 vinfos[4].maxsolutions = _nj4;
6010 vinfos[5].jointtype = 1;
6011 vinfos[5].foffset = j5;
6012 vinfos[5].indices[0] = _ij5[0];
6013 vinfos[5].indices[1] = _ij5[1];
6014 vinfos[5].maxsolutions = _nj5;
6015 std::vector<int> vfree(0);
6016 solutions.AddSolution(vinfos,vfree);
6017 }
6018 }
6019 }
6020 
6021 } else
6022 {
6023 {
6024 IkReal j3array[2], cj3array[2], sj3array[2];
6025 bool j3valid[2]={false};
6026 _nj3 = 2;
6027 CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
6028 if(!x463.valid){
6029 continue;
6030 }
6031 sj3array[0]=((-1.0)*gconst1*(x463.value));
6032 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6033 {
6034  j3valid[0] = j3valid[1] = true;
6035  j3array[0] = IKasin(sj3array[0]);
6036  cj3array[0] = IKcos(j3array[0]);
6037  sj3array[1] = sj3array[0];
6038  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6039  cj3array[1] = -cj3array[0];
6040 }
6041 else if( isnan(sj3array[0]) )
6042 {
6043  // probably any value will work
6044  j3valid[0] = true;
6045  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6046 }
6047 for(int ij3 = 0; ij3 < 2; ++ij3)
6048 {
6049 if( !j3valid[ij3] )
6050 {
6051  continue;
6052 }
6053 _ij3[0] = ij3; _ij3[1] = -1;
6054 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6055 {
6056 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6057 {
6058  j3valid[iij3]=false; _ij3[1] = iij3; break;
6059 }
6060 }
6061 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6062 {
6063 IkReal evalcond[6];
6064 IkReal x464=IKcos(j3);
6065 IkReal x465=IKsin(j3);
6066 IkReal x466=(gconst1*x465);
6067 evalcond[0]=(gconst1*x464);
6068 evalcond[1]=(new_r00*x464);
6069 evalcond[2]=((-1.0)*new_r11*x464);
6070 evalcond[3]=(((new_r00*x465))+gconst1);
6071 evalcond[4]=(x466+new_r00);
6072 evalcond[5]=(x466+new_r11);
6073 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 )
6074 {
6075 continue;
6076 }
6077 }
6078 
6079 {
6080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6081 vinfos[0].jointtype = 1;
6082 vinfos[0].foffset = j0;
6083 vinfos[0].indices[0] = _ij0[0];
6084 vinfos[0].indices[1] = _ij0[1];
6085 vinfos[0].maxsolutions = _nj0;
6086 vinfos[1].jointtype = 1;
6087 vinfos[1].foffset = j1;
6088 vinfos[1].indices[0] = _ij1[0];
6089 vinfos[1].indices[1] = _ij1[1];
6090 vinfos[1].maxsolutions = _nj1;
6091 vinfos[2].jointtype = 1;
6092 vinfos[2].foffset = j2;
6093 vinfos[2].indices[0] = _ij2[0];
6094 vinfos[2].indices[1] = _ij2[1];
6095 vinfos[2].maxsolutions = _nj2;
6096 vinfos[3].jointtype = 1;
6097 vinfos[3].foffset = j3;
6098 vinfos[3].indices[0] = _ij3[0];
6099 vinfos[3].indices[1] = _ij3[1];
6100 vinfos[3].maxsolutions = _nj3;
6101 vinfos[4].jointtype = 1;
6102 vinfos[4].foffset = j4;
6103 vinfos[4].indices[0] = _ij4[0];
6104 vinfos[4].indices[1] = _ij4[1];
6105 vinfos[4].maxsolutions = _nj4;
6106 vinfos[5].jointtype = 1;
6107 vinfos[5].foffset = j5;
6108 vinfos[5].indices[0] = _ij5[0];
6109 vinfos[5].indices[1] = _ij5[1];
6110 vinfos[5].maxsolutions = _nj5;
6111 std::vector<int> vfree(0);
6112 solutions.AddSolution(vinfos,vfree);
6113 }
6114 }
6115 }
6116 
6117 }
6118 
6119 }
6120 
6121 } else
6122 {
6123 {
6124 IkReal j3array[2], cj3array[2], sj3array[2];
6125 bool j3valid[2]={false};
6126 _nj3 = 2;
6127 CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6128 if(!x467.valid){
6129 continue;
6130 }
6131 sj3array[0]=((-1.0)*gconst1*(x467.value));
6132 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6133 {
6134  j3valid[0] = j3valid[1] = true;
6135  j3array[0] = IKasin(sj3array[0]);
6136  cj3array[0] = IKcos(j3array[0]);
6137  sj3array[1] = sj3array[0];
6138  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6139  cj3array[1] = -cj3array[0];
6140 }
6141 else if( isnan(sj3array[0]) )
6142 {
6143  // probably any value will work
6144  j3valid[0] = true;
6145  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6146 }
6147 for(int ij3 = 0; ij3 < 2; ++ij3)
6148 {
6149 if( !j3valid[ij3] )
6150 {
6151  continue;
6152 }
6153 _ij3[0] = ij3; _ij3[1] = -1;
6154 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6155 {
6156 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6157 {
6158  j3valid[iij3]=false; _ij3[1] = iij3; break;
6159 }
6160 }
6161 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6162 {
6163 IkReal evalcond[6];
6164 IkReal x468=IKcos(j3);
6165 IkReal x469=IKsin(j3);
6166 IkReal x470=(gconst1*x469);
6167 evalcond[0]=(gconst1*x468);
6168 evalcond[1]=(new_r00*x468);
6169 evalcond[2]=((-1.0)*new_r11*x468);
6170 evalcond[3]=(((new_r11*x469))+gconst1);
6171 evalcond[4]=(x470+new_r00);
6172 evalcond[5]=(x470+new_r11);
6173 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 )
6174 {
6175 continue;
6176 }
6177 }
6178 
6179 {
6180 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6181 vinfos[0].jointtype = 1;
6182 vinfos[0].foffset = j0;
6183 vinfos[0].indices[0] = _ij0[0];
6184 vinfos[0].indices[1] = _ij0[1];
6185 vinfos[0].maxsolutions = _nj0;
6186 vinfos[1].jointtype = 1;
6187 vinfos[1].foffset = j1;
6188 vinfos[1].indices[0] = _ij1[0];
6189 vinfos[1].indices[1] = _ij1[1];
6190 vinfos[1].maxsolutions = _nj1;
6191 vinfos[2].jointtype = 1;
6192 vinfos[2].foffset = j2;
6193 vinfos[2].indices[0] = _ij2[0];
6194 vinfos[2].indices[1] = _ij2[1];
6195 vinfos[2].maxsolutions = _nj2;
6196 vinfos[3].jointtype = 1;
6197 vinfos[3].foffset = j3;
6198 vinfos[3].indices[0] = _ij3[0];
6199 vinfos[3].indices[1] = _ij3[1];
6200 vinfos[3].maxsolutions = _nj3;
6201 vinfos[4].jointtype = 1;
6202 vinfos[4].foffset = j4;
6203 vinfos[4].indices[0] = _ij4[0];
6204 vinfos[4].indices[1] = _ij4[1];
6205 vinfos[4].maxsolutions = _nj4;
6206 vinfos[5].jointtype = 1;
6207 vinfos[5].foffset = j5;
6208 vinfos[5].indices[0] = _ij5[0];
6209 vinfos[5].indices[1] = _ij5[1];
6210 vinfos[5].maxsolutions = _nj5;
6211 std::vector<int> vfree(0);
6212 solutions.AddSolution(vinfos,vfree);
6213 }
6214 }
6215 }
6216 
6217 }
6218 
6219 }
6220 
6221 }
6222 } while(0);
6223 if( bgotonextstatement )
6224 {
6225 bool bgotonextstatement = true;
6226 do
6227 {
6228 evalcond[0]=IKabs(new_r11);
6229 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6230 {
6231 bgotonextstatement=false;
6232 {
6233 IkReal j3eval[1];
6234 IkReal x471=((-1.0)*new_r01);
6235 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6236 if(!x473.valid){
6237 continue;
6238 }
6239 IkReal x472=((-1.0)*(x473.value));
6240 sj4=0;
6241 cj4=1.0;
6242 j4=0;
6243 sj5=gconst1;
6244 cj5=gconst2;
6245 j5=x472;
6246 new_r11=0;
6247 IkReal gconst0=x472;
6248 IkReal gconst1=0;
6249 IkReal x474 = new_r01*new_r01;
6250 if(IKabs(x474)==0){
6251 continue;
6252 }
6253 IkReal gconst2=(x471*(pow(x474,-0.5)));
6254 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6255 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6256 {
6257 {
6258 IkReal j3eval[1];
6259 IkReal x475=((-1.0)*new_r01);
6260 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6261 if(!x477.valid){
6262 continue;
6263 }
6264 IkReal x476=((-1.0)*(x477.value));
6265 sj4=0;
6266 cj4=1.0;
6267 j4=0;
6268 sj5=gconst1;
6269 cj5=gconst2;
6270 j5=x476;
6271 new_r11=0;
6272 IkReal gconst0=x476;
6273 IkReal gconst1=0;
6274 IkReal x478 = new_r01*new_r01;
6275 if(IKabs(x478)==0){
6276 continue;
6277 }
6278 IkReal gconst2=(x475*(pow(x478,-0.5)));
6279 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6280 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6281 {
6282 {
6283 IkReal j3eval[1];
6284 IkReal x479=((-1.0)*new_r01);
6285 CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal(0),IkReal(x479),IKFAST_ATAN2_MAGTHRESH);
6286 if(!x481.valid){
6287 continue;
6288 }
6289 IkReal x480=((-1.0)*(x481.value));
6290 sj4=0;
6291 cj4=1.0;
6292 j4=0;
6293 sj5=gconst1;
6294 cj5=gconst2;
6295 j5=x480;
6296 new_r11=0;
6297 IkReal gconst0=x480;
6298 IkReal gconst1=0;
6299 IkReal x482 = new_r01*new_r01;
6300 if(IKabs(x482)==0){
6301 continue;
6302 }
6303 IkReal gconst2=(x479*(pow(x482,-0.5)));
6304 j3eval[0]=new_r01;
6305 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6306 {
6307 continue; // 3 cases reached
6308 
6309 } else
6310 {
6311 {
6312 IkReal j3array[1], cj3array[1], sj3array[1];
6313 bool j3valid[1]={false};
6314 _nj3 = 1;
6315 CheckValue<IkReal> x483=IKPowWithIntegerCheck(new_r01,-1);
6316 if(!x483.valid){
6317 continue;
6318 }
6319 CheckValue<IkReal> x484=IKPowWithIntegerCheck(gconst2,-1);
6320 if(!x484.valid){
6321 continue;
6322 }
6323 if( IKabs(((-1.0)*gconst2*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x484.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst2*(x483.value)))+IKsqr((new_r00*(x484.value)))-1) <= IKFAST_SINCOS_THRESH )
6324  continue;
6325 j3array[0]=IKatan2(((-1.0)*gconst2*(x483.value)), (new_r00*(x484.value)));
6326 sj3array[0]=IKsin(j3array[0]);
6327 cj3array[0]=IKcos(j3array[0]);
6328 if( j3array[0] > IKPI )
6329 {
6330  j3array[0]-=IK2PI;
6331 }
6332 else if( j3array[0] < -IKPI )
6333 { j3array[0]+=IK2PI;
6334 }
6335 j3valid[0] = true;
6336 for(int ij3 = 0; ij3 < 1; ++ij3)
6337 {
6338 if( !j3valid[ij3] )
6339 {
6340  continue;
6341 }
6342 _ij3[0] = ij3; _ij3[1] = -1;
6343 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6344 {
6345 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6346 {
6347  j3valid[iij3]=false; _ij3[1] = iij3; break;
6348 }