iiwa7_ikfast_solver.hpp
Go to the documentation of this file.
1 // clang-format off
2 
23 #include <tesseract_common/macros.h>
24 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
25 #ifndef IKFAST_HAS_LIBRARY
26 #define IKFAST_NO_MAIN
27 #endif
28 #include <tesseract_kinematics/ikfast/external/ikfast.h> // found inside share/openrave-X.Y/python/ikfast.h
29 using namespace ikfast;
30 
31 // check if the included ikfast version matches what this file was compiled with
32 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
33 IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61);
34 
35 #include <cmath>
36 #include <vector>
37 #include <limits>
38 #include <algorithm>
39 #include <complex>
40 
41 #ifndef IKFAST_ASSERT
42 #include <stdexcept>
43 #include <sstream>
44 #include <iostream>
45 
46 #ifdef _MSC_VER
47 #ifndef __PRETTY_FUNCTION__
48 #define __PRETTY_FUNCTION__ __FUNCDNAME__
49 #endif
50 #endif
51 
52 #ifndef __PRETTY_FUNCTION__
53 #define __PRETTY_FUNCTION__ __func__
54 #endif
55 
56 #define IKFAST_ASSERT(b) \
57  { \
58  if (!(b)) \
59  { \
60  std::stringstream ss; \
61  ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
62  << #b << "' failed"; \
63  throw std::runtime_error(ss.str()); \
64  } \
65  }
66 
67 #endif
68 
69 #if defined(_MSC_VER)
70 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
71 #else
72 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
73 #endif
74 
75 #define IK2PI ((IkReal)6.28318530717959)
76 #define IKPI ((IkReal)3.14159265358979)
77 #define IKPI_2 ((IkReal)1.57079632679490)
78 
79 #ifdef _MSC_VER
80 #ifndef isnan
81 #define isnan _isnan
82 #endif
83 #ifndef isinf
84 #define isinf _isinf
85 #endif
86 //#ifndef isfinite
87 //#define isfinite _isfinite
88 //#endif
89 #endif // _MSC_VER
90 
91 // lapack routines
92 extern "C" {
93 void dgetrf_(const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
94 void zgetrf_(const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
95 void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
96 void dgesv_(const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
97 void dgetrs_(const char* trans,
98  const int* n,
99  const int* nrhs,
100  double* a,
101  const int* lda,
102  int* ipiv,
103  double* b,
104  const int* ldb,
105  int* info);
106 void dgeev_(const char* jobvl,
107  const char* jobvr,
108  const int* n,
109  double* a,
110  const int* lda,
111  double* wr,
112  double* wi,
113  double* vl,
114  const int* ldvl,
115  double* vr,
116  const int* ldvr,
117  double* work,
118  const int* lwork,
119  int* info);
120 }
121 
122 using namespace std; // necessary to get std math routines
123 
124 #ifdef IKFAST_NAMESPACE
125 namespace IKFAST_NAMESPACE
126 {
127 #endif
128 
129 inline float IKabs(float f) { return fabsf(f); }
130 inline double IKabs(double f) { return fabs(f); }
131 
132 inline float IKsqr(float f) { return f * f; }
133 inline double IKsqr(double f) { return f * f; }
134 
135 inline float IKlog(float f) { return logf(f); }
136 inline double IKlog(double f) { return log(f); }
137 
138 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
139 #ifndef IKFAST_SINCOS_THRESH
140 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
141 #endif
142 
143 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and
144 // evaluation
145 #ifndef IKFAST_ATAN2_MAGTHRESH
146 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
147 #endif
148 
149 // minimum distance of separate solutions
150 #ifndef IKFAST_SOLUTION_THRESH
151 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
152 #endif
153 
154 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they
155 // can deviate
156 #ifndef IKFAST_EVALCOND_THRESH
157 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
158 #endif
159 
160 inline float IKasin(float f)
161 {
162  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
163  // wrong with the solver
164  if (f <= -1)
165  return float(-IKPI_2);
166  else if (f >= 1)
167  return float(IKPI_2);
168  return asinf(f);
169 }
170 inline double IKasin(double f)
171 {
172  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
173  // wrong with the solver
174  if (f <= -1)
175  return -IKPI_2;
176  else if (f >= 1)
177  return IKPI_2;
178  return asin(f);
179 }
180 
181 // return positive value in [0,y)
182 inline float IKfmod(float x, float y)
183 {
184  while (x < 0)
185  {
186  x += y;
187  }
188  return fmodf(x, y);
189 }
190 
191 // return positive value in [0,y)
192 inline double IKfmod(double x, double y)
193 {
194  while (x < 0)
195  {
196  x += y;
197  }
198  return fmod(x, y);
199 }
200 
201 inline float IKacos(float f)
202 {
203  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
204  // wrong with the solver
205  if (f <= -1)
206  return float(IKPI);
207  else if (f >= 1)
208  return float(0);
209  return acosf(f);
210 }
211 inline double IKacos(double f)
212 {
213  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
214  // wrong with the solver
215  if (f <= -1)
216  return IKPI;
217  else if (f >= 1)
218  return 0;
219  return acos(f);
220 }
221 inline float IKsin(float f) { return sinf(f); }
222 inline double IKsin(double f) { return sin(f); }
223 inline float IKcos(float f) { return cosf(f); }
224 inline double IKcos(double f) { return cos(f); }
225 inline float IKtan(float f) { return tanf(f); }
226 inline double IKtan(double f) { return tan(f); }
227 inline float IKsqrt(float f)
228 {
229  if (f <= 0.0f)
230  return 0.0f;
231  return sqrtf(f);
232 }
233 inline double IKsqrt(double f)
234 {
235  if (f <= 0.0)
236  return 0.0;
237  return sqrt(f);
238 }
239 inline float IKatan2Simple(float fy, float fx) { return atan2f(fy, fx); }
240 inline float IKatan2(float fy, float fx)
241 {
242  if (isnan(fy))
243  {
244  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
245  return float(IKPI_2);
246  }
247  else if (isnan(fx))
248  {
249  return 0;
250  }
251  return atan2f(fy, fx);
252 }
253 inline double IKatan2Simple(double fy, double fx) { return atan2(fy, fx); }
254 inline double IKatan2(double fy, double fx)
255 {
256  if (isnan(fy))
257  {
258  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
259  return IKPI_2;
260  }
261  else if (isnan(fx))
262  {
263  return 0;
264  }
265  return atan2(fy, fx);
266 }
267 
268 template <typename T>
269 struct CheckValue
270 {
271  T value;
272  bool valid;
273 };
274 
275 template <typename T>
276 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
277 {
278  CheckValue<T> ret;
279  ret.valid = false;
280  ret.value = 0;
281  if (!isnan(fy) && !isnan(fx))
282  {
283  if (IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH)
284  {
285  ret.value = IKatan2Simple(fy, fx);
286  ret.valid = true;
287  }
288  }
289  return ret;
290 }
291 
292 inline float IKsign(float f)
293 {
294  if (f > 0)
295  {
296  return float(1);
297  }
298  else if (f < 0)
299  {
300  return float(-1);
301  }
302  return 0;
303 }
304 
305 inline double IKsign(double f)
306 {
307  if (f > 0)
308  {
309  return 1.0;
310  }
311  else if (f < 0)
312  {
313  return -1.0;
314  }
315  return 0;
316 }
317 
318 template <typename T>
319 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
320 {
321  CheckValue<T> ret;
322  ret.valid = true;
323  if (n == 0)
324  {
325  ret.value = 1.0;
326  return ret;
327  }
328  else if (n == 1)
329  {
330  ret.value = f;
331  return ret;
332  }
333  else if (n < 0)
334  {
335  if (f == 0)
336  {
337  ret.valid = false;
338  ret.value = (T)1.0e30;
339  return ret;
340  }
341  if (n == -1)
342  {
343  ret.value = T(1.0) / f;
344  return ret;
345  }
346  }
347 
348  int num = n > 0 ? n : -n;
349  if (num == 2)
350  {
351  ret.value = f * f;
352  }
353  else if (num == 3)
354  {
355  ret.value = f * f * f;
356  }
357  else
358  {
359  ret.value = 1.0;
360  while (num > 0)
361  {
362  if (num & 1)
363  {
364  ret.value *= f;
365  }
366  num >>= 1;
367  f *= f;
368  }
369  }
370 
371  if (n < 0)
372  {
373  ret.value = T(1.0) / ret.value;
374  }
375  return ret;
376 }
377 
380 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
381 {
382  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,
383  x24, x25, x26, x27, x28, x29, x30, x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45, x46,
384  x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, x58, x59, x60, x61, x62, x63, x64, x65, x66, x67, x68;
385  x0 = IKcos(j[0]);
386  x1 = IKcos(j[1]);
387  x2 = IKcos(j[2]);
388  x3 = IKsin(j[0]);
389  x4 = IKsin(j[2]);
390  x5 = IKcos(j[3]);
391  x6 = IKsin(j[1]);
392  x7 = IKsin(j[3]);
393  x8 = IKcos(j[4]);
394  x9 = IKsin(j[4]);
395  x10 = IKcos(j[6]);
396  x11 = IKsin(j[6]);
397  x12 = IKsin(j[5]);
398  x13 = IKcos(j[5]);
399  x14 = ((0.4) * x3);
400  x15 = ((0.15085) * x5);
401  x16 = ((1.0) * x5);
402  x17 = ((1.0) * x8);
403  x18 = ((1.0) * x7);
404  x19 = ((1.0) * x4);
405  x20 = ((1.0) * x13);
406  x21 = ((1.0) * x1);
407  x22 = ((0.15085) * x7);
408  x23 = ((0.15085) * x8);
409  x24 = ((0.4) * x5);
410  x25 = ((0.4) * x2);
411  x26 = ((1.0) * x9);
412  x27 = ((0.15085) * x9);
413  x28 = (x0 * x6);
414  x29 = (x0 * x4);
415  x30 = ((-1.0) * x9);
416  x31 = (x5 * x6);
417  x32 = (x6 * x9);
418  x33 = (x2 * x3);
419  x34 = ((-1.0) * x8);
420  x35 = (x6 * x7);
421  x36 = (x1 * x2);
422  x37 = (x0 * x2);
423  x38 = (x2 * x6);
424  x39 = (x3 * x6);
425  x40 = (x19 * x3);
426  x41 = (x16 * x38);
427  x42 = (x19 * x32);
428  x43 = ((((-1.0) * x40)) + ((x0 * x36)));
429  x44 = (x29 + ((x1 * x33)));
430  x45 = (((x1 * x5)) + ((x2 * x35)));
431  x46 = (x40 + (((-1.0) * x21 * x37)));
432  x47 = ((((-1.0) * x37)) + ((x1 * x40)));
433  x48 = ((((-1.0) * x1 * x18)) + x41);
434  x49 = (((x0 * x1 * x19)) + (((1.0) * x33)));
435  x50 = ((((-1.0) * x0 * x19)) + (((-1.0) * x21 * x33)));
436  x51 = (x12 * x45);
437  x52 = (x43 * x5);
438  x53 = (x48 * x8);
439  x54 = (x47 * x9);
440  x55 = (x49 * x9);
441  x56 = (x50 * x7);
442  x57 = (x52 + ((x28 * x7)));
443  x58 = (((x3 * x35)) + ((x44 * x5)));
444  x59 = (((x46 * x7)) + ((x28 * x5)));
445  x60 = ((((-1.0) * x42)) + x53);
446  x61 = (((x26 * (((((-1.0) * x1 * x7)) + x41)))) + ((x17 * x4 * x6)));
447  x62 = ((((-1.0) * x18 * x28)) + (((-1.0) * x16 * x43)));
448  x63 = (((x3 * x31)) + x56);
449  x64 = ((((-1.0) * x18 * x39)) + (((-1.0) * x16 * x44)));
450  x65 = (x12 * x59);
451  x66 = (x62 * x8);
452  x67 = (x12 * x63);
453  x68 = (x54 + ((x64 * x8)));
454  eerot[0] =
455  ((((-1.0) * x10 *
456  (((((1.0) * x65)) + (((1.0) * x13 * ((((x8 * (((((-1.0) * x28 * x7)) + (((-1.0) * x52)))))) + x55)))))))) +
457  ((x11 * (((((-1.0) * x17 * x49)) + (((-1.0) * x26 * x57)))))));
458  eerot[1] = (((x11 * ((((x13 * ((x55 + x66)))) + x65)))) + ((x10 * ((((x34 * x49)) + ((x30 * x57)))))));
459  eerot[2] = (((x13 * x59)) + ((x12 * (((((-1.0) * x17 * x62)) + (((-1.0) * x26 * x49)))))));
460  eetrans[0] = (((x7 * (((((-1.0) * x0 * x1 * x25)) + ((x14 * x4)))))) + ((x13 * ((((x22 * x46)) + ((x15 * x28)))))) +
461  (((0.4) * x28)) + ((x12 * (((((-1.0) * x27 * x49)) + (((-1.0) * x23 * x62)))))) + ((x24 * x28)));
462  eerot[3] = (((x10 * (((((-1.0) * x20 * x68)) + (((-1.0) * x67)))))) + ((x11 * ((((x34 * x47)) + ((x30 * x58)))))));
463  eerot[4] = (((x10 * (((((-1.0) * x17 * x47)) + (((-1.0) * x26 * x58)))))) + ((x11 * ((((x13 * x68)) + x67)))));
464  eerot[5] = (((x13 * x63)) + ((x12 * (((((-1.0) * x17 * x64)) + (((-1.0) * x26 * x47)))))));
465  eetrans[1] = (((x13 * ((((x15 * x39)) + ((x22 * x50)))))) + ((x7 * (((((-1.0) * x14 * x36)) + (((-0.4) * x29)))))) +
466  ((x14 * x6)) + ((x14 * x31)) + ((x12 * (((((-1.0) * x27 * x47)) + (((-1.0) * x23 * x64)))))));
467  eerot[6] = (((x11 * x61)) + ((x10 * (((((-1.0) * x20 * x60)) + (((-1.0) * x51)))))));
468  eerot[7] = (((x11 * ((((x13 * x60)) + x51)))) + ((x10 * x61)));
469  eerot[8] = (((x12 * (((((-1.0) * x17 * x48)) + x42)))) + ((x13 * x45)));
470  eetrans[2] = ((0.34) + ((x1 * x24)) + (((0.4) * x1)) + ((x12 * ((((x27 * x4 * x6)) + (((-1.0) * x23 * x48)))))) +
471  ((x13 * ((((x1 * x15)) + ((x22 * x38)))))) + ((x25 * x35)));
472 }
473 
474 IKFAST_API int GetNumFreeParameters() { return 1; }
475 IKFAST_API int* GetFreeParameters()
476 {
477  static int freeparams[] = { 4 };
478  return freeparams;
479 }
480 IKFAST_API int GetNumJoints() { return 7; }
481 
482 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
483 
484 IKFAST_API int GetIkType() { return 0x67000001; }
485 
486 class IKSolver
487 {
488 public:
489  IkReal j0, cj0, sj0, htj0, j0mul, j1, cj1, sj1, htj1, j1mul, j2, cj2, sj2, htj2, j2mul, j3, cj3, sj3, htj3, j3mul, j5,
490  cj5, sj5, htj5, j5mul, j6, cj6, sj6, htj6, j6mul, j4, cj4, sj4, htj4, new_r00, r00, rxp0_0, new_r01, r01, rxp0_1,
491  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,
492  new_r21, r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx, new_py, py, npy, new_pz, pz, npz, pp;
493  unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij5[2], _nj5, _ij6[2], _nj6, _ij4[2], _nj4;
494 
495  IkReal j100, cj100, sj100;
496  unsigned char _ij100[2], _nj100;
497  bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
498  {
499  j0 = numeric_limits<IkReal>::quiet_NaN();
500  _ij0[0] = -1;
501  _ij0[1] = -1;
502  _nj0 = -1;
503  j1 = numeric_limits<IkReal>::quiet_NaN();
504  _ij1[0] = -1;
505  _ij1[1] = -1;
506  _nj1 = -1;
507  j2 = numeric_limits<IkReal>::quiet_NaN();
508  _ij2[0] = -1;
509  _ij2[1] = -1;
510  _nj2 = -1;
511  j3 = numeric_limits<IkReal>::quiet_NaN();
512  _ij3[0] = -1;
513  _ij3[1] = -1;
514  _nj3 = -1;
515  j5 = numeric_limits<IkReal>::quiet_NaN();
516  _ij5[0] = -1;
517  _ij5[1] = -1;
518  _nj5 = -1;
519  j6 = numeric_limits<IkReal>::quiet_NaN();
520  _ij6[0] = -1;
521  _ij6[1] = -1;
522  _nj6 = -1;
523  _ij4[0] = -1;
524  _ij4[1] = -1;
525  _nj4 = 0;
526  for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
527  {
528  solutions.Clear();
529  j4 = pfree[0];
530  cj4 = cos(pfree[0]);
531  sj4 = sin(pfree[0]), htj4 = tan(pfree[0] * 0.5);
532  r00 = eerot[0 * 3 + 0];
533  r01 = eerot[0 * 3 + 1];
534  r02 = eerot[0 * 3 + 2];
535  r10 = eerot[1 * 3 + 0];
536  r11 = eerot[1 * 3 + 1];
537  r12 = eerot[1 * 3 + 2];
538  r20 = eerot[2 * 3 + 0];
539  r21 = eerot[2 * 3 + 1];
540  r22 = eerot[2 * 3 + 2];
541  px = eetrans[0];
542  py = eetrans[1];
543  pz = eetrans[2];
544 
545  new_r00 = ((-1.0) * r00);
546  new_r01 = ((-1.0) * r01);
547  new_r02 = r02;
548  new_px = (px + (((-0.15085) * r02)));
549  new_r10 = ((-1.0) * r10);
550  new_r11 = ((-1.0) * r11);
551  new_r12 = r12;
552  new_py = (py + (((-0.15085) * r12)));
553  new_r20 = ((-1.0) * r20);
554  new_r21 = ((-1.0) * r21);
555  new_r22 = r22;
556  new_pz = ((-0.34) + pz + (((-0.15085) * r22)));
557  r00 = new_r00;
558  r01 = new_r01;
559  r02 = new_r02;
560  r10 = new_r10;
561  r11 = new_r11;
562  r12 = new_r12;
563  r20 = new_r20;
564  r21 = new_r21;
565  r22 = new_r22;
566  px = new_px;
567  py = new_py;
568  pz = new_pz;
569  IkReal x69 = ((1.0) * px);
570  IkReal x70 = ((1.0) * pz);
571  IkReal x71 = ((1.0) * py);
572  pp = ((px * px) + (py * py) + (pz * pz));
573  npx = (((px * r00)) + ((py * r10)) + ((pz * r20)));
574  npy = (((px * r01)) + ((py * r11)) + ((pz * r21)));
575  npz = (((px * r02)) + ((py * r12)) + ((pz * r22)));
576  rxp0_0 = ((((-1.0) * r20 * x71)) + ((pz * r10)));
577  rxp0_1 = (((px * r20)) + (((-1.0) * r00 * x70)));
578  rxp0_2 = ((((-1.0) * r10 * x69)) + ((py * r00)));
579  rxp1_0 = ((((-1.0) * r21 * x71)) + ((pz * r11)));
580  rxp1_1 = (((px * r21)) + (((-1.0) * r01 * x70)));
581  rxp1_2 = ((((-1.0) * r11 * x69)) + ((py * r01)));
582  rxp2_0 = ((((-1.0) * r22 * x71)) + ((pz * r12)));
583  rxp2_1 = (((px * r22)) + (((-1.0) * r02 * x70)));
584  rxp2_2 = ((((-1.0) * r12 * x69)) + ((py * r02)));
585  {
586  IkReal j3array[2], cj3array[2], sj3array[2];
587  bool j3valid[2] = { false };
588  _nj3 = 2;
589  cj3array[0] = ((-1.0) + (((3.125) * pp)));
590  if (cj3array[0] >= -1 - IKFAST_SINCOS_THRESH && cj3array[0] <= 1 + IKFAST_SINCOS_THRESH)
591  {
592  j3valid[0] = j3valid[1] = true;
593  j3array[0] = IKacos(cj3array[0]);
594  sj3array[0] = IKsin(j3array[0]);
595  cj3array[1] = cj3array[0];
596  j3array[1] = -j3array[0];
597  sj3array[1] = -sj3array[0];
598  }
599  else if (isnan(cj3array[0]))
600  {
601  // probably any value will work
602  j3valid[0] = true;
603  cj3array[0] = 1;
604  sj3array[0] = 0;
605  j3array[0] = 0;
606  }
607  for (int ij3 = 0; ij3 < 2; ++ij3)
608  {
609  if (!j3valid[ij3])
610  {
611  continue;
612  }
613  _ij3[0] = ij3;
614  _ij3[1] = -1;
615  for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
616  {
617  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
618  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
619  {
620  j3valid[iij3] = false;
621  _ij3[1] = iij3;
622  break;
623  }
624  }
625  j3 = j3array[ij3];
626  cj3 = cj3array[ij3];
627  sj3 = sj3array[ij3];
628 
629  {
630  IkReal j6eval[2];
631  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
632  j6eval[1] = ((npx * npx) + (npy * npy));
633  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000)
634  {
635  {
636  IkReal j5eval[2];
637  j5eval[0] = ((1.0) + (cj3 * cj3) + (((2.0) * cj3)) + (((cj4 * cj4) * (sj3 * sj3))));
638  j5eval[1] = ((((2.5) * (IKabs(((0.4) + (((0.4) * cj3))))))) + (IKabs((cj4 * sj3))));
639  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000)
640  {
641  {
642  IkReal evalcond[2];
643  bool bgotonextstatement = true;
644  do
645  {
646  evalcond[0] =
647  ((-3.14159265358979) +
648  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))), 6.28318530717959)));
649  evalcond[1] = pp;
650  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000)
651  {
652  bgotonextstatement = false;
653  {
654  IkReal j6eval[1];
655  sj3 = 0;
656  cj3 = -1.0;
657  j3 = 3.14159265358979;
658  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
659  if (IKabs(j6eval[0]) < 0.0000010000000000)
660  {
661  continue; // no branches [j5, j6]
662  }
663  else
664  {
665  {
666  IkReal j6array[2], cj6array[2], sj6array[2];
667  bool j6valid[2] = { false };
668  _nj6 = 2;
669  CheckValue<IkReal> x73 =
670  IKatan2WithCheck(IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
671  if (!x73.valid)
672  {
673  continue;
674  }
675  IkReal x72 = x73.value;
676  j6array[0] = ((-1.0) * x72);
677  sj6array[0] = IKsin(j6array[0]);
678  cj6array[0] = IKcos(j6array[0]);
679  j6array[1] = ((3.14159265358979) + (((-1.0) * x72)));
680  sj6array[1] = IKsin(j6array[1]);
681  cj6array[1] = IKcos(j6array[1]);
682  if (j6array[0] > IKPI)
683  {
684  j6array[0] -= IK2PI;
685  }
686  else if (j6array[0] < -IKPI)
687  {
688  j6array[0] += IK2PI;
689  }
690  j6valid[0] = true;
691  if (j6array[1] > IKPI)
692  {
693  j6array[1] -= IK2PI;
694  }
695  else if (j6array[1] < -IKPI)
696  {
697  j6array[1] += IK2PI;
698  }
699  j6valid[1] = true;
700  for (int ij6 = 0; ij6 < 2; ++ij6)
701  {
702  if (!j6valid[ij6])
703  {
704  continue;
705  }
706  _ij6[0] = ij6;
707  _ij6[1] = -1;
708  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
709  {
710  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
711  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
712  {
713  j6valid[iij6] = false;
714  _ij6[1] = iij6;
715  break;
716  }
717  }
718  j6 = j6array[ij6];
719  cj6 = cj6array[ij6];
720  sj6 = sj6array[ij6];
721  {
722  IkReal evalcond[1];
723  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) + (((-1.0) * npx * (IKsin(j6)))));
724  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
725  {
726  continue;
727  }
728  }
729 
730  {
731  IkReal j5eval[1];
732  sj3 = 0;
733  cj3 = -1.0;
734  j3 = 3.14159265358979;
735  j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
736  if (IKabs(j5eval[0]) < 0.0000000100000000)
737  {
738  continue; // no branches [j5]
739  }
740  else
741  {
742  IkReal op[2 + 1], zeror[2];
743  int numroots;
744  IkReal x74 = (npy * sj6);
745  IkReal x75 = (cj6 * npx);
746  op[0] = (x75 + (((-1.0) * x74)));
747  op[1] = 0;
748  op[2] = (x74 + (((-1.0) * x75)));
749  polyroots2(op, zeror, numroots);
750  IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
751  int numsolutions = 0;
752  for (int ij5 = 0; ij5 < numroots; ++ij5)
753  {
754  IkReal htj5 = zeror[ij5];
755  tempj5array[0] = ((2.0) * (atan(htj5)));
756  for (int kj5 = 0; kj5 < 1; ++kj5)
757  {
758  j5array[numsolutions] = tempj5array[kj5];
759  if (j5array[numsolutions] > IKPI)
760  {
761  j5array[numsolutions] -= IK2PI;
762  }
763  else if (j5array[numsolutions] < -IKPI)
764  {
765  j5array[numsolutions] += IK2PI;
766  }
767  sj5array[numsolutions] = IKsin(j5array[numsolutions]);
768  cj5array[numsolutions] = IKcos(j5array[numsolutions]);
769  numsolutions++;
770  }
771  }
772  bool j5valid[2] = { true, true };
773  _nj5 = 2;
774  for (int ij5 = 0; ij5 < numsolutions; ++ij5)
775  {
776  if (!j5valid[ij5])
777  {
778  continue;
779  }
780  j5 = j5array[ij5];
781  cj5 = cj5array[ij5];
782  sj5 = sj5array[ij5];
783  htj5 = IKtan(j5 / 2);
784 
785  _ij5[0] = ij5;
786  _ij5[1] = -1;
787  for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
788  {
789  if (j5valid[iij5] &&
790  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
791  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
792  {
793  j5valid[iij5] = false;
794  _ij5[1] = iij5;
795  break;
796  }
797  }
798  rotationfunction0(solutions);
799  }
800  }
801  }
802  }
803  }
804  }
805  }
806  }
807  } while (0);
808  if (bgotonextstatement)
809  {
810  bool bgotonextstatement = true;
811  do
812  {
813  if (1)
814  {
815  bgotonextstatement = false;
816  continue; // branch miss [j5, j6]
817  }
818  } while (0);
819  if (bgotonextstatement)
820  {
821  }
822  }
823  }
824  }
825  else
826  {
827  {
828  IkReal j5array[2], cj5array[2], sj5array[2];
829  bool j5valid[2] = { false };
830  _nj5 = 2;
831  IkReal x1007 = ((0.4) + (((0.4) * cj3)));
832  CheckValue<IkReal> x1010 =
833  IKatan2WithCheck(IkReal(x1007), IkReal(((0.4) * cj4 * sj3)), IKFAST_ATAN2_MAGTHRESH);
834  if (!x1010.valid)
835  {
836  continue;
837  }
838  IkReal x1008 = ((1.0) * (x1010.value));
839  if ((((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007))) < -0.00001)
840  continue;
841  CheckValue<IkReal> x1011 = IKPowWithIntegerCheck(
842  IKabs(IKsqrt(((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007)))), -1);
843  if (!x1011.valid)
844  {
845  continue;
846  }
847  if (((npz * (x1011.value))) < -1 - IKFAST_SINCOS_THRESH ||
848  ((npz * (x1011.value))) > 1 + IKFAST_SINCOS_THRESH)
849  continue;
850  IkReal x1009 = IKasin((npz * (x1011.value)));
851  j5array[0] = (x1009 + (((-1.0) * x1008)));
852  sj5array[0] = IKsin(j5array[0]);
853  cj5array[0] = IKcos(j5array[0]);
854  j5array[1] = ((3.14159265358979) + (((-1.0) * x1009)) + (((-1.0) * x1008)));
855  sj5array[1] = IKsin(j5array[1]);
856  cj5array[1] = IKcos(j5array[1]);
857  if (j5array[0] > IKPI)
858  {
859  j5array[0] -= IK2PI;
860  }
861  else if (j5array[0] < -IKPI)
862  {
863  j5array[0] += IK2PI;
864  }
865  j5valid[0] = true;
866  if (j5array[1] > IKPI)
867  {
868  j5array[1] -= IK2PI;
869  }
870  else if (j5array[1] < -IKPI)
871  {
872  j5array[1] += IK2PI;
873  }
874  j5valid[1] = true;
875  for (int ij5 = 0; ij5 < 2; ++ij5)
876  {
877  if (!j5valid[ij5])
878  {
879  continue;
880  }
881  _ij5[0] = ij5;
882  _ij5[1] = -1;
883  for (int iij5 = ij5 + 1; iij5 < 2; ++iij5)
884  {
885  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
886  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
887  {
888  j5valid[iij5] = false;
889  _ij5[1] = iij5;
890  break;
891  }
892  }
893  j5 = j5array[ij5];
894  cj5 = cj5array[ij5];
895  sj5 = sj5array[ij5];
896 
897  {
898  IkReal j6eval[3];
899  IkReal x1012 = npy * npy;
900  IkReal x1013 = npx * npx;
901  IkReal x1014 = (cj5 * sj4);
902  IkReal x1015 = (cj5 * x1013);
903  IkReal x1016 = (cj5 * x1012);
904  IkReal x1017 = ((5.0) * npz * sj5);
905  IkReal x1018 = ((2.0) * npy * sj3);
906  IkReal x1019 = ((2.0) * npx * sj3);
907  j6eval[0] = (x1015 + x1016);
908  j6eval[1] = ((IKabs((((x1014 * x1019)) + ((cj4 * x1018)) + (((-1.0) * npy * x1017))))) +
909  (IKabs((((x1014 * x1018)) + (((-1.0) * cj4 * x1019)) + ((npx * x1017))))));
910  j6eval[2] = IKsign(((((5.0) * x1016)) + (((5.0) * x1015))));
911  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
912  IKabs(j6eval[2]) < 0.0000010000000000)
913  {
914  {
915  IkReal j6eval[3];
916  IkReal x1020 = npy * npy;
917  IkReal x1021 = npx * npx;
918  IkReal x1022 = ((2.0) * cj3);
919  IkReal x1023 = ((5.0) * sj5);
920  IkReal x1024 = ((2.0) * npx);
921  IkReal x1025 = ((2.0) * npy);
922  IkReal x1026 = ((5.0) * cj5 * npz);
923  IkReal x1027 = (sj3 * sj4 * sj5);
924  j6eval[0] = (((sj5 * x1021)) + ((sj5 * x1020)));
925  j6eval[1] = IKsign((((x1021 * x1023)) + ((x1020 * x1023))));
926  j6eval[2] =
927  ((IKabs((x1024 + ((npx * x1022)) + (((-1.0) * npx * x1026)) + ((x1025 * x1027))))) +
928  (IKabs((((npy * x1026)) + (((-1.0) * x1025)) + (((-1.0) * npy * x1022)) +
929  ((x1024 * x1027))))));
930  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
931  IKabs(j6eval[2]) < 0.0000010000000000)
932  {
933  {
934  IkReal j6eval[3];
935  IkReal x1028 = npx * npx;
936  IkReal x1029 = npy * npy;
937  IkReal x1030 = ((2.0) * sj5);
938  IkReal x1031 = ((2.0) * sj3);
939  IkReal x1032 = (cj4 * cj5);
940  j6eval[0] = (x1029 + x1028);
941  j6eval[1] = ((IKabs(((((-1.0) * npx * x1031 * x1032)) + ((npy * sj4 * x1031)) +
942  ((npx * x1030)) + ((cj3 * npx * x1030))))) +
943  (IKabs((((npx * sj4 * x1031)) + ((npy * x1031 * x1032)) +
944  (((-1.0) * cj3 * npy * x1030)) + (((-1.0) * npy * x1030))))));
945  j6eval[2] = IKsign(((((5.0) * x1028)) + (((5.0) * x1029))));
946  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
947  IKabs(j6eval[2]) < 0.0000010000000000)
948  {
949  {
950  IkReal evalcond[2];
951  bool bgotonextstatement = true;
952  do
953  {
954  evalcond[0] = ((-3.14159265358979) +
955  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
956  6.28318530717959)));
957  evalcond[1] = pp;
958  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
959  IKabs(evalcond[1]) < 0.0000050000000000)
960  {
961  bgotonextstatement = false;
962  {
963  IkReal j6eval[1];
964  sj3 = 0;
965  cj3 = -1.0;
966  j3 = 3.14159265358979;
967  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
968  if (IKabs(j6eval[0]) < 0.0000010000000000)
969  {
970  {
971  IkReal j6eval[1];
972  sj3 = 0;
973  cj3 = -1.0;
974  j3 = 3.14159265358979;
975  j6eval[0] = ((IKabs((npx * sj5))) + (IKabs((npy * sj5))));
976  if (IKabs(j6eval[0]) < 0.0000010000000000)
977  {
978  {
979  IkReal j6eval[1];
980  sj3 = 0;
981  cj3 = -1.0;
982  j3 = 3.14159265358979;
983  j6eval[0] = ((IKabs((cj5 * npx))) + (IKabs((cj5 * npy))));
984  if (IKabs(j6eval[0]) < 0.0000010000000000)
985  {
986  {
987  IkReal evalcond[1];
988  bool bgotonextstatement = true;
989  do
990  {
991  evalcond[0] = ((-3.14159265358979) +
992  (IKfmod(((3.14159265358979) +
993  (IKabs(((-1.5707963267949) + j5)))),
994  6.28318530717959)));
995  if (IKabs(evalcond[0]) < 0.0000050000000000)
996  {
997  bgotonextstatement = false;
998  {
999  IkReal j6eval[1];
1000  sj3 = 0;
1001  cj3 = -1.0;
1002  j3 = 3.14159265358979;
1003  sj5 = 1.0;
1004  cj5 = 0;
1005  j5 = 1.5707963267949;
1006  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1007  if (IKabs(j6eval[0]) < 0.0000010000000000)
1008  {
1009  continue; // no branches [j6]
1010  }
1011  else
1012  {
1013  {
1014  IkReal j6array[2], cj6array[2], sj6array[2];
1015  bool j6valid[2] = { false };
1016  _nj6 = 2;
1017  CheckValue<IkReal> x1034 =
1018  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1019  IkReal(npy),
1020  IKFAST_ATAN2_MAGTHRESH);
1021  if (!x1034.valid)
1022  {
1023  continue;
1024  }
1025  IkReal x1033 = x1034.value;
1026  j6array[0] = ((-1.0) * x1033);
1027  sj6array[0] = IKsin(j6array[0]);
1028  cj6array[0] = IKcos(j6array[0]);
1029  j6array[1] = ((3.14159265358979) + (((-1.0) * x1033)));
1030  sj6array[1] = IKsin(j6array[1]);
1031  cj6array[1] = IKcos(j6array[1]);
1032  if (j6array[0] > IKPI)
1033  {
1034  j6array[0] -= IK2PI;
1035  }
1036  else if (j6array[0] < -IKPI)
1037  {
1038  j6array[0] += IK2PI;
1039  }
1040  j6valid[0] = true;
1041  if (j6array[1] > IKPI)
1042  {
1043  j6array[1] -= IK2PI;
1044  }
1045  else if (j6array[1] < -IKPI)
1046  {
1047  j6array[1] += IK2PI;
1048  }
1049  j6valid[1] = true;
1050  for (int ij6 = 0; ij6 < 2; ++ij6)
1051  {
1052  if (!j6valid[ij6])
1053  {
1054  continue;
1055  }
1056  _ij6[0] = ij6;
1057  _ij6[1] = -1;
1058  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1059  {
1060  if (j6valid[iij6] &&
1061  IKabs(cj6array[ij6] - cj6array[iij6]) <
1062  IKFAST_SOLUTION_THRESH &&
1063  IKabs(sj6array[ij6] - sj6array[iij6]) <
1064  IKFAST_SOLUTION_THRESH)
1065  {
1066  j6valid[iij6] = false;
1067  _ij6[1] = iij6;
1068  break;
1069  }
1070  }
1071  j6 = j6array[ij6];
1072  cj6 = cj6array[ij6];
1073  sj6 = sj6array[ij6];
1074  {
1075  IkReal evalcond[1];
1076  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1077  (((-1.0) * npx * (IKsin(j6)))));
1078  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1079  {
1080  continue;
1081  }
1082  }
1083 
1084  rotationfunction0(solutions);
1085  }
1086  }
1087  }
1088  }
1089  }
1090  } while (0);
1091  if (bgotonextstatement)
1092  {
1093  bool bgotonextstatement = true;
1094  do
1095  {
1096  evalcond[0] = ((-3.14159265358979) +
1097  (IKfmod(((3.14159265358979) +
1098  (IKabs(((1.5707963267949) + j5)))),
1099  6.28318530717959)));
1100  if (IKabs(evalcond[0]) < 0.0000050000000000)
1101  {
1102  bgotonextstatement = false;
1103  {
1104  IkReal j6eval[1];
1105  sj3 = 0;
1106  cj3 = -1.0;
1107  j3 = 3.14159265358979;
1108  sj5 = -1.0;
1109  cj5 = 0;
1110  j5 = -1.5707963267949;
1111  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1112  if (IKabs(j6eval[0]) < 0.0000010000000000)
1113  {
1114  continue; // no branches [j6]
1115  }
1116  else
1117  {
1118  {
1119  IkReal j6array[2], cj6array[2], sj6array[2];
1120  bool j6valid[2] = { false };
1121  _nj6 = 2;
1122  CheckValue<IkReal> x1036 =
1123  IKatan2WithCheck(IkReal(npx),
1124  IkReal(((-1.0) * npy)),
1125  IKFAST_ATAN2_MAGTHRESH);
1126  if (!x1036.valid)
1127  {
1128  continue;
1129  }
1130  IkReal x1035 = x1036.value;
1131  j6array[0] = ((-1.0) * x1035);
1132  sj6array[0] = IKsin(j6array[0]);
1133  cj6array[0] = IKcos(j6array[0]);
1134  j6array[1] =
1135  ((3.14159265358979) + (((-1.0) * x1035)));
1136  sj6array[1] = IKsin(j6array[1]);
1137  cj6array[1] = IKcos(j6array[1]);
1138  if (j6array[0] > IKPI)
1139  {
1140  j6array[0] -= IK2PI;
1141  }
1142  else if (j6array[0] < -IKPI)
1143  {
1144  j6array[0] += IK2PI;
1145  }
1146  j6valid[0] = true;
1147  if (j6array[1] > IKPI)
1148  {
1149  j6array[1] -= IK2PI;
1150  }
1151  else if (j6array[1] < -IKPI)
1152  {
1153  j6array[1] += IK2PI;
1154  }
1155  j6valid[1] = true;
1156  for (int ij6 = 0; ij6 < 2; ++ij6)
1157  {
1158  if (!j6valid[ij6])
1159  {
1160  continue;
1161  }
1162  _ij6[0] = ij6;
1163  _ij6[1] = -1;
1164  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1165  {
1166  if (j6valid[iij6] &&
1167  IKabs(cj6array[ij6] - cj6array[iij6]) <
1168  IKFAST_SOLUTION_THRESH &&
1169  IKabs(sj6array[ij6] - sj6array[iij6]) <
1170  IKFAST_SOLUTION_THRESH)
1171  {
1172  j6valid[iij6] = false;
1173  _ij6[1] = iij6;
1174  break;
1175  }
1176  }
1177  j6 = j6array[ij6];
1178  cj6 = cj6array[ij6];
1179  sj6 = sj6array[ij6];
1180  {
1181  IkReal evalcond[1];
1182  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1183  (((-1.0) * npx * (IKsin(j6)))));
1184  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1185  {
1186  continue;
1187  }
1188  }
1189 
1190  rotationfunction0(solutions);
1191  }
1192  }
1193  }
1194  }
1195  }
1196  } while (0);
1197  if (bgotonextstatement)
1198  {
1199  bool bgotonextstatement = true;
1200  do
1201  {
1202  evalcond[0] = ((-3.14159265358979) +
1203  (IKfmod(((3.14159265358979) + (IKabs(j5))),
1204  6.28318530717959)));
1205  if (IKabs(evalcond[0]) < 0.0000050000000000)
1206  {
1207  bgotonextstatement = false;
1208  {
1209  IkReal j6eval[1];
1210  sj3 = 0;
1211  cj3 = -1.0;
1212  j3 = 3.14159265358979;
1213  sj5 = 0;
1214  cj5 = 1.0;
1215  j5 = 0;
1216  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1217  if (IKabs(j6eval[0]) < 0.0000010000000000)
1218  {
1219  continue; // no branches [j6]
1220  }
1221  else
1222  {
1223  {
1224  IkReal j6array[2], cj6array[2], sj6array[2];
1225  bool j6valid[2] = { false };
1226  _nj6 = 2;
1227  CheckValue<IkReal> x1038 =
1228  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1229  IkReal(npy),
1230  IKFAST_ATAN2_MAGTHRESH);
1231  if (!x1038.valid)
1232  {
1233  continue;
1234  }
1235  IkReal x1037 = x1038.value;
1236  j6array[0] = ((-1.0) * x1037);
1237  sj6array[0] = IKsin(j6array[0]);
1238  cj6array[0] = IKcos(j6array[0]);
1239  j6array[1] =
1240  ((3.14159265358979) + (((-1.0) * x1037)));
1241  sj6array[1] = IKsin(j6array[1]);
1242  cj6array[1] = IKcos(j6array[1]);
1243  if (j6array[0] > IKPI)
1244  {
1245  j6array[0] -= IK2PI;
1246  }
1247  else if (j6array[0] < -IKPI)
1248  {
1249  j6array[0] += IK2PI;
1250  }
1251  j6valid[0] = true;
1252  if (j6array[1] > IKPI)
1253  {
1254  j6array[1] -= IK2PI;
1255  }
1256  else if (j6array[1] < -IKPI)
1257  {
1258  j6array[1] += IK2PI;
1259  }
1260  j6valid[1] = true;
1261  for (int ij6 = 0; ij6 < 2; ++ij6)
1262  {
1263  if (!j6valid[ij6])
1264  {
1265  continue;
1266  }
1267  _ij6[0] = ij6;
1268  _ij6[1] = -1;
1269  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1270  {
1271  if (j6valid[iij6] &&
1272  IKabs(cj6array[ij6] - cj6array[iij6]) <
1273  IKFAST_SOLUTION_THRESH &&
1274  IKabs(sj6array[ij6] - sj6array[iij6]) <
1275  IKFAST_SOLUTION_THRESH)
1276  {
1277  j6valid[iij6] = false;
1278  _ij6[1] = iij6;
1279  break;
1280  }
1281  }
1282  j6 = j6array[ij6];
1283  cj6 = cj6array[ij6];
1284  sj6 = sj6array[ij6];
1285  {
1286  IkReal evalcond[1];
1287  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1288  (((-1.0) * npx * (IKsin(j6)))));
1289  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1290  {
1291  continue;
1292  }
1293  }
1294 
1295  rotationfunction0(solutions);
1296  }
1297  }
1298  }
1299  }
1300  }
1301  } while (0);
1302  if (bgotonextstatement)
1303  {
1304  bool bgotonextstatement = true;
1305  do
1306  {
1307  evalcond[0] =
1308  ((-3.14159265358979) +
1309  (IKfmod(((3.14159265358979) +
1310  (IKabs(((-3.14159265358979) + j5)))),
1311  6.28318530717959)));
1312  if (IKabs(evalcond[0]) < 0.0000050000000000)
1313  {
1314  bgotonextstatement = false;
1315  {
1316  IkReal j6eval[1];
1317  sj3 = 0;
1318  cj3 = -1.0;
1319  j3 = 3.14159265358979;
1320  sj5 = 0;
1321  cj5 = -1.0;
1322  j5 = 3.14159265358979;
1323  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1324  if (IKabs(j6eval[0]) < 0.0000010000000000)
1325  {
1326  continue; // no branches [j6]
1327  }
1328  else
1329  {
1330  {
1331  IkReal j6array[2], cj6array[2], sj6array[2];
1332  bool j6valid[2] = { false };
1333  _nj6 = 2;
1334  CheckValue<IkReal> x1040 =
1335  IKatan2WithCheck(IkReal(npx),
1336  IkReal(((-1.0) * npy)),
1337  IKFAST_ATAN2_MAGTHRESH);
1338  if (!x1040.valid)
1339  {
1340  continue;
1341  }
1342  IkReal x1039 = x1040.value;
1343  j6array[0] = ((-1.0) * x1039);
1344  sj6array[0] = IKsin(j6array[0]);
1345  cj6array[0] = IKcos(j6array[0]);
1346  j6array[1] =
1347  ((3.14159265358979) + (((-1.0) * x1039)));
1348  sj6array[1] = IKsin(j6array[1]);
1349  cj6array[1] = IKcos(j6array[1]);
1350  if (j6array[0] > IKPI)
1351  {
1352  j6array[0] -= IK2PI;
1353  }
1354  else if (j6array[0] < -IKPI)
1355  {
1356  j6array[0] += IK2PI;
1357  }
1358  j6valid[0] = true;
1359  if (j6array[1] > IKPI)
1360  {
1361  j6array[1] -= IK2PI;
1362  }
1363  else if (j6array[1] < -IKPI)
1364  {
1365  j6array[1] += IK2PI;
1366  }
1367  j6valid[1] = true;
1368  for (int ij6 = 0; ij6 < 2; ++ij6)
1369  {
1370  if (!j6valid[ij6])
1371  {
1372  continue;
1373  }
1374  _ij6[0] = ij6;
1375  _ij6[1] = -1;
1376  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1377  {
1378  if (j6valid[iij6] &&
1379  IKabs(cj6array[ij6] - cj6array[iij6]) <
1380  IKFAST_SOLUTION_THRESH &&
1381  IKabs(sj6array[ij6] - sj6array[iij6]) <
1382  IKFAST_SOLUTION_THRESH)
1383  {
1384  j6valid[iij6] = false;
1385  _ij6[1] = iij6;
1386  break;
1387  }
1388  }
1389  j6 = j6array[ij6];
1390  cj6 = cj6array[ij6];
1391  sj6 = sj6array[ij6];
1392  {
1393  IkReal evalcond[1];
1394  evalcond[0] =
1395  ((((-1.0) * npy * (IKcos(j6)))) +
1396  (((-1.0) * npx * (IKsin(j6)))));
1397  if (IKabs(evalcond[0]) >
1398  IKFAST_EVALCOND_THRESH)
1399  {
1400  continue;
1401  }
1402  }
1403 
1404  rotationfunction0(solutions);
1405  }
1406  }
1407  }
1408  }
1409  }
1410  } while (0);
1411  if (bgotonextstatement)
1412  {
1413  bool bgotonextstatement = true;
1414  do
1415  {
1416  if (1)
1417  {
1418  bgotonextstatement = false;
1419  continue; // branch miss [j6]
1420  }
1421  } while (0);
1422  if (bgotonextstatement)
1423  {
1424  }
1425  }
1426  }
1427  }
1428  }
1429  }
1430  }
1431  else
1432  {
1433  {
1434  IkReal j6array[2], cj6array[2], sj6array[2];
1435  bool j6valid[2] = { false };
1436  _nj6 = 2;
1437  CheckValue<IkReal> x1042 =
1438  IKatan2WithCheck(IkReal(((-1.0) * cj5 * npx)),
1439  IkReal((cj5 * npy)),
1440  IKFAST_ATAN2_MAGTHRESH);
1441  if (!x1042.valid)
1442  {
1443  continue;
1444  }
1445  IkReal x1041 = x1042.value;
1446  j6array[0] = ((-1.0) * x1041);
1447  sj6array[0] = IKsin(j6array[0]);
1448  cj6array[0] = IKcos(j6array[0]);
1449  j6array[1] = ((3.14159265358979) + (((-1.0) * x1041)));
1450  sj6array[1] = IKsin(j6array[1]);
1451  cj6array[1] = IKcos(j6array[1]);
1452  if (j6array[0] > IKPI)
1453  {
1454  j6array[0] -= IK2PI;
1455  }
1456  else if (j6array[0] < -IKPI)
1457  {
1458  j6array[0] += IK2PI;
1459  }
1460  j6valid[0] = true;
1461  if (j6array[1] > IKPI)
1462  {
1463  j6array[1] -= IK2PI;
1464  }
1465  else if (j6array[1] < -IKPI)
1466  {
1467  j6array[1] += IK2PI;
1468  }
1469  j6valid[1] = true;
1470  for (int ij6 = 0; ij6 < 2; ++ij6)
1471  {
1472  if (!j6valid[ij6])
1473  {
1474  continue;
1475  }
1476  _ij6[0] = ij6;
1477  _ij6[1] = -1;
1478  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1479  {
1480  if (j6valid[iij6] &&
1481  IKabs(cj6array[ij6] - cj6array[iij6]) <
1482  IKFAST_SOLUTION_THRESH &&
1483  IKabs(sj6array[ij6] - sj6array[iij6]) <
1484  IKFAST_SOLUTION_THRESH)
1485  {
1486  j6valid[iij6] = false;
1487  _ij6[1] = iij6;
1488  break;
1489  }
1490  }
1491  j6 = j6array[ij6];
1492  cj6 = cj6array[ij6];
1493  sj6 = sj6array[ij6];
1494  {
1495  IkReal evalcond[3];
1496  IkReal x1043 = IKsin(j6);
1497  IkReal x1044 = IKcos(j6);
1498  IkReal x1045 = ((1.0) * npx);
1499  IkReal x1046 = (npy * x1043);
1500  evalcond[0] = (x1046 + (((-1.0) * x1044 * x1045)));
1501  evalcond[1] =
1502  ((((-1.0) * x1043 * x1045)) + (((-1.0) * npy * x1044)));
1503  evalcond[2] =
1504  (((sj5 * x1046)) + (((-1.0) * sj5 * x1044 * x1045)));
1505  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1506  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1507  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1508  {
1509  continue;
1510  }
1511  }
1512 
1513  rotationfunction0(solutions);
1514  }
1515  }
1516  }
1517  }
1518  }
1519  else
1520  {
1521  {
1522  IkReal j6array[2], cj6array[2], sj6array[2];
1523  bool j6valid[2] = { false };
1524  _nj6 = 2;
1525  CheckValue<IkReal> x1048 =
1526  IKatan2WithCheck(IkReal(((-1.0) * npx * sj5)),
1527  IkReal((npy * sj5)),
1528  IKFAST_ATAN2_MAGTHRESH);
1529  if (!x1048.valid)
1530  {
1531  continue;
1532  }
1533  IkReal x1047 = x1048.value;
1534  j6array[0] = ((-1.0) * x1047);
1535  sj6array[0] = IKsin(j6array[0]);
1536  cj6array[0] = IKcos(j6array[0]);
1537  j6array[1] = ((3.14159265358979) + (((-1.0) * x1047)));
1538  sj6array[1] = IKsin(j6array[1]);
1539  cj6array[1] = IKcos(j6array[1]);
1540  if (j6array[0] > IKPI)
1541  {
1542  j6array[0] -= IK2PI;
1543  }
1544  else if (j6array[0] < -IKPI)
1545  {
1546  j6array[0] += IK2PI;
1547  }
1548  j6valid[0] = true;
1549  if (j6array[1] > IKPI)
1550  {
1551  j6array[1] -= IK2PI;
1552  }
1553  else if (j6array[1] < -IKPI)
1554  {
1555  j6array[1] += IK2PI;
1556  }
1557  j6valid[1] = true;
1558  for (int ij6 = 0; ij6 < 2; ++ij6)
1559  {
1560  if (!j6valid[ij6])
1561  {
1562  continue;
1563  }
1564  _ij6[0] = ij6;
1565  _ij6[1] = -1;
1566  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1567  {
1568  if (j6valid[iij6] &&
1569  IKabs(cj6array[ij6] - cj6array[iij6]) <
1570  IKFAST_SOLUTION_THRESH &&
1571  IKabs(sj6array[ij6] - sj6array[iij6]) <
1572  IKFAST_SOLUTION_THRESH)
1573  {
1574  j6valid[iij6] = false;
1575  _ij6[1] = iij6;
1576  break;
1577  }
1578  }
1579  j6 = j6array[ij6];
1580  cj6 = cj6array[ij6];
1581  sj6 = sj6array[ij6];
1582  {
1583  IkReal evalcond[3];
1584  IkReal x1049 = IKsin(j6);
1585  IkReal x1050 = IKcos(j6);
1586  IkReal x1051 = ((1.0) * npx);
1587  IkReal x1052 = (npy * x1049);
1588  evalcond[0] = (x1052 + (((-1.0) * x1050 * x1051)));
1589  evalcond[1] =
1590  ((((-1.0) * npy * x1050)) + (((-1.0) * x1049 * x1051)));
1591  evalcond[2] =
1592  ((((-1.0) * cj5 * x1050 * x1051)) + ((cj5 * x1052)));
1593  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1594  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1595  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1596  {
1597  continue;
1598  }
1599  }
1600 
1601  rotationfunction0(solutions);
1602  }
1603  }
1604  }
1605  }
1606  }
1607  else
1608  {
1609  {
1610  IkReal j6array[2], cj6array[2], sj6array[2];
1611  bool j6valid[2] = { false };
1612  _nj6 = 2;
1613  CheckValue<IkReal> x1054 = IKatan2WithCheck(
1614  IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
1615  if (!x1054.valid)
1616  {
1617  continue;
1618  }
1619  IkReal x1053 = x1054.value;
1620  j6array[0] = ((-1.0) * x1053);
1621  sj6array[0] = IKsin(j6array[0]);
1622  cj6array[0] = IKcos(j6array[0]);
1623  j6array[1] = ((3.14159265358979) + (((-1.0) * x1053)));
1624  sj6array[1] = IKsin(j6array[1]);
1625  cj6array[1] = IKcos(j6array[1]);
1626  if (j6array[0] > IKPI)
1627  {
1628  j6array[0] -= IK2PI;
1629  }
1630  else if (j6array[0] < -IKPI)
1631  {
1632  j6array[0] += IK2PI;
1633  }
1634  j6valid[0] = true;
1635  if (j6array[1] > IKPI)
1636  {
1637  j6array[1] -= IK2PI;
1638  }
1639  else if (j6array[1] < -IKPI)
1640  {
1641  j6array[1] += IK2PI;
1642  }
1643  j6valid[1] = true;
1644  for (int ij6 = 0; ij6 < 2; ++ij6)
1645  {
1646  if (!j6valid[ij6])
1647  {
1648  continue;
1649  }
1650  _ij6[0] = ij6;
1651  _ij6[1] = -1;
1652  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1653  {
1654  if (j6valid[iij6] &&
1655  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
1656  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1657  {
1658  j6valid[iij6] = false;
1659  _ij6[1] = iij6;
1660  break;
1661  }
1662  }
1663  j6 = j6array[ij6];
1664  cj6 = cj6array[ij6];
1665  sj6 = sj6array[ij6];
1666  {
1667  IkReal evalcond[3];
1668  IkReal x1055 = IKsin(j6);
1669  IkReal x1056 = IKcos(j6);
1670  IkReal x1057 = ((1.0) * npx);
1671  IkReal x1058 = (npy * x1055);
1672  evalcond[0] = ((((-1.0) * npy * x1056)) + (((-1.0) * x1055 * x1057)));
1673  evalcond[1] = (((sj5 * x1058)) + (((-1.0) * sj5 * x1056 * x1057)));
1674  evalcond[2] = ((((-1.0) * cj5 * x1056 * x1057)) + ((cj5 * x1058)));
1675  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1676  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1677  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1678  {
1679  continue;
1680  }
1681  }
1682 
1683  rotationfunction0(solutions);
1684  }
1685  }
1686  }
1687  }
1688  }
1689  } while (0);
1690  if (bgotonextstatement)
1691  {
1692  bool bgotonextstatement = true;
1693  do
1694  {
1695  evalcond[0] = ((-3.14159265358979) +
1696  (IKfmod(((3.14159265358979) + (IKabs(j5))), 6.28318530717959)));
1697  if (IKabs(evalcond[0]) < 0.0000050000000000)
1698  {
1699  bgotonextstatement = false;
1700  {
1701  IkReal j6eval[3];
1702  sj5 = 0;
1703  cj5 = 1.0;
1704  j5 = 0;
1705  IkReal x1059 = npx * npx;
1706  IkReal x1060 = npy * npy;
1707  IkReal x1061 = (npy * sj3);
1708  IkReal x1062 = (npx * sj3);
1709  j6eval[0] = (x1059 + x1060);
1710  j6eval[1] = ((IKabs(((((-1.0) * cj4 * x1062)) + ((sj4 * x1061))))) +
1711  (IKabs((((cj4 * x1061)) + ((sj4 * x1062))))));
1712  j6eval[2] = IKsign(((((5.0) * x1060)) + (((5.0) * x1059))));
1713  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1714  IKabs(j6eval[1]) < 0.0000010000000000 ||
1715  IKabs(j6eval[2]) < 0.0000010000000000)
1716  {
1717  {
1718  IkReal evalcond[1];
1719  bool bgotonextstatement = true;
1720  do
1721  {
1722  evalcond[0] =
1723  ((-3.14159265358979) +
1724  (IKfmod(((3.14159265358979) + (IKabs(j3))), 6.28318530717959)));
1725  if (IKabs(evalcond[0]) < 0.0000050000000000)
1726  {
1727  bgotonextstatement = false;
1728  {
1729  IkReal j6eval[1];
1730  sj5 = 0;
1731  cj5 = 1.0;
1732  j5 = 0;
1733  sj3 = 0;
1734  cj3 = 1.0;
1735  j3 = 0;
1736  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1737  if (IKabs(j6eval[0]) < 0.0000010000000000)
1738  {
1739  continue; // no branches [j6]
1740  }
1741  else
1742  {
1743  {
1744  IkReal j6array[2], cj6array[2], sj6array[2];
1745  bool j6valid[2] = { false };
1746  _nj6 = 2;
1747  CheckValue<IkReal> x1064 =
1748  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1749  IkReal(npy),
1750  IKFAST_ATAN2_MAGTHRESH);
1751  if (!x1064.valid)
1752  {
1753  continue;
1754  }
1755  IkReal x1063 = x1064.value;
1756  j6array[0] = ((-1.0) * x1063);
1757  sj6array[0] = IKsin(j6array[0]);
1758  cj6array[0] = IKcos(j6array[0]);
1759  j6array[1] = ((3.14159265358979) + (((-1.0) * x1063)));
1760  sj6array[1] = IKsin(j6array[1]);
1761  cj6array[1] = IKcos(j6array[1]);
1762  if (j6array[0] > IKPI)
1763  {
1764  j6array[0] -= IK2PI;
1765  }
1766  else if (j6array[0] < -IKPI)
1767  {
1768  j6array[0] += IK2PI;
1769  }
1770  j6valid[0] = true;
1771  if (j6array[1] > IKPI)
1772  {
1773  j6array[1] -= IK2PI;
1774  }
1775  else if (j6array[1] < -IKPI)
1776  {
1777  j6array[1] += IK2PI;
1778  }
1779  j6valid[1] = true;
1780  for (int ij6 = 0; ij6 < 2; ++ij6)
1781  {
1782  if (!j6valid[ij6])
1783  {
1784  continue;
1785  }
1786  _ij6[0] = ij6;
1787  _ij6[1] = -1;
1788  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1789  {
1790  if (j6valid[iij6] &&
1791  IKabs(cj6array[ij6] - cj6array[iij6]) <
1792  IKFAST_SOLUTION_THRESH &&
1793  IKabs(sj6array[ij6] - sj6array[iij6]) <
1794  IKFAST_SOLUTION_THRESH)
1795  {
1796  j6valid[iij6] = false;
1797  _ij6[1] = iij6;
1798  break;
1799  }
1800  }
1801  j6 = j6array[ij6];
1802  cj6 = cj6array[ij6];
1803  sj6 = sj6array[ij6];
1804  {
1805  IkReal evalcond[1];
1806  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1807  (((-1.0) * npx * (IKsin(j6)))));
1808  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1809  {
1810  continue;
1811  }
1812  }
1813 
1814  rotationfunction0(solutions);
1815  }
1816  }
1817  }
1818  }
1819  }
1820  } while (0);
1821  if (bgotonextstatement)
1822  {
1823  bool bgotonextstatement = true;
1824  do
1825  {
1826  if (1)
1827  {
1828  bgotonextstatement = false;
1829  continue; // branch miss [j6]
1830  }
1831  } while (0);
1832  if (bgotonextstatement)
1833  {
1834  }
1835  }
1836  }
1837  }
1838  else
1839  {
1840  {
1841  IkReal j6array[1], cj6array[1], sj6array[1];
1842  bool j6valid[1] = { false };
1843  _nj6 = 1;
1844  IkReal x1065 = ((2.0) * sj3);
1845  CheckValue<IkReal> x1066 = IKatan2WithCheck(
1846  IkReal((((cj4 * npy * x1065)) + ((npx * sj4 * x1065)))),
1847  IkReal(((((-1.0) * cj4 * npx * x1065)) + ((npy * sj4 * x1065)))),
1848  IKFAST_ATAN2_MAGTHRESH);
1849  if (!x1066.valid)
1850  {
1851  continue;
1852  }
1853  CheckValue<IkReal> x1067 = IKPowWithIntegerCheck(
1854  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
1855  if (!x1067.valid)
1856  {
1857  continue;
1858  }
1859  j6array[0] = ((-1.5707963267949) + (x1066.value) +
1860  (((1.5707963267949) * (x1067.value))));
1861  sj6array[0] = IKsin(j6array[0]);
1862  cj6array[0] = IKcos(j6array[0]);
1863  if (j6array[0] > IKPI)
1864  {
1865  j6array[0] -= IK2PI;
1866  }
1867  else if (j6array[0] < -IKPI)
1868  {
1869  j6array[0] += IK2PI;
1870  }
1871  j6valid[0] = true;
1872  for (int ij6 = 0; ij6 < 1; ++ij6)
1873  {
1874  if (!j6valid[ij6])
1875  {
1876  continue;
1877  }
1878  _ij6[0] = ij6;
1879  _ij6[1] = -1;
1880  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
1881  {
1882  if (j6valid[iij6] &&
1883  IKabs(cj6array[ij6] - cj6array[iij6]) <
1884  IKFAST_SOLUTION_THRESH &&
1885  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1886  {
1887  j6valid[iij6] = false;
1888  _ij6[1] = iij6;
1889  break;
1890  }
1891  }
1892  j6 = j6array[ij6];
1893  cj6 = cj6array[ij6];
1894  sj6 = sj6array[ij6];
1895  {
1896  IkReal evalcond[2];
1897  IkReal x1068 = IKsin(j6);
1898  IkReal x1069 = IKcos(j6);
1899  IkReal x1070 = ((0.4) * sj3);
1900  IkReal x1071 = ((1.0) * x1069);
1901  evalcond[0] = ((((-1.0) * npx * x1071)) + (((-1.0) * cj4 * x1070)) +
1902  ((npy * x1068)));
1903  evalcond[1] = ((((-1.0) * npy * x1071)) + (((-1.0) * npx * x1068)) +
1904  ((sj4 * x1070)));
1905  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1906  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
1907  {
1908  continue;
1909  }
1910  }
1911 
1912  rotationfunction0(solutions);
1913  }
1914  }
1915  }
1916  }
1917  }
1918  } while (0);
1919  if (bgotonextstatement)
1920  {
1921  bool bgotonextstatement = true;
1922  do
1923  {
1924  evalcond[0] =
1925  ((-3.14159265358979) +
1926  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j5)))),
1927  6.28318530717959)));
1928  if (IKabs(evalcond[0]) < 0.0000050000000000)
1929  {
1930  bgotonextstatement = false;
1931  {
1932  IkReal j6eval[3];
1933  sj5 = 0;
1934  cj5 = -1.0;
1935  j5 = 3.14159265358979;
1936  IkReal x1072 = npx * npx;
1937  IkReal x1073 = npy * npy;
1938  IkReal x1074 = (cj4 * sj3);
1939  IkReal x1075 = (sj3 * sj4);
1940  j6eval[0] = (x1072 + x1073);
1941  j6eval[1] = ((IKabs((((npx * x1074)) + ((npy * x1075))))) +
1942  (IKabs(((((-1.0) * npy * x1074)) + ((npx * x1075))))));
1943  j6eval[2] = IKsign(((((5.0) * x1073)) + (((5.0) * x1072))));
1944  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1945  IKabs(j6eval[1]) < 0.0000010000000000 ||
1946  IKabs(j6eval[2]) < 0.0000010000000000)
1947  {
1948  {
1949  IkReal evalcond[1];
1950  bool bgotonextstatement = true;
1951  do
1952  {
1953  evalcond[0] = ((-3.14159265358979) +
1954  (IKfmod(((3.14159265358979) + (IKabs(j3))),
1955  6.28318530717959)));
1956  if (IKabs(evalcond[0]) < 0.0000050000000000)
1957  {
1958  bgotonextstatement = false;
1959  {
1960  IkReal j6eval[1];
1961  sj5 = 0;
1962  cj5 = -1.0;
1963  j5 = 3.14159265358979;
1964  sj3 = 0;
1965  cj3 = 1.0;
1966  j3 = 0;
1967  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1968  if (IKabs(j6eval[0]) < 0.0000010000000000)
1969  {
1970  continue; // no branches [j6]
1971  }
1972  else
1973  {
1974  {
1975  IkReal j6array[2], cj6array[2], sj6array[2];
1976  bool j6valid[2] = { false };
1977  _nj6 = 2;
1978  CheckValue<IkReal> x1077 =
1979  IKatan2WithCheck(IkReal(npx),
1980  IkReal(((-1.0) * npy)),
1981  IKFAST_ATAN2_MAGTHRESH);
1982  if (!x1077.valid)
1983  {
1984  continue;
1985  }
1986  IkReal x1076 = x1077.value;
1987  j6array[0] = ((-1.0) * x1076);
1988  sj6array[0] = IKsin(j6array[0]);
1989  cj6array[0] = IKcos(j6array[0]);
1990  j6array[1] = ((3.14159265358979) + (((-1.0) * x1076)));
1991  sj6array[1] = IKsin(j6array[1]);
1992  cj6array[1] = IKcos(j6array[1]);
1993  if (j6array[0] > IKPI)
1994  {
1995  j6array[0] -= IK2PI;
1996  }
1997  else if (j6array[0] < -IKPI)
1998  {
1999  j6array[0] += IK2PI;
2000  }
2001  j6valid[0] = true;
2002  if (j6array[1] > IKPI)
2003  {
2004  j6array[1] -= IK2PI;
2005  }
2006  else if (j6array[1] < -IKPI)
2007  {
2008  j6array[1] += IK2PI;
2009  }
2010  j6valid[1] = true;
2011  for (int ij6 = 0; ij6 < 2; ++ij6)
2012  {
2013  if (!j6valid[ij6])
2014  {
2015  continue;
2016  }
2017  _ij6[0] = ij6;
2018  _ij6[1] = -1;
2019  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2020  {
2021  if (j6valid[iij6] &&
2022  IKabs(cj6array[ij6] - cj6array[iij6]) <
2023  IKFAST_SOLUTION_THRESH &&
2024  IKabs(sj6array[ij6] - sj6array[iij6]) <
2025  IKFAST_SOLUTION_THRESH)
2026  {
2027  j6valid[iij6] = false;
2028  _ij6[1] = iij6;
2029  break;
2030  }
2031  }
2032  j6 = j6array[ij6];
2033  cj6 = cj6array[ij6];
2034  sj6 = sj6array[ij6];
2035  {
2036  IkReal evalcond[1];
2037  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
2038  (((-1.0) * npx * (IKsin(j6)))));
2039  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
2040  {
2041  continue;
2042  }
2043  }
2044 
2045  rotationfunction0(solutions);
2046  }
2047  }
2048  }
2049  }
2050  }
2051  } while (0);
2052  if (bgotonextstatement)
2053  {
2054  bool bgotonextstatement = true;
2055  do
2056  {
2057  if (1)
2058  {
2059  bgotonextstatement = false;
2060  continue; // branch miss [j6]
2061  }
2062  } while (0);
2063  if (bgotonextstatement)
2064  {
2065  }
2066  }
2067  }
2068  }
2069  else
2070  {
2071  {
2072  IkReal j6array[1], cj6array[1], sj6array[1];
2073  bool j6valid[1] = { false };
2074  _nj6 = 1;
2075  IkReal x1078 = ((2.0) * sj3);
2076  CheckValue<IkReal> x1079 = IKPowWithIntegerCheck(
2077  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2078  if (!x1079.valid)
2079  {
2080  continue;
2081  }
2082  CheckValue<IkReal> x1080 = IKatan2WithCheck(
2083  IkReal(((((-1.0) * cj4 * npy * x1078)) + ((npx * sj4 * x1078)))),
2084  IkReal((((cj4 * npx * x1078)) + ((npy * sj4 * x1078)))),
2085  IKFAST_ATAN2_MAGTHRESH);
2086  if (!x1080.valid)
2087  {
2088  continue;
2089  }
2090  j6array[0] = ((-1.5707963267949) +
2091  (((1.5707963267949) * (x1079.value))) + (x1080.value));
2092  sj6array[0] = IKsin(j6array[0]);
2093  cj6array[0] = IKcos(j6array[0]);
2094  if (j6array[0] > IKPI)
2095  {
2096  j6array[0] -= IK2PI;
2097  }
2098  else if (j6array[0] < -IKPI)
2099  {
2100  j6array[0] += IK2PI;
2101  }
2102  j6valid[0] = true;
2103  for (int ij6 = 0; ij6 < 1; ++ij6)
2104  {
2105  if (!j6valid[ij6])
2106  {
2107  continue;
2108  }
2109  _ij6[0] = ij6;
2110  _ij6[1] = -1;
2111  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2112  {
2113  if (j6valid[iij6] &&
2114  IKabs(cj6array[ij6] - cj6array[iij6]) <
2115  IKFAST_SOLUTION_THRESH &&
2116  IKabs(sj6array[ij6] - sj6array[iij6]) <
2117  IKFAST_SOLUTION_THRESH)
2118  {
2119  j6valid[iij6] = false;
2120  _ij6[1] = iij6;
2121  break;
2122  }
2123  }
2124  j6 = j6array[ij6];
2125  cj6 = cj6array[ij6];
2126  sj6 = sj6array[ij6];
2127  {
2128  IkReal evalcond[2];
2129  IkReal x1081 = IKsin(j6);
2130  IkReal x1082 = IKcos(j6);
2131  IkReal x1083 = ((0.4) * sj3);
2132  IkReal x1084 = ((1.0) * npy);
2133  evalcond[0] = ((((-1.0) * x1081 * x1084)) + ((npx * x1082)) +
2134  (((-1.0) * cj4 * x1083)));
2135  evalcond[1] = ((((-1.0) * x1082 * x1084)) + ((sj4 * x1083)) +
2136  (((-1.0) * npx * x1081)));
2137  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2138  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2139  {
2140  continue;
2141  }
2142  }
2143 
2144  rotationfunction0(solutions);
2145  }
2146  }
2147  }
2148  }
2149  }
2150  } while (0);
2151  if (bgotonextstatement)
2152  {
2153  bool bgotonextstatement = true;
2154  do
2155  {
2156  evalcond[0] =
2157  ((-3.14159265358979) +
2158  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j5)))),
2159  6.28318530717959)));
2160  if (IKabs(evalcond[0]) < 0.0000050000000000)
2161  {
2162  bgotonextstatement = false;
2163  {
2164  IkReal j6eval[3];
2165  sj5 = 1.0;
2166  cj5 = 0;
2167  j5 = 1.5707963267949;
2168  IkReal x1085 = npx * npx;
2169  IkReal x1086 = npy * npy;
2170  IkReal x1087 = ((2.0) * cj3);
2171  IkReal x1088 = ((2.0) * npx);
2172  IkReal x1089 = (sj3 * sj4);
2173  IkReal x1090 = ((2.0) * npy);
2174  j6eval[0] = (x1086 + x1085);
2175  j6eval[1] = ((IKabs((x1088 + ((x1089 * x1090)) + ((npx * x1087))))) +
2176  (IKabs(((((-1.0) * x1090)) + (((-1.0) * npy * x1087)) +
2177  ((x1088 * x1089))))));
2178  j6eval[2] = IKsign(((((5.0) * x1085)) + (((5.0) * x1086))));
2179  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2180  IKabs(j6eval[1]) < 0.0000010000000000 ||
2181  IKabs(j6eval[2]) < 0.0000010000000000)
2182  {
2183  continue; // no branches [j6]
2184  }
2185  else
2186  {
2187  {
2188  IkReal j6array[1], cj6array[1], sj6array[1];
2189  bool j6valid[1] = { false };
2190  _nj6 = 1;
2191  IkReal x1091 = ((2.0) * cj3);
2192  IkReal x1092 = ((2.0) * npx);
2193  IkReal x1093 = (sj3 * sj4);
2194  IkReal x1094 = ((2.0) * npy);
2195  CheckValue<IkReal> x1095 = IKatan2WithCheck(
2196  IkReal(((((-1.0) * x1094)) + ((x1092 * x1093)) +
2197  (((-1.0) * npy * x1091)))),
2198  IkReal((((npx * x1091)) + x1092 + ((x1093 * x1094)))),
2199  IKFAST_ATAN2_MAGTHRESH);
2200  if (!x1095.valid)
2201  {
2202  continue;
2203  }
2204  CheckValue<IkReal> x1096 = IKPowWithIntegerCheck(
2205  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2206  -1);
2207  if (!x1096.valid)
2208  {
2209  continue;
2210  }
2211  j6array[0] = ((-1.5707963267949) + (x1095.value) +
2212  (((1.5707963267949) * (x1096.value))));
2213  sj6array[0] = IKsin(j6array[0]);
2214  cj6array[0] = IKcos(j6array[0]);
2215  if (j6array[0] > IKPI)
2216  {
2217  j6array[0] -= IK2PI;
2218  }
2219  else if (j6array[0] < -IKPI)
2220  {
2221  j6array[0] += IK2PI;
2222  }
2223  j6valid[0] = true;
2224  for (int ij6 = 0; ij6 < 1; ++ij6)
2225  {
2226  if (!j6valid[ij6])
2227  {
2228  continue;
2229  }
2230  _ij6[0] = ij6;
2231  _ij6[1] = -1;
2232  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2233  {
2234  if (j6valid[iij6] &&
2235  IKabs(cj6array[ij6] - cj6array[iij6]) <
2236  IKFAST_SOLUTION_THRESH &&
2237  IKabs(sj6array[ij6] - sj6array[iij6]) <
2238  IKFAST_SOLUTION_THRESH)
2239  {
2240  j6valid[iij6] = false;
2241  _ij6[1] = iij6;
2242  break;
2243  }
2244  }
2245  j6 = j6array[ij6];
2246  cj6 = cj6array[ij6];
2247  sj6 = sj6array[ij6];
2248  {
2249  IkReal evalcond[2];
2250  IkReal x1097 = IKsin(j6);
2251  IkReal x1098 = IKcos(j6);
2252  IkReal x1099 = ((1.0) * npx);
2253  evalcond[0] = ((0.4) + (((0.4) * cj3)) + ((npy * x1097)) +
2254  (((-1.0) * x1098 * x1099)));
2255  evalcond[1] =
2256  ((((-1.0) * x1097 * x1099)) + (((-1.0) * npy * x1098)) +
2257  (((0.4) * sj3 * sj4)));
2258  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2259  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2260  {
2261  continue;
2262  }
2263  }
2264 
2265  rotationfunction0(solutions);
2266  }
2267  }
2268  }
2269  }
2270  }
2271  } while (0);
2272  if (bgotonextstatement)
2273  {
2274  bool bgotonextstatement = true;
2275  do
2276  {
2277  evalcond[0] =
2278  ((-3.14159265358979) +
2279  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j5)))),
2280  6.28318530717959)));
2281  if (IKabs(evalcond[0]) < 0.0000050000000000)
2282  {
2283  bgotonextstatement = false;
2284  {
2285  IkReal j6eval[3];
2286  sj5 = -1.0;
2287  cj5 = 0;
2288  j5 = -1.5707963267949;
2289  IkReal x1100 = npx * npx;
2290  IkReal x1101 = npy * npy;
2291  IkReal x1102 = ((2.0) * cj3);
2292  IkReal x1103 = ((2.0) * npx);
2293  IkReal x1104 = (sj3 * sj4);
2294  IkReal x1105 = ((2.0) * npy);
2295  j6eval[0] = (x1100 + x1101);
2296  j6eval[1] = ((IKabs(((((-1.0) * npx * x1102)) + ((x1104 * x1105)) +
2297  (((-1.0) * x1103))))) +
2298  (IKabs((x1105 + ((npy * x1102)) + ((x1103 * x1104))))));
2299  j6eval[2] = IKsign(((((5.0) * x1101)) + (((5.0) * x1100))));
2300  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2301  IKabs(j6eval[1]) < 0.0000010000000000 ||
2302  IKabs(j6eval[2]) < 0.0000010000000000)
2303  {
2304  continue; // no branches [j6]
2305  }
2306  else
2307  {
2308  {
2309  IkReal j6array[1], cj6array[1], sj6array[1];
2310  bool j6valid[1] = { false };
2311  _nj6 = 1;
2312  IkReal x1106 = ((2.0) * cj3);
2313  IkReal x1107 = ((2.0) * npx);
2314  IkReal x1108 = (sj3 * sj4);
2315  IkReal x1109 = ((2.0) * npy);
2316  CheckValue<IkReal> x1110 = IKPowWithIntegerCheck(
2317  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2318  -1);
2319  if (!x1110.valid)
2320  {
2321  continue;
2322  }
2323  CheckValue<IkReal> x1111 = IKatan2WithCheck(
2324  IkReal((x1109 + ((npy * x1106)) + ((x1107 * x1108)))),
2325  IkReal((((x1108 * x1109)) + (((-1.0) * npx * x1106)) +
2326  (((-1.0) * x1107)))),
2327  IKFAST_ATAN2_MAGTHRESH);
2328  if (!x1111.valid)
2329  {
2330  continue;
2331  }
2332  j6array[0] =
2333  ((-1.5707963267949) + (((1.5707963267949) * (x1110.value))) +
2334  (x1111.value));
2335  sj6array[0] = IKsin(j6array[0]);
2336  cj6array[0] = IKcos(j6array[0]);
2337  if (j6array[0] > IKPI)
2338  {
2339  j6array[0] -= IK2PI;
2340  }
2341  else if (j6array[0] < -IKPI)
2342  {
2343  j6array[0] += IK2PI;
2344  }
2345  j6valid[0] = true;
2346  for (int ij6 = 0; ij6 < 1; ++ij6)
2347  {
2348  if (!j6valid[ij6])
2349  {
2350  continue;
2351  }
2352  _ij6[0] = ij6;
2353  _ij6[1] = -1;
2354  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2355  {
2356  if (j6valid[iij6] &&
2357  IKabs(cj6array[ij6] - cj6array[iij6]) <
2358  IKFAST_SOLUTION_THRESH &&
2359  IKabs(sj6array[ij6] - sj6array[iij6]) <
2360  IKFAST_SOLUTION_THRESH)
2361  {
2362  j6valid[iij6] = false;
2363  _ij6[1] = iij6;
2364  break;
2365  }
2366  }
2367  j6 = j6array[ij6];
2368  cj6 = cj6array[ij6];
2369  sj6 = sj6array[ij6];
2370  {
2371  IkReal evalcond[2];
2372  IkReal x1112 = IKsin(j6);
2373  IkReal x1113 = IKcos(j6);
2374  IkReal x1114 = ((1.0) * x1112);
2375  evalcond[0] = ((0.4) + ((npx * x1113)) +
2376  (((-1.0) * npy * x1114)) + (((0.4) * cj3)));
2377  evalcond[1] =
2378  ((((-1.0) * npy * x1113)) + (((-1.0) * npx * x1114)) +
2379  (((0.4) * sj3 * sj4)));
2380  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2381  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2382  {
2383  continue;
2384  }
2385  }
2386 
2387  rotationfunction0(solutions);
2388  }
2389  }
2390  }
2391  }
2392  }
2393  } while (0);
2394  if (bgotonextstatement)
2395  {
2396  bool bgotonextstatement = true;
2397  do
2398  {
2399  if (1)
2400  {
2401  bgotonextstatement = false;
2402  continue; // branch miss [j6]
2403  }
2404  } while (0);
2405  if (bgotonextstatement)
2406  {
2407  }
2408  }
2409  }
2410  }
2411  }
2412  }
2413  }
2414  }
2415  else
2416  {
2417  {
2418  IkReal j6array[1], cj6array[1], sj6array[1];
2419  bool j6valid[1] = { false };
2420  _nj6 = 1;
2421  IkReal x1115 = ((2.0) * sj5);
2422  IkReal x1116 = ((2.0) * sj3);
2423  IkReal x1117 = (cj4 * cj5);
2424  CheckValue<IkReal> x1118 = IKatan2WithCheck(
2425  IkReal((((npy * x1116 * x1117)) + (((-1.0) * npy * x1115)) +
2426  ((npx * sj4 * x1116)) + (((-1.0) * cj3 * npy * x1115)))),
2427  IkReal((((npx * x1115)) + (((-1.0) * npx * x1116 * x1117)) +
2428  ((npy * sj4 * x1116)) + ((cj3 * npx * x1115)))),
2429  IKFAST_ATAN2_MAGTHRESH);
2430  if (!x1118.valid)
2431  {
2432  continue;
2433  }
2434  CheckValue<IkReal> x1119 = IKPowWithIntegerCheck(
2435  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2436  if (!x1119.valid)
2437  {
2438  continue;
2439  }
2440  j6array[0] =
2441  ((-1.5707963267949) + (x1118.value) + (((1.5707963267949) * (x1119.value))));
2442  sj6array[0] = IKsin(j6array[0]);
2443  cj6array[0] = IKcos(j6array[0]);
2444  if (j6array[0] > IKPI)
2445  {
2446  j6array[0] -= IK2PI;
2447  }
2448  else if (j6array[0] < -IKPI)
2449  {
2450  j6array[0] += IK2PI;
2451  }
2452  j6valid[0] = true;
2453  for (int ij6 = 0; ij6 < 1; ++ij6)
2454  {
2455  if (!j6valid[ij6])
2456  {
2457  continue;
2458  }
2459  _ij6[0] = ij6;
2460  _ij6[1] = -1;
2461  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2462  {
2463  if (j6valid[iij6] &&
2464  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2465  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2466  {
2467  j6valid[iij6] = false;
2468  _ij6[1] = iij6;
2469  break;
2470  }
2471  }
2472  j6 = j6array[ij6];
2473  cj6 = cj6array[ij6];
2474  sj6 = sj6array[ij6];
2475  {
2476  IkReal evalcond[4];
2477  IkReal x1120 = IKsin(j6);
2478  IkReal x1121 = IKcos(j6);
2479  IkReal x1122 = ((0.4) * sj3);
2480  IkReal x1123 = ((1.0) * npx);
2481  IkReal x1124 = ((0.4) * sj5);
2482  IkReal x1125 = (npy * x1120);
2483  evalcond[0] =
2484  ((((-1.0) * npy * x1121)) + ((sj4 * x1122)) + (((-1.0) * x1120 * x1123)));
2485  evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1122)) +
2486  (((-1.0) * cj5 * x1121 * x1123)) + ((cj5 * x1125)));
2487  evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * sj5 * x1121 * x1123)) +
2488  (((-1.0) * cj5 * npz)) + ((sj5 * x1125)));
2489  evalcond[3] = ((((-1.0) * cj4 * cj5 * x1122)) + x1124 + x1125 +
2490  (((-1.0) * x1121 * x1123)) + ((cj3 * x1124)));
2491  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2492  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2493  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2494  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2495  {
2496  continue;
2497  }
2498  }
2499 
2500  rotationfunction0(solutions);
2501  }
2502  }
2503  }
2504  }
2505  }
2506  else
2507  {
2508  {
2509  IkReal j6array[1], cj6array[1], sj6array[1];
2510  bool j6valid[1] = { false };
2511  _nj6 = 1;
2512  IkReal x1126 = ((2.0) * cj3);
2513  IkReal x1127 = ((5.0) * sj5);
2514  IkReal x1128 = ((2.0) * npx);
2515  IkReal x1129 = ((2.0) * npy);
2516  IkReal x1130 = ((5.0) * cj5 * npz);
2517  IkReal x1131 = (sj3 * sj4 * sj5);
2518  CheckValue<IkReal> x1132 = IKPowWithIntegerCheck(
2519  IKsign((((x1127 * (npy * npy))) + ((x1127 * (npx * npx))))), -1);
2520  if (!x1132.valid)
2521  {
2522  continue;
2523  }
2524  CheckValue<IkReal> x1133 = IKatan2WithCheck(
2525  IkReal(((((-1.0) * npy * x1126)) + ((npy * x1130)) + (((-1.0) * x1129)) +
2526  ((x1128 * x1131)))),
2527  IkReal((x1128 + ((npx * x1126)) + (((-1.0) * npx * x1130)) + ((x1129 * x1131)))),
2528  IKFAST_ATAN2_MAGTHRESH);
2529  if (!x1133.valid)
2530  {
2531  continue;
2532  }
2533  j6array[0] =
2534  ((-1.5707963267949) + (((1.5707963267949) * (x1132.value))) + (x1133.value));
2535  sj6array[0] = IKsin(j6array[0]);
2536  cj6array[0] = IKcos(j6array[0]);
2537  if (j6array[0] > IKPI)
2538  {
2539  j6array[0] -= IK2PI;
2540  }
2541  else if (j6array[0] < -IKPI)
2542  {
2543  j6array[0] += IK2PI;
2544  }
2545  j6valid[0] = true;
2546  for (int ij6 = 0; ij6 < 1; ++ij6)
2547  {
2548  if (!j6valid[ij6])
2549  {
2550  continue;
2551  }
2552  _ij6[0] = ij6;
2553  _ij6[1] = -1;
2554  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2555  {
2556  if (j6valid[iij6] &&
2557  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2558  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2559  {
2560  j6valid[iij6] = false;
2561  _ij6[1] = iij6;
2562  break;
2563  }
2564  }
2565  j6 = j6array[ij6];
2566  cj6 = cj6array[ij6];
2567  sj6 = sj6array[ij6];
2568  {
2569  IkReal evalcond[4];
2570  IkReal x1134 = IKsin(j6);
2571  IkReal x1135 = IKcos(j6);
2572  IkReal x1136 = ((0.4) * sj3);
2573  IkReal x1137 = ((1.0) * npx);
2574  IkReal x1138 = ((0.4) * sj5);
2575  IkReal x1139 = (npy * x1134);
2576  evalcond[0] =
2577  ((((-1.0) * npy * x1135)) + ((sj4 * x1136)) + (((-1.0) * x1134 * x1137)));
2578  evalcond[1] = ((((-1.0) * cj4 * x1136)) + ((npz * sj5)) +
2579  (((-1.0) * cj5 * x1135 * x1137)) + ((cj5 * x1139)));
2580  evalcond[2] = ((0.4) + (((-1.0) * sj5 * x1135 * x1137)) + (((0.4) * cj3)) +
2581  (((-1.0) * cj5 * npz)) + ((sj5 * x1139)));
2582  evalcond[3] = (x1139 + x1138 + ((cj3 * x1138)) + (((-1.0) * cj4 * cj5 * x1136)) +
2583  (((-1.0) * x1135 * x1137)));
2584  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2585  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2586  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2587  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2588  {
2589  continue;
2590  }
2591  }
2592 
2593  rotationfunction0(solutions);
2594  }
2595  }
2596  }
2597  }
2598  }
2599  else
2600  {
2601  {
2602  IkReal j6array[1], cj6array[1], sj6array[1];
2603  bool j6valid[1] = { false };
2604  _nj6 = 1;
2605  IkReal x1140 = (cj5 * sj4);
2606  IkReal x1141 = ((5.0) * cj5);
2607  IkReal x1142 = ((5.0) * npz * sj5);
2608  IkReal x1143 = ((2.0) * npy * sj3);
2609  IkReal x1144 = ((2.0) * npx * sj3);
2610  CheckValue<IkReal> x1145 = IKatan2WithCheck(
2611  IkReal((((x1140 * x1144)) + (((-1.0) * npy * x1142)) + ((cj4 * x1143)))),
2612  IkReal((((x1140 * x1143)) + (((-1.0) * cj4 * x1144)) + ((npx * x1142)))),
2613  IKFAST_ATAN2_MAGTHRESH);
2614  if (!x1145.valid)
2615  {
2616  continue;
2617  }
2618  CheckValue<IkReal> x1146 =
2619  IKPowWithIntegerCheck(IKsign((((x1141 * (npx * npx))) + ((x1141 * (npy * npy))))), -1);
2620  if (!x1146.valid)
2621  {
2622  continue;
2623  }
2624  j6array[0] = ((-1.5707963267949) + (x1145.value) + (((1.5707963267949) * (x1146.value))));
2625  sj6array[0] = IKsin(j6array[0]);
2626  cj6array[0] = IKcos(j6array[0]);
2627  if (j6array[0] > IKPI)
2628  {
2629  j6array[0] -= IK2PI;
2630  }
2631  else if (j6array[0] < -IKPI)
2632  {
2633  j6array[0] += IK2PI;
2634  }
2635  j6valid[0] = true;
2636  for (int ij6 = 0; ij6 < 1; ++ij6)
2637  {
2638  if (!j6valid[ij6])
2639  {
2640  continue;
2641  }
2642  _ij6[0] = ij6;
2643  _ij6[1] = -1;
2644  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2645  {
2646  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2647  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2648  {
2649  j6valid[iij6] = false;
2650  _ij6[1] = iij6;
2651  break;
2652  }
2653  }
2654  j6 = j6array[ij6];
2655  cj6 = cj6array[ij6];
2656  sj6 = sj6array[ij6];
2657  {
2658  IkReal evalcond[4];
2659  IkReal x1147 = IKsin(j6);
2660  IkReal x1148 = IKcos(j6);
2661  IkReal x1149 = ((0.4) * sj3);
2662  IkReal x1150 = ((1.0) * npx);
2663  IkReal x1151 = ((0.4) * sj5);
2664  IkReal x1152 = (npy * x1147);
2665  evalcond[0] = (((sj4 * x1149)) + (((-1.0) * x1147 * x1150)) + (((-1.0) * npy * x1148)));
2666  evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1149)) +
2667  (((-1.0) * cj5 * x1148 * x1150)) + ((cj5 * x1152)));
2668  evalcond[2] = ((0.4) + (((0.4) * cj3)) + ((sj5 * x1152)) + (((-1.0) * cj5 * npz)) +
2669  (((-1.0) * sj5 * x1148 * x1150)));
2670  evalcond[3] = (x1151 + x1152 + ((cj3 * x1151)) + (((-1.0) * x1148 * x1150)) +
2671  (((-1.0) * cj4 * cj5 * x1149)));
2672  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2673  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2674  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2675  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2676  {
2677  continue;
2678  }
2679  }
2680 
2681  rotationfunction0(solutions);
2682  }
2683  }
2684  }
2685  }
2686  }
2687  }
2688  }
2689  }
2690  }
2691  else
2692  {
2693  {
2694  IkReal j6array[2], cj6array[2], sj6array[2];
2695  bool j6valid[2] = { false };
2696  _nj6 = 2;
2697  CheckValue<IkReal> x1155 =
2698  IKatan2WithCheck(IkReal(((-1.0) * npy)), IkReal(((-1.0) * npx)), IKFAST_ATAN2_MAGTHRESH);
2699  if (!x1155.valid)
2700  {
2701  continue;
2702  }
2703  IkReal x1153 = ((1.0) * (x1155.value));
2704  if ((((npx * npx) + (npy * npy))) < -0.00001)
2705  continue;
2706  CheckValue<IkReal> x1156 = IKPowWithIntegerCheck(IKabs(IKsqrt(((npx * npx) + (npy * npy)))), -1);
2707  if (!x1156.valid)
2708  {
2709  continue;
2710  }
2711  if ((((0.4) * sj3 * sj4 * (x1156.value))) < -1 - IKFAST_SINCOS_THRESH ||
2712  (((0.4) * sj3 * sj4 * (x1156.value))) > 1 + IKFAST_SINCOS_THRESH)
2713  continue;
2714  IkReal x1154 = IKasin(((0.4) * sj3 * sj4 * (x1156.value)));
2715  j6array[0] = ((((-1.0) * x1153)) + (((-1.0) * x1154)));
2716  sj6array[0] = IKsin(j6array[0]);
2717  cj6array[0] = IKcos(j6array[0]);
2718  j6array[1] = ((3.14159265358979) + x1154 + (((-1.0) * x1153)));
2719  sj6array[1] = IKsin(j6array[1]);
2720  cj6array[1] = IKcos(j6array[1]);
2721  if (j6array[0] > IKPI)
2722  {
2723  j6array[0] -= IK2PI;
2724  }
2725  else if (j6array[0] < -IKPI)
2726  {
2727  j6array[0] += IK2PI;
2728  }
2729  j6valid[0] = true;
2730  if (j6array[1] > IKPI)
2731  {
2732  j6array[1] -= IK2PI;
2733  }
2734  else if (j6array[1] < -IKPI)
2735  {
2736  j6array[1] += IK2PI;
2737  }
2738  j6valid[1] = true;
2739  for (int ij6 = 0; ij6 < 2; ++ij6)
2740  {
2741  if (!j6valid[ij6])
2742  {
2743  continue;
2744  }
2745  _ij6[0] = ij6;
2746  _ij6[1] = -1;
2747  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2748  {
2749  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2750  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2751  {
2752  j6valid[iij6] = false;
2753  _ij6[1] = iij6;
2754  break;
2755  }
2756  }
2757  j6 = j6array[ij6];
2758  cj6 = cj6array[ij6];
2759  sj6 = sj6array[ij6];
2760 
2761  {
2762  IkReal j5eval[3];
2763  IkReal x1157 = (cj6 * npx);
2764  IkReal x1158 = (npy * sj6);
2765  IkReal x1159 = ((2.0) * cj3);
2766  IkReal x1160 = ((5.0) * npz);
2767  IkReal x1161 = ((0.8) * cj4 * sj3);
2768  IkReal x1162 = (cj4 * npz * sj3);
2769  j5eval[0] = (x1158 + x1162 + ((cj3 * x1158)) + (((-1.0) * cj3 * x1157)) + (((-1.0) * x1157)));
2770  j5eval[1] = ((IKabs(((-0.8) + (((-1.6) * cj3)) + ((npz * x1160)) + (((-0.8) * (cj3 * cj3)))))) +
2771  (IKabs((x1161 + ((cj3 * x1161)) + ((x1158 * x1160)) + (((-1.0) * x1157 * x1160))))));
2772  j5eval[2] = IKsign(((((-1.0) * x1157 * x1159)) + ((x1158 * x1159)) + (((2.0) * x1158)) +
2773  (((2.0) * x1162)) + (((-2.0) * x1157))));
2774  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2775  IKabs(j5eval[2]) < 0.0000010000000000)
2776  {
2777  {
2778  IkReal j5eval[3];
2779  IkReal x1163 = cj3 * cj3;
2780  IkReal x1164 = cj4 * cj4;
2781  IkReal x1165 = ((10.0) * cj3);
2782  IkReal x1166 = (npy * sj6);
2783  IkReal x1167 = (cj6 * npx);
2784  IkReal x1168 = ((4.0) * x1164);
2785  IkReal x1169 = ((10.0) * cj4 * sj3);
2786  j5eval[0] = ((1.0) + x1164 + x1163 + (((-1.0) * x1163 * x1164)) + (((2.0) * cj3)));
2787  j5eval[1] =
2788  IKsign(((4.0) + x1168 + (((8.0) * cj3)) + (((-1.0) * x1163 * x1168)) + (((4.0) * x1163))));
2789  j5eval[2] = ((IKabs((((x1165 * x1167)) + ((npz * x1169)) + (((10.0) * x1167)) +
2790  (((-10.0) * x1166)) + (((-1.0) * x1165 * x1166))))) +
2791  (IKabs((((x1166 * x1169)) + (((10.0) * npz)) + ((npz * x1165)) +
2792  (((-1.0) * x1167 * x1169))))));
2793  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2794  IKabs(j5eval[2]) < 0.0000010000000000)
2795  {
2796  {
2797  IkReal j5eval[3];
2798  IkReal x1170 = cj6 * cj6;
2799  IkReal x1171 = npy * npy;
2800  IkReal x1172 = npx * npx;
2801  IkReal x1173 = npz * npz;
2802  IkReal x1174 = ((2.0) * cj3);
2803  IkReal x1175 = (cj6 * npx);
2804  IkReal x1176 = ((2.0) * npz);
2805  IkReal x1177 = (cj4 * sj3);
2806  IkReal x1178 = ((2.0) * npy * sj6);
2807  IkReal x1179 = (x1170 * x1171);
2808  IkReal x1180 = (x1170 * x1172);
2809  j5eval[0] = (x1180 + x1173 + x1171 + (((-1.0) * x1175 * x1178)) + (((-1.0) * x1179)));
2810  j5eval[1] =
2811  ((IKabs((((x1174 * x1175)) + ((x1176 * x1177)) + (((2.0) * x1175)) +
2812  (((-1.0) * npy * sj6 * x1174)) + (((-1.0) * x1178))))) +
2813  (IKabs((x1176 + ((x1177 * x1178)) + ((npz * x1174)) + (((-2.0) * x1175 * x1177))))));
2814  j5eval[2] = IKsign(((((5.0) * x1180)) + (((-5.0) * x1179)) + (((5.0) * x1171)) +
2815  (((5.0) * x1173)) + (((-10.0) * npy * sj6 * x1175))));
2816  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2817  IKabs(j5eval[2]) < 0.0000010000000000)
2818  {
2819  {
2820  IkReal evalcond[2];
2821  bool bgotonextstatement = true;
2822  do
2823  {
2824  evalcond[0] = ((-3.14159265358979) +
2825  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
2826  6.28318530717959)));
2827  evalcond[1] = pp;
2828  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
2829  IKabs(evalcond[1]) < 0.0000050000000000)
2830  {
2831  bgotonextstatement = false;
2832  {
2833  IkReal j5eval[1];
2834  sj3 = 0;
2835  cj3 = -1.0;
2836  j3 = 3.14159265358979;
2837  j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
2838  if (IKabs(j5eval[0]) < 0.0000000100000000)
2839  {
2840  continue; // no branches [j5]
2841  }
2842  else
2843  {
2844  IkReal op[2 + 1], zeror[2];
2845  int numroots;
2846  IkReal x1181 = (npy * sj6);
2847  IkReal x1182 = (cj6 * npx);
2848  op[0] = (x1182 + (((-1.0) * x1181)));
2849  op[1] = 0;
2850  op[2] = (x1181 + (((-1.0) * x1182)));
2851  polyroots2(op, zeror, numroots);
2852  IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
2853  int numsolutions = 0;
2854  for (int ij5 = 0; ij5 < numroots; ++ij5)
2855  {
2856  IkReal htj5 = zeror[ij5];
2857  tempj5array[0] = ((2.0) * (atan(htj5)));
2858  for (int kj5 = 0; kj5 < 1; ++kj5)
2859  {
2860  j5array[numsolutions] = tempj5array[kj5];
2861  if (j5array[numsolutions] > IKPI)
2862  {
2863  j5array[numsolutions] -= IK2PI;
2864  }
2865  else if (j5array[numsolutions] < -IKPI)
2866  {
2867  j5array[numsolutions] += IK2PI;
2868  }
2869  sj5array[numsolutions] = IKsin(j5array[numsolutions]);
2870  cj5array[numsolutions] = IKcos(j5array[numsolutions]);
2871  numsolutions++;
2872  }
2873  }
2874  bool j5valid[2] = { true, true };
2875  _nj5 = 2;
2876  for (int ij5 = 0; ij5 < numsolutions; ++ij5)
2877  {
2878  if (!j5valid[ij5])
2879  {
2880  continue;
2881  }
2882  j5 = j5array[ij5];
2883  cj5 = cj5array[ij5];
2884  sj5 = sj5array[ij5];
2885  htj5 = IKtan(j5 / 2);
2886 
2887  _ij5[0] = ij5;
2888  _ij5[1] = -1;
2889  for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
2890  {
2891  if (j5valid[iij5] &&
2892  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2893  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2894  {
2895  j5valid[iij5] = false;
2896  _ij5[1] = iij5;
2897  break;
2898  }
2899  }
2900  rotationfunction0(solutions);
2901  }
2902  }
2903  }
2904  }
2905  } while (0);
2906  if (bgotonextstatement)
2907  {
2908  bool bgotonextstatement = true;
2909  do
2910  {
2911  if (1)
2912  {
2913  bgotonextstatement = false;
2914  continue; // branch miss [j5]
2915  }
2916  } while (0);
2917  if (bgotonextstatement)
2918  {
2919  }
2920  }
2921  }
2922  }
2923  else
2924  {
2925  {
2926  IkReal j5array[1], cj5array[1], sj5array[1];
2927  bool j5valid[1] = { false };
2928  _nj5 = 1;
2929  IkReal x1183 = cj6 * cj6;
2930  IkReal x1184 = npy * npy;
2931  IkReal x1185 = (npy * sj6);
2932  IkReal x1186 = ((2.0) * cj3);
2933  IkReal x1187 = (cj6 * npx);
2934  IkReal x1188 = ((2.0) * npz);
2935  IkReal x1189 = (cj4 * sj3);
2936  IkReal x1190 = ((5.0) * x1184);
2937  CheckValue<IkReal> x1191 = IKPowWithIntegerCheck(
2938  IKsign((x1190 + (((-10.0) * x1185 * x1187)) + (((5.0) * (npz * npz))) +
2939  (((-1.0) * x1183 * x1190)) + (((5.0) * x1183 * (npx * npx))))),
2940  -1);
2941  if (!x1191.valid)
2942  {
2943  continue;
2944  }
2945  CheckValue<IkReal> x1192 =
2946  IKatan2WithCheck(IkReal((((x1186 * x1187)) + ((x1188 * x1189)) + (((2.0) * x1187)) +
2947  (((-2.0) * x1185)) + (((-1.0) * x1185 * x1186)))),
2948  IkReal(((((2.0) * x1185 * x1189)) + x1188 +
2949  (((-2.0) * x1187 * x1189)) + ((npz * x1186)))),
2950  IKFAST_ATAN2_MAGTHRESH);
2951  if (!x1192.valid)
2952  {
2953  continue;
2954  }
2955  j5array[0] =
2956  ((-1.5707963267949) + (((1.5707963267949) * (x1191.value))) + (x1192.value));
2957  sj5array[0] = IKsin(j5array[0]);
2958  cj5array[0] = IKcos(j5array[0]);
2959  if (j5array[0] > IKPI)
2960  {
2961  j5array[0] -= IK2PI;
2962  }
2963  else if (j5array[0] < -IKPI)
2964  {
2965  j5array[0] += IK2PI;
2966  }
2967  j5valid[0] = true;
2968  for (int ij5 = 0; ij5 < 1; ++ij5)
2969  {
2970  if (!j5valid[ij5])
2971  {
2972  continue;
2973  }
2974  _ij5[0] = ij5;
2975  _ij5[1] = -1;
2976  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2977  {
2978  if (j5valid[iij5] &&
2979  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2980  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2981  {
2982  j5valid[iij5] = false;
2983  _ij5[1] = iij5;
2984  break;
2985  }
2986  }
2987  j5 = j5array[ij5];
2988  cj5 = cj5array[ij5];
2989  sj5 = sj5array[ij5];
2990  {
2991  IkReal evalcond[4];
2992  IkReal x1193 = IKcos(j5);
2993  IkReal x1194 = IKsin(j5);
2994  IkReal x1195 = (npy * sj6);
2995  IkReal x1196 = ((1.0) * npz);
2996  IkReal x1197 = ((1.0) * cj6 * npx);
2997  IkReal x1198 = ((0.4) * cj4 * sj3);
2998  IkReal x1199 = ((0.4) * x1194);
2999  IkReal x1200 = ((0.4) * x1193);
3000  evalcond[0] = (x1200 + ((x1194 * x1198)) + (((-1.0) * x1196)) + ((cj3 * x1200)));
3001  evalcond[1] = ((((-1.0) * x1193 * x1197)) + (((-1.0) * x1198)) + ((npz * x1194)) +
3002  ((x1193 * x1195)));
3003  evalcond[2] = ((0.4) + (((-1.0) * x1193 * x1196)) + ((x1194 * x1195)) +
3004  (((-1.0) * x1194 * x1197)) + (((0.4) * cj3)));
3005  evalcond[3] = (x1199 + x1195 + (((-1.0) * x1193 * x1198)) + (((-1.0) * x1197)) +
3006  ((cj3 * x1199)));
3007  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3008  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3009  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3010  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3011  {
3012  continue;
3013  }
3014  }
3015 
3016  rotationfunction0(solutions);
3017  }
3018  }
3019  }
3020  }
3021  }
3022  else
3023  {
3024  {
3025  IkReal j5array[1], cj5array[1], sj5array[1];
3026  bool j5valid[1] = { false };
3027  _nj5 = 1;
3028  IkReal x1201 = cj3 * cj3;
3029  IkReal x1202 = cj4 * cj4;
3030  IkReal x1203 = ((10.0) * cj3);
3031  IkReal x1204 = (npy * sj6);
3032  IkReal x1205 = (cj6 * npx);
3033  IkReal x1206 = ((10.0) * cj4 * sj3);
3034  IkReal x1207 = ((4.0) * x1202);
3035  CheckValue<IkReal> x1208 =
3036  IKPowWithIntegerCheck(IKsign(((4.0) + x1207 + (((8.0) * cj3)) + (((4.0) * x1201)) +
3037  (((-1.0) * x1201 * x1207)))),
3038  -1);
3039  if (!x1208.valid)
3040  {
3041  continue;
3042  }
3043  CheckValue<IkReal> x1209 =
3044  IKatan2WithCheck(IkReal(((((-10.0) * x1204)) + (((10.0) * x1205)) + ((npz * x1206)) +
3045  ((x1203 * x1205)) + (((-1.0) * x1203 * x1204)))),
3046  IkReal((((x1204 * x1206)) + (((10.0) * npz)) + ((npz * x1203)) +
3047  (((-1.0) * x1205 * x1206)))),
3048  IKFAST_ATAN2_MAGTHRESH);
3049  if (!x1209.valid)
3050  {
3051  continue;
3052  }
3053  j5array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x1208.value))) + (x1209.value));
3054  sj5array[0] = IKsin(j5array[0]);
3055  cj5array[0] = IKcos(j5array[0]);
3056  if (j5array[0] > IKPI)
3057  {
3058  j5array[0] -= IK2PI;
3059  }
3060  else if (j5array[0] < -IKPI)
3061  {
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;
3072  _ij5[1] = -1;
3073  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3074  {
3075  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3076  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3077  {
3078  j5valid[iij5] = false;
3079  _ij5[1] = iij5;
3080  break;
3081  }
3082  }
3083  j5 = j5array[ij5];
3084  cj5 = cj5array[ij5];
3085  sj5 = sj5array[ij5];
3086  {
3087  IkReal evalcond[4];
3088  IkReal x1210 = IKcos(j5);
3089  IkReal x1211 = IKsin(j5);
3090  IkReal x1212 = (npy * sj6);
3091  IkReal x1213 = ((1.0) * npz);
3092  IkReal x1214 = ((1.0) * cj6 * npx);
3093  IkReal x1215 = ((0.4) * cj4 * sj3);
3094  IkReal x1216 = ((0.4) * x1211);
3095  IkReal x1217 = ((0.4) * x1210);
3096  evalcond[0] = (x1217 + ((cj3 * x1217)) + ((x1211 * x1215)) + (((-1.0) * x1213)));
3097  evalcond[1] = (((x1210 * x1212)) + ((npz * x1211)) + (((-1.0) * x1210 * x1214)) +
3098  (((-1.0) * x1215)));
3099  evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * x1210 * x1213)) +
3100  (((-1.0) * x1211 * x1214)) + ((x1211 * x1212)));
3101  evalcond[3] =
3102  (x1212 + x1216 + (((-1.0) * x1210 * x1215)) + ((cj3 * x1216)) + (((-1.0) * x1214)));
3103  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3104  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3105  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3106  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3107  {
3108  continue;
3109  }
3110  }
3111 
3112  rotationfunction0(solutions);
3113  }
3114  }
3115  }
3116  }
3117  }
3118  else
3119  {
3120  {
3121  IkReal j5array[1], cj5array[1], sj5array[1];
3122  bool j5valid[1] = { false };
3123  _nj5 = 1;
3124  IkReal x1218 = (npy * sj6);
3125  IkReal x1219 = (cj6 * npx);
3126  IkReal x1220 = ((2.0) * cj3);
3127  IkReal x1221 = ((5.0) * npz);
3128  IkReal x1222 = ((0.8) * cj4 * sj3);
3129  CheckValue<IkReal> x1223 = IKatan2WithCheck(
3130  IkReal(((-0.8) + ((npz * x1221)) + (((-1.6) * cj3)) + (((-0.8) * (cj3 * cj3))))),
3131  IkReal((x1222 + ((cj3 * x1222)) + ((x1218 * x1221)) + (((-1.0) * x1219 * x1221)))),
3132  IKFAST_ATAN2_MAGTHRESH);
3133  if (!x1223.valid)
3134  {
3135  continue;
3136  }
3137  CheckValue<IkReal> x1224 = IKPowWithIntegerCheck(
3138  IKsign((((x1218 * x1220)) + (((2.0) * x1218)) + (((-1.0) * x1219 * x1220)) +
3139  (((-2.0) * x1219)) + (((2.0) * cj4 * npz * sj3)))),
3140  -1);
3141  if (!x1224.valid)
3142  {
3143  continue;
3144  }
3145  j5array[0] = ((-1.5707963267949) + (x1223.value) + (((1.5707963267949) * (x1224.value))));
3146  sj5array[0] = IKsin(j5array[0]);
3147  cj5array[0] = IKcos(j5array[0]);
3148  if (j5array[0] > IKPI)
3149  {
3150  j5array[0] -= IK2PI;
3151  }
3152  else if (j5array[0] < -IKPI)
3153  {
3154  j5array[0] += IK2PI;
3155  }
3156  j5valid[0] = true;
3157  for (int ij5 = 0; ij5 < 1; ++ij5)
3158  {
3159  if (!j5valid[ij5])
3160  {
3161  continue;
3162  }
3163  _ij5[0] = ij5;
3164  _ij5[1] = -1;
3165  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3166  {
3167  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3168  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3169  {
3170  j5valid[iij5] = false;
3171  _ij5[1] = iij5;
3172  break;
3173  }
3174  }
3175  j5 = j5array[ij5];
3176  cj5 = cj5array[ij5];
3177  sj5 = sj5array[ij5];
3178  {
3179  IkReal evalcond[4];
3180  IkReal x1225 = IKcos(j5);
3181  IkReal x1226 = IKsin(j5);
3182  IkReal x1227 = (npy * sj6);
3183  IkReal x1228 = ((1.0) * npz);
3184  IkReal x1229 = ((1.0) * cj6 * npx);
3185  IkReal x1230 = ((0.4) * cj4 * sj3);
3186  IkReal x1231 = ((0.4) * x1226);
3187  IkReal x1232 = ((0.4) * x1225);
3188  evalcond[0] = (((cj3 * x1232)) + x1232 + (((-1.0) * x1228)) + ((x1226 * x1230)));
3189  evalcond[1] =
3190  (((npz * x1226)) + (((-1.0) * x1225 * x1229)) + (((-1.0) * x1230)) + ((x1225 * x1227)));
3191  evalcond[2] = ((0.4) + (((-1.0) * x1225 * x1228)) + (((0.4) * cj3)) +
3192  (((-1.0) * x1226 * x1229)) + ((x1226 * x1227)));
3193  evalcond[3] =
3194  (((cj3 * x1231)) + x1231 + x1227 + (((-1.0) * x1225 * x1230)) + (((-1.0) * x1229)));
3195  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3196  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3197  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3198  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3199  {
3200  continue;
3201  }
3202  }
3203 
3204  rotationfunction0(solutions);
3205  }
3206  }
3207  }
3208  }
3209  }
3210  }
3211  }
3212  }
3213  }
3214  }
3215  }
3216  return solutions.GetNumSolutions() > 0;
3217  }
3218  inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
3219  {
3220  for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
3221  {
3222  IkReal x76 = (r02 * sj5);
3223  IkReal x77 = ((1.0) * cj5);
3224  IkReal x78 = ((1.0) * sj6);
3225  IkReal x79 = ((1.0) * cj4);
3226  IkReal x80 = ((1.0) * cj3);
3227  IkReal x81 = ((1.0) * sj4);
3228  IkReal x82 = ((1.0) * cj6);
3229  IkReal x83 = ((((-1.0) * r01 * x78)) + ((cj6 * r00)));
3230  IkReal x84 = (((cj6 * r10)) + (((-1.0) * r11 * x78)));
3231  IkReal x85 = ((((-1.0) * r21 * x78)) + ((cj6 * r20)));
3232  IkReal x86 = ((((-1.0) * r00 * x78)) + (((-1.0) * r01 * x82)));
3233  IkReal x87 = ((((-1.0) * r11 * x82)) + (((-1.0) * r10 * x78)));
3234  IkReal x88 = ((((-1.0) * r20 * x78)) + (((-1.0) * r21 * x82)));
3235  IkReal x89 = (((sj5 * x83)) + ((cj5 * r02)));
3236  IkReal x90 = ((((-1.0) * x77 * x83)) + x76);
3237  IkReal x91 = ((((-1.0) * x77 * x84)) + ((r12 * sj5)));
3238  IkReal x92 = (((cj5 * r12)) + ((sj5 * x84)));
3239  IkReal x93 = (((r22 * sj5)) + (((-1.0) * x77 * x85)));
3240  IkReal x94 = (((cj5 * r22)) + ((sj5 * x85)));
3241  IkReal x95 = (sj4 * x86);
3242  IkReal x96 = ((1.0) * x88);
3243  IkReal x97 = ((((-1.0) * x81 * x87)) + ((cj4 * x91)));
3244  IkReal x98 = ((((-1.0) * x81 * x88)) + ((cj4 * x93)));
3245  new_r00 = ((((-1.0) * x80 * ((((cj4 * (((((-1.0) * cj5 * x83)) + x76)))) + (((-1.0) * x95)))))) + ((sj3 * x89)));
3246  new_r01 = ((((-1.0) * x79 * x86)) + (((-1.0) * x81 * x90)));
3247  new_r02 = (((cj3 * x89)) + ((sj3 * (((((-1.0) * x81 * x86)) + ((cj4 * x90)))))));
3248  new_r10 = (((sj3 * x92)) + (((-1.0) * x80 * x97)));
3249  new_r11 = ((((-1.0) * x79 * x87)) + (((-1.0) * x81 * x91)));
3250  new_r12 = (((sj3 * x97)) + ((cj3 * x92)));
3251  new_r20 = (((sj3 * x94)) + (((-1.0) * x80 * x98)));
3252  new_r21 = ((((-1.0) * x79 * x88)) + (((-1.0) * x81 * x93)));
3253  new_r22 = (((sj3 * x98)) + ((cj3 * x94)));
3254  {
3255  IkReal j1array[2], cj1array[2], sj1array[2];
3256  bool j1valid[2] = { false };
3257  _nj1 = 2;
3258  cj1array[0] = new_r22;
3259  if (cj1array[0] >= -1 - IKFAST_SINCOS_THRESH && cj1array[0] <= 1 + IKFAST_SINCOS_THRESH)
3260  {
3261  j1valid[0] = j1valid[1] = true;
3262  j1array[0] = IKacos(cj1array[0]);
3263  sj1array[0] = IKsin(j1array[0]);
3264  cj1array[1] = cj1array[0];
3265  j1array[1] = -j1array[0];
3266  sj1array[1] = -sj1array[0];
3267  }
3268  else if (isnan(cj1array[0]))
3269  {
3270  // probably any value will work
3271  j1valid[0] = true;
3272  cj1array[0] = 1;
3273  sj1array[0] = 0;
3274  j1array[0] = 0;
3275  }
3276  for (int ij1 = 0; ij1 < 2; ++ij1)
3277  {
3278  if (!j1valid[ij1])
3279  {
3280  continue;
3281  }
3282  _ij1[0] = ij1;
3283  _ij1[1] = -1;
3284  for (int iij1 = ij1 + 1; iij1 < 2; ++iij1)
3285  {
3286  if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
3287  IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
3288  {
3289  j1valid[iij1] = false;
3290  _ij1[1] = iij1;
3291  break;
3292  }
3293  }
3294  j1 = j1array[ij1];
3295  cj1 = cj1array[ij1];
3296  sj1 = sj1array[ij1];
3297 
3298  {
3299  IkReal j0eval[3];
3300  j0eval[0] = sj1;
3301  j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3302  j0eval[2] = IKsign(sj1);
3303  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
3304  IKabs(j0eval[2]) < 0.0000010000000000)
3305  {
3306  {
3307  IkReal j2eval[3];
3308  j2eval[0] = sj1;
3309  j2eval[1] = IKsign(sj1);
3310  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
3311  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
3312  IKabs(j2eval[2]) < 0.0000010000000000)
3313  {
3314  {
3315  IkReal j0eval[2];
3316  j0eval[0] = new_r12;
3317  j0eval[1] = sj1;
3318  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
3319  {
3320  {
3321  IkReal evalcond[5];
3322  bool bgotonextstatement = true;
3323  do
3324  {
3325  evalcond[0] =
3326  ((-3.14159265358979) + (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
3327  evalcond[1] = new_r21;
3328  evalcond[2] = new_r02;
3329  evalcond[3] = new_r12;
3330  evalcond[4] = new_r20;
3331  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3332  IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3333  IKabs(evalcond[4]) < 0.0000050000000000)
3334  {
3335  bgotonextstatement = false;
3336  IkReal j2mul = 1;
3337  j2 = 0;
3338  j0mul = -1.0;
3339  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
3340  IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3341  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r00)) - 1) <=
3342  IKFAST_SINCOS_THRESH)
3343  continue;
3344  j0 = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
3345  {
3346  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3347  vinfos[0].jointtype = 1;
3348  vinfos[0].foffset = j0;
3349  vinfos[0].fmul = j0mul;
3350  vinfos[0].freeind = 0;
3351  vinfos[0].maxsolutions = 0;
3352  vinfos[1].jointtype = 1;
3353  vinfos[1].foffset = j1;
3354  vinfos[1].indices[0] = _ij1[0];
3355  vinfos[1].indices[1] = _ij1[1];
3356  vinfos[1].maxsolutions = _nj1;
3357  vinfos[2].jointtype = 1;
3358  vinfos[2].foffset = j2;
3359  vinfos[2].fmul = j2mul;
3360  vinfos[2].freeind = 0;
3361  vinfos[2].maxsolutions = 0;
3362  vinfos[3].jointtype = 1;
3363  vinfos[3].foffset = j3;
3364  vinfos[3].indices[0] = _ij3[0];
3365  vinfos[3].indices[1] = _ij3[1];
3366  vinfos[3].maxsolutions = _nj3;
3367  vinfos[4].jointtype = 1;
3368  vinfos[4].foffset = j4;
3369  vinfos[4].indices[0] = _ij4[0];
3370  vinfos[4].indices[1] = _ij4[1];
3371  vinfos[4].maxsolutions = _nj4;
3372  vinfos[5].jointtype = 1;
3373  vinfos[5].foffset = j5;
3374  vinfos[5].indices[0] = _ij5[0];
3375  vinfos[5].indices[1] = _ij5[1];
3376  vinfos[5].maxsolutions = _nj5;
3377  vinfos[6].jointtype = 1;
3378  vinfos[6].foffset = j6;
3379  vinfos[6].indices[0] = _ij6[0];
3380  vinfos[6].indices[1] = _ij6[1];
3381  vinfos[6].maxsolutions = _nj6;
3382  std::vector<int> vfree(1);
3383  vfree[0] = 2;
3384  solutions.AddSolution(vinfos, vfree);
3385  }
3386  }
3387  } while (0);
3388  if (bgotonextstatement)
3389  {
3390  bool bgotonextstatement = true;
3391  do
3392  {
3393  evalcond[0] = ((-3.14159265358979) +
3394  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
3395  6.28318530717959)));
3396  evalcond[1] = new_r21;
3397  evalcond[2] = new_r02;
3398  evalcond[3] = new_r12;
3399  evalcond[4] = new_r20;
3400  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3401  IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3402  IKabs(evalcond[4]) < 0.0000050000000000)
3403  {
3404  bgotonextstatement = false;
3405  IkReal j2mul = 1;
3406  j2 = 0;
3407  j0mul = 1.0;
3408  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
3409  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
3410  IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) - 1) <= IKFAST_SINCOS_THRESH)
3411  continue;
3412  j0 = IKatan2(new_r10, ((-1.0) * new_r11));
3413  {
3414  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3415  vinfos[0].jointtype = 1;
3416  vinfos[0].foffset = j0;
3417  vinfos[0].fmul = j0mul;
3418  vinfos[0].freeind = 0;
3419  vinfos[0].maxsolutions = 0;
3420  vinfos[1].jointtype = 1;
3421  vinfos[1].foffset = j1;
3422  vinfos[1].indices[0] = _ij1[0];
3423  vinfos[1].indices[1] = _ij1[1];
3424  vinfos[1].maxsolutions = _nj1;
3425  vinfos[2].jointtype = 1;
3426  vinfos[2].foffset = j2;
3427  vinfos[2].fmul = j2mul;
3428  vinfos[2].freeind = 0;
3429  vinfos[2].maxsolutions = 0;
3430  vinfos[3].jointtype = 1;
3431  vinfos[3].foffset = j3;
3432  vinfos[3].indices[0] = _ij3[0];
3433  vinfos[3].indices[1] = _ij3[1];
3434  vinfos[3].maxsolutions = _nj3;
3435  vinfos[4].jointtype = 1;
3436  vinfos[4].foffset = j4;
3437  vinfos[4].indices[0] = _ij4[0];
3438  vinfos[4].indices[1] = _ij4[1];
3439  vinfos[4].maxsolutions = _nj4;
3440  vinfos[5].jointtype = 1;
3441  vinfos[5].foffset = j5;
3442  vinfos[5].indices[0] = _ij5[0];
3443  vinfos[5].indices[1] = _ij5[1];
3444  vinfos[5].maxsolutions = _nj5;
3445  vinfos[6].jointtype = 1;
3446  vinfos[6].foffset = j6;
3447  vinfos[6].indices[0] = _ij6[0];
3448  vinfos[6].indices[1] = _ij6[1];
3449  vinfos[6].maxsolutions = _nj6;
3450  std::vector<int> vfree(1);
3451  vfree[0] = 2;
3452  solutions.AddSolution(vinfos, vfree);
3453  }
3454  }
3455  } while (0);
3456  if (bgotonextstatement)
3457  {
3458  bool bgotonextstatement = true;
3459  do
3460  {
3461  evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3462  if (IKabs(evalcond[0]) < 0.0000050000000000)
3463  {
3464  bgotonextstatement = false;
3465  {
3466  IkReal j0eval[1];
3467  new_r02 = 0;
3468  new_r12 = 0;
3469  new_r20 = 0;
3470  new_r21 = 0;
3471  IkReal x99 = new_r22 * new_r22;
3472  IkReal x100 = ((16.0) * new_r10);
3473  IkReal x101 = ((16.0) * new_r01);
3474  IkReal x102 = ((16.0) * new_r22);
3475  IkReal x103 = ((8.0) * new_r11);
3476  IkReal x104 = ((8.0) * new_r00);
3477  IkReal x105 = (x100 * x99);
3478  IkReal x106 = (x101 * x99);
3479  j0eval[0] =
3480  ((IKabs(
3481  ((((16.0) * new_r00)) + (((-32.0) * new_r00 * x99)) + ((new_r11 * x102))))) +
3482  (IKabs(((((-1.0) * x105)) + x100))) + (IKabs(((((-1.0) * x100)) + x105))) +
3483  (IKabs(((((32.0) * new_r11)) + (((-1.0) * new_r00 * x102)) +
3484  (((-16.0) * new_r11 * x99))))) +
3485  (IKabs(((((-1.0) * x101)) + x106))) +
3486  (IKabs((((x103 * x99)) + (((-1.0) * new_r22 * x104))))) +
3487  (IKabs((((new_r22 * x103)) + (((-1.0) * x104))))) +
3488  (IKabs(((((-1.0) * x106)) + x101))));
3489  if (IKabs(j0eval[0]) < 0.0000000100000000)
3490  {
3491  continue; // no branches [j0, j2]
3492  }
3493  else
3494  {
3495  IkReal op[4 + 1], zeror[4];
3496  int numroots;
3497  IkReal j0evalpoly[1];
3498  IkReal x107 = new_r22 * new_r22;
3499  IkReal x108 = ((16.0) * new_r10);
3500  IkReal x109 = (new_r11 * new_r22);
3501  IkReal x110 = (x107 * x108);
3502  IkReal x111 = ((((8.0) * x109)) + (((-8.0) * new_r00)));
3503  op[0] = x111;
3504  op[1] = ((((-1.0) * x110)) + x108);
3505  op[2] = ((((16.0) * x109)) + (((-32.0) * new_r00 * x107)) + (((16.0) * new_r00)));
3506  op[3] = ((((-1.0) * x108)) + x110);
3507  op[4] = x111;
3508  polyroots4(op, zeror, numroots);
3509  IkReal j0array[4], cj0array[4], sj0array[4], tempj0array[1];
3510  int numsolutions = 0;
3511  for (int ij0 = 0; ij0 < numroots; ++ij0)
3512  {
3513  IkReal htj0 = zeror[ij0];
3514  tempj0array[0] = ((2.0) * (atan(htj0)));
3515  for (int kj0 = 0; kj0 < 1; ++kj0)
3516  {
3517  j0array[numsolutions] = tempj0array[kj0];
3518  if (j0array[numsolutions] > IKPI)
3519  {
3520  j0array[numsolutions] -= IK2PI;
3521  }
3522  else if (j0array[numsolutions] < -IKPI)
3523  {
3524  j0array[numsolutions] += IK2PI;
3525  }
3526  sj0array[numsolutions] = IKsin(j0array[numsolutions]);
3527  cj0array[numsolutions] = IKcos(j0array[numsolutions]);
3528  numsolutions++;
3529  }
3530  }
3531  bool j0valid[4] = { true, true, true, true };
3532  _nj0 = 4;
3533  for (int ij0 = 0; ij0 < numsolutions; ++ij0)
3534  {
3535  if (!j0valid[ij0])
3536  {
3537  continue;
3538  }
3539  j0 = j0array[ij0];
3540  cj0 = cj0array[ij0];
3541  sj0 = sj0array[ij0];
3542  htj0 = IKtan(j0 / 2);
3543 
3544  IkReal x112 = ((16.0) * new_r01);
3545  IkReal x113 = new_r22 * new_r22;
3546  IkReal x114 = (new_r00 * new_r22);
3547  IkReal x115 = ((8.0) * x114);
3548  IkReal x116 = (new_r11 * x113);
3549  IkReal x117 = (x112 * x113);
3550  IkReal x118 = ((8.0) * x116);
3551  j0evalpoly[0] =
3552  (((htj0 * (((((-1.0) * x117)) + x112)))) +
3553  (((htj0 * htj0 * htj0) * (((((-1.0) * x112)) + x117)))) + (((-1.0) * x115)) +
3554  x118 +
3555  (((htj0 * htj0) *
3556  (((((32.0) * new_r11)) + (((-16.0) * x114)) + (((-16.0) * x116)))))) +
3557  (((htj0 * htj0 * htj0 * htj0) * (((((-1.0) * x115)) + x118)))));
3558  if (IKabs(j0evalpoly[0]) > 0.0000001000000000)
3559  {
3560  continue;
3561  }
3562  _ij0[0] = ij0;
3563  _ij0[1] = -1;
3564  for (int iij0 = ij0 + 1; iij0 < numsolutions; ++iij0)
3565  {
3566  if (j0valid[iij0] &&
3567  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
3568  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
3569  {
3570  j0valid[iij0] = false;
3571  _ij0[1] = iij0;
3572  break;
3573  }
3574  }
3575  {
3576  IkReal j2eval[3];
3577  new_r02 = 0;
3578  new_r12 = 0;
3579  new_r20 = 0;
3580  new_r21 = 0;
3581  IkReal x119 = cj0 * cj0;
3582  IkReal x120 = new_r22 * new_r22;
3583  IkReal x121 = ((1.0) * cj0);
3584  IkReal x122 = (new_r22 * sj0);
3585  IkReal x123 = (x120 + x119 + (((-1.0) * x119 * x120)));
3586  j2eval[0] = x123;
3587  j2eval[1] =
3588  ((IKabs(((((-1.0) * new_r10 * x122)) + (((-1.0) * new_r11 * x121))))) +
3589  (IKabs(((((-1.0) * new_r10 * x121)) + ((new_r11 * x122))))));
3590  j2eval[2] = IKsign(x123);
3591  if (IKabs(j2eval[0]) < 0.0000010000000000 ||
3592  IKabs(j2eval[1]) < 0.0000010000000000 ||
3593  IKabs(j2eval[2]) < 0.0000010000000000)
3594  {
3595  {
3596  IkReal j2eval[1];
3597  new_r02 = 0;
3598  new_r12 = 0;
3599  new_r20 = 0;
3600  new_r21 = 0;
3601  j2eval[0] = new_r22;
3602  if (IKabs(j2eval[0]) < 0.0000010000000000)
3603  {
3604  {
3605  IkReal j2eval[1];
3606  new_r02 = 0;
3607  new_r12 = 0;
3608  new_r20 = 0;
3609  new_r21 = 0;
3610  j2eval[0] = cj0;
3611  if (IKabs(j2eval[0]) < 0.0000010000000000)
3612  {
3613  {
3614  IkReal evalcond[1];
3615  bool bgotonextstatement = true;
3616  do
3617  {
3618  evalcond[0] = ((-3.14159265358979) +
3619  (IKfmod(((3.14159265358979) +
3620  (IKabs(((-1.5707963267949) + j0)))),
3621  6.28318530717959)));
3622  if (IKabs(evalcond[0]) < 0.0000050000000000)
3623  {
3624  bgotonextstatement = false;
3625  {
3626  IkReal j2array[1], cj2array[1], sj2array[1];
3627  bool j2valid[1] = { false };
3628  _nj2 = 1;
3629  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
3630  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
3631  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
3632  IKFAST_SINCOS_THRESH)
3633  continue;
3634  j2array[0] = IKatan2(new_r00, new_r01);
3635  sj2array[0] = IKsin(j2array[0]);
3636  cj2array[0] = IKcos(j2array[0]);
3637  if (j2array[0] > IKPI)
3638  {
3639  j2array[0] -= IK2PI;
3640  }
3641  else if (j2array[0] < -IKPI)
3642  {
3643  j2array[0] += IK2PI;
3644  }
3645  j2valid[0] = true;
3646  for (int ij2 = 0; ij2 < 1; ++ij2)
3647  {
3648  if (!j2valid[ij2])
3649  {
3650  continue;
3651  }
3652  _ij2[0] = ij2;
3653  _ij2[1] = -1;
3654  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3655  {
3656  if (j2valid[iij2] &&
3657  IKabs(cj2array[ij2] - cj2array[iij2]) <
3658  IKFAST_SOLUTION_THRESH &&
3659  IKabs(sj2array[ij2] - sj2array[iij2]) <
3660  IKFAST_SOLUTION_THRESH)
3661  {
3662  j2valid[iij2] = false;
3663  _ij2[1] = iij2;
3664  break;
3665  }
3666  }
3667  j2 = j2array[ij2];
3668  cj2 = cj2array[ij2];
3669  sj2 = sj2array[ij2];
3670  {
3671  IkReal evalcond[4];
3672  IkReal x124 = IKsin(j2);
3673  IkReal x125 = IKcos(j2);
3674  evalcond[0] = x125;
3675  evalcond[1] = ((-1.0) * x124);
3676  evalcond[2] = (x124 + (((-1.0) * new_r00)));
3677  evalcond[3] = (x125 + (((-1.0) * new_r01)));
3678  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3679  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3680  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3681  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3682  {
3683  continue;
3684  }
3685  }
3686 
3687  {
3688  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3689  vinfos[0].jointtype = 1;
3690  vinfos[0].foffset = j0;
3691  vinfos[0].indices[0] = _ij0[0];
3692  vinfos[0].indices[1] = _ij0[1];
3693  vinfos[0].maxsolutions = _nj0;
3694  vinfos[1].jointtype = 1;
3695  vinfos[1].foffset = j1;
3696  vinfos[1].indices[0] = _ij1[0];
3697  vinfos[1].indices[1] = _ij1[1];
3698  vinfos[1].maxsolutions = _nj1;
3699  vinfos[2].jointtype = 1;
3700  vinfos[2].foffset = j2;
3701  vinfos[2].indices[0] = _ij2[0];
3702  vinfos[2].indices[1] = _ij2[1];
3703  vinfos[2].maxsolutions = _nj2;
3704  vinfos[3].jointtype = 1;
3705  vinfos[3].foffset = j3;
3706  vinfos[3].indices[0] = _ij3[0];
3707  vinfos[3].indices[1] = _ij3[1];
3708  vinfos[3].maxsolutions = _nj3;
3709  vinfos[4].jointtype = 1;
3710  vinfos[4].foffset = j4;
3711  vinfos[4].indices[0] = _ij4[0];
3712  vinfos[4].indices[1] = _ij4[1];
3713  vinfos[4].maxsolutions = _nj4;
3714  vinfos[5].jointtype = 1;
3715  vinfos[5].foffset = j5;
3716  vinfos[5].indices[0] = _ij5[0];
3717  vinfos[5].indices[1] = _ij5[1];
3718  vinfos[5].maxsolutions = _nj5;
3719  vinfos[6].jointtype = 1;
3720  vinfos[6].foffset = j6;
3721  vinfos[6].indices[0] = _ij6[0];
3722  vinfos[6].indices[1] = _ij6[1];
3723  vinfos[6].maxsolutions = _nj6;
3724  std::vector<int> vfree(0);
3725  solutions.AddSolution(vinfos, vfree);
3726  }
3727  }
3728  }
3729  }
3730  } while (0);
3731  if (bgotonextstatement)
3732  {
3733  bool bgotonextstatement = true;
3734  do
3735  {
3736  evalcond[0] = ((-3.14159265358979) +
3737  (IKfmod(((3.14159265358979) +
3738  (IKabs(((1.5707963267949) + j0)))),
3739  6.28318530717959)));
3740  if (IKabs(evalcond[0]) < 0.0000050000000000)
3741  {
3742  bgotonextstatement = false;
3743  {
3744  IkReal j2array[1], cj2array[1], sj2array[1];
3745  bool j2valid[1] = { false };
3746  _nj2 = 1;
3747  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3748  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
3749  IKabs(IKsqr(((-1.0) * new_r00)) +
3750  IKsqr(((-1.0) * new_r01)) - 1) <=
3751  IKFAST_SINCOS_THRESH)
3752  continue;
3753  j2array[0] =
3754  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
3755  sj2array[0] = IKsin(j2array[0]);
3756  cj2array[0] = IKcos(j2array[0]);
3757  if (j2array[0] > IKPI)
3758  {
3759  j2array[0] -= IK2PI;
3760  }
3761  else if (j2array[0] < -IKPI)
3762  {
3763  j2array[0] += IK2PI;
3764  }
3765  j2valid[0] = true;
3766  for (int ij2 = 0; ij2 < 1; ++ij2)
3767  {
3768  if (!j2valid[ij2])
3769  {
3770  continue;
3771  }
3772  _ij2[0] = ij2;
3773  _ij2[1] = -1;
3774  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3775  {
3776  if (j2valid[iij2] &&
3777  IKabs(cj2array[ij2] - cj2array[iij2]) <
3778  IKFAST_SOLUTION_THRESH &&
3779  IKabs(sj2array[ij2] - sj2array[iij2]) <
3780  IKFAST_SOLUTION_THRESH)
3781  {
3782  j2valid[iij2] = false;
3783  _ij2[1] = iij2;
3784  break;
3785  }
3786  }
3787  j2 = j2array[ij2];
3788  cj2 = cj2array[ij2];
3789  sj2 = sj2array[ij2];
3790  {
3791  IkReal evalcond[4];
3792  IkReal x126 = IKcos(j2);
3793  IkReal x127 = IKsin(j2);
3794  evalcond[0] = x126;
3795  evalcond[1] = (x127 + new_r00);
3796  evalcond[2] = (x126 + new_r01);
3797  evalcond[3] = ((-1.0) * x127);
3798  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3799  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3800  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3801  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3802  {
3803  continue;
3804  }
3805  }
3806 
3807  {
3808  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3809  vinfos[0].jointtype = 1;
3810  vinfos[0].foffset = j0;
3811  vinfos[0].indices[0] = _ij0[0];
3812  vinfos[0].indices[1] = _ij0[1];
3813  vinfos[0].maxsolutions = _nj0;
3814  vinfos[1].jointtype = 1;
3815  vinfos[1].foffset = j1;
3816  vinfos[1].indices[0] = _ij1[0];
3817  vinfos[1].indices[1] = _ij1[1];
3818  vinfos[1].maxsolutions = _nj1;
3819  vinfos[2].jointtype = 1;
3820  vinfos[2].foffset = j2;
3821  vinfos[2].indices[0] = _ij2[0];
3822  vinfos[2].indices[1] = _ij2[1];
3823  vinfos[2].maxsolutions = _nj2;
3824  vinfos[3].jointtype = 1;
3825  vinfos[3].foffset = j3;
3826  vinfos[3].indices[0] = _ij3[0];
3827  vinfos[3].indices[1] = _ij3[1];
3828  vinfos[3].maxsolutions = _nj3;
3829  vinfos[4].jointtype = 1;
3830  vinfos[4].foffset = j4;
3831  vinfos[4].indices[0] = _ij4[0];
3832  vinfos[4].indices[1] = _ij4[1];
3833  vinfos[4].maxsolutions = _nj4;
3834  vinfos[5].jointtype = 1;
3835  vinfos[5].foffset = j5;
3836  vinfos[5].indices[0] = _ij5[0];
3837  vinfos[5].indices[1] = _ij5[1];
3838  vinfos[5].maxsolutions = _nj5;
3839  vinfos[6].jointtype = 1;
3840  vinfos[6].foffset = j6;
3841  vinfos[6].indices[0] = _ij6[0];
3842  vinfos[6].indices[1] = _ij6[1];
3843  vinfos[6].maxsolutions = _nj6;
3844  std::vector<int> vfree(0);
3845  solutions.AddSolution(vinfos, vfree);
3846  }
3847  }
3848  }
3849  }
3850  } while (0);
3851  if (bgotonextstatement)
3852  {
3853  bool bgotonextstatement = true;
3854  do
3855  {
3856  IkReal x128 = new_r22 * new_r22;
3857  CheckValue<IkReal> x129 =
3858  IKPowWithIntegerCheck(((1.0) + (((-1.0) * x128))), -1);
3859  if (!x129.valid)
3860  {
3861  continue;
3862  }
3863  if ((((-1.0) * x128 * (x129.value))) < -0.00001)
3864  continue;
3865  IkReal gconst12 = IKsqrt(((-1.0) * x128 * (x129.value)));
3866  evalcond[0] =
3867  ((-3.14159265358979) +
3868  (IKfmod(((3.14159265358979) +
3869  (IKabs((cj0 + (((-1.0) * gconst12))))) +
3870  (IKabs(((-1.0) + (IKsign(sj0)))))),
3871  6.28318530717959)));
3872  if (IKabs(evalcond[0]) < 0.0000050000000000)
3873  {
3874  bgotonextstatement = false;
3875  {
3876  IkReal j2eval[1];
3877  IkReal x130 = new_r22 * new_r22;
3878  new_r02 = 0;
3879  new_r12 = 0;
3880  new_r20 = 0;
3881  new_r21 = 0;
3882  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3883  -0.00001)
3884  continue;
3885  sj0 =
3886  IKsqrt(((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3887  cj0 = gconst12;
3888  if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
3889  (gconst12) > 1 + IKFAST_SINCOS_THRESH)
3890  continue;
3891  j0 = IKacos(gconst12);
3892  CheckValue<IkReal> x131 = IKPowWithIntegerCheck(
3893  ((1.0) + (((-1.0) * x130))), -1);
3894  if (!x131.valid)
3895  {
3896  continue;
3897  }
3898  if ((((-1.0) * x130 * (x131.value))) < -0.00001)
3899  continue;
3900  IkReal gconst12 = IKsqrt(((-1.0) * x130 * (x131.value)));
3901  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
3902  if (IKabs(j2eval[0]) < 0.0000010000000000)
3903  {
3904  {
3905  IkReal j2array[1], cj2array[1], sj2array[1];
3906  bool j2valid[1] = { false };
3907  _nj2 = 1;
3908  CheckValue<IkReal> x132 =
3909  IKPowWithIntegerCheck(gconst12, -1);
3910  if (!x132.valid)
3911  {
3912  continue;
3913  }
3914  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3915  -0.00001)
3916  continue;
3917  if (IKabs(((-1.0) * new_r10 * (x132.value))) <
3918  IKFAST_ATAN2_MAGTHRESH &&
3919  IKabs((((new_r01 *
3920  (IKsqrt(((1.0) +
3921  (((-1.0) *
3922  (gconst12 * gconst12)))))))) +
3923  (((-1.0) * gconst12 * new_r11)))) <
3924  IKFAST_ATAN2_MAGTHRESH &&
3925  IKabs(IKsqr(((-1.0) * new_r10 * (x132.value))) +
3926  IKsqr((
3927  ((new_r01 *
3928  (IKsqrt(((1.0) + (((-1.0) *
3929  (gconst12 *
3930  gconst12)))))))) +
3931  (((-1.0) * gconst12 * new_r11)))) -
3932  1) <= IKFAST_SINCOS_THRESH)
3933  continue;
3934  j2array[0] = IKatan2(
3935  ((-1.0) * new_r10 * (x132.value)),
3936  (((new_r01 *
3937  (IKsqrt(((1.0) + (((-1.0) * (gconst12 *
3938  gconst12)))))))) +
3939  (((-1.0) * gconst12 * new_r11))));
3940  sj2array[0] = IKsin(j2array[0]);
3941  cj2array[0] = IKcos(j2array[0]);
3942  if (j2array[0] > IKPI)
3943  {
3944  j2array[0] -= IK2PI;
3945  }
3946  else if (j2array[0] < -IKPI)
3947  {
3948  j2array[0] += IK2PI;
3949  }
3950  j2valid[0] = true;
3951  for (int ij2 = 0; ij2 < 1; ++ij2)
3952  {
3953  if (!j2valid[ij2])
3954  {
3955  continue;
3956  }
3957  _ij2[0] = ij2;
3958  _ij2[1] = -1;
3959  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3960  {
3961  if (j2valid[iij2] &&
3962  IKabs(cj2array[ij2] - cj2array[iij2]) <
3963  IKFAST_SOLUTION_THRESH &&
3964  IKabs(sj2array[ij2] - sj2array[iij2]) <
3965  IKFAST_SOLUTION_THRESH)
3966  {
3967  j2valid[iij2] = false;
3968  _ij2[1] = iij2;
3969  break;
3970  }
3971  }
3972  j2 = j2array[ij2];
3973  cj2 = cj2array[ij2];
3974  sj2 = sj2array[ij2];
3975  {
3976  IkReal evalcond[8];
3977  IkReal x133 = IKsin(j2);
3978  IkReal x134 = IKcos(j2);
3979  if ((((1.0) +
3980  (((-1.0) * (gconst12 * gconst12))))) <
3981  -0.00001)
3982  continue;
3983  IkReal x135 = IKsqrt(
3984  ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3985  IkReal x136 = ((1.0) * x135);
3986  evalcond[0] = x134;
3987  evalcond[1] = ((-1.0) * x133);
3988  evalcond[2] = (((gconst12 * x133)) + new_r10);
3989  evalcond[3] = (((gconst12 * x134)) + new_r11);
3990  evalcond[4] =
3991  ((((-1.0) * x133 * x136)) + new_r00);
3992  evalcond[5] =
3993  ((((-1.0) * x134 * x136)) + new_r01);
3994  evalcond[6] = ((((-1.0) * new_r00 * x136)) +
3995  x133 + ((gconst12 * new_r10)));
3996  evalcond[7] = ((((-1.0) * new_r01 * x136)) +
3997  x134 + ((gconst12 * new_r11)));
3998  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3999  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4000  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4001  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4002  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4003  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4004  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4005  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4006  {
4007  continue;
4008  }
4009  }
4010 
4011  {
4012  std::vector<IkSingleDOFSolutionBase<IkReal> >
4013  vinfos(7);
4014  vinfos[0].jointtype = 1;
4015  vinfos[0].foffset = j0;
4016  vinfos[0].indices[0] = _ij0[0];
4017  vinfos[0].indices[1] = _ij0[1];
4018  vinfos[0].maxsolutions = _nj0;
4019  vinfos[1].jointtype = 1;
4020  vinfos[1].foffset = j1;
4021  vinfos[1].indices[0] = _ij1[0];
4022  vinfos[1].indices[1] = _ij1[1];
4023  vinfos[1].maxsolutions = _nj1;
4024  vinfos[2].jointtype = 1;
4025  vinfos[2].foffset = j2;
4026  vinfos[2].indices[0] = _ij2[0];
4027  vinfos[2].indices[1] = _ij2[1];
4028  vinfos[2].maxsolutions = _nj2;
4029  vinfos[3].jointtype = 1;
4030  vinfos[3].foffset = j3;
4031  vinfos[3].indices[0] = _ij3[0];
4032  vinfos[3].indices[1] = _ij3[1];
4033  vinfos[3].maxsolutions = _nj3;
4034  vinfos[4].jointtype = 1;
4035  vinfos[4].foffset = j4;
4036  vinfos[4].indices[0] = _ij4[0];
4037  vinfos[4].indices[1] = _ij4[1];
4038  vinfos[4].maxsolutions = _nj4;
4039  vinfos[5].jointtype = 1;
4040  vinfos[5].foffset = j5;
4041  vinfos[5].indices[0] = _ij5[0];
4042  vinfos[5].indices[1] = _ij5[1];
4043  vinfos[5].maxsolutions = _nj5;
4044  vinfos[6].jointtype = 1;
4045  vinfos[6].foffset = j6;
4046  vinfos[6].indices[0] = _ij6[0];
4047  vinfos[6].indices[1] = _ij6[1];
4048  vinfos[6].maxsolutions = _nj6;
4049  std::vector<int> vfree(0);
4050  solutions.AddSolution(vinfos, vfree);
4051  }
4052  }
4053  }
4054  }
4055  else
4056  {
4057  {
4058  IkReal j2array[1], cj2array[1], sj2array[1];
4059  bool j2valid[1] = { false };
4060  _nj2 = 1;
4061  CheckValue<IkReal> x137 =
4062  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4063  IkReal(((-1.0) * new_r11)),
4064  IKFAST_ATAN2_MAGTHRESH);
4065  if (!x137.valid)
4066  {
4067  continue;
4068  }
4069  CheckValue<IkReal> x138 =
4070  IKPowWithIntegerCheck(IKsign(gconst12), -1);
4071  if (!x138.valid)
4072  {
4073  continue;
4074  }
4075  j2array[0] = ((-1.5707963267949) + (x137.value) +
4076  (((1.5707963267949) * (x138.value))));
4077  sj2array[0] = IKsin(j2array[0]);
4078  cj2array[0] = IKcos(j2array[0]);
4079  if (j2array[0] > IKPI)
4080  {
4081  j2array[0] -= IK2PI;
4082  }
4083  else if (j2array[0] < -IKPI)
4084  {
4085  j2array[0] += IK2PI;
4086  }
4087  j2valid[0] = true;
4088  for (int ij2 = 0; ij2 < 1; ++ij2)
4089  {
4090  if (!j2valid[ij2])
4091  {
4092  continue;
4093  }
4094  _ij2[0] = ij2;
4095  _ij2[1] = -1;
4096  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4097  {
4098  if (j2valid[iij2] &&
4099  IKabs(cj2array[ij2] - cj2array[iij2]) <
4100  IKFAST_SOLUTION_THRESH &&
4101  IKabs(sj2array[ij2] - sj2array[iij2]) <
4102  IKFAST_SOLUTION_THRESH)
4103  {
4104  j2valid[iij2] = false;
4105  _ij2[1] = iij2;
4106  break;
4107  }
4108  }
4109  j2 = j2array[ij2];
4110  cj2 = cj2array[ij2];
4111  sj2 = sj2array[ij2];
4112  {
4113  IkReal evalcond[8];
4114  IkReal x139 = IKsin(j2);
4115  IkReal x140 = IKcos(j2);
4116  if ((((1.0) +
4117  (((-1.0) * (gconst12 * gconst12))))) <
4118  -0.00001)
4119  continue;
4120  IkReal x141 = IKsqrt(
4121  ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
4122  IkReal x142 = ((1.0) * x141);
4123  evalcond[0] = x140;
4124  evalcond[1] = ((-1.0) * x139);
4125  evalcond[2] = (((gconst12 * x139)) + new_r10);
4126  evalcond[3] = (((gconst12 * x140)) + new_r11);
4127  evalcond[4] =
4128  ((((-1.0) * x139 * x142)) + new_r00);
4129  evalcond[5] =
4130  ((((-1.0) * x140 * x142)) + new_r01);
4131  evalcond[6] =
4132  (x139 + (((-1.0) * new_r00 * x142)) +
4133  ((gconst12 * new_r10)));
4134  evalcond[7] = ((((-1.0) * new_r01 * x142)) +
4135  x140 + ((gconst12 * new_r11)));
4136  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
4137  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4138  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4139  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4140  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4141  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4142  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4143  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4144  {
4145  continue;
4146  }
4147  }
4148 
4149  {
4150  std::vector<IkSingleDOFSolutionBase<IkReal> >
4151  vinfos(7);
4152  vinfos[0].jointtype = 1;
4153  vinfos[0].foffset = j0;
4154  vinfos[0].indices[0] = _ij0[0];
4155  vinfos[0].indices[1] = _ij0[1];
4156  vinfos[0].maxsolutions = _nj0;
4157  vinfos[1].jointtype = 1;
4158  vinfos[1].foffset = j1;
4159  vinfos[1].indices[0] = _ij1[0];
4160  vinfos[1].indices[1] = _ij1[1];
4161  vinfos[1].maxsolutions = _nj1;
4162  vinfos[2].jointtype = 1;
4163  vinfos[2].foffset = j2;
4164  vinfos[2].indices[0] = _ij2[0];
4165  vinfos[2].indices[1] = _ij2[1];
4166  vinfos[2].maxsolutions = _nj2;
4167  vinfos[3].jointtype = 1;
4168  vinfos[3].foffset = j3;
4169  vinfos[3].indices[0] = _ij3[0];
4170  vinfos[3].indices[1] = _ij3[1];
4171  vinfos[3].maxsolutions = _nj3;
4172  vinfos[4].jointtype = 1;
4173  vinfos[4].foffset = j4;
4174  vinfos[4].indices[0] = _ij4[0];
4175  vinfos[4].indices[1] = _ij4[1];
4176  vinfos[4].maxsolutions = _nj4;
4177  vinfos[5].jointtype = 1;
4178  vinfos[5].foffset = j5;
4179  vinfos[5].indices[0] = _ij5[0];
4180  vinfos[5].indices[1] = _ij5[1];
4181  vinfos[5].maxsolutions = _nj5;
4182  vinfos[6].jointtype = 1;
4183  vinfos[6].foffset = j6;
4184  vinfos[6].indices[0] = _ij6[0];
4185  vinfos[6].indices[1] = _ij6[1];
4186  vinfos[6].maxsolutions = _nj6;
4187  std::vector<int> vfree(0);
4188  solutions.AddSolution(vinfos, vfree);
4189  }
4190  }
4191  }
4192  }
4193  }
4194  }
4195  } while (0);
4196  if (bgotonextstatement)
4197  {
4198  bool bgotonextstatement = true;
4199  do
4200  {
4201  IkReal x143 = new_r22 * new_r22;
4202  CheckValue<IkReal> x144 =
4203  IKPowWithIntegerCheck(((1.0) + (((-1.0) * x143))), -1);
4204  if (!x144.valid)
4205  {
4206  continue;
4207  }
4208  if ((((-1.0) * x143 * (x144.value))) < -0.00001)
4209  continue;
4210  IkReal gconst12 = IKsqrt(((-1.0) * x143 * (x144.value)));
4211  evalcond[0] =
4212  ((-3.14159265358979) +
4213  (IKfmod(((3.14159265358979) +
4214  (IKabs((cj0 + (((-1.0) * gconst12))))) +
4215  (IKabs(((1.0) + (IKsign(sj0)))))),
4216  6.28318530717959)));
4217  if (IKabs(evalcond[0]) < 0.0000050000000000)
4218  {
4219  bgotonextstatement = false;
4220  {
4221  IkReal j2eval[1];
4222  IkReal x145 = new_r22 * new_r22;
4223  new_r02 = 0;
4224  new_r12 = 0;
4225  new_r20 = 0;
4226  new_r21 = 0;
4227  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4228  -0.00001)
4229  continue;
4230  sj0 = ((-1.0) *
4231  (IKsqrt(((1.0) +
4232  (((-1.0) * (gconst12 * gconst12)))))));
4233  cj0 = gconst12;
4234  if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
4235  (gconst12) > 1 + IKFAST_SINCOS_THRESH)
4236  continue;
4237  j0 = ((-1.0) * (IKacos(gconst12)));
4238  CheckValue<IkReal> x146 = IKPowWithIntegerCheck(
4239  ((1.0) + (((-1.0) * x145))), -1);
4240  if (!x146.valid)
4241  {
4242  continue;
4243  }
4244  if ((((-1.0) * x145 * (x146.value))) < -0.00001)
4245  continue;
4246  IkReal gconst12 =
4247  IKsqrt(((-1.0) * x145 * (x146.value)));
4248  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4249  if (IKabs(j2eval[0]) < 0.0000010000000000)
4250  {
4251  {
4252  IkReal j2array[1], cj2array[1], sj2array[1];
4253  bool j2valid[1] = { false };
4254  _nj2 = 1;
4255  CheckValue<IkReal> x147 =
4256  IKPowWithIntegerCheck(gconst12, -1);
4257  if (!x147.valid)
4258  {
4259  continue;
4260  }
4261  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4262  -0.00001)
4263  continue;
4264  if (IKabs(((-1.0) * new_r10 * (x147.value))) <
4265  IKFAST_ATAN2_MAGTHRESH &&
4266  IKabs(((((-1.0) * new_r01 *
4267  (IKsqrt(((1.0) +
4268  (((-1.0) * (gconst12 *
4269  gconst12)))))))) +
4270  (((-1.0) * gconst12 * new_r11)))) <
4271  IKFAST_ATAN2_MAGTHRESH &&
4272  IKabs(IKsqr(((-1.0) * new_r10 * (x147.value))) +
4273  IKsqr(((((-1.0) * new_r01 *
4274  (IKsqrt(((1.0) +
4275  (((-1.0) *
4276  (gconst12 *
4277  gconst12)))))))) +
4278  (((-1.0) * gconst12 * new_r11)))) -
4279  1) <= IKFAST_SINCOS_THRESH)
4280  continue;
4281  j2array[0] = IKatan2(
4282  ((-1.0) * new_r10 * (x147.value)),
4283  ((((-1.0) * new_r01 *
4284  (IKsqrt((
4285  (1.0) +
4286  (((-1.0) * (gconst12 * gconst12)))))))) +
4287  (((-1.0) * gconst12 * new_r11))));
4288  sj2array[0] = IKsin(j2array[0]);
4289  cj2array[0] = IKcos(j2array[0]);
4290  if (j2array[0] > IKPI)
4291  {
4292  j2array[0] -= IK2PI;
4293  }
4294  else if (j2array[0] < -IKPI)
4295  {
4296  j2array[0] += IK2PI;
4297  }
4298  j2valid[0] = true;
4299  for (int ij2 = 0; ij2 < 1; ++ij2)
4300  {
4301  if (!j2valid[ij2])
4302  {
4303  continue;
4304  }
4305  _ij2[0] = ij2;
4306  _ij2[1] = -1;
4307  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4308  {
4309  if (j2valid[iij2] &&
4310  IKabs(cj2array[ij2] - cj2array[iij2]) <
4311  IKFAST_SOLUTION_THRESH &&
4312  IKabs(sj2array[ij2] - sj2array[iij2]) <
4313  IKFAST_SOLUTION_THRESH)
4314  {
4315  j2valid[iij2] = false;
4316  _ij2[1] = iij2;
4317  break;
4318  }
4319  }
4320  j2 = j2array[ij2];
4321  cj2 = cj2array[ij2];
4322  sj2 = sj2array[ij2];
4323  {
4324  IkReal evalcond[8];
4325  IkReal x148 = IKsin(j2);
4326  IkReal x149 = IKcos(j2);
4327  if ((((1.0) +
4328  (((-1.0) * (gconst12 * gconst12))))) <
4329  -0.00001)
4330  continue;
4331  IkReal x150 = IKsqrt(
4332  ((1.0) +
4333  (((-1.0) * (gconst12 * gconst12)))));
4334  evalcond[0] = x149;
4335  evalcond[1] = ((-1.0) * x148);
4336  evalcond[2] = (((gconst12 * x148)) + new_r10);
4337  evalcond[3] = (((gconst12 * x149)) + new_r11);
4338  evalcond[4] = (((x148 * x150)) + new_r00);
4339  evalcond[5] = (((x149 * x150)) + new_r01);
4340  evalcond[6] = (((new_r00 * x150)) + x148 +
4341  ((gconst12 * new_r10)));
4342  evalcond[7] = (((new_r01 * x150)) + x149 +
4343  ((gconst12 * new_r11)));
4344  if (IKabs(evalcond[0]) >
4345  IKFAST_EVALCOND_THRESH ||
4346  IKabs(evalcond[1]) >
4347  IKFAST_EVALCOND_THRESH ||
4348  IKabs(evalcond[2]) >
4349  IKFAST_EVALCOND_THRESH ||
4350  IKabs(evalcond[3]) >
4351  IKFAST_EVALCOND_THRESH ||
4352  IKabs(evalcond[4]) >
4353  IKFAST_EVALCOND_THRESH ||
4354  IKabs(evalcond[5]) >
4355  IKFAST_EVALCOND_THRESH ||
4356  IKabs(evalcond[6]) >
4357  IKFAST_EVALCOND_THRESH ||
4358  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4359  {
4360  continue;
4361  }
4362  }
4363 
4364  {
4365  std::vector<IkSingleDOFSolutionBase<IkReal> >
4366  vinfos(7);
4367  vinfos[0].jointtype = 1;
4368  vinfos[0].foffset = j0;
4369  vinfos[0].indices[0] = _ij0[0];
4370  vinfos[0].indices[1] = _ij0[1];
4371  vinfos[0].maxsolutions = _nj0;
4372  vinfos[1].jointtype = 1;
4373  vinfos[1].foffset = j1;
4374  vinfos[1].indices[0] = _ij1[0];
4375  vinfos[1].indices[1] = _ij1[1];
4376  vinfos[1].maxsolutions = _nj1;
4377  vinfos[2].jointtype = 1;
4378  vinfos[2].foffset = j2;
4379  vinfos[2].indices[0] = _ij2[0];
4380  vinfos[2].indices[1] = _ij2[1];
4381  vinfos[2].maxsolutions = _nj2;
4382  vinfos[3].jointtype = 1;
4383  vinfos[3].foffset = j3;
4384  vinfos[3].indices[0] = _ij3[0];
4385  vinfos[3].indices[1] = _ij3[1];
4386  vinfos[3].maxsolutions = _nj3;
4387  vinfos[4].jointtype = 1;
4388  vinfos[4].foffset = j4;
4389  vinfos[4].indices[0] = _ij4[0];
4390  vinfos[4].indices[1] = _ij4[1];
4391  vinfos[4].maxsolutions = _nj4;
4392  vinfos[5].jointtype = 1;
4393  vinfos[5].foffset = j5;
4394  vinfos[5].indices[0] = _ij5[0];
4395  vinfos[5].indices[1] = _ij5[1];
4396  vinfos[5].maxsolutions = _nj5;
4397  vinfos[6].jointtype = 1;
4398  vinfos[6].foffset = j6;
4399  vinfos[6].indices[0] = _ij6[0];
4400  vinfos[6].indices[1] = _ij6[1];
4401  vinfos[6].maxsolutions = _nj6;
4402  std::vector<int> vfree(0);
4403  solutions.AddSolution(vinfos, vfree);
4404  }
4405  }
4406  }
4407  }
4408  else
4409  {
4410  {
4411  IkReal j2array[1], cj2array[1], sj2array[1];
4412  bool j2valid[1] = { false };
4413  _nj2 = 1;
4414  CheckValue<IkReal> x151 =
4415  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4416  IkReal(((-1.0) * new_r11)),
4417  IKFAST_ATAN2_MAGTHRESH);
4418  if (!x151.valid)
4419  {
4420  continue;
4421  }
4422  CheckValue<IkReal> x152 =
4423  IKPowWithIntegerCheck(IKsign(gconst12), -1);
4424  if (!x152.valid)
4425  {
4426  continue;
4427  }
4428  j2array[0] = ((-1.5707963267949) + (x151.value) +
4429  (((1.5707963267949) * (x152.value))));
4430  sj2array[0] = IKsin(j2array[0]);
4431  cj2array[0] = IKcos(j2array[0]);
4432  if (j2array[0] > IKPI)
4433  {
4434  j2array[0] -= IK2PI;
4435  }
4436  else if (j2array[0] < -IKPI)
4437  {
4438  j2array[0] += IK2PI;
4439  }
4440  j2valid[0] = true;
4441  for (int ij2 = 0; ij2 < 1; ++ij2)
4442  {
4443  if (!j2valid[ij2])
4444  {
4445  continue;
4446  }
4447  _ij2[0] = ij2;
4448  _ij2[1] = -1;
4449  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4450  {
4451  if (j2valid[iij2] &&
4452  IKabs(cj2array[ij2] - cj2array[iij2]) <
4453  IKFAST_SOLUTION_THRESH &&
4454  IKabs(sj2array[ij2] - sj2array[iij2]) <
4455  IKFAST_SOLUTION_THRESH)
4456  {
4457  j2valid[iij2] = false;
4458  _ij2[1] = iij2;
4459  break;
4460  }
4461  }
4462  j2 = j2array[ij2];
4463  cj2 = cj2array[ij2];
4464  sj2 = sj2array[ij2];
4465  {
4466  IkReal evalcond[8];
4467  IkReal x153 = IKsin(j2);
4468  IkReal x154 = IKcos(j2);
4469  if ((((1.0) +
4470  (((-1.0) * (gconst12 * gconst12))))) <
4471  -0.00001)
4472  continue;
4473  IkReal x155 = IKsqrt(
4474  ((1.0) +
4475  (((-1.0) * (gconst12 * gconst12)))));
4476  evalcond[0] = x154;
4477  evalcond[1] = ((-1.0) * x153);
4478  evalcond[2] = (((gconst12 * x153)) + new_r10);
4479  evalcond[3] = (((gconst12 * x154)) + new_r11);
4480  evalcond[4] = (((x153 * x155)) + new_r00);
4481  evalcond[5] = (((x154 * x155)) + new_r01);
4482  evalcond[6] = (((new_r00 * x155)) + x153 +
4483  ((gconst12 * new_r10)));
4484  evalcond[7] = (((new_r01 * x155)) + x154 +
4485  ((gconst12 * new_r11)));
4486  if (IKabs(evalcond[0]) >
4487  IKFAST_EVALCOND_THRESH ||
4488  IKabs(evalcond[1]) >
4489  IKFAST_EVALCOND_THRESH ||
4490  IKabs(evalcond[2]) >
4491  IKFAST_EVALCOND_THRESH ||
4492  IKabs(evalcond[3]) >
4493  IKFAST_EVALCOND_THRESH ||
4494  IKabs(evalcond[4]) >
4495  IKFAST_EVALCOND_THRESH ||
4496  IKabs(evalcond[5]) >
4497  IKFAST_EVALCOND_THRESH ||
4498  IKabs(evalcond[6]) >
4499  IKFAST_EVALCOND_THRESH ||
4500  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4501  {
4502  continue;
4503  }
4504  }
4505 
4506  {
4507  std::vector<IkSingleDOFSolutionBase<IkReal> >
4508  vinfos(7);
4509  vinfos[0].jointtype = 1;
4510  vinfos[0].foffset = j0;
4511  vinfos[0].indices[0] = _ij0[0];
4512  vinfos[0].indices[1] = _ij0[1];
4513  vinfos[0].maxsolutions = _nj0;
4514  vinfos[1].jointtype = 1;
4515  vinfos[1].foffset = j1;
4516  vinfos[1].indices[0] = _ij1[0];
4517  vinfos[1].indices[1] = _ij1[1];
4518  vinfos[1].maxsolutions = _nj1;
4519  vinfos[2].jointtype = 1;
4520  vinfos[2].foffset = j2;
4521  vinfos[2].indices[0] = _ij2[0];
4522  vinfos[2].indices[1] = _ij2[1];
4523  vinfos[2].maxsolutions = _nj2;
4524  vinfos[3].jointtype = 1;
4525  vinfos[3].foffset = j3;
4526  vinfos[3].indices[0] = _ij3[0];
4527  vinfos[3].indices[1] = _ij3[1];
4528  vinfos[3].maxsolutions = _nj3;
4529  vinfos[4].jointtype = 1;
4530  vinfos[4].foffset = j4;
4531  vinfos[4].indices[0] = _ij4[0];
4532  vinfos[4].indices[1] = _ij4[1];
4533  vinfos[4].maxsolutions = _nj4;
4534  vinfos[5].jointtype = 1;
4535  vinfos[5].foffset = j5;
4536  vinfos[5].indices[0] = _ij5[0];
4537  vinfos[5].indices[1] = _ij5[1];
4538  vinfos[5].maxsolutions = _nj5;
4539  vinfos[6].jointtype = 1;
4540  vinfos[6].foffset = j6;
4541  vinfos[6].indices[0] = _ij6[0];
4542  vinfos[6].indices[1] = _ij6[1];
4543  vinfos[6].maxsolutions = _nj6;
4544  std::vector<int> vfree(0);
4545  solutions.AddSolution(vinfos, vfree);
4546  }
4547  }
4548  }
4549  }
4550  }
4551  }
4552  } while (0);
4553  if (bgotonextstatement)
4554  {
4555  bool bgotonextstatement = true;
4556  do
4557  {
4558  IkReal x156 = new_r22 * new_r22;
4559  CheckValue<IkReal> x157 = IKPowWithIntegerCheck(
4560  ((1.0) + (((-1.0) * x156))), -1);
4561  if (!x157.valid)
4562  {
4563  continue;
4564  }
4565  if ((((-1.0) * x156 * (x157.value))) < -0.00001)
4566  continue;
4567  IkReal gconst13 =
4568  ((-1.0) * (IKsqrt(((-1.0) * x156 * (x157.value)))));
4569  evalcond[0] =
4570  ((-3.14159265358979) +
4571  (IKfmod(((3.14159265358979) +
4572  (IKabs((cj0 + (((-1.0) * gconst13))))) +
4573  (IKabs(((-1.0) + (IKsign(sj0)))))),
4574  6.28318530717959)));
4575  if (IKabs(evalcond[0]) < 0.0000050000000000)
4576  {
4577  bgotonextstatement = false;
4578  {
4579  IkReal j2eval[1];
4580  IkReal x158 = new_r22 * new_r22;
4581  new_r02 = 0;
4582  new_r12 = 0;
4583  new_r20 = 0;
4584  new_r21 = 0;
4585  if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4586  -0.00001)
4587  continue;
4588  sj0 = IKsqrt(
4589  ((1.0) + (((-1.0) * (gconst13 * gconst13)))));
4590  cj0 = gconst13;
4591  if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4592  (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4593  continue;
4594  j0 = IKacos(gconst13);
4595  CheckValue<IkReal> x159 = IKPowWithIntegerCheck(
4596  ((1.0) + (((-1.0) * x158))), -1);
4597  if (!x159.valid)
4598  {
4599  continue;
4600  }
4601  if ((((-1.0) * x158 * (x159.value))) < -0.00001)
4602  continue;
4603  IkReal gconst13 =
4604  ((-1.0) *
4605  (IKsqrt(((-1.0) * x158 * (x159.value)))));
4606  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4607  if (IKabs(j2eval[0]) < 0.0000010000000000)
4608  {
4609  {
4610  IkReal j2array[1], cj2array[1], sj2array[1];
4611  bool j2valid[1] = { false };
4612  _nj2 = 1;
4613  CheckValue<IkReal> x160 =
4614  IKPowWithIntegerCheck(gconst13, -1);
4615  if (!x160.valid)
4616  {
4617  continue;
4618  }
4619  if ((((1.0) +
4620  (((-1.0) * (gconst13 * gconst13))))) <
4621  -0.00001)
4622  continue;
4623  if (IKabs(((-1.0) * new_r10 * (x160.value))) <
4624  IKFAST_ATAN2_MAGTHRESH &&
4625  IKabs((
4626  (((-1.0) * gconst13 * new_r11)) +
4627  ((new_r01 *
4628  (IKsqrt(((1.0) + (((-1.0) *
4629  (gconst13 *
4630  gconst13)))))))))) <
4631  IKFAST_ATAN2_MAGTHRESH &&
4632  IKabs(
4633  IKsqr(((-1.0) * new_r10 * (x160.value))) +
4634  IKsqr(((((-1.0) * gconst13 * new_r11)) +
4635  ((new_r01 *
4636  (IKsqrt(((1.0) +
4637  (((-1.0) *
4638  (gconst13 *
4639  gconst13)))))))))) -
4640  1) <= IKFAST_SINCOS_THRESH)
4641  continue;
4642  j2array[0] = IKatan2(
4643  ((-1.0) * new_r10 * (x160.value)),
4644  ((((-1.0) * gconst13 * new_r11)) +
4645  ((new_r01 *
4646  (IKsqrt(((1.0) +
4647  (((-1.0) *
4648  (gconst13 * gconst13))))))))));
4649  sj2array[0] = IKsin(j2array[0]);
4650  cj2array[0] = IKcos(j2array[0]);
4651  if (j2array[0] > IKPI)
4652  {
4653  j2array[0] -= IK2PI;
4654  }
4655  else if (j2array[0] < -IKPI)
4656  {
4657  j2array[0] += IK2PI;
4658  }
4659  j2valid[0] = true;
4660  for (int ij2 = 0; ij2 < 1; ++ij2)
4661  {
4662  if (!j2valid[ij2])
4663  {
4664  continue;
4665  }
4666  _ij2[0] = ij2;
4667  _ij2[1] = -1;
4668  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4669  {
4670  if (j2valid[iij2] &&
4671  IKabs(cj2array[ij2] - cj2array[iij2]) <
4672  IKFAST_SOLUTION_THRESH &&
4673  IKabs(sj2array[ij2] - sj2array[iij2]) <
4674  IKFAST_SOLUTION_THRESH)
4675  {
4676  j2valid[iij2] = false;
4677  _ij2[1] = iij2;
4678  break;
4679  }
4680  }
4681  j2 = j2array[ij2];
4682  cj2 = cj2array[ij2];
4683  sj2 = sj2array[ij2];
4684  {
4685  IkReal evalcond[8];
4686  IkReal x161 = IKsin(j2);
4687  IkReal x162 = IKcos(j2);
4688  if ((((1.0) +
4689  (((-1.0) * (gconst13 * gconst13))))) <
4690  -0.00001)
4691  continue;
4692  IkReal x163 = IKsqrt(
4693  ((1.0) +
4694  (((-1.0) * (gconst13 * gconst13)))));
4695  IkReal x164 = ((1.0) * x163);
4696  evalcond[0] = x162;
4697  evalcond[1] = ((-1.0) * x161);
4698  evalcond[2] = (new_r10 + ((gconst13 * x161)));
4699  evalcond[3] = (new_r11 + ((gconst13 * x162)));
4700  evalcond[4] =
4701  ((((-1.0) * x161 * x164)) + new_r00);
4702  evalcond[5] =
4703  ((((-1.0) * x162 * x164)) + new_r01);
4704  evalcond[6] = ((((-1.0) * new_r00 * x164)) +
4705  x161 + ((gconst13 * new_r10)));
4706  evalcond[7] = (x162 + ((gconst13 * new_r11)) +
4707  (((-1.0) * new_r01 * x164)));
4708  if (IKabs(evalcond[0]) >
4709  IKFAST_EVALCOND_THRESH ||
4710  IKabs(evalcond[1]) >
4711  IKFAST_EVALCOND_THRESH ||
4712  IKabs(evalcond[2]) >
4713  IKFAST_EVALCOND_THRESH ||
4714  IKabs(evalcond[3]) >
4715  IKFAST_EVALCOND_THRESH ||
4716  IKabs(evalcond[4]) >
4717  IKFAST_EVALCOND_THRESH ||
4718  IKabs(evalcond[5]) >
4719  IKFAST_EVALCOND_THRESH ||
4720  IKabs(evalcond[6]) >
4721  IKFAST_EVALCOND_THRESH ||
4722  IKabs(evalcond[7]) >
4723  IKFAST_EVALCOND_THRESH)
4724  {
4725  continue;
4726  }
4727  }
4728 
4729  {
4730  std::vector<IkSingleDOFSolutionBase<IkReal> >
4731  vinfos(7);
4732  vinfos[0].jointtype = 1;
4733  vinfos[0].foffset = j0;
4734  vinfos[0].indices[0] = _ij0[0];
4735  vinfos[0].indices[1] = _ij0[1];
4736  vinfos[0].maxsolutions = _nj0;
4737  vinfos[1].jointtype = 1;
4738  vinfos[1].foffset = j1;
4739  vinfos[1].indices[0] = _ij1[0];
4740  vinfos[1].indices[1] = _ij1[1];
4741  vinfos[1].maxsolutions = _nj1;
4742  vinfos[2].jointtype = 1;
4743  vinfos[2].foffset = j2;
4744  vinfos[2].indices[0] = _ij2[0];
4745  vinfos[2].indices[1] = _ij2[1];
4746  vinfos[2].maxsolutions = _nj2;
4747  vinfos[3].jointtype = 1;
4748  vinfos[3].foffset = j3;
4749  vinfos[3].indices[0] = _ij3[0];
4750  vinfos[3].indices[1] = _ij3[1];
4751  vinfos[3].maxsolutions = _nj3;
4752  vinfos[4].jointtype = 1;
4753  vinfos[4].foffset = j4;
4754  vinfos[4].indices[0] = _ij4[0];
4755  vinfos[4].indices[1] = _ij4[1];
4756  vinfos[4].maxsolutions = _nj4;
4757  vinfos[5].jointtype = 1;
4758  vinfos[5].foffset = j5;
4759  vinfos[5].indices[0] = _ij5[0];
4760  vinfos[5].indices[1] = _ij5[1];
4761  vinfos[5].maxsolutions = _nj5;
4762  vinfos[6].jointtype = 1;
4763  vinfos[6].foffset = j6;
4764  vinfos[6].indices[0] = _ij6[0];
4765  vinfos[6].indices[1] = _ij6[1];
4766  vinfos[6].maxsolutions = _nj6;
4767  std::vector<int> vfree(0);
4768  solutions.AddSolution(vinfos, vfree);
4769  }
4770  }
4771  }
4772  }
4773  else
4774  {
4775  {
4776  IkReal j2array[1], cj2array[1], sj2array[1];
4777  bool j2valid[1] = { false };
4778  _nj2 = 1;
4779  CheckValue<IkReal> x165 =
4780  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4781  IkReal(((-1.0) * new_r11)),
4782  IKFAST_ATAN2_MAGTHRESH);
4783  if (!x165.valid)
4784  {
4785  continue;
4786  }
4787  CheckValue<IkReal> x166 =
4788  IKPowWithIntegerCheck(IKsign(gconst13), -1);
4789  if (!x166.valid)
4790  {
4791  continue;
4792  }
4793  j2array[0] =
4794  ((-1.5707963267949) + (x165.value) +
4795  (((1.5707963267949) * (x166.value))));
4796  sj2array[0] = IKsin(j2array[0]);
4797  cj2array[0] = IKcos(j2array[0]);
4798  if (j2array[0] > IKPI)
4799  {
4800  j2array[0] -= IK2PI;
4801  }
4802  else if (j2array[0] < -IKPI)
4803  {
4804  j2array[0] += IK2PI;
4805  }
4806  j2valid[0] = true;
4807  for (int ij2 = 0; ij2 < 1; ++ij2)
4808  {
4809  if (!j2valid[ij2])
4810  {
4811  continue;
4812  }
4813  _ij2[0] = ij2;
4814  _ij2[1] = -1;
4815  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4816  {
4817  if (j2valid[iij2] &&
4818  IKabs(cj2array[ij2] - cj2array[iij2]) <
4819  IKFAST_SOLUTION_THRESH &&
4820  IKabs(sj2array[ij2] - sj2array[iij2]) <
4821  IKFAST_SOLUTION_THRESH)
4822  {
4823  j2valid[iij2] = false;
4824  _ij2[1] = iij2;
4825  break;
4826  }
4827  }
4828  j2 = j2array[ij2];
4829  cj2 = cj2array[ij2];
4830  sj2 = sj2array[ij2];
4831  {
4832  IkReal evalcond[8];
4833  IkReal x167 = IKsin(j2);
4834  IkReal x168 = IKcos(j2);
4835  if ((((1.0) +
4836  (((-1.0) * (gconst13 * gconst13))))) <
4837  -0.00001)
4838  continue;
4839  IkReal x169 = IKsqrt(
4840  ((1.0) +
4841  (((-1.0) * (gconst13 * gconst13)))));
4842  IkReal x170 = ((1.0) * x169);
4843  evalcond[0] = x168;
4844  evalcond[1] = ((-1.0) * x167);
4845  evalcond[2] = (new_r10 + ((gconst13 * x167)));
4846  evalcond[3] = (new_r11 + ((gconst13 * x168)));
4847  evalcond[4] =
4848  (new_r00 + (((-1.0) * x167 * x170)));
4849  evalcond[5] =
4850  ((((-1.0) * x168 * x170)) + new_r01);
4851  evalcond[6] = (x167 + ((gconst13 * new_r10)) +
4852  (((-1.0) * new_r00 * x170)));
4853  evalcond[7] = (x168 + ((gconst13 * new_r11)) +
4854  (((-1.0) * new_r01 * x170)));
4855  if (IKabs(evalcond[0]) >
4856  IKFAST_EVALCOND_THRESH ||
4857  IKabs(evalcond[1]) >
4858  IKFAST_EVALCOND_THRESH ||
4859  IKabs(evalcond[2]) >
4860  IKFAST_EVALCOND_THRESH ||
4861  IKabs(evalcond[3]) >
4862  IKFAST_EVALCOND_THRESH ||
4863  IKabs(evalcond[4]) >
4864  IKFAST_EVALCOND_THRESH ||
4865  IKabs(evalcond[5]) >
4866  IKFAST_EVALCOND_THRESH ||
4867  IKabs(evalcond[6]) >
4868  IKFAST_EVALCOND_THRESH ||
4869  IKabs(evalcond[7]) >
4870  IKFAST_EVALCOND_THRESH)
4871  {
4872  continue;
4873  }
4874  }
4875 
4876  {
4877  std::vector<IkSingleDOFSolutionBase<IkReal> >
4878  vinfos(7);
4879  vinfos[0].jointtype = 1;
4880  vinfos[0].foffset = j0;
4881  vinfos[0].indices[0] = _ij0[0];
4882  vinfos[0].indices[1] = _ij0[1];
4883  vinfos[0].maxsolutions = _nj0;
4884  vinfos[1].jointtype = 1;
4885  vinfos[1].foffset = j1;
4886  vinfos[1].indices[0] = _ij1[0];
4887  vinfos[1].indices[1] = _ij1[1];
4888  vinfos[1].maxsolutions = _nj1;
4889  vinfos[2].jointtype = 1;
4890  vinfos[2].foffset = j2;
4891  vinfos[2].indices[0] = _ij2[0];
4892  vinfos[2].indices[1] = _ij2[1];
4893  vinfos[2].maxsolutions = _nj2;
4894  vinfos[3].jointtype = 1;
4895  vinfos[3].foffset = j3;
4896  vinfos[3].indices[0] = _ij3[0];
4897  vinfos[3].indices[1] = _ij3[1];
4898  vinfos[3].maxsolutions = _nj3;
4899  vinfos[4].jointtype = 1;
4900  vinfos[4].foffset = j4;
4901  vinfos[4].indices[0] = _ij4[0];
4902  vinfos[4].indices[1] = _ij4[1];
4903  vinfos[4].maxsolutions = _nj4;
4904  vinfos[5].jointtype = 1;
4905  vinfos[5].foffset = j5;
4906  vinfos[5].indices[0] = _ij5[0];
4907  vinfos[5].indices[1] = _ij5[1];
4908  vinfos[5].maxsolutions = _nj5;
4909  vinfos[6].jointtype = 1;
4910  vinfos[6].foffset = j6;
4911  vinfos[6].indices[0] = _ij6[0];
4912  vinfos[6].indices[1] = _ij6[1];
4913  vinfos[6].maxsolutions = _nj6;
4914  std::vector<int> vfree(0);
4915  solutions.AddSolution(vinfos, vfree);
4916  }
4917  }
4918  }
4919  }
4920  }
4921  }
4922  } while (0);
4923  if (bgotonextstatement)
4924  {
4925  bool bgotonextstatement = true;
4926  do
4927  {
4928  IkReal x171 = new_r22 * new_r22;
4929  CheckValue<IkReal> x172 = IKPowWithIntegerCheck(
4930  ((1.0) + (((-1.0) * x171))), -1);
4931  if (!x172.valid)
4932  {
4933  continue;
4934  }
4935  if ((((-1.0) * x171 * (x172.value))) < -0.00001)
4936  continue;
4937  IkReal gconst13 =
4938  ((-1.0) * (IKsqrt(((-1.0) * x171 * (x172.value)))));
4939  evalcond[0] =
4940  ((-3.14159265358979) +
4941  (IKfmod(((3.14159265358979) +
4942  (IKabs(((1.0) + (IKsign(sj0))))) +
4943  (IKabs((cj0 + (((-1.0) * gconst13)))))),
4944  6.28318530717959)));
4945  if (IKabs(evalcond[0]) < 0.0000050000000000)
4946  {
4947  bgotonextstatement = false;
4948  {
4949  IkReal j2eval[1];
4950  IkReal x173 = new_r22 * new_r22;
4951  new_r02 = 0;
4952  new_r12 = 0;
4953  new_r20 = 0;
4954  new_r21 = 0;
4955  if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4956  -0.00001)
4957  continue;
4958  sj0 = ((-1.0) *
4959  (IKsqrt(((1.0) + (((-1.0) * (gconst13 *
4960  gconst13)))))));
4961  cj0 = gconst13;
4962  if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4963  (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4964  continue;
4965  j0 = ((-1.0) * (IKacos(gconst13)));
4966  CheckValue<IkReal> x174 = IKPowWithIntegerCheck(
4967  ((1.0) + (((-1.0) * x173))), -1);
4968  if (!x174.valid)
4969  {
4970  continue;
4971  }
4972  if ((((-1.0) * x173 * (x174.value))) < -0.00001)
4973  continue;
4974  IkReal gconst13 =
4975  ((-1.0) *
4976  (IKsqrt(((-1.0) * x173 * (x174.value)))));
4977  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4978  if (IKabs(j2eval[0]) < 0.0000010000000000)
4979  {
4980  {
4981  IkReal j2array[1], cj2array[1], sj2array[1];
4982  bool j2valid[1] = { false };
4983  _nj2 = 1;
4984  CheckValue<IkReal> x175 =
4985  IKPowWithIntegerCheck(gconst13, -1);
4986  if (!x175.valid)
4987  {
4988  continue;
4989  }
4990  if ((((1.0) +
4991  (((-1.0) * (gconst13 * gconst13))))) <
4992  -0.00001)
4993  continue;
4994  if (IKabs(((-1.0) * new_r10 * (x175.value))) <
4995  IKFAST_ATAN2_MAGTHRESH &&
4996  IKabs(((((-1.0) * gconst13 * new_r11)) +
4997  (((-1.0) * new_r01 *
4998  (IKsqrt(((1.0) +
4999  (((-1.0) *
5000  (gconst13 *
5001  gconst13)))))))))) <
5002  IKFAST_ATAN2_MAGTHRESH &&
5003  IKabs(IKsqr(((-1.0) * new_r10 *
5004  (x175.value))) +
5005  IKsqr((
5006  (((-1.0) * gconst13 * new_r11)) +
5007  (((-1.0) * new_r01 *
5008  (IKsqrt(((1.0) +
5009  (((-1.0) *
5010  (gconst13 *
5011  gconst13)))))))))) -
5012  1) <= IKFAST_SINCOS_THRESH)
5013  continue;
5014  j2array[0] = IKatan2(
5015  ((-1.0) * new_r10 * (x175.value)),
5016  ((((-1.0) * gconst13 * new_r11)) +
5017  (((-1.0) * new_r01 *
5018  (IKsqrt(((1.0) +
5019  (((-1.0) * (gconst13 *
5020  gconst13))))))))));
5021  sj2array[0] = IKsin(j2array[0]);
5022  cj2array[0] = IKcos(j2array[0]);
5023  if (j2array[0] > IKPI)
5024  {
5025  j2array[0] -= IK2PI;
5026  }
5027  else if (j2array[0] < -IKPI)
5028  {
5029  j2array[0] += IK2PI;
5030  }
5031  j2valid[0] = true;
5032  for (int ij2 = 0; ij2 < 1; ++ij2)
5033  {
5034  if (!j2valid[ij2])
5035  {
5036  continue;
5037  }
5038  _ij2[0] = ij2;
5039  _ij2[1] = -1;
5040  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5041  {
5042  if (j2valid[iij2] &&
5043  IKabs(cj2array[ij2] - cj2array[iij2]) <
5044  IKFAST_SOLUTION_THRESH &&
5045  IKabs(sj2array[ij2] - sj2array[iij2]) <
5046  IKFAST_SOLUTION_THRESH)
5047  {
5048  j2valid[iij2] = false;
5049  _ij2[1] = iij2;
5050  break;
5051  }
5052  }
5053  j2 = j2array[ij2];
5054  cj2 = cj2array[ij2];
5055  sj2 = sj2array[ij2];
5056  {
5057  IkReal evalcond[8];
5058  IkReal x176 = IKsin(j2);
5059  IkReal x177 = IKcos(j2);
5060  if ((((1.0) +
5061  (((-1.0) * (gconst13 * gconst13))))) <
5062  -0.00001)
5063  continue;
5064  IkReal x178 = IKsqrt(
5065  ((1.0) +
5066  (((-1.0) * (gconst13 * gconst13)))));
5067  evalcond[0] = x177;
5068  evalcond[1] = ((-1.0) * x176);
5069  evalcond[2] =
5070  (new_r10 + ((gconst13 * x176)));
5071  evalcond[3] =
5072  (new_r11 + ((gconst13 * x177)));
5073  evalcond[4] = (((x176 * x178)) + new_r00);
5074  evalcond[5] = (new_r01 + ((x177 * x178)));
5075  evalcond[6] = (((new_r00 * x178)) + x176 +
5076  ((gconst13 * new_r10)));
5077  evalcond[7] = (((new_r01 * x178)) + x177 +
5078  ((gconst13 * new_r11)));
5079  if (IKabs(evalcond[0]) >
5080  IKFAST_EVALCOND_THRESH ||
5081  IKabs(evalcond[1]) >
5082  IKFAST_EVALCOND_THRESH ||
5083  IKabs(evalcond[2]) >
5084  IKFAST_EVALCOND_THRESH ||
5085  IKabs(evalcond[3]) >
5086  IKFAST_EVALCOND_THRESH ||
5087  IKabs(evalcond[4]) >
5088  IKFAST_EVALCOND_THRESH ||
5089  IKabs(evalcond[5]) >
5090  IKFAST_EVALCOND_THRESH ||
5091  IKabs(evalcond[6]) >
5092  IKFAST_EVALCOND_THRESH ||
5093  IKabs(evalcond[7]) >
5094  IKFAST_EVALCOND_THRESH)
5095  {
5096  continue;
5097  }
5098  }
5099 
5100  {
5101  std::vector<
5102  IkSingleDOFSolutionBase<IkReal> >
5103  vinfos(7);
5104  vinfos[0].jointtype = 1;
5105  vinfos[0].foffset = j0;
5106  vinfos[0].indices[0] = _ij0[0];
5107  vinfos[0].indices[1] = _ij0[1];
5108  vinfos[0].maxsolutions = _nj0;
5109  vinfos[1].jointtype = 1;
5110  vinfos[1].foffset = j1;
5111  vinfos[1].indices[0] = _ij1[0];
5112  vinfos[1].indices[1] = _ij1[1];
5113  vinfos[1].maxsolutions = _nj1;
5114  vinfos[2].jointtype = 1;
5115  vinfos[2].foffset = j2;
5116  vinfos[2].indices[0] = _ij2[0];
5117  vinfos[2].indices[1] = _ij2[1];
5118  vinfos[2].maxsolutions = _nj2;
5119  vinfos[3].jointtype = 1;
5120  vinfos[3].foffset = j3;
5121  vinfos[3].indices[0] = _ij3[0];
5122  vinfos[3].indices[1] = _ij3[1];
5123  vinfos[3].maxsolutions = _nj3;
5124  vinfos[4].jointtype = 1;
5125  vinfos[4].foffset = j4;
5126  vinfos[4].indices[0] = _ij4[0];
5127  vinfos[4].indices[1] = _ij4[1];
5128  vinfos[4].maxsolutions = _nj4;
5129  vinfos[5].jointtype = 1;
5130  vinfos[5].foffset = j5;
5131  vinfos[5].indices[0] = _ij5[0];
5132  vinfos[5].indices[1] = _ij5[1];
5133  vinfos[5].maxsolutions = _nj5;
5134  vinfos[6].jointtype = 1;
5135  vinfos[6].foffset = j6;
5136  vinfos[6].indices[0] = _ij6[0];
5137  vinfos[6].indices[1] = _ij6[1];
5138  vinfos[6].maxsolutions = _nj6;
5139  std::vector<int> vfree(0);
5140  solutions.AddSolution(vinfos, vfree);
5141  }
5142  }
5143  }
5144  }
5145  else
5146  {
5147  {
5148  IkReal j2array[1], cj2array[1], sj2array[1];
5149  bool j2valid[1] = { false };
5150  _nj2 = 1;
5151  CheckValue<IkReal> x179 =
5152  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
5153  IkReal(((-1.0) * new_r11)),
5154  IKFAST_ATAN2_MAGTHRESH);
5155  if (!x179.valid)
5156  {
5157  continue;
5158  }
5159  CheckValue<IkReal> x180 =
5160  IKPowWithIntegerCheck(IKsign(gconst13), -1);
5161  if (!x180.valid)
5162  {
5163  continue;
5164  }
5165  j2array[0] =
5166  ((-1.5707963267949) + (x179.value) +
5167  (((1.5707963267949) * (x180.value))));
5168  sj2array[0] = IKsin(j2array[0]);
5169  cj2array[0] = IKcos(j2array[0]);
5170  if (j2array[0] > IKPI)
5171  {
5172  j2array[0] -= IK2PI;
5173  }
5174  else if (j2array[0] < -IKPI)
5175  {
5176  j2array[0] += IK2PI;
5177  }
5178  j2valid[0] = true;
5179  for (int ij2 = 0; ij2 < 1; ++ij2)
5180  {
5181  if (!j2valid[ij2])
5182  {
5183  continue;
5184  }
5185  _ij2[0] = ij2;
5186  _ij2[1] = -1;
5187  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5188  {
5189  if (j2valid[iij2] &&
5190  IKabs(cj2array[ij2] - cj2array[iij2]) <
5191  IKFAST_SOLUTION_THRESH &&
5192  IKabs(sj2array[ij2] - sj2array[iij2]) <
5193  IKFAST_SOLUTION_THRESH)
5194  {
5195  j2valid[iij2] = false;
5196  _ij2[1] = iij2;
5197  break;
5198  }
5199  }
5200  j2 = j2array[ij2];
5201  cj2 = cj2array[ij2];
5202  sj2 = sj2array[ij2];
5203  {
5204  IkReal evalcond[8];
5205  IkReal x181 = IKsin(j2);
5206  IkReal x182 = IKcos(j2);
5207  if ((((1.0) +
5208  (((-1.0) * (gconst13 * gconst13))))) <
5209  -0.00001)
5210  continue;
5211  IkReal x183 = IKsqrt(
5212  ((1.0) +
5213  (((-1.0) * (gconst13 * gconst13)))));
5214  evalcond[0] = x182;
5215  evalcond[1] = ((-1.0) * x181);
5216  evalcond[2] =
5217  (((gconst13 * x181)) + new_r10);
5218  evalcond[3] =
5219  (((gconst13 * x182)) + new_r11);
5220  evalcond[4] = (new_r00 + ((x181 * x183)));
5221  evalcond[5] = (new_r01 + ((x182 * x183)));
5222  evalcond[6] = (((new_r00 * x183)) + x181 +
5223  ((gconst13 * new_r10)));
5224  evalcond[7] = (((new_r01 * x183)) + x182 +
5225  ((gconst13 * new_r11)));
5226  if (IKabs(evalcond[0]) >
5227  IKFAST_EVALCOND_THRESH ||
5228  IKabs(evalcond[1]) >
5229  IKFAST_EVALCOND_THRESH ||
5230  IKabs(evalcond[2]) >
5231  IKFAST_EVALCOND_THRESH ||
5232  IKabs(evalcond[3]) >
5233  IKFAST_EVALCOND_THRESH ||
5234  IKabs(evalcond[4]) >
5235  IKFAST_EVALCOND_THRESH ||
5236  IKabs(evalcond[5]) >
5237  IKFAST_EVALCOND_THRESH ||
5238  IKabs(evalcond[6]) >
5239  IKFAST_EVALCOND_THRESH ||
5240  IKabs(evalcond[7]) >
5241  IKFAST_EVALCOND_THRESH)
5242  {
5243  continue;
5244  }
5245  }
5246 
5247  {
5248  std::vector<
5249  IkSingleDOFSolutionBase<IkReal> >
5250  vinfos(7);
5251  vinfos[0].jointtype = 1;
5252  vinfos[0].foffset = j0;
5253  vinfos[0].indices[0] = _ij0[0];
5254  vinfos[0].indices[1] = _ij0[1];
5255  vinfos[0].maxsolutions = _nj0;
5256  vinfos[1].jointtype = 1;
5257  vinfos[1].foffset = j1;
5258  vinfos[1].indices[0] = _ij1[0];
5259  vinfos[1].indices[1] = _ij1[1];
5260  vinfos[1].maxsolutions = _nj1;
5261  vinfos[2].jointtype = 1;
5262  vinfos[2].foffset = j2;
5263  vinfos[2].indices[0] = _ij2[0];
5264  vinfos[2].indices[1] = _ij2[1];
5265  vinfos[2].maxsolutions = _nj2;
5266  vinfos[3].jointtype = 1;
5267  vinfos[3].foffset = j3;
5268  vinfos[3].indices[0] = _ij3[0];
5269  vinfos[3].indices[1] = _ij3[1];
5270  vinfos[3].maxsolutions = _nj3;
5271  vinfos[4].jointtype = 1;
5272  vinfos[4].foffset = j4;
5273  vinfos[4].indices[0] = _ij4[0];
5274  vinfos[4].indices[1] = _ij4[1];
5275  vinfos[4].maxsolutions = _nj4;
5276  vinfos[5].jointtype = 1;
5277  vinfos[5].foffset = j5;
5278  vinfos[5].indices[0] = _ij5[0];
5279  vinfos[5].indices[1] = _ij5[1];
5280  vinfos[5].maxsolutions = _nj5;
5281  vinfos[6].jointtype = 1;
5282  vinfos[6].foffset = j6;
5283  vinfos[6].indices[0] = _ij6[0];
5284  vinfos[6].indices[1] = _ij6[1];
5285  vinfos[6].maxsolutions = _nj6;
5286  std::vector<int> vfree(0);
5287  solutions.AddSolution(vinfos, vfree);
5288  }
5289  }
5290  }
5291  }
5292  }
5293  }
5294  } while (0);
5295  if (bgotonextstatement)
5296  {
5297  bool bgotonextstatement = true;
5298  do
5299  {
5300  if (1)
5301  {
5302  bgotonextstatement = false;
5303  continue; // branch miss [j2]
5304  }
5305  } while (0);
5306  if (bgotonextstatement)
5307  {
5308  }
5309  }
5310  }
5311  }
5312  }
5313  }
5314  }
5315  }
5316  }
5317  else
5318  {
5319  {
5320  IkReal j2array[1], cj2array[1], sj2array[1];
5321  bool j2valid[1] = { false };
5322  _nj2 = 1;
5323  IkReal x184 = (new_r01 * new_r22);
5324  IkReal x185 = (cj0 * new_r11);
5325  CheckValue<IkReal> x186 = IKPowWithIntegerCheck(cj0, -1);
5326  if (!x186.valid)
5327  {
5328  continue;
5329  }
5330  if (IKabs(((x186.value) *
5331  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5332  (((-1.0) * x184)) + (((-1.0) * new_r10)))))) <
5333  IKFAST_ATAN2_MAGTHRESH &&
5334  IKabs((((new_r01 * sj0)) + (((-1.0) * x185)))) <
5335  IKFAST_ATAN2_MAGTHRESH &&
5336  IKabs(
5337  IKsqr(((x186.value) *
5338  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5339  (((-1.0) * x184)) + (((-1.0) * new_r10)))))) +
5340  IKsqr((((new_r01 * sj0)) + (((-1.0) * x185)))) - 1) <=
5341  IKFAST_SINCOS_THRESH)
5342  continue;
5343  j2array[0] =
5344  IKatan2(((x186.value) *
5345  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5346  (((-1.0) * x184)) + (((-1.0) * new_r10))))),
5347  (((new_r01 * sj0)) + (((-1.0) * x185))));
5348  sj2array[0] = IKsin(j2array[0]);
5349  cj2array[0] = IKcos(j2array[0]);
5350  if (j2array[0] > IKPI)
5351  {
5352  j2array[0] -= IK2PI;
5353  }
5354  else if (j2array[0] < -IKPI)
5355  {
5356  j2array[0] += IK2PI;
5357  }
5358  j2valid[0] = true;
5359  for (int ij2 = 0; ij2 < 1; ++ij2)
5360  {
5361  if (!j2valid[ij2])
5362  {
5363  continue;
5364  }
5365  _ij2[0] = ij2;
5366  _ij2[1] = -1;
5367  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5368  {
5369  if (j2valid[iij2] &&
5370  IKabs(cj2array[ij2] - cj2array[iij2]) <
5371  IKFAST_SOLUTION_THRESH &&
5372  IKabs(sj2array[ij2] - sj2array[iij2]) <
5373  IKFAST_SOLUTION_THRESH)
5374  {
5375  j2valid[iij2] = false;
5376  _ij2[1] = iij2;
5377  break;
5378  }
5379  }
5380  j2 = j2array[ij2];
5381  cj2 = cj2array[ij2];
5382  sj2 = sj2array[ij2];
5383  {
5384  IkReal evalcond[10];
5385  IkReal x187 = IKcos(j2);
5386  IkReal x188 = IKsin(j2);
5387  IkReal x189 = (cj0 * new_r22);
5388  IkReal x190 = ((1.0) * sj0);
5389  IkReal x191 = (new_r22 * sj0);
5390  IkReal x192 = (new_r22 * x188);
5391  IkReal x193 = ((1.0) * x188);
5392  evalcond[0] =
5393  (x188 + (((-1.0) * new_r00 * x190)) + ((cj0 * new_r10)));
5394  evalcond[1] =
5395  (x187 + (((-1.0) * new_r01 * x190)) + ((cj0 * new_r11)));
5396  evalcond[2] = (((new_r10 * sj0)) + ((new_r22 * x187)) +
5397  ((cj0 * new_r00)));
5398  evalcond[3] = (((new_r10 * x191)) + ((new_r00 * x189)) + x187);
5399  evalcond[4] = (((cj0 * x188)) + ((x187 * x191)) + new_r10);
5400  evalcond[5] =
5401  (((new_r11 * sj0)) + (((-1.0) * x192)) + ((cj0 * new_r01)));
5402  evalcond[6] =
5403  ((((-1.0) * x188 * x190)) + ((x187 * x189)) + new_r00);
5404  evalcond[7] =
5405  (((cj0 * x187)) + (((-1.0) * x190 * x192)) + new_r11);
5406  evalcond[8] = (((new_r11 * x191)) + ((new_r01 * x189)) +
5407  (((-1.0) * x193)));
5408  evalcond[9] = ((((-1.0) * x189 * x193)) +
5409  (((-1.0) * x187 * x190)) + new_r01);
5410  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5411  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5412  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5413  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5414  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5415  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5416  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5417  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5418  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5419  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5420  {
5421  continue;
5422  }
5423  }
5424 
5425  {
5426  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5427  vinfos[0].jointtype = 1;
5428  vinfos[0].foffset = j0;
5429  vinfos[0].indices[0] = _ij0[0];
5430  vinfos[0].indices[1] = _ij0[1];
5431  vinfos[0].maxsolutions = _nj0;
5432  vinfos[1].jointtype = 1;
5433  vinfos[1].foffset = j1;
5434  vinfos[1].indices[0] = _ij1[0];
5435  vinfos[1].indices[1] = _ij1[1];
5436  vinfos[1].maxsolutions = _nj1;
5437  vinfos[2].jointtype = 1;
5438  vinfos[2].foffset = j2;
5439  vinfos[2].indices[0] = _ij2[0];
5440  vinfos[2].indices[1] = _ij2[1];
5441  vinfos[2].maxsolutions = _nj2;
5442  vinfos[3].jointtype = 1;
5443  vinfos[3].foffset = j3;
5444  vinfos[3].indices[0] = _ij3[0];
5445  vinfos[3].indices[1] = _ij3[1];
5446  vinfos[3].maxsolutions = _nj3;
5447  vinfos[4].jointtype = 1;
5448  vinfos[4].foffset = j4;
5449  vinfos[4].indices[0] = _ij4[0];
5450  vinfos[4].indices[1] = _ij4[1];
5451  vinfos[4].maxsolutions = _nj4;
5452  vinfos[5].jointtype = 1;
5453  vinfos[5].foffset = j5;
5454  vinfos[5].indices[0] = _ij5[0];
5455  vinfos[5].indices[1] = _ij5[1];
5456  vinfos[5].maxsolutions = _nj5;
5457  vinfos[6].jointtype = 1;
5458  vinfos[6].foffset = j6;
5459  vinfos[6].indices[0] = _ij6[0];
5460  vinfos[6].indices[1] = _ij6[1];
5461  vinfos[6].maxsolutions = _nj6;
5462  std::vector<int> vfree(0);
5463  solutions.AddSolution(vinfos, vfree);
5464  }
5465  }
5466  }
5467  }
5468  }
5469  }
5470  else
5471  {
5472  {
5473  IkReal j2array[1], cj2array[1], sj2array[1];
5474  bool j2valid[1] = { false };
5475  _nj2 = 1;
5476  IkReal x194 = ((1.0) * cj0);
5477  CheckValue<IkReal> x195 = IKPowWithIntegerCheck(new_r22, -1);
5478  if (!x195.valid)
5479  {
5480  continue;
5481  }
5482  if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) <
5483  IKFAST_ATAN2_MAGTHRESH &&
5484  IKabs(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5485  (((-1.0) * new_r00 * x194)))))) <
5486  IKFAST_ATAN2_MAGTHRESH &&
5487  IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) +
5488  IKsqr(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5489  (((-1.0) * new_r00 * x194)))))) -
5490  1) <= IKFAST_SINCOS_THRESH)
5491  continue;
5492  j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194))),
5493  ((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5494  (((-1.0) * new_r00 * x194))))));
5495  sj2array[0] = IKsin(j2array[0]);
5496  cj2array[0] = IKcos(j2array[0]);
5497  if (j2array[0] > IKPI)
5498  {
5499  j2array[0] -= IK2PI;
5500  }
5501  else if (j2array[0] < -IKPI)
5502  {
5503  j2array[0] += IK2PI;
5504  }
5505  j2valid[0] = true;
5506  for (int ij2 = 0; ij2 < 1; ++ij2)
5507  {
5508  if (!j2valid[ij2])
5509  {
5510  continue;
5511  }
5512  _ij2[0] = ij2;
5513  _ij2[1] = -1;
5514  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5515  {
5516  if (j2valid[iij2] &&
5517  IKabs(cj2array[ij2] - cj2array[iij2]) <
5518  IKFAST_SOLUTION_THRESH &&
5519  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5520  {
5521  j2valid[iij2] = false;
5522  _ij2[1] = iij2;
5523  break;
5524  }
5525  }
5526  j2 = j2array[ij2];
5527  cj2 = cj2array[ij2];
5528  sj2 = sj2array[ij2];
5529  {
5530  IkReal evalcond[10];
5531  IkReal x196 = IKcos(j2);
5532  IkReal x197 = IKsin(j2);
5533  IkReal x198 = (cj0 * new_r22);
5534  IkReal x199 = ((1.0) * sj0);
5535  IkReal x200 = (new_r22 * sj0);
5536  IkReal x201 = (new_r22 * x197);
5537  IkReal x202 = ((1.0) * x197);
5538  evalcond[0] =
5539  (x197 + (((-1.0) * new_r00 * x199)) + ((cj0 * new_r10)));
5540  evalcond[1] =
5541  (x196 + (((-1.0) * new_r01 * x199)) + ((cj0 * new_r11)));
5542  evalcond[2] =
5543  (((new_r10 * sj0)) + ((new_r22 * x196)) + ((cj0 * new_r00)));
5544  evalcond[3] = (((new_r00 * x198)) + ((new_r10 * x200)) + x196);
5545  evalcond[4] = (((x196 * x200)) + ((cj0 * x197)) + new_r10);
5546  evalcond[5] =
5547  (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x201)));
5548  evalcond[6] =
5549  ((((-1.0) * x197 * x199)) + ((x196 * x198)) + new_r00);
5550  evalcond[7] = (((cj0 * x196)) + (((-1.0) * x199 * x201)) + new_r11);
5551  evalcond[8] =
5552  (((new_r01 * x198)) + ((new_r11 * x200)) + (((-1.0) * x202)));
5553  evalcond[9] =
5554  ((((-1.0) * x196 * x199)) + (((-1.0) * x198 * x202)) + new_r01);
5555  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5556  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5557  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5558  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5559  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5560  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5561  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5562  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5563  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5564  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5565  {
5566  continue;
5567  }
5568  }
5569 
5570  {
5571  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5572  vinfos[0].jointtype = 1;
5573  vinfos[0].foffset = j0;
5574  vinfos[0].indices[0] = _ij0[0];
5575  vinfos[0].indices[1] = _ij0[1];
5576  vinfos[0].maxsolutions = _nj0;
5577  vinfos[1].jointtype = 1;
5578  vinfos[1].foffset = j1;
5579  vinfos[1].indices[0] = _ij1[0];
5580  vinfos[1].indices[1] = _ij1[1];
5581  vinfos[1].maxsolutions = _nj1;
5582  vinfos[2].jointtype = 1;
5583  vinfos[2].foffset = j2;
5584  vinfos[2].indices[0] = _ij2[0];
5585  vinfos[2].indices[1] = _ij2[1];
5586  vinfos[2].maxsolutions = _nj2;
5587  vinfos[3].jointtype = 1;
5588  vinfos[3].foffset = j3;
5589  vinfos[3].indices[0] = _ij3[0];
5590  vinfos[3].indices[1] = _ij3[1];
5591  vinfos[3].maxsolutions = _nj3;
5592  vinfos[4].jointtype = 1;
5593  vinfos[4].foffset = j4;
5594  vinfos[4].indices[0] = _ij4[0];
5595  vinfos[4].indices[1] = _ij4[1];
5596  vinfos[4].maxsolutions = _nj4;
5597  vinfos[5].jointtype = 1;
5598  vinfos[5].foffset = j5;
5599  vinfos[5].indices[0] = _ij5[0];
5600  vinfos[5].indices[1] = _ij5[1];
5601  vinfos[5].maxsolutions = _nj5;
5602  vinfos[6].jointtype = 1;
5603  vinfos[6].foffset = j6;
5604  vinfos[6].indices[0] = _ij6[0];
5605  vinfos[6].indices[1] = _ij6[1];
5606  vinfos[6].maxsolutions = _nj6;
5607  std::vector<int> vfree(0);
5608  solutions.AddSolution(vinfos, vfree);
5609  }
5610  }
5611  }
5612  }
5613  }
5614  }
5615  else
5616  {
5617  {
5618  IkReal j2array[1], cj2array[1], sj2array[1];
5619  bool j2valid[1] = { false };
5620  _nj2 = 1;
5621  IkReal x203 = cj0 * cj0;
5622  IkReal x204 = new_r22 * new_r22;
5623  IkReal x205 = ((1.0) * cj0);
5624  IkReal x206 = (new_r22 * sj0);
5625  CheckValue<IkReal> x207 = IKatan2WithCheck(
5626  IkReal((((new_r11 * x206)) + (((-1.0) * new_r10 * x205)))),
5627  IkReal(((((-1.0) * new_r11 * x205)) + (((-1.0) * new_r10 * x206)))),
5628  IKFAST_ATAN2_MAGTHRESH);
5629  if (!x207.valid)
5630  {
5631  continue;
5632  }
5633  CheckValue<IkReal> x208 = IKPowWithIntegerCheck(
5634  IKsign((x204 + x203 + (((-1.0) * x203 * x204)))), -1);
5635  if (!x208.valid)
5636  {
5637  continue;
5638  }
5639  j2array[0] = ((-1.5707963267949) + (x207.value) +
5640  (((1.5707963267949) * (x208.value))));
5641  sj2array[0] = IKsin(j2array[0]);
5642  cj2array[0] = IKcos(j2array[0]);
5643  if (j2array[0] > IKPI)
5644  {
5645  j2array[0] -= IK2PI;
5646  }
5647  else if (j2array[0] < -IKPI)
5648  {
5649  j2array[0] += IK2PI;
5650  }
5651  j2valid[0] = true;
5652  for (int ij2 = 0; ij2 < 1; ++ij2)
5653  {
5654  if (!j2valid[ij2])
5655  {
5656  continue;
5657  }
5658  _ij2[0] = ij2;
5659  _ij2[1] = -1;
5660  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5661  {
5662  if (j2valid[iij2] &&
5663  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5664  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5665  {
5666  j2valid[iij2] = false;
5667  _ij2[1] = iij2;
5668  break;
5669  }
5670  }
5671  j2 = j2array[ij2];
5672  cj2 = cj2array[ij2];
5673  sj2 = sj2array[ij2];
5674  {
5675  IkReal evalcond[10];
5676  IkReal x209 = IKcos(j2);
5677  IkReal x210 = IKsin(j2);
5678  IkReal x211 = (cj0 * new_r22);
5679  IkReal x212 = ((1.0) * sj0);
5680  IkReal x213 = (new_r22 * sj0);
5681  IkReal x214 = (new_r22 * x210);
5682  IkReal x215 = ((1.0) * x210);
5683  evalcond[0] = (x210 + (((-1.0) * new_r00 * x212)) + ((cj0 * new_r10)));
5684  evalcond[1] = (x209 + (((-1.0) * new_r01 * x212)) + ((cj0 * new_r11)));
5685  evalcond[2] =
5686  (((new_r10 * sj0)) + ((new_r22 * x209)) + ((cj0 * new_r00)));
5687  evalcond[3] = (x209 + ((new_r00 * x211)) + ((new_r10 * x213)));
5688  evalcond[4] = (((cj0 * x210)) + ((x209 * x213)) + new_r10);
5689  evalcond[5] =
5690  (((new_r11 * sj0)) + (((-1.0) * x214)) + ((cj0 * new_r01)));
5691  evalcond[6] = (((x209 * x211)) + (((-1.0) * x210 * x212)) + new_r00);
5692  evalcond[7] = (((cj0 * x209)) + new_r11 + (((-1.0) * x212 * x214)));
5693  evalcond[8] =
5694  (((new_r01 * x211)) + (((-1.0) * x215)) + ((new_r11 * x213)));
5695  evalcond[9] =
5696  ((((-1.0) * x211 * x215)) + new_r01 + (((-1.0) * x209 * x212)));
5697  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5698  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5699  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5700  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5701  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5702  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5703  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5704  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5705  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5706  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5707  {
5708  continue;
5709  }
5710  }
5711 
5712  {
5713  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5714  vinfos[0].jointtype = 1;
5715  vinfos[0].foffset = j0;
5716  vinfos[0].indices[0] = _ij0[0];
5717  vinfos[0].indices[1] = _ij0[1];
5718  vinfos[0].maxsolutions = _nj0;
5719  vinfos[1].jointtype = 1;
5720  vinfos[1].foffset = j1;
5721  vinfos[1].indices[0] = _ij1[0];
5722  vinfos[1].indices[1] = _ij1[1];
5723  vinfos[1].maxsolutions = _nj1;
5724  vinfos[2].jointtype = 1;
5725  vinfos[2].foffset = j2;
5726  vinfos[2].indices[0] = _ij2[0];
5727  vinfos[2].indices[1] = _ij2[1];
5728  vinfos[2].maxsolutions = _nj2;
5729  vinfos[3].jointtype = 1;
5730  vinfos[3].foffset = j3;
5731  vinfos[3].indices[0] = _ij3[0];
5732  vinfos[3].indices[1] = _ij3[1];
5733  vinfos[3].maxsolutions = _nj3;
5734  vinfos[4].jointtype = 1;
5735  vinfos[4].foffset = j4;
5736  vinfos[4].indices[0] = _ij4[0];
5737  vinfos[4].indices[1] = _ij4[1];
5738  vinfos[4].maxsolutions = _nj4;
5739  vinfos[5].jointtype = 1;
5740  vinfos[5].foffset = j5;
5741  vinfos[5].indices[0] = _ij5[0];
5742  vinfos[5].indices[1] = _ij5[1];
5743  vinfos[5].maxsolutions = _nj5;
5744  vinfos[6].jointtype = 1;
5745  vinfos[6].foffset = j6;
5746  vinfos[6].indices[0] = _ij6[0];
5747  vinfos[6].indices[1] = _ij6[1];
5748  vinfos[6].maxsolutions = _nj6;
5749  std::vector<int> vfree(0);
5750  solutions.AddSolution(vinfos, vfree);
5751  }
5752  }
5753  }
5754  }
5755  }
5756  }
5757  }
5758  }
5759  }
5760  } while (0);
5761  if (bgotonextstatement)
5762  {
5763  bool bgotonextstatement = true;
5764  do
5765  {
5766  if (1)
5767  {
5768  bgotonextstatement = false;
5769  continue; // branch miss [j0, j2]
5770  }
5771  } while (0);
5772  if (bgotonextstatement)
5773  {
5774  }
5775  }
5776  }
5777  }
5778  }
5779  }
5780  else
5781  {
5782  {
5783  IkReal j0array[1], cj0array[1], sj0array[1];
5784  bool j0valid[1] = { false };
5785  _nj0 = 1;
5786  CheckValue<IkReal> x217 = IKPowWithIntegerCheck(sj1, -1);
5787  if (!x217.valid)
5788  {
5789  continue;
5790  }
5791  IkReal x216 = x217.value;
5792  CheckValue<IkReal> x218 = IKPowWithIntegerCheck(new_r12, -1);
5793  if (!x218.valid)
5794  {
5795  continue;
5796  }
5797  if (IKabs((x216 * (x218.value) *
5798  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) <
5799  IKFAST_ATAN2_MAGTHRESH &&
5800  IKabs((new_r02 * x216)) < IKFAST_ATAN2_MAGTHRESH &&
5801  IKabs(IKsqr((x216 * (x218.value) *
5802  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) +
5803  IKsqr((new_r02 * x216)) - 1) <= IKFAST_SINCOS_THRESH)
5804  continue;
5805  j0array[0] = IKatan2((x216 * (x218.value) *
5806  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1)))))),
5807  (new_r02 * x216));
5808  sj0array[0] = IKsin(j0array[0]);
5809  cj0array[0] = IKcos(j0array[0]);
5810  if (j0array[0] > IKPI)
5811  {
5812  j0array[0] -= IK2PI;
5813  }
5814  else if (j0array[0] < -IKPI)
5815  {
5816  j0array[0] += IK2PI;
5817  }
5818  j0valid[0] = true;
5819  for (int ij0 = 0; ij0 < 1; ++ij0)
5820  {
5821  if (!j0valid[ij0])
5822  {
5823  continue;
5824  }
5825  _ij0[0] = ij0;
5826  _ij0[1] = -1;
5827  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
5828  {
5829  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
5830  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
5831  {
5832  j0valid[iij0] = false;
5833  _ij0[1] = iij0;
5834  break;
5835  }
5836  }
5837  j0 = j0array[ij0];
5838  cj0 = cj0array[ij0];
5839  sj0 = sj0array[ij0];
5840  {
5841  IkReal evalcond[8];
5842  IkReal x219 = IKcos(j0);
5843  IkReal x220 = IKsin(j0);
5844  IkReal x221 = ((1.0) * cj1);
5845  IkReal x222 = ((1.0) * sj1);
5846  IkReal x223 = (new_r12 * x220);
5847  IkReal x224 = (new_r02 * x219);
5848  evalcond[0] = ((((-1.0) * x219 * x222)) + new_r02);
5849  evalcond[1] = ((((-1.0) * x220 * x222)) + new_r12);
5850  evalcond[2] = (((new_r12 * x219)) + (((-1.0) * new_r02 * x220)));
5851  evalcond[3] = (x223 + x224 + (((-1.0) * x222)));
5852  evalcond[4] = (((cj1 * x224)) + ((cj1 * x223)) + (((-1.0) * new_r22 * x222)));
5853  evalcond[5] = ((((-1.0) * new_r10 * x220 * x222)) + (((-1.0) * new_r00 * x219 * x222)) +
5854  (((-1.0) * new_r20 * x221)));
5855  evalcond[6] = ((((-1.0) * new_r11 * x220 * x222)) + (((-1.0) * new_r01 * x219 * x222)) +
5856  (((-1.0) * new_r21 * x221)));
5857  evalcond[7] = ((1.0) + (((-1.0) * x222 * x223)) + (((-1.0) * x222 * x224)) +
5858  (((-1.0) * new_r22 * x221)));
5859  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5860  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5861  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5862  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5863  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5864  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5865  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5866  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5867  {
5868  continue;
5869  }
5870  }
5871 
5872  {
5873  IkReal j2eval[3];
5874  j2eval[0] = sj1;
5875  j2eval[1] = IKsign(sj1);
5876  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
5877  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
5878  IKabs(j2eval[2]) < 0.0000010000000000)
5879  {
5880  {
5881  IkReal j2eval[2];
5882  j2eval[0] = sj1;
5883  j2eval[1] = cj0;
5884  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
5885  {
5886  {
5887  IkReal j2eval[3];
5888  j2eval[0] = sj1;
5889  j2eval[1] = cj1;
5890  j2eval[2] = sj0;
5891  if (IKabs(j2eval[0]) < 0.0000010000000000 ||
5892  IKabs(j2eval[1]) < 0.0000010000000000 || IKabs(j2eval[2]) < 0.0000010000000000)
5893  {
5894  {
5895  IkReal evalcond[5];
5896  bool bgotonextstatement = true;
5897  do
5898  {
5899  evalcond[0] =
5900  ((-3.14159265358979) +
5901  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
5902  evalcond[1] = new_r21;
5903  evalcond[2] = new_r02;
5904  evalcond[3] = new_r12;
5905  evalcond[4] = new_r20;
5906  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
5907  IKabs(evalcond[1]) < 0.0000050000000000 &&
5908  IKabs(evalcond[2]) < 0.0000050000000000 &&
5909  IKabs(evalcond[3]) < 0.0000050000000000 &&
5910  IKabs(evalcond[4]) < 0.0000050000000000)
5911  {
5912  bgotonextstatement = false;
5913  {
5914  IkReal j2array[1], cj2array[1], sj2array[1];
5915  bool j2valid[1] = { false };
5916  _nj2 = 1;
5917  IkReal x225 = ((1.0) * cj0);
5918  if (IKabs(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) <
5919  IKFAST_ATAN2_MAGTHRESH &&
5920  IKabs(((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225)))) <
5921  IKFAST_ATAN2_MAGTHRESH &&
5922  IKabs(IKsqr(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) +
5923  IKsqr(((((-1.0) * new_r10 * sj0)) +
5924  (((-1.0) * new_r00 * x225)))) -
5925  1) <= IKFAST_SINCOS_THRESH)
5926  continue;
5927  j2array[0] =
5928  IKatan2(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0))),
5929  ((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225))));
5930  sj2array[0] = IKsin(j2array[0]);
5931  cj2array[0] = IKcos(j2array[0]);
5932  if (j2array[0] > IKPI)
5933  {
5934  j2array[0] -= IK2PI;
5935  }
5936  else if (j2array[0] < -IKPI)
5937  {
5938  j2array[0] += IK2PI;
5939  }
5940  j2valid[0] = true;
5941  for (int ij2 = 0; ij2 < 1; ++ij2)
5942  {
5943  if (!j2valid[ij2])
5944  {
5945  continue;
5946  }
5947  _ij2[0] = ij2;
5948  _ij2[1] = -1;
5949  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5950  {
5951  if (j2valid[iij2] &&
5952  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5953  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5954  {
5955  j2valid[iij2] = false;
5956  _ij2[1] = iij2;
5957  break;
5958  }
5959  }
5960  j2 = j2array[ij2];
5961  cj2 = cj2array[ij2];
5962  sj2 = sj2array[ij2];
5963  {
5964  IkReal evalcond[8];
5965  IkReal x226 = IKcos(j2);
5966  IkReal x227 = IKsin(j2);
5967  IkReal x228 = ((1.0) * sj0);
5968  IkReal x229 = (cj0 * x226);
5969  IkReal x230 = (cj0 * x227);
5970  IkReal x231 = (x227 * x228);
5971  evalcond[0] = (((new_r10 * sj0)) + x226 + ((cj0 * new_r00)));
5972  evalcond[1] =
5973  (x227 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x228)));
5974  evalcond[2] =
5975  ((((-1.0) * new_r01 * x228)) + x226 + ((cj0 * new_r11)));
5976  evalcond[3] = (((sj0 * x226)) + x230 + new_r10);
5977  evalcond[4] =
5978  (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x227)));
5979  evalcond[5] = ((((-1.0) * x231)) + x229 + new_r00);
5980  evalcond[6] = ((((-1.0) * x231)) + x229 + new_r11);
5981  evalcond[7] =
5982  (new_r01 + (((-1.0) * x226 * x228)) + (((-1.0) * x230)));
5983  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5984  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5985  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5986  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5987  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5988  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5989  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5990  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5991  {
5992  continue;
5993  }
5994  }
5995 
5996  {
5997  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5998  vinfos[0].jointtype = 1;
5999  vinfos[0].foffset = j0;
6000  vinfos[0].indices[0] = _ij0[0];
6001  vinfos[0].indices[1] = _ij0[1];
6002  vinfos[0].maxsolutions = _nj0;
6003  vinfos[1].jointtype = 1;
6004  vinfos[1].foffset = j1;
6005  vinfos[1].indices[0] = _ij1[0];
6006  vinfos[1].indices[1] = _ij1[1];
6007  vinfos[1].maxsolutions = _nj1;
6008  vinfos[2].jointtype = 1;
6009  vinfos[2].foffset = j2;
6010  vinfos[2].indices[0] = _ij2[0];
6011  vinfos[2].indices[1] = _ij2[1];
6012  vinfos[2].maxsolutions = _nj2;
6013  vinfos[3].jointtype = 1;
6014  vinfos[3].foffset = j3;
6015  vinfos[3].indices[0] = _ij3[0];
6016  vinfos[3].indices[1] = _ij3[1];
6017  vinfos[3].maxsolutions = _nj3;
6018  vinfos[4].jointtype = 1;
6019  vinfos[4].foffset = j4;
6020  vinfos[4].indices[0] = _ij4[0];
6021  vinfos[4].indices[1] = _ij4[1];
6022  vinfos[4].maxsolutions = _nj4;
6023  vinfos[5].jointtype = 1;
6024  vinfos[5].foffset = j5;
6025  vinfos[5].indices[0] = _ij5[0];
6026  vinfos[5].indices[1] = _ij5[1];
6027  vinfos[5].maxsolutions = _nj5;
6028  vinfos[6].jointtype = 1;
6029  vinfos[6].foffset = j6;
6030  vinfos[6].indices[0] = _ij6[0];
6031  vinfos[6].indices[1] = _ij6[1];
6032  vinfos[6].maxsolutions = _nj6;
6033  std::vector<int> vfree(0);
6034  solutions.AddSolution(vinfos, vfree);
6035  }
6036  }
6037  }
6038  }
6039  } while (0);
6040  if (bgotonextstatement)
6041  {
6042  bool bgotonextstatement = true;
6043  do
6044  {
6045  evalcond[0] =
6046  ((-3.14159265358979) +
6047  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
6048  6.28318530717959)));
6049  evalcond[1] = new_r21;
6050  evalcond[2] = new_r02;
6051  evalcond[3] = new_r12;
6052  evalcond[4] = new_r20;
6053  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6054  IKabs(evalcond[1]) < 0.0000050000000000 &&
6055  IKabs(evalcond[2]) < 0.0000050000000000 &&
6056  IKabs(evalcond[3]) < 0.0000050000000000 &&
6057  IKabs(evalcond[4]) < 0.0000050000000000)
6058  {
6059  bgotonextstatement = false;
6060  {
6061  IkReal j2array[1], cj2array[1], sj2array[1];
6062  bool j2valid[1] = { false };
6063  _nj2 = 1;
6064  IkReal x232 = ((1.0) * cj0);
6065  if (IKabs(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232)))) <
6066  IKFAST_ATAN2_MAGTHRESH &&
6067  IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) <
6068  IKFAST_ATAN2_MAGTHRESH &&
6069  IKabs(IKsqr(((((-1.0) * new_r11 * sj0)) +
6070  (((-1.0) * new_r10 * x232)))) +
6071  IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) -
6072  1) <= IKFAST_SINCOS_THRESH)
6073  continue;
6074  j2array[0] =
6075  IKatan2(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232))),
6076  (((new_r10 * sj0)) + (((-1.0) * new_r11 * x232))));
6077  sj2array[0] = IKsin(j2array[0]);
6078  cj2array[0] = IKcos(j2array[0]);
6079  if (j2array[0] > IKPI)
6080  {
6081  j2array[0] -= IK2PI;
6082  }
6083  else if (j2array[0] < -IKPI)
6084  {
6085  j2array[0] += IK2PI;
6086  }
6087  j2valid[0] = true;
6088  for (int ij2 = 0; ij2 < 1; ++ij2)
6089  {
6090  if (!j2valid[ij2])
6091  {
6092  continue;
6093  }
6094  _ij2[0] = ij2;
6095  _ij2[1] = -1;
6096  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6097  {
6098  if (j2valid[iij2] &&
6099  IKabs(cj2array[ij2] - cj2array[iij2]) <
6100  IKFAST_SOLUTION_THRESH &&
6101  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
6102  {
6103  j2valid[iij2] = false;
6104  _ij2[1] = iij2;
6105  break;
6106  }
6107  }
6108  j2 = j2array[ij2];
6109  cj2 = cj2array[ij2];
6110  sj2 = sj2array[ij2];
6111  {
6112  IkReal evalcond[8];
6113  IkReal x233 = IKsin(j2);
6114  IkReal x234 = IKcos(j2);
6115  IkReal x235 = ((1.0) * sj0);
6116  IkReal x236 = (cj0 * x233);
6117  IkReal x237 = ((1.0) * x234);
6118  IkReal x238 = (x234 * x235);
6119  evalcond[0] = (((new_r11 * sj0)) + x233 + ((cj0 * new_r01)));
6120  evalcond[1] =
6121  (x233 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x235)));
6122  evalcond[2] =
6123  (x234 + (((-1.0) * new_r01 * x235)) + ((cj0 * new_r11)));
6124  evalcond[3] =
6125  (((new_r10 * sj0)) + (((-1.0) * x237)) + ((cj0 * new_r00)));
6126  evalcond[4] = (((sj0 * x233)) + new_r11 + ((cj0 * x234)));
6127  evalcond[5] = ((((-1.0) * x238)) + x236 + new_r10);
6128  evalcond[6] = ((((-1.0) * x238)) + x236 + new_r01);
6129  evalcond[7] =
6130  ((((-1.0) * x233 * x235)) + (((-1.0) * cj0 * x237)) + new_r00);
6131  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6132  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6133  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6134  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6135  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6136  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6137  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6138  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6139  {
6140  continue;
6141  }
6142  }
6143 
6144  {
6145  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6146  vinfos[0].jointtype = 1;
6147  vinfos[0].foffset = j0;
6148  vinfos[0].indices[0] = _ij0[0];
6149  vinfos[0].indices[1] = _ij0[1];
6150  vinfos[0].maxsolutions = _nj0;
6151  vinfos[1].jointtype = 1;
6152  vinfos[1].foffset = j1;
6153  vinfos[1].indices[0] = _ij1[0];
6154  vinfos[1].indices[1] = _ij1[1];
6155  vinfos[1].maxsolutions = _nj1;
6156  vinfos[2].jointtype = 1;
6157  vinfos[2].foffset = j2;
6158  vinfos[2].indices[0] = _ij2[0];
6159  vinfos[2].indices[1] = _ij2[1];
6160  vinfos[2].maxsolutions = _nj2;
6161  vinfos[3].jointtype = 1;
6162  vinfos[3].foffset = j3;
6163  vinfos[3].indices[0] = _ij3[0];
6164  vinfos[3].indices[1] = _ij3[1];
6165  vinfos[3].maxsolutions = _nj3;
6166  vinfos[4].jointtype = 1;
6167  vinfos[4].foffset = j4;
6168  vinfos[4].indices[0] = _ij4[0];
6169  vinfos[4].indices[1] = _ij4[1];
6170  vinfos[4].maxsolutions = _nj4;
6171  vinfos[5].jointtype = 1;
6172  vinfos[5].foffset = j5;
6173  vinfos[5].indices[0] = _ij5[0];
6174  vinfos[5].indices[1] = _ij5[1];
6175  vinfos[5].maxsolutions = _nj5;
6176  vinfos[6].jointtype = 1;
6177  vinfos[6].foffset = j6;
6178  vinfos[6].indices[0] = _ij6[0];
6179  vinfos[6].indices[1] = _ij6[1];
6180  vinfos[6].maxsolutions = _nj6;
6181  std::vector<int> vfree(0);
6182  solutions.AddSolution(vinfos, vfree);
6183  }
6184  }
6185  }
6186  }
6187  } while (0);
6188  if (bgotonextstatement)
6189  {
6190  bool bgotonextstatement = true;
6191  do
6192  {
6193  evalcond[0] =
6194  ((-3.14159265358979) +
6195  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
6196  6.28318530717959)));
6197  evalcond[1] = new_r22;
6198  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6199  IKabs(evalcond[1]) < 0.0000050000000000)
6200  {
6201  bgotonextstatement = false;
6202  {
6203  IkReal j2array[1], cj2array[1], sj2array[1];
6204  bool j2valid[1] = { false };
6205  _nj2 = 1;
6206  if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
6207  IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
6208  IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
6209  IKFAST_SINCOS_THRESH)
6210  continue;
6211  j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
6212  sj2array[0] = IKsin(j2array[0]);
6213  cj2array[0] = IKcos(j2array[0]);
6214  if (j2array[0] > IKPI)
6215  {
6216  j2array[0] -= IK2PI;
6217  }
6218  else if (j2array[0] < -IKPI)
6219  {
6220  j2array[0] += IK2PI;
6221  }
6222  j2valid[0] = true;
6223  for (int ij2 = 0; ij2 < 1; ++ij2)
6224  {
6225  if (!j2valid[ij2])
6226  {
6227  continue;
6228  }
6229  _ij2[0] = ij2;
6230  _ij2[1] = -1;
6231  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6232  {
6233  if (j2valid[iij2] &&
6234  IKabs(cj2array[ij2] - cj2array[iij2]) <
6235  IKFAST_SOLUTION_THRESH &&
6236  IKabs(sj2array[ij2] - sj2array[iij2]) <
6237  IKFAST_SOLUTION_THRESH)
6238  {
6239  j2valid[iij2] = false;
6240  _ij2[1] = iij2;
6241  break;
6242  }
6243  }
6244  j2 = j2array[ij2];
6245  cj2 = cj2array[ij2];
6246  sj2 = sj2array[ij2];
6247  {
6248  IkReal evalcond[8];
6249  IkReal x239 = IKsin(j2);
6250  IkReal x240 = IKcos(j2);
6251  IkReal x241 = ((1.0) * sj0);
6252  evalcond[0] = (x239 + new_r21);
6253  evalcond[1] = ((((-1.0) * x240)) + new_r20);
6254  evalcond[2] = (((new_r02 * x239)) + new_r10);
6255  evalcond[3] = (((cj0 * x240)) + new_r11);
6256  evalcond[4] = (new_r00 + (((-1.0) * x239 * x241)));
6257  evalcond[5] = ((((-1.0) * x240 * x241)) + new_r01);
6258  evalcond[6] =
6259  ((((-1.0) * new_r00 * x241)) + x239 + ((cj0 * new_r10)));
6260  evalcond[7] =
6261  ((((-1.0) * new_r01 * x241)) + x240 + ((cj0 * new_r11)));
6262  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6263  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6264  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6265  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6266  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6267  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6268  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6269  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6270  {
6271  continue;
6272  }
6273  }
6274 
6275  {
6276  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6277  vinfos[0].jointtype = 1;
6278  vinfos[0].foffset = j0;
6279  vinfos[0].indices[0] = _ij0[0];
6280  vinfos[0].indices[1] = _ij0[1];
6281  vinfos[0].maxsolutions = _nj0;
6282  vinfos[1].jointtype = 1;
6283  vinfos[1].foffset = j1;
6284  vinfos[1].indices[0] = _ij1[0];
6285  vinfos[1].indices[1] = _ij1[1];
6286  vinfos[1].maxsolutions = _nj1;
6287  vinfos[2].jointtype = 1;
6288  vinfos[2].foffset = j2;
6289  vinfos[2].indices[0] = _ij2[0];
6290  vinfos[2].indices[1] = _ij2[1];
6291  vinfos[2].maxsolutions = _nj2;
6292  vinfos[3].jointtype = 1;
6293  vinfos[3].foffset = j3;
6294  vinfos[3].indices[0] = _ij3[0];
6295  vinfos[3].indices[1] = _ij3[1];
6296  vinfos[3].maxsolutions = _nj3;
6297  vinfos[4].jointtype = 1;
6298  vinfos[4].foffset = j4;
6299  vinfos[4].indices[0] = _ij4[0];
6300  vinfos[4].indices[1] = _ij4[1];
6301  vinfos[4].maxsolutions = _nj4;
6302  vinfos[5].jointtype = 1;
6303  vinfos[5].foffset = j5;
6304  vinfos[5].indices[0] = _ij5[0];
6305  vinfos[5].indices[1] = _ij5[1];
6306  vinfos[5].maxsolutions = _nj5;
6307  vinfos[6].jointtype = 1;
6308  vinfos[6].foffset = j6;
6309  vinfos[6].indices[0] = _ij6[0];
6310  vinfos[6].indices[1] = _ij6[1];
6311  vinfos[6].maxsolutions = _nj6;
6312  std::vector<int> vfree(0);
6313  solutions.AddSolution(vinfos, vfree);
6314  }
6315  }
6316  }
6317  }
6318  } while (0);
6319  if (bgotonextstatement)
6320  {
6321  bool bgotonextstatement = true;
6322  do
6323  {
6324  evalcond[0] =
6325  ((-3.14159265358979) +
6326  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
6327  6.28318530717959)));
6328  evalcond[1] = new_r22;
6329  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6330  IKabs(evalcond[1]) < 0.0000050000000000)
6331  {
6332  bgotonextstatement = false;
6333  {
6334  IkReal j2array[1], cj2array[1], sj2array[1];
6335  bool j2valid[1] = { false };
6336  _nj2 = 1;
6337  if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
6338  IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
6339  IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
6340  IKFAST_SINCOS_THRESH)
6341  continue;
6342  j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
6343  sj2array[0] = IKsin(j2array[0]);
6344  cj2array[0] = IKcos(j2array[0]);
6345  if (j2array[0] > IKPI)
6346  {
6347  j2array[0] -= IK2PI;
6348  }
6349  else if (j2array[0] < -IKPI)
6350  {
6351  j2array[0] += IK2PI;
6352  }
6353  j2valid[0] = true;
6354  for (int ij2 = 0; ij2 < 1; ++ij2)
6355  {
6356  if (!j2valid[ij2])
6357  {
6358  continue;
6359  }
6360  _ij2[0] = ij2;
6361  _ij2[1] = -1;
6362  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6363  {
6364  if (j2valid[iij2] &&
6365  IKabs(cj2array[ij2] - cj2array[iij2]) <
6366  IKFAST_SOLUTION_THRESH &&
6367  IKabs(sj2array[ij2] - sj2array[iij2]) <
6368  IKFAST_SOLUTION_THRESH)
6369  {
6370  j2valid[iij2] = false;
6371  _ij2[1] = iij2;
6372  break;
6373  }
6374  }
6375  j2 = j2array[ij2];
6376  cj2 = cj2array[ij2];
6377  sj2 = sj2array[ij2];
6378  {
6379  IkReal evalcond[8];
6380  IkReal x242 = IKcos(j2);
6381  IkReal x243 = IKsin(j2);
6382  IkReal x244 = ((1.0) * sj0);
6383  IkReal x245 = ((1.0) * x243);
6384  evalcond[0] = (x242 + new_r20);
6385  evalcond[1] = ((((-1.0) * x245)) + new_r21);
6386  evalcond[2] = (((cj0 * x242)) + new_r11);
6387  evalcond[3] = ((((-1.0) * new_r02 * x245)) + new_r10);
6388  evalcond[4] = ((((-1.0) * x243 * x244)) + new_r00);
6389  evalcond[5] = ((((-1.0) * x242 * x244)) + new_r01);
6390  evalcond[6] =
6391  ((((-1.0) * new_r00 * x244)) + x243 + ((cj0 * new_r10)));
6392  evalcond[7] =
6393  ((((-1.0) * new_r01 * x244)) + x242 + ((cj0 * new_r11)));
6394  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6395  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6396  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6397  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6398  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6399  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6400  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6401  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6402  {
6403  continue;
6404  }
6405  }
6406 
6407  {
6408  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6409  vinfos[0].jointtype = 1;
6410  vinfos[0].foffset = j0;
6411  vinfos[0].indices[0] = _ij0[0];
6412  vinfos[0].indices[1] = _ij0[1];
6413  vinfos[0].maxsolutions = _nj0;
6414  vinfos[1].jointtype = 1;
6415  vinfos[1].foffset = j1;
6416  vinfos[1].indices[0] = _ij1[0];
6417  vinfos[1].indices[1] = _ij1[1];
6418  vinfos[1].maxsolutions = _nj1;
6419  vinfos[2].jointtype = 1;
6420  vinfos[2].foffset = j2;
6421  vinfos[2].indices[0] = _ij2[0];
6422  vinfos[2].indices[1] = _ij2[1];
6423  vinfos[2].maxsolutions = _nj2;
6424  vinfos[3].jointtype = 1;
6425  vinfos[3].foffset = j3;
6426  vinfos[3].indices[0] = _ij3[0];
6427  vinfos[3].indices[1] = _ij3[1];
6428  vinfos[3].maxsolutions = _nj3;
6429  vinfos[4].jointtype = 1;
6430  vinfos[4].foffset = j4;
6431  vinfos[4].indices[0] = _ij4[0];
6432  vinfos[4].indices[1] = _ij4[1];
6433  vinfos[4].maxsolutions = _nj4;
6434  vinfos[5].jointtype = 1;
6435  vinfos[5].foffset = j5;
6436  vinfos[5].indices[0] = _ij5[0];
6437  vinfos[5].indices[1] = _ij5[1];
6438  vinfos[5].maxsolutions = _nj5;
6439  vinfos[6].jointtype = 1;
6440  vinfos[6].foffset = j6;
6441  vinfos[6].indices[0] = _ij6[0];
6442  vinfos[6].indices[1] = _ij6[1];
6443  vinfos[6].maxsolutions = _nj6;
6444  std::vector<int> vfree(0);
6445  solutions.AddSolution(vinfos, vfree);
6446  }
6447  }
6448  }
6449  }
6450  } while (0);
6451  if (bgotonextstatement)
6452  {
6453  bool bgotonextstatement = true;
6454  do
6455  {
6456  evalcond[0] =
6457  ((-3.14159265358979) +
6458  (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
6459  evalcond[1] = new_r12;
6460  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6461  IKabs(evalcond[1]) < 0.0000050000000000)
6462  {
6463  bgotonextstatement = false;
6464  {
6465  IkReal j2array[1], cj2array[1], sj2array[1];
6466  bool j2valid[1] = { false };
6467  _nj2 = 1;
6468  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
6469  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
6470  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) -
6471  1) <= IKFAST_SINCOS_THRESH)
6472  continue;
6473  j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
6474  sj2array[0] = IKsin(j2array[0]);
6475  cj2array[0] = IKcos(j2array[0]);
6476  if (j2array[0] > IKPI)
6477  {
6478  j2array[0] -= IK2PI;
6479  }
6480  else if (j2array[0] < -IKPI)
6481  {
6482  j2array[0] += IK2PI;
6483  }
6484  j2valid[0] = true;
6485  for (int ij2 = 0; ij2 < 1; ++ij2)
6486  {
6487  if (!j2valid[ij2])
6488  {
6489  continue;
6490  }
6491  _ij2[0] = ij2;
6492  _ij2[1] = -1;
6493  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6494  {
6495  if (j2valid[iij2] &&
6496  IKabs(cj2array[ij2] - cj2array[iij2]) <
6497  IKFAST_SOLUTION_THRESH &&
6498  IKabs(sj2array[ij2] - sj2array[iij2]) <
6499  IKFAST_SOLUTION_THRESH)
6500  {
6501  j2valid[iij2] = false;
6502  _ij2[1] = iij2;
6503  break;
6504  }
6505  }
6506  j2 = j2array[ij2];
6507  cj2 = cj2array[ij2];
6508  sj2 = sj2array[ij2];
6509  {
6510  IkReal evalcond[8];
6511  IkReal x246 = IKsin(j2);
6512  IkReal x247 = IKcos(j2);
6513  IkReal x248 = ((1.0) * sj1);
6514  IkReal x249 = ((1.0) * x246);
6515  evalcond[0] = (x246 + new_r10);
6516  evalcond[1] = (x247 + new_r11);
6517  evalcond[2] = (new_r21 + ((sj1 * x246)));
6518  evalcond[3] = (((cj1 * x247)) + new_r00);
6519  evalcond[4] = (new_r20 + (((-1.0) * x247 * x248)));
6520  evalcond[5] = ((((-1.0) * cj1 * x249)) + new_r01);
6521  evalcond[6] =
6522  (((cj1 * new_r00)) + x247 + (((-1.0) * new_r20 * x248)));
6523  evalcond[7] = ((((-1.0) * new_r21 * x248)) +
6524  ((cj1 * new_r01)) + (((-1.0) * x249)));
6525  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6526  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6527  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6528  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6529  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6530  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6531  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6532  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6533  {
6534  continue;
6535  }
6536  }
6537 
6538  {
6539  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6540  vinfos[0].jointtype = 1;
6541  vinfos[0].foffset = j0;
6542  vinfos[0].indices[0] = _ij0[0];
6543  vinfos[0].indices[1] = _ij0[1];
6544  vinfos[0].maxsolutions = _nj0;
6545  vinfos[1].jointtype = 1;
6546  vinfos[1].foffset = j1;
6547  vinfos[1].indices[0] = _ij1[0];
6548  vinfos[1].indices[1] = _ij1[1];
6549  vinfos[1].maxsolutions = _nj1;
6550  vinfos[2].jointtype = 1;
6551  vinfos[2].foffset = j2;
6552  vinfos[2].indices[0] = _ij2[0];
6553  vinfos[2].indices[1] = _ij2[1];
6554  vinfos[2].maxsolutions = _nj2;
6555  vinfos[3].jointtype = 1;
6556  vinfos[3].foffset = j3;
6557  vinfos[3].indices[0] = _ij3[0];
6558  vinfos[3].indices[1] = _ij3[1];
6559  vinfos[3].maxsolutions = _nj3;
6560  vinfos[4].jointtype = 1;
6561  vinfos[4].foffset = j4;
6562  vinfos[4].indices[0] = _ij4[0];
6563  vinfos[4].indices[1] = _ij4[1];
6564  vinfos[4].maxsolutions = _nj4;
6565  vinfos[5].jointtype = 1;
6566  vinfos[5].foffset = j5;
6567  vinfos[5].indices[0] = _ij5[0];
6568  vinfos[5].indices[1] = _ij5[1];
6569  vinfos[5].maxsolutions = _nj5;
6570  vinfos[6].jointtype = 1;
6571  vinfos[6].foffset = j6;
6572  vinfos[6].indices[0] = _ij6[0];
6573  vinfos[6].indices[1] = _ij6[1];
6574  vinfos[6].maxsolutions = _nj6;
6575  std::vector<int> vfree(0);
6576  solutions.AddSolution(vinfos, vfree);
6577  }
6578  }
6579  }
6580  }
6581  } while (0);
6582  if (bgotonextstatement)
6583  {
6584  bool bgotonextstatement = true;
6585  do
6586  {
6587  evalcond[0] = ((-3.14159265358979) +
6588  (IKfmod(((3.14159265358979) +
6589  (IKabs(((-3.14159265358979) + j0)))),
6590  6.28318530717959)));
6591  evalcond[1] = new_r12;
6592  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6593  IKabs(evalcond[1]) < 0.0000050000000000)
6594  {
6595  bgotonextstatement = false;
6596  {
6597  IkReal j2array[1], cj2array[1], sj2array[1];
6598  bool j2valid[1] = { false };
6599  _nj2 = 1;
6600  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
6601  IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
6602  IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <=
6603  IKFAST_SINCOS_THRESH)
6604  continue;
6605  j2array[0] = IKatan2(new_r10, new_r11);
6606  sj2array[0] = IKsin(j2array[0]);
6607  cj2array[0] = IKcos(j2array[0]);
6608  if (j2array[0] > IKPI)
6609  {
6610  j2array[0] -= IK2PI;
6611  }
6612  else if (j2array[0] < -IKPI)
6613  {
6614  j2array[0] += IK2PI;
6615  }
6616  j2valid[0] = true;
6617  for (int ij2 = 0; ij2 < 1; ++ij2)
6618  {
6619  if (!j2valid[ij2])
6620  {
6621  continue;
6622  }
6623  _ij2[0] = ij2;
6624  _ij2[1] = -1;
6625  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6626  {
6627  if (j2valid[iij2] &&
6628  IKabs(cj2array[ij2] - cj2array[iij2]) <
6629  IKFAST_SOLUTION_THRESH &&
6630  IKabs(sj2array[ij2] - sj2array[iij2]) <
6631  IKFAST_SOLUTION_THRESH)
6632  {
6633  j2valid[iij2] = false;
6634  _ij2[1] = iij2;
6635  break;
6636  }
6637  }
6638  j2 = j2array[ij2];
6639  cj2 = cj2array[ij2];
6640  sj2 = sj2array[ij2];
6641  {
6642  IkReal evalcond[8];
6643  IkReal x250 = IKsin(j2);
6644  IkReal x251 = IKcos(j2);
6645  IkReal x252 = ((1.0) * sj1);
6646  IkReal x253 = ((1.0) * new_r00);
6647  IkReal x254 = ((1.0) * new_r01);
6648  IkReal x255 = ((1.0) * x250);
6649  evalcond[0] = (((sj1 * x250)) + new_r21);
6650  evalcond[1] = (x250 + (((-1.0) * new_r10)));
6651  evalcond[2] = (x251 + (((-1.0) * new_r11)));
6652  evalcond[3] = ((((-1.0) * x251 * x252)) + new_r20);
6653  evalcond[4] = (((cj1 * x251)) + (((-1.0) * x253)));
6654  evalcond[5] = ((((-1.0) * cj1 * x255)) + (((-1.0) * x254)));
6655  evalcond[6] = (x251 + (((-1.0) * cj1 * x253)) +
6656  (((-1.0) * new_r20 * x252)));
6657  evalcond[7] = ((((-1.0) * new_r21 * x252)) +
6658  (((-1.0) * cj1 * x254)) + (((-1.0) * x255)));
6659  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6660  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6661  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6662  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6663  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6664  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6665  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6666  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6667  {
6668  continue;
6669  }
6670  }
6671 
6672  {
6673  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6674  vinfos[0].jointtype = 1;
6675  vinfos[0].foffset = j0;
6676  vinfos[0].indices[0] = _ij0[0];
6677  vinfos[0].indices[1] = _ij0[1];
6678  vinfos[0].maxsolutions = _nj0;
6679  vinfos[1].jointtype = 1;
6680  vinfos[1].foffset = j1;
6681  vinfos[1].indices[0] = _ij1[0];
6682  vinfos[1].indices[1] = _ij1[1];
6683  vinfos[1].maxsolutions = _nj1;
6684  vinfos[2].jointtype = 1;
6685  vinfos[2].foffset = j2;
6686  vinfos[2].indices[0] = _ij2[0];
6687  vinfos[2].indices[1] = _ij2[1];
6688  vinfos[2].maxsolutions = _nj2;
6689  vinfos[3].jointtype = 1;
6690  vinfos[3].foffset = j3;
6691  vinfos[3].indices[0] = _ij3[0];
6692  vinfos[3].indices[1] = _ij3[1];
6693  vinfos[3].maxsolutions = _nj3;
6694  vinfos[4].jointtype = 1;
6695  vinfos[4].foffset = j4;
6696  vinfos[4].indices[0] = _ij4[0];
6697  vinfos[4].indices[1] = _ij4[1];
6698  vinfos[4].maxsolutions = _nj4;
6699  vinfos[5].jointtype = 1;
6700  vinfos[5].foffset = j5;
6701  vinfos[5].indices[0] = _ij5[0];
6702  vinfos[5].indices[1] = _ij5[1];
6703  vinfos[5].maxsolutions = _nj5;
6704  vinfos[6].jointtype = 1;
6705  vinfos[6].foffset = j6;
6706  vinfos[6].indices[0] = _ij6[0];
6707  vinfos[6].indices[1] = _ij6[1];
6708  vinfos[6].maxsolutions = _nj6;
6709  std::vector<int> vfree(0);
6710  solutions.AddSolution(vinfos, vfree);
6711  }
6712  }
6713  }
6714  }
6715  } while (0);
6716  if (bgotonextstatement)
6717  {
6718  bool bgotonextstatement = true;
6719  do
6720  {
6721  evalcond[0] = ((-3.14159265358979) +
6722  (IKfmod(((3.14159265358979) +
6723  (IKabs(((-1.5707963267949) + j0)))),
6724  6.28318530717959)));
6725  evalcond[1] = new_r02;
6726  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6727  IKabs(evalcond[1]) < 0.0000050000000000)
6728  {
6729  bgotonextstatement = false;
6730  {
6731  IkReal j2array[1], cj2array[1], sj2array[1];
6732  bool j2valid[1] = { false };
6733  _nj2 = 1;
6734  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
6735  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
6736  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
6737  IKFAST_SINCOS_THRESH)
6738  continue;
6739  j2array[0] = IKatan2(new_r00, new_r01);
6740  sj2array[0] = IKsin(j2array[0]);
6741  cj2array[0] = IKcos(j2array[0]);
6742  if (j2array[0] > IKPI)
6743  {
6744  j2array[0] -= IK2PI;
6745  }
6746  else if (j2array[0] < -IKPI)
6747  {
6748  j2array[0] += IK2PI;
6749  }
6750  j2valid[0] = true;
6751  for (int ij2 = 0; ij2 < 1; ++ij2)
6752  {
6753  if (!j2valid[ij2])
6754  {
6755  continue;
6756  }
6757  _ij2[0] = ij2;
6758  _ij2[1] = -1;
6759  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6760  {
6761  if (j2valid[iij2] &&
6762  IKabs(cj2array[ij2] - cj2array[iij2]) <
6763  IKFAST_SOLUTION_THRESH &&
6764  IKabs(sj2array[ij2] - sj2array[iij2]) <
6765  IKFAST_SOLUTION_THRESH)
6766  {
6767  j2valid[iij2] = false;
6768  _ij2[1] = iij2;
6769  break;
6770  }
6771  }
6772  j2 = j2array[ij2];
6773  cj2 = cj2array[ij2];
6774  sj2 = sj2array[ij2];
6775  {
6776  IkReal evalcond[8];
6777  IkReal x256 = IKsin(j2);
6778  IkReal x257 = IKcos(j2);
6779  IkReal x258 = ((1.0) * sj1);
6780  IkReal x259 = ((1.0) * x256);
6781  evalcond[0] = (((sj1 * x256)) + new_r21);
6782  evalcond[1] = (x256 + (((-1.0) * new_r00)));
6783  evalcond[2] = (x257 + (((-1.0) * new_r01)));
6784  evalcond[3] = (((cj1 * x257)) + new_r10);
6785  evalcond[4] = ((((-1.0) * x257 * x258)) + new_r20);
6786  evalcond[5] = ((((-1.0) * cj1 * x259)) + new_r11);
6787  evalcond[6] = (((cj1 * new_r10)) + x257 +
6788  (((-1.0) * new_r20 * x258)));
6789  evalcond[7] = ((((-1.0) * new_r21 * x258)) +
6790  ((cj1 * new_r11)) + (((-1.0) * x259)));
6791  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6792  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6793  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6794  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6795  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6796  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6797  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6798  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6799  {
6800  continue;
6801  }
6802  }
6803 
6804  {
6805  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6806  vinfos[0].jointtype = 1;
6807  vinfos[0].foffset = j0;
6808  vinfos[0].indices[0] = _ij0[0];
6809  vinfos[0].indices[1] = _ij0[1];
6810  vinfos[0].maxsolutions = _nj0;
6811  vinfos[1].jointtype = 1;
6812  vinfos[1].foffset = j1;
6813  vinfos[1].indices[0] = _ij1[0];
6814  vinfos[1].indices[1] = _ij1[1];
6815  vinfos[1].maxsolutions = _nj1;
6816  vinfos[2].jointtype = 1;
6817  vinfos[2].foffset = j2;
6818  vinfos[2].indices[0] = _ij2[0];
6819  vinfos[2].indices[1] = _ij2[1];
6820  vinfos[2].maxsolutions = _nj2;
6821  vinfos[3].jointtype = 1;
6822  vinfos[3].foffset = j3;
6823  vinfos[3].indices[0] = _ij3[0];
6824  vinfos[3].indices[1] = _ij3[1];
6825  vinfos[3].maxsolutions = _nj3;
6826  vinfos[4].jointtype = 1;
6827  vinfos[4].foffset = j4;
6828  vinfos[4].indices[0] = _ij4[0];
6829  vinfos[4].indices[1] = _ij4[1];
6830  vinfos[4].maxsolutions = _nj4;
6831  vinfos[5].jointtype = 1;
6832  vinfos[5].foffset = j5;
6833  vinfos[5].indices[0] = _ij5[0];
6834  vinfos[5].indices[1] = _ij5[1];
6835  vinfos[5].maxsolutions = _nj5;
6836  vinfos[6].jointtype = 1;
6837  vinfos[6].foffset = j6;
6838  vinfos[6].indices[0] = _ij6[0];
6839  vinfos[6].indices[1] = _ij6[1];
6840  vinfos[6].maxsolutions = _nj6;
6841  std::vector<int> vfree(0);
6842  solutions.AddSolution(vinfos, vfree);
6843  }
6844  }
6845  }
6846  }
6847  } while (0);
6848  if (bgotonextstatement)
6849  {
6850  bool bgotonextstatement = true;
6851  do
6852  {
6853  evalcond[0] = ((-3.14159265358979) +
6854  (IKfmod(((3.14159265358979) +
6855  (IKabs(((1.5707963267949) + j0)))),
6856  6.28318530717959)));
6857  evalcond[1] = new_r02;
6858  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6859  IKabs(evalcond[1]) < 0.0000050000000000)
6860  {
6861  bgotonextstatement = false;
6862  {
6863  IkReal j2array[1], cj2array[1], sj2array[1];
6864  bool j2valid[1] = { false };
6865  _nj2 = 1;
6866  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
6867  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
6868  IKabs(IKsqr(((-1.0) * new_r00)) +
6869  IKsqr(((-1.0) * new_r01)) - 1) <=
6870  IKFAST_SINCOS_THRESH)
6871  continue;
6872  j2array[0] =
6873  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
6874  sj2array[0] = IKsin(j2array[0]);
6875  cj2array[0] = IKcos(j2array[0]);
6876  if (j2array[0] > IKPI)
6877  {
6878  j2array[0] -= IK2PI;
6879  }
6880  else if (j2array[0] < -IKPI)
6881  {
6882  j2array[0] += IK2PI;
6883  }
6884  j2valid[0] = true;
6885  for (int ij2 = 0; ij2 < 1; ++ij2)
6886  {
6887  if (!j2valid[ij2])
6888  {
6889  continue;
6890  }
6891  _ij2[0] = ij2;
6892  _ij2[1] = -1;
6893  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6894  {
6895  if (j2valid[iij2] &&
6896  IKabs(cj2array[ij2] - cj2array[iij2]) <
6897  IKFAST_SOLUTION_THRESH &&
6898  IKabs(sj2array[ij2] - sj2array[iij2]) <
6899  IKFAST_SOLUTION_THRESH)
6900  {
6901  j2valid[iij2] = false;
6902  _ij2[1] = iij2;
6903  break;
6904  }
6905  }
6906  j2 = j2array[ij2];
6907  cj2 = cj2array[ij2];
6908  sj2 = sj2array[ij2];
6909  {
6910  IkReal evalcond[8];
6911  IkReal x260 = IKsin(j2);
6912  IkReal x261 = IKcos(j2);
6913  IkReal x262 = ((1.0) * new_r11);
6914  IkReal x263 = ((1.0) * sj1);
6915  IkReal x264 = ((1.0) * new_r10);
6916  IkReal x265 = ((1.0) * x260);
6917  evalcond[0] = (x260 + new_r00);
6918  evalcond[1] = (x261 + new_r01);
6919  evalcond[2] = (new_r21 + ((sj1 * x260)));
6920  evalcond[3] = ((((-1.0) * x261 * x263)) + new_r20);
6921  evalcond[4] = (((cj1 * x261)) + (((-1.0) * x264)));
6922  evalcond[5] =
6923  ((((-1.0) * cj1 * x265)) + (((-1.0) * x262)));
6924  evalcond[6] = ((((-1.0) * cj1 * x264)) + x261 +
6925  (((-1.0) * new_r20 * x263)));
6926  evalcond[7] =
6927  ((((-1.0) * cj1 * x262)) +
6928  (((-1.0) * new_r21 * x263)) + (((-1.0) * x265)));
6929  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6930  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6931  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6932  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6933  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6934  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6935  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6936  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6937  {
6938  continue;
6939  }
6940  }
6941 
6942  {
6943  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6944  vinfos[0].jointtype = 1;
6945  vinfos[0].foffset = j0;
6946  vinfos[0].indices[0] = _ij0[0];
6947  vinfos[0].indices[1] = _ij0[1];
6948  vinfos[0].maxsolutions = _nj0;
6949  vinfos[1].jointtype = 1;
6950  vinfos[1].foffset = j1;
6951  vinfos[1].indices[0] = _ij1[0];
6952  vinfos[1].indices[1] = _ij1[1];
6953  vinfos[1].maxsolutions = _nj1;
6954  vinfos[2].jointtype = 1;
6955  vinfos[2].foffset = j2;
6956  vinfos[2].indices[0] = _ij2[0];
6957  vinfos[2].indices[1] = _ij2[1];
6958  vinfos[2].maxsolutions = _nj2;
6959  vinfos[3].jointtype = 1;
6960  vinfos[3].foffset = j3;
6961  vinfos[3].indices[0] = _ij3[0];
6962  vinfos[3].indices[1] = _ij3[1];
6963  vinfos[3].maxsolutions = _nj3;
6964  vinfos[4].jointtype = 1;
6965  vinfos[4].foffset = j4;
6966  vinfos[4].indices[0] = _ij4[0];
6967  vinfos[4].indices[1] = _ij4[1];
6968  vinfos[4].maxsolutions = _nj4;
6969  vinfos[5].jointtype = 1;
6970  vinfos[5].foffset = j5;
6971  vinfos[5].indices[0] = _ij5[0];
6972  vinfos[5].indices[1] = _ij5[1];
6973  vinfos[5].maxsolutions = _nj5;
6974  vinfos[6].jointtype = 1;
6975  vinfos[6].foffset = j6;
6976  vinfos[6].indices[0] = _ij6[0];
6977  vinfos[6].indices[1] = _ij6[1];
6978  vinfos[6].maxsolutions = _nj6;
6979  std::vector<int> vfree(0);
6980  solutions.AddSolution(vinfos, vfree);
6981  }
6982  }
6983  }
6984  }
6985  } while (0);
6986  if (bgotonextstatement)
6987  {
6988  bool bgotonextstatement = true;
6989  do
6990  {
6991  evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
6992  if (IKabs(evalcond[0]) < 0.0000050000000000)
6993  {
6994  bgotonextstatement = false;
6995  {
6996  IkReal j2eval[1];
6997  new_r21 = 0;
6998  new_r20 = 0;
6999  new_r02 = 0;
7000  new_r12 = 0;
7001  j2eval[0] = IKabs(new_r22);
7002  if (IKabs(j2eval[0]) < 0.0000000100000000)
7003  {
7004  continue; // no branches [j2]
7005  }
7006  else
7007  {
7008  IkReal op[2 + 1], zeror[2];
7009  int numroots;
7010  op[0] = ((-1.0) * new_r22);
7011  op[1] = 0;
7012  op[2] = new_r22;
7013  polyroots2(op, zeror, numroots);
7014  IkReal j2array[2], cj2array[2], sj2array[2],
7015  tempj2array[1];
7016  int numsolutions = 0;
7017  for (int ij2 = 0; ij2 < numroots; ++ij2)
7018  {
7019  IkReal htj2 = zeror[ij2];
7020  tempj2array[0] = ((2.0) * (atan(htj2)));
7021  for (int kj2 = 0; kj2 < 1; ++kj2)
7022  {
7023  j2array[numsolutions] = tempj2array[kj2];
7024  if (j2array[numsolutions] > IKPI)
7025  {
7026  j2array[numsolutions] -= IK2PI;
7027  }
7028  else if (j2array[numsolutions] < -IKPI)
7029  {
7030  j2array[numsolutions] += IK2PI;
7031  }
7032  sj2array[numsolutions] =
7033  IKsin(j2array[numsolutions]);
7034  cj2array[numsolutions] =
7035  IKcos(j2array[numsolutions]);
7036  numsolutions++;
7037  }
7038  }
7039  bool j2valid[2] = { true, true };
7040  _nj2 = 2;
7041  for (int ij2 = 0; ij2 < numsolutions; ++ij2)
7042  {
7043  if (!j2valid[ij2])
7044  {
7045  continue;
7046  }
7047  j2 = j2array[ij2];
7048  cj2 = cj2array[ij2];
7049  sj2 = sj2array[ij2];
7050  htj2 = IKtan(j2 / 2);
7051 
7052  _ij2[0] = ij2;
7053  _ij2[1] = -1;
7054  for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
7055  {
7056  if (j2valid[iij2] &&
7057  IKabs(cj2array[ij2] - cj2array[iij2]) <
7058  IKFAST_SOLUTION_THRESH &&
7059  IKabs(sj2array[ij2] - sj2array[iij2]) <
7060  IKFAST_SOLUTION_THRESH)
7061  {
7062  j2valid[iij2] = false;
7063  _ij2[1] = iij2;
7064  break;
7065  }
7066  }
7067  {
7068  std::vector<IkSingleDOFSolutionBase<IkReal> >
7069  vinfos(7);
7070  vinfos[0].jointtype = 1;
7071  vinfos[0].foffset = j0;
7072  vinfos[0].indices[0] = _ij0[0];
7073  vinfos[0].indices[1] = _ij0[1];
7074  vinfos[0].maxsolutions = _nj0;
7075  vinfos[1].jointtype = 1;
7076  vinfos[1].foffset = j1;
7077  vinfos[1].indices[0] = _ij1[0];
7078  vinfos[1].indices[1] = _ij1[1];
7079  vinfos[1].maxsolutions = _nj1;
7080  vinfos[2].jointtype = 1;
7081  vinfos[2].foffset = j2;
7082  vinfos[2].indices[0] = _ij2[0];
7083  vinfos[2].indices[1] = _ij2[1];
7084  vinfos[2].maxsolutions = _nj2;
7085  vinfos[3].jointtype = 1;
7086  vinfos[3].foffset = j3;
7087  vinfos[3].indices[0] = _ij3[0];
7088  vinfos[3].indices[1] = _ij3[1];
7089  vinfos[3].maxsolutions = _nj3;
7090  vinfos[4].jointtype = 1;
7091  vinfos[4].foffset = j4;
7092  vinfos[4].indices[0] = _ij4[0];
7093  vinfos[4].indices[1] = _ij4[1];
7094  vinfos[4].maxsolutions = _nj4;
7095  vinfos[5].jointtype = 1;
7096  vinfos[5].foffset = j5;
7097  vinfos[5].indices[0] = _ij5[0];
7098  vinfos[5].indices[1] = _ij5[1];
7099  vinfos[5].maxsolutions = _nj5;
7100  vinfos[6].jointtype = 1;
7101  vinfos[6].foffset = j6;
7102  vinfos[6].indices[0] = _ij6[0];
7103  vinfos[6].indices[1] = _ij6[1];
7104  vinfos[6].maxsolutions = _nj6;
7105  std::vector<int> vfree(0);
7106  solutions.AddSolution(vinfos, vfree);
7107  }
7108  }
7109  }
7110  }
7111  }
7112  } while (0);
7113  if (bgotonextstatement)
7114  {
7115  bool bgotonextstatement = true;
7116  do
7117  {
7118  if (1)
7119  {
7120  bgotonextstatement = false;
7121  continue; // branch miss [j2]
7122  }
7123  } while (0);
7124  if (bgotonextstatement)
7125  {
7126  }
7127  }
7128  }
7129  }
7130  }
7131  }
7132  }
7133  }
7134  }
7135  }
7136  }
7137  }
7138  else
7139  {
7140  {
7141  IkReal j2array[1], cj2array[1], sj2array[1];
7142  bool j2valid[1] = { false };
7143  _nj2 = 1;
7144  CheckValue<IkReal> x267 = IKPowWithIntegerCheck(sj1, -1);
7145  if (!x267.valid)
7146  {
7147  continue;
7148  }
7149  IkReal x266 = x267.value;
7150  CheckValue<IkReal> x268 = IKPowWithIntegerCheck(cj1, -1);
7151  if (!x268.valid)
7152  {
7153  continue;
7154  }
7155  CheckValue<IkReal> x269 = IKPowWithIntegerCheck(sj0, -1);
7156  if (!x269.valid)
7157  {
7158  continue;
7159  }
7160  if (IKabs(((-1.0) * new_r21 * x266)) < IKFAST_ATAN2_MAGTHRESH &&
7161  IKabs((x266 * (x268.value) * (x269.value) *
7162  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
7163  IKFAST_ATAN2_MAGTHRESH &&
7164  IKabs(IKsqr(((-1.0) * new_r21 * x266)) +
7165  IKsqr((x266 * (x268.value) * (x269.value) *
7166  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
7167  1) <= IKFAST_SINCOS_THRESH)
7168  continue;
7169  j2array[0] = IKatan2(((-1.0) * new_r21 * x266),
7170  (x266 * (x268.value) * (x269.value) *
7171  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
7172  sj2array[0] = IKsin(j2array[0]);
7173  cj2array[0] = IKcos(j2array[0]);
7174  if (j2array[0] > IKPI)
7175  {
7176  j2array[0] -= IK2PI;
7177  }
7178  else if (j2array[0] < -IKPI)
7179  {
7180  j2array[0] += IK2PI;
7181  }
7182  j2valid[0] = true;
7183  for (int ij2 = 0; ij2 < 1; ++ij2)
7184  {
7185  if (!j2valid[ij2])
7186  {
7187  continue;
7188  }
7189  _ij2[0] = ij2;
7190  _ij2[1] = -1;
7191  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7192  {
7193  if (j2valid[iij2] &&
7194  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7195  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7196  {
7197  j2valid[iij2] = false;
7198  _ij2[1] = iij2;
7199  break;
7200  }
7201  }
7202  j2 = j2array[ij2];
7203  cj2 = cj2array[ij2];
7204  sj2 = sj2array[ij2];
7205  {
7206  IkReal evalcond[12];
7207  IkReal x270 = IKsin(j2);
7208  IkReal x271 = IKcos(j2);
7209  IkReal x272 = ((1.0) * sj0);
7210  IkReal x273 = ((1.0) * sj1);
7211  IkReal x274 = (cj0 * new_r00);
7212  IkReal x275 = (cj1 * sj0);
7213  IkReal x276 = (cj0 * new_r01);
7214  IkReal x277 = (cj1 * x271);
7215  IkReal x278 = (cj1 * x270);
7216  evalcond[0] = (new_r21 + ((sj1 * x270)));
7217  evalcond[1] = ((((-1.0) * x271 * x273)) + new_r20);
7218  evalcond[2] = ((((-1.0) * new_r00 * x272)) + x270 + ((cj0 * new_r10)));
7219  evalcond[3] = ((((-1.0) * new_r01 * x272)) + x271 + ((cj0 * new_r11)));
7220  evalcond[4] = (((new_r10 * sj0)) + x277 + x274);
7221  evalcond[5] = (((x271 * x275)) + new_r10 + ((cj0 * x270)));
7222  evalcond[6] = ((((-1.0) * x278)) + ((new_r11 * sj0)) + x276);
7223  evalcond[7] = ((((-1.0) * x270 * x272)) + new_r00 + ((cj0 * x277)));
7224  evalcond[8] = ((((-1.0) * x272 * x278)) + new_r11 + ((cj0 * x271)));
7225  evalcond[9] =
7226  ((((-1.0) * cj0 * x278)) + (((-1.0) * x271 * x272)) + new_r01);
7227  evalcond[10] = (((cj1 * x274)) + x271 + (((-1.0) * new_r20 * x273)) +
7228  ((new_r10 * x275)));
7229  evalcond[11] = ((((-1.0) * x270)) + ((cj1 * x276)) +
7230  (((-1.0) * new_r21 * x273)) + ((new_r11 * x275)));
7231  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7232  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7233  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7234  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7235  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7236  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7237  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7238  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7239  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7240  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7241  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7242  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7243  {
7244  continue;
7245  }
7246  }
7247 
7248  {
7249  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7250  vinfos[0].jointtype = 1;
7251  vinfos[0].foffset = j0;
7252  vinfos[0].indices[0] = _ij0[0];
7253  vinfos[0].indices[1] = _ij0[1];
7254  vinfos[0].maxsolutions = _nj0;
7255  vinfos[1].jointtype = 1;
7256  vinfos[1].foffset = j1;
7257  vinfos[1].indices[0] = _ij1[0];
7258  vinfos[1].indices[1] = _ij1[1];
7259  vinfos[1].maxsolutions = _nj1;
7260  vinfos[2].jointtype = 1;
7261  vinfos[2].foffset = j2;
7262  vinfos[2].indices[0] = _ij2[0];
7263  vinfos[2].indices[1] = _ij2[1];
7264  vinfos[2].maxsolutions = _nj2;
7265  vinfos[3].jointtype = 1;
7266  vinfos[3].foffset = j3;
7267  vinfos[3].indices[0] = _ij3[0];
7268  vinfos[3].indices[1] = _ij3[1];
7269  vinfos[3].maxsolutions = _nj3;
7270  vinfos[4].jointtype = 1;
7271  vinfos[4].foffset = j4;
7272  vinfos[4].indices[0] = _ij4[0];
7273  vinfos[4].indices[1] = _ij4[1];
7274  vinfos[4].maxsolutions = _nj4;
7275  vinfos[5].jointtype = 1;
7276  vinfos[5].foffset = j5;
7277  vinfos[5].indices[0] = _ij5[0];
7278  vinfos[5].indices[1] = _ij5[1];
7279  vinfos[5].maxsolutions = _nj5;
7280  vinfos[6].jointtype = 1;
7281  vinfos[6].foffset = j6;
7282  vinfos[6].indices[0] = _ij6[0];
7283  vinfos[6].indices[1] = _ij6[1];
7284  vinfos[6].maxsolutions = _nj6;
7285  std::vector<int> vfree(0);
7286  solutions.AddSolution(vinfos, vfree);
7287  }
7288  }
7289  }
7290  }
7291  }
7292  }
7293  else
7294  {
7295  {
7296  IkReal j2array[1], cj2array[1], sj2array[1];
7297  bool j2valid[1] = { false };
7298  _nj2 = 1;
7299  CheckValue<IkReal> x280 = IKPowWithIntegerCheck(sj1, -1);
7300  if (!x280.valid)
7301  {
7302  continue;
7303  }
7304  IkReal x279 = x280.value;
7305  CheckValue<IkReal> x281 = IKPowWithIntegerCheck(cj0, -1);
7306  if (!x281.valid)
7307  {
7308  continue;
7309  }
7310  if (IKabs(((-1.0) * new_r21 * x279)) < IKFAST_ATAN2_MAGTHRESH &&
7311  IKabs((x279 * (x281.value) *
7312  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
7313  IKFAST_ATAN2_MAGTHRESH &&
7314  IKabs(
7315  IKsqr(((-1.0) * new_r21 * x279)) +
7316  IKsqr((x279 * (x281.value) *
7317  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
7318  1) <= IKFAST_SINCOS_THRESH)
7319  continue;
7320  j2array[0] =
7321  IKatan2(((-1.0) * new_r21 * x279),
7322  (x279 * (x281.value) *
7323  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
7324  sj2array[0] = IKsin(j2array[0]);
7325  cj2array[0] = IKcos(j2array[0]);
7326  if (j2array[0] > IKPI)
7327  {
7328  j2array[0] -= IK2PI;
7329  }
7330  else if (j2array[0] < -IKPI)
7331  {
7332  j2array[0] += IK2PI;
7333  }
7334  j2valid[0] = true;
7335  for (int ij2 = 0; ij2 < 1; ++ij2)
7336  {
7337  if (!j2valid[ij2])
7338  {
7339  continue;
7340  }
7341  _ij2[0] = ij2;
7342  _ij2[1] = -1;
7343  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7344  {
7345  if (j2valid[iij2] &&
7346  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7347  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7348  {
7349  j2valid[iij2] = false;
7350  _ij2[1] = iij2;
7351  break;
7352  }
7353  }
7354  j2 = j2array[ij2];
7355  cj2 = cj2array[ij2];
7356  sj2 = sj2array[ij2];
7357  {
7358  IkReal evalcond[12];
7359  IkReal x282 = IKsin(j2);
7360  IkReal x283 = IKcos(j2);
7361  IkReal x284 = ((1.0) * sj0);
7362  IkReal x285 = ((1.0) * sj1);
7363  IkReal x286 = (cj0 * new_r00);
7364  IkReal x287 = (cj1 * sj0);
7365  IkReal x288 = (cj0 * new_r01);
7366  IkReal x289 = (cj1 * x283);
7367  IkReal x290 = (cj1 * x282);
7368  evalcond[0] = (new_r21 + ((sj1 * x282)));
7369  evalcond[1] = (new_r20 + (((-1.0) * x283 * x285)));
7370  evalcond[2] = ((((-1.0) * new_r00 * x284)) + x282 + ((cj0 * new_r10)));
7371  evalcond[3] = (x283 + (((-1.0) * new_r01 * x284)) + ((cj0 * new_r11)));
7372  evalcond[4] = (((new_r10 * sj0)) + x289 + x286);
7373  evalcond[5] = (((cj0 * x282)) + new_r10 + ((x283 * x287)));
7374  evalcond[6] = ((((-1.0) * x290)) + ((new_r11 * sj0)) + x288);
7375  evalcond[7] = (((cj0 * x289)) + (((-1.0) * x282 * x284)) + new_r00);
7376  evalcond[8] = ((((-1.0) * x284 * x290)) + ((cj0 * x283)) + new_r11);
7377  evalcond[9] = ((((-1.0) * cj0 * x290)) + new_r01 + (((-1.0) * x283 * x284)));
7378  evalcond[10] =
7379  (x283 + (((-1.0) * new_r20 * x285)) + ((cj1 * x286)) + ((new_r10 * x287)));
7380  evalcond[11] = ((((-1.0) * x282)) + (((-1.0) * new_r21 * x285)) +
7381  ((new_r11 * x287)) + ((cj1 * x288)));
7382  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7383  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7384  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7385  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7386  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7387  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7388  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7389  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7390  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7391  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7392  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7393  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7394  {
7395  continue;
7396  }
7397  }
7398 
7399  {
7400  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7401  vinfos[0].jointtype = 1;
7402  vinfos[0].foffset = j0;
7403  vinfos[0].indices[0] = _ij0[0];
7404  vinfos[0].indices[1] = _ij0[1];
7405  vinfos[0].maxsolutions = _nj0;
7406  vinfos[1].jointtype = 1;
7407  vinfos[1].foffset = j1;
7408  vinfos[1].indices[0] = _ij1[0];
7409  vinfos[1].indices[1] = _ij1[1];
7410  vinfos[1].maxsolutions = _nj1;
7411  vinfos[2].jointtype = 1;
7412  vinfos[2].foffset = j2;
7413  vinfos[2].indices[0] = _ij2[0];
7414  vinfos[2].indices[1] = _ij2[1];
7415  vinfos[2].maxsolutions = _nj2;
7416  vinfos[3].jointtype = 1;
7417  vinfos[3].foffset = j3;
7418  vinfos[3].indices[0] = _ij3[0];
7419  vinfos[3].indices[1] = _ij3[1];
7420  vinfos[3].maxsolutions = _nj3;
7421  vinfos[4].jointtype = 1;
7422  vinfos[4].foffset = j4;
7423  vinfos[4].indices[0] = _ij4[0];
7424  vinfos[4].indices[1] = _ij4[1];
7425  vinfos[4].maxsolutions = _nj4;
7426  vinfos[5].jointtype = 1;
7427  vinfos[5].foffset = j5;
7428  vinfos[5].indices[0] = _ij5[0];
7429  vinfos[5].indices[1] = _ij5[1];
7430  vinfos[5].maxsolutions = _nj5;
7431  vinfos[6].jointtype = 1;
7432  vinfos[6].foffset = j6;
7433  vinfos[6].indices[0] = _ij6[0];
7434  vinfos[6].indices[1] = _ij6[1];
7435  vinfos[6].maxsolutions = _nj6;
7436  std::vector<int> vfree(0);
7437  solutions.AddSolution(vinfos, vfree);
7438  }
7439  }
7440  }
7441  }
7442  }
7443  }
7444  else
7445  {
7446  {
7447  IkReal j2array[1], cj2array[1], sj2array[1];
7448  bool j2valid[1] = { false };
7449  _nj2 = 1;
7450  CheckValue<IkReal> x291 = IKatan2WithCheck(
7451  IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7452  if (!x291.valid)
7453  {
7454  continue;
7455  }
7456  CheckValue<IkReal> x292 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7457  if (!x292.valid)
7458  {
7459  continue;
7460  }
7461  j2array[0] = ((-1.5707963267949) + (x291.value) + (((1.5707963267949) * (x292.value))));
7462  sj2array[0] = IKsin(j2array[0]);
7463  cj2array[0] = IKcos(j2array[0]);
7464  if (j2array[0] > IKPI)
7465  {
7466  j2array[0] -= IK2PI;
7467  }
7468  else if (j2array[0] < -IKPI)
7469  {
7470  j2array[0] += IK2PI;
7471  }
7472  j2valid[0] = true;
7473  for (int ij2 = 0; ij2 < 1; ++ij2)
7474  {
7475  if (!j2valid[ij2])
7476  {
7477  continue;
7478  }
7479  _ij2[0] = ij2;
7480  _ij2[1] = -1;
7481  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7482  {
7483  if (j2valid[iij2] &&
7484  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7485  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7486  {
7487  j2valid[iij2] = false;
7488  _ij2[1] = iij2;
7489  break;
7490  }
7491  }
7492  j2 = j2array[ij2];
7493  cj2 = cj2array[ij2];
7494  sj2 = sj2array[ij2];
7495  {
7496  IkReal evalcond[12];
7497  IkReal x293 = IKsin(j2);
7498  IkReal x294 = IKcos(j2);
7499  IkReal x295 = ((1.0) * sj0);
7500  IkReal x296 = ((1.0) * sj1);
7501  IkReal x297 = (cj0 * new_r00);
7502  IkReal x298 = (cj1 * sj0);
7503  IkReal x299 = (cj0 * new_r01);
7504  IkReal x300 = (cj1 * x294);
7505  IkReal x301 = (cj1 * x293);
7506  evalcond[0] = (((sj1 * x293)) + new_r21);
7507  evalcond[1] = (new_r20 + (((-1.0) * x294 * x296)));
7508  evalcond[2] = (x293 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x295)));
7509  evalcond[3] = ((((-1.0) * new_r01 * x295)) + x294 + ((cj0 * new_r11)));
7510  evalcond[4] = (((new_r10 * sj0)) + x300 + x297);
7511  evalcond[5] = (((x294 * x298)) + new_r10 + ((cj0 * x293)));
7512  evalcond[6] = (((new_r11 * sj0)) + (((-1.0) * x301)) + x299);
7513  evalcond[7] = ((((-1.0) * x293 * x295)) + ((cj0 * x300)) + new_r00);
7514  evalcond[8] = ((((-1.0) * x295 * x301)) + new_r11 + ((cj0 * x294)));
7515  evalcond[9] = ((((-1.0) * cj0 * x301)) + new_r01 + (((-1.0) * x294 * x295)));
7516  evalcond[10] =
7517  ((((-1.0) * new_r20 * x296)) + ((cj1 * x297)) + ((new_r10 * x298)) + x294);
7518  evalcond[11] = ((((-1.0) * x293)) + (((-1.0) * new_r21 * x296)) +
7519  ((new_r11 * x298)) + ((cj1 * x299)));
7520  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7521  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7522  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7523  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7524  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7525  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7526  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7527  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7528  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7529  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7530  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7531  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7532  {
7533  continue;
7534  }
7535  }
7536 
7537  {
7538  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7539  vinfos[0].jointtype = 1;
7540  vinfos[0].foffset = j0;
7541  vinfos[0].indices[0] = _ij0[0];
7542  vinfos[0].indices[1] = _ij0[1];
7543  vinfos[0].maxsolutions = _nj0;
7544  vinfos[1].jointtype = 1;
7545  vinfos[1].foffset = j1;
7546  vinfos[1].indices[0] = _ij1[0];
7547  vinfos[1].indices[1] = _ij1[1];
7548  vinfos[1].maxsolutions = _nj1;
7549  vinfos[2].jointtype = 1;
7550  vinfos[2].foffset = j2;
7551  vinfos[2].indices[0] = _ij2[0];
7552  vinfos[2].indices[1] = _ij2[1];
7553  vinfos[2].maxsolutions = _nj2;
7554  vinfos[3].jointtype = 1;
7555  vinfos[3].foffset = j3;
7556  vinfos[3].indices[0] = _ij3[0];
7557  vinfos[3].indices[1] = _ij3[1];
7558  vinfos[3].maxsolutions = _nj3;
7559  vinfos[4].jointtype = 1;
7560  vinfos[4].foffset = j4;
7561  vinfos[4].indices[0] = _ij4[0];
7562  vinfos[4].indices[1] = _ij4[1];
7563  vinfos[4].maxsolutions = _nj4;
7564  vinfos[5].jointtype = 1;
7565  vinfos[5].foffset = j5;
7566  vinfos[5].indices[0] = _ij5[0];
7567  vinfos[5].indices[1] = _ij5[1];
7568  vinfos[5].maxsolutions = _nj5;
7569  vinfos[6].jointtype = 1;
7570  vinfos[6].foffset = j6;
7571  vinfos[6].indices[0] = _ij6[0];
7572  vinfos[6].indices[1] = _ij6[1];
7573  vinfos[6].maxsolutions = _nj6;
7574  std::vector<int> vfree(0);
7575  solutions.AddSolution(vinfos, vfree);
7576  }
7577  }
7578  }
7579  }
7580  }
7581  }
7582  }
7583  }
7584  }
7585  }
7586  else
7587  {
7588  {
7589  IkReal j2array[1], cj2array[1], sj2array[1];
7590  bool j2valid[1] = { false };
7591  _nj2 = 1;
7592  CheckValue<IkReal> x302 =
7593  IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7594  if (!x302.valid)
7595  {
7596  continue;
7597  }
7598  CheckValue<IkReal> x303 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7599  if (!x303.valid)
7600  {
7601  continue;
7602  }
7603  j2array[0] = ((-1.5707963267949) + (x302.value) + (((1.5707963267949) * (x303.value))));
7604  sj2array[0] = IKsin(j2array[0]);
7605  cj2array[0] = IKcos(j2array[0]);
7606  if (j2array[0] > IKPI)
7607  {
7608  j2array[0] -= IK2PI;
7609  }
7610  else if (j2array[0] < -IKPI)
7611  {
7612  j2array[0] += IK2PI;
7613  }
7614  j2valid[0] = true;
7615  for (int ij2 = 0; ij2 < 1; ++ij2)
7616  {
7617  if (!j2valid[ij2])
7618  {
7619  continue;
7620  }
7621  _ij2[0] = ij2;
7622  _ij2[1] = -1;
7623  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7624  {
7625  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7626  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7627  {
7628  j2valid[iij2] = false;
7629  _ij2[1] = iij2;
7630  break;
7631  }
7632  }
7633  j2 = j2array[ij2];
7634  cj2 = cj2array[ij2];
7635  sj2 = sj2array[ij2];
7636  {
7637  IkReal evalcond[2];
7638  evalcond[0] = (((sj1 * (IKsin(j2)))) + new_r21);
7639  evalcond[1] = ((((-1.0) * sj1 * (IKcos(j2)))) + new_r20);
7640  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
7641  {
7642  continue;
7643  }
7644  }
7645 
7646  {
7647  IkReal j0eval[3];
7648  j0eval[0] = sj1;
7649  j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
7650  j0eval[2] = IKsign(sj1);
7651  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
7652  IKabs(j0eval[2]) < 0.0000010000000000)
7653  {
7654  {
7655  IkReal j0eval[2];
7656  j0eval[0] = new_r00;
7657  j0eval[1] = sj1;
7658  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
7659  {
7660  {
7661  IkReal evalcond[5];
7662  bool bgotonextstatement = true;
7663  do
7664  {
7665  evalcond[0] = ((-3.14159265358979) +
7666  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
7667  evalcond[1] = new_r21;
7668  evalcond[2] = new_r02;
7669  evalcond[3] = new_r12;
7670  evalcond[4] = new_r20;
7671  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
7672  IKabs(evalcond[1]) < 0.0000050000000000 &&
7673  IKabs(evalcond[2]) < 0.0000050000000000 &&
7674  IKabs(evalcond[3]) < 0.0000050000000000 &&
7675  IKabs(evalcond[4]) < 0.0000050000000000)
7676  {
7677  bgotonextstatement = false;
7678  {
7679  IkReal j0eval[3];
7680  sj1 = 0;
7681  cj1 = 1.0;
7682  j1 = 0;
7683  IkReal x304 = ((1.0) * cj2);
7684  IkReal x305 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7685  j0eval[0] = x305;
7686  j0eval[1] = ((IKabs((((new_r00 * sj2)) + (((-1.0) * new_r10 * x304))))) +
7687  (IKabs(((((-1.0) * new_r10 * sj2)) + (((-1.0) * new_r00 * x304))))));
7688  j0eval[2] = IKsign(x305);
7689  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7690  IKabs(j0eval[1]) < 0.0000010000000000 ||
7691  IKabs(j0eval[2]) < 0.0000010000000000)
7692  {
7693  {
7694  IkReal j0eval[3];
7695  sj1 = 0;
7696  cj1 = 1.0;
7697  j1 = 0;
7698  IkReal x306 = ((1.0) * cj2);
7699  IkReal x307 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7700  j0eval[0] = x307;
7701  j0eval[1] =
7702  ((IKabs(((((-1.0) * new_r01 * x306)) + (((-1.0) * new_r10 * x306))))) +
7703  (IKabs((((cj2 * new_r00)) + (((-1.0) * new_r11 * x306))))));
7704  j0eval[2] = IKsign(x307);
7705  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7706  IKabs(j0eval[1]) < 0.0000010000000000 ||
7707  IKabs(j0eval[2]) < 0.0000010000000000)
7708  {
7709  {
7710  IkReal j0eval[3];
7711  sj1 = 0;
7712  cj1 = 1.0;
7713  j1 = 0;
7714  IkReal x308 = ((1.0) * new_r10);
7715  IkReal x309 = ((((-1.0) * sj2 * x308)) + ((cj2 * new_r00)));
7716  j0eval[0] = x309;
7717  j0eval[1] = IKsign(x309);
7718  j0eval[2] = ((IKabs(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10)))) +
7719  (IKabs((((cj2 * sj2)) + (((-1.0) * new_r00 * x308))))));
7720  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7721  IKabs(j0eval[1]) < 0.0000010000000000 ||
7722  IKabs(j0eval[2]) < 0.0000010000000000)
7723  {
7724  {
7725  IkReal evalcond[1];
7726  bool bgotonextstatement = true;
7727  do
7728  {
7729  IkReal x312 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7730  if (IKabs(x312) == 0)
7731  {
7732  continue;
7733  }
7734  IkReal x310 = pow(x312, -0.5);
7735  IkReal x311 = ((-1.0) * x310);
7736  CheckValue<IkReal> x313 =
7737  IKatan2WithCheck(IkReal(new_r00),
7738  IkReal(((-1.0) * new_r10)),
7739  IKFAST_ATAN2_MAGTHRESH);
7740  if (!x313.valid)
7741  {
7742  continue;
7743  }
7744  IkReal gconst0 = ((-1.0) * (x313.value));
7745  IkReal gconst1 = (new_r00 * x311);
7746  IkReal gconst2 = (new_r10 * x311);
7747  CheckValue<IkReal> x314 =
7748  IKatan2WithCheck(IkReal(new_r00),
7749  IkReal(((-1.0) * new_r10)),
7750  IKFAST_ATAN2_MAGTHRESH);
7751  if (!x314.valid)
7752  {
7753  continue;
7754  }
7755  evalcond[0] =
7756  ((-3.14159265358979) +
7757  (IKfmod(((3.14159265358979) + (IKabs(((x314.value) + j2)))),
7758  6.28318530717959)));
7759  if (IKabs(evalcond[0]) < 0.0000050000000000)
7760  {
7761  bgotonextstatement = false;
7762  {
7763  IkReal j0eval[2];
7764  CheckValue<IkReal> x318 =
7765  IKatan2WithCheck(IkReal(new_r00),
7766  IkReal(((-1.0) * new_r10)),
7767  IKFAST_ATAN2_MAGTHRESH);
7768  if (!x318.valid)
7769  {
7770  continue;
7771  }
7772  IkReal x315 = ((-1.0) * (x318.value));
7773  IkReal x316 = x310;
7774  IkReal x317 = ((-1.0) * x316);
7775  sj1 = 0;
7776  cj1 = 1.0;
7777  j1 = 0;
7778  sj2 = gconst1;
7779  cj2 = gconst2;
7780  j2 = x315;
7781  IkReal gconst0 = x315;
7782  IkReal gconst1 = (new_r00 * x317);
7783  IkReal gconst2 = (new_r10 * x317);
7784  IkReal x319 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7785  j0eval[0] = x319;
7786  j0eval[1] = IKsign(x319);
7787  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7788  IKabs(j0eval[1]) < 0.0000010000000000)
7789  {
7790  {
7791  IkReal j0eval[3];
7792  CheckValue<IkReal> x323 =
7793  IKatan2WithCheck(IkReal(new_r00),
7794  IkReal(((-1.0) * new_r10)),
7795  IKFAST_ATAN2_MAGTHRESH);
7796  if (!x323.valid)
7797  {
7798  continue;
7799  }
7800  IkReal x320 = ((-1.0) * (x323.value));
7801  IkReal x321 = x310;
7802  IkReal x322 = ((-1.0) * x321);
7803  sj1 = 0;
7804  cj1 = 1.0;
7805  j1 = 0;
7806  sj2 = gconst1;
7807  cj2 = gconst2;
7808  j2 = x320;
7809  IkReal gconst0 = x320;
7810  IkReal gconst1 = (new_r00 * x322);
7811  IkReal gconst2 = (new_r10 * x322);
7812  IkReal x324 = new_r10 * new_r10;
7813  IkReal x325 =
7814  (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7815  IkReal x326 = x310;
7816  IkReal x327 = (new_r10 * x326);
7817  j0eval[0] = x325;
7818  j0eval[1] = IKsign(x325);
7819  j0eval[2] =
7820  ((IKabs(((((-1.0) * new_r00 * x327)) +
7821  ((new_r11 * x327))))) +
7822  (IKabs((((new_r01 * x327)) + ((x324 * x326))))));
7823  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7824  IKabs(j0eval[1]) < 0.0000010000000000 ||
7825  IKabs(j0eval[2]) < 0.0000010000000000)
7826  {
7827  {
7828  IkReal j0eval[1];
7829  CheckValue<IkReal> x331 =
7830  IKatan2WithCheck(IkReal(new_r00),
7831  IkReal(((-1.0) * new_r10)),
7832  IKFAST_ATAN2_MAGTHRESH);
7833  if (!x331.valid)
7834  {
7835  continue;
7836  }
7837  IkReal x328 = ((-1.0) * (x331.value));
7838  IkReal x329 = x310;
7839  IkReal x330 = ((-1.0) * x329);
7840  sj1 = 0;
7841  cj1 = 1.0;
7842  j1 = 0;
7843  sj2 = gconst1;
7844  cj2 = gconst2;
7845  j2 = x328;
7846  IkReal gconst0 = x328;
7847  IkReal gconst1 = (new_r00 * x330);
7848  IkReal gconst2 = (new_r10 * x330);
7849  IkReal x332 = new_r10 * new_r10;
7850  IkReal x333 = new_r00 * new_r00;
7851  CheckValue<IkReal> x340 =
7852  IKPowWithIntegerCheck((x333 + x332), -1);
7853  if (!x340.valid)
7854  {
7855  continue;
7856  }
7857  IkReal x334 = x340.value;
7858  IkReal x335 = (x332 * x334);
7859  CheckValue<IkReal> x341 = IKPowWithIntegerCheck(
7860  ((((-1.0) * x332)) + (((-1.0) * x333))), -1);
7861  if (!x341.valid)
7862  {
7863  continue;
7864  }
7865  IkReal x336 = x341.value;
7866  IkReal x337 = ((1.0) * x336);
7867  IkReal x338 = (new_r00 * x337);
7868  IkReal x339 = (new_r10 * x337);
7869  j0eval[0] =
7870  ((IKabs(((((-1.0) * new_r10 * x338)) +
7871  (((-1.0) * x338 *
7872  (new_r10 * new_r10 * new_r10))) +
7873  (((-1.0) * new_r10 * x338 *
7874  (new_r00 * new_r00)))))) +
7875  (IKabs((((x334 * (x333 * x333))) +
7876  (((-1.0) * x335)) + ((x333 * x335))))));
7877  if (IKabs(j0eval[0]) < 0.0000010000000000)
7878  {
7879  continue; // no branches [j0]
7880  }
7881  else
7882  {
7883  {
7884  IkReal j0array[1], cj0array[1], sj0array[1];
7885  bool j0valid[1] = { false };
7886  _nj0 = 1;
7887  CheckValue<IkReal> x342 = IKatan2WithCheck(
7888  IkReal(((((-1.0) * (gconst2 * gconst2))) +
7889  (new_r00 * new_r00))),
7890  IkReal(((((-1.0) * gconst1 * gconst2)) +
7891  (((-1.0) * new_r00 * new_r10)))),
7892  IKFAST_ATAN2_MAGTHRESH);
7893  if (!x342.valid)
7894  {
7895  continue;
7896  }
7897  CheckValue<IkReal> x343 = IKPowWithIntegerCheck(
7898  IKsign((((gconst2 * new_r10)) +
7899  ((gconst1 * new_r00)))),
7900  -1);
7901  if (!x343.valid)
7902  {
7903  continue;
7904  }
7905  j0array[0] = ((-1.5707963267949) + (x342.value) +
7906  (((1.5707963267949) * (x343.value))));
7907  sj0array[0] = IKsin(j0array[0]);
7908  cj0array[0] = IKcos(j0array[0]);
7909  if (j0array[0] > IKPI)
7910  {
7911  j0array[0] -= IK2PI;
7912  }
7913  else if (j0array[0] < -IKPI)
7914  {
7915  j0array[0] += IK2PI;
7916  }
7917  j0valid[0] = true;
7918  for (int ij0 = 0; ij0 < 1; ++ij0)
7919  {
7920  if (!j0valid[ij0])
7921  {
7922  continue;
7923  }
7924  _ij0[0] = ij0;
7925  _ij0[1] = -1;
7926  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
7927  {
7928  if (j0valid[iij0] &&
7929  IKabs(cj0array[ij0] - cj0array[iij0]) <
7930  IKFAST_SOLUTION_THRESH &&
7931  IKabs(sj0array[ij0] - sj0array[iij0]) <
7932  IKFAST_SOLUTION_THRESH)
7933  {
7934  j0valid[iij0] = false;
7935  _ij0[1] = iij0;
7936  break;
7937  }
7938  }
7939  j0 = j0array[ij0];
7940  cj0 = cj0array[ij0];
7941  sj0 = sj0array[ij0];
7942  {
7943  IkReal evalcond[8];
7944  IkReal x344 = IKcos(j0);
7945  IkReal x345 = IKsin(j0);
7946  IkReal x346 = ((1.0) * gconst1);
7947  IkReal x347 = (gconst2 * x344);
7948  IkReal x348 = (gconst1 * x344);
7949  IkReal x349 = (gconst2 * x345);
7950  IkReal x350 = ((1.0) * x345);
7951  IkReal x351 = (x345 * x346);
7952  evalcond[0] = (gconst2 + ((new_r00 * x344)) +
7953  ((new_r10 * x345)));
7954  evalcond[1] = (x348 + x349 + new_r10);
7955  evalcond[2] = (gconst1 + ((new_r10 * x344)) +
7956  (((-1.0) * new_r00 * x350)));
7957  evalcond[3] = (gconst2 + ((new_r11 * x344)) +
7958  (((-1.0) * new_r01 * x350)));
7959  evalcond[4] =
7960  ((((-1.0) * x351)) + x347 + new_r00);
7961  evalcond[5] =
7962  ((((-1.0) * x351)) + x347 + new_r11);
7963  evalcond[6] =
7964  (((new_r11 * x345)) + ((new_r01 * x344)) +
7965  (((-1.0) * x346)));
7966  evalcond[7] = ((((-1.0) * x344 * x346)) +
7967  new_r01 + (((-1.0) * x349)));
7968  if (IKabs(evalcond[0]) >
7969  IKFAST_EVALCOND_THRESH ||
7970  IKabs(evalcond[1]) >
7971  IKFAST_EVALCOND_THRESH ||
7972  IKabs(evalcond[2]) >
7973  IKFAST_EVALCOND_THRESH ||
7974  IKabs(evalcond[3]) >
7975  IKFAST_EVALCOND_THRESH ||
7976  IKabs(evalcond[4]) >
7977  IKFAST_EVALCOND_THRESH ||
7978  IKabs(evalcond[5]) >
7979  IKFAST_EVALCOND_THRESH ||
7980  IKabs(evalcond[6]) >
7981  IKFAST_EVALCOND_THRESH ||
7982  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
7983  {
7984  continue;
7985  }
7986  }
7987 
7988  {
7989  std::vector<IkSingleDOFSolutionBase<IkReal> >
7990  vinfos(7);
7991  vinfos[0].jointtype = 1;
7992  vinfos[0].foffset = j0;
7993  vinfos[0].indices[0] = _ij0[0];
7994  vinfos[0].indices[1] = _ij0[1];
7995  vinfos[0].maxsolutions = _nj0;
7996  vinfos[1].jointtype = 1;
7997  vinfos[1].foffset = j1;
7998  vinfos[1].indices[0] = _ij1[0];
7999  vinfos[1].indices[1] = _ij1[1];
8000  vinfos[1].maxsolutions = _nj1;
8001  vinfos[2].jointtype = 1;
8002  vinfos[2].foffset = j2;
8003  vinfos[2].indices[0] = _ij2[0];
8004  vinfos[2].indices[1] = _ij2[1];
8005  vinfos[2].maxsolutions = _nj2;
8006  vinfos[3].jointtype = 1;
8007  vinfos[3].foffset = j3;
8008  vinfos[3].indices[0] = _ij3[0];
8009  vinfos[3].indices[1] = _ij3[1];
8010  vinfos[3].maxsolutions = _nj3;
8011  vinfos[4].jointtype = 1;
8012  vinfos[4].foffset = j4;
8013  vinfos[4].indices[0] = _ij4[0];
8014  vinfos[4].indices[1] = _ij4[1];
8015  vinfos[4].maxsolutions = _nj4;
8016  vinfos[5].jointtype = 1;
8017  vinfos[5].foffset = j5;
8018  vinfos[5].indices[0] = _ij5[0];
8019  vinfos[5].indices[1] = _ij5[1];
8020  vinfos[5].maxsolutions = _nj5;
8021  vinfos[6].jointtype = 1;
8022  vinfos[6].foffset = j6;
8023  vinfos[6].indices[0] = _ij6[0];
8024  vinfos[6].indices[1] = _ij6[1];
8025  vinfos[6].maxsolutions = _nj6;
8026  std::vector<int> vfree(0);
8027  solutions.AddSolution(vinfos, vfree);
8028  }
8029  }
8030  }
8031  }
8032  }
8033  }
8034  else
8035  {
8036  {
8037  IkReal j0array[1], cj0array[1], sj0array[1];
8038  bool j0valid[1] = { false };
8039  _nj0 = 1;
8040  IkReal x352 = ((1.0) * gconst2);
8041  CheckValue<IkReal> x353 = IKatan2WithCheck(
8042  IkReal((((gconst2 * new_r00)) +
8043  (((-1.0) * new_r11 * x352)))),
8044  IkReal(((((-1.0) * new_r01 * x352)) +
8045  (((-1.0) * new_r10 * x352)))),
8046  IKFAST_ATAN2_MAGTHRESH);
8047  if (!x353.valid)
8048  {
8049  continue;
8050  }
8051  CheckValue<IkReal> x354 = IKPowWithIntegerCheck(
8052  IKsign((((new_r10 * new_r11)) +
8053  ((new_r00 * new_r01)))),
8054  -1);
8055  if (!x354.valid)
8056  {
8057  continue;
8058  }
8059  j0array[0] = ((-1.5707963267949) + (x353.value) +
8060  (((1.5707963267949) * (x354.value))));
8061  sj0array[0] = IKsin(j0array[0]);
8062  cj0array[0] = IKcos(j0array[0]);
8063  if (j0array[0] > IKPI)
8064  {
8065  j0array[0] -= IK2PI;
8066  }
8067  else if (j0array[0] < -IKPI)
8068  {
8069  j0array[0] += IK2PI;
8070  }
8071  j0valid[0] = true;
8072  for (int ij0 = 0; ij0 < 1; ++ij0)
8073  {
8074  if (!j0valid[ij0])
8075  {
8076  continue;
8077  }
8078  _ij0[0] = ij0;
8079  _ij0[1] = -1;
8080  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8081  {
8082  if (j0valid[iij0] &&
8083  IKabs(cj0array[ij0] - cj0array[iij0]) <
8084  IKFAST_SOLUTION_THRESH &&
8085  IKabs(sj0array[ij0] - sj0array[iij0]) <
8086  IKFAST_SOLUTION_THRESH)
8087  {
8088  j0valid[iij0] = false;
8089  _ij0[1] = iij0;
8090  break;
8091  }
8092  }
8093  j0 = j0array[ij0];
8094  cj0 = cj0array[ij0];
8095  sj0 = sj0array[ij0];
8096  {
8097  IkReal evalcond[8];
8098  IkReal x355 = IKcos(j0);
8099  IkReal x356 = IKsin(j0);
8100  IkReal x357 = ((1.0) * gconst1);
8101  IkReal x358 = (gconst2 * x355);
8102  IkReal x359 = (gconst1 * x355);
8103  IkReal x360 = (gconst2 * x356);
8104  IkReal x361 = ((1.0) * x356);
8105  IkReal x362 = (x356 * x357);
8106  evalcond[0] = (gconst2 + ((new_r10 * x356)) +
8107  ((new_r00 * x355)));
8108  evalcond[1] = (x359 + x360 + new_r10);
8109  evalcond[2] = ((((-1.0) * new_r00 * x361)) +
8110  gconst1 + ((new_r10 * x355)));
8111  evalcond[3] = (gconst2 + ((new_r11 * x355)) +
8112  (((-1.0) * new_r01 * x361)));
8113  evalcond[4] = ((((-1.0) * x362)) + x358 + new_r00);
8114  evalcond[5] = ((((-1.0) * x362)) + x358 + new_r11);
8115  evalcond[6] =
8116  (((new_r01 * x355)) + (((-1.0) * x357)) +
8117  ((new_r11 * x356)));
8118  evalcond[7] = ((((-1.0) * x360)) + new_r01 +
8119  (((-1.0) * x355 * x357)));
8120  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8121  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8122  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8123  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8124  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8125  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8126  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8127  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8128  {
8129  continue;
8130  }
8131  }
8132 
8133  {
8134  std::vector<IkSingleDOFSolutionBase<IkReal> >
8135  vinfos(7);
8136  vinfos[0].jointtype = 1;
8137  vinfos[0].foffset = j0;
8138  vinfos[0].indices[0] = _ij0[0];
8139  vinfos[0].indices[1] = _ij0[1];
8140  vinfos[0].maxsolutions = _nj0;
8141  vinfos[1].jointtype = 1;
8142  vinfos[1].foffset = j1;
8143  vinfos[1].indices[0] = _ij1[0];
8144  vinfos[1].indices[1] = _ij1[1];
8145  vinfos[1].maxsolutions = _nj1;
8146  vinfos[2].jointtype = 1;
8147  vinfos[2].foffset = j2;
8148  vinfos[2].indices[0] = _ij2[0];
8149  vinfos[2].indices[1] = _ij2[1];
8150  vinfos[2].maxsolutions = _nj2;
8151  vinfos[3].jointtype = 1;
8152  vinfos[3].foffset = j3;
8153  vinfos[3].indices[0] = _ij3[0];
8154  vinfos[3].indices[1] = _ij3[1];
8155  vinfos[3].maxsolutions = _nj3;
8156  vinfos[4].jointtype = 1;
8157  vinfos[4].foffset = j4;
8158  vinfos[4].indices[0] = _ij4[0];
8159  vinfos[4].indices[1] = _ij4[1];
8160  vinfos[4].maxsolutions = _nj4;
8161  vinfos[5].jointtype = 1;
8162  vinfos[5].foffset = j5;
8163  vinfos[5].indices[0] = _ij5[0];
8164  vinfos[5].indices[1] = _ij5[1];
8165  vinfos[5].maxsolutions = _nj5;
8166  vinfos[6].jointtype = 1;
8167  vinfos[6].foffset = j6;
8168  vinfos[6].indices[0] = _ij6[0];
8169  vinfos[6].indices[1] = _ij6[1];
8170  vinfos[6].maxsolutions = _nj6;
8171  std::vector<int> vfree(0);
8172  solutions.AddSolution(vinfos, vfree);
8173  }
8174  }
8175  }
8176  }
8177  }
8178  }
8179  else
8180  {
8181  {
8182  IkReal j0array[1], cj0array[1], sj0array[1];
8183  bool j0valid[1] = { false };
8184  _nj0 = 1;
8185  IkReal x363 = ((1.0) * new_r10);
8186  CheckValue<IkReal> x364 = IKPowWithIntegerCheck(
8187  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8188  -1);
8189  if (!x364.valid)
8190  {
8191  continue;
8192  }
8193  CheckValue<IkReal> x365 = IKatan2WithCheck(
8194  IkReal((((gconst1 * new_r00)) +
8195  (((-1.0) * gconst2 * x363)))),
8196  IkReal(((((-1.0) * gconst1 * x363)) +
8197  (((-1.0) * gconst2 * new_r00)))),
8198  IKFAST_ATAN2_MAGTHRESH);
8199  if (!x365.valid)
8200  {
8201  continue;
8202  }
8203  j0array[0] =
8204  ((-1.5707963267949) +
8205  (((1.5707963267949) * (x364.value))) + (x365.value));
8206  sj0array[0] = IKsin(j0array[0]);
8207  cj0array[0] = IKcos(j0array[0]);
8208  if (j0array[0] > IKPI)
8209  {
8210  j0array[0] -= IK2PI;
8211  }
8212  else if (j0array[0] < -IKPI)
8213  {
8214  j0array[0] += IK2PI;
8215  }
8216  j0valid[0] = true;
8217  for (int ij0 = 0; ij0 < 1; ++ij0)
8218  {
8219  if (!j0valid[ij0])
8220  {
8221  continue;
8222  }
8223  _ij0[0] = ij0;
8224  _ij0[1] = -1;
8225  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8226  {
8227  if (j0valid[iij0] &&
8228  IKabs(cj0array[ij0] - cj0array[iij0]) <
8229  IKFAST_SOLUTION_THRESH &&
8230  IKabs(sj0array[ij0] - sj0array[iij0]) <
8231  IKFAST_SOLUTION_THRESH)
8232  {
8233  j0valid[iij0] = false;
8234  _ij0[1] = iij0;
8235  break;
8236  }
8237  }
8238  j0 = j0array[ij0];
8239  cj0 = cj0array[ij0];
8240  sj0 = sj0array[ij0];
8241  {
8242  IkReal evalcond[8];
8243  IkReal x366 = IKcos(j0);
8244  IkReal x367 = IKsin(j0);
8245  IkReal x368 = ((1.0) * gconst1);
8246  IkReal x369 = (gconst2 * x366);
8247  IkReal x370 = (gconst1 * x366);
8248  IkReal x371 = (gconst2 * x367);
8249  IkReal x372 = ((1.0) * x367);
8250  IkReal x373 = (x367 * x368);
8251  evalcond[0] =
8252  (gconst2 + ((new_r00 * x366)) + ((new_r10 * x367)));
8253  evalcond[1] = (x371 + x370 + new_r10);
8254  evalcond[2] = ((((-1.0) * new_r00 * x372)) + gconst1 +
8255  ((new_r10 * x366)));
8256  evalcond[3] = (gconst2 + (((-1.0) * new_r01 * x372)) +
8257  ((new_r11 * x366)));
8258  evalcond[4] = ((((-1.0) * x373)) + x369 + new_r00);
8259  evalcond[5] = ((((-1.0) * x373)) + x369 + new_r11);
8260  evalcond[6] = ((((-1.0) * x368)) + ((new_r01 * x366)) +
8261  ((new_r11 * x367)));
8262  evalcond[7] = ((((-1.0) * x371)) +
8263  (((-1.0) * x366 * x368)) + new_r01);
8264  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8265  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8266  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8267  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8268  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8269  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8270  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8271  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8272  {
8273  continue;
8274  }
8275  }
8276 
8277  {
8278  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8279  vinfos[0].jointtype = 1;
8280  vinfos[0].foffset = j0;
8281  vinfos[0].indices[0] = _ij0[0];
8282  vinfos[0].indices[1] = _ij0[1];
8283  vinfos[0].maxsolutions = _nj0;
8284  vinfos[1].jointtype = 1;
8285  vinfos[1].foffset = j1;
8286  vinfos[1].indices[0] = _ij1[0];
8287  vinfos[1].indices[1] = _ij1[1];
8288  vinfos[1].maxsolutions = _nj1;
8289  vinfos[2].jointtype = 1;
8290  vinfos[2].foffset = j2;
8291  vinfos[2].indices[0] = _ij2[0];
8292  vinfos[2].indices[1] = _ij2[1];
8293  vinfos[2].maxsolutions = _nj2;
8294  vinfos[3].jointtype = 1;
8295  vinfos[3].foffset = j3;
8296  vinfos[3].indices[0] = _ij3[0];
8297  vinfos[3].indices[1] = _ij3[1];
8298  vinfos[3].maxsolutions = _nj3;
8299  vinfos[4].jointtype = 1;
8300  vinfos[4].foffset = j4;
8301  vinfos[4].indices[0] = _ij4[0];
8302  vinfos[4].indices[1] = _ij4[1];
8303  vinfos[4].maxsolutions = _nj4;
8304  vinfos[5].jointtype = 1;
8305  vinfos[5].foffset = j5;
8306  vinfos[5].indices[0] = _ij5[0];
8307  vinfos[5].indices[1] = _ij5[1];
8308  vinfos[5].maxsolutions = _nj5;
8309  vinfos[6].jointtype = 1;
8310  vinfos[6].foffset = j6;
8311  vinfos[6].indices[0] = _ij6[0];
8312  vinfos[6].indices[1] = _ij6[1];
8313  vinfos[6].maxsolutions = _nj6;
8314  std::vector<int> vfree(0);
8315  solutions.AddSolution(vinfos, vfree);
8316  }
8317  }
8318  }
8319  }
8320  }
8321  }
8322  } while (0);
8323  if (bgotonextstatement)
8324  {
8325  bool bgotonextstatement = true;
8326  do
8327  {
8328  IkReal x376 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8329  if (IKabs(x376) == 0)
8330  {
8331  continue;
8332  }
8333  IkReal x374 = pow(x376, -0.5);
8334  IkReal x375 = ((1.0) * x374);
8335  CheckValue<IkReal> x377 =
8336  IKatan2WithCheck(IkReal(new_r00),
8337  IkReal(((-1.0) * new_r10)),
8338  IKFAST_ATAN2_MAGTHRESH);
8339  if (!x377.valid)
8340  {
8341  continue;
8342  }
8343  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * (x377.value))));
8344  IkReal gconst4 = (new_r00 * x375);
8345  IkReal gconst5 = (new_r10 * x375);
8346  CheckValue<IkReal> x378 =
8347  IKatan2WithCheck(IkReal(new_r00),
8348  IkReal(((-1.0) * new_r10)),
8349  IKFAST_ATAN2_MAGTHRESH);
8350  if (!x378.valid)
8351  {
8352  continue;
8353  }
8354  evalcond[0] =
8355  ((-3.14159265358979) +
8356  (IKfmod(((3.14159265358979) +
8357  (IKabs(((-3.14159265358979) + (x378.value) + j2)))),
8358  6.28318530717959)));
8359  if (IKabs(evalcond[0]) < 0.0000050000000000)
8360  {
8361  bgotonextstatement = false;
8362  {
8363  IkReal j0eval[2];
8364  CheckValue<IkReal> x382 =
8365  IKatan2WithCheck(IkReal(new_r00),
8366  IkReal(((-1.0) * new_r10)),
8367  IKFAST_ATAN2_MAGTHRESH);
8368  if (!x382.valid)
8369  {
8370  continue;
8371  }
8372  IkReal x379 = ((1.0) * (x382.value));
8373  IkReal x380 = x374;
8374  IkReal x381 = ((1.0) * x380);
8375  sj1 = 0;
8376  cj1 = 1.0;
8377  j1 = 0;
8378  sj2 = gconst4;
8379  cj2 = gconst5;
8380  j2 = ((3.14159265) + (((-1.0) * x379)));
8381  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x379)));
8382  IkReal gconst4 = (new_r00 * x381);
8383  IkReal gconst5 = (new_r10 * x381);
8384  IkReal x383 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8385  j0eval[0] = x383;
8386  j0eval[1] = IKsign(x383);
8387  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8388  IKabs(j0eval[1]) < 0.0000010000000000)
8389  {
8390  {
8391  IkReal j0eval[3];
8392  CheckValue<IkReal> x387 =
8393  IKatan2WithCheck(IkReal(new_r00),
8394  IkReal(((-1.0) * new_r10)),
8395  IKFAST_ATAN2_MAGTHRESH);
8396  if (!x387.valid)
8397  {
8398  continue;
8399  }
8400  IkReal x384 = ((1.0) * (x387.value));
8401  IkReal x385 = x374;
8402  IkReal x386 = ((1.0) * x385);
8403  sj1 = 0;
8404  cj1 = 1.0;
8405  j1 = 0;
8406  sj2 = gconst4;
8407  cj2 = gconst5;
8408  j2 = ((3.14159265) + (((-1.0) * x384)));
8409  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x384)));
8410  IkReal gconst4 = (new_r00 * x386);
8411  IkReal gconst5 = (new_r10 * x386);
8412  IkReal x388 = new_r10 * new_r10;
8413  IkReal x389 = (new_r10 * new_r11);
8414  IkReal x390 = (((new_r00 * new_r01)) + x389);
8415  IkReal x391 = x374;
8416  IkReal x392 = ((1.0) * x391);
8417  j0eval[0] = x390;
8418  j0eval[1] =
8419  ((IKabs(((((-1.0) * new_r01 * new_r10 * x392)) +
8420  (((-1.0) * x388 * x392))))) +
8421  (IKabs(((((-1.0) * x389 * x392)) +
8422  ((new_r00 * new_r10 * x391))))));
8423  j0eval[2] = IKsign(x390);
8424  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8425  IKabs(j0eval[1]) < 0.0000010000000000 ||
8426  IKabs(j0eval[2]) < 0.0000010000000000)
8427  {
8428  {
8429  IkReal j0eval[1];
8430  CheckValue<IkReal> x396 =
8431  IKatan2WithCheck(IkReal(new_r00),
8432  IkReal(((-1.0) * new_r10)),
8433  IKFAST_ATAN2_MAGTHRESH);
8434  if (!x396.valid)
8435  {
8436  continue;
8437  }
8438  IkReal x393 = ((1.0) * (x396.value));
8439  IkReal x394 = x374;
8440  IkReal x395 = ((1.0) * x394);
8441  sj1 = 0;
8442  cj1 = 1.0;
8443  j1 = 0;
8444  sj2 = gconst4;
8445  cj2 = gconst5;
8446  j2 = ((3.14159265) + (((-1.0) * x393)));
8447  IkReal gconst3 =
8448  ((3.14159265358979) + (((-1.0) * x393)));
8449  IkReal gconst4 = (new_r00 * x395);
8450  IkReal gconst5 = (new_r10 * x395);
8451  IkReal x397 = new_r10 * new_r10;
8452  IkReal x398 = new_r00 * new_r00;
8453  CheckValue<IkReal> x405 =
8454  IKPowWithIntegerCheck((x397 + x398), -1);
8455  if (!x405.valid)
8456  {
8457  continue;
8458  }
8459  IkReal x399 = x405.value;
8460  IkReal x400 = (x397 * x399);
8461  CheckValue<IkReal> x406 = IKPowWithIntegerCheck(
8462  ((((-1.0) * x398)) + (((-1.0) * x397))), -1);
8463  if (!x406.valid)
8464  {
8465  continue;
8466  }
8467  IkReal x401 = x406.value;
8468  IkReal x402 = ((1.0) * x401);
8469  IkReal x403 = (new_r00 * x402);
8470  IkReal x404 = (new_r10 * x402);
8471  j0eval[0] =
8472  ((IKabs((((x399 * (x398 * x398))) +
8473  ((x398 * x400)) + (((-1.0) * x400))))) +
8474  (IKabs(((((-1.0) * x403 *
8475  (new_r10 * new_r10 * new_r10))) +
8476  (((-1.0) * new_r10 * x403)) +
8477  (((-1.0) * new_r10 * x403 *
8478  (new_r00 * new_r00)))))));
8479  if (IKabs(j0eval[0]) < 0.0000010000000000)
8480  {
8481  continue; // no branches [j0]
8482  }
8483  else
8484  {
8485  {
8486  IkReal j0array[1], cj0array[1], sj0array[1];
8487  bool j0valid[1] = { false };
8488  _nj0 = 1;
8489  CheckValue<IkReal> x407 = IKPowWithIntegerCheck(
8490  IKsign((((gconst4 * new_r00)) +
8491  ((gconst5 * new_r10)))),
8492  -1);
8493  if (!x407.valid)
8494  {
8495  continue;
8496  }
8497  CheckValue<IkReal> x408 = IKatan2WithCheck(
8498  IkReal(((((-1.0) * (gconst5 * gconst5))) +
8499  (new_r00 * new_r00))),
8500  IkReal(((((-1.0) * gconst4 * gconst5)) +
8501  (((-1.0) * new_r00 * new_r10)))),
8502  IKFAST_ATAN2_MAGTHRESH);
8503  if (!x408.valid)
8504  {
8505  continue;
8506  }
8507  j0array[0] =
8508  ((-1.5707963267949) +
8509  (((1.5707963267949) * (x407.value))) +
8510  (x408.value));
8511  sj0array[0] = IKsin(j0array[0]);
8512  cj0array[0] = IKcos(j0array[0]);
8513  if (j0array[0] > IKPI)
8514  {
8515  j0array[0] -= IK2PI;
8516  }
8517  else if (j0array[0] < -IKPI)
8518  {
8519  j0array[0] += IK2PI;
8520  }
8521  j0valid[0] = true;
8522  for (int ij0 = 0; ij0 < 1; ++ij0)
8523  {
8524  if (!j0valid[ij0])
8525  {
8526  continue;
8527  }
8528  _ij0[0] = ij0;
8529  _ij0[1] = -1;
8530  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8531  {
8532  if (j0valid[iij0] &&
8533  IKabs(cj0array[ij0] - cj0array[iij0]) <
8534  IKFAST_SOLUTION_THRESH &&
8535  IKabs(sj0array[ij0] - sj0array[iij0]) <
8536  IKFAST_SOLUTION_THRESH)
8537  {
8538  j0valid[iij0] = false;
8539  _ij0[1] = iij0;
8540  break;
8541  }
8542  }
8543  j0 = j0array[ij0];
8544  cj0 = cj0array[ij0];
8545  sj0 = sj0array[ij0];
8546  {
8547  IkReal evalcond[8];
8548  IkReal x409 = IKcos(j0);
8549  IkReal x410 = IKsin(j0);
8550  IkReal x411 = ((1.0) * gconst4);
8551  IkReal x412 = (gconst5 * x409);
8552  IkReal x413 = ((1.0) * x410);
8553  IkReal x414 = (x410 * x411);
8554  evalcond[0] = (gconst5 + ((new_r10 * x410)) +
8555  ((new_r00 * x409)));
8556  evalcond[1] = (((gconst5 * x410)) +
8557  ((gconst4 * x409)) + new_r10);
8558  evalcond[2] = (gconst4 + ((new_r10 * x409)) +
8559  (((-1.0) * new_r00 * x413)));
8560  evalcond[3] = (gconst5 + ((new_r11 * x409)) +
8561  (((-1.0) * new_r01 * x413)));
8562  evalcond[4] =
8563  (x412 + new_r00 + (((-1.0) * x414)));
8564  evalcond[5] =
8565  (x412 + new_r11 + (((-1.0) * x414)));
8566  evalcond[6] =
8567  (((new_r11 * x410)) + ((new_r01 * x409)) +
8568  (((-1.0) * x411)));
8569  evalcond[7] =
8570  ((((-1.0) * gconst5 * x413)) + new_r01 +
8571  (((-1.0) * x409 * x411)));
8572  if (IKabs(evalcond[0]) >
8573  IKFAST_EVALCOND_THRESH ||
8574  IKabs(evalcond[1]) >
8575  IKFAST_EVALCOND_THRESH ||
8576  IKabs(evalcond[2]) >
8577  IKFAST_EVALCOND_THRESH ||
8578  IKabs(evalcond[3]) >
8579  IKFAST_EVALCOND_THRESH ||
8580  IKabs(evalcond[4]) >
8581  IKFAST_EVALCOND_THRESH ||
8582  IKabs(evalcond[5]) >
8583  IKFAST_EVALCOND_THRESH ||
8584  IKabs(evalcond[6]) >
8585  IKFAST_EVALCOND_THRESH ||
8586  IKabs(evalcond[7]) >
8587  IKFAST_EVALCOND_THRESH)
8588  {
8589  continue;
8590  }
8591  }
8592 
8593  {
8594  std::vector<IkSingleDOFSolutionBase<IkReal> >
8595  vinfos(7);
8596  vinfos[0].jointtype = 1;
8597  vinfos[0].foffset = j0;
8598  vinfos[0].indices[0] = _ij0[0];
8599  vinfos[0].indices[1] = _ij0[1];
8600  vinfos[0].maxsolutions = _nj0;
8601  vinfos[1].jointtype = 1;
8602  vinfos[1].foffset = j1;
8603  vinfos[1].indices[0] = _ij1[0];
8604  vinfos[1].indices[1] = _ij1[1];
8605  vinfos[1].maxsolutions = _nj1;
8606  vinfos[2].jointtype = 1;
8607  vinfos[2].foffset = j2;
8608  vinfos[2].indices[0] = _ij2[0];
8609  vinfos[2].indices[1] = _ij2[1];
8610  vinfos[2].maxsolutions = _nj2;
8611  vinfos[3].jointtype = 1;
8612  vinfos[3].foffset = j3;
8613  vinfos[3].indices[0] = _ij3[0];
8614  vinfos[3].indices[1] = _ij3[1];
8615  vinfos[3].maxsolutions = _nj3;
8616  vinfos[4].jointtype = 1;
8617  vinfos[4].foffset = j4;
8618  vinfos[4].indices[0] = _ij4[0];
8619  vinfos[4].indices[1] = _ij4[1];
8620  vinfos[4].maxsolutions = _nj4;
8621  vinfos[5].jointtype = 1;
8622  vinfos[5].foffset = j5;
8623  vinfos[5].indices[0] = _ij5[0];
8624  vinfos[5].indices[1] = _ij5[1];
8625  vinfos[5].maxsolutions = _nj5;
8626  vinfos[6].jointtype = 1;
8627  vinfos[6].foffset = j6;
8628  vinfos[6].indices[0] = _ij6[0];
8629  vinfos[6].indices[1] = _ij6[1];
8630  vinfos[6].maxsolutions = _nj6;
8631  std::vector<int> vfree(0);
8632  solutions.AddSolution(vinfos, vfree);
8633  }
8634  }
8635  }
8636  }
8637  }
8638  }
8639  else
8640  {
8641  {
8642  IkReal j0array[1], cj0array[1], sj0array[1];
8643  bool j0valid[1] = { false };
8644  _nj0 = 1;
8645  IkReal x415 = ((1.0) * gconst5);
8646  CheckValue<IkReal> x416 = IKatan2WithCheck(
8647  IkReal((((gconst5 * new_r00)) +
8648  (((-1.0) * new_r11 * x415)))),
8649  IkReal(((((-1.0) * new_r10 * x415)) +
8650  (((-1.0) * new_r01 * x415)))),
8651  IKFAST_ATAN2_MAGTHRESH);
8652  if (!x416.valid)
8653  {
8654  continue;
8655  }
8656  CheckValue<IkReal> x417 = IKPowWithIntegerCheck(
8657  IKsign((((new_r10 * new_r11)) +
8658  ((new_r00 * new_r01)))),
8659  -1);
8660  if (!x417.valid)
8661  {
8662  continue;
8663  }
8664  j0array[0] = ((-1.5707963267949) + (x416.value) +
8665  (((1.5707963267949) * (x417.value))));
8666  sj0array[0] = IKsin(j0array[0]);
8667  cj0array[0] = IKcos(j0array[0]);
8668  if (j0array[0] > IKPI)
8669  {
8670  j0array[0] -= IK2PI;
8671  }
8672  else if (j0array[0] < -IKPI)
8673  {
8674  j0array[0] += IK2PI;
8675  }
8676  j0valid[0] = true;
8677  for (int ij0 = 0; ij0 < 1; ++ij0)
8678  {
8679  if (!j0valid[ij0])
8680  {
8681  continue;
8682  }
8683  _ij0[0] = ij0;
8684  _ij0[1] = -1;
8685  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8686  {
8687  if (j0valid[iij0] &&
8688  IKabs(cj0array[ij0] - cj0array[iij0]) <
8689  IKFAST_SOLUTION_THRESH &&
8690  IKabs(sj0array[ij0] - sj0array[iij0]) <
8691  IKFAST_SOLUTION_THRESH)
8692  {
8693  j0valid[iij0] = false;
8694  _ij0[1] = iij0;
8695  break;
8696  }
8697  }
8698  j0 = j0array[ij0];
8699  cj0 = cj0array[ij0];
8700  sj0 = sj0array[ij0];
8701  {
8702  IkReal evalcond[8];
8703  IkReal x418 = IKcos(j0);
8704  IkReal x419 = IKsin(j0);
8705  IkReal x420 = ((1.0) * gconst4);
8706  IkReal x421 = (gconst5 * x418);
8707  IkReal x422 = ((1.0) * x419);
8708  IkReal x423 = (x419 * x420);
8709  evalcond[0] = (gconst5 + ((new_r10 * x419)) +
8710  ((new_r00 * x418)));
8711  evalcond[1] = (((gconst5 * x419)) + new_r10 +
8712  ((gconst4 * x418)));
8713  evalcond[2] = ((((-1.0) * new_r00 * x422)) +
8714  gconst4 + ((new_r10 * x418)));
8715  evalcond[3] = (gconst5 + ((new_r11 * x418)) +
8716  (((-1.0) * new_r01 * x422)));
8717  evalcond[4] =
8718  ((((-1.0) * x423)) + x421 + new_r00);
8719  evalcond[5] =
8720  ((((-1.0) * x423)) + x421 + new_r11);
8721  evalcond[6] =
8722  ((((-1.0) * x420)) + ((new_r11 * x419)) +
8723  ((new_r01 * x418)));
8724  evalcond[7] =
8725  ((((-1.0) * x418 * x420)) + new_r01 +
8726  (((-1.0) * gconst5 * x422)));
8727  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8728  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8729  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8730  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8731  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8732  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8733  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8734  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8735  {
8736  continue;
8737  }
8738  }
8739 
8740  {
8741  std::vector<IkSingleDOFSolutionBase<IkReal> >
8742  vinfos(7);
8743  vinfos[0].jointtype = 1;
8744  vinfos[0].foffset = j0;
8745  vinfos[0].indices[0] = _ij0[0];
8746  vinfos[0].indices[1] = _ij0[1];
8747  vinfos[0].maxsolutions = _nj0;
8748  vinfos[1].jointtype = 1;
8749  vinfos[1].foffset = j1;
8750  vinfos[1].indices[0] = _ij1[0];
8751  vinfos[1].indices[1] = _ij1[1];
8752  vinfos[1].maxsolutions = _nj1;
8753  vinfos[2].jointtype = 1;
8754  vinfos[2].foffset = j2;
8755  vinfos[2].indices[0] = _ij2[0];
8756  vinfos[2].indices[1] = _ij2[1];
8757  vinfos[2].maxsolutions = _nj2;
8758  vinfos[3].jointtype = 1;
8759  vinfos[3].foffset = j3;
8760  vinfos[3].indices[0] = _ij3[0];
8761  vinfos[3].indices[1] = _ij3[1];
8762  vinfos[3].maxsolutions = _nj3;
8763  vinfos[4].jointtype = 1;
8764  vinfos[4].foffset = j4;
8765  vinfos[4].indices[0] = _ij4[0];
8766  vinfos[4].indices[1] = _ij4[1];
8767  vinfos[4].maxsolutions = _nj4;
8768  vinfos[5].jointtype = 1;
8769  vinfos[5].foffset = j5;
8770  vinfos[5].indices[0] = _ij5[0];
8771  vinfos[5].indices[1] = _ij5[1];
8772  vinfos[5].maxsolutions = _nj5;
8773  vinfos[6].jointtype = 1;
8774  vinfos[6].foffset = j6;
8775  vinfos[6].indices[0] = _ij6[0];
8776  vinfos[6].indices[1] = _ij6[1];
8777  vinfos[6].maxsolutions = _nj6;
8778  std::vector<int> vfree(0);
8779  solutions.AddSolution(vinfos, vfree);
8780  }
8781  }
8782  }
8783  }
8784  }
8785  }
8786  else
8787  {
8788  {
8789  IkReal j0array[1], cj0array[1], sj0array[1];
8790  bool j0valid[1] = { false };
8791  _nj0 = 1;
8792  IkReal x424 = ((1.0) * new_r10);
8793  CheckValue<IkReal> x425 = IKatan2WithCheck(
8794  IkReal((((gconst4 * new_r00)) +
8795  (((-1.0) * gconst5 * x424)))),
8796  IkReal(((((-1.0) * gconst4 * x424)) +
8797  (((-1.0) * gconst5 * new_r00)))),
8798  IKFAST_ATAN2_MAGTHRESH);
8799  if (!x425.valid)
8800  {
8801  continue;
8802  }
8803  CheckValue<IkReal> x426 = IKPowWithIntegerCheck(
8804  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8805  -1);
8806  if (!x426.valid)
8807  {
8808  continue;
8809  }
8810  j0array[0] = ((-1.5707963267949) + (x425.value) +
8811  (((1.5707963267949) * (x426.value))));
8812  sj0array[0] = IKsin(j0array[0]);
8813  cj0array[0] = IKcos(j0array[0]);
8814  if (j0array[0] > IKPI)
8815  {
8816  j0array[0] -= IK2PI;
8817  }
8818  else if (j0array[0] < -IKPI)
8819  {
8820  j0array[0] += IK2PI;
8821  }
8822  j0valid[0] = true;
8823  for (int ij0 = 0; ij0 < 1; ++ij0)
8824  {
8825  if (!j0valid[ij0])
8826  {
8827  continue;
8828  }
8829  _ij0[0] = ij0;
8830  _ij0[1] = -1;
8831  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8832  {
8833  if (j0valid[iij0] &&
8834  IKabs(cj0array[ij0] - cj0array[iij0]) <
8835  IKFAST_SOLUTION_THRESH &&
8836  IKabs(sj0array[ij0] - sj0array[iij0]) <
8837  IKFAST_SOLUTION_THRESH)
8838  {
8839  j0valid[iij0] = false;
8840  _ij0[1] = iij0;
8841  break;
8842  }
8843  }
8844  j0 = j0array[ij0];
8845  cj0 = cj0array[ij0];
8846  sj0 = sj0array[ij0];
8847  {
8848  IkReal evalcond[8];
8849  IkReal x427 = IKcos(j0);
8850  IkReal x428 = IKsin(j0);
8851  IkReal x429 = ((1.0) * gconst4);
8852  IkReal x430 = (gconst5 * x427);
8853  IkReal x431 = ((1.0) * x428);
8854  IkReal x432 = (x428 * x429);
8855  evalcond[0] = (gconst5 + ((new_r10 * x428)) +
8856  ((new_r00 * x427)));
8857  evalcond[1] = (((gconst4 * x427)) +
8858  ((gconst5 * x428)) + new_r10);
8859  evalcond[2] = ((((-1.0) * new_r00 * x431)) + gconst4 +
8860  ((new_r10 * x427)));
8861  evalcond[3] = ((((-1.0) * new_r01 * x431)) + gconst5 +
8862  ((new_r11 * x427)));
8863  evalcond[4] = ((((-1.0) * x432)) + x430 + new_r00);
8864  evalcond[5] = ((((-1.0) * x432)) + x430 + new_r11);
8865  evalcond[6] =
8866  ((((-1.0) * x429)) + ((new_r11 * x428)) +
8867  ((new_r01 * x427)));
8868  evalcond[7] = ((((-1.0) * x427 * x429)) +
8869  (((-1.0) * gconst5 * x431)) + new_r01);
8870  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8871  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8872  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8873  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8874  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8875  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8876  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8877  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8878  {
8879  continue;
8880  }
8881  }
8882 
8883  {
8884  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
8885  7);
8886  vinfos[0].jointtype = 1;
8887  vinfos[0].foffset = j0;
8888  vinfos[0].indices[0] = _ij0[0];
8889  vinfos[0].indices[1] = _ij0[1];
8890  vinfos[0].maxsolutions = _nj0;
8891  vinfos[1].jointtype = 1;
8892  vinfos[1].foffset = j1;
8893  vinfos[1].indices[0] = _ij1[0];
8894  vinfos[1].indices[1] = _ij1[1];
8895  vinfos[1].maxsolutions = _nj1;
8896  vinfos[2].jointtype = 1;
8897  vinfos[2].foffset = j2;
8898  vinfos[2].indices[0] = _ij2[0];
8899  vinfos[2].indices[1] = _ij2[1];
8900  vinfos[2].maxsolutions = _nj2;
8901  vinfos[3].jointtype = 1;
8902  vinfos[3].foffset = j3;
8903  vinfos[3].indices[0] = _ij3[0];
8904  vinfos[3].indices[1] = _ij3[1];
8905  vinfos[3].maxsolutions = _nj3;
8906  vinfos[4].jointtype = 1;
8907  vinfos[4].foffset = j4;
8908  vinfos[4].indices[0] = _ij4[0];
8909  vinfos[4].indices[1] = _ij4[1];
8910  vinfos[4].maxsolutions = _nj4;
8911  vinfos[5].jointtype = 1;
8912  vinfos[5].foffset = j5;
8913  vinfos[5].indices[0] = _ij5[0];
8914  vinfos[5].indices[1] = _ij5[1];
8915  vinfos[5].maxsolutions = _nj5;
8916  vinfos[6].jointtype = 1;
8917  vinfos[6].foffset = j6;
8918  vinfos[6].indices[0] = _ij6[0];
8919  vinfos[6].indices[1] = _ij6[1];
8920  vinfos[6].maxsolutions = _nj6;
8921  std::vector<int> vfree(0);
8922  solutions.AddSolution(vinfos, vfree);
8923  }
8924  }
8925  }
8926  }
8927  }
8928  }
8929  } while (0);
8930  if (bgotonextstatement)
8931  {
8932  bool bgotonextstatement = true;
8933  do
8934  {
8935  evalcond[0] = ((-3.14159265358979) +
8936  (IKfmod(((3.14159265358979) +
8937  (IKabs(((-1.5707963267949) + j2)))),
8938  6.28318530717959)));
8939  if (IKabs(evalcond[0]) < 0.0000050000000000)
8940  {
8941  bgotonextstatement = false;
8942  {
8943  IkReal j0array[1], cj0array[1], sj0array[1];
8944  bool j0valid[1] = { false };
8945  _nj0 = 1;
8946  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
8947  IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
8948  IKabs(IKsqr(new_r00) + IKsqr(((-1.0) * new_r10)) - 1) <=
8949  IKFAST_SINCOS_THRESH)
8950  continue;
8951  j0array[0] = IKatan2(new_r00, ((-1.0) * new_r10));
8952  sj0array[0] = IKsin(j0array[0]);
8953  cj0array[0] = IKcos(j0array[0]);
8954  if (j0array[0] > IKPI)
8955  {
8956  j0array[0] -= IK2PI;
8957  }
8958  else if (j0array[0] < -IKPI)
8959  {
8960  j0array[0] += IK2PI;
8961  }
8962  j0valid[0] = true;
8963  for (int ij0 = 0; ij0 < 1; ++ij0)
8964  {
8965  if (!j0valid[ij0])
8966  {
8967  continue;
8968  }
8969  _ij0[0] = ij0;
8970  _ij0[1] = -1;
8971  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8972  {
8973  if (j0valid[iij0] &&
8974  IKabs(cj0array[ij0] - cj0array[iij0]) <
8975  IKFAST_SOLUTION_THRESH &&
8976  IKabs(sj0array[ij0] - sj0array[iij0]) <
8977  IKFAST_SOLUTION_THRESH)
8978  {
8979  j0valid[iij0] = false;
8980  _ij0[1] = iij0;
8981  break;
8982  }
8983  }
8984  j0 = j0array[ij0];
8985  cj0 = cj0array[ij0];
8986  sj0 = sj0array[ij0];
8987  {
8988  IkReal evalcond[8];
8989  IkReal x433 = IKcos(j0);
8990  IkReal x434 = IKsin(j0);
8991  IkReal x435 = ((1.0) * x434);
8992  evalcond[0] = (x433 + new_r10);
8993  evalcond[1] = ((((-1.0) * x435)) + new_r00);
8994  evalcond[2] = ((((-1.0) * x435)) + new_r11);
8995  evalcond[3] = ((((-1.0) * x433)) + new_r01);
8996  evalcond[4] = (((new_r00 * x433)) + ((new_r10 * x434)));
8997  evalcond[5] =
8998  ((((-1.0) * new_r01 * x435)) + ((new_r11 * x433)));
8999  evalcond[6] =
9000  ((-1.0) + ((new_r01 * x433)) + ((new_r11 * x434)));
9001  evalcond[7] = ((1.0) + (((-1.0) * new_r00 * x435)) +
9002  ((new_r10 * x433)));
9003  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9004  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9005  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9006  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9007  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9008  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9009  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9010  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9011  {
9012  continue;
9013  }
9014  }
9015 
9016  {
9017  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9018  vinfos[0].jointtype = 1;
9019  vinfos[0].foffset = j0;
9020  vinfos[0].indices[0] = _ij0[0];
9021  vinfos[0].indices[1] = _ij0[1];
9022  vinfos[0].maxsolutions = _nj0;
9023  vinfos[1].jointtype = 1;
9024  vinfos[1].foffset = j1;
9025  vinfos[1].indices[0] = _ij1[0];
9026  vinfos[1].indices[1] = _ij1[1];
9027  vinfos[1].maxsolutions = _nj1;
9028  vinfos[2].jointtype = 1;
9029  vinfos[2].foffset = j2;
9030  vinfos[2].indices[0] = _ij2[0];
9031  vinfos[2].indices[1] = _ij2[1];
9032  vinfos[2].maxsolutions = _nj2;
9033  vinfos[3].jointtype = 1;
9034  vinfos[3].foffset = j3;
9035  vinfos[3].indices[0] = _ij3[0];
9036  vinfos[3].indices[1] = _ij3[1];
9037  vinfos[3].maxsolutions = _nj3;
9038  vinfos[4].jointtype = 1;
9039  vinfos[4].foffset = j4;
9040  vinfos[4].indices[0] = _ij4[0];
9041  vinfos[4].indices[1] = _ij4[1];
9042  vinfos[4].maxsolutions = _nj4;
9043  vinfos[5].jointtype = 1;
9044  vinfos[5].foffset = j5;
9045  vinfos[5].indices[0] = _ij5[0];
9046  vinfos[5].indices[1] = _ij5[1];
9047  vinfos[5].maxsolutions = _nj5;
9048  vinfos[6].jointtype = 1;
9049  vinfos[6].foffset = j6;
9050  vinfos[6].indices[0] = _ij6[0];
9051  vinfos[6].indices[1] = _ij6[1];
9052  vinfos[6].maxsolutions = _nj6;
9053  std::vector<int> vfree(0);
9054  solutions.AddSolution(vinfos, vfree);
9055  }
9056  }
9057  }
9058  }
9059  } while (0);
9060  if (bgotonextstatement)
9061  {
9062  bool bgotonextstatement = true;
9063  do
9064  {
9065  evalcond[0] = ((-3.14159265358979) +
9066  (IKfmod(((3.14159265358979) +
9067  (IKabs(((1.5707963267949) + j2)))),
9068  6.28318530717959)));
9069  if (IKabs(evalcond[0]) < 0.0000050000000000)
9070  {
9071  bgotonextstatement = false;
9072  {
9073  IkReal j0array[1], cj0array[1], sj0array[1];
9074  bool j0valid[1] = { false };
9075  _nj0 = 1;
9076  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
9077  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
9078  IKabs(IKsqr(((-1.0) * new_r00)) +
9079  IKsqr(((-1.0) * new_r01)) - 1) <=
9080  IKFAST_SINCOS_THRESH)
9081  continue;
9082  j0array[0] =
9083  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
9084  sj0array[0] = IKsin(j0array[0]);
9085  cj0array[0] = IKcos(j0array[0]);
9086  if (j0array[0] > IKPI)
9087  {
9088  j0array[0] -= IK2PI;
9089  }
9090  else if (j0array[0] < -IKPI)
9091  {
9092  j0array[0] += IK2PI;
9093  }
9094  j0valid[0] = true;
9095  for (int ij0 = 0; ij0 < 1; ++ij0)
9096  {
9097  if (!j0valid[ij0])
9098  {
9099  continue;
9100  }
9101  _ij0[0] = ij0;
9102  _ij0[1] = -1;
9103  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9104  {
9105  if (j0valid[iij0] &&
9106  IKabs(cj0array[ij0] - cj0array[iij0]) <
9107  IKFAST_SOLUTION_THRESH &&
9108  IKabs(sj0array[ij0] - sj0array[iij0]) <
9109  IKFAST_SOLUTION_THRESH)
9110  {
9111  j0valid[iij0] = false;
9112  _ij0[1] = iij0;
9113  break;
9114  }
9115  }
9116  j0 = j0array[ij0];
9117  cj0 = cj0array[ij0];
9118  sj0 = sj0array[ij0];
9119  {
9120  IkReal evalcond[8];
9121  IkReal x436 = IKsin(j0);
9122  IkReal x437 = IKcos(j0);
9123  IkReal x438 = ((1.0) * x436);
9124  evalcond[0] = (x436 + new_r00);
9125  evalcond[1] = (x436 + new_r11);
9126  evalcond[2] = (x437 + new_r01);
9127  evalcond[3] = ((((-1.0) * x437)) + new_r10);
9128  evalcond[4] =
9129  (((new_r00 * x437)) + ((new_r10 * x436)));
9130  evalcond[5] = ((((-1.0) * new_r01 * x438)) +
9131  ((new_r11 * x437)));
9132  evalcond[6] =
9133  ((1.0) + ((new_r01 * x437)) + ((new_r11 * x436)));
9134  evalcond[7] = ((-1.0) + (((-1.0) * new_r00 * x438)) +
9135  ((new_r10 * x437)));
9136  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9137  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9138  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9139  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9140  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9141  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9142  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9143  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9144  {
9145  continue;
9146  }
9147  }
9148 
9149  {
9150  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
9151  7);
9152  vinfos[0].jointtype = 1;
9153  vinfos[0].foffset = j0;
9154  vinfos[0].indices[0] = _ij0[0];
9155  vinfos[0].indices[1] = _ij0[1];
9156  vinfos[0].maxsolutions = _nj0;
9157  vinfos[1].jointtype = 1;
9158  vinfos[1].foffset = j1;
9159  vinfos[1].indices[0] = _ij1[0];
9160  vinfos[1].indices[1] = _ij1[1];
9161  vinfos[1].maxsolutions = _nj1;
9162  vinfos[2].jointtype = 1;
9163  vinfos[2].foffset = j2;
9164  vinfos[2].indices[0] = _ij2[0];
9165  vinfos[2].indices[1] = _ij2[1];
9166  vinfos[2].maxsolutions = _nj2;
9167  vinfos[3].jointtype = 1;
9168  vinfos[3].foffset = j3;
9169  vinfos[3].indices[0] = _ij3[0];
9170  vinfos[3].indices[1] = _ij3[1];
9171  vinfos[3].maxsolutions = _nj3;
9172  vinfos[4].jointtype = 1;
9173  vinfos[4].foffset = j4;
9174  vinfos[4].indices[0] = _ij4[0];
9175  vinfos[4].indices[1] = _ij4[1];
9176  vinfos[4].maxsolutions = _nj4;
9177  vinfos[5].jointtype = 1;
9178  vinfos[5].foffset = j5;
9179  vinfos[5].indices[0] = _ij5[0];
9180  vinfos[5].indices[1] = _ij5[1];
9181  vinfos[5].maxsolutions = _nj5;
9182  vinfos[6].jointtype = 1;
9183  vinfos[6].foffset = j6;
9184  vinfos[6].indices[0] = _ij6[0];
9185  vinfos[6].indices[1] = _ij6[1];
9186  vinfos[6].maxsolutions = _nj6;
9187  std::vector<int> vfree(0);
9188  solutions.AddSolution(vinfos, vfree);
9189  }
9190  }
9191  }
9192  }
9193  } while (0);
9194  if (bgotonextstatement)
9195  {
9196  bool bgotonextstatement = true;
9197  do
9198  {
9199  evalcond[0] = ((new_r10 * new_r10) + (new_r00 * new_r00));
9200  if (IKabs(evalcond[0]) < 0.0000050000000000)
9201  {
9202  bgotonextstatement = false;
9203  {
9204  IkReal j0eval[1];
9205  sj1 = 0;
9206  cj1 = 1.0;
9207  j1 = 0;
9208  new_r10 = 0;
9209  new_r00 = 0;
9210  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9211  if (IKabs(j0eval[0]) < 0.0000010000000000)
9212  {
9213  continue; // 3 cases reached
9214  }
9215  else
9216  {
9217  {
9218  IkReal j0array[2], cj0array[2], sj0array[2];
9219  bool j0valid[2] = { false };
9220  _nj0 = 2;
9221  CheckValue<IkReal> x440 =
9222  IKatan2WithCheck(IkReal(new_r01),
9223  IkReal(new_r11),
9224  IKFAST_ATAN2_MAGTHRESH);
9225  if (!x440.valid)
9226  {
9227  continue;
9228  }
9229  IkReal x439 = x440.value;
9230  j0array[0] = ((-1.0) * x439);
9231  sj0array[0] = IKsin(j0array[0]);
9232  cj0array[0] = IKcos(j0array[0]);
9233  j0array[1] =
9234  ((3.14159265358979) + (((-1.0) * x439)));
9235  sj0array[1] = IKsin(j0array[1]);
9236  cj0array[1] = IKcos(j0array[1]);
9237  if (j0array[0] > IKPI)
9238  {
9239  j0array[0] -= IK2PI;
9240  }
9241  else if (j0array[0] < -IKPI)
9242  {
9243  j0array[0] += IK2PI;
9244  }
9245  j0valid[0] = true;
9246  if (j0array[1] > IKPI)
9247  {
9248  j0array[1] -= IK2PI;
9249  }
9250  else if (j0array[1] < -IKPI)
9251  {
9252  j0array[1] += IK2PI;
9253  }
9254  j0valid[1] = true;
9255  for (int ij0 = 0; ij0 < 2; ++ij0)
9256  {
9257  if (!j0valid[ij0])
9258  {
9259  continue;
9260  }
9261  _ij0[0] = ij0;
9262  _ij0[1] = -1;
9263  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9264  {
9265  if (j0valid[iij0] &&
9266  IKabs(cj0array[ij0] - cj0array[iij0]) <
9267  IKFAST_SOLUTION_THRESH &&
9268  IKabs(sj0array[ij0] - sj0array[iij0]) <
9269  IKFAST_SOLUTION_THRESH)
9270  {
9271  j0valid[iij0] = false;
9272  _ij0[1] = iij0;
9273  break;
9274  }
9275  }
9276  j0 = j0array[ij0];
9277  cj0 = cj0array[ij0];
9278  sj0 = sj0array[ij0];
9279  {
9280  IkReal evalcond[1];
9281  evalcond[0] =
9282  (((new_r11 * (IKcos(j0)))) +
9283  (((-1.0) * new_r01 * (IKsin(j0)))));
9284  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
9285  {
9286  continue;
9287  }
9288  }
9289 
9290  {
9291  std::vector<IkSingleDOFSolutionBase<IkReal> >
9292  vinfos(7);
9293  vinfos[0].jointtype = 1;
9294  vinfos[0].foffset = j0;
9295  vinfos[0].indices[0] = _ij0[0];
9296  vinfos[0].indices[1] = _ij0[1];
9297  vinfos[0].maxsolutions = _nj0;
9298  vinfos[1].jointtype = 1;
9299  vinfos[1].foffset = j1;
9300  vinfos[1].indices[0] = _ij1[0];
9301  vinfos[1].indices[1] = _ij1[1];
9302  vinfos[1].maxsolutions = _nj1;
9303  vinfos[2].jointtype = 1;
9304  vinfos[2].foffset = j2;
9305  vinfos[2].indices[0] = _ij2[0];
9306  vinfos[2].indices[1] = _ij2[1];
9307  vinfos[2].maxsolutions = _nj2;
9308  vinfos[3].jointtype = 1;
9309  vinfos[3].foffset = j3;
9310  vinfos[3].indices[0] = _ij3[0];
9311  vinfos[3].indices[1] = _ij3[1];
9312  vinfos[3].maxsolutions = _nj3;
9313  vinfos[4].jointtype = 1;
9314  vinfos[4].foffset = j4;
9315  vinfos[4].indices[0] = _ij4[0];
9316  vinfos[4].indices[1] = _ij4[1];
9317  vinfos[4].maxsolutions = _nj4;
9318  vinfos[5].jointtype = 1;
9319  vinfos[5].foffset = j5;
9320  vinfos[5].indices[0] = _ij5[0];
9321  vinfos[5].indices[1] = _ij5[1];
9322  vinfos[5].maxsolutions = _nj5;
9323  vinfos[6].jointtype = 1;
9324  vinfos[6].foffset = j6;
9325  vinfos[6].indices[0] = _ij6[0];
9326  vinfos[6].indices[1] = _ij6[1];
9327  vinfos[6].maxsolutions = _nj6;
9328  std::vector<int> vfree(0);
9329  solutions.AddSolution(vinfos, vfree);
9330  }
9331  }
9332  }
9333  }
9334  }
9335  }
9336  } while (0);
9337  if (bgotonextstatement)
9338  {
9339  bool bgotonextstatement = true;
9340  do
9341  {
9342  evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
9343  if (IKabs(evalcond[0]) < 0.0000050000000000)
9344  {
9345  bgotonextstatement = false;
9346  {
9347  IkReal j0eval[1];
9348  sj1 = 0;
9349  cj1 = 1.0;
9350  j1 = 0;
9351  new_r00 = 0;
9352  new_r10 = 0;
9353  new_r21 = 0;
9354  new_r22 = 0;
9355  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9356  if (IKabs(j0eval[0]) < 0.0000010000000000)
9357  {
9358  continue; // no branches [j0]
9359  }
9360  else
9361  {
9362  {
9363  IkReal j0array[2], cj0array[2], sj0array[2];
9364  bool j0valid[2] = { false };
9365  _nj0 = 2;
9366  CheckValue<IkReal> x442 =
9367  IKatan2WithCheck(IkReal(new_r01),
9368  IkReal(new_r11),
9369  IKFAST_ATAN2_MAGTHRESH);
9370  if (!x442.valid)
9371  {
9372  continue;
9373  }
9374  IkReal x441 = x442.value;
9375  j0array[0] = ((-1.0) * x441);
9376  sj0array[0] = IKsin(j0array[0]);
9377  cj0array[0] = IKcos(j0array[0]);
9378  j0array[1] =
9379  ((3.14159265358979) + (((-1.0) * x441)));
9380  sj0array[1] = IKsin(j0array[1]);
9381  cj0array[1] = IKcos(j0array[1]);
9382  if (j0array[0] > IKPI)
9383  {
9384  j0array[0] -= IK2PI;
9385  }
9386  else if (j0array[0] < -IKPI)
9387  {
9388  j0array[0] += IK2PI;
9389  }
9390  j0valid[0] = true;
9391  if (j0array[1] > IKPI)
9392  {
9393  j0array[1] -= IK2PI;
9394  }
9395  else if (j0array[1] < -IKPI)
9396  {
9397  j0array[1] += IK2PI;
9398  }
9399  j0valid[1] = true;
9400  for (int ij0 = 0; ij0 < 2; ++ij0)
9401  {
9402  if (!j0valid[ij0])
9403  {
9404  continue;
9405  }
9406  _ij0[0] = ij0;
9407  _ij0[1] = -1;
9408  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9409  {
9410  if (j0valid[iij0] &&
9411  IKabs(cj0array[ij0] - cj0array[iij0]) <
9412  IKFAST_SOLUTION_THRESH &&
9413  IKabs(sj0array[ij0] - sj0array[iij0]) <
9414  IKFAST_SOLUTION_THRESH)
9415  {
9416  j0valid[iij0] = false;
9417  _ij0[1] = iij0;
9418  break;
9419  }
9420  }
9421  j0 = j0array[ij0];
9422  cj0 = cj0array[ij0];
9423  sj0 = sj0array[ij0];
9424  {
9425  IkReal evalcond[1];
9426  evalcond[0] =
9427  (((new_r11 * (IKcos(j0)))) +
9428  (((-1.0) * new_r01 * (IKsin(j0)))));
9429  if (IKabs(evalcond[0]) >
9430  IKFAST_EVALCOND_THRESH)
9431  {
9432  continue;
9433  }
9434  }
9435 
9436  {
9437  std::vector<IkSingleDOFSolutionBase<IkReal> >
9438  vinfos(7);
9439  vinfos[0].jointtype = 1;
9440  vinfos[0].foffset = j0;
9441  vinfos[0].indices[0] = _ij0[0];
9442  vinfos[0].indices[1] = _ij0[1];
9443  vinfos[0].maxsolutions = _nj0;
9444  vinfos[1].jointtype = 1;
9445  vinfos[1].foffset = j1;
9446  vinfos[1].indices[0] = _ij1[0];
9447  vinfos[1].indices[1] = _ij1[1];
9448  vinfos[1].maxsolutions = _nj1;
9449  vinfos[2].jointtype = 1;
9450  vinfos[2].foffset = j2;
9451  vinfos[2].indices[0] = _ij2[0];
9452  vinfos[2].indices[1] = _ij2[1];
9453  vinfos[2].maxsolutions = _nj2;
9454  vinfos[3].jointtype = 1;
9455  vinfos[3].foffset = j3;
9456  vinfos[3].indices[0] = _ij3[0];
9457  vinfos[3].indices[1] = _ij3[1];
9458  vinfos[3].maxsolutions = _nj3;
9459  vinfos[4].jointtype = 1;
9460  vinfos[4].foffset = j4;
9461  vinfos[4].indices[0] = _ij4[0];
9462  vinfos[4].indices[1] = _ij4[1];
9463  vinfos[4].maxsolutions = _nj4;
9464  vinfos[5].jointtype = 1;
9465  vinfos[5].foffset = j5;
9466  vinfos[5].indices[0] = _ij5[0];
9467  vinfos[5].indices[1] = _ij5[1];
9468  vinfos[5].maxsolutions = _nj5;
9469  vinfos[6].jointtype = 1;
9470  vinfos[6].foffset = j6;
9471  vinfos[6].indices[0] = _ij6[0];
9472  vinfos[6].indices[1] = _ij6[1];
9473  vinfos[6].maxsolutions = _nj6;
9474  std::vector<int> vfree(0);
9475  solutions.AddSolution(vinfos, vfree);
9476  }
9477  }
9478  }
9479  }
9480  }
9481  }
9482  } while (0);
9483  if (bgotonextstatement)
9484  {
9485  bool bgotonextstatement = true;
9486  do
9487  {
9488  if (1)
9489  {
9490  bgotonextstatement = false;
9491  continue; // branch miss [j0]
9492  }
9493  } while (0);
9494  if (bgotonextstatement)
9495  {
9496  }
9497  }
9498  }
9499  }
9500  }
9501  }
9502  }
9503  }
9504  }
9505  else
9506  {
9507  {
9508  IkReal j0array[1], cj0array[1], sj0array[1];
9509  bool j0valid[1] = { false };
9510  _nj0 = 1;
9511  IkReal x443 = ((1.0) * new_r10);
9512  CheckValue<IkReal> x444 = IKatan2WithCheck(
9513  IkReal((((cj2 * sj2)) + (((-1.0) * new_r00 * x443)))),
9514  IkReal(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10))),
9515  IKFAST_ATAN2_MAGTHRESH);
9516  if (!x444.valid)
9517  {
9518  continue;
9519  }
9520  CheckValue<IkReal> x445 = IKPowWithIntegerCheck(
9521  IKsign(((((-1.0) * sj2 * x443)) + ((cj2 * new_r00)))), -1);
9522  if (!x445.valid)
9523  {
9524  continue;
9525  }
9526  j0array[0] = ((-1.5707963267949) + (x444.value) +
9527  (((1.5707963267949) * (x445.value))));
9528  sj0array[0] = IKsin(j0array[0]);
9529  cj0array[0] = IKcos(j0array[0]);
9530  if (j0array[0] > IKPI)
9531  {
9532  j0array[0] -= IK2PI;
9533  }
9534  else if (j0array[0] < -IKPI)
9535  {
9536  j0array[0] += IK2PI;
9537  }
9538  j0valid[0] = true;
9539  for (int ij0 = 0; ij0 < 1; ++ij0)
9540  {
9541  if (!j0valid[ij0])
9542  {
9543  continue;
9544  }
9545  _ij0[0] = ij0;
9546  _ij0[1] = -1;
9547  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9548  {
9549  if (j0valid[iij0] &&
9550  IKabs(cj0array[ij0] - cj0array[iij0]) <
9551  IKFAST_SOLUTION_THRESH &&
9552  IKabs(sj0array[ij0] - sj0array[iij0]) <
9553  IKFAST_SOLUTION_THRESH)
9554  {
9555  j0valid[iij0] = false;
9556  _ij0[1] = iij0;
9557  break;
9558  }
9559  }
9560  j0 = j0array[ij0];
9561  cj0 = cj0array[ij0];
9562  sj0 = sj0array[ij0];
9563  {
9564  IkReal evalcond[8];
9565  IkReal x446 = IKcos(j0);
9566  IkReal x447 = IKsin(j0);
9567  IkReal x448 = ((1.0) * sj2);
9568  IkReal x449 = (cj2 * x446);
9569  IkReal x450 = ((1.0) * x447);
9570  IkReal x451 = (x447 * x448);
9571  evalcond[0] = (((new_r00 * x446)) + cj2 + ((new_r10 * x447)));
9572  evalcond[1] = (((cj2 * x447)) + ((sj2 * x446)) + new_r10);
9573  evalcond[2] =
9574  (sj2 + (((-1.0) * new_r00 * x450)) + ((new_r10 * x446)));
9575  evalcond[3] =
9576  (cj2 + (((-1.0) * new_r01 * x450)) + ((new_r11 * x446)));
9577  evalcond[4] = ((((-1.0) * x451)) + x449 + new_r00);
9578  evalcond[5] = ((((-1.0) * x451)) + x449 + new_r11);
9579  evalcond[6] =
9580  ((((-1.0) * x448)) + ((new_r01 * x446)) + ((new_r11 * x447)));
9581  evalcond[7] = (new_r01 + (((-1.0) * cj2 * x450)) +
9582  (((-1.0) * x446 * x448)));
9583  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9584  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9585  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9586  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9587  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9588  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9589  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9590  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9591  {
9592  continue;
9593  }
9594  }
9595 
9596  {
9597  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9598  vinfos[0].jointtype = 1;
9599  vinfos[0].foffset = j0;
9600  vinfos[0].indices[0] = _ij0[0];
9601  vinfos[0].indices[1] = _ij0[1];
9602  vinfos[0].maxsolutions = _nj0;
9603  vinfos[1].jointtype = 1;
9604  vinfos[1].foffset = j1;
9605  vinfos[1].indices[0] = _ij1[0];
9606  vinfos[1].indices[1] = _ij1[1];
9607  vinfos[1].maxsolutions = _nj1;
9608  vinfos[2].jointtype = 1;
9609  vinfos[2].foffset = j2;
9610  vinfos[2].indices[0] = _ij2[0];
9611  vinfos[2].indices[1] = _ij2[1];
9612  vinfos[2].maxsolutions = _nj2;
9613  vinfos[3].jointtype = 1;
9614  vinfos[3].foffset = j3;
9615  vinfos[3].indices[0] = _ij3[0];
9616  vinfos[3].indices[1] = _ij3[1];
9617  vinfos[3].maxsolutions = _nj3;
9618  vinfos[4].jointtype = 1;
9619  vinfos[4].foffset = j4;
9620  vinfos[4].indices[0] = _ij4[0];
9621  vinfos[4].indices[1] = _ij4[1];
9622  vinfos[4].maxsolutions = _nj4;
9623  vinfos[5].jointtype = 1;
9624  vinfos[5].foffset = j5;
9625  vinfos[5].indices[0] = _ij5[0];
9626  vinfos[5].indices[1] = _ij5[1];
9627  vinfos[5].maxsolutions = _nj5;
9628  vinfos[6].jointtype = 1;
9629  vinfos[6].foffset = j6;
9630  vinfos[6].indices[0] = _ij6[0];
9631  vinfos[6].indices[1] = _ij6[1];
9632  vinfos[6].maxsolutions = _nj6;
9633  std::vector<int> vfree(0);
9634  solutions.AddSolution(vinfos, vfree);
9635  }
9636  }
9637  }
9638  }
9639  }
9640  }
9641  else
9642  {
9643  {
9644  IkReal j0array[1], cj0array[1], sj0array[1];
9645  bool j0valid[1] = { false };
9646  _nj0 = 1;
9647  IkReal x452 = ((1.0) * cj2);
9648  CheckValue<IkReal> x453 = IKatan2WithCheck(
9649  IkReal(((((-1.0) * new_r11 * x452)) + ((cj2 * new_r00)))),
9650  IkReal(((((-1.0) * new_r10 * x452)) + (((-1.0) * new_r01 * x452)))),
9651  IKFAST_ATAN2_MAGTHRESH);
9652  if (!x453.valid)
9653  {
9654  continue;
9655  }
9656  CheckValue<IkReal> x454 = IKPowWithIntegerCheck(
9657  IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
9658  if (!x454.valid)
9659  {
9660  continue;
9661  }
9662  j0array[0] = ((-1.5707963267949) + (x453.value) +
9663  (((1.5707963267949) * (x454.value))));
9664  sj0array[0] = IKsin(j0array[0]);
9665  cj0array[0] = IKcos(j0array[0]);
9666  if (j0array[0] > IKPI)
9667  {
9668  j0array[0] -= IK2PI;
9669  }
9670  else if (j0array[0] < -IKPI)
9671  {
9672  j0array[0] += IK2PI;
9673  }
9674  j0valid[0] = true;
9675  for (int ij0 = 0; ij0 < 1; ++ij0)
9676  {
9677  if (!j0valid[ij0])
9678  {
9679  continue;
9680  }
9681  _ij0[0] = ij0;
9682  _ij0[1] = -1;
9683  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9684  {
9685  if (j0valid[iij0] &&
9686  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9687  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9688  {
9689  j0valid[iij0] = false;
9690  _ij0[1] = iij0;
9691  break;
9692  }
9693  }
9694  j0 = j0array[ij0];
9695  cj0 = cj0array[ij0];
9696  sj0 = sj0array[ij0];
9697  {
9698  IkReal evalcond[8];
9699  IkReal x455 = IKcos(j0);
9700  IkReal x456 = IKsin(j0);
9701  IkReal x457 = ((1.0) * sj2);
9702  IkReal x458 = (cj2 * x455);
9703  IkReal x459 = ((1.0) * x456);
9704  IkReal x460 = (x456 * x457);
9705  evalcond[0] = (((new_r10 * x456)) + cj2 + ((new_r00 * x455)));
9706  evalcond[1] = (((sj2 * x455)) + ((cj2 * x456)) + new_r10);
9707  evalcond[2] =
9708  (((new_r10 * x455)) + sj2 + (((-1.0) * new_r00 * x459)));
9709  evalcond[3] =
9710  (((new_r11 * x455)) + cj2 + (((-1.0) * new_r01 * x459)));
9711  evalcond[4] = ((((-1.0) * x460)) + x458 + new_r00);
9712  evalcond[5] = ((((-1.0) * x460)) + x458 + new_r11);
9713  evalcond[6] =
9714  (((new_r11 * x456)) + ((new_r01 * x455)) + (((-1.0) * x457)));
9715  evalcond[7] =
9716  ((((-1.0) * x455 * x457)) + new_r01 + (((-1.0) * cj2 * x459)));
9717  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9718  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9719  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9720  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9721  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9722  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9723  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9724  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9725  {
9726  continue;
9727  }
9728  }
9729 
9730  {
9731  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9732  vinfos[0].jointtype = 1;
9733  vinfos[0].foffset = j0;
9734  vinfos[0].indices[0] = _ij0[0];
9735  vinfos[0].indices[1] = _ij0[1];
9736  vinfos[0].maxsolutions = _nj0;
9737  vinfos[1].jointtype = 1;
9738  vinfos[1].foffset = j1;
9739  vinfos[1].indices[0] = _ij1[0];
9740  vinfos[1].indices[1] = _ij1[1];
9741  vinfos[1].maxsolutions = _nj1;
9742  vinfos[2].jointtype = 1;
9743  vinfos[2].foffset = j2;
9744  vinfos[2].indices[0] = _ij2[0];
9745  vinfos[2].indices[1] = _ij2[1];
9746  vinfos[2].maxsolutions = _nj2;
9747  vinfos[3].jointtype = 1;
9748  vinfos[3].foffset = j3;
9749  vinfos[3].indices[0] = _ij3[0];
9750  vinfos[3].indices[1] = _ij3[1];
9751  vinfos[3].maxsolutions = _nj3;
9752  vinfos[4].jointtype = 1;
9753  vinfos[4].foffset = j4;
9754  vinfos[4].indices[0] = _ij4[0];
9755  vinfos[4].indices[1] = _ij4[1];
9756  vinfos[4].maxsolutions = _nj4;
9757  vinfos[5].jointtype = 1;
9758  vinfos[5].foffset = j5;
9759  vinfos[5].indices[0] = _ij5[0];
9760  vinfos[5].indices[1] = _ij5[1];
9761  vinfos[5].maxsolutions = _nj5;
9762  vinfos[6].jointtype = 1;
9763  vinfos[6].foffset = j6;
9764  vinfos[6].indices[0] = _ij6[0];
9765  vinfos[6].indices[1] = _ij6[1];
9766  vinfos[6].maxsolutions = _nj6;
9767  std::vector<int> vfree(0);
9768  solutions.AddSolution(vinfos, vfree);
9769  }
9770  }
9771  }
9772  }
9773  }
9774  }
9775  else
9776  {
9777  {
9778  IkReal j0array[1], cj0array[1], sj0array[1];
9779  bool j0valid[1] = { false };
9780  _nj0 = 1;
9781  IkReal x461 = ((1.0) * cj2);
9782  CheckValue<IkReal> x462 = IKPowWithIntegerCheck(
9783  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))), -1);
9784  if (!x462.valid)
9785  {
9786  continue;
9787  }
9788  CheckValue<IkReal> x463 = IKatan2WithCheck(
9789  IkReal(((((-1.0) * new_r10 * x461)) + ((new_r00 * sj2)))),
9790  IkReal(((((-1.0) * new_r00 * x461)) + (((-1.0) * new_r10 * sj2)))),
9791  IKFAST_ATAN2_MAGTHRESH);
9792  if (!x463.valid)
9793  {
9794  continue;
9795  }
9796  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x462.value))) +
9797  (x463.value));
9798  sj0array[0] = IKsin(j0array[0]);
9799  cj0array[0] = IKcos(j0array[0]);
9800  if (j0array[0] > IKPI)
9801  {
9802  j0array[0] -= IK2PI;
9803  }
9804  else if (j0array[0] < -IKPI)
9805  {
9806  j0array[0] += IK2PI;
9807  }
9808  j0valid[0] = true;
9809  for (int ij0 = 0; ij0 < 1; ++ij0)
9810  {
9811  if (!j0valid[ij0])
9812  {
9813  continue;
9814  }
9815  _ij0[0] = ij0;
9816  _ij0[1] = -1;
9817  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9818  {
9819  if (j0valid[iij0] &&
9820  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9821  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9822  {
9823  j0valid[iij0] = false;
9824  _ij0[1] = iij0;
9825  break;
9826  }
9827  }
9828  j0 = j0array[ij0];
9829  cj0 = cj0array[ij0];
9830  sj0 = sj0array[ij0];
9831  {
9832  IkReal evalcond[8];
9833  IkReal x464 = IKcos(j0);
9834  IkReal x465 = IKsin(j0);
9835  IkReal x466 = ((1.0) * sj2);
9836  IkReal x467 = (cj2 * x464);
9837  IkReal x468 = ((1.0) * x465);
9838  IkReal x469 = (x465 * x466);
9839  evalcond[0] = (((new_r10 * x465)) + cj2 + ((new_r00 * x464)));
9840  evalcond[1] = (((cj2 * x465)) + new_r10 + ((sj2 * x464)));
9841  evalcond[2] = (sj2 + ((new_r10 * x464)) + (((-1.0) * new_r00 * x468)));
9842  evalcond[3] = (((new_r11 * x464)) + cj2 + (((-1.0) * new_r01 * x468)));
9843  evalcond[4] = ((((-1.0) * x469)) + x467 + new_r00);
9844  evalcond[5] = ((((-1.0) * x469)) + x467 + new_r11);
9845  evalcond[6] =
9846  (((new_r11 * x465)) + ((new_r01 * x464)) + (((-1.0) * x466)));
9847  evalcond[7] =
9848  ((((-1.0) * x464 * x466)) + new_r01 + (((-1.0) * cj2 * x468)));
9849  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9850  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9851  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9852  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9853  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9854  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9855  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9856  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9857  {
9858  continue;
9859  }
9860  }
9861 
9862  {
9863  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9864  vinfos[0].jointtype = 1;
9865  vinfos[0].foffset = j0;
9866  vinfos[0].indices[0] = _ij0[0];
9867  vinfos[0].indices[1] = _ij0[1];
9868  vinfos[0].maxsolutions = _nj0;
9869  vinfos[1].jointtype = 1;
9870  vinfos[1].foffset = j1;
9871  vinfos[1].indices[0] = _ij1[0];
9872  vinfos[1].indices[1] = _ij1[1];
9873  vinfos[1].maxsolutions = _nj1;
9874  vinfos[2].jointtype = 1;
9875  vinfos[2].foffset = j2;
9876  vinfos[2].indices[0] = _ij2[0];
9877  vinfos[2].indices[1] = _ij2[1];
9878  vinfos[2].maxsolutions = _nj2;
9879  vinfos[3].jointtype = 1;
9880  vinfos[3].foffset = j3;
9881  vinfos[3].indices[0] = _ij3[0];
9882  vinfos[3].indices[1] = _ij3[1];
9883  vinfos[3].maxsolutions = _nj3;
9884  vinfos[4].jointtype = 1;
9885  vinfos[4].foffset = j4;
9886  vinfos[4].indices[0] = _ij4[0];
9887  vinfos[4].indices[1] = _ij4[1];
9888  vinfos[4].maxsolutions = _nj4;
9889  vinfos[5].jointtype = 1;
9890  vinfos[5].foffset = j5;
9891  vinfos[5].indices[0] = _ij5[0];
9892  vinfos[5].indices[1] = _ij5[1];
9893  vinfos[5].maxsolutions = _nj5;
9894  vinfos[6].jointtype = 1;
9895  vinfos[6].foffset = j6;
9896  vinfos[6].indices[0] = _ij6[0];
9897  vinfos[6].indices[1] = _ij6[1];
9898  vinfos[6].maxsolutions = _nj6;
9899  std::vector<int> vfree(0);
9900  solutions.AddSolution(vinfos, vfree);
9901  }
9902  }
9903  }
9904  }
9905  }
9906  }
9907  } while (0);
9908  if (bgotonextstatement)
9909  {
9910  bool bgotonextstatement = true;
9911  do
9912  {
9913  evalcond[0] = ((-3.14159265358979) +
9914  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
9915  6.28318530717959)));
9916  evalcond[1] = new_r21;
9917  evalcond[2] = new_r02;
9918  evalcond[3] = new_r12;
9919  evalcond[4] = new_r20;
9920  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
9921  IKabs(evalcond[1]) < 0.0000050000000000 &&
9922  IKabs(evalcond[2]) < 0.0000050000000000 &&
9923  IKabs(evalcond[3]) < 0.0000050000000000 &&
9924  IKabs(evalcond[4]) < 0.0000050000000000)
9925  {
9926  bgotonextstatement = false;
9927  {
9928  IkReal j0eval[3];
9929  sj1 = 0;
9930  cj1 = -1.0;
9931  j1 = 3.14159265358979;
9932  IkReal x470 = ((1.0) * sj2);
9933  IkReal x471 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
9934  j0eval[0] = x471;
9935  j0eval[1] =
9936  ((IKabs(((((-1.0) * new_r00 * x470)) + (((-1.0) * new_r11 * x470))))) +
9937  (IKabs((((new_r01 * sj2)) + (((-1.0) * new_r10 * x470))))));
9938  j0eval[2] = IKsign(x471);
9939  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9940  IKabs(j0eval[1]) < 0.0000010000000000 ||
9941  IKabs(j0eval[2]) < 0.0000010000000000)
9942  {
9943  {
9944  IkReal j0eval[3];
9945  sj1 = 0;
9946  cj1 = -1.0;
9947  j1 = 3.14159265358979;
9948  IkReal x472 = ((1.0) * new_r11);
9949  IkReal x473 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9950  j0eval[0] = x473;
9951  j0eval[1] =
9952  ((IKabs(((((-1.0) * sj2 * x472)) + ((cj2 * new_r01))))) +
9953  (IKabs(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x472))))));
9954  j0eval[2] = IKsign(x473);
9955  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9956  IKabs(j0eval[1]) < 0.0000010000000000 ||
9957  IKabs(j0eval[2]) < 0.0000010000000000)
9958  {
9959  {
9960  IkReal j0eval[3];
9961  sj1 = 0;
9962  cj1 = -1.0;
9963  j1 = 3.14159265358979;
9964  IkReal x474 = (((new_r11 * sj2)) + ((cj2 * new_r01)));
9965  j0eval[0] = x474;
9966  j0eval[1] = IKsign(x474);
9967  j0eval[2] = ((IKabs(((((-1.0) * cj2 * sj2)) +
9968  (((-1.0) * new_r10 * new_r11))))) +
9969  (IKabs(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10))))));
9970  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9971  IKabs(j0eval[1]) < 0.0000010000000000 ||
9972  IKabs(j0eval[2]) < 0.0000010000000000)
9973  {
9974  {
9975  IkReal evalcond[1];
9976  bool bgotonextstatement = true;
9977  do
9978  {
9979  IkReal x476 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9980  if (IKabs(x476) == 0)
9981  {
9982  continue;
9983  }
9984  IkReal x475 = pow(x476, -0.5);
9985  CheckValue<IkReal> x477 = IKatan2WithCheck(
9986  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9987  if (!x477.valid)
9988  {
9989  continue;
9990  }
9991  IkReal gconst6 = ((-1.0) * (x477.value));
9992  IkReal gconst7 = ((-1.0) * new_r01 * x475);
9993  IkReal gconst8 = (new_r11 * x475);
9994  CheckValue<IkReal> x478 = IKatan2WithCheck(
9995  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9996  if (!x478.valid)
9997  {
9998  continue;
9999  }
10000  evalcond[0] =
10001  ((-3.14159265358979) +
10002  (IKfmod(((3.14159265358979) + (IKabs(((x478.value) + j2)))),
10003  6.28318530717959)));
10004  if (IKabs(evalcond[0]) < 0.0000050000000000)
10005  {
10006  bgotonextstatement = false;
10007  {
10008  IkReal j0eval[3];
10009  CheckValue<IkReal> x481 = IKatan2WithCheck(
10010  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
10011  if (!x481.valid)
10012  {
10013  continue;
10014  }
10015  IkReal x479 = ((-1.0) * (x481.value));
10016  IkReal x480 = x475;
10017  sj1 = 0;
10018  cj1 = -1.0;
10019  j1 = 3.14159265358979;
10020  sj2 = gconst7;
10021  cj2 = gconst8;
10022  j2 = x479;
10023  IkReal gconst6 = x479;
10024  IkReal gconst7 = ((-1.0) * new_r01 * x480);
10025  IkReal gconst8 = (new_r11 * x480);
10026  IkReal x482 = new_r01 * new_r01;
10027  IkReal x483 = (new_r00 * new_r01);
10028  IkReal x484 = (((new_r10 * new_r11)) + x483);
10029  IkReal x485 = x475;
10030  IkReal x486 = (new_r01 * x485);
10031  j0eval[0] = x484;
10032  j0eval[1] = IKsign(x484);
10033  j0eval[2] = ((IKabs((((new_r10 * x486)) +
10034  (((-1.0) * x482 * x485))))) +
10035  (IKabs((((x483 * x485)) + ((new_r11 * x486))))));
10036  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10037  IKabs(j0eval[1]) < 0.0000010000000000 ||
10038  IKabs(j0eval[2]) < 0.0000010000000000)
10039  {
10040  {
10041  IkReal j0eval[2];
10042  CheckValue<IkReal> x489 =
10043  IKatan2WithCheck(IkReal(new_r01),
10044  IkReal(new_r11),
10045  IKFAST_ATAN2_MAGTHRESH);
10046  if (!x489.valid)
10047  {
10048  continue;
10049  }
10050  IkReal x487 = ((-1.0) * (x489.value));
10051  IkReal x488 = x475;
10052  sj1 = 0;
10053  cj1 = -1.0;
10054  j1 = 3.14159265358979;
10055  sj2 = gconst7;
10056  cj2 = gconst8;
10057  j2 = x487;
10058  IkReal gconst6 = x487;
10059  IkReal gconst7 = ((-1.0) * new_r01 * x488);
10060  IkReal gconst8 = (new_r11 * x488);
10061  IkReal x490 = ((new_r01 * new_r01) + (new_r11 * new_r11));
10062  j0eval[0] = x490;
10063  j0eval[1] = IKsign(x490);
10064  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10065  IKabs(j0eval[1]) < 0.0000010000000000)
10066  {
10067  {
10068  IkReal j0eval[1];
10069  CheckValue<IkReal> x493 =
10070  IKatan2WithCheck(IkReal(new_r01),
10071  IkReal(new_r11),
10072  IKFAST_ATAN2_MAGTHRESH);
10073  if (!x493.valid)
10074  {
10075  continue;
10076  }
10077  IkReal x491 = ((-1.0) * (x493.value));
10078  IkReal x492 = x475;
10079  sj1 = 0;
10080  cj1 = -1.0;
10081  j1 = 3.14159265358979;
10082  sj2 = gconst7;
10083  cj2 = gconst8;
10084  j2 = x491;
10085  IkReal gconst6 = x491;
10086  IkReal gconst7 = ((-1.0) * new_r01 * x492);
10087  IkReal gconst8 = (new_r11 * x492);
10088  IkReal x494 = new_r01 * new_r01;
10089  IkReal x495 = new_r11 * new_r11;
10090  IkReal x496 = ((1.0) * x494);
10091  CheckValue<IkReal> x502 =
10092  IKPowWithIntegerCheck((x494 + x495), -1);
10093  if (!x502.valid)
10094  {
10095  continue;
10096  }
10097  IkReal x497 = x502.value;
10098  CheckValue<IkReal> x503 = IKPowWithIntegerCheck(
10099  ((((-1.0) * x496)) + (((-1.0) * x495))), -1);
10100  if (!x503.valid)
10101  {
10102  continue;
10103  }
10104  IkReal x498 = x503.value;
10105  IkReal x499 = ((1.0) * x498);
10106  IkReal x500 = (new_r11 * x499);
10107  IkReal x501 = (new_r01 * x499);
10108  j0eval[0] =
10109  ((IKabs((((x494 * x495 * x497)) +
10110  (((-1.0) * x496 * x497)) +
10111  ((x497 * (x495 * x495)))))) +
10112  (IKabs(((((-1.0) * new_r01 * x500)) +
10113  (((-1.0) * x500 *
10114  (new_r01 * new_r01 * new_r01))) +
10115  (((-1.0) * new_r01 * x500 *
10116  (new_r11 * new_r11)))))));
10117  if (IKabs(j0eval[0]) < 0.0000010000000000)
10118  {
10119  {
10120  IkReal evalcond[3];
10121  bool bgotonextstatement = true;
10122  do
10123  {
10124  evalcond[0] =
10125  ((IKabs(new_r11)) + (IKabs(new_r00)));
10126  if (IKabs(evalcond[0]) < 0.0000050000000000)
10127  {
10128  bgotonextstatement = false;
10129  {
10130  IkReal j0eval[1];
10131  CheckValue<IkReal> x505 = IKatan2WithCheck(
10132  IkReal(new_r01),
10133  IkReal(0),
10134  IKFAST_ATAN2_MAGTHRESH);
10135  if (!x505.valid)
10136  {
10137  continue;
10138  }
10139  IkReal x504 = ((-1.0) * (x505.value));
10140  sj1 = 0;
10141  cj1 = -1.0;
10142  j1 = 3.14159265358979;
10143  sj2 = gconst7;
10144  cj2 = gconst8;
10145  j2 = x504;
10146  new_r11 = 0;
10147  new_r00 = 0;
10148  IkReal gconst6 = x504;
10149  IkReal x506 = new_r01 * new_r01;
10150  if (IKabs(x506) == 0)
10151  {
10152  continue;
10153  }
10154  IkReal gconst7 =
10155  ((-1.0) * new_r01 * (pow(x506, -0.5)));
10156  IkReal gconst8 = 0;
10157  j0eval[0] = new_r10;
10158  if (IKabs(j0eval[0]) < 0.0000010000000000)
10159  {
10160  {
10161  IkReal j0array[2], cj0array[2],
10162  sj0array[2];
10163  bool j0valid[2] = { false };
10164  _nj0 = 2;
10165  CheckValue<IkReal> x507 =
10166  IKPowWithIntegerCheck(gconst7, -1);
10167  if (!x507.valid)
10168  {
10169  continue;
10170  }
10171  cj0array[0] =
10172  ((-1.0) * new_r10 * (x507.value));
10173  if (cj0array[0] >=
10174  -1 - IKFAST_SINCOS_THRESH &&
10175  cj0array[0] <=
10176  1 + IKFAST_SINCOS_THRESH)
10177  {
10178  j0valid[0] = j0valid[1] = true;
10179  j0array[0] = IKacos(cj0array[0]);
10180  sj0array[0] = IKsin(j0array[0]);
10181  cj0array[1] = cj0array[0];
10182  j0array[1] = -j0array[0];
10183  sj0array[1] = -sj0array[0];
10184  }
10185  else if (isnan(cj0array[0]))
10186  {
10187  // probably any value will work
10188  j0valid[0] = true;
10189  cj0array[0] = 1;
10190  sj0array[0] = 0;
10191  j0array[0] = 0;
10192  }
10193  for (int ij0 = 0; ij0 < 2; ++ij0)
10194  {
10195  if (!j0valid[ij0])
10196  {
10197  continue;
10198  }
10199  _ij0[0] = ij0;
10200  _ij0[1] = -1;
10201  for (int iij0 = ij0 + 1; iij0 < 2;
10202  ++iij0)
10203  {
10204  if (j0valid[iij0] &&
10205  IKabs(cj0array[ij0] -
10206  cj0array[iij0]) <
10207  IKFAST_SOLUTION_THRESH &&
10208  IKabs(sj0array[ij0] -
10209  sj0array[iij0]) <
10210  IKFAST_SOLUTION_THRESH)
10211  {
10212  j0valid[iij0] = false;
10213  _ij0[1] = iij0;
10214  break;
10215  }
10216  }
10217  j0 = j0array[ij0];
10218  cj0 = cj0array[ij0];
10219  sj0 = sj0array[ij0];
10220  {
10221  IkReal evalcond[6];
10222  IkReal x508 = IKsin(j0);
10223  IkReal x509 = IKcos(j0);
10224  IkReal x510 = ((-1.0) * x508);
10225  evalcond[0] = (new_r10 * x508);
10226  evalcond[1] = (new_r01 * x510);
10227  evalcond[2] = (gconst7 * x510);
10228  evalcond[3] =
10229  (gconst7 + ((new_r10 * x509)));
10230  evalcond[4] =
10231  (gconst7 + ((new_r01 * x509)));
10232  evalcond[5] =
10233  (((gconst7 * x509)) + new_r01);
10234  if (IKabs(evalcond[0]) >
10235  IKFAST_EVALCOND_THRESH ||
10236  IKabs(evalcond[1]) >
10237  IKFAST_EVALCOND_THRESH ||
10238  IKabs(evalcond[2]) >
10239  IKFAST_EVALCOND_THRESH ||
10240  IKabs(evalcond[3]) >
10241  IKFAST_EVALCOND_THRESH ||
10242  IKabs(evalcond[4]) >
10243  IKFAST_EVALCOND_THRESH ||
10244  IKabs(evalcond[5]) >
10245  IKFAST_EVALCOND_THRESH)
10246  {
10247  continue;
10248  }
10249  }
10250 
10251  {
10252  std::vector<IkSingleDOFSolutionBase<
10253  IkReal> >
10254  vinfos(7);
10255  vinfos[0].jointtype = 1;
10256  vinfos[0].foffset = j0;
10257  vinfos[0].indices[0] = _ij0[0];
10258  vinfos[0].indices[1] = _ij0[1];
10259  vinfos[0].maxsolutions = _nj0;
10260  vinfos[1].jointtype = 1;
10261  vinfos[1].foffset = j1;
10262  vinfos[1].indices[0] = _ij1[0];
10263  vinfos[1].indices[1] = _ij1[1];
10264  vinfos[1].maxsolutions = _nj1;
10265  vinfos[2].jointtype = 1;
10266  vinfos[2].foffset = j2;
10267  vinfos[2].indices[0] = _ij2[0];
10268  vinfos[2].indices[1] = _ij2[1];
10269  vinfos[2].maxsolutions = _nj2;
10270  vinfos[3].jointtype = 1;
10271  vinfos[3].foffset = j3;
10272  vinfos[3].indices[0] = _ij3[0];
10273  vinfos[3].indices[1] = _ij3[1];
10274  vinfos[3].maxsolutions = _nj3;
10275  vinfos[4].jointtype = 1;
10276  vinfos[4].foffset = j4;
10277  vinfos[4].indices[0] = _ij4[0];
10278  vinfos[4].indices[1] = _ij4[1];
10279  vinfos[4].maxsolutions = _nj4;
10280  vinfos[5].jointtype = 1;
10281  vinfos[5].foffset = j5;
10282  vinfos[5].indices[0] = _ij5[0];
10283  vinfos[5].indices[1] = _ij5[1];
10284  vinfos[5].maxsolutions = _nj5;
10285  vinfos[6].jointtype = 1;
10286  vinfos[6].foffset = j6;
10287  vinfos[6].indices[0] = _ij6[0];
10288  vinfos[6].indices[1] = _ij6[1];
10289  vinfos[6].maxsolutions = _nj6;
10290  std::vector<int> vfree(0);
10291  solutions.AddSolution(vinfos,
10292  vfree);
10293  }
10294  }
10295  }
10296  }
10297  else
10298  {
10299  {
10300  IkReal j0array[2], cj0array[2],
10301  sj0array[2];
10302  bool j0valid[2] = { false };
10303  _nj0 = 2;
10304  CheckValue<IkReal> x511 =
10305  IKPowWithIntegerCheck(new_r10, -1);
10306  if (!x511.valid)
10307  {
10308  continue;
10309  }
10310  cj0array[0] =
10311  ((-1.0) * gconst7 * (x511.value));
10312  if (cj0array[0] >=
10313  -1 - IKFAST_SINCOS_THRESH &&
10314  cj0array[0] <=
10315  1 + IKFAST_SINCOS_THRESH)
10316  {
10317  j0valid[0] = j0valid[1] = true;
10318  j0array[0] = IKacos(cj0array[0]);
10319  sj0array[0] = IKsin(j0array[0]);
10320  cj0array[1] = cj0array[0];
10321  j0array[1] = -j0array[0];
10322  sj0array[1] = -sj0array[0];
10323  }
10324  else if (isnan(cj0array[0]))
10325  {
10326  // probably any value will work
10327  j0valid[0] = true;
10328  cj0array[0] = 1;
10329  sj0array[0] = 0;
10330  j0array[0] = 0;
10331  }
10332  for (int ij0 = 0; ij0 < 2; ++ij0)
10333  {
10334  if (!j0valid[ij0])
10335  {
10336  continue;
10337  }
10338  _ij0[0] = ij0;
10339  _ij0[1] = -1;
10340  for (int iij0 = ij0 + 1; iij0 < 2;
10341  ++iij0)
10342  {
10343  if (j0valid[iij0] &&
10344  IKabs(cj0array[ij0] -
10345  cj0array[iij0]) <
10346  IKFAST_SOLUTION_THRESH &&
10347  IKabs(sj0array[ij0] -
10348  sj0array[iij0]) <
10349  IKFAST_SOLUTION_THRESH)
10350  {
10351  j0valid[iij0] = false;
10352  _ij0[1] = iij0;
10353  break;
10354  }
10355  }
10356  j0 = j0array[ij0];
10357  cj0 = cj0array[ij0];
10358  sj0 = sj0array[ij0];
10359  {
10360  IkReal evalcond[6];
10361  IkReal x512 = IKsin(j0);
10362  IkReal x513 = IKcos(j0);
10363  IkReal x514 = (gconst7 * x513);
10364  IkReal x515 = ((-1.0) * x512);
10365  evalcond[0] = (new_r10 * x512);
10366  evalcond[1] = (new_r01 * x515);
10367  evalcond[2] = (gconst7 * x515);
10368  evalcond[3] = (x514 + new_r10);
10369  evalcond[4] =
10370  (((new_r01 * x513)) + gconst7);
10371  evalcond[5] = (x514 + new_r01);
10372  if (IKabs(evalcond[0]) >
10373  IKFAST_EVALCOND_THRESH ||
10374  IKabs(evalcond[1]) >
10375  IKFAST_EVALCOND_THRESH ||
10376  IKabs(evalcond[2]) >
10377  IKFAST_EVALCOND_THRESH ||
10378  IKabs(evalcond[3]) >
10379  IKFAST_EVALCOND_THRESH ||
10380  IKabs(evalcond[4]) >
10381  IKFAST_EVALCOND_THRESH ||
10382  IKabs(evalcond[5]) >
10383  IKFAST_EVALCOND_THRESH)
10384  {
10385  continue;
10386  }
10387  }
10388 
10389  {
10390  std::vector<IkSingleDOFSolutionBase<
10391  IkReal> >
10392  vinfos(7);
10393  vinfos[0].jointtype = 1;
10394  vinfos[0].foffset = j0;
10395  vinfos[0].indices[0] = _ij0[0];
10396  vinfos[0].indices[1] = _ij0[1];
10397  vinfos[0].maxsolutions = _nj0;
10398  vinfos[1].jointtype = 1;
10399  vinfos[1].foffset = j1;
10400  vinfos[1].indices[0] = _ij1[0];
10401  vinfos[1].indices[1] = _ij1[1];
10402  vinfos[1].maxsolutions = _nj1;
10403  vinfos[2].jointtype = 1;
10404  vinfos[2].foffset = j2;
10405  vinfos[2].indices[0] = _ij2[0];
10406  vinfos[2].indices[1] = _ij2[1];
10407  vinfos[2].maxsolutions = _nj2;
10408  vinfos[3].jointtype = 1;
10409  vinfos[3].foffset = j3;
10410  vinfos[3].indices[0] = _ij3[0];
10411  vinfos[3].indices[1] = _ij3[1];
10412  vinfos[3].maxsolutions = _nj3;
10413  vinfos[4].jointtype = 1;
10414  vinfos[4].foffset = j4;
10415  vinfos[4].indices[0] = _ij4[0];
10416  vinfos[4].indices[1] = _ij4[1];
10417  vinfos[4].maxsolutions = _nj4;
10418  vinfos[5].jointtype = 1;
10419  vinfos[5].foffset = j5;
10420  vinfos[5].indices[0] = _ij5[0];
10421  vinfos[5].indices[1] = _ij5[1];
10422  vinfos[5].maxsolutions = _nj5;
10423  vinfos[6].jointtype = 1;
10424  vinfos[6].foffset = j6;
10425  vinfos[6].indices[0] = _ij6[0];
10426  vinfos[6].indices[1] = _ij6[1];
10427  vinfos[6].maxsolutions = _nj6;
10428  std::vector<int> vfree(0);
10429  solutions.AddSolution(vinfos,
10430  vfree);
10431  }
10432  }
10433  }
10434  }
10435  }
10436  }
10437  } while (0);
10438  if (bgotonextstatement)
10439  {
10440  bool bgotonextstatement = true;
10441  do
10442  {
10443  evalcond[0] =
10444  ((IKabs(new_r10)) + (IKabs(new_r00)));
10445  evalcond[1] = gconst7;
10446  evalcond[2] = gconst8;
10447  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
10448  IKabs(evalcond[1]) < 0.0000050000000000 &&
10449  IKabs(evalcond[2]) < 0.0000050000000000)
10450  {
10451  bgotonextstatement = false;
10452  {
10453  IkReal j0eval[3];
10454  CheckValue<IkReal> x517 =
10455  IKatan2WithCheck(
10456  IkReal(new_r01),
10457  IkReal(new_r11),
10458  IKFAST_ATAN2_MAGTHRESH);
10459  if (!x517.valid)
10460  {
10461  continue;
10462  }
10463  IkReal x516 = ((-1.0) * (x517.value));
10464  sj1 = 0;
10465  cj1 = -1.0;
10466  j1 = 3.14159265358979;
10467  sj2 = gconst7;
10468  cj2 = gconst8;
10469  j2 = x516;
10470  new_r00 = 0;
10471  new_r10 = 0;
10472  new_r21 = 0;
10473  new_r22 = 0;
10474  IkReal gconst6 = x516;
10475  IkReal gconst7 = ((-1.0) * new_r01);
10476  IkReal gconst8 = new_r11;
10477  j0eval[0] = -1.0;
10478  j0eval[1] =
10479  ((IKabs((new_r01 * new_r11))) +
10480  (IKabs(((1.0) +
10481  (((-1.0) *
10482  (new_r01 * new_r01)))))));
10483  j0eval[2] = -1.0;
10484  if (IKabs(j0eval[0]) <
10485  0.0000010000000000 ||
10486  IKabs(j0eval[1]) <
10487  0.0000010000000000 ||
10488  IKabs(j0eval[2]) < 0.0000010000000000)
10489  {
10490  {
10491  IkReal j0eval[3];
10492  CheckValue<IkReal> x519 =
10493  IKatan2WithCheck(
10494  IkReal(new_r01),
10495  IkReal(new_r11),
10496  IKFAST_ATAN2_MAGTHRESH);
10497  if (!x519.valid)
10498  {
10499  continue;
10500  }
10501  IkReal x518 = ((-1.0) * (x519.value));
10502  sj1 = 0;
10503  cj1 = -1.0;
10504  j1 = 3.14159265358979;
10505  sj2 = gconst7;
10506  cj2 = gconst8;
10507  j2 = x518;
10508  new_r00 = 0;
10509  new_r10 = 0;
10510  new_r21 = 0;
10511  new_r22 = 0;
10512  IkReal gconst6 = x518;
10513  IkReal gconst7 = ((-1.0) * new_r01);
10514  IkReal gconst8 = new_r11;
10515  j0eval[0] = -1.0;
10516  j0eval[1] =
10517  ((IKabs((new_r01 * new_r11))) +
10518  (IKabs(
10519  ((1.0) +
10520  (((-1.0) *
10521  (new_r01 * new_r01)))))));
10522  j0eval[2] = -1.0;
10523  if (IKabs(j0eval[0]) <
10524  0.0000010000000000 ||
10525  IKabs(j0eval[1]) <
10526  0.0000010000000000 ||
10527  IKabs(j0eval[2]) <
10528  0.0000010000000000)
10529  {
10530  {
10531  IkReal j0eval[3];
10532  CheckValue<IkReal> x521 =
10533  IKatan2WithCheck(
10534  IkReal(new_r01),
10535  IkReal(new_r11),
10536  IKFAST_ATAN2_MAGTHRESH);
10537  if (!x521.valid)
10538  {
10539  continue;
10540  }
10541  IkReal x520 =
10542  ((-1.0) * (x521.value));
10543  sj1 = 0;
10544  cj1 = -1.0;
10545  j1 = 3.14159265358979;
10546  sj2 = gconst7;
10547  cj2 = gconst8;
10548  j2 = x520;
10549  new_r00 = 0;
10550  new_r10 = 0;
10551  new_r21 = 0;
10552  new_r22 = 0;
10553  IkReal gconst6 = x520;
10554  IkReal gconst7 =
10555  ((-1.0) * new_r01);
10556  IkReal gconst8 = new_r11;
10557  j0eval[0] = 1.0;
10558  j0eval[1] =
10559  ((((0.5) *
10560  (IKabs(
10561  ((-1.0) +
10562  (((2.0) *
10563  (new_r01 *
10564  new_r01)))))))) +
10565  (IKabs(
10566  (new_r01 * new_r11))));
10567  j0eval[2] = 1.0;
10568  if (IKabs(j0eval[0]) <
10569  0.0000010000000000 ||
10570  IKabs(j0eval[1]) <
10571  0.0000010000000000 ||
10572  IKabs(j0eval[2]) <
10573  0.0000010000000000)
10574  {
10575  continue; // 3 cases reached
10576  }
10577  else
10578  {
10579  {
10580  IkReal j0array[1],
10581  cj0array[1], sj0array[1];
10582  bool j0valid[1] = { false };
10583  _nj0 = 1;
10584  IkReal x522 =
10585  ((1.0) * new_r11);
10586  CheckValue<IkReal> x523 =
10587  IKatan2WithCheck(
10588  IkReal((((gconst8 *
10589  new_r01)) +
10590  (((-1.0) *
10591  gconst7 *
10592  x522)))),
10593  IkReal(((((-1.0) *
10594  gconst8 *
10595  x522)) +
10596  (((-1.0) *
10597  gconst7 *
10598  new_r01)))),
10599  IKFAST_ATAN2_MAGTHRESH);
10600  if (!x523.valid)
10601  {
10602  continue;
10603  }
10604  CheckValue<IkReal> x524 =
10605  IKPowWithIntegerCheck(
10606  IKsign(((new_r01 *
10607  new_r01) +
10608  (new_r11 *
10609  new_r11))),
10610  -1);
10611  if (!x524.valid)
10612  {
10613  continue;
10614  }
10615  j0array[0] =
10616  ((-1.5707963267949) +
10617  (x523.value) +
10618  (((1.5707963267949) *
10619  (x524.value))));
10620  sj0array[0] =
10621  IKsin(j0array[0]);
10622  cj0array[0] =
10623  IKcos(j0array[0]);
10624  if (j0array[0] > IKPI)
10625  {
10626  j0array[0] -= IK2PI;
10627  }
10628  else if (j0array[0] < -IKPI)
10629  {
10630  j0array[0] += IK2PI;
10631  }
10632  j0valid[0] = true;
10633  for (int ij0 = 0; ij0 < 1;
10634  ++ij0)
10635  {
10636  if (!j0valid[ij0])
10637  {
10638  continue;
10639  }
10640  _ij0[0] = ij0;
10641  _ij0[1] = -1;
10642  for (int iij0 = ij0 + 1;
10643  iij0 < 1;
10644  ++iij0)
10645  {
10646  if (j0valid[iij0] &&
10647  IKabs(
10648  cj0array[ij0] -
10649  cj0array[iij0]) <
10650  IKFAST_SOLUTION_THRESH &&
10651  IKabs(
10652  sj0array[ij0] -
10653  sj0array[iij0]) <
10654  IKFAST_SOLUTION_THRESH)
10655  {
10656  j0valid[iij0] = false;
10657  _ij0[1] = iij0;
10658  break;
10659  }
10660  }
10661  j0 = j0array[ij0];
10662  cj0 = cj0array[ij0];
10663  sj0 = sj0array[ij0];
10664  {
10665  IkReal evalcond[6];
10666  IkReal x525 = IKsin(j0);
10667  IkReal x526 = IKcos(j0);
10668  IkReal x527 =
10669  (gconst7 * x526);
10670  IkReal x528 =
10671  ((1.0) * x525);
10672  IkReal x529 =
10673  (gconst8 * x526);
10674  IkReal x530 =
10675  (gconst8 * x528);
10676  evalcond[0] =
10677  ((((-1.0) * x530)) +
10678  x527);
10679  evalcond[1] =
10680  (((new_r01 * x526)) +
10681  gconst7 +
10682  ((new_r11 * x525)));
10683  evalcond[2] =
10684  (((gconst7 * x525)) +
10685  x529 + new_r11);
10686  evalcond[3] =
10687  (gconst8 +
10688  ((new_r11 * x526)) +
10689  (((-1.0) * new_r01 *
10690  x528)));
10691  evalcond[4] =
10692  ((((-1.0) * x529)) +
10693  (((-1.0) * gconst7 *
10694  x528)));
10695  evalcond[5] =
10696  ((((-1.0) * x530)) +
10697  x527 + new_r01);
10698  if (IKabs(evalcond[0]) >
10699  IKFAST_EVALCOND_THRESH ||
10700  IKabs(evalcond[1]) >
10701  IKFAST_EVALCOND_THRESH ||
10702  IKabs(evalcond[2]) >
10703  IKFAST_EVALCOND_THRESH ||
10704  IKabs(evalcond[3]) >
10705  IKFAST_EVALCOND_THRESH ||
10706  IKabs(evalcond[4]) >
10707  IKFAST_EVALCOND_THRESH ||
10708  IKabs(evalcond[5]) >
10709  IKFAST_EVALCOND_THRESH)
10710  {
10711  continue;
10712  }
10713  }
10714 
10715  {
10716  std::vector<
10717  IkSingleDOFSolutionBase<
10718  IkReal> >
10719  vinfos(7);
10720  vinfos[0].jointtype = 1;
10721  vinfos[0].foffset = j0;
10722  vinfos[0].indices[0] =
10723  _ij0[0];
10724  vinfos[0].indices[1] =
10725  _ij0[1];
10726  vinfos[0].maxsolutions =
10727  _nj0;
10728  vinfos[1].jointtype = 1;
10729  vinfos[1].foffset = j1;
10730  vinfos[1].indices[0] =
10731  _ij1[0];
10732  vinfos[1].indices[1] =
10733  _ij1[1];
10734  vinfos[1].maxsolutions =
10735  _nj1;
10736  vinfos[2].jointtype = 1;
10737  vinfos[2].foffset = j2;
10738  vinfos[2].indices[0] =
10739  _ij2[0];
10740  vinfos[2].indices[1] =
10741  _ij2[1];
10742  vinfos[2].maxsolutions =
10743  _nj2;
10744  vinfos[3].jointtype = 1;
10745  vinfos[3].foffset = j3;
10746  vinfos[3].indices[0] =
10747  _ij3[0];
10748  vinfos[3].indices[1] =
10749  _ij3[1];
10750  vinfos[3].maxsolutions =
10751  _nj3;
10752  vinfos[4].jointtype = 1;
10753  vinfos[4].foffset = j4;
10754  vinfos[4].indices[0] =
10755  _ij4[0];
10756  vinfos[4].indices[1] =
10757  _ij4[1];
10758  vinfos[4].maxsolutions =
10759  _nj4;
10760  vinfos[5].jointtype = 1;
10761  vinfos[5].foffset = j5;
10762  vinfos[5].indices[0] =
10763  _ij5[0];
10764  vinfos[5].indices[1] =
10765  _ij5[1];
10766  vinfos[5].maxsolutions =
10767  _nj5;
10768  vinfos[6].jointtype = 1;
10769  vinfos[6].foffset = j6;
10770  vinfos[6].indices[0] =
10771  _ij6[0];
10772  vinfos[6].indices[1] =
10773  _ij6[1];
10774  vinfos[6].maxsolutions =
10775  _nj6;
10776  std::vector<int> vfree(0);
10777  solutions.AddSolution(
10778  vinfos, vfree);
10779  }
10780  }
10781  }
10782  }
10783  }
10784  }
10785  else
10786  {
10787  {
10788  IkReal j0array[1], cj0array[1],
10789  sj0array[1];
10790  bool j0valid[1] = { false };
10791  _nj0 = 1;
10792  CheckValue<IkReal> x531 =
10793  IKatan2WithCheck(
10794  IkReal(
10795  (gconst7 * new_r11)),
10796  IkReal(
10797  (gconst8 * new_r11)),
10798  IKFAST_ATAN2_MAGTHRESH);
10799  if (!x531.valid)
10800  {
10801  continue;
10802  }
10803  CheckValue<IkReal> x532 =
10804  IKPowWithIntegerCheck(
10805  IKsign(((((-1.0) *
10806  (gconst8 *
10807  gconst8))) +
10808  (((-1.0) *
10809  (gconst7 *
10810  gconst7))))),
10811  -1);
10812  if (!x532.valid)
10813  {
10814  continue;
10815  }
10816  j0array[0] =
10817  ((-1.5707963267949) +
10818  (x531.value) +
10819  (((1.5707963267949) *
10820  (x532.value))));
10821  sj0array[0] = IKsin(j0array[0]);
10822  cj0array[0] = IKcos(j0array[0]);
10823  if (j0array[0] > IKPI)
10824  {
10825  j0array[0] -= IK2PI;
10826  }
10827  else if (j0array[0] < -IKPI)
10828  {
10829  j0array[0] += IK2PI;
10830  }
10831  j0valid[0] = true;
10832  for (int ij0 = 0; ij0 < 1; ++ij0)
10833  {
10834  if (!j0valid[ij0])
10835  {
10836  continue;
10837  }
10838  _ij0[0] = ij0;
10839  _ij0[1] = -1;
10840  for (int iij0 = ij0 + 1;
10841  iij0 < 1;
10842  ++iij0)
10843  {
10844  if (j0valid[iij0] &&
10845  IKabs(cj0array[ij0] -
10846  cj0array[iij0]) <
10847  IKFAST_SOLUTION_THRESH &&
10848  IKabs(sj0array[ij0] -
10849  sj0array[iij0]) <
10850  IKFAST_SOLUTION_THRESH)
10851  {
10852  j0valid[iij0] = false;
10853  _ij0[1] = iij0;
10854  break;
10855  }
10856  }
10857  j0 = j0array[ij0];
10858  cj0 = cj0array[ij0];
10859  sj0 = sj0array[ij0];
10860  {
10861  IkReal evalcond[6];
10862  IkReal x533 = IKsin(j0);
10863  IkReal x534 = IKcos(j0);
10864  IkReal x535 =
10865  (gconst7 * x534);
10866  IkReal x536 = ((1.0) * x533);
10867  IkReal x537 =
10868  (gconst8 * x534);
10869  IkReal x538 =
10870  (gconst8 * x536);
10871  evalcond[0] =
10872  ((((-1.0) * x538)) +
10873  x535);
10874  evalcond[1] =
10875  (((new_r01 * x534)) +
10876  gconst7 +
10877  ((new_r11 * x533)));
10878  evalcond[2] =
10879  (((gconst7 * x533)) +
10880  x537 + new_r11);
10881  evalcond[3] =
10882  (gconst8 +
10883  ((new_r11 * x534)) +
10884  (((-1.0) * new_r01 *
10885  x536)));
10886  evalcond[4] =
10887  ((((-1.0) * x537)) +
10888  (((-1.0) * gconst7 *
10889  x536)));
10890  evalcond[5] =
10891  ((((-1.0) * x538)) +
10892  x535 + new_r01);
10893  if (IKabs(evalcond[0]) >
10894  IKFAST_EVALCOND_THRESH ||
10895  IKabs(evalcond[1]) >
10896  IKFAST_EVALCOND_THRESH ||
10897  IKabs(evalcond[2]) >
10898  IKFAST_EVALCOND_THRESH ||
10899  IKabs(evalcond[3]) >
10900  IKFAST_EVALCOND_THRESH ||
10901  IKabs(evalcond[4]) >
10902  IKFAST_EVALCOND_THRESH ||
10903  IKabs(evalcond[5]) >
10904  IKFAST_EVALCOND_THRESH)
10905  {
10906  continue;
10907  }
10908  }
10909 
10910  {
10911  std::vector<
10912  IkSingleDOFSolutionBase<
10913  IkReal> >
10914  vinfos(7);
10915  vinfos[0].jointtype = 1;
10916  vinfos[0].foffset = j0;
10917  vinfos[0].indices[0] =
10918  _ij0[0];
10919  vinfos[0].indices[1] =
10920  _ij0[1];
10921  vinfos[0].maxsolutions = _nj0;
10922  vinfos[1].jointtype = 1;
10923  vinfos[1].foffset = j1;
10924  vinfos[1].indices[0] =
10925  _ij1[0];
10926  vinfos[1].indices[1] =
10927  _ij1[1];
10928  vinfos[1].maxsolutions = _nj1;
10929  vinfos[2].jointtype = 1;
10930  vinfos[2].foffset = j2;
10931  vinfos[2].indices[0] =
10932  _ij2[0];
10933  vinfos[2].indices[1] =
10934  _ij2[1];
10935  vinfos[2].maxsolutions = _nj2;
10936  vinfos[3].jointtype = 1;
10937  vinfos[3].foffset = j3;
10938  vinfos[3].indices[0] =
10939  _ij3[0];
10940  vinfos[3].indices[1] =
10941  _ij3[1];
10942  vinfos[3].maxsolutions = _nj3;
10943  vinfos[4].jointtype = 1;
10944  vinfos[4].foffset = j4;
10945  vinfos[4].indices[0] =
10946  _ij4[0];
10947  vinfos[4].indices[1] =
10948  _ij4[1];
10949  vinfos[4].maxsolutions = _nj4;
10950  vinfos[5].jointtype = 1;
10951  vinfos[5].foffset = j5;
10952  vinfos[5].indices[0] =
10953  _ij5[0];
10954  vinfos[5].indices[1] =
10955  _ij5[1];
10956  vinfos[5].maxsolutions = _nj5;
10957  vinfos[6].jointtype = 1;
10958  vinfos[6].foffset = j6;
10959  vinfos[6].indices[0] =
10960  _ij6[0];
10961  vinfos[6].indices[1] =
10962  _ij6[1];
10963  vinfos[6].maxsolutions = _nj6;
10964  std::vector<int> vfree(0);
10965  solutions.AddSolution(vinfos,
10966  vfree);
10967  }
10968  }
10969  }
10970  }
10971  }
10972  }
10973  else
10974  {
10975  {
10976  IkReal j0array[1], cj0array[1],
10977  sj0array[1];
10978  bool j0valid[1] = { false };
10979  _nj0 = 1;
10980  CheckValue<IkReal> x539 =
10981  IKatan2WithCheck(
10982  IkReal((gconst7 * gconst8)),
10983  IkReal(gconst8 * gconst8),
10984  IKFAST_ATAN2_MAGTHRESH);
10985  if (!x539.valid)
10986  {
10987  continue;
10988  }
10989  CheckValue<IkReal> x540 =
10990  IKPowWithIntegerCheck(
10991  IKsign(
10992  ((((-1.0) * gconst8 *
10993  new_r11)) +
10994  ((gconst7 * new_r01)))),
10995  -1);
10996  if (!x540.valid)
10997  {
10998  continue;
10999  }
11000  j0array[0] = ((-1.5707963267949) +
11001  (x539.value) +
11002  (((1.5707963267949) *
11003  (x540.value))));
11004  sj0array[0] = IKsin(j0array[0]);
11005  cj0array[0] = IKcos(j0array[0]);
11006  if (j0array[0] > IKPI)
11007  {
11008  j0array[0] -= IK2PI;
11009  }
11010  else if (j0array[0] < -IKPI)
11011  {
11012  j0array[0] += IK2PI;
11013  }
11014  j0valid[0] = true;
11015  for (int ij0 = 0; ij0 < 1; ++ij0)
11016  {
11017  if (!j0valid[ij0])
11018  {
11019  continue;
11020  }
11021  _ij0[0] = ij0;
11022  _ij0[1] = -1;
11023  for (int iij0 = ij0 + 1; iij0 < 1;
11024  ++iij0)
11025  {
11026  if (j0valid[iij0] &&
11027  IKabs(cj0array[ij0] -
11028  cj0array[iij0]) <
11029  IKFAST_SOLUTION_THRESH &&
11030  IKabs(sj0array[ij0] -
11031  sj0array[iij0]) <
11032  IKFAST_SOLUTION_THRESH)
11033  {
11034  j0valid[iij0] = false;
11035  _ij0[1] = iij0;
11036  break;
11037  }
11038  }
11039  j0 = j0array[ij0];
11040  cj0 = cj0array[ij0];
11041  sj0 = sj0array[ij0];
11042  {
11043  IkReal evalcond[6];
11044  IkReal x541 = IKsin(j0);
11045  IkReal x542 = IKcos(j0);
11046  IkReal x543 = (gconst7 * x542);
11047  IkReal x544 = ((1.0) * x541);
11048  IkReal x545 = (gconst8 * x542);
11049  IkReal x546 = (gconst8 * x544);
11050  evalcond[0] =
11051  ((((-1.0) * x546)) + x543);
11052  evalcond[1] =
11053  (gconst7 +
11054  ((new_r11 * x541)) +
11055  ((new_r01 * x542)));
11056  evalcond[2] =
11057  (x545 + ((gconst7 * x541)) +
11058  new_r11);
11059  evalcond[3] =
11060  (gconst8 +
11061  ((new_r11 * x542)) +
11062  (((-1.0) * new_r01 * x544)));
11063  evalcond[4] =
11064  ((((-1.0) * x545)) +
11065  (((-1.0) * gconst7 * x544)));
11066  evalcond[5] = ((((-1.0) * x546)) +
11067  x543 + new_r01);
11068  if (IKabs(evalcond[0]) >
11069  IKFAST_EVALCOND_THRESH ||
11070  IKabs(evalcond[1]) >
11071  IKFAST_EVALCOND_THRESH ||
11072  IKabs(evalcond[2]) >
11073  IKFAST_EVALCOND_THRESH ||
11074  IKabs(evalcond[3]) >
11075  IKFAST_EVALCOND_THRESH ||
11076  IKabs(evalcond[4]) >
11077  IKFAST_EVALCOND_THRESH ||
11078  IKabs(evalcond[5]) >
11079  IKFAST_EVALCOND_THRESH)
11080  {
11081  continue;
11082  }
11083  }
11084 
11085  {
11086  std::vector<
11087  IkSingleDOFSolutionBase<
11088  IkReal> >
11089  vinfos(7);
11090  vinfos[0].jointtype = 1;
11091  vinfos[0].foffset = j0;
11092  vinfos[0].indices[0] = _ij0[0];
11093  vinfos[0].indices[1] = _ij0[1];
11094  vinfos[0].maxsolutions = _nj0;
11095  vinfos[1].jointtype = 1;
11096  vinfos[1].foffset = j1;
11097  vinfos[1].indices[0] = _ij1[0];
11098  vinfos[1].indices[1] = _ij1[1];
11099  vinfos[1].maxsolutions = _nj1;
11100  vinfos[2].jointtype = 1;
11101  vinfos[2].foffset = j2;
11102  vinfos[2].indices[0] = _ij2[0];
11103  vinfos[2].indices[1] = _ij2[1];
11104  vinfos[2].maxsolutions = _nj2;
11105  vinfos[3].jointtype = 1;
11106  vinfos[3].foffset = j3;
11107  vinfos[3].indices[0] = _ij3[0];
11108  vinfos[3].indices[1] = _ij3[1];
11109  vinfos[3].maxsolutions = _nj3;
11110  vinfos[4].jointtype = 1;
11111  vinfos[4].foffset = j4;
11112  vinfos[4].indices[0] = _ij4[0];
11113  vinfos[4].indices[1] = _ij4[1];
11114  vinfos[4].maxsolutions = _nj4;
11115  vinfos[5].jointtype = 1;
11116  vinfos[5].foffset = j5;
11117  vinfos[5].indices[0] = _ij5[0];
11118  vinfos[5].indices[1] = _ij5[1];
11119  vinfos[5].maxsolutions = _nj5;
11120  vinfos[6].jointtype = 1;
11121  vinfos[6].foffset = j6;
11122  vinfos[6].indices[0] = _ij6[0];
11123  vinfos[6].indices[1] = _ij6[1];
11124  vinfos[6].maxsolutions = _nj6;
11125  std::vector<int> vfree(0);
11126  solutions.AddSolution(vinfos,
11127  vfree);
11128  }
11129  }
11130  }
11131  }
11132  }
11133  }
11134  } while (0);
11135  if (bgotonextstatement)
11136  {
11137  bool bgotonextstatement = true;
11138  do
11139  {
11140  evalcond[0] =
11141  ((IKabs(new_r10)) + (IKabs(new_r01)));
11142  if (IKabs(evalcond[0]) < 0.0000050000000000)
11143  {
11144  bgotonextstatement = false;
11145  {
11146  IkReal j0array[2], cj0array[2],
11147  sj0array[2];
11148  bool j0valid[2] = { false };
11149  _nj0 = 2;
11150  CheckValue<IkReal> x547 =
11151  IKPowWithIntegerCheck(gconst8, -1);
11152  if (!x547.valid)
11153  {
11154  continue;
11155  }
11156  cj0array[0] = (new_r00 * (x547.value));
11157  if (cj0array[0] >=
11158  -1 - IKFAST_SINCOS_THRESH &&
11159  cj0array[0] <=
11160  1 + IKFAST_SINCOS_THRESH)
11161  {
11162  j0valid[0] = j0valid[1] = true;
11163  j0array[0] = IKacos(cj0array[0]);
11164  sj0array[0] = IKsin(j0array[0]);
11165  cj0array[1] = cj0array[0];
11166  j0array[1] = -j0array[0];
11167  sj0array[1] = -sj0array[0];
11168  }
11169  else if (isnan(cj0array[0]))
11170  {
11171  // probably any value will work
11172  j0valid[0] = true;
11173  cj0array[0] = 1;
11174  sj0array[0] = 0;
11175  j0array[0] = 0;
11176  }
11177  for (int ij0 = 0; ij0 < 2; ++ij0)
11178  {
11179  if (!j0valid[ij0])
11180  {
11181  continue;
11182  }
11183  _ij0[0] = ij0;
11184  _ij0[1] = -1;
11185  for (int iij0 = ij0 + 1; iij0 < 2;
11186  ++iij0)
11187  {
11188  if (j0valid[iij0] &&
11189  IKabs(cj0array[ij0] -
11190  cj0array[iij0]) <
11191  IKFAST_SOLUTION_THRESH &&
11192  IKabs(sj0array[ij0] -
11193  sj0array[iij0]) <
11194  IKFAST_SOLUTION_THRESH)
11195  {
11196  j0valid[iij0] = false;
11197  _ij0[1] = iij0;
11198  break;
11199  }
11200  }
11201  j0 = j0array[ij0];
11202  cj0 = cj0array[ij0];
11203  sj0 = sj0array[ij0];
11204  {
11205  IkReal evalcond[6];
11206  IkReal x548 = IKsin(j0);
11207  IkReal x549 = IKcos(j0);
11208  IkReal x550 = ((-1.0) * x548);
11209  evalcond[0] = (new_r11 * x548);
11210  evalcond[1] = (new_r00 * x550);
11211  evalcond[2] = (gconst8 * x550);
11212  evalcond[3] =
11213  (gconst8 + ((new_r11 * x549)));
11214  evalcond[4] =
11215  (((gconst8 * x549)) + new_r11);
11216  evalcond[5] =
11217  (((new_r00 * x549)) +
11218  (((-1.0) * gconst8)));
11219  if (IKabs(evalcond[0]) >
11220  IKFAST_EVALCOND_THRESH ||
11221  IKabs(evalcond[1]) >
11222  IKFAST_EVALCOND_THRESH ||
11223  IKabs(evalcond[2]) >
11224  IKFAST_EVALCOND_THRESH ||
11225  IKabs(evalcond[3]) >
11226  IKFAST_EVALCOND_THRESH ||
11227  IKabs(evalcond[4]) >
11228  IKFAST_EVALCOND_THRESH ||
11229  IKabs(evalcond[5]) >
11230  IKFAST_EVALCOND_THRESH)
11231  {
11232  continue;
11233  }
11234  }
11235 
11236  {
11237  std::vector<IkSingleDOFSolutionBase<
11238  IkReal> >
11239  vinfos(7);
11240  vinfos[0].jointtype = 1;
11241  vinfos[0].foffset = j0;
11242  vinfos[0].indices[0] = _ij0[0];
11243  vinfos[0].indices[1] = _ij0[1];
11244  vinfos[0].maxsolutions = _nj0;
11245  vinfos[1].jointtype = 1;
11246  vinfos[1].foffset = j1;
11247  vinfos[1].indices[0] = _ij1[0];
11248  vinfos[1].indices[1] = _ij1[1];
11249  vinfos[1].maxsolutions = _nj1;
11250  vinfos[2].jointtype = 1;
11251  vinfos[2].foffset = j2;
11252  vinfos[2].indices[0] = _ij2[0];
11253  vinfos[2].indices[1] = _ij2[1];
11254  vinfos[2].maxsolutions = _nj2;
11255  vinfos[3].jointtype = 1;
11256  vinfos[3].foffset = j3;
11257  vinfos[3].indices[0] = _ij3[0];
11258  vinfos[3].indices[1] = _ij3[1];
11259  vinfos[3].maxsolutions = _nj3;
11260  vinfos[4].jointtype = 1;
11261  vinfos[4].foffset = j4;
11262  vinfos[4].indices[0] = _ij4[0];
11263  vinfos[4].indices[1] = _ij4[1];
11264  vinfos[4].maxsolutions = _nj4;
11265  vinfos[5].jointtype = 1;
11266  vinfos[5].foffset = j5;
11267  vinfos[5].indices[0] = _ij5[0];
11268  vinfos[5].indices[1] = _ij5[1];
11269  vinfos[5].maxsolutions = _nj5;
11270  vinfos[6].jointtype = 1;
11271  vinfos[6].foffset = j6;
11272  vinfos[6].indices[0] = _ij6[0];
11273  vinfos[6].indices[1] = _ij6[1];
11274  vinfos[6].maxsolutions = _nj6;
11275  std::vector<int> vfree(0);
11276  solutions.AddSolution(vinfos,
11277  vfree);
11278  }
11279  }
11280  }
11281  }
11282  } while (0);
11283  if (bgotonextstatement)
11284  {
11285  bool bgotonextstatement = true;
11286  do
11287  {
11288  evalcond[0] =
11289  ((IKabs(new_r00)) + (IKabs(new_r01)));
11290  if (IKabs(evalcond[0]) <
11291  0.0000050000000000)
11292  {
11293  bgotonextstatement = false;
11294  {
11295  IkReal j0eval[1];
11296  CheckValue<IkReal> x552 =
11297  IKatan2WithCheck(
11298  IkReal(0),
11299  IkReal(new_r11),
11300  IKFAST_ATAN2_MAGTHRESH);
11301  if (!x552.valid)
11302  {
11303  continue;
11304  }
11305  IkReal x551 = ((-1.0) * (x552.value));
11306  sj1 = 0;
11307  cj1 = -1.0;
11308  j1 = 3.14159265358979;
11309  sj2 = gconst7;
11310  cj2 = gconst8;
11311  j2 = x551;
11312  new_r00 = 0;
11313  new_r01 = 0;
11314  new_r12 = 0;
11315  new_r22 = 0;
11316  IkReal gconst6 = x551;
11317  IkReal gconst7 = 0;
11318  IkReal x553 =
11319  ((1.0) + (((-1.0) *
11320  (new_r10 * new_r10))));
11321  if (IKabs(x553) == 0)
11322  {
11323  continue;
11324  }
11325  IkReal gconst8 =
11326  (new_r11 * (pow(x553, -0.5)));
11327  j0eval[0] = ((IKabs(new_r11)) +
11328  (IKabs(new_r10)));
11329  if (IKabs(j0eval[0]) <
11330  0.0000010000000000)
11331  {
11332  {
11333  IkReal j0eval[1];
11334  CheckValue<IkReal> x555 =
11335  IKatan2WithCheck(
11336  IkReal(0),
11337  IkReal(new_r11),
11338  IKFAST_ATAN2_MAGTHRESH);
11339  if (!x555.valid)
11340  {
11341  continue;
11342  }
11343  IkReal x554 =
11344  ((-1.0) * (x555.value));
11345  sj1 = 0;
11346  cj1 = -1.0;
11347  j1 = 3.14159265358979;
11348  sj2 = gconst7;
11349  cj2 = gconst8;
11350  j2 = x554;
11351  new_r00 = 0;
11352  new_r01 = 0;
11353  new_r12 = 0;
11354  new_r22 = 0;
11355  IkReal gconst6 = x554;
11356  IkReal gconst7 = 0;
11357  IkReal x556 =
11358  ((1.0) +
11359  (((-1.0) *
11360  (new_r10 * new_r10))));
11361  if (IKabs(x556) == 0)
11362  {
11363  continue;
11364  }
11365  IkReal gconst8 =
11366  (new_r11 * (pow(x556, -0.5)));
11367  j0eval[0] = new_r11;
11368  if (IKabs(j0eval[0]) <
11369  0.0000010000000000)
11370  {
11371  {
11372  IkReal j0eval[2];
11373  CheckValue<IkReal> x558 =
11374  IKatan2WithCheck(
11375  IkReal(0),
11376  IkReal(new_r11),
11377  IKFAST_ATAN2_MAGTHRESH);
11378  if (!x558.valid)
11379  {
11380  continue;
11381  }
11382  IkReal x557 =
11383  ((-1.0) * (x558.value));
11384  sj1 = 0;
11385  cj1 = -1.0;
11386  j1 = 3.14159265358979;
11387  sj2 = gconst7;
11388  cj2 = gconst8;
11389  j2 = x557;
11390  new_r00 = 0;
11391  new_r01 = 0;
11392  new_r12 = 0;
11393  new_r22 = 0;
11394  IkReal gconst6 = x557;
11395  IkReal gconst7 = 0;
11396  IkReal x559 =
11397  ((1.0) +
11398  (((-1.0) *
11399  (new_r10 * new_r10))));
11400  if (IKabs(x559) == 0)
11401  {
11402  continue;
11403  }
11404  IkReal gconst8 =
11405  (new_r11 *
11406  (pow(x559, -0.5)));
11407  j0eval[0] = new_r10;
11408  j0eval[1] = new_r11;
11409  if (IKabs(j0eval[0]) <
11410  0.0000010000000000 ||
11411  IKabs(j0eval[1]) <
11412  0.0000010000000000)
11413  {
11414  continue; // 3 cases
11415  // reached
11416  }
11417  else
11418  {
11419  {
11420  IkReal j0array[1],
11421  cj0array[1],
11422  sj0array[1];
11423  bool j0valid[1] = {
11424  false
11425  };
11426  _nj0 = 1;
11427  CheckValue<IkReal> x560 =
11428  IKPowWithIntegerCheck(
11429  new_r10, -1);
11430  if (!x560.valid)
11431  {
11432  continue;
11433  }
11434  CheckValue<IkReal> x561 =
11435  IKPowWithIntegerCheck(
11436  new_r11, -1);
11437  if (!x561.valid)
11438  {
11439  continue;
11440  }
11441  if (IKabs(
11442  (gconst8 *
11443  (x560.value))) <
11444  IKFAST_ATAN2_MAGTHRESH &&
11445  IKabs((
11446  (-1.0) * gconst8 *
11447  (x561.value))) <
11448  IKFAST_ATAN2_MAGTHRESH &&
11449  IKabs(
11450  IKsqr((
11451  gconst8 *
11452  (x560.value))) +
11453  IKsqr((
11454  (-1.0) *
11455  gconst8 *
11456  (x561.value))) -
11457  1) <=
11458  IKFAST_SINCOS_THRESH)
11459  continue;
11460  j0array[0] = IKatan2(
11461  (gconst8 *
11462  (x560.value)),
11463  ((-1.0) * gconst8 *
11464  (x561.value)));
11465  sj0array[0] =
11466  IKsin(j0array[0]);
11467  cj0array[0] =
11468  IKcos(j0array[0]);
11469  if (j0array[0] > IKPI)
11470  {
11471  j0array[0] -= IK2PI;
11472  }
11473  else if (j0array[0] <
11474  -IKPI)
11475  {
11476  j0array[0] += IK2PI;
11477  }
11478  j0valid[0] = true;
11479  for (int ij0 = 0; ij0 < 1;
11480  ++ij0)
11481  {
11482  if (!j0valid[ij0])
11483  {
11484  continue;
11485  }
11486  _ij0[0] = ij0;
11487  _ij0[1] = -1;
11488  for (int iij0 = ij0 + 1;
11489  iij0 < 1;
11490  ++iij0)
11491  {
11492  if (j0valid[iij0] &&
11493  IKabs(
11494  cj0array
11495  [ij0] -
11496  cj0array
11497  [iij0]) <
11498  IKFAST_SOLUTION_THRESH &&
11499  IKabs(
11500  sj0array
11501  [ij0] -
11502  sj0array
11503  [iij0]) <
11504  IKFAST_SOLUTION_THRESH)
11505  {
11506  j0valid[iij0] =
11507  false;
11508  _ij0[1] = iij0;
11509  break;
11510  }
11511  }
11512  j0 = j0array[ij0];
11513  cj0 = cj0array[ij0];
11514  sj0 = sj0array[ij0];
11515  {
11516  IkReal evalcond[8];
11517  IkReal x562 =
11518  IKcos(j0);
11519  IkReal x563 =
11520  IKsin(j0);
11521  IkReal x564 =
11522  (gconst8 * x563);
11523  IkReal x565 =
11524  (gconst8 * x562);
11525  evalcond[0] =
11526  (new_r10 * x562);
11527  evalcond[1] =
11528  (new_r11 * x563);
11529  evalcond[2] =
11530  ((-1.0) * x565);
11531  evalcond[3] =
11532  ((-1.0) * x564);
11533  evalcond[4] =
11534  (gconst8 +
11535  ((new_r11 *
11536  x562)));
11537  evalcond[5] =
11538  (x565 + new_r11);
11539  evalcond[6] =
11540  ((((-1.0) *
11541  x564)) +
11542  new_r10);
11543  evalcond[7] =
11544  ((((-1.0) *
11545  gconst8)) +
11546  ((new_r10 *
11547  x563)));
11548  if (IKabs(
11549  evalcond[0]) >
11550  IKFAST_EVALCOND_THRESH ||
11551  IKabs(
11552  evalcond[1]) >
11553  IKFAST_EVALCOND_THRESH ||
11554  IKabs(
11555  evalcond[2]) >
11556  IKFAST_EVALCOND_THRESH ||
11557  IKabs(
11558  evalcond[3]) >
11559  IKFAST_EVALCOND_THRESH ||
11560  IKabs(
11561  evalcond[4]) >
11562  IKFAST_EVALCOND_THRESH ||
11563  IKabs(
11564  evalcond[5]) >
11565  IKFAST_EVALCOND_THRESH ||
11566  IKabs(
11567  evalcond[6]) >
11568  IKFAST_EVALCOND_THRESH ||
11569  IKabs(
11570  evalcond[7]) >
11571  IKFAST_EVALCOND_THRESH)
11572  {
11573  continue;
11574  }
11575  }
11576 
11577  {
11578  std::vector<
11579  IkSingleDOFSolutionBase<
11580  IkReal> >
11581  vinfos(7);
11582  vinfos[0].jointtype =
11583  1;
11584  vinfos[0].foffset =
11585  j0;
11586  vinfos[0].indices[0] =
11587  _ij0[0];
11588  vinfos[0].indices[1] =
11589  _ij0[1];
11590  vinfos[0]
11591  .maxsolutions =
11592  _nj0;
11593  vinfos[1].jointtype =
11594  1;
11595  vinfos[1].foffset =
11596  j1;
11597  vinfos[1].indices[0] =
11598  _ij1[0];
11599  vinfos[1].indices[1] =
11600  _ij1[1];
11601  vinfos[1]
11602  .maxsolutions =
11603  _nj1;
11604  vinfos[2].jointtype =
11605  1;
11606  vinfos[2].foffset =
11607  j2;
11608  vinfos[2].indices[0] =
11609  _ij2[0];
11610  vinfos[2].indices[1] =
11611  _ij2[1];
11612  vinfos[2]
11613  .maxsolutions =
11614  _nj2;
11615  vinfos[3].jointtype =
11616  1;
11617  vinfos[3].foffset =
11618  j3;
11619  vinfos[3].indices[0] =
11620  _ij3[0];
11621  vinfos[3].indices[1] =
11622  _ij3[1];
11623  vinfos[3]
11624  .maxsolutions =
11625  _nj3;
11626  vinfos[4].jointtype =
11627  1;
11628  vinfos[4].foffset =
11629  j4;
11630  vinfos[4].indices[0] =
11631  _ij4[0];
11632  vinfos[4].indices[1] =
11633  _ij4[1];
11634  vinfos[4]
11635  .maxsolutions =
11636  _nj4;
11637  vinfos[5].jointtype =
11638  1;
11639  vinfos[5].foffset =
11640  j5;
11641  vinfos[5].indices[0] =
11642  _ij5[0];
11643  vinfos[5].indices[1] =
11644  _ij5[1];
11645  vinfos[5]
11646  .maxsolutions =
11647  _nj5;
11648  vinfos[6].jointtype =
11649  1;
11650  vinfos[6].foffset =
11651  j6;
11652  vinfos[6].indices[0] =
11653  _ij6[0];
11654  vinfos[6].indices[1] =
11655  _ij6[1];
11656  vinfos[6]
11657  .maxsolutions =
11658  _nj6;
11659  std::vector<int>
11660  vfree(0);
11661  solutions.AddSolution(
11662  vinfos, vfree);
11663  }
11664  }
11665  }
11666  }
11667  }
11668  }
11669  else
11670  {
11671  {
11672  IkReal j0array[1],
11673  cj0array[1], sj0array[1];
11674  bool j0valid[1] = { false };
11675  _nj0 = 1;
11676  CheckValue<IkReal> x566 =
11677  IKPowWithIntegerCheck(
11678  gconst8, -1);
11679  if (!x566.valid)
11680  {
11681  continue;
11682  }
11683  CheckValue<IkReal> x567 =
11684  IKPowWithIntegerCheck(
11685  new_r11, -1);
11686  if (!x567.valid)
11687  {
11688  continue;
11689  }
11690  if (IKabs((new_r10 *
11691  (x566.value))) <
11692  IKFAST_ATAN2_MAGTHRESH &&
11693  IKabs(((-1.0) * gconst8 *
11694  (x567.value))) <
11695  IKFAST_ATAN2_MAGTHRESH &&
11696  IKabs(
11697  IKsqr(
11698  (new_r10 *
11699  (x566.value))) +
11700  IKsqr((
11701  (-1.0) * gconst8 *
11702  (x567.value))) -
11703  1) <=
11704  IKFAST_SINCOS_THRESH)
11705  continue;
11706  j0array[0] = IKatan2(
11707  (new_r10 * (x566.value)),
11708  ((-1.0) * gconst8 *
11709  (x567.value)));
11710  sj0array[0] =
11711  IKsin(j0array[0]);
11712  cj0array[0] =
11713  IKcos(j0array[0]);
11714  if (j0array[0] > IKPI)
11715  {
11716  j0array[0] -= IK2PI;
11717  }
11718  else if (j0array[0] < -IKPI)
11719  {
11720  j0array[0] += IK2PI;
11721  }
11722  j0valid[0] = true;
11723  for (int ij0 = 0; ij0 < 1;
11724  ++ij0)
11725  {
11726  if (!j0valid[ij0])
11727  {
11728  continue;
11729  }
11730  _ij0[0] = ij0;
11731  _ij0[1] = -1;
11732  for (int iij0 = ij0 + 1;
11733  iij0 < 1;
11734  ++iij0)
11735  {
11736  if (j0valid[iij0] &&
11737  IKabs(
11738  cj0array[ij0] -
11739  cj0array[iij0]) <
11740  IKFAST_SOLUTION_THRESH &&
11741  IKabs(
11742  sj0array[ij0] -
11743  sj0array[iij0]) <
11744  IKFAST_SOLUTION_THRESH)
11745  {
11746  j0valid[iij0] = false;
11747  _ij0[1] = iij0;
11748  break;
11749  }
11750  }
11751  j0 = j0array[ij0];
11752  cj0 = cj0array[ij0];
11753  sj0 = sj0array[ij0];
11754  {
11755  IkReal evalcond[8];
11756  IkReal x568 = IKcos(j0);
11757  IkReal x569 = IKsin(j0);
11758  IkReal x570 =
11759  (gconst8 * x569);
11760  IkReal x571 =
11761  (gconst8 * x568);
11762  evalcond[0] =
11763  (new_r10 * x568);
11764  evalcond[1] =
11765  (new_r11 * x569);
11766  evalcond[2] =
11767  ((-1.0) * x571);
11768  evalcond[3] =
11769  ((-1.0) * x570);
11770  evalcond[4] =
11771  (gconst8 +
11772  ((new_r11 * x568)));
11773  evalcond[5] =
11774  (x571 + new_r11);
11775  evalcond[6] =
11776  ((((-1.0) * x570)) +
11777  new_r10);
11778  evalcond[7] =
11779  ((((-1.0) *
11780  gconst8)) +
11781  ((new_r10 * x569)));
11782  if (IKabs(evalcond[0]) >
11783  IKFAST_EVALCOND_THRESH ||
11784  IKabs(evalcond[1]) >
11785  IKFAST_EVALCOND_THRESH ||
11786  IKabs(evalcond[2]) >
11787  IKFAST_EVALCOND_THRESH ||
11788  IKabs(evalcond[3]) >
11789  IKFAST_EVALCOND_THRESH ||
11790  IKabs(evalcond[4]) >
11791  IKFAST_EVALCOND_THRESH ||
11792  IKabs(evalcond[5]) >
11793  IKFAST_EVALCOND_THRESH ||
11794  IKabs(evalcond[6]) >
11795  IKFAST_EVALCOND_THRESH ||
11796  IKabs(evalcond[7]) >
11797  IKFAST_EVALCOND_THRESH)
11798  {
11799  continue;
11800  }
11801  }
11802 
11803  {
11804  std::vector<
11805  IkSingleDOFSolutionBase<
11806  IkReal> >
11807  vinfos(7);
11808  vinfos[0].jointtype = 1;
11809  vinfos[0].foffset = j0;
11810  vinfos[0].indices[0] =
11811  _ij0[0];
11812  vinfos[0].indices[1] =
11813  _ij0[1];
11814  vinfos[0].maxsolutions =
11815  _nj0;
11816  vinfos[1].jointtype = 1;
11817  vinfos[1].foffset = j1;
11818  vinfos[1].indices[0] =
11819  _ij1[0];
11820  vinfos[1].indices[1] =
11821  _ij1[1];
11822  vinfos[1].maxsolutions =
11823  _nj1;
11824  vinfos[2].jointtype = 1;
11825  vinfos[2].foffset = j2;
11826  vinfos[2].indices[0] =
11827  _ij2[0];
11828  vinfos[2].indices[1] =
11829  _ij2[1];
11830  vinfos[2].maxsolutions =
11831  _nj2;
11832  vinfos[3].jointtype = 1;
11833  vinfos[3].foffset = j3;
11834  vinfos[3].indices[0] =
11835  _ij3[0];
11836  vinfos[3].indices[1] =
11837  _ij3[1];
11838  vinfos[3].maxsolutions =
11839  _nj3;
11840  vinfos[4].jointtype = 1;
11841  vinfos[4].foffset = j4;
11842  vinfos[4].indices[0] =
11843  _ij4[0];
11844  vinfos[4].indices[1] =
11845  _ij4[1];
11846  vinfos[4].maxsolutions =
11847  _nj4;
11848  vinfos[5].jointtype = 1;
11849  vinfos[5].foffset = j5;
11850  vinfos[5].indices[0] =
11851  _ij5[0];
11852  vinfos[5].indices[1] =
11853  _ij5[1];
11854  vinfos[5].maxsolutions =
11855  _nj5;
11856  vinfos[6].jointtype = 1;
11857  vinfos[6].foffset = j6;
11858  vinfos[6].indices[0] =
11859  _ij6[0];
11860  vinfos[6].indices[1] =
11861  _ij6[1];
11862  vinfos[6].maxsolutions =
11863  _nj6;
11864  std::vector<int> vfree(0);
11865  solutions.AddSolution(
11866  vinfos, vfree);
11867  }
11868  }
11869  }
11870  }
11871  }
11872  }
11873  else
11874  {
11875  {
11876  IkReal j0array[1], cj0array[1],
11877  sj0array[1];
11878  bool j0valid[1] = { false };
11879  _nj0 = 1;
11880  CheckValue<IkReal> x572 =
11881  IKatan2WithCheck(
11882  IkReal(new_r10),
11883  IkReal(
11884  ((-1.0) * new_r11)),
11885  IKFAST_ATAN2_MAGTHRESH);
11886  if (!x572.valid)
11887  {
11888  continue;
11889  }
11890  CheckValue<IkReal> x573 =
11891  IKPowWithIntegerCheck(
11892  IKsign(gconst8), -1);
11893  if (!x573.valid)
11894  {
11895  continue;
11896  }
11897  j0array[0] =
11898  ((-1.5707963267949) +
11899  (x572.value) +
11900  (((1.5707963267949) *
11901  (x573.value))));
11902  sj0array[0] = IKsin(j0array[0]);
11903  cj0array[0] = IKcos(j0array[0]);
11904  if (j0array[0] > IKPI)
11905  {
11906  j0array[0] -= IK2PI;
11907  }
11908  else if (j0array[0] < -IKPI)
11909  {
11910  j0array[0] += IK2PI;
11911  }
11912  j0valid[0] = true;
11913  for (int ij0 = 0; ij0 < 1; ++ij0)
11914  {
11915  if (!j0valid[ij0])
11916  {
11917  continue;
11918  }
11919  _ij0[0] = ij0;
11920  _ij0[1] = -1;
11921  for (int iij0 = ij0 + 1;
11922  iij0 < 1;
11923  ++iij0)
11924  {
11925  if (j0valid[iij0] &&
11926  IKabs(cj0array[ij0] -
11927  cj0array[iij0]) <
11928  IKFAST_SOLUTION_THRESH &&
11929  IKabs(sj0array[ij0] -
11930  sj0array[iij0]) <
11931  IKFAST_SOLUTION_THRESH)
11932  {
11933  j0valid[iij0] = false;
11934  _ij0[1] = iij0;
11935  break;
11936  }
11937  }
11938  j0 = j0array[ij0];
11939  cj0 = cj0array[ij0];
11940  sj0 = sj0array[ij0];
11941  {
11942  IkReal evalcond[8];
11943  IkReal x574 = IKcos(j0);
11944  IkReal x575 = IKsin(j0);
11945  IkReal x576 =
11946  (gconst8 * x575);
11947  IkReal x577 =
11948  (gconst8 * x574);
11949  evalcond[0] =
11950  (new_r10 * x574);
11951  evalcond[1] =
11952  (new_r11 * x575);
11953  evalcond[2] = ((-1.0) * x577);
11954  evalcond[3] = ((-1.0) * x576);
11955  evalcond[4] =
11956  (((new_r11 * x574)) +
11957  gconst8);
11958  evalcond[5] =
11959  (x577 + new_r11);
11960  evalcond[6] =
11961  ((((-1.0) * x576)) +
11962  new_r10);
11963  evalcond[7] =
11964  (((new_r10 * x575)) +
11965  (((-1.0) * gconst8)));
11966  if (IKabs(evalcond[0]) >
11967  IKFAST_EVALCOND_THRESH ||
11968  IKabs(evalcond[1]) >
11969  IKFAST_EVALCOND_THRESH ||
11970  IKabs(evalcond[2]) >
11971  IKFAST_EVALCOND_THRESH ||
11972  IKabs(evalcond[3]) >
11973  IKFAST_EVALCOND_THRESH ||
11974  IKabs(evalcond[4]) >
11975  IKFAST_EVALCOND_THRESH ||
11976  IKabs(evalcond[5]) >
11977  IKFAST_EVALCOND_THRESH ||
11978  IKabs(evalcond[6]) >
11979  IKFAST_EVALCOND_THRESH ||
11980  IKabs(evalcond[7]) >
11981  IKFAST_EVALCOND_THRESH)
11982  {
11983  continue;
11984  }
11985  }
11986 
11987  {
11988  std::vector<
11989  IkSingleDOFSolutionBase<
11990  IkReal> >
11991  vinfos(7);
11992  vinfos[0].jointtype = 1;
11993  vinfos[0].foffset = j0;
11994  vinfos[0].indices[0] =
11995  _ij0[0];
11996  vinfos[0].indices[1] =
11997  _ij0[1];
11998  vinfos[0].maxsolutions = _nj0;
11999  vinfos[1].jointtype = 1;
12000  vinfos[1].foffset = j1;
12001  vinfos[1].indices[0] =
12002  _ij1[0];
12003  vinfos[1].indices[1] =
12004  _ij1[1];
12005  vinfos[1].maxsolutions = _nj1;
12006  vinfos[2].jointtype = 1;
12007  vinfos[2].foffset = j2;
12008  vinfos[2].indices[0] =
12009  _ij2[0];
12010  vinfos[2].indices[1] =
12011  _ij2[1];
12012  vinfos[2].maxsolutions = _nj2;
12013  vinfos[3].jointtype = 1;
12014  vinfos[3].foffset = j3;
12015  vinfos[3].indices[0] =
12016  _ij3[0];
12017  vinfos[3].indices[1] =
12018  _ij3[1];
12019  vinfos[3].maxsolutions = _nj3;
12020  vinfos[4].jointtype = 1;
12021  vinfos[4].foffset = j4;
12022  vinfos[4].indices[0] =
12023  _ij4[0];
12024  vinfos[4].indices[1] =
12025  _ij4[1];
12026  vinfos[4].maxsolutions = _nj4;
12027  vinfos[5].jointtype = 1;
12028  vinfos[5].foffset = j5;
12029  vinfos[5].indices[0] =
12030  _ij5[0];
12031  vinfos[5].indices[1] =
12032  _ij5[1];
12033  vinfos[5].maxsolutions = _nj5;
12034  vinfos[6].jointtype = 1;
12035  vinfos[6].foffset = j6;
12036  vinfos[6].indices[0] =
12037  _ij6[0];
12038  vinfos[6].indices[1] =
12039  _ij6[1];
12040  vinfos[6].maxsolutions = _nj6;
12041  std::vector<int> vfree(0);
12042  solutions.AddSolution(vinfos,
12043  vfree);
12044  }
12045  }
12046  }
12047  }
12048  }
12049  }
12050  } while (0);
12051  if (bgotonextstatement)
12052  {
12053  bool bgotonextstatement = true;
12054  do
12055  {
12056  evalcond[0] = IKabs(new_r01);
12057  if (IKabs(evalcond[0]) <
12058  0.0000050000000000)
12059  {
12060  bgotonextstatement = false;
12061  {
12062  IkReal j0eval[1];
12063  CheckValue<IkReal> x579 =
12064  IKatan2WithCheck(
12065  IkReal(0),
12066  IkReal(new_r11),
12067  IKFAST_ATAN2_MAGTHRESH);
12068  if (!x579.valid)
12069  {
12070  continue;
12071  }
12072  IkReal x578 =
12073  ((-1.0) * (x579.value));
12074  sj1 = 0;
12075  cj1 = -1.0;
12076  j1 = 3.14159265358979;
12077  sj2 = gconst7;
12078  cj2 = gconst8;
12079  j2 = x578;
12080  new_r01 = 0;
12081  IkReal gconst6 = x578;
12082  IkReal gconst7 = 0;
12083  IkReal x580 = new_r11 * new_r11;
12084  if (IKabs(x580) == 0)
12085  {
12086  continue;
12087  }
12088  IkReal gconst8 =
12089  (new_r11 * (pow(x580, -0.5)));
12090  j0eval[0] = ((IKabs(new_r10)) +
12091  (IKabs(new_r00)));
12092  if (IKabs(j0eval[0]) <
12093  0.0000010000000000)
12094  {
12095  {
12096  IkReal j0eval[1];
12097  CheckValue<IkReal> x582 =
12098  IKatan2WithCheck(
12099  IkReal(0),
12100  IkReal(new_r11),
12101  IKFAST_ATAN2_MAGTHRESH);
12102  if (!x582.valid)
12103  {
12104  continue;
12105  }
12106  IkReal x581 =
12107  ((-1.0) * (x582.value));
12108  sj1 = 0;
12109  cj1 = -1.0;
12110  j1 = 3.14159265358979;
12111  sj2 = gconst7;
12112  cj2 = gconst8;
12113  j2 = x581;
12114  new_r01 = 0;
12115  IkReal gconst6 = x581;
12116  IkReal gconst7 = 0;
12117  IkReal x583 = new_r11 * new_r11;
12118  if (IKabs(x583) == 0)
12119  {
12120  continue;
12121  }
12122  IkReal gconst8 =
12123  (new_r11 *
12124  (pow(x583, -0.5)));
12125  j0eval[0] = ((IKabs(new_r11)) +
12126  (IKabs(new_r10)));
12127  if (IKabs(j0eval[0]) <
12128  0.0000010000000000)
12129  {
12130  {
12131  IkReal j0eval[1];
12132  CheckValue<IkReal> x585 =
12133  IKatan2WithCheck(
12134  IkReal(0),
12135  IkReal(new_r11),
12136  IKFAST_ATAN2_MAGTHRESH);
12137  if (!x585.valid)
12138  {
12139  continue;
12140  }
12141  IkReal x584 =
12142  ((-1.0) * (x585.value));
12143  sj1 = 0;
12144  cj1 = -1.0;
12145  j1 = 3.14159265358979;
12146  sj2 = gconst7;
12147  cj2 = gconst8;
12148  j2 = x584;
12149  new_r01 = 0;
12150  IkReal gconst6 = x584;
12151  IkReal gconst7 = 0;
12152  IkReal x586 =
12153  new_r11 * new_r11;
12154  if (IKabs(x586) == 0)
12155  {
12156  continue;
12157  }
12158  IkReal gconst8 =
12159  (new_r11 *
12160  (pow(x586, -0.5)));
12161  j0eval[0] = new_r11;
12162  if (IKabs(j0eval[0]) <
12163  0.0000010000000000)
12164  {
12165  continue; // 3 cases
12166  // reached
12167  }
12168  else
12169  {
12170  {
12171  IkReal j0array[1],
12172  cj0array[1],
12173  sj0array[1];
12174  bool j0valid[1] = {
12175  false
12176  };
12177  _nj0 = 1;
12178  CheckValue<IkReal> x587 =
12179  IKPowWithIntegerCheck(
12180  gconst8, -1);
12181  if (!x587.valid)
12182  {
12183  continue;
12184  }
12185  CheckValue<IkReal> x588 =
12186  IKPowWithIntegerCheck(
12187  new_r11, -1);
12188  if (!x588.valid)
12189  {
12190  continue;
12191  }
12192  if (IKabs((
12193  new_r10 *
12194  (x587.value))) <
12195  IKFAST_ATAN2_MAGTHRESH &&
12196  IKabs((
12197  (-1.0) *
12198  gconst8 *
12199  (x588.value))) <
12200  IKFAST_ATAN2_MAGTHRESH &&
12201  IKabs(
12202  IKsqr((
12203  new_r10 *
12204  (x587.value))) +
12205  IKsqr((
12206  (-1.0) *
12207  gconst8 *
12208  (x588.value))) -
12209  1) <=
12210  IKFAST_SINCOS_THRESH)
12211  continue;
12212  j0array[0] = IKatan2(
12213  (new_r10 *
12214  (x587.value)),
12215  ((-1.0) * gconst8 *
12216  (x588.value)));
12217  sj0array[0] =
12218  IKsin(j0array[0]);
12219  cj0array[0] =
12220  IKcos(j0array[0]);
12221  if (j0array[0] > IKPI)
12222  {
12223  j0array[0] -= IK2PI;
12224  }
12225  else if (j0array[0] <
12226  -IKPI)
12227  {
12228  j0array[0] += IK2PI;
12229  }
12230  j0valid[0] = true;
12231  for (int ij0 = 0;
12232  ij0 < 1;
12233  ++ij0)
12234  {
12235  if (!j0valid[ij0])
12236  {
12237  continue;
12238  }
12239  _ij0[0] = ij0;
12240  _ij0[1] = -1;
12241  for (int iij0 =
12242  ij0 + 1;
12243  iij0 < 1;
12244  ++iij0)
12245  {
12246  if (j0valid[iij0] &&
12247  IKabs(
12248  cj0array
12249  [ij0] -
12250  cj0array
12251  [iij0]) <
12252  IKFAST_SOLUTION_THRESH &&
12253  IKabs(
12254  sj0array
12255  [ij0] -
12256  sj0array
12257  [iij0]) <
12258  IKFAST_SOLUTION_THRESH)
12259  {
12260  j0valid[iij0] =
12261  false;
12262  _ij0[1] = iij0;
12263  break;
12264  }
12265  }
12266  j0 = j0array[ij0];
12267  cj0 = cj0array[ij0];
12268  sj0 = sj0array[ij0];
12269  {
12270  IkReal evalcond[8];
12271  IkReal x589 =
12272  IKsin(j0);
12273  IkReal x590 =
12274  IKcos(j0);
12275  IkReal x591 =
12276  ((1.0) *
12277  gconst8);
12278  IkReal x592 =
12279  ((1.0) * x589);
12280  evalcond[0] =
12281  (new_r11 *
12282  x589);
12283  evalcond[1] =
12284  ((-1.0) *
12285  gconst8 *
12286  x589);
12287  evalcond[2] =
12288  (((new_r11 *
12289  x590)) +
12290  gconst8);
12291  evalcond[3] =
12292  (((gconst8 *
12293  x590)) +
12294  new_r11);
12295  evalcond[4] =
12296  ((((-1.0) *
12297  x589 *
12298  x591)) +
12299  new_r10);
12300  evalcond[5] =
12301  ((((-1.0) *
12302  x590 *
12303  x591)) +
12304  new_r00);
12305  evalcond[6] =
12306  ((((-1.0) *
12307  new_r00 *
12308  x592)) +
12309  ((new_r10 *
12310  x590)));
12311  evalcond[7] =
12312  (((new_r00 *
12313  x590)) +
12314  (((-1.0) *
12315  x591)) +
12316  ((new_r10 *
12317  x589)));
12318  if (IKabs(evalcond
12319  [0]) >
12320  IKFAST_EVALCOND_THRESH ||
12321  IKabs(evalcond
12322  [1]) >
12323  IKFAST_EVALCOND_THRESH ||
12324  IKabs(evalcond
12325  [2]) >
12326  IKFAST_EVALCOND_THRESH ||
12327  IKabs(evalcond
12328  [3]) >
12329  IKFAST_EVALCOND_THRESH ||
12330  IKabs(evalcond
12331  [4]) >
12332  IKFAST_EVALCOND_THRESH ||
12333  IKabs(evalcond
12334  [5]) >
12335  IKFAST_EVALCOND_THRESH ||
12336  IKabs(evalcond
12337  [6]) >
12338  IKFAST_EVALCOND_THRESH ||
12339  IKabs(evalcond
12340  [7]) >
12341  IKFAST_EVALCOND_THRESH)
12342  {
12343  continue;
12344  }
12345  }
12346 
12347  {
12348  std::vector<
12349  IkSingleDOFSolutionBase<
12350  IkReal> >
12351  vinfos(7);
12352  vinfos[0]
12353  .jointtype = 1;
12354  vinfos[0].foffset =
12355  j0;
12356  vinfos[0]
12357  .indices[0] =
12358  _ij0[0];
12359  vinfos[0]
12360  .indices[1] =
12361  _ij0[1];
12362  vinfos[0]
12363  .maxsolutions =
12364  _nj0;
12365  vinfos[1]
12366  .jointtype = 1;
12367  vinfos[1].foffset =
12368  j1;
12369  vinfos[1]
12370  .indices[0] =
12371  _ij1[0];
12372  vinfos[1]
12373  .indices[1] =
12374  _ij1[1];
12375  vinfos[1]
12376  .maxsolutions =
12377  _nj1;
12378  vinfos[2]
12379  .jointtype = 1;
12380  vinfos[2].foffset =
12381  j2;
12382  vinfos[2]
12383  .indices[0] =
12384  _ij2[0];
12385  vinfos[2]
12386  .indices[1] =
12387  _ij2[1];
12388  vinfos[2]
12389  .maxsolutions =
12390  _nj2;
12391  vinfos[3]
12392  .jointtype = 1;
12393  vinfos[3].foffset =
12394  j3;
12395  vinfos[3]
12396  .indices[0] =
12397  _ij3[0];
12398  vinfos[3]
12399  .indices[1] =
12400  _ij3[1];
12401  vinfos[3]
12402  .maxsolutions =
12403  _nj3;
12404  vinfos[4]
12405  .jointtype = 1;
12406  vinfos[4].foffset =
12407  j4;
12408  vinfos[4]
12409  .indices[0] =
12410  _ij4[0];
12411  vinfos[4]
12412  .indices[1] =
12413  _ij4[1];
12414  vinfos[4]
12415  .maxsolutions =
12416  _nj4;
12417  vinfos[5]
12418  .jointtype = 1;
12419  vinfos[5].foffset =
12420  j5;
12421  vinfos[5]
12422  .indices[0] =
12423  _ij5[0];
12424  vinfos[5]
12425  .indices[1] =
12426  _ij5[1];
12427  vinfos[5]
12428  .maxsolutions =
12429  _nj5;
12430  vinfos[6]
12431  .jointtype = 1;
12432  vinfos[6].foffset =
12433  j6;
12434  vinfos[6]
12435  .indices[0] =
12436  _ij6[0];
12437  vinfos[6]
12438  .indices[1] =
12439  _ij6[1];
12440  vinfos[6]
12441  .maxsolutions =
12442  _nj6;
12443  std::vector<int>
12444  vfree(0);
12445  solutions
12446  .AddSolution(
12447  vinfos,
12448  vfree);
12449  }
12450  }
12451  }
12452  }
12453  }
12454  }
12455  else
12456  {
12457  {
12458  IkReal j0array[1],
12459  cj0array[1],
12460  sj0array[1];
12461  bool j0valid[1] = { false };
12462  _nj0 = 1;
12463  CheckValue<IkReal> x593 =
12464  IKatan2WithCheck(
12465  IkReal(new_r10),
12466  IkReal(((-1.0) *
12467  new_r11)),
12468  IKFAST_ATAN2_MAGTHRESH);
12469  if (!x593.valid)
12470  {
12471  continue;
12472  }
12473  CheckValue<IkReal> x594 =
12474  IKPowWithIntegerCheck(
12475  IKsign(gconst8),
12476  -1);
12477  if (!x594.valid)
12478  {
12479  continue;
12480  }
12481  j0array[0] =
12482  ((-1.5707963267949) +
12483  (x593.value) +
12484  (((1.5707963267949) *
12485  (x594.value))));
12486  sj0array[0] =
12487  IKsin(j0array[0]);
12488  cj0array[0] =
12489  IKcos(j0array[0]);
12490  if (j0array[0] > IKPI)
12491  {
12492  j0array[0] -= IK2PI;
12493  }
12494  else if (j0array[0] < -IKPI)
12495  {
12496  j0array[0] += IK2PI;
12497  }
12498  j0valid[0] = true;
12499  for (int ij0 = 0; ij0 < 1;
12500  ++ij0)
12501  {
12502  if (!j0valid[ij0])
12503  {
12504  continue;
12505  }
12506  _ij0[0] = ij0;
12507  _ij0[1] = -1;
12508  for (int iij0 = ij0 + 1;
12509  iij0 < 1;
12510  ++iij0)
12511  {
12512  if (j0valid[iij0] &&
12513  IKabs(
12514  cj0array[ij0] -
12515  cj0array
12516  [iij0]) <
12517  IKFAST_SOLUTION_THRESH &&
12518  IKabs(
12519  sj0array[ij0] -
12520  sj0array
12521  [iij0]) <
12522  IKFAST_SOLUTION_THRESH)
12523  {
12524  j0valid[iij0] = false;
12525  _ij0[1] = iij0;
12526  break;
12527  }
12528  }
12529  j0 = j0array[ij0];
12530  cj0 = cj0array[ij0];
12531  sj0 = sj0array[ij0];
12532  {
12533  IkReal evalcond[8];
12534  IkReal x595 = IKsin(j0);
12535  IkReal x596 = IKcos(j0);
12536  IkReal x597 =
12537  ((1.0) * gconst8);
12538  IkReal x598 =
12539  ((1.0) * x595);
12540  evalcond[0] =
12541  (new_r11 * x595);
12542  evalcond[1] =
12543  ((-1.0) * gconst8 *
12544  x595);
12545  evalcond[2] =
12546  (((new_r11 *
12547  x596)) +
12548  gconst8);
12549  evalcond[3] =
12550  (((gconst8 *
12551  x596)) +
12552  new_r11);
12553  evalcond[4] =
12554  ((((-1.0) * x595 *
12555  x597)) +
12556  new_r10);
12557  evalcond[5] =
12558  ((((-1.0) * x596 *
12559  x597)) +
12560  new_r00);
12561  evalcond[6] =
12562  ((((-1.0) *
12563  new_r00 *
12564  x598)) +
12565  ((new_r10 *
12566  x596)));
12567  evalcond[7] =
12568  (((new_r10 *
12569  x595)) +
12570  ((new_r00 *
12571  x596)) +
12572  (((-1.0) * x597)));
12573  if (IKabs(evalcond[0]) >
12574  IKFAST_EVALCOND_THRESH ||
12575  IKabs(evalcond[1]) >
12576  IKFAST_EVALCOND_THRESH ||
12577  IKabs(evalcond[2]) >
12578  IKFAST_EVALCOND_THRESH ||
12579  IKabs(evalcond[3]) >
12580  IKFAST_EVALCOND_THRESH ||
12581  IKabs(evalcond[4]) >
12582  IKFAST_EVALCOND_THRESH ||
12583  IKabs(evalcond[5]) >
12584  IKFAST_EVALCOND_THRESH ||
12585  IKabs(evalcond[6]) >
12586  IKFAST_EVALCOND_THRESH ||
12587  IKabs(evalcond[7]) >
12588  IKFAST_EVALCOND_THRESH)
12589  {
12590  continue;
12591  }
12592  }
12593 
12594  {
12595  std::vector<
12596  IkSingleDOFSolutionBase<
12597  IkReal> >
12598  vinfos(7);
12599  vinfos[0].jointtype = 1;
12600  vinfos[0].foffset = j0;
12601  vinfos[0].indices[0] =
12602  _ij0[0];
12603  vinfos[0].indices[1] =
12604  _ij0[1];
12605  vinfos[0].maxsolutions =
12606  _nj0;
12607  vinfos[1].jointtype = 1;
12608  vinfos[1].foffset = j1;
12609  vinfos[1].indices[0] =
12610  _ij1[0];
12611  vinfos[1].indices[1] =
12612  _ij1[1];
12613  vinfos[1].maxsolutions =
12614  _nj1;
12615  vinfos[2].jointtype = 1;
12616  vinfos[2].foffset = j2;
12617  vinfos[2].indices[0] =
12618  _ij2[0];
12619  vinfos[2].indices[1] =
12620  _ij2[1];
12621  vinfos[2].maxsolutions =
12622  _nj2;
12623  vinfos[3].jointtype = 1;
12624  vinfos[3].foffset = j3;
12625  vinfos[3].indices[0] =
12626  _ij3[0];
12627  vinfos[3].indices[1] =
12628  _ij3[1];
12629  vinfos[3].maxsolutions =
12630  _nj3;
12631  vinfos[4].jointtype = 1;
12632  vinfos[4].foffset = j4;
12633  vinfos[4].indices[0] =
12634  _ij4[0];
12635  vinfos[4].indices[1] =
12636  _ij4[1];
12637  vinfos[4].maxsolutions =
12638  _nj4;
12639  vinfos[5].jointtype = 1;
12640  vinfos[5].foffset = j5;
12641  vinfos[5].indices[0] =
12642  _ij5[0];
12643  vinfos[5].indices[1] =
12644  _ij5[1];
12645  vinfos[5].maxsolutions =
12646  _nj5;
12647  vinfos[6].jointtype = 1;
12648  vinfos[6].foffset = j6;
12649  vinfos[6].indices[0] =
12650  _ij6[0];
12651  vinfos[6].indices[1] =
12652  _ij6[1];
12653  vinfos[6].maxsolutions =
12654  _nj6;
12655  std::vector<int> vfree(
12656  0);
12657  solutions.AddSolution(
12658  vinfos, vfree);
12659  }
12660  }
12661  }
12662  }
12663  }
12664  }
12665  else
12666  {
12667  {
12668  IkReal j0array[1], cj0array[1],
12669  sj0array[1];
12670  bool j0valid[1] = { false };
12671  _nj0 = 1;
12672  CheckValue<IkReal> x599 =
12673  IKPowWithIntegerCheck(
12674  IKsign(gconst8), -1);
12675  if (!x599.valid)
12676  {
12677  continue;
12678  }
12679  CheckValue<IkReal> x600 =
12680  IKatan2WithCheck(
12681  IkReal(new_r10),
12682  IkReal(new_r00),
12683  IKFAST_ATAN2_MAGTHRESH);
12684  if (!x600.valid)
12685  {
12686  continue;
12687  }
12688  j0array[0] =
12689  ((-1.5707963267949) +
12690  (((1.5707963267949) *
12691  (x599.value))) +
12692  (x600.value));
12693  sj0array[0] = IKsin(j0array[0]);
12694  cj0array[0] = IKcos(j0array[0]);
12695  if (j0array[0] > IKPI)
12696  {
12697  j0array[0] -= IK2PI;
12698  }
12699  else if (j0array[0] < -IKPI)
12700  {
12701  j0array[0] += IK2PI;
12702  }
12703  j0valid[0] = true;
12704  for (int ij0 = 0; ij0 < 1;
12705  ++ij0)
12706  {
12707  if (!j0valid[ij0])
12708  {
12709  continue;
12710  }
12711  _ij0[0] = ij0;
12712  _ij0[1] = -1;
12713  for (int iij0 = ij0 + 1;
12714  iij0 < 1;
12715  ++iij0)
12716  {
12717  if (j0valid[iij0] &&
12718  IKabs(cj0array[ij0] -
12719  cj0array[iij0]) <
12720  IKFAST_SOLUTION_THRESH &&
12721  IKabs(sj0array[ij0] -
12722  sj0array[iij0]) <
12723  IKFAST_SOLUTION_THRESH)
12724  {
12725  j0valid[iij0] = false;
12726  _ij0[1] = iij0;
12727  break;
12728  }
12729  }
12730  j0 = j0array[ij0];
12731  cj0 = cj0array[ij0];
12732  sj0 = sj0array[ij0];
12733  {
12734  IkReal evalcond[8];
12735  IkReal x601 = IKsin(j0);
12736  IkReal x602 = IKcos(j0);
12737  IkReal x603 =
12738  ((1.0) * gconst8);
12739  IkReal x604 =
12740  ((1.0) * x601);
12741  evalcond[0] =
12742  (new_r11 * x601);
12743  evalcond[1] =
12744  ((-1.0) * gconst8 *
12745  x601);
12746  evalcond[2] =
12747  (gconst8 +
12748  ((new_r11 * x602)));
12749  evalcond[3] =
12750  (((gconst8 * x602)) +
12751  new_r11);
12752  evalcond[4] =
12753  (new_r10 +
12754  (((-1.0) * x601 *
12755  x603)));
12756  evalcond[5] =
12757  ((((-1.0) * x602 *
12758  x603)) +
12759  new_r00);
12760  evalcond[6] =
12761  (((new_r10 * x602)) +
12762  (((-1.0) * new_r00 *
12763  x604)));
12764  evalcond[7] =
12765  (((new_r10 * x601)) +
12766  ((new_r00 * x602)) +
12767  (((-1.0) * x603)));
12768  if (IKabs(evalcond[0]) >
12769  IKFAST_EVALCOND_THRESH ||
12770  IKabs(evalcond[1]) >
12771  IKFAST_EVALCOND_THRESH ||
12772  IKabs(evalcond[2]) >
12773  IKFAST_EVALCOND_THRESH ||
12774  IKabs(evalcond[3]) >
12775  IKFAST_EVALCOND_THRESH ||
12776  IKabs(evalcond[4]) >
12777  IKFAST_EVALCOND_THRESH ||
12778  IKabs(evalcond[5]) >
12779  IKFAST_EVALCOND_THRESH ||
12780  IKabs(evalcond[6]) >
12781  IKFAST_EVALCOND_THRESH ||
12782  IKabs(evalcond[7]) >
12783  IKFAST_EVALCOND_THRESH)
12784  {
12785  continue;
12786  }
12787  }
12788 
12789  {
12790  std::vector<
12791  IkSingleDOFSolutionBase<
12792  IkReal> >
12793  vinfos(7);
12794  vinfos[0].jointtype = 1;
12795  vinfos[0].foffset = j0;
12796  vinfos[0].indices[0] =
12797  _ij0[0];
12798  vinfos[0].indices[1] =
12799  _ij0[1];
12800  vinfos[0].maxsolutions =
12801  _nj0;
12802  vinfos[1].jointtype = 1;
12803  vinfos[1].foffset = j1;
12804  vinfos[1].indices[0] =
12805  _ij1[0];
12806  vinfos[1].indices[1] =
12807  _ij1[1];
12808  vinfos[1].maxsolutions =
12809  _nj1;
12810  vinfos[2].jointtype = 1;
12811  vinfos[2].foffset = j2;
12812  vinfos[2].indices[0] =
12813  _ij2[0];
12814  vinfos[2].indices[1] =
12815  _ij2[1];
12816  vinfos[2].maxsolutions =
12817  _nj2;
12818  vinfos[3].jointtype = 1;
12819  vinfos[3].foffset = j3;
12820  vinfos[3].indices[0] =
12821  _ij3[0];
12822  vinfos[3].indices[1] =
12823  _ij3[1];
12824  vinfos[3].maxsolutions =
12825  _nj3;
12826  vinfos[4].jointtype = 1;
12827  vinfos[4].foffset = j4;
12828  vinfos[4].indices[0] =
12829  _ij4[0];
12830  vinfos[4].indices[1] =
12831  _ij4[1];
12832  vinfos[4].maxsolutions =
12833  _nj4;
12834  vinfos[5].jointtype = 1;
12835  vinfos[5].foffset = j5;
12836  vinfos[5].indices[0] =
12837  _ij5[0];
12838  vinfos[5].indices[1] =
12839  _ij5[1];
12840  vinfos[5].maxsolutions =
12841  _nj5;
12842  vinfos[6].jointtype = 1;
12843  vinfos[6].foffset = j6;
12844  vinfos[6].indices[0] =
12845  _ij6[0];
12846  vinfos[6].indices[1] =
12847  _ij6[1];
12848  vinfos[6].maxsolutions =
12849  _nj6;
12850  std::vector<int> vfree(0);
12851  solutions.AddSolution(
12852  vinfos, vfree);
12853  }
12854  }
12855  }
12856  }
12857  }
12858  }
12859  } while (0);
12860  if (bgotonextstatement)
12861  {
12862  bool bgotonextstatement = true;
12863  do
12864  {
12865  if (1)
12866  {
12867  bgotonextstatement = false;
12868  continue; // branch miss [j0]
12869  }
12870  } while (0);
12871  if (bgotonextstatement)
12872  {
12873  }
12874  }
12875  }
12876  }
12877  }
12878  }
12879  }
12880  }
12881  else
12882  {
12883  {
12884  IkReal j0array[1], cj0array[1], sj0array[1];
12885  bool j0valid[1] = { false };
12886  _nj0 = 1;
12887  IkReal x605 = ((1.0) * new_r11);
12888  CheckValue<IkReal> x606 = IKatan2WithCheck(
12889  IkReal(((((-1.0) * new_r01 * x605)) +
12890  ((gconst7 * gconst8)))),
12891  IkReal(((((-1.0) * (gconst7 * gconst7))) +
12892  (new_r11 * new_r11))),
12893  IKFAST_ATAN2_MAGTHRESH);
12894  if (!x606.valid)
12895  {
12896  continue;
12897  }
12898  CheckValue<IkReal> x607 = IKPowWithIntegerCheck(
12899  IKsign(((((-1.0) * gconst8 * x605)) +
12900  ((gconst7 * new_r01)))),
12901  -1);
12902  if (!x607.valid)
12903  {
12904  continue;
12905  }
12906  j0array[0] =
12907  ((-1.5707963267949) + (x606.value) +
12908  (((1.5707963267949) * (x607.value))));
12909  sj0array[0] = IKsin(j0array[0]);
12910  cj0array[0] = IKcos(j0array[0]);
12911  if (j0array[0] > IKPI)
12912  {
12913  j0array[0] -= IK2PI;
12914  }
12915  else if (j0array[0] < -IKPI)
12916  {
12917  j0array[0] += IK2PI;
12918  }
12919  j0valid[0] = true;
12920  for (int ij0 = 0; ij0 < 1; ++ij0)
12921  {
12922  if (!j0valid[ij0])
12923  {
12924  continue;
12925  }
12926  _ij0[0] = ij0;
12927  _ij0[1] = -1;
12928  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
12929  {
12930  if (j0valid[iij0] &&
12931  IKabs(cj0array[ij0] - cj0array[iij0]) <
12932  IKFAST_SOLUTION_THRESH &&
12933  IKabs(sj0array[ij0] - sj0array[iij0]) <
12934  IKFAST_SOLUTION_THRESH)
12935  {
12936  j0valid[iij0] = false;
12937  _ij0[1] = iij0;
12938  break;
12939  }
12940  }
12941  j0 = j0array[ij0];
12942  cj0 = cj0array[ij0];
12943  sj0 = sj0array[ij0];
12944  {
12945  IkReal evalcond[8];
12946  IkReal x608 = IKcos(j0);
12947  IkReal x609 = IKsin(j0);
12948  IkReal x610 = (gconst7 * x608);
12949  IkReal x611 = ((1.0) * x609);
12950  IkReal x612 = (gconst8 * x608);
12951  IkReal x613 = (gconst8 * x611);
12952  evalcond[0] = (gconst7 + ((new_r11 * x609)) +
12953  ((new_r01 * x608)));
12954  evalcond[1] =
12955  (((gconst7 * x609)) + x612 + new_r11);
12956  evalcond[2] = (gconst7 + ((new_r10 * x608)) +
12957  (((-1.0) * new_r00 * x611)));
12958  evalcond[3] = (gconst8 + ((new_r11 * x608)) +
12959  (((-1.0) * new_r01 * x611)));
12960  evalcond[4] =
12961  ((((-1.0) * x613)) + x610 + new_r10);
12962  evalcond[5] =
12963  ((((-1.0) * x613)) + x610 + new_r01);
12964  evalcond[6] =
12965  ((((-1.0) * gconst8)) +
12966  ((new_r10 * x609)) + ((new_r00 * x608)));
12967  evalcond[7] = ((((-1.0) * x612)) + new_r00 +
12968  (((-1.0) * gconst7 * x611)));
12969  if (IKabs(evalcond[0]) >
12970  IKFAST_EVALCOND_THRESH ||
12971  IKabs(evalcond[1]) >
12972  IKFAST_EVALCOND_THRESH ||
12973  IKabs(evalcond[2]) >
12974  IKFAST_EVALCOND_THRESH ||
12975  IKabs(evalcond[3]) >
12976  IKFAST_EVALCOND_THRESH ||
12977  IKabs(evalcond[4]) >
12978  IKFAST_EVALCOND_THRESH ||
12979  IKabs(evalcond[5]) >
12980  IKFAST_EVALCOND_THRESH ||
12981  IKabs(evalcond[6]) >
12982  IKFAST_EVALCOND_THRESH ||
12983  IKabs(evalcond[7]) >
12984  IKFAST_EVALCOND_THRESH)
12985  {
12986  continue;
12987  }
12988  }
12989 
12990  {
12991  std::vector<IkSingleDOFSolutionBase<IkReal> >
12992  vinfos(7);
12993  vinfos[0].jointtype = 1;
12994  vinfos[0].foffset = j0;
12995  vinfos[0].indices[0] = _ij0[0];
12996  vinfos[0].indices[1] = _ij0[1];
12997  vinfos[0].maxsolutions = _nj0;
12998  vinfos[1].jointtype = 1;
12999  vinfos[1].foffset = j1;
13000  vinfos[1].indices[0] = _ij1[0];
13001  vinfos[1].indices[1] = _ij1[1];
13002  vinfos[1].maxsolutions = _nj1;
13003  vinfos[2].jointtype = 1;
13004  vinfos[2].foffset = j2;
13005  vinfos[2].indices[0] = _ij2[0];
13006  vinfos[2].indices[1] = _ij2[1];
13007  vinfos[2].maxsolutions = _nj2;
13008  vinfos[3].jointtype = 1;
13009  vinfos[3].foffset = j3;
13010  vinfos[3].indices[0] = _ij3[0];
13011  vinfos[3].indices[1] = _ij3[1];
13012  vinfos[3].maxsolutions = _nj3;
13013  vinfos[4].jointtype = 1;
13014  vinfos[4].foffset = j4;
13015  vinfos[4].indices[0] = _ij4[0];
13016  vinfos[4].indices[1] = _ij4[1];
13017  vinfos[4].maxsolutions = _nj4;
13018  vinfos[5].jointtype = 1;
13019  vinfos[5].foffset = j5;
13020  vinfos[5].indices[0] = _ij5[0];
13021  vinfos[5].indices[1] = _ij5[1];
13022  vinfos[5].maxsolutions = _nj5;
13023  vinfos[6].jointtype = 1;
13024  vinfos[6].foffset = j6;
13025  vinfos[6].indices[0] = _ij6[0];
13026  vinfos[6].indices[1] = _ij6[1];
13027  vinfos[6].maxsolutions = _nj6;
13028  std::vector<int> vfree(0);
13029  solutions.AddSolution(vinfos, vfree);
13030  }
13031  }
13032  }
13033  }
13034  }
13035  }
13036  else
13037  {
13038  {
13039  IkReal j0array[1], cj0array[1], sj0array[1];
13040  bool j0valid[1] = { false };
13041  _nj0 = 1;
13042  IkReal x614 = ((1.0) * new_r11);
13043  CheckValue<IkReal> x615 = IKPowWithIntegerCheck(
13044  IKsign(
13045  ((new_r01 * new_r01) + (new_r11 * new_r11))),
13046  -1);
13047  if (!x615.valid)
13048  {
13049  continue;
13050  }
13051  CheckValue<IkReal> x616 = IKatan2WithCheck(
13052  IkReal((((gconst8 * new_r01)) +
13053  (((-1.0) * gconst7 * x614)))),
13054  IkReal(((((-1.0) * gconst7 * new_r01)) +
13055  (((-1.0) * gconst8 * x614)))),
13056  IKFAST_ATAN2_MAGTHRESH);
13057  if (!x616.valid)
13058  {
13059  continue;
13060  }
13061  j0array[0] = ((-1.5707963267949) +
13062  (((1.5707963267949) * (x615.value))) +
13063  (x616.value));
13064  sj0array[0] = IKsin(j0array[0]);
13065  cj0array[0] = IKcos(j0array[0]);
13066  if (j0array[0] > IKPI)
13067  {
13068  j0array[0] -= IK2PI;
13069  }
13070  else if (j0array[0] < -IKPI)
13071  {
13072  j0array[0] += IK2PI;
13073  }
13074  j0valid[0] = true;
13075  for (int ij0 = 0; ij0 < 1; ++ij0)
13076  {
13077  if (!j0valid[ij0])
13078  {
13079  continue;
13080  }
13081  _ij0[0] = ij0;
13082  _ij0[1] = -1;
13083  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13084  {
13085  if (j0valid[iij0] &&
13086  IKabs(cj0array[ij0] - cj0array[iij0]) <
13087  IKFAST_SOLUTION_THRESH &&
13088  IKabs(sj0array[ij0] - sj0array[iij0]) <
13089  IKFAST_SOLUTION_THRESH)
13090  {
13091  j0valid[iij0] = false;
13092  _ij0[1] = iij0;
13093  break;
13094  }
13095  }
13096  j0 = j0array[ij0];
13097  cj0 = cj0array[ij0];
13098  sj0 = sj0array[ij0];
13099  {
13100  IkReal evalcond[8];
13101  IkReal x617 = IKcos(j0);
13102  IkReal x618 = IKsin(j0);
13103  IkReal x619 = (gconst7 * x617);
13104  IkReal x620 = ((1.0) * x618);
13105  IkReal x621 = (gconst8 * x617);
13106  IkReal x622 = (gconst8 * x620);
13107  evalcond[0] = (gconst7 + ((new_r11 * x618)) +
13108  ((new_r01 * x617)));
13109  evalcond[1] =
13110  (((gconst7 * x618)) + x621 + new_r11);
13111  evalcond[2] = ((((-1.0) * new_r00 * x620)) +
13112  gconst7 + ((new_r10 * x617)));
13113  evalcond[3] = ((((-1.0) * new_r01 * x620)) +
13114  gconst8 + ((new_r11 * x617)));
13115  evalcond[4] =
13116  ((((-1.0) * x622)) + x619 + new_r10);
13117  evalcond[5] =
13118  ((((-1.0) * x622)) + x619 + new_r01);
13119  evalcond[6] =
13120  ((((-1.0) * gconst8)) + ((new_r10 * x618)) +
13121  ((new_r00 * x617)));
13122  evalcond[7] = ((((-1.0) * gconst7 * x620)) +
13123  (((-1.0) * x621)) + new_r00);
13124  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13125  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13126  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13127  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13128  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13129  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13130  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13131  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13132  {
13133  continue;
13134  }
13135  }
13136 
13137  {
13138  std::vector<IkSingleDOFSolutionBase<IkReal> >
13139  vinfos(7);
13140  vinfos[0].jointtype = 1;
13141  vinfos[0].foffset = j0;
13142  vinfos[0].indices[0] = _ij0[0];
13143  vinfos[0].indices[1] = _ij0[1];
13144  vinfos[0].maxsolutions = _nj0;
13145  vinfos[1].jointtype = 1;
13146  vinfos[1].foffset = j1;
13147  vinfos[1].indices[0] = _ij1[0];
13148  vinfos[1].indices[1] = _ij1[1];
13149  vinfos[1].maxsolutions = _nj1;
13150  vinfos[2].jointtype = 1;
13151  vinfos[2].foffset = j2;
13152  vinfos[2].indices[0] = _ij2[0];
13153  vinfos[2].indices[1] = _ij2[1];
13154  vinfos[2].maxsolutions = _nj2;
13155  vinfos[3].jointtype = 1;
13156  vinfos[3].foffset = j3;
13157  vinfos[3].indices[0] = _ij3[0];
13158  vinfos[3].indices[1] = _ij3[1];
13159  vinfos[3].maxsolutions = _nj3;
13160  vinfos[4].jointtype = 1;
13161  vinfos[4].foffset = j4;
13162  vinfos[4].indices[0] = _ij4[0];
13163  vinfos[4].indices[1] = _ij4[1];
13164  vinfos[4].maxsolutions = _nj4;
13165  vinfos[5].jointtype = 1;
13166  vinfos[5].foffset = j5;
13167  vinfos[5].indices[0] = _ij5[0];
13168  vinfos[5].indices[1] = _ij5[1];
13169  vinfos[5].maxsolutions = _nj5;
13170  vinfos[6].jointtype = 1;
13171  vinfos[6].foffset = j6;
13172  vinfos[6].indices[0] = _ij6[0];
13173  vinfos[6].indices[1] = _ij6[1];
13174  vinfos[6].maxsolutions = _nj6;
13175  std::vector<int> vfree(0);
13176  solutions.AddSolution(vinfos, vfree);
13177  }
13178  }
13179  }
13180  }
13181  }
13182  }
13183  else
13184  {
13185  {
13186  IkReal j0array[1], cj0array[1], sj0array[1];
13187  bool j0valid[1] = { false };
13188  _nj0 = 1;
13189  IkReal x623 = ((1.0) * gconst7);
13190  CheckValue<IkReal> x624 = IKatan2WithCheck(
13191  IkReal(((((-1.0) * new_r10 * x623)) +
13192  ((gconst7 * new_r01)))),
13193  IkReal(((((-1.0) * new_r00 * x623)) +
13194  (((-1.0) * new_r11 * x623)))),
13195  IKFAST_ATAN2_MAGTHRESH);
13196  if (!x624.valid)
13197  {
13198  continue;
13199  }
13200  CheckValue<IkReal> x625 =
13201  IKPowWithIntegerCheck(IKsign((((new_r10 * new_r11)) +
13202  ((new_r00 * new_r01)))),
13203  -1);
13204  if (!x625.valid)
13205  {
13206  continue;
13207  }
13208  j0array[0] = ((-1.5707963267949) + (x624.value) +
13209  (((1.5707963267949) * (x625.value))));
13210  sj0array[0] = IKsin(j0array[0]);
13211  cj0array[0] = IKcos(j0array[0]);
13212  if (j0array[0] > IKPI)
13213  {
13214  j0array[0] -= IK2PI;
13215  }
13216  else if (j0array[0] < -IKPI)
13217  {
13218  j0array[0] += IK2PI;
13219  }
13220  j0valid[0] = true;
13221  for (int ij0 = 0; ij0 < 1; ++ij0)
13222  {
13223  if (!j0valid[ij0])
13224  {
13225  continue;
13226  }
13227  _ij0[0] = ij0;
13228  _ij0[1] = -1;
13229  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13230  {
13231  if (j0valid[iij0] &&
13232  IKabs(cj0array[ij0] - cj0array[iij0]) <
13233  IKFAST_SOLUTION_THRESH &&
13234  IKabs(sj0array[ij0] - sj0array[iij0]) <
13235  IKFAST_SOLUTION_THRESH)
13236  {
13237  j0valid[iij0] = false;
13238  _ij0[1] = iij0;
13239  break;
13240  }
13241  }
13242  j0 = j0array[ij0];
13243  cj0 = cj0array[ij0];
13244  sj0 = sj0array[ij0];
13245  {
13246  IkReal evalcond[8];
13247  IkReal x626 = IKcos(j0);
13248  IkReal x627 = IKsin(j0);
13249  IkReal x628 = (gconst7 * x626);
13250  IkReal x629 = ((1.0) * x627);
13251  IkReal x630 = (gconst8 * x626);
13252  IkReal x631 = (gconst8 * x629);
13253  evalcond[0] = (gconst7 + ((new_r01 * x626)) +
13254  ((new_r11 * x627)));
13255  evalcond[1] = (x630 + new_r11 + ((gconst7 * x627)));
13256  evalcond[2] = ((((-1.0) * new_r00 * x629)) + gconst7 +
13257  ((new_r10 * x626)));
13258  evalcond[3] = ((((-1.0) * new_r01 * x629)) + gconst8 +
13259  ((new_r11 * x626)));
13260  evalcond[4] = ((((-1.0) * x631)) + x628 + new_r10);
13261  evalcond[5] = ((((-1.0) * x631)) + x628 + new_r01);
13262  evalcond[6] =
13263  ((((-1.0) * gconst8)) + ((new_r00 * x626)) +
13264  ((new_r10 * x627)));
13265  evalcond[7] = ((((-1.0) * gconst7 * x629)) +
13266  (((-1.0) * x630)) + new_r00);
13267  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13268  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13269  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13270  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13271  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13272  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13273  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13274  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13275  {
13276  continue;
13277  }
13278  }
13279 
13280  {
13281  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
13282  7);
13283  vinfos[0].jointtype = 1;
13284  vinfos[0].foffset = j0;
13285  vinfos[0].indices[0] = _ij0[0];
13286  vinfos[0].indices[1] = _ij0[1];
13287  vinfos[0].maxsolutions = _nj0;
13288  vinfos[1].jointtype = 1;
13289  vinfos[1].foffset = j1;
13290  vinfos[1].indices[0] = _ij1[0];
13291  vinfos[1].indices[1] = _ij1[1];
13292  vinfos[1].maxsolutions = _nj1;
13293  vinfos[2].jointtype = 1;
13294  vinfos[2].foffset = j2;
13295  vinfos[2].indices[0] = _ij2[0];
13296  vinfos[2].indices[1] = _ij2[1];
13297  vinfos[2].maxsolutions = _nj2;
13298  vinfos[3].jointtype = 1;
13299  vinfos[3].foffset = j3;
13300  vinfos[3].indices[0] = _ij3[0];
13301  vinfos[3].indices[1] = _ij3[1];
13302  vinfos[3].maxsolutions = _nj3;
13303  vinfos[4].jointtype = 1;
13304  vinfos[4].foffset = j4;
13305  vinfos[4].indices[0] = _ij4[0];
13306  vinfos[4].indices[1] = _ij4[1];
13307  vinfos[4].maxsolutions = _nj4;
13308  vinfos[5].jointtype = 1;
13309  vinfos[5].foffset = j5;
13310  vinfos[5].indices[0] = _ij5[0];
13311  vinfos[5].indices[1] = _ij5[1];
13312  vinfos[5].maxsolutions = _nj5;
13313  vinfos[6].jointtype = 1;
13314  vinfos[6].foffset = j6;
13315  vinfos[6].indices[0] = _ij6[0];
13316  vinfos[6].indices[1] = _ij6[1];
13317  vinfos[6].maxsolutions = _nj6;
13318  std::vector<int> vfree(0);
13319  solutions.AddSolution(vinfos, vfree);
13320  }
13321  }
13322  }
13323  }
13324  }
13325  }
13326  } while (0);
13327  if (bgotonextstatement)
13328  {
13329  bool bgotonextstatement = true;
13330  do
13331  {
13332  IkReal x633 = ((new_r01 * new_r01) + (new_r11 * new_r11));
13333  if (IKabs(x633) == 0)
13334  {
13335  continue;
13336  }
13337  IkReal x632 = pow(x633, -0.5);
13338  CheckValue<IkReal> x634 = IKatan2WithCheck(
13339  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13340  if (!x634.valid)
13341  {
13342  continue;
13343  }
13344  IkReal gconst9 =
13345  ((3.14159265358979) + (((-1.0) * (x634.value))));
13346  IkReal gconst10 = ((1.0) * new_r01 * x632);
13347  IkReal gconst11 = ((-1.0) * new_r11 * x632);
13348  CheckValue<IkReal> x635 = IKatan2WithCheck(
13349  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13350  if (!x635.valid)
13351  {
13352  continue;
13353  }
13354  evalcond[0] =
13355  ((-3.14159265358979) +
13356  (IKfmod(
13357  ((3.14159265358979) +
13358  (IKabs(((-3.14159265358979) + (x635.value) + j2)))),
13359  6.28318530717959)));
13360  if (IKabs(evalcond[0]) < 0.0000050000000000)
13361  {
13362  bgotonextstatement = false;
13363  {
13364  IkReal j0eval[3];
13365  CheckValue<IkReal> x638 =
13366  IKatan2WithCheck(IkReal(new_r01),
13367  IkReal(new_r11),
13368  IKFAST_ATAN2_MAGTHRESH);
13369  if (!x638.valid)
13370  {
13371  continue;
13372  }
13373  IkReal x636 = ((1.0) * (x638.value));
13374  IkReal x637 = x632;
13375  sj1 = 0;
13376  cj1 = -1.0;
13377  j1 = 3.14159265358979;
13378  sj2 = gconst10;
13379  cj2 = gconst11;
13380  j2 = ((3.14159265) + (((-1.0) * x636)));
13381  IkReal gconst9 = ((3.14159265358979) + (((-1.0) * x636)));
13382  IkReal gconst10 = ((1.0) * new_r01 * x637);
13383  IkReal gconst11 = ((-1.0) * new_r11 * x637);
13384  IkReal x639 = new_r01 * new_r01;
13385  IkReal x640 =
13386  (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
13387  IkReal x641 = x632;
13388  IkReal x642 = ((1.0) * new_r01 * x641);
13389  j0eval[0] = x640;
13390  j0eval[1] = ((IKabs((((x639 * x641)) +
13391  (((-1.0) * new_r10 * x642))))) +
13392  (IKabs(((((-1.0) * new_r11 * x642)) +
13393  (((-1.0) * new_r00 * x642))))));
13394  j0eval[2] = IKsign(x640);
13395  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13396  IKabs(j0eval[1]) < 0.0000010000000000 ||
13397  IKabs(j0eval[2]) < 0.0000010000000000)
13398  {
13399  {
13400  IkReal j0eval[2];
13401  CheckValue<IkReal> x645 =
13402  IKatan2WithCheck(IkReal(new_r01),
13403  IkReal(new_r11),
13404  IKFAST_ATAN2_MAGTHRESH);
13405  if (!x645.valid)
13406  {
13407  continue;
13408  }
13409  IkReal x643 = ((1.0) * (x645.value));
13410  IkReal x644 = x632;
13411  sj1 = 0;
13412  cj1 = -1.0;
13413  j1 = 3.14159265358979;
13414  sj2 = gconst10;
13415  cj2 = gconst11;
13416  j2 = ((3.14159265) + (((-1.0) * x643)));
13417  IkReal gconst9 =
13418  ((3.14159265358979) + (((-1.0) * x643)));
13419  IkReal gconst10 = ((1.0) * new_r01 * x644);
13420  IkReal gconst11 = ((-1.0) * new_r11 * x644);
13421  IkReal x646 =
13422  ((new_r01 * new_r01) + (new_r11 * new_r11));
13423  j0eval[0] = x646;
13424  j0eval[1] = IKsign(x646);
13425  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13426  IKabs(j0eval[1]) < 0.0000010000000000)
13427  {
13428  {
13429  IkReal j0eval[1];
13430  CheckValue<IkReal> x649 =
13431  IKatan2WithCheck(IkReal(new_r01),
13432  IkReal(new_r11),
13433  IKFAST_ATAN2_MAGTHRESH);
13434  if (!x649.valid)
13435  {
13436  continue;
13437  }
13438  IkReal x647 = ((1.0) * (x649.value));
13439  IkReal x648 = x632;
13440  sj1 = 0;
13441  cj1 = -1.0;
13442  j1 = 3.14159265358979;
13443  sj2 = gconst10;
13444  cj2 = gconst11;
13445  j2 = ((3.14159265) + (((-1.0) * x647)));
13446  IkReal gconst9 =
13447  ((3.14159265358979) + (((-1.0) * x647)));
13448  IkReal gconst10 = ((1.0) * new_r01 * x648);
13449  IkReal gconst11 = ((-1.0) * new_r11 * x648);
13450  IkReal x650 = new_r01 * new_r01;
13451  IkReal x651 = new_r11 * new_r11;
13452  IkReal x652 = ((1.0) * x650);
13453  CheckValue<IkReal> x658 =
13454  IKPowWithIntegerCheck((x650 + x651), -1);
13455  if (!x658.valid)
13456  {
13457  continue;
13458  }
13459  IkReal x653 = x658.value;
13460  CheckValue<IkReal> x659 = IKPowWithIntegerCheck(
13461  ((((-1.0) * x651)) + (((-1.0) * x652))), -1);
13462  if (!x659.valid)
13463  {
13464  continue;
13465  }
13466  IkReal x654 = x659.value;
13467  IkReal x655 = ((1.0) * x654);
13468  IkReal x656 = (new_r11 * x655);
13469  IkReal x657 = (new_r01 * x655);
13470  j0eval[0] =
13471  ((IKabs(((((-1.0) * new_r01 * x656)) +
13472  (((-1.0) * x656 *
13473  (new_r01 * new_r01 * new_r01))) +
13474  (((-1.0) * new_r01 * x656 *
13475  (new_r11 * new_r11)))))) +
13476  (IKabs((((x650 * x651 * x653)) +
13477  ((x653 * (x651 * x651))) +
13478  (((-1.0) * x652 * x653))))));
13479  if (IKabs(j0eval[0]) < 0.0000010000000000)
13480  {
13481  {
13482  IkReal evalcond[3];
13483  bool bgotonextstatement = true;
13484  do
13485  {
13486  evalcond[0] =
13487  ((IKabs(new_r11)) + (IKabs(new_r00)));
13488  if (IKabs(evalcond[0]) < 0.0000050000000000)
13489  {
13490  bgotonextstatement = false;
13491  {
13492  IkReal j0eval[1];
13493  CheckValue<IkReal> x661 =
13494  IKatan2WithCheck(
13495  IkReal(new_r01),
13496  IkReal(0),
13497  IKFAST_ATAN2_MAGTHRESH);
13498  if (!x661.valid)
13499  {
13500  continue;
13501  }
13502  IkReal x660 = ((1.0) * (x661.value));
13503  sj1 = 0;
13504  cj1 = -1.0;
13505  j1 = 3.14159265358979;
13506  sj2 = gconst10;
13507  cj2 = gconst11;
13508  j2 = ((3.14159265) + (((-1.0) * x660)));
13509  new_r11 = 0;
13510  new_r00 = 0;
13511  IkReal gconst9 = ((3.14159265358979) +
13512  (((-1.0) * x660)));
13513  IkReal x662 = new_r01 * new_r01;
13514  if (IKabs(x662) == 0)
13515  {
13516  continue;
13517  }
13518  IkReal gconst10 =
13519  ((1.0) * new_r01 * (pow(x662, -0.5)));
13520  IkReal gconst11 = 0;
13521  j0eval[0] = new_r10;
13522  if (IKabs(j0eval[0]) < 0.0000010000000000)
13523  {
13524  {
13525  IkReal j0array[2], cj0array[2],
13526  sj0array[2];
13527  bool j0valid[2] = { false };
13528  _nj0 = 2;
13529  CheckValue<IkReal> x663 =
13530  IKPowWithIntegerCheck(gconst10,
13531  -1);
13532  if (!x663.valid)
13533  {
13534  continue;
13535  }
13536  cj0array[0] =
13537  ((-1.0) * new_r10 * (x663.value));
13538  if (cj0array[0] >=
13539  -1 - IKFAST_SINCOS_THRESH &&
13540  cj0array[0] <=
13541  1 + IKFAST_SINCOS_THRESH)
13542  {
13543  j0valid[0] = j0valid[1] = true;
13544  j0array[0] = IKacos(cj0array[0]);
13545  sj0array[0] = IKsin(j0array[0]);
13546  cj0array[1] = cj0array[0];
13547  j0array[1] = -j0array[0];
13548  sj0array[1] = -sj0array[0];
13549  }
13550  else if (isnan(cj0array[0]))
13551  {
13552  // probably any value will work
13553  j0valid[0] = true;
13554  cj0array[0] = 1;
13555  sj0array[0] = 0;
13556  j0array[0] = 0;
13557  }
13558  for (int ij0 = 0; ij0 < 2; ++ij0)
13559  {
13560  if (!j0valid[ij0])
13561  {
13562  continue;
13563  }
13564  _ij0[0] = ij0;
13565  _ij0[1] = -1;
13566  for (int iij0 = ij0 + 1; iij0 < 2;
13567  ++iij0)
13568  {
13569  if (j0valid[iij0] &&
13570  IKabs(cj0array[ij0] -
13571  cj0array[iij0]) <
13572  IKFAST_SOLUTION_THRESH &&
13573  IKabs(sj0array[ij0] -
13574  sj0array[iij0]) <
13575  IKFAST_SOLUTION_THRESH)
13576  {
13577  j0valid[iij0] = false;
13578  _ij0[1] = iij0;
13579  break;
13580  }
13581  }
13582  j0 = j0array[ij0];
13583  cj0 = cj0array[ij0];
13584  sj0 = sj0array[ij0];
13585  {
13586  IkReal evalcond[6];
13587  IkReal x664 = IKsin(j0);
13588  IkReal x665 = IKcos(j0);
13589  IkReal x666 = ((-1.0) * x664);
13590  evalcond[0] = (new_r10 * x664);
13591  evalcond[1] = (new_r01 * x666);
13592  evalcond[2] = (gconst10 * x666);
13593  evalcond[3] =
13594  (gconst10 +
13595  ((new_r10 * x665)));
13596  evalcond[4] =
13597  (gconst10 +
13598  ((new_r01 * x665)));
13599  evalcond[5] =
13600  (new_r01 +
13601  ((gconst10 * x665)));
13602  if (IKabs(evalcond[0]) >
13603  IKFAST_EVALCOND_THRESH ||
13604  IKabs(evalcond[1]) >
13605  IKFAST_EVALCOND_THRESH ||
13606  IKabs(evalcond[2]) >
13607  IKFAST_EVALCOND_THRESH ||
13608  IKabs(evalcond[3]) >
13609  IKFAST_EVALCOND_THRESH ||
13610  IKabs(evalcond[4]) >
13611  IKFAST_EVALCOND_THRESH ||
13612  IKabs(evalcond[5]) >
13613  IKFAST_EVALCOND_THRESH)
13614  {
13615  continue;
13616  }
13617  }
13618 
13619  {
13620  std::vector<
13621  IkSingleDOFSolutionBase<
13622  IkReal> >
13623  vinfos(7);
13624  vinfos[0].jointtype = 1;
13625  vinfos[0].foffset = j0;
13626  vinfos[0].indices[0] = _ij0[0];
13627  vinfos[0].indices[1] = _ij0[1];
13628  vinfos[0].maxsolutions = _nj0;
13629  vinfos[1].jointtype = 1;
13630  vinfos[1].foffset = j1;
13631  vinfos[1].indices[0] = _ij1[0];
13632  vinfos[1].indices[1] = _ij1[1];
13633  vinfos[1].maxsolutions = _nj1;
13634  vinfos[2].jointtype = 1;
13635  vinfos[2].foffset = j2;
13636  vinfos[2].indices[0] = _ij2[0];
13637  vinfos[2].indices[1] = _ij2[1];
13638  vinfos[2].maxsolutions = _nj2;
13639  vinfos[3].jointtype = 1;
13640  vinfos[3].foffset = j3;
13641  vinfos[3].indices[0] = _ij3[0];
13642  vinfos[3].indices[1] = _ij3[1];
13643  vinfos[3].maxsolutions = _nj3;
13644  vinfos[4].jointtype = 1;
13645  vinfos[4].foffset = j4;
13646  vinfos[4].indices[0] = _ij4[0];
13647  vinfos[4].indices[1] = _ij4[1];
13648  vinfos[4].maxsolutions = _nj4;
13649  vinfos[5].jointtype = 1;
13650  vinfos[5].foffset = j5;
13651  vinfos[5].indices[0] = _ij5[0];
13652  vinfos[5].indices[1] = _ij5[1];
13653  vinfos[5].maxsolutions = _nj5;
13654  vinfos[6].jointtype = 1;
13655  vinfos[6].foffset = j6;
13656  vinfos[6].indices[0] = _ij6[0];
13657  vinfos[6].indices[1] = _ij6[1];
13658  vinfos[6].maxsolutions = _nj6;
13659  std::vector<int> vfree(0);
13660  solutions.AddSolution(vinfos,
13661  vfree);
13662  }
13663  }
13664  }
13665  }
13666  else
13667  {
13668  {
13669  IkReal j0array[2], cj0array[2],
13670  sj0array[2];
13671  bool j0valid[2] = { false };
13672  _nj0 = 2;
13673  CheckValue<IkReal> x667 =
13674  IKPowWithIntegerCheck(new_r10,
13675  -1);
13676  if (!x667.valid)
13677  {
13678  continue;
13679  }
13680  cj0array[0] = ((-1.0) * gconst10 *
13681  (x667.value));
13682  if (cj0array[0] >=
13683  -1 - IKFAST_SINCOS_THRESH &&
13684  cj0array[0] <=
13685  1 + IKFAST_SINCOS_THRESH)
13686  {
13687  j0valid[0] = j0valid[1] = true;
13688  j0array[0] = IKacos(cj0array[0]);
13689  sj0array[0] = IKsin(j0array[0]);
13690  cj0array[1] = cj0array[0];
13691  j0array[1] = -j0array[0];
13692  sj0array[1] = -sj0array[0];
13693  }
13694  else if (isnan(cj0array[0]))
13695  {
13696  // probably any value will work
13697  j0valid[0] = true;
13698  cj0array[0] = 1;
13699  sj0array[0] = 0;
13700  j0array[0] = 0;
13701  }
13702  for (int ij0 = 0; ij0 < 2; ++ij0)
13703  {
13704  if (!j0valid[ij0])
13705  {
13706  continue;
13707  }
13708  _ij0[0] = ij0;
13709  _ij0[1] = -1;
13710  for (int iij0 = ij0 + 1; iij0 < 2;
13711  ++iij0)
13712  {
13713  if (j0valid[iij0] &&
13714  IKabs(cj0array[ij0] -
13715  cj0array[iij0]) <
13716  IKFAST_SOLUTION_THRESH &&
13717  IKabs(sj0array[ij0] -
13718  sj0array[iij0]) <
13719  IKFAST_SOLUTION_THRESH)
13720  {
13721  j0valid[iij0] = false;
13722  _ij0[1] = iij0;
13723  break;
13724  }
13725  }
13726  j0 = j0array[ij0];
13727  cj0 = cj0array[ij0];
13728  sj0 = sj0array[ij0];
13729  {
13730  IkReal evalcond[6];
13731  IkReal x668 = IKsin(j0);
13732  IkReal x669 = IKcos(j0);
13733  IkReal x670 = (gconst10 * x669);
13734  IkReal x671 = ((-1.0) * x668);
13735  evalcond[0] = (new_r10 * x668);
13736  evalcond[1] = (new_r01 * x671);
13737  evalcond[2] = (gconst10 * x671);
13738  evalcond[3] = (x670 + new_r10);
13739  evalcond[4] =
13740  (gconst10 +
13741  ((new_r01 * x669)));
13742  evalcond[5] = (x670 + new_r01);
13743  if (IKabs(evalcond[0]) >
13744  IKFAST_EVALCOND_THRESH ||
13745  IKabs(evalcond[1]) >
13746  IKFAST_EVALCOND_THRESH ||
13747  IKabs(evalcond[2]) >
13748  IKFAST_EVALCOND_THRESH ||
13749  IKabs(evalcond[3]) >
13750  IKFAST_EVALCOND_THRESH ||
13751  IKabs(evalcond[4]) >
13752  IKFAST_EVALCOND_THRESH ||
13753  IKabs(evalcond[5]) >
13754  IKFAST_EVALCOND_THRESH)
13755  {
13756  continue;
13757  }
13758  }
13759 
13760  {
13761  std::vector<
13762  IkSingleDOFSolutionBase<
13763  IkReal> >
13764  vinfos(7);
13765  vinfos[0].jointtype = 1;
13766  vinfos[0].foffset = j0;
13767  vinfos[0].indices[0] = _ij0[0];
13768  vinfos[0].indices[1] = _ij0[1];
13769  vinfos[0].maxsolutions = _nj0;
13770  vinfos[1].jointtype = 1;
13771  vinfos[1].foffset = j1;
13772  vinfos[1].indices[0] = _ij1[0];
13773  vinfos[1].indices[1] = _ij1[1];
13774  vinfos[1].maxsolutions = _nj1;
13775  vinfos[2].jointtype = 1;
13776  vinfos[2].foffset = j2;
13777  vinfos[2].indices[0] = _ij2[0];
13778  vinfos[2].indices[1] = _ij2[1];
13779  vinfos[2].maxsolutions = _nj2;
13780  vinfos[3].jointtype = 1;
13781  vinfos[3].foffset = j3;
13782  vinfos[3].indices[0] = _ij3[0];
13783  vinfos[3].indices[1] = _ij3[1];
13784  vinfos[3].maxsolutions = _nj3;
13785  vinfos[4].jointtype = 1;
13786  vinfos[4].foffset = j4;
13787  vinfos[4].indices[0] = _ij4[0];
13788  vinfos[4].indices[1] = _ij4[1];
13789  vinfos[4].maxsolutions = _nj4;
13790  vinfos[5].jointtype = 1;
13791  vinfos[5].foffset = j5;
13792  vinfos[5].indices[0] = _ij5[0];
13793  vinfos[5].indices[1] = _ij5[1];
13794  vinfos[5].maxsolutions = _nj5;
13795  vinfos[6].jointtype = 1;
13796  vinfos[6].foffset = j6;
13797  vinfos[6].indices[0] = _ij6[0];
13798  vinfos[6].indices[1] = _ij6[1];
13799  vinfos[6].maxsolutions = _nj6;
13800  std::vector<int> vfree(0);
13801  solutions.AddSolution(vinfos,
13802  vfree);
13803  }
13804  }
13805  }
13806  }
13807  }
13808  }
13809  } while (0);
13810  if (bgotonextstatement)
13811  {
13812  bool bgotonextstatement = true;
13813  do
13814  {
13815  evalcond[0] =
13816  ((IKabs(new_r10)) + (IKabs(new_r00)));
13817  evalcond[1] = gconst10;
13818  evalcond[2] = gconst11;
13819  if (IKabs(evalcond[0]) <
13820  0.0000050000000000 &&
13821  IKabs(evalcond[1]) <
13822  0.0000050000000000 &&
13823  IKabs(evalcond[2]) < 0.0000050000000000)
13824  {
13825  bgotonextstatement = false;
13826  {
13827  IkReal j0eval[3];
13828  CheckValue<IkReal> x673 =
13829  IKatan2WithCheck(
13830  IkReal(new_r01),
13831  IkReal(new_r11),
13832  IKFAST_ATAN2_MAGTHRESH);
13833  if (!x673.valid)
13834  {
13835  continue;
13836  }
13837  IkReal x672 = ((1.0) * (x673.value));
13838  sj1 = 0;
13839  cj1 = -1.0;
13840  j1 = 3.14159265358979;
13841  sj2 = gconst10;
13842  cj2 = gconst11;
13843  j2 = ((3.14159265) + (((-1.0) * x672)));
13844  new_r00 = 0;
13845  new_r10 = 0;
13846  new_r21 = 0;
13847  new_r22 = 0;
13848  IkReal gconst9 = ((3.14159265358979) +
13849  (((-1.0) * x672)));
13850  IkReal gconst10 = ((1.0) * new_r01);
13851  IkReal gconst11 = ((-1.0) * new_r11);
13852  j0eval[0] = 1.0;
13853  j0eval[1] = 1.0;
13854  j0eval[2] =
13855  ((IKabs(((1.0) +
13856  (((-1.0) * (new_r01 *
13857  new_r01)))))) +
13858  (IKabs(
13859  ((1.0) * new_r01 * new_r11))));
13860  if (IKabs(j0eval[0]) <
13861  0.0000010000000000 ||
13862  IKabs(j0eval[1]) <
13863  0.0000010000000000 ||
13864  IKabs(j0eval[2]) <
13865  0.0000010000000000)
13866  {
13867  {
13868  IkReal j0eval[3];
13869  CheckValue<IkReal> x675 =
13870  IKatan2WithCheck(
13871  IkReal(new_r01),
13872  IkReal(new_r11),
13873  IKFAST_ATAN2_MAGTHRESH);
13874  if (!x675.valid)
13875  {
13876  continue;
13877  }
13878  IkReal x674 =
13879  ((1.0) * (x675.value));
13880  sj1 = 0;
13881  cj1 = -1.0;
13882  j1 = 3.14159265358979;
13883  sj2 = gconst10;
13884  cj2 = gconst11;
13885  j2 = ((3.14159265) +
13886  (((-1.0) * x674)));
13887  new_r00 = 0;
13888  new_r10 = 0;
13889  new_r21 = 0;
13890  new_r22 = 0;
13891  IkReal gconst9 =
13892  ((3.14159265358979) +
13893  (((-1.0) * x674)));
13894  IkReal gconst10 = ((1.0) * new_r01);
13895  IkReal gconst11 =
13896  ((-1.0) * new_r11);
13897  j0eval[0] = -1.0;
13898  j0eval[1] = -1.0;
13899  j0eval[2] =
13900  ((IKabs(((-1.0) + (new_r01 *
13901  new_r01)))) +
13902  (IKabs(((1.0) * new_r01 *
13903  new_r11))));
13904  if (IKabs(j0eval[0]) <
13905  0.0000010000000000 ||
13906  IKabs(j0eval[1]) <
13907  0.0000010000000000 ||
13908  IKabs(j0eval[2]) <
13909  0.0000010000000000)
13910  {
13911  {
13912  IkReal j0eval[3];
13913  CheckValue<IkReal> x677 =
13914  IKatan2WithCheck(
13915  IkReal(new_r01),
13916  IkReal(new_r11),
13917  IKFAST_ATAN2_MAGTHRESH);
13918  if (!x677.valid)
13919  {
13920  continue;
13921  }
13922  IkReal x676 =
13923  ((1.0) * (x677.value));
13924  sj1 = 0;
13925  cj1 = -1.0;
13926  j1 = 3.14159265358979;
13927  sj2 = gconst10;
13928  cj2 = gconst11;
13929  j2 = ((3.14159265) +
13930  (((-1.0) * x676)));
13931  new_r00 = 0;
13932  new_r10 = 0;
13933  new_r21 = 0;
13934  new_r22 = 0;
13935  IkReal gconst9 =
13936  ((3.14159265358979) +
13937  (((-1.0) * x676)));
13938  IkReal gconst10 =
13939  ((1.0) * new_r01);
13940  IkReal gconst11 =
13941  ((-1.0) * new_r11);
13942  j0eval[0] = 1.0;
13943  j0eval[1] = 1.0;
13944  j0eval[2] =
13945  ((IKabs(((2.0) * new_r01 *
13946  new_r11))) +
13947  (IKabs(((1.0) +
13948  (((-2.0) *
13949  (new_r01 *
13950  new_r01)))))));
13951  if (IKabs(j0eval[0]) <
13952  0.0000010000000000 ||
13953  IKabs(j0eval[1]) <
13954  0.0000010000000000 ||
13955  IKabs(j0eval[2]) <
13956  0.0000010000000000)
13957  {
13958  continue; // 3 cases reached
13959  }
13960  else
13961  {
13962  {
13963  IkReal j0array[1],
13964  cj0array[1],
13965  sj0array[1];
13966  bool j0valid[1] = { false };
13967  _nj0 = 1;
13968  IkReal x678 =
13969  ((1.0) * new_r11);
13970  CheckValue<IkReal> x679 =
13971  IKatan2WithCheck(
13972  IkReal(
13973  (((gconst11 *
13974  new_r01)) +
13975  (((-1.0) *
13976  gconst10 *
13977  x678)))),
13978  IkReal(
13979  ((((-1.0) *
13980  gconst11 *
13981  x678)) +
13982  (((-1.0) *
13983  gconst10 *
13984  new_r01)))),
13985  IKFAST_ATAN2_MAGTHRESH);
13986  if (!x679.valid)
13987  {
13988  continue;
13989  }
13990  CheckValue<IkReal> x680 =
13991  IKPowWithIntegerCheck(
13992  IKsign(((new_r01 *
13993  new_r01) +
13994  (new_r11 *
13995  new_r11))),
13996  -1);
13997  if (!x680.valid)
13998  {
13999  continue;
14000  }
14001  j0array[0] =
14002  ((-1.5707963267949) +
14003  (x679.value) +
14004  (((1.5707963267949) *
14005  (x680.value))));
14006  sj0array[0] =
14007  IKsin(j0array[0]);
14008  cj0array[0] =
14009  IKcos(j0array[0]);
14010  if (j0array[0] > IKPI)
14011  {
14012  j0array[0] -= IK2PI;
14013  }
14014  else if (j0array[0] < -IKPI)
14015  {
14016  j0array[0] += IK2PI;
14017  }
14018  j0valid[0] = true;
14019  for (int ij0 = 0; ij0 < 1;
14020  ++ij0)
14021  {
14022  if (!j0valid[ij0])
14023  {
14024  continue;
14025  }
14026  _ij0[0] = ij0;
14027  _ij0[1] = -1;
14028  for (int iij0 = ij0 + 1;
14029  iij0 < 1;
14030  ++iij0)
14031  {
14032  if (j0valid[iij0] &&
14033  IKabs(
14034  cj0array[ij0] -
14035  cj0array
14036  [iij0]) <
14037  IKFAST_SOLUTION_THRESH &&
14038  IKabs(
14039  sj0array[ij0] -
14040  sj0array
14041  [iij0]) <
14042  IKFAST_SOLUTION_THRESH)
14043  {
14044  j0valid[iij0] = false;
14045  _ij0[1] = iij0;
14046  break;
14047  }
14048  }
14049  j0 = j0array[ij0];
14050  cj0 = cj0array[ij0];
14051  sj0 = sj0array[ij0];
14052  {
14053  IkReal evalcond[6];
14054  IkReal x681 = IKsin(j0);
14055  IkReal x682 = IKcos(j0);
14056  IkReal x683 =
14057  (gconst10 * x682);
14058  IkReal x684 =
14059  (gconst11 * x682);
14060  IkReal x685 =
14061  (gconst10 * x681);
14062  IkReal x686 =
14063  ((1.0) * x681);
14064  IkReal x687 =
14065  (gconst11 * x686);
14066  evalcond[0] =
14067  ((((-1.0) * x687)) +
14068  x683);
14069  evalcond[1] =
14070  (gconst10 +
14071  ((new_r01 *
14072  x682)) +
14073  ((new_r11 *
14074  x681)));
14075  evalcond[2] =
14076  (x685 + x684 +
14077  new_r11);
14078  evalcond[3] =
14079  ((((-1.0) *
14080  new_r01 *
14081  x686)) +
14082  gconst11 +
14083  ((new_r11 *
14084  x682)));
14085  evalcond[4] =
14086  ((((-1.0) * x685)) +
14087  (((-1.0) * x684)));
14088  evalcond[5] =
14089  ((((-1.0) * x687)) +
14090  x683 + new_r01);
14091  if (IKabs(evalcond[0]) >
14092  IKFAST_EVALCOND_THRESH ||
14093  IKabs(evalcond[1]) >
14094  IKFAST_EVALCOND_THRESH ||
14095  IKabs(evalcond[2]) >
14096  IKFAST_EVALCOND_THRESH ||
14097  IKabs(evalcond[3]) >
14098  IKFAST_EVALCOND_THRESH ||
14099  IKabs(evalcond[4]) >
14100  IKFAST_EVALCOND_THRESH ||
14101  IKabs(evalcond[5]) >
14102  IKFAST_EVALCOND_THRESH)
14103  {
14104  continue;
14105  }
14106  }
14107 
14108  {
14109  std::vector<
14110  IkSingleDOFSolutionBase<
14111  IkReal> >
14112  vinfos(7);
14113  vinfos[0].jointtype = 1;
14114  vinfos[0].foffset = j0;
14115  vinfos[0].indices[0] =
14116  _ij0[0];
14117  vinfos[0].indices[1] =
14118  _ij0[1];
14119  vinfos[0].maxsolutions =
14120  _nj0;
14121  vinfos[1].jointtype = 1;
14122  vinfos[1].foffset = j1;
14123  vinfos[1].indices[0] =
14124  _ij1[0];
14125  vinfos[1].indices[1] =
14126  _ij1[1];
14127  vinfos[1].maxsolutions =
14128  _nj1;
14129  vinfos[2].jointtype = 1;
14130  vinfos[2].foffset = j2;
14131  vinfos[2].indices[0] =
14132  _ij2[0];
14133  vinfos[2].indices[1] =
14134  _ij2[1];
14135  vinfos[2].maxsolutions =
14136  _nj2;
14137  vinfos[3].jointtype = 1;
14138  vinfos[3].foffset = j3;
14139  vinfos[3].indices[0] =
14140  _ij3[0];
14141  vinfos[3].indices[1] =
14142  _ij3[1];
14143  vinfos[3].maxsolutions =
14144  _nj3;
14145  vinfos[4].jointtype = 1;
14146  vinfos[4].foffset = j4;
14147  vinfos[4].indices[0] =
14148  _ij4[0];
14149  vinfos[4].indices[1] =
14150  _ij4[1];
14151  vinfos[4].maxsolutions =
14152  _nj4;
14153  vinfos[5].jointtype = 1;
14154  vinfos[5].foffset = j5;
14155  vinfos[5].indices[0] =
14156  _ij5[0];
14157  vinfos[5].indices[1] =
14158  _ij5[1];
14159  vinfos[5].maxsolutions =
14160  _nj5;
14161  vinfos[6].jointtype = 1;
14162  vinfos[6].foffset = j6;
14163  vinfos[6].indices[0] =
14164  _ij6[0];
14165  vinfos[6].indices[1] =
14166  _ij6[1];
14167  vinfos[6].maxsolutions =
14168  _nj6;
14169  std::vector<int> vfree(
14170  0);
14171  solutions.AddSolution(
14172  vinfos, vfree);
14173  }
14174  }
14175  }
14176  }
14177  }
14178  }
14179  else
14180  {
14181  {
14182  IkReal j0array[1], cj0array[1],
14183  sj0array[1];
14184  bool j0valid[1] = { false };
14185  _nj0 = 1;
14186  CheckValue<IkReal> x688 =
14187  IKPowWithIntegerCheck(
14188  IKsign(
14189  ((((-1.0) *
14190  (gconst11 *
14191  gconst11))) +
14192  (((-1.0) *
14193  (gconst10 *
14194  gconst10))))),
14195  -1);
14196  if (!x688.valid)
14197  {
14198  continue;
14199  }
14200  CheckValue<IkReal> x689 =
14201  IKatan2WithCheck(
14202  IkReal((gconst10 *
14203  new_r11)),
14204  IkReal((gconst11 *
14205  new_r11)),
14206  IKFAST_ATAN2_MAGTHRESH);
14207  if (!x689.valid)
14208  {
14209  continue;
14210  }
14211  j0array[0] =
14212  ((-1.5707963267949) +
14213  (((1.5707963267949) *
14214  (x688.value))) +
14215  (x689.value));
14216  sj0array[0] = IKsin(j0array[0]);
14217  cj0array[0] = IKcos(j0array[0]);
14218  if (j0array[0] > IKPI)
14219  {
14220  j0array[0] -= IK2PI;
14221  }
14222  else if (j0array[0] < -IKPI)
14223  {
14224  j0array[0] += IK2PI;
14225  }
14226  j0valid[0] = true;
14227  for (int ij0 = 0; ij0 < 1;
14228  ++ij0)
14229  {
14230  if (!j0valid[ij0])
14231  {
14232  continue;
14233  }
14234  _ij0[0] = ij0;
14235  _ij0[1] = -1;
14236  for (int iij0 = ij0 + 1;
14237  iij0 < 1;
14238  ++iij0)
14239  {
14240  if (j0valid[iij0] &&
14241  IKabs(cj0array[ij0] -
14242  cj0array[iij0]) <
14243  IKFAST_SOLUTION_THRESH &&
14244  IKabs(sj0array[ij0] -
14245  sj0array[iij0]) <
14246  IKFAST_SOLUTION_THRESH)
14247  {
14248  j0valid[iij0] = false;
14249  _ij0[1] = iij0;
14250  break;
14251  }
14252  }
14253  j0 = j0array[ij0];
14254  cj0 = cj0array[ij0];
14255  sj0 = sj0array[ij0];
14256  {
14257  IkReal evalcond[6];
14258  IkReal x690 = IKsin(j0);
14259  IkReal x691 = IKcos(j0);
14260  IkReal x692 =
14261  (gconst10 * x691);
14262  IkReal x693 =
14263  (gconst11 * x691);
14264  IkReal x694 =
14265  (gconst10 * x690);
14266  IkReal x695 =
14267  ((1.0) * x690);
14268  IkReal x696 =
14269  (gconst11 * x695);
14270  evalcond[0] =
14271  (x692 +
14272  (((-1.0) * x696)));
14273  evalcond[1] =
14274  (gconst10 +
14275  ((new_r11 * x690)) +
14276  ((new_r01 * x691)));
14277  evalcond[2] =
14278  (x694 + x693 + new_r11);
14279  evalcond[3] =
14280  ((((-1.0) * new_r01 *
14281  x695)) +
14282  gconst11 +
14283  ((new_r11 * x691)));
14284  evalcond[4] =
14285  ((((-1.0) * x693)) +
14286  (((-1.0) * x694)));
14287  evalcond[5] =
14288  (x692 +
14289  (((-1.0) * x696)) +
14290  new_r01);
14291  if (IKabs(evalcond[0]) >
14292  IKFAST_EVALCOND_THRESH ||
14293  IKabs(evalcond[1]) >
14294  IKFAST_EVALCOND_THRESH ||
14295  IKabs(evalcond[2]) >
14296  IKFAST_EVALCOND_THRESH ||
14297  IKabs(evalcond[3]) >
14298  IKFAST_EVALCOND_THRESH ||
14299  IKabs(evalcond[4]) >
14300  IKFAST_EVALCOND_THRESH ||
14301  IKabs(evalcond[5]) >
14302  IKFAST_EVALCOND_THRESH)
14303  {
14304  continue;
14305  }
14306  }
14307 
14308  {
14309  std::vector<
14310  IkSingleDOFSolutionBase<
14311  IkReal> >
14312  vinfos(7);
14313  vinfos[0].jointtype = 1;
14314  vinfos[0].foffset = j0;
14315  vinfos[0].indices[0] =
14316  _ij0[0];
14317  vinfos[0].indices[1] =
14318  _ij0[1];
14319  vinfos[0].maxsolutions =
14320  _nj0;
14321  vinfos[1].jointtype = 1;
14322  vinfos[1].foffset = j1;
14323  vinfos[1].indices[0] =
14324  _ij1[0];
14325  vinfos[1].indices[1] =
14326  _ij1[1];
14327  vinfos[1].maxsolutions =
14328  _nj1;
14329  vinfos[2].jointtype = 1;
14330  vinfos[2].foffset = j2;
14331  vinfos[2].indices[0] =
14332  _ij2[0];
14333  vinfos[2].indices[1] =
14334  _ij2[1];
14335  vinfos[2].maxsolutions =
14336  _nj2;
14337  vinfos[3].jointtype = 1;
14338  vinfos[3].foffset = j3;
14339  vinfos[3].indices[0] =
14340  _ij3[0];
14341  vinfos[3].indices[1] =
14342  _ij3[1];
14343  vinfos[3].maxsolutions =
14344  _nj3;
14345  vinfos[4].jointtype = 1;
14346  vinfos[4].foffset = j4;
14347  vinfos[4].indices[0] =
14348  _ij4[0];
14349  vinfos[4].indices[1] =
14350  _ij4[1];
14351  vinfos[4].maxsolutions =
14352  _nj4;
14353  vinfos[5].jointtype = 1;
14354  vinfos[5].foffset = j5;
14355  vinfos[5].indices[0] =
14356  _ij5[0];
14357  vinfos[5].indices[1] =
14358  _ij5[1];
14359  vinfos[5].maxsolutions =
14360  _nj5;
14361  vinfos[6].jointtype = 1;
14362  vinfos[6].foffset = j6;
14363  vinfos[6].indices[0] =
14364  _ij6[0];
14365  vinfos[6].indices[1] =
14366  _ij6[1];
14367  vinfos[6].maxsolutions =
14368  _nj6;
14369  std::vector<int> vfree(0);
14370  solutions.AddSolution(
14371  vinfos, vfree);
14372  }
14373  }
14374  }
14375  }
14376  }
14377  }
14378  else
14379  {
14380  {
14381  IkReal j0array[1], cj0array[1],
14382  sj0array[1];
14383  bool j0valid[1] = { false };
14384  _nj0 = 1;
14385  CheckValue<IkReal> x697 =
14386  IKPowWithIntegerCheck(
14387  IKsign((
14388  ((gconst10 * new_r01)) +
14389  (((-1.0) * gconst11 *
14390  new_r11)))),
14391  -1);
14392  if (!x697.valid)
14393  {
14394  continue;
14395  }
14396  CheckValue<IkReal> x698 =
14397  IKatan2WithCheck(
14398  IkReal(
14399  (gconst10 * gconst11)),
14400  IkReal(gconst11 * gconst11),
14401  IKFAST_ATAN2_MAGTHRESH);
14402  if (!x698.valid)
14403  {
14404  continue;
14405  }
14406  j0array[0] = ((-1.5707963267949) +
14407  (((1.5707963267949) *
14408  (x697.value))) +
14409  (x698.value));
14410  sj0array[0] = IKsin(j0array[0]);
14411  cj0array[0] = IKcos(j0array[0]);
14412  if (j0array[0] > IKPI)
14413  {
14414  j0array[0] -= IK2PI;
14415  }
14416  else if (j0array[0] < -IKPI)
14417  {
14418  j0array[0] += IK2PI;
14419  }
14420  j0valid[0] = true;
14421  for (int ij0 = 0; ij0 < 1; ++ij0)
14422  {
14423  if (!j0valid[ij0])
14424  {
14425  continue;
14426  }
14427  _ij0[0] = ij0;
14428  _ij0[1] = -1;
14429  for (int iij0 = ij0 + 1; iij0 < 1;
14430  ++iij0)
14431  {
14432  if (j0valid[iij0] &&
14433  IKabs(cj0array[ij0] -
14434  cj0array[iij0]) <
14435  IKFAST_SOLUTION_THRESH &&
14436  IKabs(sj0array[ij0] -
14437  sj0array[iij0]) <
14438  IKFAST_SOLUTION_THRESH)
14439  {
14440  j0valid[iij0] = false;
14441  _ij0[1] = iij0;
14442  break;
14443  }
14444  }
14445  j0 = j0array[ij0];
14446  cj0 = cj0array[ij0];
14447  sj0 = sj0array[ij0];
14448  {
14449  IkReal evalcond[6];
14450  IkReal x699 = IKsin(j0);
14451  IkReal x700 = IKcos(j0);
14452  IkReal x701 = (gconst10 * x700);
14453  IkReal x702 = (gconst11 * x700);
14454  IkReal x703 = (gconst10 * x699);
14455  IkReal x704 = ((1.0) * x699);
14456  IkReal x705 = (gconst11 * x704);
14457  evalcond[0] =
14458  ((((-1.0) * x705)) + x701);
14459  evalcond[1] =
14460  (((new_r01 * x700)) +
14461  gconst10 +
14462  ((new_r11 * x699)));
14463  evalcond[2] =
14464  (x702 + x703 + new_r11);
14465  evalcond[3] =
14466  (gconst11 +
14467  ((new_r11 * x700)) +
14468  (((-1.0) * new_r01 *
14469  x704)));
14470  evalcond[4] =
14471  ((((-1.0) * x702)) +
14472  (((-1.0) * x703)));
14473  evalcond[5] =
14474  ((((-1.0) * x705)) + x701 +
14475  new_r01);
14476  if (IKabs(evalcond[0]) >
14477  IKFAST_EVALCOND_THRESH ||
14478  IKabs(evalcond[1]) >
14479  IKFAST_EVALCOND_THRESH ||
14480  IKabs(evalcond[2]) >
14481  IKFAST_EVALCOND_THRESH ||
14482  IKabs(evalcond[3]) >
14483  IKFAST_EVALCOND_THRESH ||
14484  IKabs(evalcond[4]) >
14485  IKFAST_EVALCOND_THRESH ||
14486  IKabs(evalcond[5]) >
14487  IKFAST_EVALCOND_THRESH)
14488  {
14489  continue;
14490  }
14491  }
14492 
14493  {
14494  std::vector<
14495  IkSingleDOFSolutionBase<
14496  IkReal> >
14497  vinfos(7);
14498  vinfos[0].jointtype = 1;
14499  vinfos[0].foffset = j0;
14500  vinfos[0].indices[0] = _ij0[0];
14501  vinfos[0].indices[1] = _ij0[1];
14502  vinfos[0].maxsolutions = _nj0;
14503  vinfos[1].jointtype = 1;
14504  vinfos[1].foffset = j1;
14505  vinfos[1].indices[0] = _ij1[0];
14506  vinfos[1].indices[1] = _ij1[1];
14507  vinfos[1].maxsolutions = _nj1;
14508  vinfos[2].jointtype = 1;
14509  vinfos[2].foffset = j2;
14510  vinfos[2].indices[0] = _ij2[0];
14511  vinfos[2].indices[1] = _ij2[1];
14512  vinfos[2].maxsolutions = _nj2;
14513  vinfos[3].jointtype = 1;
14514  vinfos[3].foffset = j3;
14515  vinfos[3].indices[0] = _ij3[0];
14516  vinfos[3].indices[1] = _ij3[1];
14517  vinfos[3].maxsolutions = _nj3;
14518  vinfos[4].jointtype = 1;
14519  vinfos[4].foffset = j4;
14520  vinfos[4].indices[0] = _ij4[0];
14521  vinfos[4].indices[1] = _ij4[1];
14522  vinfos[4].maxsolutions = _nj4;
14523  vinfos[5].jointtype = 1;
14524  vinfos[5].foffset = j5;
14525  vinfos[5].indices[0] = _ij5[0];
14526  vinfos[5].indices[1] = _ij5[1];
14527  vinfos[5].maxsolutions = _nj5;
14528  vinfos[6].jointtype = 1;
14529  vinfos[6].foffset = j6;
14530  vinfos[6].indices[0] = _ij6[0];
14531  vinfos[6].indices[1] = _ij6[1];
14532  vinfos[6].maxsolutions = _nj6;
14533  std::vector<int> vfree(0);
14534  solutions.AddSolution(vinfos,
14535  vfree);
14536  }
14537  }
14538  }
14539  }
14540  }
14541  }
14542  } while (0);
14543  if (bgotonextstatement)
14544  {
14545  bool bgotonextstatement = true;
14546  do
14547  {
14548  evalcond[0] =
14549  ((IKabs(new_r10)) + (IKabs(new_r01)));
14550  if (IKabs(evalcond[0]) <
14551  0.0000050000000000)
14552  {
14553  bgotonextstatement = false;
14554  {
14555  IkReal j0array[2], cj0array[2],
14556  sj0array[2];
14557  bool j0valid[2] = { false };
14558  _nj0 = 2;
14559  CheckValue<IkReal> x706 =
14560  IKPowWithIntegerCheck(gconst11,
14561  -1);
14562  if (!x706.valid)
14563  {
14564  continue;
14565  }
14566  cj0array[0] =
14567  (new_r00 * (x706.value));
14568  if (cj0array[0] >=
14569  -1 - IKFAST_SINCOS_THRESH &&
14570  cj0array[0] <=
14571  1 + IKFAST_SINCOS_THRESH)
14572  {
14573  j0valid[0] = j0valid[1] = true;
14574  j0array[0] = IKacos(cj0array[0]);
14575  sj0array[0] = IKsin(j0array[0]);
14576  cj0array[1] = cj0array[0];
14577  j0array[1] = -j0array[0];
14578  sj0array[1] = -sj0array[0];
14579  }
14580  else if (isnan(cj0array[0]))
14581  {
14582  // probably any value will work
14583  j0valid[0] = true;
14584  cj0array[0] = 1;
14585  sj0array[0] = 0;
14586  j0array[0] = 0;
14587  }
14588  for (int ij0 = 0; ij0 < 2; ++ij0)
14589  {
14590  if (!j0valid[ij0])
14591  {
14592  continue;
14593  }
14594  _ij0[0] = ij0;
14595  _ij0[1] = -1;
14596  for (int iij0 = ij0 + 1; iij0 < 2;
14597  ++iij0)
14598  {
14599  if (j0valid[iij0] &&
14600  IKabs(cj0array[ij0] -
14601  cj0array[iij0]) <
14602  IKFAST_SOLUTION_THRESH &&
14603  IKabs(sj0array[ij0] -
14604  sj0array[iij0]) <
14605  IKFAST_SOLUTION_THRESH)
14606  {
14607  j0valid[iij0] = false;
14608  _ij0[1] = iij0;
14609  break;
14610  }
14611  }
14612  j0 = j0array[ij0];
14613  cj0 = cj0array[ij0];
14614  sj0 = sj0array[ij0];
14615  {
14616  IkReal evalcond[6];
14617  IkReal x707 = IKsin(j0);
14618  IkReal x708 = IKcos(j0);
14619  IkReal x709 = ((-1.0) * x707);
14620  evalcond[0] = (new_r11 * x707);
14621  evalcond[1] = (new_r00 * x709);
14622  evalcond[2] = (gconst11 * x709);
14623  evalcond[3] =
14624  (gconst11 +
14625  ((new_r11 * x708)));
14626  evalcond[4] =
14627  (new_r11 +
14628  ((gconst11 * x708)));
14629  evalcond[5] =
14630  (((new_r00 * x708)) +
14631  (((-1.0) * gconst11)));
14632  if (IKabs(evalcond[0]) >
14633  IKFAST_EVALCOND_THRESH ||
14634  IKabs(evalcond[1]) >
14635  IKFAST_EVALCOND_THRESH ||
14636  IKabs(evalcond[2]) >
14637  IKFAST_EVALCOND_THRESH ||
14638  IKabs(evalcond[3]) >
14639  IKFAST_EVALCOND_THRESH ||
14640  IKabs(evalcond[4]) >
14641  IKFAST_EVALCOND_THRESH ||
14642  IKabs(evalcond[5]) >
14643  IKFAST_EVALCOND_THRESH)
14644  {
14645  continue;
14646  }
14647  }
14648 
14649  {
14650  std::vector<
14651  IkSingleDOFSolutionBase<
14652  IkReal> >
14653  vinfos(7);
14654  vinfos[0].jointtype = 1;
14655  vinfos[0].foffset = j0;
14656  vinfos[0].indices[0] = _ij0[0];
14657  vinfos[0].indices[1] = _ij0[1];
14658  vinfos[0].maxsolutions = _nj0;
14659  vinfos[1].jointtype = 1;
14660  vinfos[1].foffset = j1;
14661  vinfos[1].indices[0] = _ij1[0];
14662  vinfos[1].indices[1] = _ij1[1];
14663  vinfos[1].maxsolutions = _nj1;
14664  vinfos[2].jointtype = 1;
14665  vinfos[2].foffset = j2;
14666  vinfos[2].indices[0] = _ij2[0];
14667  vinfos[2].indices[1] = _ij2[1];
14668  vinfos[2].maxsolutions = _nj2;
14669  vinfos[3].jointtype = 1;
14670  vinfos[3].foffset = j3;
14671  vinfos[3].indices[0] = _ij3[0];
14672  vinfos[3].indices[1] = _ij3[1];
14673  vinfos[3].maxsolutions = _nj3;
14674  vinfos[4].jointtype = 1;
14675  vinfos[4].foffset = j4;
14676  vinfos[4].indices[0] = _ij4[0];
14677  vinfos[4].indices[1] = _ij4[1];
14678  vinfos[4].maxsolutions = _nj4;
14679  vinfos[5].jointtype = 1;
14680  vinfos[5].foffset = j5;
14681  vinfos[5].indices[0] = _ij5[0];
14682  vinfos[5].indices[1] = _ij5[1];
14683  vinfos[5].maxsolutions = _nj5;
14684  vinfos[6].jointtype = 1;
14685  vinfos[6].foffset = j6;
14686  vinfos[6].indices[0] = _ij6[0];
14687  vinfos[6].indices[1] = _ij6[1];
14688  vinfos[6].maxsolutions = _nj6;
14689  std::vector<int> vfree(0);
14690  solutions.AddSolution(vinfos,
14691  vfree);
14692  }
14693  }
14694  }
14695  }
14696  } while (0);
14697  if (bgotonextstatement)
14698  {
14699  bool bgotonextstatement = true;
14700  do
14701  {
14702  evalcond[0] = ((IKabs(new_r00)) +
14703  (IKabs(new_r01)));
14704  if (IKabs(evalcond[0]) <
14705  0.0000050000000000)
14706  {
14707  bgotonextstatement = false;
14708  {
14709  IkReal j0eval[1];
14710  CheckValue<IkReal> x711 =
14711  IKatan2WithCheck(
14712  IkReal(0),
14713  IkReal(new_r11),
14714  IKFAST_ATAN2_MAGTHRESH);
14715  if (!x711.valid)
14716  {
14717  continue;
14718  }
14719  IkReal x710 =
14720  ((1.0) * (x711.value));
14721  sj1 = 0;
14722  cj1 = -1.0;
14723  j1 = 3.14159265358979;
14724  sj2 = gconst10;
14725  cj2 = gconst11;
14726  j2 = ((3.14159265) +
14727  (((-1.0) * x710)));
14728  new_r00 = 0;
14729  new_r01 = 0;
14730  new_r12 = 0;
14731  new_r22 = 0;
14732  IkReal gconst9 =
14733  ((3.14159265358979) +
14734  (((-1.0) * x710)));
14735  IkReal gconst10 = 0;
14736  IkReal x712 =
14737  ((1.0) +
14738  (((-1.0) *
14739  (new_r10 * new_r10))));
14740  if (IKabs(x712) == 0)
14741  {
14742  continue;
14743  }
14744  IkReal gconst11 =
14745  ((-1.0) * new_r11 *
14746  (pow(x712, -0.5)));
14747  j0eval[0] = ((IKabs(new_r11)) +
14748  (IKabs(new_r10)));
14749  if (IKabs(j0eval[0]) <
14750  0.0000010000000000)
14751  {
14752  {
14753  IkReal j0eval[1];
14754  CheckValue<IkReal> x714 =
14755  IKatan2WithCheck(
14756  IkReal(0),
14757  IkReal(new_r11),
14758  IKFAST_ATAN2_MAGTHRESH);
14759  if (!x714.valid)
14760  {
14761  continue;
14762  }
14763  IkReal x713 =
14764  ((1.0) * (x714.value));
14765  sj1 = 0;
14766  cj1 = -1.0;
14767  j1 = 3.14159265358979;
14768  sj2 = gconst10;
14769  cj2 = gconst11;
14770  j2 = ((3.14159265) +
14771  (((-1.0) * x713)));
14772  new_r00 = 0;
14773  new_r01 = 0;
14774  new_r12 = 0;
14775  new_r22 = 0;
14776  IkReal gconst9 =
14777  ((3.14159265358979) +
14778  (((-1.0) * x713)));
14779  IkReal gconst10 = 0;
14780  IkReal x715 =
14781  ((1.0) +
14782  (((-1.0) *
14783  (new_r10 * new_r10))));
14784  if (IKabs(x715) == 0)
14785  {
14786  continue;
14787  }
14788  IkReal gconst11 =
14789  ((-1.0) * new_r11 *
14790  (pow(x715, -0.5)));
14791  j0eval[0] = new_r11;
14792  if (IKabs(j0eval[0]) <
14793  0.0000010000000000)
14794  {
14795  {
14796  IkReal j0eval[2];
14797  CheckValue<IkReal> x717 =
14798  IKatan2WithCheck(
14799  IkReal(0),
14800  IkReal(new_r11),
14801  IKFAST_ATAN2_MAGTHRESH);
14802  if (!x717.valid)
14803  {
14804  continue;
14805  }
14806  IkReal x716 =
14807  ((1.0) * (x717.value));
14808  sj1 = 0;
14809  cj1 = -1.0;
14810  j1 = 3.14159265358979;
14811  sj2 = gconst10;
14812  cj2 = gconst11;
14813  j2 = ((3.14159265) +
14814  (((-1.0) * x716)));
14815  new_r00 = 0;
14816  new_r01 = 0;
14817  new_r12 = 0;
14818  new_r22 = 0;
14819  IkReal gconst9 =
14820  ((3.14159265358979) +
14821  (((-1.0) * x716)));
14822  IkReal gconst10 = 0;
14823  IkReal x718 =
14824  ((1.0) + (((-1.0) *
14825  (new_r10 *
14826  new_r10))));
14827  if (IKabs(x718) == 0)
14828  {
14829  continue;
14830  }
14831  IkReal gconst11 =
14832  ((-1.0) * new_r11 *
14833  (pow(x718, -0.5)));
14834  j0eval[0] = new_r10;
14835  j0eval[1] = new_r11;
14836  if (IKabs(j0eval[0]) <
14837  0.0000010000000000 ||
14838  IKabs(j0eval[1]) <
14839  0.0000010000000000)
14840  {
14841  continue; // 3 cases
14842  // reached
14843  }
14844  else
14845  {
14846  {
14847  IkReal j0array[1],
14848  cj0array[1],
14849  sj0array[1];
14850  bool j0valid[1] = {
14851  false
14852  };
14853  _nj0 = 1;
14854  CheckValue<IkReal> x719 =
14855  IKPowWithIntegerCheck(
14856  new_r10, -1);
14857  if (!x719.valid)
14858  {
14859  continue;
14860  }
14861  CheckValue<IkReal> x720 =
14862  IKPowWithIntegerCheck(
14863  new_r11, -1);
14864  if (!x720.valid)
14865  {
14866  continue;
14867  }
14868  if (IKabs((
14869  gconst11 *
14870  (x719.value))) <
14871  IKFAST_ATAN2_MAGTHRESH &&
14872  IKabs((
14873  (-1.0) *
14874  gconst11 *
14875  (x720.value))) <
14876  IKFAST_ATAN2_MAGTHRESH &&
14877  IKabs(
14878  IKsqr((
14879  gconst11 *
14880  (x719.value))) +
14881  IKsqr((
14882  (-1.0) *
14883  gconst11 *
14884  (x720.value))) -
14885  1) <=
14886  IKFAST_SINCOS_THRESH)
14887  continue;
14888  j0array[0] = IKatan2(
14889  (gconst11 *
14890  (x719.value)),
14891  ((-1.0) * gconst11 *
14892  (x720.value)));
14893  sj0array[0] =
14894  IKsin(j0array[0]);
14895  cj0array[0] =
14896  IKcos(j0array[0]);
14897  if (j0array[0] > IKPI)
14898  {
14899  j0array[0] -= IK2PI;
14900  }
14901  else if (j0array[0] <
14902  -IKPI)
14903  {
14904  j0array[0] += IK2PI;
14905  }
14906  j0valid[0] = true;
14907  for (int ij0 = 0;
14908  ij0 < 1;
14909  ++ij0)
14910  {
14911  if (!j0valid[ij0])
14912  {
14913  continue;
14914  }
14915  _ij0[0] = ij0;
14916  _ij0[1] = -1;
14917  for (int iij0 =
14918  ij0 + 1;
14919  iij0 < 1;
14920  ++iij0)
14921  {
14922  if (j0valid[iij0] &&
14923  IKabs(
14924  cj0array
14925  [ij0] -
14926  cj0array
14927  [iij0]) <
14928  IKFAST_SOLUTION_THRESH &&
14929  IKabs(
14930  sj0array
14931  [ij0] -
14932  sj0array
14933  [iij0]) <
14934  IKFAST_SOLUTION_THRESH)
14935  {
14936  j0valid[iij0] =
14937  false;
14938  _ij0[1] = iij0;
14939  break;
14940  }
14941  }
14942  j0 = j0array[ij0];
14943  cj0 = cj0array[ij0];
14944  sj0 = sj0array[ij0];
14945  {
14946  IkReal evalcond[8];
14947  IkReal x721 =
14948  IKcos(j0);
14949  IkReal x722 =
14950  IKsin(j0);
14951  IkReal x723 =
14952  ((1.0) *
14953  gconst11);
14954  IkReal x724 =
14955  ((-1.0) *
14956  gconst11);
14957  evalcond[0] =
14958  (new_r10 *
14959  x721);
14960  evalcond[1] =
14961  (new_r11 *
14962  x722);
14963  evalcond[2] =
14964  (x721 * x724);
14965  evalcond[3] =
14966  (x722 * x724);
14967  evalcond[4] =
14968  (gconst11 +
14969  ((new_r11 *
14970  x721)));
14971  evalcond[5] =
14972  (((gconst11 *
14973  x721)) +
14974  new_r11);
14975  evalcond[6] =
14976  ((((-1.0) *
14977  x722 *
14978  x723)) +
14979  new_r10);
14980  evalcond[7] =
14981  ((((-1.0) *
14982  x723)) +
14983  ((new_r10 *
14984  x722)));
14985  if (IKabs(evalcond
14986  [0]) >
14987  IKFAST_EVALCOND_THRESH ||
14988  IKabs(evalcond
14989  [1]) >
14990  IKFAST_EVALCOND_THRESH ||
14991  IKabs(evalcond
14992  [2]) >
14993  IKFAST_EVALCOND_THRESH ||
14994  IKabs(evalcond
14995  [3]) >
14996  IKFAST_EVALCOND_THRESH ||
14997  IKabs(evalcond
14998  [4]) >
14999  IKFAST_EVALCOND_THRESH ||
15000  IKabs(evalcond
15001  [5]) >
15002  IKFAST_EVALCOND_THRESH ||
15003  IKabs(evalcond
15004  [6]) >
15005  IKFAST_EVALCOND_THRESH ||
15006  IKabs(evalcond
15007  [7]) >
15008  IKFAST_EVALCOND_THRESH)
15009  {
15010  continue;
15011  }
15012  }
15013 
15014  {
15015  std::vector<
15016  IkSingleDOFSolutionBase<
15017  IkReal> >
15018  vinfos(7);
15019  vinfos[0]
15020  .jointtype = 1;
15021  vinfos[0].foffset =
15022  j0;
15023  vinfos[0]
15024  .indices[0] =
15025  _ij0[0];
15026  vinfos[0]
15027  .indices[1] =
15028  _ij0[1];
15029  vinfos[0]
15030  .maxsolutions =
15031  _nj0;
15032  vinfos[1]
15033  .jointtype = 1;
15034  vinfos[1].foffset =
15035  j1;
15036  vinfos[1]
15037  .indices[0] =
15038  _ij1[0];
15039  vinfos[1]
15040  .indices[1] =
15041  _ij1[1];
15042  vinfos[1]
15043  .maxsolutions =
15044  _nj1;
15045  vinfos[2]
15046  .jointtype = 1;
15047  vinfos[2].foffset =
15048  j2;
15049  vinfos[2]
15050  .indices[0] =
15051  _ij2[0];
15052  vinfos[2]
15053  .indices[1] =
15054  _ij2[1];
15055  vinfos[2]
15056  .maxsolutions =
15057  _nj2;
15058  vinfos[3]
15059  .jointtype = 1;
15060  vinfos[3].foffset =
15061  j3;
15062  vinfos[3]
15063  .indices[0] =
15064  _ij3[0];
15065  vinfos[3]
15066  .indices[1] =
15067  _ij3[1];
15068  vinfos[3]
15069  .maxsolutions =
15070  _nj3;
15071  vinfos[4]
15072  .jointtype = 1;
15073  vinfos[4].foffset =
15074  j4;
15075  vinfos[4]
15076  .indices[0] =
15077  _ij4[0];
15078  vinfos[4]
15079  .indices[1] =
15080  _ij4[1];
15081  vinfos[4]
15082  .maxsolutions =
15083  _nj4;
15084  vinfos[5]
15085  .jointtype = 1;
15086  vinfos[5].foffset =
15087  j5;
15088  vinfos[5]
15089  .indices[0] =
15090  _ij5[0];
15091  vinfos[5]
15092  .indices[1] =
15093  _ij5[1];
15094  vinfos[5]
15095  .maxsolutions =
15096  _nj5;
15097  vinfos[6]
15098  .jointtype = 1;
15099  vinfos[6].foffset =
15100  j6;
15101  vinfos[6]
15102  .indices[0] =
15103  _ij6[0];
15104  vinfos[6]
15105  .indices[1] =
15106  _ij6[1];
15107  vinfos[6]
15108  .maxsolutions =
15109  _nj6;
15110  std::vector<int>
15111  vfree(0);
15112  solutions
15113  .AddSolution(
15114  vinfos,
15115  vfree);
15116  }
15117  }
15118  }
15119  }
15120  }
15121  }
15122  else
15123  {
15124  {
15125  IkReal j0array[1],
15126  cj0array[1],
15127  sj0array[1];
15128  bool j0valid[1] = { false };
15129  _nj0 = 1;
15130  CheckValue<IkReal> x725 =
15131  IKPowWithIntegerCheck(
15132  gconst11, -1);
15133  if (!x725.valid)
15134  {
15135  continue;
15136  }
15137  CheckValue<IkReal> x726 =
15138  IKPowWithIntegerCheck(
15139  new_r11, -1);
15140  if (!x726.valid)
15141  {
15142  continue;
15143  }
15144  if (IKabs((new_r10 *
15145  (x725.value))) <
15146  IKFAST_ATAN2_MAGTHRESH &&
15147  IKabs(
15148  ((-1.0) * gconst11 *
15149  (x726.value))) <
15150  IKFAST_ATAN2_MAGTHRESH &&
15151  IKabs(
15152  IKsqr((
15153  new_r10 *
15154  (x725.value))) +
15155  IKsqr((
15156  (-1.0) *
15157  gconst11 *
15158  (x726.value))) -
15159  1) <=
15160  IKFAST_SINCOS_THRESH)
15161  continue;
15162  j0array[0] = IKatan2(
15163  (new_r10 *
15164  (x725.value)),
15165  ((-1.0) * gconst11 *
15166  (x726.value)));
15167  sj0array[0] =
15168  IKsin(j0array[0]);
15169  cj0array[0] =
15170  IKcos(j0array[0]);
15171  if (j0array[0] > IKPI)
15172  {
15173  j0array[0] -= IK2PI;
15174  }
15175  else if (j0array[0] < -IKPI)
15176  {
15177  j0array[0] += IK2PI;
15178  }
15179  j0valid[0] = true;
15180  for (int ij0 = 0; ij0 < 1;
15181  ++ij0)
15182  {
15183  if (!j0valid[ij0])
15184  {
15185  continue;
15186  }
15187  _ij0[0] = ij0;
15188  _ij0[1] = -1;
15189  for (int iij0 = ij0 + 1;
15190  iij0 < 1;
15191  ++iij0)
15192  {
15193  if (j0valid[iij0] &&
15194  IKabs(
15195  cj0array[ij0] -
15196  cj0array
15197  [iij0]) <
15198  IKFAST_SOLUTION_THRESH &&
15199  IKabs(
15200  sj0array[ij0] -
15201  sj0array
15202  [iij0]) <
15203  IKFAST_SOLUTION_THRESH)
15204  {
15205  j0valid[iij0] = false;
15206  _ij0[1] = iij0;
15207  break;
15208  }
15209  }
15210  j0 = j0array[ij0];
15211  cj0 = cj0array[ij0];
15212  sj0 = sj0array[ij0];
15213  {
15214  IkReal evalcond[8];
15215  IkReal x727 = IKcos(j0);
15216  IkReal x728 = IKsin(j0);
15217  IkReal x729 =
15218  ((1.0) * gconst11);
15219  IkReal x730 =
15220  ((-1.0) * gconst11);
15221  evalcond[0] =
15222  (new_r10 * x727);
15223  evalcond[1] =
15224  (new_r11 * x728);
15225  evalcond[2] =
15226  (x727 * x730);
15227  evalcond[3] =
15228  (x728 * x730);
15229  evalcond[4] =
15230  (gconst11 +
15231  ((new_r11 *
15232  x727)));
15233  evalcond[5] =
15234  (((gconst11 *
15235  x727)) +
15236  new_r11);
15237  evalcond[6] =
15238  (new_r10 +
15239  (((-1.0) * x728 *
15240  x729)));
15241  evalcond[7] =
15242  ((((-1.0) * x729)) +
15243  ((new_r10 *
15244  x728)));
15245  if (IKabs(evalcond[0]) >
15246  IKFAST_EVALCOND_THRESH ||
15247  IKabs(evalcond[1]) >
15248  IKFAST_EVALCOND_THRESH ||
15249  IKabs(evalcond[2]) >
15250  IKFAST_EVALCOND_THRESH ||
15251  IKabs(evalcond[3]) >
15252  IKFAST_EVALCOND_THRESH ||
15253  IKabs(evalcond[4]) >
15254  IKFAST_EVALCOND_THRESH ||
15255  IKabs(evalcond[5]) >
15256  IKFAST_EVALCOND_THRESH ||
15257  IKabs(evalcond[6]) >
15258  IKFAST_EVALCOND_THRESH ||
15259  IKabs(evalcond[7]) >
15260  IKFAST_EVALCOND_THRESH)
15261  {
15262  continue;
15263  }
15264  }
15265 
15266  {
15267  std::vector<
15268  IkSingleDOFSolutionBase<
15269  IkReal> >
15270  vinfos(7);
15271  vinfos[0].jointtype = 1;
15272  vinfos[0].foffset = j0;
15273  vinfos[0].indices[0] =
15274  _ij0[0];
15275  vinfos[0].indices[1] =
15276  _ij0[1];
15277  vinfos[0].maxsolutions =
15278  _nj0;
15279  vinfos[1].jointtype = 1;
15280  vinfos[1].foffset = j1;
15281  vinfos[1].indices[0] =
15282  _ij1[0];
15283  vinfos[1].indices[1] =
15284  _ij1[1];
15285  vinfos[1].maxsolutions =
15286  _nj1;
15287  vinfos[2].jointtype = 1;
15288  vinfos[2].foffset = j2;
15289  vinfos[2].indices[0] =
15290  _ij2[0];
15291  vinfos[2].indices[1] =
15292  _ij2[1];
15293  vinfos[2].maxsolutions =
15294  _nj2;
15295  vinfos[3].jointtype = 1;
15296  vinfos[3].foffset = j3;
15297  vinfos[3].indices[0] =
15298  _ij3[0];
15299  vinfos[3].indices[1] =
15300  _ij3[1];
15301  vinfos[3].maxsolutions =
15302  _nj3;
15303  vinfos[4].jointtype = 1;
15304  vinfos[4].foffset = j4;
15305  vinfos[4].indices[0] =
15306  _ij4[0];
15307  vinfos[4].indices[1] =
15308  _ij4[1];
15309  vinfos[4].maxsolutions =
15310  _nj4;
15311  vinfos[5].jointtype = 1;
15312  vinfos[5].foffset = j5;
15313  vinfos[5].indices[0] =
15314  _ij5[0];
15315  vinfos[5].indices[1] =
15316  _ij5[1];
15317  vinfos[5].maxsolutions =
15318  _nj5;
15319  vinfos[6].jointtype = 1;
15320  vinfos[6].foffset = j6;
15321  vinfos[6].indices[0] =
15322  _ij6[0];
15323  vinfos[6].indices[1] =
15324  _ij6[1];
15325  vinfos[6].maxsolutions =
15326  _nj6;
15327  std::vector<int> vfree(
15328  0);
15329  solutions.AddSolution(
15330  vinfos, vfree);
15331  }
15332  }
15333  }
15334  }
15335  }
15336  }
15337  else
15338  {
15339  {
15340  IkReal j0array[1], cj0array[1],
15341  sj0array[1];
15342  bool j0valid[1] = { false };
15343  _nj0 = 1;
15344  CheckValue<IkReal> x731 =
15345  IKPowWithIntegerCheck(
15346  IKsign(gconst11), -1);
15347  if (!x731.valid)
15348  {
15349  continue;
15350  }
15351  CheckValue<IkReal> x732 =
15352  IKatan2WithCheck(
15353  IkReal(new_r10),
15354  IkReal(
15355  ((-1.0) * new_r11)),
15356  IKFAST_ATAN2_MAGTHRESH);
15357  if (!x732.valid)
15358  {
15359  continue;
15360  }
15361  j0array[0] =
15362  ((-1.5707963267949) +
15363  (((1.5707963267949) *
15364  (x731.value))) +
15365  (x732.value));
15366  sj0array[0] = IKsin(j0array[0]);
15367  cj0array[0] = IKcos(j0array[0]);
15368  if (j0array[0] > IKPI)
15369  {
15370  j0array[0] -= IK2PI;
15371  }
15372  else if (j0array[0] < -IKPI)
15373  {
15374  j0array[0] += IK2PI;
15375  }
15376  j0valid[0] = true;
15377  for (int ij0 = 0; ij0 < 1;
15378  ++ij0)
15379  {
15380  if (!j0valid[ij0])
15381  {
15382  continue;
15383  }
15384  _ij0[0] = ij0;
15385  _ij0[1] = -1;
15386  for (int iij0 = ij0 + 1;
15387  iij0 < 1;
15388  ++iij0)
15389  {
15390  if (j0valid[iij0] &&
15391  IKabs(cj0array[ij0] -
15392  cj0array[iij0]) <
15393  IKFAST_SOLUTION_THRESH &&
15394  IKabs(sj0array[ij0] -
15395  sj0array[iij0]) <
15396  IKFAST_SOLUTION_THRESH)
15397  {
15398  j0valid[iij0] = false;
15399  _ij0[1] = iij0;
15400  break;
15401  }
15402  }
15403  j0 = j0array[ij0];
15404  cj0 = cj0array[ij0];
15405  sj0 = sj0array[ij0];
15406  {
15407  IkReal evalcond[8];
15408  IkReal x733 = IKcos(j0);
15409  IkReal x734 = IKsin(j0);
15410  IkReal x735 =
15411  ((1.0) * gconst11);
15412  IkReal x736 =
15413  ((-1.0) * gconst11);
15414  evalcond[0] =
15415  (new_r10 * x733);
15416  evalcond[1] =
15417  (new_r11 * x734);
15418  evalcond[2] = (x733 * x736);
15419  evalcond[3] = (x734 * x736);
15420  evalcond[4] =
15421  (gconst11 +
15422  ((new_r11 * x733)));
15423  evalcond[5] =
15424  (new_r11 +
15425  ((gconst11 * x733)));
15426  evalcond[6] =
15427  ((((-1.0) * x734 *
15428  x735)) +
15429  new_r10);
15430  evalcond[7] =
15431  ((((-1.0) * x735)) +
15432  ((new_r10 * x734)));
15433  if (IKabs(evalcond[0]) >
15434  IKFAST_EVALCOND_THRESH ||
15435  IKabs(evalcond[1]) >
15436  IKFAST_EVALCOND_THRESH ||
15437  IKabs(evalcond[2]) >
15438  IKFAST_EVALCOND_THRESH ||
15439  IKabs(evalcond[3]) >
15440  IKFAST_EVALCOND_THRESH ||
15441  IKabs(evalcond[4]) >
15442  IKFAST_EVALCOND_THRESH ||
15443  IKabs(evalcond[5]) >
15444  IKFAST_EVALCOND_THRESH ||
15445  IKabs(evalcond[6]) >
15446  IKFAST_EVALCOND_THRESH ||
15447  IKabs(evalcond[7]) >
15448  IKFAST_EVALCOND_THRESH)
15449  {
15450  continue;
15451  }
15452  }
15453 
15454  {
15455  std::vector<
15456  IkSingleDOFSolutionBase<
15457  IkReal> >
15458  vinfos(7);
15459  vinfos[0].jointtype = 1;
15460  vinfos[0].foffset = j0;
15461  vinfos[0].indices[0] =
15462  _ij0[0];
15463  vinfos[0].indices[1] =
15464  _ij0[1];
15465  vinfos[0].maxsolutions =
15466  _nj0;
15467  vinfos[1].jointtype = 1;
15468  vinfos[1].foffset = j1;
15469  vinfos[1].indices[0] =
15470  _ij1[0];
15471  vinfos[1].indices[1] =
15472  _ij1[1];
15473  vinfos[1].maxsolutions =
15474  _nj1;
15475  vinfos[2].jointtype = 1;
15476  vinfos[2].foffset = j2;
15477  vinfos[2].indices[0] =
15478  _ij2[0];
15479  vinfos[2].indices[1] =
15480  _ij2[1];
15481  vinfos[2].maxsolutions =
15482  _nj2;
15483  vinfos[3].jointtype = 1;
15484  vinfos[3].foffset = j3;
15485  vinfos[3].indices[0] =
15486  _ij3[0];
15487  vinfos[3].indices[1] =
15488  _ij3[1];
15489  vinfos[3].maxsolutions =
15490  _nj3;
15491  vinfos[4].jointtype = 1;
15492  vinfos[4].foffset = j4;
15493  vinfos[4].indices[0] =
15494  _ij4[0];
15495  vinfos[4].indices[1] =
15496  _ij4[1];
15497  vinfos[4].maxsolutions =
15498  _nj4;
15499  vinfos[5].jointtype = 1;
15500  vinfos[5].foffset = j5;
15501  vinfos[5].indices[0] =
15502  _ij5[0];
15503  vinfos[5].indices[1] =
15504  _ij5[1];
15505  vinfos[5].maxsolutions =
15506  _nj5;
15507  vinfos[6].jointtype = 1;
15508  vinfos[6].foffset = j6;
15509  vinfos[6].indices[0] =
15510  _ij6[0];
15511  vinfos[6].indices[1] =
15512  _ij6[1];
15513  vinfos[6].maxsolutions =
15514  _nj6;
15515  std::vector<int> vfree(0);
15516  solutions.AddSolution(
15517  vinfos, vfree);
15518  }
15519  }
15520  }
15521  }
15522  }
15523  }
15524  } while (0);
15525  if (bgotonextstatement)
15526  {
15527  bool bgotonextstatement = true;
15528  do
15529  {
15530  evalcond[0] = IKabs(new_r01);
15531  if (IKabs(evalcond[0]) <
15532  0.0000050000000000)
15533  {
15534  bgotonextstatement = false;
15535  {
15536  IkReal j0eval[1];
15537  CheckValue<IkReal> x738 =
15538  IKatan2WithCheck(
15539  IkReal(0),
15540  IkReal(new_r11),
15541  IKFAST_ATAN2_MAGTHRESH);
15542  if (!x738.valid)
15543  {
15544  continue;
15545  }
15546  IkReal x737 =
15547  ((1.0) * (x738.value));
15548  sj1 = 0;
15549  cj1 = -1.0;
15550  j1 = 3.14159265358979;
15551  sj2 = gconst10;
15552  cj2 = gconst11;
15553  j2 = ((3.14159265) +
15554  (((-1.0) * x737)));
15555  new_r01 = 0;
15556  IkReal gconst9 =
15557  ((3.14159265358979) +
15558  (((-1.0) * x737)));
15559  IkReal gconst10 = 0;
15560  IkReal x739 = new_r11 * new_r11;
15561  if (IKabs(x739) == 0)
15562  {
15563  continue;
15564  }
15565  IkReal gconst11 =
15566  ((-1.0) * new_r11 *
15567  (pow(x739, -0.5)));
15568  j0eval[0] = ((IKabs(new_r10)) +
15569  (IKabs(new_r00)));
15570  if (IKabs(j0eval[0]) <
15571  0.0000010000000000)
15572  {
15573  {
15574  IkReal j0eval[1];
15575  CheckValue<IkReal> x741 =
15576  IKatan2WithCheck(
15577  IkReal(0),
15578  IkReal(new_r11),
15579  IKFAST_ATAN2_MAGTHRESH);
15580  if (!x741.valid)
15581  {
15582  continue;
15583  }
15584  IkReal x740 =
15585  ((1.0) * (x741.value));
15586  sj1 = 0;
15587  cj1 = -1.0;
15588  j1 = 3.14159265358979;
15589  sj2 = gconst10;
15590  cj2 = gconst11;
15591  j2 = ((3.14159265) +
15592  (((-1.0) * x740)));
15593  new_r01 = 0;
15594  IkReal gconst9 =
15595  ((3.14159265358979) +
15596  (((-1.0) * x740)));
15597  IkReal gconst10 = 0;
15598  IkReal x742 =
15599  new_r11 * new_r11;
15600  if (IKabs(x742) == 0)
15601  {
15602  continue;
15603  }
15604  IkReal gconst11 =
15605  ((-1.0) * new_r11 *
15606  (pow(x742, -0.5)));
15607  j0eval[0] =
15608  ((IKabs(new_r11)) +
15609  (IKabs(new_r10)));
15610  if (IKabs(j0eval[0]) <
15611  0.0000010000000000)
15612  {
15613  {
15614  IkReal j0eval[1];
15615  CheckValue<IkReal> x744 =
15616  IKatan2WithCheck(
15617  IkReal(0),
15618  IkReal(new_r11),
15619  IKFAST_ATAN2_MAGTHRESH);
15620  if (!x744.valid)
15621  {
15622  continue;
15623  }
15624  IkReal x743 =
15625  ((1.0) *
15626  (x744.value));
15627  sj1 = 0;
15628  cj1 = -1.0;
15629  j1 = 3.14159265358979;
15630  sj2 = gconst10;
15631  cj2 = gconst11;
15632  j2 = ((3.14159265) +
15633  (((-1.0) * x743)));
15634  new_r01 = 0;
15635  IkReal gconst9 =
15636  ((3.14159265358979) +
15637  (((-1.0) * x743)));
15638  IkReal gconst10 = 0;
15639  IkReal x745 =
15640  new_r11 * new_r11;
15641  if (IKabs(x745) == 0)
15642  {
15643  continue;
15644  }
15645  IkReal gconst11 =
15646  ((-1.0) * new_r11 *
15647  (pow(x745, -0.5)));
15648  j0eval[0] = new_r11;
15649  if (IKabs(j0eval[0]) <
15650  0.0000010000000000)
15651  {
15652  continue; // 3 cases
15653  // reached
15654  }
15655  else
15656  {
15657  {
15658  IkReal j0array[1],
15659  cj0array[1],
15660  sj0array[1];
15661  bool j0valid[1] = {
15662  false
15663  };
15664  _nj0 = 1;
15665  CheckValue<IkReal> x746 =
15666  IKPowWithIntegerCheck(
15667  gconst11, -1);
15668  if (!x746.valid)
15669  {
15670  continue;
15671  }
15672  CheckValue<IkReal> x747 =
15673  IKPowWithIntegerCheck(
15674  new_r11, -1);
15675  if (!x747.valid)
15676  {
15677  continue;
15678  }
15679  if (IKabs((
15680  new_r10 *
15681  (x746.value))) <
15682  IKFAST_ATAN2_MAGTHRESH &&
15683  IKabs((
15684  (-1.0) *
15685  gconst11 *
15686  (x747.value))) <
15687  IKFAST_ATAN2_MAGTHRESH &&
15688  IKabs(
15689  IKsqr((
15690  new_r10 *
15691  (x746.value))) +
15692  IKsqr((
15693  (-1.0) *
15694  gconst11 *
15695  (x747.value))) -
15696  1) <=
15697  IKFAST_SINCOS_THRESH)
15698  continue;
15699  j0array[0] = IKatan2(
15700  (new_r10 *
15701  (x746.value)),
15702  ((-1.0) *
15703  gconst11 *
15704  (x747.value)));
15705  sj0array[0] =
15706  IKsin(j0array[0]);
15707  cj0array[0] =
15708  IKcos(j0array[0]);
15709  if (j0array[0] > IKPI)
15710  {
15711  j0array[0] -= IK2PI;
15712  }
15713  else if (j0array[0] <
15714  -IKPI)
15715  {
15716  j0array[0] += IK2PI;
15717  }
15718  j0valid[0] = true;
15719  for (int ij0 = 0;
15720  ij0 < 1;
15721  ++ij0)
15722  {
15723  if (!j0valid[ij0])
15724  {
15725  continue;
15726  }
15727  _ij0[0] = ij0;
15728  _ij0[1] = -1;
15729  for (int iij0 =
15730  ij0 + 1;
15731  iij0 < 1;
15732  ++iij0)
15733  {
15734  if (j0valid
15735  [iij0] &&
15736  IKabs(
15737  cj0array
15738  [ij0] -
15739  cj0array
15740  [iij0]) <
15741  IKFAST_SOLUTION_THRESH &&
15742  IKabs(
15743  sj0array
15744  [ij0] -
15745  sj0array
15746  [iij0]) <
15747  IKFAST_SOLUTION_THRESH)
15748  {
15749  j0valid[iij0] =
15750  false;
15751  _ij0[1] = iij0;
15752  break;
15753  }
15754  }
15755  j0 = j0array[ij0];
15756  cj0 = cj0array[ij0];
15757  sj0 = sj0array[ij0];
15758  {
15759  IkReal
15760  evalcond[8];
15761  IkReal x748 =
15762  IKsin(j0);
15763  IkReal x749 =
15764  IKcos(j0);
15765  IkReal x750 =
15766  (gconst11 *
15767  x748);
15768  IkReal x751 =
15769  (gconst11 *
15770  x749);
15771  evalcond[0] =
15772  (new_r11 *
15773  x748);
15774  evalcond[1] =
15775  ((-1.0) *
15776  x750);
15777  evalcond[2] =
15778  (gconst11 +
15779  ((new_r11 *
15780  x749)));
15781  evalcond[3] =
15782  (x751 +
15783  new_r11);
15784  evalcond[4] =
15785  ((((-1.0) *
15786  x750)) +
15787  new_r10);
15788  evalcond[5] =
15789  ((((-1.0) *
15790  x751)) +
15791  new_r00);
15792  evalcond[6] =
15793  ((((-1.0) *
15794  new_r00 *
15795  x748)) +
15796  ((new_r10 *
15797  x749)));
15798  evalcond[7] =
15799  (((new_r10 *
15800  x748)) +
15801  ((new_r00 *
15802  x749)) +
15803  (((-1.0) *
15804  gconst11)));
15805  if (IKabs(
15806  evalcond
15807  [0]) >
15808  IKFAST_EVALCOND_THRESH ||
15809  IKabs(
15810  evalcond
15811  [1]) >
15812  IKFAST_EVALCOND_THRESH ||
15813  IKabs(
15814  evalcond
15815  [2]) >
15816  IKFAST_EVALCOND_THRESH ||
15817  IKabs(
15818  evalcond
15819  [3]) >
15820  IKFAST_EVALCOND_THRESH ||
15821  IKabs(
15822  evalcond
15823  [4]) >
15824  IKFAST_EVALCOND_THRESH ||
15825  IKabs(
15826  evalcond
15827  [5]) >
15828  IKFAST_EVALCOND_THRESH ||
15829  IKabs(
15830  evalcond
15831  [6]) >
15832  IKFAST_EVALCOND_THRESH ||
15833  IKabs(
15834  evalcond
15835  [7]) >
15836  IKFAST_EVALCOND_THRESH)
15837  {
15838  continue;
15839  }
15840  }
15841 
15842  {
15843  std::vector<
15844  IkSingleDOFSolutionBase<
15845  IkReal> >
15846  vinfos(7);
15847  vinfos[0]
15848  .jointtype =
15849  1;
15850  vinfos[0]
15851  .foffset = j0;
15852  vinfos[0]
15853  .indices[0] =
15854  _ij0[0];
15855  vinfos[0]
15856  .indices[1] =
15857  _ij0[1];
15858  vinfos[0]
15859  .maxsolutions =
15860  _nj0;
15861  vinfos[1]
15862  .jointtype =
15863  1;
15864  vinfos[1]
15865  .foffset = j1;
15866  vinfos[1]
15867  .indices[0] =
15868  _ij1[0];
15869  vinfos[1]
15870  .indices[1] =
15871  _ij1[1];
15872  vinfos[1]
15873  .maxsolutions =
15874  _nj1;
15875  vinfos[2]
15876  .jointtype =
15877  1;
15878  vinfos[2]
15879  .foffset = j2;
15880  vinfos[2]
15881  .indices[0] =
15882  _ij2[0];
15883  vinfos[2]
15884  .indices[1] =
15885  _ij2[1];
15886  vinfos[2]
15887  .maxsolutions =
15888  _nj2;
15889  vinfos[3]
15890  .jointtype =
15891  1;
15892  vinfos[3]
15893  .foffset = j3;
15894  vinfos[3]
15895  .indices[0] =
15896  _ij3[0];
15897  vinfos[3]
15898  .indices[1] =
15899  _ij3[1];
15900  vinfos[3]
15901  .maxsolutions =
15902  _nj3;
15903  vinfos[4]
15904  .jointtype =
15905  1;
15906  vinfos[4]
15907  .foffset = j4;
15908  vinfos[4]
15909  .indices[0] =
15910  _ij4[0];
15911  vinfos[4]
15912  .indices[1] =
15913  _ij4[1];
15914  vinfos[4]
15915  .maxsolutions =
15916  _nj4;
15917  vinfos[5]
15918  .jointtype =
15919  1;
15920  vinfos[5]
15921  .foffset = j5;
15922  vinfos[5]
15923  .indices[0] =
15924  _ij5[0];
15925  vinfos[5]
15926  .indices[1] =
15927  _ij5[1];
15928  vinfos[5]
15929  .maxsolutions =
15930  _nj5;
15931  vinfos[6]
15932  .jointtype =
15933  1;
15934  vinfos[6]
15935  .foffset = j6;
15936  vinfos[6]
15937  .indices[0] =
15938  _ij6[0];
15939  vinfos[6]
15940  .indices[1] =
15941  _ij6[1];
15942  vinfos[6]
15943  .maxsolutions =
15944  _nj6;
15945  std::vector<int>
15946  vfree(0);
15947  solutions
15948  .AddSolution(
15949  vinfos,
15950  vfree);
15951  }
15952  }
15953  }
15954  }
15955  }
15956  }
15957  else
15958  {
15959  {
15960  IkReal j0array[1],
15961  cj0array[1],
15962  sj0array[1];
15963  bool j0valid[1] = {
15964  false
15965  };
15966  _nj0 = 1;
15967  CheckValue<IkReal> x752 =
15968  IKPowWithIntegerCheck(
15969  IKsign(gconst11),
15970  -1);
15971  if (!x752.valid)
15972  {
15973  continue;
15974  }
15975  CheckValue<IkReal> x753 =
15976  IKatan2WithCheck(
15977  IkReal(new_r10),
15978  IkReal(((-1.0) *
15979  new_r11)),
15980  IKFAST_ATAN2_MAGTHRESH);
15981  if (!x753.valid)
15982  {
15983  continue;
15984  }
15985  j0array[0] =
15986  ((-1.5707963267949) +
15987  (((1.5707963267949) *
15988  (x752.value))) +
15989  (x753.value));
15990  sj0array[0] =
15991  IKsin(j0array[0]);
15992  cj0array[0] =
15993  IKcos(j0array[0]);
15994  if (j0array[0] > IKPI)
15995  {
15996  j0array[0] -= IK2PI;
15997  }
15998  else if (j0array[0] <
15999  -IKPI)
16000  {
16001  j0array[0] += IK2PI;
16002  }
16003  j0valid[0] = true;
16004  for (int ij0 = 0; ij0 < 1;
16005  ++ij0)
16006  {
16007  if (!j0valid[ij0])
16008  {
16009  continue;
16010  }
16011  _ij0[0] = ij0;
16012  _ij0[1] = -1;
16013  for (int iij0 = ij0 + 1;
16014  iij0 < 1;
16015  ++iij0)
16016  {
16017  if (j0valid[iij0] &&
16018  IKabs(
16019  cj0array
16020  [ij0] -
16021  cj0array
16022  [iij0]) <
16023  IKFAST_SOLUTION_THRESH &&
16024  IKabs(
16025  sj0array
16026  [ij0] -
16027  sj0array
16028  [iij0]) <
16029  IKFAST_SOLUTION_THRESH)
16030  {
16031  j0valid[iij0] =
16032  false;
16033  _ij0[1] = iij0;
16034  break;
16035  }
16036  }
16037  j0 = j0array[ij0];
16038  cj0 = cj0array[ij0];
16039  sj0 = sj0array[ij0];
16040  {
16041  IkReal evalcond[8];
16042  IkReal x754 =
16043  IKsin(j0);
16044  IkReal x755 =
16045  IKcos(j0);
16046  IkReal x756 =
16047  (gconst11 * x754);
16048  IkReal x757 =
16049  (gconst11 * x755);
16050  evalcond[0] =
16051  (new_r11 * x754);
16052  evalcond[1] =
16053  ((-1.0) * x756);
16054  evalcond[2] =
16055  (gconst11 +
16056  ((new_r11 *
16057  x755)));
16058  evalcond[3] =
16059  (x757 + new_r11);
16060  evalcond[4] =
16061  ((((-1.0) *
16062  x756)) +
16063  new_r10);
16064  evalcond[5] =
16065  ((((-1.0) *
16066  x757)) +
16067  new_r00);
16068  evalcond[6] =
16069  ((((-1.0) *
16070  new_r00 *
16071  x754)) +
16072  ((new_r10 *
16073  x755)));
16074  evalcond[7] =
16075  (((new_r00 *
16076  x755)) +
16077  ((new_r10 *
16078  x754)) +
16079  (((-1.0) *
16080  gconst11)));
16081  if (IKabs(
16082  evalcond[0]) >
16083  IKFAST_EVALCOND_THRESH ||
16084  IKabs(
16085  evalcond[1]) >
16086  IKFAST_EVALCOND_THRESH ||
16087  IKabs(
16088  evalcond[2]) >
16089  IKFAST_EVALCOND_THRESH ||
16090  IKabs(
16091  evalcond[3]) >
16092  IKFAST_EVALCOND_THRESH ||
16093  IKabs(
16094  evalcond[4]) >
16095  IKFAST_EVALCOND_THRESH ||
16096  IKabs(
16097  evalcond[5]) >
16098  IKFAST_EVALCOND_THRESH ||
16099  IKabs(
16100  evalcond[6]) >
16101  IKFAST_EVALCOND_THRESH ||
16102  IKabs(
16103  evalcond[7]) >
16104  IKFAST_EVALCOND_THRESH)
16105  {
16106  continue;
16107  }
16108  }
16109 
16110  {
16111  std::vector<
16112  IkSingleDOFSolutionBase<
16113  IkReal> >
16114  vinfos(7);
16115  vinfos[0].jointtype =
16116  1;
16117  vinfos[0].foffset =
16118  j0;
16119  vinfos[0].indices[0] =
16120  _ij0[0];
16121  vinfos[0].indices[1] =
16122  _ij0[1];
16123  vinfos[0]
16124  .maxsolutions =
16125  _nj0;
16126  vinfos[1].jointtype =
16127  1;
16128  vinfos[1].foffset =
16129  j1;
16130  vinfos[1].indices[0] =
16131  _ij1[0];
16132  vinfos[1].indices[1] =
16133  _ij1[1];
16134  vinfos[1]
16135  .maxsolutions =
16136  _nj1;
16137  vinfos[2].jointtype =
16138  1;
16139  vinfos[2].foffset =
16140  j2;
16141  vinfos[2].indices[0] =
16142  _ij2[0];
16143  vinfos[2].indices[1] =
16144  _ij2[1];
16145  vinfos[2]
16146  .maxsolutions =
16147  _nj2;
16148  vinfos[3].jointtype =
16149  1;
16150  vinfos[3].foffset =
16151  j3;
16152  vinfos[3].indices[0] =
16153  _ij3[0];
16154  vinfos[3].indices[1] =
16155  _ij3[1];
16156  vinfos[3]
16157  .maxsolutions =
16158  _nj3;
16159  vinfos[4].jointtype =
16160  1;
16161  vinfos[4].foffset =
16162  j4;
16163  vinfos[4].indices[0] =
16164  _ij4[0];
16165  vinfos[4].indices[1] =
16166  _ij4[1];
16167  vinfos[4]
16168  .maxsolutions =
16169  _nj4;
16170  vinfos[5].jointtype =
16171  1;
16172  vinfos[5].foffset =
16173  j5;
16174  vinfos[5].indices[0] =
16175  _ij5[0];
16176  vinfos[5].indices[1] =
16177  _ij5[1];
16178  vinfos[5]
16179  .maxsolutions =
16180  _nj5;
16181  vinfos[6].jointtype =
16182  1;
16183  vinfos[6].foffset =
16184  j6;
16185  vinfos[6].indices[0] =
16186  _ij6[0];
16187  vinfos[6].indices[1] =
16188  _ij6[1];
16189  vinfos[6]
16190  .maxsolutions =
16191  _nj6;
16192  std::vector<int>
16193  vfree(0);
16194  solutions.AddSolution(
16195  vinfos, vfree);
16196  }
16197  }
16198  }
16199  }
16200  }
16201  }
16202  else
16203  {
16204  {
16205  IkReal j0array[1],
16206  cj0array[1], sj0array[1];
16207  bool j0valid[1] = { false };
16208  _nj0 = 1;
16209  CheckValue<IkReal> x758 =
16210  IKPowWithIntegerCheck(
16211  IKsign(gconst11), -1);
16212  if (!x758.valid)
16213  {
16214  continue;
16215  }
16216  CheckValue<IkReal> x759 =
16217  IKatan2WithCheck(
16218  IkReal(new_r10),
16219  IkReal(new_r00),
16220  IKFAST_ATAN2_MAGTHRESH);
16221  if (!x759.valid)
16222  {
16223  continue;
16224  }
16225  j0array[0] =
16226  ((-1.5707963267949) +
16227  (((1.5707963267949) *
16228  (x758.value))) +
16229  (x759.value));
16230  sj0array[0] =
16231  IKsin(j0array[0]);
16232  cj0array[0] =
16233  IKcos(j0array[0]);
16234  if (j0array[0] > IKPI)
16235  {
16236  j0array[0] -= IK2PI;
16237  }
16238  else if (j0array[0] < -IKPI)
16239  {
16240  j0array[0] += IK2PI;
16241  }
16242  j0valid[0] = true;
16243  for (int ij0 = 0; ij0 < 1;
16244  ++ij0)
16245  {
16246  if (!j0valid[ij0])
16247  {
16248  continue;
16249  }
16250  _ij0[0] = ij0;
16251  _ij0[1] = -1;
16252  for (int iij0 = ij0 + 1;
16253  iij0 < 1;
16254  ++iij0)
16255  {
16256  if (j0valid[iij0] &&
16257  IKabs(
16258  cj0array[ij0] -
16259  cj0array[iij0]) <
16260  IKFAST_SOLUTION_THRESH &&
16261  IKabs(
16262  sj0array[ij0] -
16263  sj0array[iij0]) <
16264  IKFAST_SOLUTION_THRESH)
16265  {
16266  j0valid[iij0] = false;
16267  _ij0[1] = iij0;
16268  break;
16269  }
16270  }
16271  j0 = j0array[ij0];
16272  cj0 = cj0array[ij0];
16273  sj0 = sj0array[ij0];
16274  {
16275  IkReal evalcond[8];
16276  IkReal x760 = IKsin(j0);
16277  IkReal x761 = IKcos(j0);
16278  IkReal x762 =
16279  (gconst11 * x760);
16280  IkReal x763 =
16281  (gconst11 * x761);
16282  evalcond[0] =
16283  (new_r11 * x760);
16284  evalcond[1] =
16285  ((-1.0) * x762);
16286  evalcond[2] =
16287  (((new_r11 * x761)) +
16288  gconst11);
16289  evalcond[3] =
16290  (x763 + new_r11);
16291  evalcond[4] =
16292  ((((-1.0) * x762)) +
16293  new_r10);
16294  evalcond[5] =
16295  ((((-1.0) * x763)) +
16296  new_r00);
16297  evalcond[6] =
16298  (((new_r10 * x761)) +
16299  (((-1.0) * new_r00 *
16300  x760)));
16301  evalcond[7] =
16302  (((new_r10 * x760)) +
16303  ((new_r00 * x761)) +
16304  (((-1.0) *
16305  gconst11)));
16306  if (IKabs(evalcond[0]) >
16307  IKFAST_EVALCOND_THRESH ||
16308  IKabs(evalcond[1]) >
16309  IKFAST_EVALCOND_THRESH ||
16310  IKabs(evalcond[2]) >
16311  IKFAST_EVALCOND_THRESH ||
16312  IKabs(evalcond[3]) >
16313  IKFAST_EVALCOND_THRESH ||
16314  IKabs(evalcond[4]) >
16315  IKFAST_EVALCOND_THRESH ||
16316  IKabs(evalcond[5]) >
16317  IKFAST_EVALCOND_THRESH ||
16318  IKabs(evalcond[6]) >
16319  IKFAST_EVALCOND_THRESH ||
16320  IKabs(evalcond[7]) >
16321  IKFAST_EVALCOND_THRESH)
16322  {
16323  continue;
16324  }
16325  }
16326 
16327  {
16328  std::vector<
16329  IkSingleDOFSolutionBase<
16330  IkReal> >
16331  vinfos(7);
16332  vinfos[0].jointtype = 1;
16333  vinfos[0].foffset = j0;
16334  vinfos[0].indices[0] =
16335  _ij0[0];
16336  vinfos[0].indices[1] =
16337  _ij0[1];
16338  vinfos[0].maxsolutions =
16339  _nj0;
16340  vinfos[1].jointtype = 1;
16341  vinfos[1].foffset = j1;
16342  vinfos[1].indices[0] =
16343  _ij1[0];
16344  vinfos[1].indices[1] =
16345  _ij1[1];
16346  vinfos[1].maxsolutions =
16347  _nj1;
16348  vinfos[2].jointtype = 1;
16349  vinfos[2].foffset = j2;
16350  vinfos[2].indices[0] =
16351  _ij2[0];
16352  vinfos[2].indices[1] =
16353  _ij2[1];
16354  vinfos[2].maxsolutions =
16355  _nj2;
16356  vinfos[3].jointtype = 1;
16357  vinfos[3].foffset = j3;
16358  vinfos[3].indices[0] =
16359  _ij3[0];
16360  vinfos[3].indices[1] =
16361  _ij3[1];
16362  vinfos[3].maxsolutions =
16363  _nj3;
16364  vinfos[4].jointtype = 1;
16365  vinfos[4].foffset = j4;
16366  vinfos[4].indices[0] =
16367  _ij4[0];
16368  vinfos[4].indices[1] =
16369  _ij4[1];
16370  vinfos[4].maxsolutions =
16371  _nj4;
16372  vinfos[5].jointtype = 1;
16373  vinfos[5].foffset = j5;
16374  vinfos[5].indices[0] =
16375  _ij5[0];
16376  vinfos[5].indices[1] =
16377  _ij5[1];
16378  vinfos[5].maxsolutions =
16379  _nj5;
16380  vinfos[6].jointtype = 1;
16381  vinfos[6].foffset = j6;
16382  vinfos[6].indices[0] =
16383  _ij6[0];
16384  vinfos[6].indices[1] =
16385  _ij6[1];
16386  vinfos[6].maxsolutions =
16387  _nj6;
16388  std::vector<int> vfree(0);
16389  solutions.AddSolution(
16390  vinfos, vfree);
16391  }
16392  }
16393  }
16394  }
16395  }
16396  }
16397  } while (0);
16398  if (bgotonextstatement)
16399  {
16400  bool bgotonextstatement = true;
16401  do
16402  {
16403  if (1)
16404  {
16405  bgotonextstatement = false;
16406  continue; // branch miss [j0]
16407  }
16408  } while (0);
16409  if (bgotonextstatement)
16410  {
16411  }
16412  }
16413  }
16414  }
16415  }
16416  }
16417  }
16418  }
16419  else
16420  {
16421  {
16422  IkReal j0array[1], cj0array[1], sj0array[1];
16423  bool j0valid[1] = { false };
16424  _nj0 = 1;
16425  IkReal x764 = ((1.0) * new_r11);
16426  CheckValue<IkReal> x765 = IKPowWithIntegerCheck(
16427  IKsign(((((-1.0) * gconst11 * x764)) +
16428  ((gconst10 * new_r01)))),
16429  -1);
16430  if (!x765.valid)
16431  {
16432  continue;
16433  }
16434  CheckValue<IkReal> x766 = IKatan2WithCheck(
16435  IkReal((((gconst10 * gconst11)) +
16436  (((-1.0) * new_r01 * x764)))),
16437  IkReal(
16438  ((new_r11 * new_r11) +
16439  (((-1.0) * (gconst10 * gconst10))))),
16440  IKFAST_ATAN2_MAGTHRESH);
16441  if (!x766.valid)
16442  {
16443  continue;
16444  }
16445  j0array[0] =
16446  ((-1.5707963267949) +
16447  (((1.5707963267949) * (x765.value))) +
16448  (x766.value));
16449  sj0array[0] = IKsin(j0array[0]);
16450  cj0array[0] = IKcos(j0array[0]);
16451  if (j0array[0] > IKPI)
16452  {
16453  j0array[0] -= IK2PI;
16454  }
16455  else if (j0array[0] < -IKPI)
16456  {
16457  j0array[0] += IK2PI;
16458  }
16459  j0valid[0] = true;
16460  for (int ij0 = 0; ij0 < 1; ++ij0)
16461  {
16462  if (!j0valid[ij0])
16463  {
16464  continue;
16465  }
16466  _ij0[0] = ij0;
16467  _ij0[1] = -1;
16468  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16469  {
16470  if (j0valid[iij0] &&
16471  IKabs(cj0array[ij0] - cj0array[iij0]) <
16472  IKFAST_SOLUTION_THRESH &&
16473  IKabs(sj0array[ij0] - sj0array[iij0]) <
16474  IKFAST_SOLUTION_THRESH)
16475  {
16476  j0valid[iij0] = false;
16477  _ij0[1] = iij0;
16478  break;
16479  }
16480  }
16481  j0 = j0array[ij0];
16482  cj0 = cj0array[ij0];
16483  sj0 = sj0array[ij0];
16484  {
16485  IkReal evalcond[8];
16486  IkReal x767 = IKcos(j0);
16487  IkReal x768 = IKsin(j0);
16488  IkReal x769 = (gconst10 * x767);
16489  IkReal x770 = (gconst11 * x767);
16490  IkReal x771 = ((1.0) * x768);
16491  IkReal x772 = (gconst11 * x771);
16492  evalcond[0] =
16493  (((new_r11 * x768)) + gconst10 +
16494  ((new_r01 * x767)));
16495  evalcond[1] =
16496  (((gconst10 * x768)) + x770 + new_r11);
16497  evalcond[2] = (((new_r10 * x767)) +
16498  (((-1.0) * new_r00 * x771)) +
16499  gconst10);
16500  evalcond[3] = (((new_r11 * x767)) +
16501  (((-1.0) * new_r01 * x771)) +
16502  gconst11);
16503  evalcond[4] =
16504  (x769 + (((-1.0) * x772)) + new_r10);
16505  evalcond[5] =
16506  (x769 + (((-1.0) * x772)) + new_r01);
16507  evalcond[6] = (((new_r10 * x768)) +
16508  ((new_r00 * x767)) +
16509  (((-1.0) * gconst11)));
16510  evalcond[7] =
16511  ((((-1.0) * gconst10 * x771)) +
16512  new_r00 + (((-1.0) * x770)));
16513  if (IKabs(evalcond[0]) >
16514  IKFAST_EVALCOND_THRESH ||
16515  IKabs(evalcond[1]) >
16516  IKFAST_EVALCOND_THRESH ||
16517  IKabs(evalcond[2]) >
16518  IKFAST_EVALCOND_THRESH ||
16519  IKabs(evalcond[3]) >
16520  IKFAST_EVALCOND_THRESH ||
16521  IKabs(evalcond[4]) >
16522  IKFAST_EVALCOND_THRESH ||
16523  IKabs(evalcond[5]) >
16524  IKFAST_EVALCOND_THRESH ||
16525  IKabs(evalcond[6]) >
16526  IKFAST_EVALCOND_THRESH ||
16527  IKabs(evalcond[7]) >
16528  IKFAST_EVALCOND_THRESH)
16529  {
16530  continue;
16531  }
16532  }
16533 
16534  {
16535  std::vector<
16536  IkSingleDOFSolutionBase<IkReal> >
16537  vinfos(7);
16538  vinfos[0].jointtype = 1;
16539  vinfos[0].foffset = j0;
16540  vinfos[0].indices[0] = _ij0[0];
16541  vinfos[0].indices[1] = _ij0[1];
16542  vinfos[0].maxsolutions = _nj0;
16543  vinfos[1].jointtype = 1;
16544  vinfos[1].foffset = j1;
16545  vinfos[1].indices[0] = _ij1[0];
16546  vinfos[1].indices[1] = _ij1[1];
16547  vinfos[1].maxsolutions = _nj1;
16548  vinfos[2].jointtype = 1;
16549  vinfos[2].foffset = j2;
16550  vinfos[2].indices[0] = _ij2[0];
16551  vinfos[2].indices[1] = _ij2[1];
16552  vinfos[2].maxsolutions = _nj2;
16553  vinfos[3].jointtype = 1;
16554  vinfos[3].foffset = j3;
16555  vinfos[3].indices[0] = _ij3[0];
16556  vinfos[3].indices[1] = _ij3[1];
16557  vinfos[3].maxsolutions = _nj3;
16558  vinfos[4].jointtype = 1;
16559  vinfos[4].foffset = j4;
16560  vinfos[4].indices[0] = _ij4[0];
16561  vinfos[4].indices[1] = _ij4[1];
16562  vinfos[4].maxsolutions = _nj4;
16563  vinfos[5].jointtype = 1;
16564  vinfos[5].foffset = j5;
16565  vinfos[5].indices[0] = _ij5[0];
16566  vinfos[5].indices[1] = _ij5[1];
16567  vinfos[5].maxsolutions = _nj5;
16568  vinfos[6].jointtype = 1;
16569  vinfos[6].foffset = j6;
16570  vinfos[6].indices[0] = _ij6[0];
16571  vinfos[6].indices[1] = _ij6[1];
16572  vinfos[6].maxsolutions = _nj6;
16573  std::vector<int> vfree(0);
16574  solutions.AddSolution(vinfos, vfree);
16575  }
16576  }
16577  }
16578  }
16579  }
16580  }
16581  else
16582  {
16583  {
16584  IkReal j0array[1], cj0array[1], sj0array[1];
16585  bool j0valid[1] = { false };
16586  _nj0 = 1;
16587  IkReal x773 = ((1.0) * new_r11);
16588  CheckValue<IkReal> x774 = IKPowWithIntegerCheck(
16589  IKsign(((new_r01 * new_r01) +
16590  (new_r11 * new_r11))),
16591  -1);
16592  if (!x774.valid)
16593  {
16594  continue;
16595  }
16596  CheckValue<IkReal> x775 = IKatan2WithCheck(
16597  IkReal((((gconst11 * new_r01)) +
16598  (((-1.0) * gconst10 * x773)))),
16599  IkReal(((((-1.0) * gconst10 * new_r01)) +
16600  (((-1.0) * gconst11 * x773)))),
16601  IKFAST_ATAN2_MAGTHRESH);
16602  if (!x775.valid)
16603  {
16604  continue;
16605  }
16606  j0array[0] = ((-1.5707963267949) +
16607  (((1.5707963267949) * (x774.value))) +
16608  (x775.value));
16609  sj0array[0] = IKsin(j0array[0]);
16610  cj0array[0] = IKcos(j0array[0]);
16611  if (j0array[0] > IKPI)
16612  {
16613  j0array[0] -= IK2PI;
16614  }
16615  else if (j0array[0] < -IKPI)
16616  {
16617  j0array[0] += IK2PI;
16618  }
16619  j0valid[0] = true;
16620  for (int ij0 = 0; ij0 < 1; ++ij0)
16621  {
16622  if (!j0valid[ij0])
16623  {
16624  continue;
16625  }
16626  _ij0[0] = ij0;
16627  _ij0[1] = -1;
16628  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16629  {
16630  if (j0valid[iij0] &&
16631  IKabs(cj0array[ij0] - cj0array[iij0]) <
16632  IKFAST_SOLUTION_THRESH &&
16633  IKabs(sj0array[ij0] - sj0array[iij0]) <
16634  IKFAST_SOLUTION_THRESH)
16635  {
16636  j0valid[iij0] = false;
16637  _ij0[1] = iij0;
16638  break;
16639  }
16640  }
16641  j0 = j0array[ij0];
16642  cj0 = cj0array[ij0];
16643  sj0 = sj0array[ij0];
16644  {
16645  IkReal evalcond[8];
16646  IkReal x776 = IKcos(j0);
16647  IkReal x777 = IKsin(j0);
16648  IkReal x778 = (gconst10 * x776);
16649  IkReal x779 = (gconst11 * x776);
16650  IkReal x780 = ((1.0) * x777);
16651  IkReal x781 = (gconst11 * x780);
16652  evalcond[0] = (gconst10 + ((new_r11 * x777)) +
16653  ((new_r01 * x776)));
16654  evalcond[1] =
16655  (x779 + new_r11 + ((gconst10 * x777)));
16656  evalcond[2] = (gconst10 + ((new_r10 * x776)) +
16657  (((-1.0) * new_r00 * x780)));
16658  evalcond[3] = ((((-1.0) * new_r01 * x780)) +
16659  gconst11 + ((new_r11 * x776)));
16660  evalcond[4] =
16661  ((((-1.0) * x781)) + x778 + new_r10);
16662  evalcond[5] =
16663  ((((-1.0) * x781)) + x778 + new_r01);
16664  evalcond[6] =
16665  (((new_r00 * x776)) + ((new_r10 * x777)) +
16666  (((-1.0) * gconst11)));
16667  evalcond[7] = (new_r00 + (((-1.0) * x779)) +
16668  (((-1.0) * gconst10 * x780)));
16669  if (IKabs(evalcond[0]) >
16670  IKFAST_EVALCOND_THRESH ||
16671  IKabs(evalcond[1]) >
16672  IKFAST_EVALCOND_THRESH ||
16673  IKabs(evalcond[2]) >
16674  IKFAST_EVALCOND_THRESH ||
16675  IKabs(evalcond[3]) >
16676  IKFAST_EVALCOND_THRESH ||
16677  IKabs(evalcond[4]) >
16678  IKFAST_EVALCOND_THRESH ||
16679  IKabs(evalcond[5]) >
16680  IKFAST_EVALCOND_THRESH ||
16681  IKabs(evalcond[6]) >
16682  IKFAST_EVALCOND_THRESH ||
16683  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16684  {
16685  continue;
16686  }
16687  }
16688 
16689  {
16690  std::vector<IkSingleDOFSolutionBase<IkReal> >
16691  vinfos(7);
16692  vinfos[0].jointtype = 1;
16693  vinfos[0].foffset = j0;
16694  vinfos[0].indices[0] = _ij0[0];
16695  vinfos[0].indices[1] = _ij0[1];
16696  vinfos[0].maxsolutions = _nj0;
16697  vinfos[1].jointtype = 1;
16698  vinfos[1].foffset = j1;
16699  vinfos[1].indices[0] = _ij1[0];
16700  vinfos[1].indices[1] = _ij1[1];
16701  vinfos[1].maxsolutions = _nj1;
16702  vinfos[2].jointtype = 1;
16703  vinfos[2].foffset = j2;
16704  vinfos[2].indices[0] = _ij2[0];
16705  vinfos[2].indices[1] = _ij2[1];
16706  vinfos[2].maxsolutions = _nj2;
16707  vinfos[3].jointtype = 1;
16708  vinfos[3].foffset = j3;
16709  vinfos[3].indices[0] = _ij3[0];
16710  vinfos[3].indices[1] = _ij3[1];
16711  vinfos[3].maxsolutions = _nj3;
16712  vinfos[4].jointtype = 1;
16713  vinfos[4].foffset = j4;
16714  vinfos[4].indices[0] = _ij4[0];
16715  vinfos[4].indices[1] = _ij4[1];
16716  vinfos[4].maxsolutions = _nj4;
16717  vinfos[5].jointtype = 1;
16718  vinfos[5].foffset = j5;
16719  vinfos[5].indices[0] = _ij5[0];
16720  vinfos[5].indices[1] = _ij5[1];
16721  vinfos[5].maxsolutions = _nj5;
16722  vinfos[6].jointtype = 1;
16723  vinfos[6].foffset = j6;
16724  vinfos[6].indices[0] = _ij6[0];
16725  vinfos[6].indices[1] = _ij6[1];
16726  vinfos[6].maxsolutions = _nj6;
16727  std::vector<int> vfree(0);
16728  solutions.AddSolution(vinfos, vfree);
16729  }
16730  }
16731  }
16732  }
16733  }
16734  }
16735  else
16736  {
16737  {
16738  IkReal j0array[1], cj0array[1], sj0array[1];
16739  bool j0valid[1] = { false };
16740  _nj0 = 1;
16741  IkReal x782 = ((1.0) * gconst10);
16742  CheckValue<IkReal> x783 = IKatan2WithCheck(
16743  IkReal(((((-1.0) * new_r10 * x782)) +
16744  ((gconst10 * new_r01)))),
16745  IkReal(((((-1.0) * new_r11 * x782)) +
16746  (((-1.0) * new_r00 * x782)))),
16747  IKFAST_ATAN2_MAGTHRESH);
16748  if (!x783.valid)
16749  {
16750  continue;
16751  }
16752  CheckValue<IkReal> x784 = IKPowWithIntegerCheck(
16753  IKsign((((new_r10 * new_r11)) +
16754  ((new_r00 * new_r01)))),
16755  -1);
16756  if (!x784.valid)
16757  {
16758  continue;
16759  }
16760  j0array[0] = ((-1.5707963267949) + (x783.value) +
16761  (((1.5707963267949) * (x784.value))));
16762  sj0array[0] = IKsin(j0array[0]);
16763  cj0array[0] = IKcos(j0array[0]);
16764  if (j0array[0] > IKPI)
16765  {
16766  j0array[0] -= IK2PI;
16767  }
16768  else if (j0array[0] < -IKPI)
16769  {
16770  j0array[0] += IK2PI;
16771  }
16772  j0valid[0] = true;
16773  for (int ij0 = 0; ij0 < 1; ++ij0)
16774  {
16775  if (!j0valid[ij0])
16776  {
16777  continue;
16778  }
16779  _ij0[0] = ij0;
16780  _ij0[1] = -1;
16781  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16782  {
16783  if (j0valid[iij0] &&
16784  IKabs(cj0array[ij0] - cj0array[iij0]) <
16785  IKFAST_SOLUTION_THRESH &&
16786  IKabs(sj0array[ij0] - sj0array[iij0]) <
16787  IKFAST_SOLUTION_THRESH)
16788  {
16789  j0valid[iij0] = false;
16790  _ij0[1] = iij0;
16791  break;
16792  }
16793  }
16794  j0 = j0array[ij0];
16795  cj0 = cj0array[ij0];
16796  sj0 = sj0array[ij0];
16797  {
16798  IkReal evalcond[8];
16799  IkReal x785 = IKcos(j0);
16800  IkReal x786 = IKsin(j0);
16801  IkReal x787 = (gconst10 * x785);
16802  IkReal x788 = (gconst11 * x785);
16803  IkReal x789 = ((1.0) * x786);
16804  IkReal x790 = (gconst11 * x789);
16805  evalcond[0] = (((new_r11 * x786)) + gconst10 +
16806  ((new_r01 * x785)));
16807  evalcond[1] =
16808  (((gconst10 * x786)) + x788 + new_r11);
16809  evalcond[2] = (((new_r10 * x785)) + gconst10 +
16810  (((-1.0) * new_r00 * x789)));
16811  evalcond[3] = ((((-1.0) * new_r01 * x789)) +
16812  ((new_r11 * x785)) + gconst11);
16813  evalcond[4] = ((((-1.0) * x790)) + x787 + new_r10);
16814  evalcond[5] = ((((-1.0) * x790)) + x787 + new_r01);
16815  evalcond[6] =
16816  (((new_r10 * x786)) + ((new_r00 * x785)) +
16817  (((-1.0) * gconst11)));
16818  evalcond[7] = ((((-1.0) * x788)) + new_r00 +
16819  (((-1.0) * gconst10 * x789)));
16820  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
16821  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
16822  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
16823  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
16824  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
16825  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
16826  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
16827  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16828  {
16829  continue;
16830  }
16831  }
16832 
16833  {
16834  std::vector<IkSingleDOFSolutionBase<IkReal> >
16835  vinfos(7);
16836  vinfos[0].jointtype = 1;
16837  vinfos[0].foffset = j0;
16838  vinfos[0].indices[0] = _ij0[0];
16839  vinfos[0].indices[1] = _ij0[1];
16840  vinfos[0].maxsolutions = _nj0;
16841  vinfos[1].jointtype = 1;
16842  vinfos[1].foffset = j1;
16843  vinfos[1].indices[0] = _ij1[0];
16844  vinfos[1].indices[1] = _ij1[1];
16845  vinfos[1].maxsolutions = _nj1;
16846  vinfos[2].jointtype = 1;
16847  vinfos[2].foffset = j2;
16848  vinfos[2].indices[0] = _ij2[0];
16849  vinfos[2].indices[1] = _ij2[1];
16850  vinfos[2].maxsolutions = _nj2;
16851  vinfos[3].jointtype = 1;
16852  vinfos[3].foffset = j3;
16853  vinfos[3].indices[0] = _ij3[0];
16854  vinfos[3].indices[1] = _ij3[1];
16855  vinfos[3].maxsolutions = _nj3;
16856  vinfos[4].jointtype = 1;
16857  vinfos[4].foffset = j4;
16858  vinfos[4].indices[0] = _ij4[0];
16859  vinfos[4].indices[1] = _ij4[1];
16860  vinfos[4].maxsolutions = _nj4;
16861  vinfos[5].jointtype = 1;
16862  vinfos[5].foffset = j5;
16863  vinfos[5].indices[0] = _ij5[0];
16864  vinfos[5].indices[1] = _ij5[1];
16865  vinfos[5].maxsolutions = _nj5;
16866  vinfos[6].jointtype = 1;
16867  vinfos[6].foffset = j6;
16868  vinfos[6].indices[0] = _ij6[0];
16869  vinfos[6].indices[1] = _ij6[1];
16870  vinfos[6].maxsolutions = _nj6;
16871  std::vector<int> vfree(0);
16872  solutions.AddSolution(vinfos, vfree);
16873  }
16874  }
16875  }
16876  }
16877  }
16878  }
16879  } while (0);
16880  if (bgotonextstatement)
16881  {
16882  bool bgotonextstatement = true;
16883  do
16884  {
16885  evalcond[0] = ((new_r01 * new_r01) + (new_r11 * new_r11));
16886  if (IKabs(evalcond[0]) < 0.0000050000000000)
16887  {
16888  bgotonextstatement = false;
16889  {
16890  IkReal j0eval[1];
16891  sj1 = 0;
16892  cj1 = -1.0;
16893  j1 = 3.14159265358979;
16894  new_r01 = 0;
16895  new_r11 = 0;
16896  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
16897  if (IKabs(j0eval[0]) < 0.0000010000000000)
16898  {
16899  continue; // 3 cases reached
16900  }
16901  else
16902  {
16903  {
16904  IkReal j0array[2], cj0array[2], sj0array[2];
16905  bool j0valid[2] = { false };
16906  _nj0 = 2;
16907  CheckValue<IkReal> x792 =
16908  IKatan2WithCheck(IkReal(new_r00),
16909  IkReal(new_r10),
16910  IKFAST_ATAN2_MAGTHRESH);
16911  if (!x792.valid)
16912  {
16913  continue;
16914  }
16915  IkReal x791 = x792.value;
16916  j0array[0] = ((-1.0) * x791);
16917  sj0array[0] = IKsin(j0array[0]);
16918  cj0array[0] = IKcos(j0array[0]);
16919  j0array[1] = ((3.14159265358979) + (((-1.0) * x791)));
16920  sj0array[1] = IKsin(j0array[1]);
16921  cj0array[1] = IKcos(j0array[1]);
16922  if (j0array[0] > IKPI)
16923  {
16924  j0array[0] -= IK2PI;
16925  }
16926  else if (j0array[0] < -IKPI)
16927  {
16928  j0array[0] += IK2PI;
16929  }
16930  j0valid[0] = true;
16931  if (j0array[1] > IKPI)
16932  {
16933  j0array[1] -= IK2PI;
16934  }
16935  else if (j0array[1] < -IKPI)
16936  {
16937  j0array[1] += IK2PI;
16938  }
16939  j0valid[1] = true;
16940  for (int ij0 = 0; ij0 < 2; ++ij0)
16941  {
16942  if (!j0valid[ij0])
16943  {
16944  continue;
16945  }
16946  _ij0[0] = ij0;
16947  _ij0[1] = -1;
16948  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
16949  {
16950  if (j0valid[iij0] &&
16951  IKabs(cj0array[ij0] - cj0array[iij0]) <
16952  IKFAST_SOLUTION_THRESH &&
16953  IKabs(sj0array[ij0] - sj0array[iij0]) <
16954  IKFAST_SOLUTION_THRESH)
16955  {
16956  j0valid[iij0] = false;
16957  _ij0[1] = iij0;
16958  break;
16959  }
16960  }
16961  j0 = j0array[ij0];
16962  cj0 = cj0array[ij0];
16963  sj0 = sj0array[ij0];
16964  {
16965  IkReal evalcond[1];
16966  evalcond[0] =
16967  ((((-1.0) * new_r00 * (IKsin(j0)))) +
16968  ((new_r10 * (IKcos(j0)))));
16969  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
16970  {
16971  continue;
16972  }
16973  }
16974 
16975  {
16976  std::vector<IkSingleDOFSolutionBase<IkReal> >
16977  vinfos(7);
16978  vinfos[0].jointtype = 1;
16979  vinfos[0].foffset = j0;
16980  vinfos[0].indices[0] = _ij0[0];
16981  vinfos[0].indices[1] = _ij0[1];
16982  vinfos[0].maxsolutions = _nj0;
16983  vinfos[1].jointtype = 1;
16984  vinfos[1].foffset = j1;
16985  vinfos[1].indices[0] = _ij1[0];
16986  vinfos[1].indices[1] = _ij1[1];
16987  vinfos[1].maxsolutions = _nj1;
16988  vinfos[2].jointtype = 1;
16989  vinfos[2].foffset = j2;
16990  vinfos[2].indices[0] = _ij2[0];
16991  vinfos[2].indices[1] = _ij2[1];
16992  vinfos[2].maxsolutions = _nj2;
16993  vinfos[3].jointtype = 1;
16994  vinfos[3].foffset = j3;
16995  vinfos[3].indices[0] = _ij3[0];
16996  vinfos[3].indices[1] = _ij3[1];
16997  vinfos[3].maxsolutions = _nj3;
16998  vinfos[4].jointtype = 1;
16999  vinfos[4].foffset = j4;
17000  vinfos[4].indices[0] = _ij4[0];
17001  vinfos[4].indices[1] = _ij4[1];
17002  vinfos[4].maxsolutions = _nj4;
17003  vinfos[5].jointtype = 1;
17004  vinfos[5].foffset = j5;
17005  vinfos[5].indices[0] = _ij5[0];
17006  vinfos[5].indices[1] = _ij5[1];
17007  vinfos[5].maxsolutions = _nj5;
17008  vinfos[6].jointtype = 1;
17009  vinfos[6].foffset = j6;
17010  vinfos[6].indices[0] = _ij6[0];
17011  vinfos[6].indices[1] = _ij6[1];
17012  vinfos[6].maxsolutions = _nj6;
17013  std::vector<int> vfree(0);
17014  solutions.AddSolution(vinfos, vfree);
17015  }
17016  }
17017  }
17018  }
17019  }
17020  }
17021  } while (0);
17022  if (bgotonextstatement)
17023  {
17024  bool bgotonextstatement = true;
17025  do
17026  {
17027  evalcond[0] = ((-3.14159265358979) +
17028  (IKfmod(((3.14159265358979) + (IKabs(j2))),
17029  6.28318530717959)));
17030  if (IKabs(evalcond[0]) < 0.0000050000000000)
17031  {
17032  bgotonextstatement = false;
17033  {
17034  IkReal j0array[1], cj0array[1], sj0array[1];
17035  bool j0valid[1] = { false };
17036  _nj0 = 1;
17037  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
17038  IKabs(((-1.0) * new_r11)) <
17039  IKFAST_ATAN2_MAGTHRESH &&
17040  IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) -
17041  1) <= IKFAST_SINCOS_THRESH)
17042  continue;
17043  j0array[0] = IKatan2(new_r10, ((-1.0) * new_r11));
17044  sj0array[0] = IKsin(j0array[0]);
17045  cj0array[0] = IKcos(j0array[0]);
17046  if (j0array[0] > IKPI)
17047  {
17048  j0array[0] -= IK2PI;
17049  }
17050  else if (j0array[0] < -IKPI)
17051  {
17052  j0array[0] += IK2PI;
17053  }
17054  j0valid[0] = true;
17055  for (int ij0 = 0; ij0 < 1; ++ij0)
17056  {
17057  if (!j0valid[ij0])
17058  {
17059  continue;
17060  }
17061  _ij0[0] = ij0;
17062  _ij0[1] = -1;
17063  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17064  {
17065  if (j0valid[iij0] &&
17066  IKabs(cj0array[ij0] - cj0array[iij0]) <
17067  IKFAST_SOLUTION_THRESH &&
17068  IKabs(sj0array[ij0] - sj0array[iij0]) <
17069  IKFAST_SOLUTION_THRESH)
17070  {
17071  j0valid[iij0] = false;
17072  _ij0[1] = iij0;
17073  break;
17074  }
17075  }
17076  j0 = j0array[ij0];
17077  cj0 = cj0array[ij0];
17078  sj0 = sj0array[ij0];
17079  {
17080  IkReal evalcond[8];
17081  IkReal x793 = IKcos(j0);
17082  IkReal x794 = IKsin(j0);
17083  IkReal x795 = ((1.0) * x794);
17084  evalcond[0] = (x793 + new_r11);
17085  evalcond[1] = ((((-1.0) * x795)) + new_r10);
17086  evalcond[2] = ((((-1.0) * x793)) + new_r00);
17087  evalcond[3] = ((((-1.0) * x795)) + new_r01);
17088  evalcond[4] =
17089  (((new_r11 * x794)) + ((new_r01 * x793)));
17090  evalcond[5] = (((new_r10 * x793)) +
17091  (((-1.0) * new_r00 * x795)));
17092  evalcond[6] = ((-1.0) + ((new_r10 * x794)) +
17093  ((new_r00 * x793)));
17094  evalcond[7] = ((1.0) + (((-1.0) * new_r01 * x795)) +
17095  ((new_r11 * x793)));
17096  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17097  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17098  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17099  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17100  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17101  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17102  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17103  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17104  {
17105  continue;
17106  }
17107  }
17108 
17109  {
17110  std::vector<IkSingleDOFSolutionBase<IkReal> >
17111  vinfos(7);
17112  vinfos[0].jointtype = 1;
17113  vinfos[0].foffset = j0;
17114  vinfos[0].indices[0] = _ij0[0];
17115  vinfos[0].indices[1] = _ij0[1];
17116  vinfos[0].maxsolutions = _nj0;
17117  vinfos[1].jointtype = 1;
17118  vinfos[1].foffset = j1;
17119  vinfos[1].indices[0] = _ij1[0];
17120  vinfos[1].indices[1] = _ij1[1];
17121  vinfos[1].maxsolutions = _nj1;
17122  vinfos[2].jointtype = 1;
17123  vinfos[2].foffset = j2;
17124  vinfos[2].indices[0] = _ij2[0];
17125  vinfos[2].indices[1] = _ij2[1];
17126  vinfos[2].maxsolutions = _nj2;
17127  vinfos[3].jointtype = 1;
17128  vinfos[3].foffset = j3;
17129  vinfos[3].indices[0] = _ij3[0];
17130  vinfos[3].indices[1] = _ij3[1];
17131  vinfos[3].maxsolutions = _nj3;
17132  vinfos[4].jointtype = 1;
17133  vinfos[4].foffset = j4;
17134  vinfos[4].indices[0] = _ij4[0];
17135  vinfos[4].indices[1] = _ij4[1];
17136  vinfos[4].maxsolutions = _nj4;
17137  vinfos[5].jointtype = 1;
17138  vinfos[5].foffset = j5;
17139  vinfos[5].indices[0] = _ij5[0];
17140  vinfos[5].indices[1] = _ij5[1];
17141  vinfos[5].maxsolutions = _nj5;
17142  vinfos[6].jointtype = 1;
17143  vinfos[6].foffset = j6;
17144  vinfos[6].indices[0] = _ij6[0];
17145  vinfos[6].indices[1] = _ij6[1];
17146  vinfos[6].maxsolutions = _nj6;
17147  std::vector<int> vfree(0);
17148  solutions.AddSolution(vinfos, vfree);
17149  }
17150  }
17151  }
17152  }
17153  } while (0);
17154  if (bgotonextstatement)
17155  {
17156  bool bgotonextstatement = true;
17157  do
17158  {
17159  evalcond[0] =
17160  ((-3.14159265358979) +
17161  (IKfmod(((3.14159265358979) +
17162  (IKabs(((-3.14159265358979) + j2)))),
17163  6.28318530717959)));
17164  if (IKabs(evalcond[0]) < 0.0000050000000000)
17165  {
17166  bgotonextstatement = false;
17167  {
17168  IkReal j0array[1], cj0array[1], sj0array[1];
17169  bool j0valid[1] = { false };
17170  _nj0 = 1;
17171  if (IKabs(((-1.0) * new_r10)) <
17172  IKFAST_ATAN2_MAGTHRESH &&
17173  IKabs(((-1.0) * new_r00)) <
17174  IKFAST_ATAN2_MAGTHRESH &&
17175  IKabs(IKsqr(((-1.0) * new_r10)) +
17176  IKsqr(((-1.0) * new_r00)) - 1) <=
17177  IKFAST_SINCOS_THRESH)
17178  continue;
17179  j0array[0] =
17180  IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
17181  sj0array[0] = IKsin(j0array[0]);
17182  cj0array[0] = IKcos(j0array[0]);
17183  if (j0array[0] > IKPI)
17184  {
17185  j0array[0] -= IK2PI;
17186  }
17187  else if (j0array[0] < -IKPI)
17188  {
17189  j0array[0] += IK2PI;
17190  }
17191  j0valid[0] = true;
17192  for (int ij0 = 0; ij0 < 1; ++ij0)
17193  {
17194  if (!j0valid[ij0])
17195  {
17196  continue;
17197  }
17198  _ij0[0] = ij0;
17199  _ij0[1] = -1;
17200  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17201  {
17202  if (j0valid[iij0] &&
17203  IKabs(cj0array[ij0] - cj0array[iij0]) <
17204  IKFAST_SOLUTION_THRESH &&
17205  IKabs(sj0array[ij0] - sj0array[iij0]) <
17206  IKFAST_SOLUTION_THRESH)
17207  {
17208  j0valid[iij0] = false;
17209  _ij0[1] = iij0;
17210  break;
17211  }
17212  }
17213  j0 = j0array[ij0];
17214  cj0 = cj0array[ij0];
17215  sj0 = sj0array[ij0];
17216  {
17217  IkReal evalcond[8];
17218  IkReal x796 = IKsin(j0);
17219  IkReal x797 = IKcos(j0);
17220  IkReal x798 = ((1.0) * x796);
17221  evalcond[0] = (x796 + new_r10);
17222  evalcond[1] = (x797 + new_r00);
17223  evalcond[2] = (x796 + new_r01);
17224  evalcond[3] = ((((-1.0) * x797)) + new_r11);
17225  evalcond[4] =
17226  (((new_r11 * x796)) + ((new_r01 * x797)));
17227  evalcond[5] = (((new_r10 * x797)) +
17228  (((-1.0) * new_r00 * x798)));
17229  evalcond[6] = ((1.0) + ((new_r10 * x796)) +
17230  ((new_r00 * x797)));
17231  evalcond[7] =
17232  ((-1.0) + (((-1.0) * new_r01 * x798)) +
17233  ((new_r11 * x797)));
17234  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17235  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17236  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17237  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17238  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17239  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17240  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17241  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17242  {
17243  continue;
17244  }
17245  }
17246 
17247  {
17248  std::vector<IkSingleDOFSolutionBase<IkReal> >
17249  vinfos(7);
17250  vinfos[0].jointtype = 1;
17251  vinfos[0].foffset = j0;
17252  vinfos[0].indices[0] = _ij0[0];
17253  vinfos[0].indices[1] = _ij0[1];
17254  vinfos[0].maxsolutions = _nj0;
17255  vinfos[1].jointtype = 1;
17256  vinfos[1].foffset = j1;
17257  vinfos[1].indices[0] = _ij1[0];
17258  vinfos[1].indices[1] = _ij1[1];
17259  vinfos[1].maxsolutions = _nj1;
17260  vinfos[2].jointtype = 1;
17261  vinfos[2].foffset = j2;
17262  vinfos[2].indices[0] = _ij2[0];
17263  vinfos[2].indices[1] = _ij2[1];
17264  vinfos[2].maxsolutions = _nj2;
17265  vinfos[3].jointtype = 1;
17266  vinfos[3].foffset = j3;
17267  vinfos[3].indices[0] = _ij3[0];
17268  vinfos[3].indices[1] = _ij3[1];
17269  vinfos[3].maxsolutions = _nj3;
17270  vinfos[4].jointtype = 1;
17271  vinfos[4].foffset = j4;
17272  vinfos[4].indices[0] = _ij4[0];
17273  vinfos[4].indices[1] = _ij4[1];
17274  vinfos[4].maxsolutions = _nj4;
17275  vinfos[5].jointtype = 1;
17276  vinfos[5].foffset = j5;
17277  vinfos[5].indices[0] = _ij5[0];
17278  vinfos[5].indices[1] = _ij5[1];
17279  vinfos[5].maxsolutions = _nj5;
17280  vinfos[6].jointtype = 1;
17281  vinfos[6].foffset = j6;
17282  vinfos[6].indices[0] = _ij6[0];
17283  vinfos[6].indices[1] = _ij6[1];
17284  vinfos[6].maxsolutions = _nj6;
17285  std::vector<int> vfree(0);
17286  solutions.AddSolution(vinfos, vfree);
17287  }
17288  }
17289  }
17290  }
17291  } while (0);
17292  if (bgotonextstatement)
17293  {
17294  bool bgotonextstatement = true;
17295  do
17296  {
17297  evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r00)));
17298  if (IKabs(evalcond[0]) < 0.0000050000000000)
17299  {
17300  bgotonextstatement = false;
17301  {
17302  IkReal j0eval[3];
17303  sj1 = 0;
17304  cj1 = -1.0;
17305  j1 = 3.14159265358979;
17306  new_r11 = 0;
17307  new_r00 = 0;
17308  j0eval[0] = new_r01;
17309  j0eval[1] = IKsign(new_r01);
17310  j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
17311  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17312  IKabs(j0eval[1]) < 0.0000010000000000 ||
17313  IKabs(j0eval[2]) < 0.0000010000000000)
17314  {
17315  {
17316  IkReal j0eval[3];
17317  sj1 = 0;
17318  cj1 = -1.0;
17319  j1 = 3.14159265358979;
17320  new_r11 = 0;
17321  new_r00 = 0;
17322  j0eval[0] = new_r10;
17323  j0eval[1] = ((IKabs(cj2)) + (IKabs(sj2)));
17324  j0eval[2] = IKsign(new_r10);
17325  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17326  IKabs(j0eval[1]) < 0.0000010000000000 ||
17327  IKabs(j0eval[2]) < 0.0000010000000000)
17328  {
17329  {
17330  IkReal j0eval[2];
17331  sj1 = 0;
17332  cj1 = -1.0;
17333  j1 = 3.14159265358979;
17334  new_r11 = 0;
17335  new_r00 = 0;
17336  j0eval[0] = new_r01;
17337  j0eval[1] = new_r10;
17338  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17339  IKabs(j0eval[1]) < 0.0000010000000000)
17340  {
17341  continue; // no branches [j0]
17342  }
17343  else
17344  {
17345  {
17346  IkReal j0array[1], cj0array[1],
17347  sj0array[1];
17348  bool j0valid[1] = { false };
17349  _nj0 = 1;
17350  CheckValue<IkReal> x799 =
17351  IKPowWithIntegerCheck(new_r01, -1);
17352  if (!x799.valid)
17353  {
17354  continue;
17355  }
17356  CheckValue<IkReal> x800 =
17357  IKPowWithIntegerCheck(new_r10, -1);
17358  if (!x800.valid)
17359  {
17360  continue;
17361  }
17362  if (IKabs((cj2 * (x799.value))) <
17363  IKFAST_ATAN2_MAGTHRESH &&
17364  IKabs(
17365  ((-1.0) * sj2 * (x800.value))) <
17366  IKFAST_ATAN2_MAGTHRESH &&
17367  IKabs(IKsqr((cj2 * (x799.value))) +
17368  IKsqr(((-1.0) * sj2 *
17369  (x800.value))) -
17370  1) <= IKFAST_SINCOS_THRESH)
17371  continue;
17372  j0array[0] = IKatan2(
17373  (cj2 * (x799.value)),
17374  ((-1.0) * sj2 * (x800.value)));
17375  sj0array[0] = IKsin(j0array[0]);
17376  cj0array[0] = IKcos(j0array[0]);
17377  if (j0array[0] > IKPI)
17378  {
17379  j0array[0] -= IK2PI;
17380  }
17381  else if (j0array[0] < -IKPI)
17382  {
17383  j0array[0] += IK2PI;
17384  }
17385  j0valid[0] = true;
17386  for (int ij0 = 0; ij0 < 1; ++ij0)
17387  {
17388  if (!j0valid[ij0])
17389  {
17390  continue;
17391  }
17392  _ij0[0] = ij0;
17393  _ij0[1] = -1;
17394  for (int iij0 = ij0 + 1; iij0 < 1;
17395  ++iij0)
17396  {
17397  if (j0valid[iij0] &&
17398  IKabs(cj0array[ij0] -
17399  cj0array[iij0]) <
17400  IKFAST_SOLUTION_THRESH &&
17401  IKabs(sj0array[ij0] -
17402  sj0array[iij0]) <
17403  IKFAST_SOLUTION_THRESH)
17404  {
17405  j0valid[iij0] = false;
17406  _ij0[1] = iij0;
17407  break;
17408  }
17409  }
17410  j0 = j0array[ij0];
17411  cj0 = cj0array[ij0];
17412  sj0 = sj0array[ij0];
17413  {
17414  IkReal evalcond[7];
17415  IkReal x801 = IKcos(j0);
17416  IkReal x802 = IKsin(j0);
17417  IkReal x803 = ((1.0) * cj2);
17418  IkReal x804 = (sj2 * x801);
17419  IkReal x805 = ((1.0) * x802);
17420  IkReal x806 = (x802 * x803);
17421  evalcond[0] =
17422  (sj2 + ((new_r10 * x801)));
17423  evalcond[1] =
17424  (sj2 + ((new_r01 * x801)));
17425  evalcond[2] =
17426  ((((-1.0) * new_r01 * x805)) +
17427  cj2);
17428  evalcond[3] = (((new_r10 * x802)) +
17429  (((-1.0) * x803)));
17430  evalcond[4] = (new_r10 + x804 +
17431  (((-1.0) * x806)));
17432  evalcond[5] =
17433  ((((-1.0) * sj2 * x805)) +
17434  (((-1.0) * x801 * x803)));
17435  evalcond[6] = (new_r01 + x804 +
17436  (((-1.0) * x806)));
17437  if (IKabs(evalcond[0]) >
17438  IKFAST_EVALCOND_THRESH ||
17439  IKabs(evalcond[1]) >
17440  IKFAST_EVALCOND_THRESH ||
17441  IKabs(evalcond[2]) >
17442  IKFAST_EVALCOND_THRESH ||
17443  IKabs(evalcond[3]) >
17444  IKFAST_EVALCOND_THRESH ||
17445  IKabs(evalcond[4]) >
17446  IKFAST_EVALCOND_THRESH ||
17447  IKabs(evalcond[5]) >
17448  IKFAST_EVALCOND_THRESH ||
17449  IKabs(evalcond[6]) >
17450  IKFAST_EVALCOND_THRESH)
17451  {
17452  continue;
17453  }
17454  }
17455 
17456  {
17457  std::vector<IkSingleDOFSolutionBase<
17458  IkReal> >
17459  vinfos(7);
17460  vinfos[0].jointtype = 1;
17461  vinfos[0].foffset = j0;
17462  vinfos[0].indices[0] = _ij0[0];
17463  vinfos[0].indices[1] = _ij0[1];
17464  vinfos[0].maxsolutions = _nj0;
17465  vinfos[1].jointtype = 1;
17466  vinfos[1].foffset = j1;
17467  vinfos[1].indices[0] = _ij1[0];
17468  vinfos[1].indices[1] = _ij1[1];
17469  vinfos[1].maxsolutions = _nj1;
17470  vinfos[2].jointtype = 1;
17471  vinfos[2].foffset = j2;
17472  vinfos[2].indices[0] = _ij2[0];
17473  vinfos[2].indices[1] = _ij2[1];
17474  vinfos[2].maxsolutions = _nj2;
17475  vinfos[3].jointtype = 1;
17476  vinfos[3].foffset = j3;
17477  vinfos[3].indices[0] = _ij3[0];
17478  vinfos[3].indices[1] = _ij3[1];
17479  vinfos[3].maxsolutions = _nj3;
17480  vinfos[4].jointtype = 1;
17481  vinfos[4].foffset = j4;
17482  vinfos[4].indices[0] = _ij4[0];
17483  vinfos[4].indices[1] = _ij4[1];
17484  vinfos[4].maxsolutions = _nj4;
17485  vinfos[5].jointtype = 1;
17486  vinfos[5].foffset = j5;
17487  vinfos[5].indices[0] = _ij5[0];
17488  vinfos[5].indices[1] = _ij5[1];
17489  vinfos[5].maxsolutions = _nj5;
17490  vinfos[6].jointtype = 1;
17491  vinfos[6].foffset = j6;
17492  vinfos[6].indices[0] = _ij6[0];
17493  vinfos[6].indices[1] = _ij6[1];
17494  vinfos[6].maxsolutions = _nj6;
17495  std::vector<int> vfree(0);
17496  solutions.AddSolution(vinfos,
17497  vfree);
17498  }
17499  }
17500  }
17501  }
17502  }
17503  }
17504  else
17505  {
17506  {
17507  IkReal j0array[1], cj0array[1], sj0array[1];
17508  bool j0valid[1] = { false };
17509  _nj0 = 1;
17510  CheckValue<IkReal> x807 =
17511  IKPowWithIntegerCheck(IKsign(new_r10),
17512  -1);
17513  if (!x807.valid)
17514  {
17515  continue;
17516  }
17517  CheckValue<IkReal> x808 = IKatan2WithCheck(
17518  IkReal(cj2),
17519  IkReal(((-1.0) * sj2)),
17520  IKFAST_ATAN2_MAGTHRESH);
17521  if (!x808.valid)
17522  {
17523  continue;
17524  }
17525  j0array[0] =
17526  ((-1.5707963267949) +
17527  (((1.5707963267949) * (x807.value))) +
17528  (x808.value));
17529  sj0array[0] = IKsin(j0array[0]);
17530  cj0array[0] = IKcos(j0array[0]);
17531  if (j0array[0] > IKPI)
17532  {
17533  j0array[0] -= IK2PI;
17534  }
17535  else if (j0array[0] < -IKPI)
17536  {
17537  j0array[0] += IK2PI;
17538  }
17539  j0valid[0] = true;
17540  for (int ij0 = 0; ij0 < 1; ++ij0)
17541  {
17542  if (!j0valid[ij0])
17543  {
17544  continue;
17545  }
17546  _ij0[0] = ij0;
17547  _ij0[1] = -1;
17548  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17549  {
17550  if (j0valid[iij0] &&
17551  IKabs(cj0array[ij0] -
17552  cj0array[iij0]) <
17553  IKFAST_SOLUTION_THRESH &&
17554  IKabs(sj0array[ij0] -
17555  sj0array[iij0]) <
17556  IKFAST_SOLUTION_THRESH)
17557  {
17558  j0valid[iij0] = false;
17559  _ij0[1] = iij0;
17560  break;
17561  }
17562  }
17563  j0 = j0array[ij0];
17564  cj0 = cj0array[ij0];
17565  sj0 = sj0array[ij0];
17566  {
17567  IkReal evalcond[7];
17568  IkReal x809 = IKcos(j0);
17569  IkReal x810 = IKsin(j0);
17570  IkReal x811 = ((1.0) * cj2);
17571  IkReal x812 = (sj2 * x809);
17572  IkReal x813 = ((1.0) * x810);
17573  IkReal x814 = (x810 * x811);
17574  evalcond[0] =
17575  (sj2 + ((new_r10 * x809)));
17576  evalcond[1] =
17577  (sj2 + ((new_r01 * x809)));
17578  evalcond[2] =
17579  (cj2 + (((-1.0) * new_r01 * x813)));
17580  evalcond[3] = (((new_r10 * x810)) +
17581  (((-1.0) * x811)));
17582  evalcond[4] = ((((-1.0) * x814)) +
17583  new_r10 + x812);
17584  evalcond[5] =
17585  ((((-1.0) * x809 * x811)) +
17586  (((-1.0) * sj2 * x813)));
17587  evalcond[6] = ((((-1.0) * x814)) +
17588  new_r01 + x812);
17589  if (IKabs(evalcond[0]) >
17590  IKFAST_EVALCOND_THRESH ||
17591  IKabs(evalcond[1]) >
17592  IKFAST_EVALCOND_THRESH ||
17593  IKabs(evalcond[2]) >
17594  IKFAST_EVALCOND_THRESH ||
17595  IKabs(evalcond[3]) >
17596  IKFAST_EVALCOND_THRESH ||
17597  IKabs(evalcond[4]) >
17598  IKFAST_EVALCOND_THRESH ||
17599  IKabs(evalcond[5]) >
17600  IKFAST_EVALCOND_THRESH ||
17601  IKabs(evalcond[6]) >
17602  IKFAST_EVALCOND_THRESH)
17603  {
17604  continue;
17605  }
17606  }
17607 
17608  {
17609  std::vector<
17610  IkSingleDOFSolutionBase<IkReal> >
17611  vinfos(7);
17612  vinfos[0].jointtype = 1;
17613  vinfos[0].foffset = j0;
17614  vinfos[0].indices[0] = _ij0[0];
17615  vinfos[0].indices[1] = _ij0[1];
17616  vinfos[0].maxsolutions = _nj0;
17617  vinfos[1].jointtype = 1;
17618  vinfos[1].foffset = j1;
17619  vinfos[1].indices[0] = _ij1[0];
17620  vinfos[1].indices[1] = _ij1[1];
17621  vinfos[1].maxsolutions = _nj1;
17622  vinfos[2].jointtype = 1;
17623  vinfos[2].foffset = j2;
17624  vinfos[2].indices[0] = _ij2[0];
17625  vinfos[2].indices[1] = _ij2[1];
17626  vinfos[2].maxsolutions = _nj2;
17627  vinfos[3].jointtype = 1;
17628  vinfos[3].foffset = j3;
17629  vinfos[3].indices[0] = _ij3[0];
17630  vinfos[3].indices[1] = _ij3[1];
17631  vinfos[3].maxsolutions = _nj3;
17632  vinfos[4].jointtype = 1;
17633  vinfos[4].foffset = j4;
17634  vinfos[4].indices[0] = _ij4[0];
17635  vinfos[4].indices[1] = _ij4[1];
17636  vinfos[4].maxsolutions = _nj4;
17637  vinfos[5].jointtype = 1;
17638  vinfos[5].foffset = j5;
17639  vinfos[5].indices[0] = _ij5[0];
17640  vinfos[5].indices[1] = _ij5[1];
17641  vinfos[5].maxsolutions = _nj5;
17642  vinfos[6].jointtype = 1;
17643  vinfos[6].foffset = j6;
17644  vinfos[6].indices[0] = _ij6[0];
17645  vinfos[6].indices[1] = _ij6[1];
17646  vinfos[6].maxsolutions = _nj6;
17647  std::vector<int> vfree(0);
17648  solutions.AddSolution(vinfos, vfree);
17649  }
17650  }
17651  }
17652  }
17653  }
17654  }
17655  else
17656  {
17657  {
17658  IkReal j0array[1], cj0array[1], sj0array[1];
17659  bool j0valid[1] = { false };
17660  _nj0 = 1;
17661  CheckValue<IkReal> x815 =
17662  IKPowWithIntegerCheck(IKsign(new_r01), -1);
17663  if (!x815.valid)
17664  {
17665  continue;
17666  }
17667  CheckValue<IkReal> x816 =
17668  IKatan2WithCheck(IkReal(cj2),
17669  IkReal(((-1.0) * sj2)),
17670  IKFAST_ATAN2_MAGTHRESH);
17671  if (!x816.valid)
17672  {
17673  continue;
17674  }
17675  j0array[0] =
17676  ((-1.5707963267949) +
17677  (((1.5707963267949) * (x815.value))) +
17678  (x816.value));
17679  sj0array[0] = IKsin(j0array[0]);
17680  cj0array[0] = IKcos(j0array[0]);
17681  if (j0array[0] > IKPI)
17682  {
17683  j0array[0] -= IK2PI;
17684  }
17685  else if (j0array[0] < -IKPI)
17686  {
17687  j0array[0] += IK2PI;
17688  }
17689  j0valid[0] = true;
17690  for (int ij0 = 0; ij0 < 1; ++ij0)
17691  {
17692  if (!j0valid[ij0])
17693  {
17694  continue;
17695  }
17696  _ij0[0] = ij0;
17697  _ij0[1] = -1;
17698  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17699  {
17700  if (j0valid[iij0] &&
17701  IKabs(cj0array[ij0] - cj0array[iij0]) <
17702  IKFAST_SOLUTION_THRESH &&
17703  IKabs(sj0array[ij0] - sj0array[iij0]) <
17704  IKFAST_SOLUTION_THRESH)
17705  {
17706  j0valid[iij0] = false;
17707  _ij0[1] = iij0;
17708  break;
17709  }
17710  }
17711  j0 = j0array[ij0];
17712  cj0 = cj0array[ij0];
17713  sj0 = sj0array[ij0];
17714  {
17715  IkReal evalcond[7];
17716  IkReal x817 = IKcos(j0);
17717  IkReal x818 = IKsin(j0);
17718  IkReal x819 = ((1.0) * cj2);
17719  IkReal x820 = (sj2 * x817);
17720  IkReal x821 = ((1.0) * x818);
17721  IkReal x822 = (x818 * x819);
17722  evalcond[0] = (sj2 + ((new_r10 * x817)));
17723  evalcond[1] = (sj2 + ((new_r01 * x817)));
17724  evalcond[2] =
17725  (cj2 + (((-1.0) * new_r01 * x821)));
17726  evalcond[3] = (((new_r10 * x818)) +
17727  (((-1.0) * x819)));
17728  evalcond[4] =
17729  ((((-1.0) * x822)) + new_r10 + x820);
17730  evalcond[5] = ((((-1.0) * x817 * x819)) +
17731  (((-1.0) * sj2 * x821)));
17732  evalcond[6] =
17733  ((((-1.0) * x822)) + new_r01 + x820);
17734  if (IKabs(evalcond[0]) >
17735  IKFAST_EVALCOND_THRESH ||
17736  IKabs(evalcond[1]) >
17737  IKFAST_EVALCOND_THRESH ||
17738  IKabs(evalcond[2]) >
17739  IKFAST_EVALCOND_THRESH ||
17740  IKabs(evalcond[3]) >
17741  IKFAST_EVALCOND_THRESH ||
17742  IKabs(evalcond[4]) >
17743  IKFAST_EVALCOND_THRESH ||
17744  IKabs(evalcond[5]) >
17745  IKFAST_EVALCOND_THRESH ||
17746  IKabs(evalcond[6]) >
17747  IKFAST_EVALCOND_THRESH)
17748  {
17749  continue;
17750  }
17751  }
17752 
17753  {
17754  std::vector<
17755  IkSingleDOFSolutionBase<IkReal> >
17756  vinfos(7);
17757  vinfos[0].jointtype = 1;
17758  vinfos[0].foffset = j0;
17759  vinfos[0].indices[0] = _ij0[0];
17760  vinfos[0].indices[1] = _ij0[1];
17761  vinfos[0].maxsolutions = _nj0;
17762  vinfos[1].jointtype = 1;
17763  vinfos[1].foffset = j1;
17764  vinfos[1].indices[0] = _ij1[0];
17765  vinfos[1].indices[1] = _ij1[1];
17766  vinfos[1].maxsolutions = _nj1;
17767  vinfos[2].jointtype = 1;
17768  vinfos[2].foffset = j2;
17769  vinfos[2].indices[0] = _ij2[0];
17770  vinfos[2].indices[1] = _ij2[1];
17771  vinfos[2].maxsolutions = _nj2;
17772  vinfos[3].jointtype = 1;
17773  vinfos[3].foffset = j3;
17774  vinfos[3].indices[0] = _ij3[0];
17775  vinfos[3].indices[1] = _ij3[1];
17776  vinfos[3].maxsolutions = _nj3;
17777  vinfos[4].jointtype = 1;
17778  vinfos[4].foffset = j4;
17779  vinfos[4].indices[0] = _ij4[0];
17780  vinfos[4].indices[1] = _ij4[1];
17781  vinfos[4].maxsolutions = _nj4;
17782  vinfos[5].jointtype = 1;
17783  vinfos[5].foffset = j5;
17784  vinfos[5].indices[0] = _ij5[0];
17785  vinfos[5].indices[1] = _ij5[1];
17786  vinfos[5].maxsolutions = _nj5;
17787  vinfos[6].jointtype = 1;
17788  vinfos[6].foffset = j6;
17789  vinfos[6].indices[0] = _ij6[0];
17790  vinfos[6].indices[1] = _ij6[1];
17791  vinfos[6].maxsolutions = _nj6;
17792  std::vector<int> vfree(0);
17793  solutions.AddSolution(vinfos, vfree);
17794  }
17795  }
17796  }
17797  }
17798  }
17799  }
17800  } while (0);
17801  if (bgotonextstatement)
17802  {
17803  bool bgotonextstatement = true;
17804  do
17805  {
17806  evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
17807  if (IKabs(evalcond[0]) < 0.0000050000000000)
17808  {
17809  bgotonextstatement = false;
17810  {
17811  IkReal j0eval[1];
17812  sj1 = 0;
17813  cj1 = -1.0;
17814  j1 = 3.14159265358979;
17815  new_r11 = 0;
17816  new_r01 = 0;
17817  new_r22 = 0;
17818  new_r20 = 0;
17819  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17820  if (IKabs(j0eval[0]) < 0.0000010000000000)
17821  {
17822  continue; // no branches [j0]
17823  }
17824  else
17825  {
17826  {
17827  IkReal j0array[2], cj0array[2], sj0array[2];
17828  bool j0valid[2] = { false };
17829  _nj0 = 2;
17830  CheckValue<IkReal> x824 =
17831  IKatan2WithCheck(IkReal(new_r00),
17832  IkReal(new_r10),
17833  IKFAST_ATAN2_MAGTHRESH);
17834  if (!x824.valid)
17835  {
17836  continue;
17837  }
17838  IkReal x823 = x824.value;
17839  j0array[0] = ((-1.0) * x823);
17840  sj0array[0] = IKsin(j0array[0]);
17841  cj0array[0] = IKcos(j0array[0]);
17842  j0array[1] =
17843  ((3.14159265358979) + (((-1.0) * x823)));
17844  sj0array[1] = IKsin(j0array[1]);
17845  cj0array[1] = IKcos(j0array[1]);
17846  if (j0array[0] > IKPI)
17847  {
17848  j0array[0] -= IK2PI;
17849  }
17850  else if (j0array[0] < -IKPI)
17851  {
17852  j0array[0] += IK2PI;
17853  }
17854  j0valid[0] = true;
17855  if (j0array[1] > IKPI)
17856  {
17857  j0array[1] -= IK2PI;
17858  }
17859  else if (j0array[1] < -IKPI)
17860  {
17861  j0array[1] += IK2PI;
17862  }
17863  j0valid[1] = true;
17864  for (int ij0 = 0; ij0 < 2; ++ij0)
17865  {
17866  if (!j0valid[ij0])
17867  {
17868  continue;
17869  }
17870  _ij0[0] = ij0;
17871  _ij0[1] = -1;
17872  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
17873  {
17874  if (j0valid[iij0] &&
17875  IKabs(cj0array[ij0] -
17876  cj0array[iij0]) <
17877  IKFAST_SOLUTION_THRESH &&
17878  IKabs(sj0array[ij0] -
17879  sj0array[iij0]) <
17880  IKFAST_SOLUTION_THRESH)
17881  {
17882  j0valid[iij0] = false;
17883  _ij0[1] = iij0;
17884  break;
17885  }
17886  }
17887  j0 = j0array[ij0];
17888  cj0 = cj0array[ij0];
17889  sj0 = sj0array[ij0];
17890  {
17891  IkReal evalcond[1];
17892  evalcond[0] =
17893  ((((-1.0) * new_r00 * (IKsin(j0)))) +
17894  ((new_r10 * (IKcos(j0)))));
17895  if (IKabs(evalcond[0]) >
17896  IKFAST_EVALCOND_THRESH)
17897  {
17898  continue;
17899  }
17900  }
17901 
17902  {
17903  std::vector<
17904  IkSingleDOFSolutionBase<IkReal> >
17905  vinfos(7);
17906  vinfos[0].jointtype = 1;
17907  vinfos[0].foffset = j0;
17908  vinfos[0].indices[0] = _ij0[0];
17909  vinfos[0].indices[1] = _ij0[1];
17910  vinfos[0].maxsolutions = _nj0;
17911  vinfos[1].jointtype = 1;
17912  vinfos[1].foffset = j1;
17913  vinfos[1].indices[0] = _ij1[0];
17914  vinfos[1].indices[1] = _ij1[1];
17915  vinfos[1].maxsolutions = _nj1;
17916  vinfos[2].jointtype = 1;
17917  vinfos[2].foffset = j2;
17918  vinfos[2].indices[0] = _ij2[0];
17919  vinfos[2].indices[1] = _ij2[1];
17920  vinfos[2].maxsolutions = _nj2;
17921  vinfos[3].jointtype = 1;
17922  vinfos[3].foffset = j3;
17923  vinfos[3].indices[0] = _ij3[0];
17924  vinfos[3].indices[1] = _ij3[1];
17925  vinfos[3].maxsolutions = _nj3;
17926  vinfos[4].jointtype = 1;
17927  vinfos[4].foffset = j4;
17928  vinfos[4].indices[0] = _ij4[0];
17929  vinfos[4].indices[1] = _ij4[1];
17930  vinfos[4].maxsolutions = _nj4;
17931  vinfos[5].jointtype = 1;
17932  vinfos[5].foffset = j5;
17933  vinfos[5].indices[0] = _ij5[0];
17934  vinfos[5].indices[1] = _ij5[1];
17935  vinfos[5].maxsolutions = _nj5;
17936  vinfos[6].jointtype = 1;
17937  vinfos[6].foffset = j6;
17938  vinfos[6].indices[0] = _ij6[0];
17939  vinfos[6].indices[1] = _ij6[1];
17940  vinfos[6].maxsolutions = _nj6;
17941  std::vector<int> vfree(0);
17942  solutions.AddSolution(vinfos, vfree);
17943  }
17944  }
17945  }
17946  }
17947  }
17948  }
17949  } while (0);
17950  if (bgotonextstatement)
17951  {
17952  bool bgotonextstatement = true;
17953  do
17954  {
17955  evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17956  if (IKabs(evalcond[0]) < 0.0000050000000000)
17957  {
17958  bgotonextstatement = false;
17959  {
17960  IkReal j0eval[1];
17961  sj1 = 0;
17962  cj1 = -1.0;
17963  j1 = 3.14159265358979;
17964  new_r00 = 0;
17965  new_r10 = 0;
17966  new_r21 = 0;
17967  new_r22 = 0;
17968  j0eval[0] =
17969  ((IKabs(new_r11)) + (IKabs(new_r01)));
17970  if (IKabs(j0eval[0]) < 0.0000010000000000)
17971  {
17972  continue; // no branches [j0]
17973  }
17974  else
17975  {
17976  {
17977  IkReal j0array[2], cj0array[2], sj0array[2];
17978  bool j0valid[2] = { false };
17979  _nj0 = 2;
17980  CheckValue<IkReal> x826 = IKatan2WithCheck(
17981  IkReal(new_r01),
17982  IkReal(new_r11),
17983  IKFAST_ATAN2_MAGTHRESH);
17984  if (!x826.valid)
17985  {
17986  continue;
17987  }
17988  IkReal x825 = x826.value;
17989  j0array[0] = ((-1.0) * x825);
17990  sj0array[0] = IKsin(j0array[0]);
17991  cj0array[0] = IKcos(j0array[0]);
17992  j0array[1] = ((3.14159265358979) +
17993  (((-1.0) * x825)));
17994  sj0array[1] = IKsin(j0array[1]);
17995  cj0array[1] = IKcos(j0array[1]);
17996  if (j0array[0] > IKPI)
17997  {
17998  j0array[0] -= IK2PI;
17999  }
18000  else if (j0array[0] < -IKPI)
18001  {
18002  j0array[0] += IK2PI;
18003  }
18004  j0valid[0] = true;
18005  if (j0array[1] > IKPI)
18006  {
18007  j0array[1] -= IK2PI;
18008  }
18009  else if (j0array[1] < -IKPI)
18010  {
18011  j0array[1] += IK2PI;
18012  }
18013  j0valid[1] = true;
18014  for (int ij0 = 0; ij0 < 2; ++ij0)
18015  {
18016  if (!j0valid[ij0])
18017  {
18018  continue;
18019  }
18020  _ij0[0] = ij0;
18021  _ij0[1] = -1;
18022  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18023  {
18024  if (j0valid[iij0] &&
18025  IKabs(cj0array[ij0] -
18026  cj0array[iij0]) <
18027  IKFAST_SOLUTION_THRESH &&
18028  IKabs(sj0array[ij0] -
18029  sj0array[iij0]) <
18030  IKFAST_SOLUTION_THRESH)
18031  {
18032  j0valid[iij0] = false;
18033  _ij0[1] = iij0;
18034  break;
18035  }
18036  }
18037  j0 = j0array[ij0];
18038  cj0 = cj0array[ij0];
18039  sj0 = sj0array[ij0];
18040  {
18041  IkReal evalcond[1];
18042  evalcond[0] =
18043  (((new_r11 * (IKcos(j0)))) +
18044  (((-1.0) * new_r01 *
18045  (IKsin(j0)))));
18046  if (IKabs(evalcond[0]) >
18047  IKFAST_EVALCOND_THRESH)
18048  {
18049  continue;
18050  }
18051  }
18052 
18053  {
18054  std::vector<
18055  IkSingleDOFSolutionBase<IkReal> >
18056  vinfos(7);
18057  vinfos[0].jointtype = 1;
18058  vinfos[0].foffset = j0;
18059  vinfos[0].indices[0] = _ij0[0];
18060  vinfos[0].indices[1] = _ij0[1];
18061  vinfos[0].maxsolutions = _nj0;
18062  vinfos[1].jointtype = 1;
18063  vinfos[1].foffset = j1;
18064  vinfos[1].indices[0] = _ij1[0];
18065  vinfos[1].indices[1] = _ij1[1];
18066  vinfos[1].maxsolutions = _nj1;
18067  vinfos[2].jointtype = 1;
18068  vinfos[2].foffset = j2;
18069  vinfos[2].indices[0] = _ij2[0];
18070  vinfos[2].indices[1] = _ij2[1];
18071  vinfos[2].maxsolutions = _nj2;
18072  vinfos[3].jointtype = 1;
18073  vinfos[3].foffset = j3;
18074  vinfos[3].indices[0] = _ij3[0];
18075  vinfos[3].indices[1] = _ij3[1];
18076  vinfos[3].maxsolutions = _nj3;
18077  vinfos[4].jointtype = 1;
18078  vinfos[4].foffset = j4;
18079  vinfos[4].indices[0] = _ij4[0];
18080  vinfos[4].indices[1] = _ij4[1];
18081  vinfos[4].maxsolutions = _nj4;
18082  vinfos[5].jointtype = 1;
18083  vinfos[5].foffset = j5;
18084  vinfos[5].indices[0] = _ij5[0];
18085  vinfos[5].indices[1] = _ij5[1];
18086  vinfos[5].maxsolutions = _nj5;
18087  vinfos[6].jointtype = 1;
18088  vinfos[6].foffset = j6;
18089  vinfos[6].indices[0] = _ij6[0];
18090  vinfos[6].indices[1] = _ij6[1];
18091  vinfos[6].maxsolutions = _nj6;
18092  std::vector<int> vfree(0);
18093  solutions.AddSolution(vinfos, vfree);
18094  }
18095  }
18096  }
18097  }
18098  }
18099  }
18100  } while (0);
18101  if (bgotonextstatement)
18102  {
18103  bool bgotonextstatement = true;
18104  do
18105  {
18106  evalcond[0] =
18107  ((IKabs(new_r10)) + (IKabs(new_r01)));
18108  if (IKabs(evalcond[0]) < 0.0000050000000000)
18109  {
18110  bgotonextstatement = false;
18111  {
18112  IkReal j0eval[3];
18113  sj1 = 0;
18114  cj1 = -1.0;
18115  j1 = 3.14159265358979;
18116  new_r01 = 0;
18117  new_r10 = 0;
18118  j0eval[0] = new_r11;
18119  j0eval[1] = IKsign(new_r11);
18120  j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
18121  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
18122  IKabs(j0eval[1]) < 0.0000010000000000 ||
18123  IKabs(j0eval[2]) < 0.0000010000000000)
18124  {
18125  {
18126  IkReal j0eval[2];
18127  sj1 = 0;
18128  cj1 = -1.0;
18129  j1 = 3.14159265358979;
18130  new_r01 = 0;
18131  new_r10 = 0;
18132  j0eval[0] = new_r00;
18133  j0eval[1] = new_r11;
18134  if (IKabs(j0eval[0]) <
18135  0.0000010000000000 ||
18136  IKabs(j0eval[1]) < 0.0000010000000000)
18137  {
18138  continue; // no branches [j0]
18139  }
18140  else
18141  {
18142  {
18143  IkReal j0array[1], cj0array[1],
18144  sj0array[1];
18145  bool j0valid[1] = { false };
18146  _nj0 = 1;
18147  CheckValue<IkReal> x827 =
18148  IKPowWithIntegerCheck(new_r00,
18149  -1);
18150  if (!x827.valid)
18151  {
18152  continue;
18153  }
18154  CheckValue<IkReal> x828 =
18155  IKPowWithIntegerCheck(new_r11,
18156  -1);
18157  if (!x828.valid)
18158  {
18159  continue;
18160  }
18161  if (IKabs((sj2 * (x827.value))) <
18162  IKFAST_ATAN2_MAGTHRESH &&
18163  IKabs(((-1.0) * cj2 *
18164  (x828.value))) <
18165  IKFAST_ATAN2_MAGTHRESH &&
18166  IKabs(
18167  IKsqr((sj2 * (x827.value))) +
18168  IKsqr(((-1.0) * cj2 *
18169  (x828.value))) -
18170  1) <= IKFAST_SINCOS_THRESH)
18171  continue;
18172  j0array[0] = IKatan2(
18173  (sj2 * (x827.value)),
18174  ((-1.0) * cj2 * (x828.value)));
18175  sj0array[0] = IKsin(j0array[0]);
18176  cj0array[0] = IKcos(j0array[0]);
18177  if (j0array[0] > IKPI)
18178  {
18179  j0array[0] -= IK2PI;
18180  }
18181  else if (j0array[0] < -IKPI)
18182  {
18183  j0array[0] += IK2PI;
18184  }
18185  j0valid[0] = true;
18186  for (int ij0 = 0; ij0 < 1; ++ij0)
18187  {
18188  if (!j0valid[ij0])
18189  {
18190  continue;
18191  }
18192  _ij0[0] = ij0;
18193  _ij0[1] = -1;
18194  for (int iij0 = ij0 + 1; iij0 < 1;
18195  ++iij0)
18196  {
18197  if (j0valid[iij0] &&
18198  IKabs(cj0array[ij0] -
18199  cj0array[iij0]) <
18200  IKFAST_SOLUTION_THRESH &&
18201  IKabs(sj0array[ij0] -
18202  sj0array[iij0]) <
18203  IKFAST_SOLUTION_THRESH)
18204  {
18205  j0valid[iij0] = false;
18206  _ij0[1] = iij0;
18207  break;
18208  }
18209  }
18210  j0 = j0array[ij0];
18211  cj0 = cj0array[ij0];
18212  sj0 = sj0array[ij0];
18213  {
18214  IkReal evalcond[7];
18215  IkReal x829 = IKsin(j0);
18216  IkReal x830 = IKcos(j0);
18217  IkReal x831 = ((1.0) * cj2);
18218  IkReal x832 = (sj2 * x829);
18219  evalcond[0] =
18220  (((new_r11 * x830)) + cj2);
18221  evalcond[1] =
18222  (sj2 + ((new_r11 * x829)));
18223  evalcond[2] =
18224  (sj2 +
18225  (((-1.0) * new_r00 * x829)));
18226  evalcond[3] =
18227  (((new_r00 * x830)) +
18228  (((-1.0) * x831)));
18229  evalcond[4] =
18230  (((sj2 * x830)) +
18231  (((-1.0) * x829 * x831)));
18232  evalcond[5] = (new_r11 + x832 +
18233  ((cj2 * x830)));
18234  evalcond[6] =
18235  ((((-1.0) * x830 * x831)) +
18236  (((-1.0) * x832)) + new_r00);
18237  if (IKabs(evalcond[0]) >
18238  IKFAST_EVALCOND_THRESH ||
18239  IKabs(evalcond[1]) >
18240  IKFAST_EVALCOND_THRESH ||
18241  IKabs(evalcond[2]) >
18242  IKFAST_EVALCOND_THRESH ||
18243  IKabs(evalcond[3]) >
18244  IKFAST_EVALCOND_THRESH ||
18245  IKabs(evalcond[4]) >
18246  IKFAST_EVALCOND_THRESH ||
18247  IKabs(evalcond[5]) >
18248  IKFAST_EVALCOND_THRESH ||
18249  IKabs(evalcond[6]) >
18250  IKFAST_EVALCOND_THRESH)
18251  {
18252  continue;
18253  }
18254  }
18255 
18256  {
18257  std::vector<
18258  IkSingleDOFSolutionBase<
18259  IkReal> >
18260  vinfos(7);
18261  vinfos[0].jointtype = 1;
18262  vinfos[0].foffset = j0;
18263  vinfos[0].indices[0] = _ij0[0];
18264  vinfos[0].indices[1] = _ij0[1];
18265  vinfos[0].maxsolutions = _nj0;
18266  vinfos[1].jointtype = 1;
18267  vinfos[1].foffset = j1;
18268  vinfos[1].indices[0] = _ij1[0];
18269  vinfos[1].indices[1] = _ij1[1];
18270  vinfos[1].maxsolutions = _nj1;
18271  vinfos[2].jointtype = 1;
18272  vinfos[2].foffset = j2;
18273  vinfos[2].indices[0] = _ij2[0];
18274  vinfos[2].indices[1] = _ij2[1];
18275  vinfos[2].maxsolutions = _nj2;
18276  vinfos[3].jointtype = 1;
18277  vinfos[3].foffset = j3;
18278  vinfos[3].indices[0] = _ij3[0];
18279  vinfos[3].indices[1] = _ij3[1];
18280  vinfos[3].maxsolutions = _nj3;
18281  vinfos[4].jointtype = 1;
18282  vinfos[4].foffset = j4;
18283  vinfos[4].indices[0] = _ij4[0];
18284  vinfos[4].indices[1] = _ij4[1];
18285  vinfos[4].maxsolutions = _nj4;
18286  vinfos[5].jointtype = 1;
18287  vinfos[5].foffset = j5;
18288  vinfos[5].indices[0] = _ij5[0];
18289  vinfos[5].indices[1] = _ij5[1];
18290  vinfos[5].maxsolutions = _nj5;
18291  vinfos[6].jointtype = 1;
18292  vinfos[6].foffset = j6;
18293  vinfos[6].indices[0] = _ij6[0];
18294  vinfos[6].indices[1] = _ij6[1];
18295  vinfos[6].maxsolutions = _nj6;
18296  std::vector<int> vfree(0);
18297  solutions.AddSolution(vinfos,
18298  vfree);
18299  }
18300  }
18301  }
18302  }
18303  }
18304  }
18305  else
18306  {
18307  {
18308  IkReal j0array[1], cj0array[1],
18309  sj0array[1];
18310  bool j0valid[1] = { false };
18311  _nj0 = 1;
18312  CheckValue<IkReal> x833 =
18313  IKPowWithIntegerCheck(IKsign(new_r11),
18314  -1);
18315  if (!x833.valid)
18316  {
18317  continue;
18318  }
18319  CheckValue<IkReal> x834 =
18320  IKatan2WithCheck(
18321  IkReal(((-1.0) * sj2)),
18322  IkReal(((-1.0) * cj2)),
18323  IKFAST_ATAN2_MAGTHRESH);
18324  if (!x834.valid)
18325  {
18326  continue;
18327  }
18328  j0array[0] = ((-1.5707963267949) +
18329  (((1.5707963267949) *
18330  (x833.value))) +
18331  (x834.value));
18332  sj0array[0] = IKsin(j0array[0]);
18333  cj0array[0] = IKcos(j0array[0]);
18334  if (j0array[0] > IKPI)
18335  {
18336  j0array[0] -= IK2PI;
18337  }
18338  else if (j0array[0] < -IKPI)
18339  {
18340  j0array[0] += IK2PI;
18341  }
18342  j0valid[0] = true;
18343  for (int ij0 = 0; ij0 < 1; ++ij0)
18344  {
18345  if (!j0valid[ij0])
18346  {
18347  continue;
18348  }
18349  _ij0[0] = ij0;
18350  _ij0[1] = -1;
18351  for (int iij0 = ij0 + 1; iij0 < 1;
18352  ++iij0)
18353  {
18354  if (j0valid[iij0] &&
18355  IKabs(cj0array[ij0] -
18356  cj0array[iij0]) <
18357  IKFAST_SOLUTION_THRESH &&
18358  IKabs(sj0array[ij0] -
18359  sj0array[iij0]) <
18360  IKFAST_SOLUTION_THRESH)
18361  {
18362  j0valid[iij0] = false;
18363  _ij0[1] = iij0;
18364  break;
18365  }
18366  }
18367  j0 = j0array[ij0];
18368  cj0 = cj0array[ij0];
18369  sj0 = sj0array[ij0];
18370  {
18371  IkReal evalcond[7];
18372  IkReal x835 = IKsin(j0);
18373  IkReal x836 = IKcos(j0);
18374  IkReal x837 = ((1.0) * cj2);
18375  IkReal x838 = (sj2 * x835);
18376  evalcond[0] =
18377  (((new_r11 * x836)) + cj2);
18378  evalcond[1] =
18379  (((new_r11 * x835)) + sj2);
18380  evalcond[2] =
18381  (sj2 +
18382  (((-1.0) * new_r00 * x835)));
18383  evalcond[3] = (((new_r00 * x836)) +
18384  (((-1.0) * x837)));
18385  evalcond[4] =
18386  ((((-1.0) * x835 * x837)) +
18387  ((sj2 * x836)));
18388  evalcond[5] =
18389  (new_r11 + x838 + ((cj2 * x836)));
18390  evalcond[6] =
18391  ((((-1.0) * x836 * x837)) +
18392  (((-1.0) * x838)) + new_r00);
18393  if (IKabs(evalcond[0]) >
18394  IKFAST_EVALCOND_THRESH ||
18395  IKabs(evalcond[1]) >
18396  IKFAST_EVALCOND_THRESH ||
18397  IKabs(evalcond[2]) >
18398  IKFAST_EVALCOND_THRESH ||
18399  IKabs(evalcond[3]) >
18400  IKFAST_EVALCOND_THRESH ||
18401  IKabs(evalcond[4]) >
18402  IKFAST_EVALCOND_THRESH ||
18403  IKabs(evalcond[5]) >
18404  IKFAST_EVALCOND_THRESH ||
18405  IKabs(evalcond[6]) >
18406  IKFAST_EVALCOND_THRESH)
18407  {
18408  continue;
18409  }
18410  }
18411 
18412  {
18413  std::vector<
18414  IkSingleDOFSolutionBase<IkReal> >
18415  vinfos(7);
18416  vinfos[0].jointtype = 1;
18417  vinfos[0].foffset = j0;
18418  vinfos[0].indices[0] = _ij0[0];
18419  vinfos[0].indices[1] = _ij0[1];
18420  vinfos[0].maxsolutions = _nj0;
18421  vinfos[1].jointtype = 1;
18422  vinfos[1].foffset = j1;
18423  vinfos[1].indices[0] = _ij1[0];
18424  vinfos[1].indices[1] = _ij1[1];
18425  vinfos[1].maxsolutions = _nj1;
18426  vinfos[2].jointtype = 1;
18427  vinfos[2].foffset = j2;
18428  vinfos[2].indices[0] = _ij2[0];
18429  vinfos[2].indices[1] = _ij2[1];
18430  vinfos[2].maxsolutions = _nj2;
18431  vinfos[3].jointtype = 1;
18432  vinfos[3].foffset = j3;
18433  vinfos[3].indices[0] = _ij3[0];
18434  vinfos[3].indices[1] = _ij3[1];
18435  vinfos[3].maxsolutions = _nj3;
18436  vinfos[4].jointtype = 1;
18437  vinfos[4].foffset = j4;
18438  vinfos[4].indices[0] = _ij4[0];
18439  vinfos[4].indices[1] = _ij4[1];
18440  vinfos[4].maxsolutions = _nj4;
18441  vinfos[5].jointtype = 1;
18442  vinfos[5].foffset = j5;
18443  vinfos[5].indices[0] = _ij5[0];
18444  vinfos[5].indices[1] = _ij5[1];
18445  vinfos[5].maxsolutions = _nj5;
18446  vinfos[6].jointtype = 1;
18447  vinfos[6].foffset = j6;
18448  vinfos[6].indices[0] = _ij6[0];
18449  vinfos[6].indices[1] = _ij6[1];
18450  vinfos[6].maxsolutions = _nj6;
18451  std::vector<int> vfree(0);
18452  solutions.AddSolution(vinfos, vfree);
18453  }
18454  }
18455  }
18456  }
18457  }
18458  }
18459  } while (0);
18460  if (bgotonextstatement)
18461  {
18462  bool bgotonextstatement = true;
18463  do
18464  {
18465  if (1)
18466  {
18467  bgotonextstatement = false;
18468  continue; // branch miss [j0]
18469  }
18470  } while (0);
18471  if (bgotonextstatement)
18472  {
18473  }
18474  }
18475  }
18476  }
18477  }
18478  }
18479  }
18480  }
18481  }
18482  }
18483  }
18484  }
18485  else
18486  {
18487  {
18488  IkReal j0array[1], cj0array[1], sj0array[1];
18489  bool j0valid[1] = { false };
18490  _nj0 = 1;
18491  CheckValue<IkReal> x839 = IKPowWithIntegerCheck(
18492  IKsign((((new_r11 * sj2)) + ((cj2 * new_r01)))), -1);
18493  if (!x839.valid)
18494  {
18495  continue;
18496  }
18497  CheckValue<IkReal> x840 = IKatan2WithCheck(
18498  IkReal(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10)))),
18499  IkReal(
18500  ((((-1.0) * cj2 * sj2)) + (((-1.0) * new_r10 * new_r11)))),
18501  IKFAST_ATAN2_MAGTHRESH);
18502  if (!x840.valid)
18503  {
18504  continue;
18505  }
18506  j0array[0] = ((-1.5707963267949) +
18507  (((1.5707963267949) * (x839.value))) + (x840.value));
18508  sj0array[0] = IKsin(j0array[0]);
18509  cj0array[0] = IKcos(j0array[0]);
18510  if (j0array[0] > IKPI)
18511  {
18512  j0array[0] -= IK2PI;
18513  }
18514  else if (j0array[0] < -IKPI)
18515  {
18516  j0array[0] += IK2PI;
18517  }
18518  j0valid[0] = true;
18519  for (int ij0 = 0; ij0 < 1; ++ij0)
18520  {
18521  if (!j0valid[ij0])
18522  {
18523  continue;
18524  }
18525  _ij0[0] = ij0;
18526  _ij0[1] = -1;
18527  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18528  {
18529  if (j0valid[iij0] &&
18530  IKabs(cj0array[ij0] - cj0array[iij0]) <
18531  IKFAST_SOLUTION_THRESH &&
18532  IKabs(sj0array[ij0] - sj0array[iij0]) <
18533  IKFAST_SOLUTION_THRESH)
18534  {
18535  j0valid[iij0] = false;
18536  _ij0[1] = iij0;
18537  break;
18538  }
18539  }
18540  j0 = j0array[ij0];
18541  cj0 = cj0array[ij0];
18542  sj0 = sj0array[ij0];
18543  {
18544  IkReal evalcond[8];
18545  IkReal x841 = IKcos(j0);
18546  IkReal x842 = IKsin(j0);
18547  IkReal x843 = ((1.0) * cj2);
18548  IkReal x844 = (sj2 * x841);
18549  IkReal x845 = (sj2 * x842);
18550  IkReal x846 = ((1.0) * x842);
18551  IkReal x847 = (x842 * x843);
18552  evalcond[0] = (sj2 + ((new_r01 * x841)) + ((new_r11 * x842)));
18553  evalcond[1] = (((cj2 * x841)) + new_r11 + x845);
18554  evalcond[2] =
18555  (((new_r10 * x841)) + sj2 + (((-1.0) * new_r00 * x846)));
18556  evalcond[3] =
18557  (cj2 + (((-1.0) * new_r01 * x846)) + ((new_r11 * x841)));
18558  evalcond[4] = ((((-1.0) * x847)) + new_r10 + x844);
18559  evalcond[5] = ((((-1.0) * x847)) + new_r01 + x844);
18560  evalcond[6] = (((new_r10 * x842)) + ((new_r00 * x841)) +
18561  (((-1.0) * x843)));
18562  evalcond[7] =
18563  ((((-1.0) * x841 * x843)) + (((-1.0) * x845)) + new_r00);
18564  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18565  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18566  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18567  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18568  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18569  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18570  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18571  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18572  {
18573  continue;
18574  }
18575  }
18576 
18577  {
18578  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18579  vinfos[0].jointtype = 1;
18580  vinfos[0].foffset = j0;
18581  vinfos[0].indices[0] = _ij0[0];
18582  vinfos[0].indices[1] = _ij0[1];
18583  vinfos[0].maxsolutions = _nj0;
18584  vinfos[1].jointtype = 1;
18585  vinfos[1].foffset = j1;
18586  vinfos[1].indices[0] = _ij1[0];
18587  vinfos[1].indices[1] = _ij1[1];
18588  vinfos[1].maxsolutions = _nj1;
18589  vinfos[2].jointtype = 1;
18590  vinfos[2].foffset = j2;
18591  vinfos[2].indices[0] = _ij2[0];
18592  vinfos[2].indices[1] = _ij2[1];
18593  vinfos[2].maxsolutions = _nj2;
18594  vinfos[3].jointtype = 1;
18595  vinfos[3].foffset = j3;
18596  vinfos[3].indices[0] = _ij3[0];
18597  vinfos[3].indices[1] = _ij3[1];
18598  vinfos[3].maxsolutions = _nj3;
18599  vinfos[4].jointtype = 1;
18600  vinfos[4].foffset = j4;
18601  vinfos[4].indices[0] = _ij4[0];
18602  vinfos[4].indices[1] = _ij4[1];
18603  vinfos[4].maxsolutions = _nj4;
18604  vinfos[5].jointtype = 1;
18605  vinfos[5].foffset = j5;
18606  vinfos[5].indices[0] = _ij5[0];
18607  vinfos[5].indices[1] = _ij5[1];
18608  vinfos[5].maxsolutions = _nj5;
18609  vinfos[6].jointtype = 1;
18610  vinfos[6].foffset = j6;
18611  vinfos[6].indices[0] = _ij6[0];
18612  vinfos[6].indices[1] = _ij6[1];
18613  vinfos[6].maxsolutions = _nj6;
18614  std::vector<int> vfree(0);
18615  solutions.AddSolution(vinfos, vfree);
18616  }
18617  }
18618  }
18619  }
18620  }
18621  }
18622  else
18623  {
18624  {
18625  IkReal j0array[1], cj0array[1], sj0array[1];
18626  bool j0valid[1] = { false };
18627  _nj0 = 1;
18628  IkReal x848 = ((1.0) * new_r11);
18629  CheckValue<IkReal> x849 = IKPowWithIntegerCheck(
18630  IKsign(((new_r01 * new_r01) + (new_r11 * new_r11))), -1);
18631  if (!x849.valid)
18632  {
18633  continue;
18634  }
18635  CheckValue<IkReal> x850 = IKatan2WithCheck(
18636  IkReal(((((-1.0) * sj2 * x848)) + ((cj2 * new_r01)))),
18637  IkReal(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x848)))),
18638  IKFAST_ATAN2_MAGTHRESH);
18639  if (!x850.valid)
18640  {
18641  continue;
18642  }
18643  j0array[0] = ((-1.5707963267949) +
18644  (((1.5707963267949) * (x849.value))) + (x850.value));
18645  sj0array[0] = IKsin(j0array[0]);
18646  cj0array[0] = IKcos(j0array[0]);
18647  if (j0array[0] > IKPI)
18648  {
18649  j0array[0] -= IK2PI;
18650  }
18651  else if (j0array[0] < -IKPI)
18652  {
18653  j0array[0] += IK2PI;
18654  }
18655  j0valid[0] = true;
18656  for (int ij0 = 0; ij0 < 1; ++ij0)
18657  {
18658  if (!j0valid[ij0])
18659  {
18660  continue;
18661  }
18662  _ij0[0] = ij0;
18663  _ij0[1] = -1;
18664  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18665  {
18666  if (j0valid[iij0] &&
18667  IKabs(cj0array[ij0] - cj0array[iij0]) <
18668  IKFAST_SOLUTION_THRESH &&
18669  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18670  {
18671  j0valid[iij0] = false;
18672  _ij0[1] = iij0;
18673  break;
18674  }
18675  }
18676  j0 = j0array[ij0];
18677  cj0 = cj0array[ij0];
18678  sj0 = sj0array[ij0];
18679  {
18680  IkReal evalcond[8];
18681  IkReal x851 = IKcos(j0);
18682  IkReal x852 = IKsin(j0);
18683  IkReal x853 = ((1.0) * cj2);
18684  IkReal x854 = (sj2 * x851);
18685  IkReal x855 = (sj2 * x852);
18686  IkReal x856 = ((1.0) * x852);
18687  IkReal x857 = (x852 * x853);
18688  evalcond[0] = (sj2 + ((new_r11 * x852)) + ((new_r01 * x851)));
18689  evalcond[1] = (((cj2 * x851)) + new_r11 + x855);
18690  evalcond[2] =
18691  (sj2 + (((-1.0) * new_r00 * x856)) + ((new_r10 * x851)));
18692  evalcond[3] =
18693  (cj2 + (((-1.0) * new_r01 * x856)) + ((new_r11 * x851)));
18694  evalcond[4] = ((((-1.0) * x857)) + new_r10 + x854);
18695  evalcond[5] = ((((-1.0) * x857)) + new_r01 + x854);
18696  evalcond[6] =
18697  ((((-1.0) * x853)) + ((new_r10 * x852)) + ((new_r00 * x851)));
18698  evalcond[7] =
18699  ((((-1.0) * x855)) + new_r00 + (((-1.0) * x851 * x853)));
18700  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18701  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18702  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18703  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18704  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18705  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18706  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18707  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18708  {
18709  continue;
18710  }
18711  }
18712 
18713  {
18714  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18715  vinfos[0].jointtype = 1;
18716  vinfos[0].foffset = j0;
18717  vinfos[0].indices[0] = _ij0[0];
18718  vinfos[0].indices[1] = _ij0[1];
18719  vinfos[0].maxsolutions = _nj0;
18720  vinfos[1].jointtype = 1;
18721  vinfos[1].foffset = j1;
18722  vinfos[1].indices[0] = _ij1[0];
18723  vinfos[1].indices[1] = _ij1[1];
18724  vinfos[1].maxsolutions = _nj1;
18725  vinfos[2].jointtype = 1;
18726  vinfos[2].foffset = j2;
18727  vinfos[2].indices[0] = _ij2[0];
18728  vinfos[2].indices[1] = _ij2[1];
18729  vinfos[2].maxsolutions = _nj2;
18730  vinfos[3].jointtype = 1;
18731  vinfos[3].foffset = j3;
18732  vinfos[3].indices[0] = _ij3[0];
18733  vinfos[3].indices[1] = _ij3[1];
18734  vinfos[3].maxsolutions = _nj3;
18735  vinfos[4].jointtype = 1;
18736  vinfos[4].foffset = j4;
18737  vinfos[4].indices[0] = _ij4[0];
18738  vinfos[4].indices[1] = _ij4[1];
18739  vinfos[4].maxsolutions = _nj4;
18740  vinfos[5].jointtype = 1;
18741  vinfos[5].foffset = j5;
18742  vinfos[5].indices[0] = _ij5[0];
18743  vinfos[5].indices[1] = _ij5[1];
18744  vinfos[5].maxsolutions = _nj5;
18745  vinfos[6].jointtype = 1;
18746  vinfos[6].foffset = j6;
18747  vinfos[6].indices[0] = _ij6[0];
18748  vinfos[6].indices[1] = _ij6[1];
18749  vinfos[6].maxsolutions = _nj6;
18750  std::vector<int> vfree(0);
18751  solutions.AddSolution(vinfos, vfree);
18752  }
18753  }
18754  }
18755  }
18756  }
18757  }
18758  else
18759  {
18760  {
18761  IkReal j0array[1], cj0array[1], sj0array[1];
18762  bool j0valid[1] = { false };
18763  _nj0 = 1;
18764  IkReal x858 = ((1.0) * sj2);
18765  CheckValue<IkReal> x859 = IKatan2WithCheck(
18766  IkReal((((new_r01 * sj2)) + (((-1.0) * new_r10 * x858)))),
18767  IkReal(((((-1.0) * new_r00 * x858)) + (((-1.0) * new_r11 * x858)))),
18768  IKFAST_ATAN2_MAGTHRESH);
18769  if (!x859.valid)
18770  {
18771  continue;
18772  }
18773  CheckValue<IkReal> x860 = IKPowWithIntegerCheck(
18774  IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
18775  if (!x860.valid)
18776  {
18777  continue;
18778  }
18779  j0array[0] = ((-1.5707963267949) + (x859.value) +
18780  (((1.5707963267949) * (x860.value))));
18781  sj0array[0] = IKsin(j0array[0]);
18782  cj0array[0] = IKcos(j0array[0]);
18783  if (j0array[0] > IKPI)
18784  {
18785  j0array[0] -= IK2PI;
18786  }
18787  else if (j0array[0] < -IKPI)
18788  {
18789  j0array[0] += IK2PI;
18790  }
18791  j0valid[0] = true;
18792  for (int ij0 = 0; ij0 < 1; ++ij0)
18793  {
18794  if (!j0valid[ij0])
18795  {
18796  continue;
18797  }
18798  _ij0[0] = ij0;
18799  _ij0[1] = -1;
18800  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18801  {
18802  if (j0valid[iij0] &&
18803  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
18804  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18805  {
18806  j0valid[iij0] = false;
18807  _ij0[1] = iij0;
18808  break;
18809  }
18810  }
18811  j0 = j0array[ij0];
18812  cj0 = cj0array[ij0];
18813  sj0 = sj0array[ij0];
18814  {
18815  IkReal evalcond[8];
18816  IkReal x861 = IKcos(j0);
18817  IkReal x862 = IKsin(j0);
18818  IkReal x863 = ((1.0) * cj2);
18819  IkReal x864 = (sj2 * x861);
18820  IkReal x865 = (sj2 * x862);
18821  IkReal x866 = ((1.0) * x862);
18822  IkReal x867 = (x862 * x863);
18823  evalcond[0] = (((new_r01 * x861)) + sj2 + ((new_r11 * x862)));
18824  evalcond[1] = (((cj2 * x861)) + new_r11 + x865);
18825  evalcond[2] = (sj2 + (((-1.0) * new_r00 * x866)) + ((new_r10 * x861)));
18826  evalcond[3] = (cj2 + (((-1.0) * new_r01 * x866)) + ((new_r11 * x861)));
18827  evalcond[4] = ((((-1.0) * x867)) + new_r10 + x864);
18828  evalcond[5] = ((((-1.0) * x867)) + new_r01 + x864);
18829  evalcond[6] =
18830  ((((-1.0) * x863)) + ((new_r00 * x861)) + ((new_r10 * x862)));
18831  evalcond[7] = ((((-1.0) * x865)) + (((-1.0) * x861 * x863)) + new_r00);
18832  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18833  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18834  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18835  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18836  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18837  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18838  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18839  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18840  {
18841  continue;
18842  }
18843  }
18844 
18845  {
18846  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18847  vinfos[0].jointtype = 1;
18848  vinfos[0].foffset = j0;
18849  vinfos[0].indices[0] = _ij0[0];
18850  vinfos[0].indices[1] = _ij0[1];
18851  vinfos[0].maxsolutions = _nj0;
18852  vinfos[1].jointtype = 1;
18853  vinfos[1].foffset = j1;
18854  vinfos[1].indices[0] = _ij1[0];
18855  vinfos[1].indices[1] = _ij1[1];
18856  vinfos[1].maxsolutions = _nj1;
18857  vinfos[2].jointtype = 1;
18858  vinfos[2].foffset = j2;
18859  vinfos[2].indices[0] = _ij2[0];
18860  vinfos[2].indices[1] = _ij2[1];
18861  vinfos[2].maxsolutions = _nj2;
18862  vinfos[3].jointtype = 1;
18863  vinfos[3].foffset = j3;
18864  vinfos[3].indices[0] = _ij3[0];
18865  vinfos[3].indices[1] = _ij3[1];
18866  vinfos[3].maxsolutions = _nj3;
18867  vinfos[4].jointtype = 1;
18868  vinfos[4].foffset = j4;
18869  vinfos[4].indices[0] = _ij4[0];
18870  vinfos[4].indices[1] = _ij4[1];
18871  vinfos[4].maxsolutions = _nj4;
18872  vinfos[5].jointtype = 1;
18873  vinfos[5].foffset = j5;
18874  vinfos[5].indices[0] = _ij5[0];
18875  vinfos[5].indices[1] = _ij5[1];
18876  vinfos[5].maxsolutions = _nj5;
18877  vinfos[6].jointtype = 1;
18878  vinfos[6].foffset = j6;
18879  vinfos[6].indices[0] = _ij6[0];
18880  vinfos[6].indices[1] = _ij6[1];
18881  vinfos[6].maxsolutions = _nj6;
18882  std::vector<int> vfree(0);
18883  solutions.AddSolution(vinfos, vfree);
18884  }
18885  }
18886  }
18887  }
18888  }
18889  }
18890  } while (0);
18891  if (bgotonextstatement)
18892  {
18893  bool bgotonextstatement = true;
18894  do
18895  {
18896  evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
18897  if (IKabs(evalcond[0]) < 0.0000050000000000)
18898  {
18899  bgotonextstatement = false;
18900  {
18901  IkReal j0eval[1];
18902  new_r02 = 0;
18903  new_r12 = 0;
18904  new_r20 = 0;
18905  new_r21 = 0;
18906  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
18907  if (IKabs(j0eval[0]) < 0.0000010000000000)
18908  {
18909  {
18910  IkReal j0eval[1];
18911  new_r02 = 0;
18912  new_r12 = 0;
18913  new_r20 = 0;
18914  new_r21 = 0;
18915  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
18916  if (IKabs(j0eval[0]) < 0.0000010000000000)
18917  {
18918  {
18919  IkReal j0eval[1];
18920  new_r02 = 0;
18921  new_r12 = 0;
18922  new_r20 = 0;
18923  new_r21 = 0;
18924  j0eval[0] =
18925  ((IKabs((new_r10 * new_r22))) + (IKabs((new_r00 * new_r22))));
18926  if (IKabs(j0eval[0]) < 0.0000010000000000)
18927  {
18928  continue; // no branches [j0]
18929  }
18930  else
18931  {
18932  {
18933  IkReal j0array[2], cj0array[2], sj0array[2];
18934  bool j0valid[2] = { false };
18935  _nj0 = 2;
18936  CheckValue<IkReal> x869 =
18937  IKatan2WithCheck(IkReal((new_r00 * new_r22)),
18938  IkReal((new_r10 * new_r22)),
18939  IKFAST_ATAN2_MAGTHRESH);
18940  if (!x869.valid)
18941  {
18942  continue;
18943  }
18944  IkReal x868 = x869.value;
18945  j0array[0] = ((-1.0) * x868);
18946  sj0array[0] = IKsin(j0array[0]);
18947  cj0array[0] = IKcos(j0array[0]);
18948  j0array[1] = ((3.14159265358979) + (((-1.0) * x868)));
18949  sj0array[1] = IKsin(j0array[1]);
18950  cj0array[1] = IKcos(j0array[1]);
18951  if (j0array[0] > IKPI)
18952  {
18953  j0array[0] -= IK2PI;
18954  }
18955  else if (j0array[0] < -IKPI)
18956  {
18957  j0array[0] += IK2PI;
18958  }
18959  j0valid[0] = true;
18960  if (j0array[1] > IKPI)
18961  {
18962  j0array[1] -= IK2PI;
18963  }
18964  else if (j0array[1] < -IKPI)
18965  {
18966  j0array[1] += IK2PI;
18967  }
18968  j0valid[1] = true;
18969  for (int ij0 = 0; ij0 < 2; ++ij0)
18970  {
18971  if (!j0valid[ij0])
18972  {
18973  continue;
18974  }
18975  _ij0[0] = ij0;
18976  _ij0[1] = -1;
18977  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18978  {
18979  if (j0valid[iij0] &&
18980  IKabs(cj0array[ij0] - cj0array[iij0]) <
18981  IKFAST_SOLUTION_THRESH &&
18982  IKabs(sj0array[ij0] - sj0array[iij0]) <
18983  IKFAST_SOLUTION_THRESH)
18984  {
18985  j0valid[iij0] = false;
18986  _ij0[1] = iij0;
18987  break;
18988  }
18989  }
18990  j0 = j0array[ij0];
18991  cj0 = cj0array[ij0];
18992  sj0 = sj0array[ij0];
18993  {
18994  IkReal evalcond[5];
18995  IkReal x870 = IKsin(j0);
18996  IkReal x871 = IKcos(j0);
18997  IkReal x872 = ((1.0) * x870);
18998  IkReal x873 = (new_r11 * x870);
18999  IkReal x874 = (new_r01 * x871);
19000  evalcond[0] = (((new_r00 * x871)) + ((new_r10 * x870)));
19001  evalcond[1] = (x873 + x874);
19002  evalcond[2] =
19003  ((((-1.0) * new_r00 * x872)) + ((new_r10 * x871)));
19004  evalcond[3] =
19005  ((((-1.0) * new_r01 * x872)) + ((new_r11 * x871)));
19006  evalcond[4] = (((new_r22 * x874)) + ((new_r22 * x873)));
19007  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19008  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19009  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19010  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19011  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19012  {
19013  continue;
19014  }
19015  }
19016 
19017  {
19018  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19019  vinfos[0].jointtype = 1;
19020  vinfos[0].foffset = j0;
19021  vinfos[0].indices[0] = _ij0[0];
19022  vinfos[0].indices[1] = _ij0[1];
19023  vinfos[0].maxsolutions = _nj0;
19024  vinfos[1].jointtype = 1;
19025  vinfos[1].foffset = j1;
19026  vinfos[1].indices[0] = _ij1[0];
19027  vinfos[1].indices[1] = _ij1[1];
19028  vinfos[1].maxsolutions = _nj1;
19029  vinfos[2].jointtype = 1;
19030  vinfos[2].foffset = j2;
19031  vinfos[2].indices[0] = _ij2[0];
19032  vinfos[2].indices[1] = _ij2[1];
19033  vinfos[2].maxsolutions = _nj2;
19034  vinfos[3].jointtype = 1;
19035  vinfos[3].foffset = j3;
19036  vinfos[3].indices[0] = _ij3[0];
19037  vinfos[3].indices[1] = _ij3[1];
19038  vinfos[3].maxsolutions = _nj3;
19039  vinfos[4].jointtype = 1;
19040  vinfos[4].foffset = j4;
19041  vinfos[4].indices[0] = _ij4[0];
19042  vinfos[4].indices[1] = _ij4[1];
19043  vinfos[4].maxsolutions = _nj4;
19044  vinfos[5].jointtype = 1;
19045  vinfos[5].foffset = j5;
19046  vinfos[5].indices[0] = _ij5[0];
19047  vinfos[5].indices[1] = _ij5[1];
19048  vinfos[5].maxsolutions = _nj5;
19049  vinfos[6].jointtype = 1;
19050  vinfos[6].foffset = j6;
19051  vinfos[6].indices[0] = _ij6[0];
19052  vinfos[6].indices[1] = _ij6[1];
19053  vinfos[6].maxsolutions = _nj6;
19054  std::vector<int> vfree(0);
19055  solutions.AddSolution(vinfos, vfree);
19056  }
19057  }
19058  }
19059  }
19060  }
19061  }
19062  else
19063  {
19064  {
19065  IkReal j0array[2], cj0array[2], sj0array[2];
19066  bool j0valid[2] = { false };
19067  _nj0 = 2;
19068  CheckValue<IkReal> x876 = IKatan2WithCheck(
19069  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
19070  if (!x876.valid)
19071  {
19072  continue;
19073  }
19074  IkReal x875 = x876.value;
19075  j0array[0] = ((-1.0) * x875);
19076  sj0array[0] = IKsin(j0array[0]);
19077  cj0array[0] = IKcos(j0array[0]);
19078  j0array[1] = ((3.14159265358979) + (((-1.0) * x875)));
19079  sj0array[1] = IKsin(j0array[1]);
19080  cj0array[1] = IKcos(j0array[1]);
19081  if (j0array[0] > IKPI)
19082  {
19083  j0array[0] -= IK2PI;
19084  }
19085  else if (j0array[0] < -IKPI)
19086  {
19087  j0array[0] += IK2PI;
19088  }
19089  j0valid[0] = true;
19090  if (j0array[1] > IKPI)
19091  {
19092  j0array[1] -= IK2PI;
19093  }
19094  else if (j0array[1] < -IKPI)
19095  {
19096  j0array[1] += IK2PI;
19097  }
19098  j0valid[1] = true;
19099  for (int ij0 = 0; ij0 < 2; ++ij0)
19100  {
19101  if (!j0valid[ij0])
19102  {
19103  continue;
19104  }
19105  _ij0[0] = ij0;
19106  _ij0[1] = -1;
19107  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19108  {
19109  if (j0valid[iij0] &&
19110  IKabs(cj0array[ij0] - cj0array[iij0]) <
19111  IKFAST_SOLUTION_THRESH &&
19112  IKabs(sj0array[ij0] - sj0array[iij0]) <
19113  IKFAST_SOLUTION_THRESH)
19114  {
19115  j0valid[iij0] = false;
19116  _ij0[1] = iij0;
19117  break;
19118  }
19119  }
19120  j0 = j0array[ij0];
19121  cj0 = cj0array[ij0];
19122  sj0 = sj0array[ij0];
19123  {
19124  IkReal evalcond[5];
19125  IkReal x877 = IKcos(j0);
19126  IkReal x878 = IKsin(j0);
19127  IkReal x879 = (new_r10 * x878);
19128  IkReal x880 = ((1.0) * x878);
19129  IkReal x881 = (new_r00 * x877);
19130  evalcond[0] = (x879 + x881);
19131  evalcond[1] = ((((-1.0) * new_r00 * x880)) + ((new_r10 * x877)));
19132  evalcond[2] = ((((-1.0) * new_r01 * x880)) + ((new_r11 * x877)));
19133  evalcond[3] = (((new_r22 * x881)) + ((new_r22 * x879)));
19134  evalcond[4] =
19135  (((new_r11 * new_r22 * x878)) + ((new_r01 * new_r22 * x877)));
19136  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19137  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19138  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19139  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19140  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19141  {
19142  continue;
19143  }
19144  }
19145 
19146  {
19147  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19148  vinfos[0].jointtype = 1;
19149  vinfos[0].foffset = j0;
19150  vinfos[0].indices[0] = _ij0[0];
19151  vinfos[0].indices[1] = _ij0[1];
19152  vinfos[0].maxsolutions = _nj0;
19153  vinfos[1].jointtype = 1;
19154  vinfos[1].foffset = j1;
19155  vinfos[1].indices[0] = _ij1[0];
19156  vinfos[1].indices[1] = _ij1[1];
19157  vinfos[1].maxsolutions = _nj1;
19158  vinfos[2].jointtype = 1;
19159  vinfos[2].foffset = j2;
19160  vinfos[2].indices[0] = _ij2[0];
19161  vinfos[2].indices[1] = _ij2[1];
19162  vinfos[2].maxsolutions = _nj2;
19163  vinfos[3].jointtype = 1;
19164  vinfos[3].foffset = j3;
19165  vinfos[3].indices[0] = _ij3[0];
19166  vinfos[3].indices[1] = _ij3[1];
19167  vinfos[3].maxsolutions = _nj3;
19168  vinfos[4].jointtype = 1;
19169  vinfos[4].foffset = j4;
19170  vinfos[4].indices[0] = _ij4[0];
19171  vinfos[4].indices[1] = _ij4[1];
19172  vinfos[4].maxsolutions = _nj4;
19173  vinfos[5].jointtype = 1;
19174  vinfos[5].foffset = j5;
19175  vinfos[5].indices[0] = _ij5[0];
19176  vinfos[5].indices[1] = _ij5[1];
19177  vinfos[5].maxsolutions = _nj5;
19178  vinfos[6].jointtype = 1;
19179  vinfos[6].foffset = j6;
19180  vinfos[6].indices[0] = _ij6[0];
19181  vinfos[6].indices[1] = _ij6[1];
19182  vinfos[6].maxsolutions = _nj6;
19183  std::vector<int> vfree(0);
19184  solutions.AddSolution(vinfos, vfree);
19185  }
19186  }
19187  }
19188  }
19189  }
19190  }
19191  else
19192  {
19193  {
19194  IkReal j0array[2], cj0array[2], sj0array[2];
19195  bool j0valid[2] = { false };
19196  _nj0 = 2;
19197  CheckValue<IkReal> x883 = IKatan2WithCheck(
19198  IkReal(new_r00), IkReal(new_r10), IKFAST_ATAN2_MAGTHRESH);
19199  if (!x883.valid)
19200  {
19201  continue;
19202  }
19203  IkReal x882 = x883.value;
19204  j0array[0] = ((-1.0) * x882);
19205  sj0array[0] = IKsin(j0array[0]);
19206  cj0array[0] = IKcos(j0array[0]);
19207  j0array[1] = ((3.14159265358979) + (((-1.0) * x882)));
19208  sj0array[1] = IKsin(j0array[1]);
19209  cj0array[1] = IKcos(j0array[1]);
19210  if (j0array[0] > IKPI)
19211  {
19212  j0array[0] -= IK2PI;
19213  }
19214  else if (j0array[0] < -IKPI)
19215  {
19216  j0array[0] += IK2PI;
19217  }
19218  j0valid[0] = true;
19219  if (j0array[1] > IKPI)
19220  {
19221  j0array[1] -= IK2PI;
19222  }
19223  else if (j0array[1] < -IKPI)
19224  {
19225  j0array[1] += IK2PI;
19226  }
19227  j0valid[1] = true;
19228  for (int ij0 = 0; ij0 < 2; ++ij0)
19229  {
19230  if (!j0valid[ij0])
19231  {
19232  continue;
19233  }
19234  _ij0[0] = ij0;
19235  _ij0[1] = -1;
19236  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19237  {
19238  if (j0valid[iij0] &&
19239  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19240  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19241  {
19242  j0valid[iij0] = false;
19243  _ij0[1] = iij0;
19244  break;
19245  }
19246  }
19247  j0 = j0array[ij0];
19248  cj0 = cj0array[ij0];
19249  sj0 = sj0array[ij0];
19250  {
19251  IkReal evalcond[5];
19252  IkReal x884 = IKcos(j0);
19253  IkReal x885 = IKsin(j0);
19254  IkReal x886 = ((1.0) * x885);
19255  IkReal x887 = (new_r11 * x885);
19256  IkReal x888 = (new_r22 * x884);
19257  evalcond[0] = (((new_r01 * x884)) + x887);
19258  evalcond[1] = (((new_r10 * x884)) + (((-1.0) * new_r00 * x886)));
19259  evalcond[2] = (((new_r11 * x884)) + (((-1.0) * new_r01 * x886)));
19260  evalcond[3] = (((new_r00 * x888)) + ((new_r10 * new_r22 * x885)));
19261  evalcond[4] = (((new_r01 * x888)) + ((new_r22 * x887)));
19262  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19263  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19264  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19265  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19266  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19267  {
19268  continue;
19269  }
19270  }
19271 
19272  {
19273  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19274  vinfos[0].jointtype = 1;
19275  vinfos[0].foffset = j0;
19276  vinfos[0].indices[0] = _ij0[0];
19277  vinfos[0].indices[1] = _ij0[1];
19278  vinfos[0].maxsolutions = _nj0;
19279  vinfos[1].jointtype = 1;
19280  vinfos[1].foffset = j1;
19281  vinfos[1].indices[0] = _ij1[0];
19282  vinfos[1].indices[1] = _ij1[1];
19283  vinfos[1].maxsolutions = _nj1;
19284  vinfos[2].jointtype = 1;
19285  vinfos[2].foffset = j2;
19286  vinfos[2].indices[0] = _ij2[0];
19287  vinfos[2].indices[1] = _ij2[1];
19288  vinfos[2].maxsolutions = _nj2;
19289  vinfos[3].jointtype = 1;
19290  vinfos[3].foffset = j3;
19291  vinfos[3].indices[0] = _ij3[0];
19292  vinfos[3].indices[1] = _ij3[1];
19293  vinfos[3].maxsolutions = _nj3;
19294  vinfos[4].jointtype = 1;
19295  vinfos[4].foffset = j4;
19296  vinfos[4].indices[0] = _ij4[0];
19297  vinfos[4].indices[1] = _ij4[1];
19298  vinfos[4].maxsolutions = _nj4;
19299  vinfos[5].jointtype = 1;
19300  vinfos[5].foffset = j5;
19301  vinfos[5].indices[0] = _ij5[0];
19302  vinfos[5].indices[1] = _ij5[1];
19303  vinfos[5].maxsolutions = _nj5;
19304  vinfos[6].jointtype = 1;
19305  vinfos[6].foffset = j6;
19306  vinfos[6].indices[0] = _ij6[0];
19307  vinfos[6].indices[1] = _ij6[1];
19308  vinfos[6].maxsolutions = _nj6;
19309  std::vector<int> vfree(0);
19310  solutions.AddSolution(vinfos, vfree);
19311  }
19312  }
19313  }
19314  }
19315  }
19316  }
19317  } while (0);
19318  if (bgotonextstatement)
19319  {
19320  bool bgotonextstatement = true;
19321  do
19322  {
19323  if (1)
19324  {
19325  bgotonextstatement = false;
19326  continue; // branch miss [j0]
19327  }
19328  } while (0);
19329  if (bgotonextstatement)
19330  {
19331  }
19332  }
19333  }
19334  }
19335  }
19336  }
19337  else
19338  {
19339  {
19340  IkReal j0array[1], cj0array[1], sj0array[1];
19341  bool j0valid[1] = { false };
19342  _nj0 = 1;
19343  CheckValue<IkReal> x890 = IKPowWithIntegerCheck(sj1, -1);
19344  if (!x890.valid)
19345  {
19346  continue;
19347  }
19348  IkReal x889 = x890.value;
19349  CheckValue<IkReal> x891 = IKPowWithIntegerCheck(new_r00, -1);
19350  if (!x891.valid)
19351  {
19352  continue;
19353  }
19354  if (IKabs((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) <
19355  IKFAST_ATAN2_MAGTHRESH &&
19356  IKabs((new_r02 * x889)) < IKFAST_ATAN2_MAGTHRESH &&
19357  IKabs(IKsqr((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) +
19358  IKsqr((new_r02 * x889)) - 1) <= IKFAST_SINCOS_THRESH)
19359  continue;
19360  j0array[0] = IKatan2((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10))))),
19361  (new_r02 * x889));
19362  sj0array[0] = IKsin(j0array[0]);
19363  cj0array[0] = IKcos(j0array[0]);
19364  if (j0array[0] > IKPI)
19365  {
19366  j0array[0] -= IK2PI;
19367  }
19368  else if (j0array[0] < -IKPI)
19369  {
19370  j0array[0] += IK2PI;
19371  }
19372  j0valid[0] = true;
19373  for (int ij0 = 0; ij0 < 1; ++ij0)
19374  {
19375  if (!j0valid[ij0])
19376  {
19377  continue;
19378  }
19379  _ij0[0] = ij0;
19380  _ij0[1] = -1;
19381  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19382  {
19383  if (j0valid[iij0] &&
19384  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19385  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19386  {
19387  j0valid[iij0] = false;
19388  _ij0[1] = iij0;
19389  break;
19390  }
19391  }
19392  j0 = j0array[ij0];
19393  cj0 = cj0array[ij0];
19394  sj0 = sj0array[ij0];
19395  {
19396  IkReal evalcond[18];
19397  IkReal x892 = IKcos(j0);
19398  IkReal x893 = IKsin(j0);
19399  IkReal x894 = ((1.0) * sj1);
19400  IkReal x895 = ((1.0) * sj2);
19401  IkReal x896 = ((1.0) * cj1);
19402  IkReal x897 = (new_r10 * x893);
19403  IkReal x898 = (cj1 * x892);
19404  IkReal x899 = (cj1 * x893);
19405  IkReal x900 = (new_r00 * x892);
19406  IkReal x901 = ((1.0) * x893);
19407  IkReal x902 = (new_r11 * x893);
19408  IkReal x903 = (new_r12 * x893);
19409  IkReal x904 = (new_r02 * x892);
19410  IkReal x905 = (new_r01 * x892);
19411  evalcond[0] = ((((-1.0) * x892 * x894)) + new_r02);
19412  evalcond[1] = ((((-1.0) * x893 * x894)) + new_r12);
19413  evalcond[2] = ((((-1.0) * new_r02 * x901)) + ((new_r12 * x892)));
19414  evalcond[3] = (sj2 + ((new_r10 * x892)) + (((-1.0) * new_r00 * x901)));
19415  evalcond[4] = (((new_r11 * x892)) + cj2 + (((-1.0) * new_r01 * x901)));
19416  evalcond[5] = (((cj2 * x899)) + ((sj2 * x892)) + new_r10);
19417  evalcond[6] = ((((-1.0) * x894)) + x904 + x903);
19418  evalcond[7] = (((cj1 * cj2)) + x897 + x900);
19419  evalcond[8] = (((cj2 * x898)) + (((-1.0) * x893 * x895)) + new_r00);
19420  evalcond[9] = (((cj2 * x892)) + (((-1.0) * x895 * x899)) + new_r11);
19421  evalcond[10] = ((((-1.0) * cj1 * x895)) + x905 + x902);
19422  evalcond[11] = ((((-1.0) * x895 * x898)) + new_r01 + (((-1.0) * cj2 * x901)));
19423  evalcond[12] =
19424  (((new_r12 * x899)) + ((new_r02 * x898)) + (((-1.0) * new_r22 * x894)));
19425  evalcond[13] =
19426  (cj2 + ((new_r00 * x898)) + (((-1.0) * new_r20 * x894)) + ((cj1 * x897)));
19427  evalcond[14] = ((((-1.0) * x894 * x900)) + (((-1.0) * new_r20 * x896)) +
19428  (((-1.0) * x894 * x897)));
19429  evalcond[15] = ((((-1.0) * x894 * x902)) + (((-1.0) * x894 * x905)) +
19430  (((-1.0) * new_r21 * x896)));
19431  evalcond[16] = ((1.0) + (((-1.0) * x894 * x903)) + (((-1.0) * x894 * x904)) +
19432  (((-1.0) * new_r22 * x896)));
19433  evalcond[17] = (((new_r11 * x899)) + ((new_r01 * x898)) +
19434  (((-1.0) * new_r21 * x894)) + (((-1.0) * x895)));
19435  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19436  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19437  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19438  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19439  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19440  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19441  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19442  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19443  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19444  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19445  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19446  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19447  IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19448  IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19449  IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19450  IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19451  IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19452  IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19453  {
19454  continue;
19455  }
19456  }
19457 
19458  {
19459  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19460  vinfos[0].jointtype = 1;
19461  vinfos[0].foffset = j0;
19462  vinfos[0].indices[0] = _ij0[0];
19463  vinfos[0].indices[1] = _ij0[1];
19464  vinfos[0].maxsolutions = _nj0;
19465  vinfos[1].jointtype = 1;
19466  vinfos[1].foffset = j1;
19467  vinfos[1].indices[0] = _ij1[0];
19468  vinfos[1].indices[1] = _ij1[1];
19469  vinfos[1].maxsolutions = _nj1;
19470  vinfos[2].jointtype = 1;
19471  vinfos[2].foffset = j2;
19472  vinfos[2].indices[0] = _ij2[0];
19473  vinfos[2].indices[1] = _ij2[1];
19474  vinfos[2].maxsolutions = _nj2;
19475  vinfos[3].jointtype = 1;
19476  vinfos[3].foffset = j3;
19477  vinfos[3].indices[0] = _ij3[0];
19478  vinfos[3].indices[1] = _ij3[1];
19479  vinfos[3].maxsolutions = _nj3;
19480  vinfos[4].jointtype = 1;
19481  vinfos[4].foffset = j4;
19482  vinfos[4].indices[0] = _ij4[0];
19483  vinfos[4].indices[1] = _ij4[1];
19484  vinfos[4].maxsolutions = _nj4;
19485  vinfos[5].jointtype = 1;
19486  vinfos[5].foffset = j5;
19487  vinfos[5].indices[0] = _ij5[0];
19488  vinfos[5].indices[1] = _ij5[1];
19489  vinfos[5].maxsolutions = _nj5;
19490  vinfos[6].jointtype = 1;
19491  vinfos[6].foffset = j6;
19492  vinfos[6].indices[0] = _ij6[0];
19493  vinfos[6].indices[1] = _ij6[1];
19494  vinfos[6].maxsolutions = _nj6;
19495  std::vector<int> vfree(0);
19496  solutions.AddSolution(vinfos, vfree);
19497  }
19498  }
19499  }
19500  }
19501  }
19502  }
19503  else
19504  {
19505  {
19506  IkReal j0array[1], cj0array[1], sj0array[1];
19507  bool j0valid[1] = { false };
19508  _nj0 = 1;
19509  CheckValue<IkReal> x906 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19510  if (!x906.valid)
19511  {
19512  continue;
19513  }
19514  CheckValue<IkReal> x907 =
19515  IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19516  if (!x907.valid)
19517  {
19518  continue;
19519  }
19520  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x906.value))) + (x907.value));
19521  sj0array[0] = IKsin(j0array[0]);
19522  cj0array[0] = IKcos(j0array[0]);
19523  if (j0array[0] > IKPI)
19524  {
19525  j0array[0] -= IK2PI;
19526  }
19527  else if (j0array[0] < -IKPI)
19528  {
19529  j0array[0] += IK2PI;
19530  }
19531  j0valid[0] = true;
19532  for (int ij0 = 0; ij0 < 1; ++ij0)
19533  {
19534  if (!j0valid[ij0])
19535  {
19536  continue;
19537  }
19538  _ij0[0] = ij0;
19539  _ij0[1] = -1;
19540  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19541  {
19542  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19543  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19544  {
19545  j0valid[iij0] = false;
19546  _ij0[1] = iij0;
19547  break;
19548  }
19549  }
19550  j0 = j0array[ij0];
19551  cj0 = cj0array[ij0];
19552  sj0 = sj0array[ij0];
19553  {
19554  IkReal evalcond[18];
19555  IkReal x908 = IKcos(j0);
19556  IkReal x909 = IKsin(j0);
19557  IkReal x910 = ((1.0) * sj1);
19558  IkReal x911 = ((1.0) * sj2);
19559  IkReal x912 = ((1.0) * cj1);
19560  IkReal x913 = (new_r10 * x909);
19561  IkReal x914 = (cj1 * x908);
19562  IkReal x915 = (cj1 * x909);
19563  IkReal x916 = (new_r00 * x908);
19564  IkReal x917 = ((1.0) * x909);
19565  IkReal x918 = (new_r11 * x909);
19566  IkReal x919 = (new_r12 * x909);
19567  IkReal x920 = (new_r02 * x908);
19568  IkReal x921 = (new_r01 * x908);
19569  evalcond[0] = ((((-1.0) * x908 * x910)) + new_r02);
19570  evalcond[1] = (new_r12 + (((-1.0) * x909 * x910)));
19571  evalcond[2] = ((((-1.0) * new_r02 * x917)) + ((new_r12 * x908)));
19572  evalcond[3] = (sj2 + ((new_r10 * x908)) + (((-1.0) * new_r00 * x917)));
19573  evalcond[4] = (cj2 + ((new_r11 * x908)) + (((-1.0) * new_r01 * x917)));
19574  evalcond[5] = (((sj2 * x908)) + ((cj2 * x915)) + new_r10);
19575  evalcond[6] = ((((-1.0) * x910)) + x920 + x919);
19576  evalcond[7] = (((cj1 * cj2)) + x913 + x916);
19577  evalcond[8] = (((cj2 * x914)) + new_r00 + (((-1.0) * x909 * x911)));
19578  evalcond[9] = (((cj2 * x908)) + new_r11 + (((-1.0) * x911 * x915)));
19579  evalcond[10] = ((((-1.0) * cj1 * x911)) + x921 + x918);
19580  evalcond[11] = (new_r01 + (((-1.0) * x911 * x914)) + (((-1.0) * cj2 * x917)));
19581  evalcond[12] = ((((-1.0) * new_r22 * x910)) + ((new_r12 * x915)) + ((new_r02 * x914)));
19582  evalcond[13] =
19583  (cj2 + (((-1.0) * new_r20 * x910)) + ((new_r00 * x914)) + ((cj1 * x913)));
19584  evalcond[14] =
19585  ((((-1.0) * new_r20 * x912)) + (((-1.0) * x910 * x913)) + (((-1.0) * x910 * x916)));
19586  evalcond[15] =
19587  ((((-1.0) * new_r21 * x912)) + (((-1.0) * x910 * x918)) + (((-1.0) * x910 * x921)));
19588  evalcond[16] = ((1.0) + (((-1.0) * new_r22 * x912)) + (((-1.0) * x910 * x919)) +
19589  (((-1.0) * x910 * x920)));
19590  evalcond[17] = ((((-1.0) * new_r21 * x910)) + ((new_r11 * x915)) + (((-1.0) * x911)) +
19591  ((new_r01 * x914)));
19592  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19593  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19594  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19595  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19596  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19597  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19598  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19599  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19600  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19601  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19602  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19603  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19604  IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19605  IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19606  IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19607  IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19608  IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19609  IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19610  {
19611  continue;
19612  }
19613  }
19614 
19615  {
19616  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19617  vinfos[0].jointtype = 1;
19618  vinfos[0].foffset = j0;
19619  vinfos[0].indices[0] = _ij0[0];
19620  vinfos[0].indices[1] = _ij0[1];
19621  vinfos[0].maxsolutions = _nj0;
19622  vinfos[1].jointtype = 1;
19623  vinfos[1].foffset = j1;
19624  vinfos[1].indices[0] = _ij1[0];
19625  vinfos[1].indices[1] = _ij1[1];
19626  vinfos[1].maxsolutions = _nj1;
19627  vinfos[2].jointtype = 1;
19628  vinfos[2].foffset = j2;
19629  vinfos[2].indices[0] = _ij2[0];
19630  vinfos[2].indices[1] = _ij2[1];
19631  vinfos[2].maxsolutions = _nj2;
19632  vinfos[3].jointtype = 1;
19633  vinfos[3].foffset = j3;
19634  vinfos[3].indices[0] = _ij3[0];
19635  vinfos[3].indices[1] = _ij3[1];
19636  vinfos[3].maxsolutions = _nj3;
19637  vinfos[4].jointtype = 1;
19638  vinfos[4].foffset = j4;
19639  vinfos[4].indices[0] = _ij4[0];
19640  vinfos[4].indices[1] = _ij4[1];
19641  vinfos[4].maxsolutions = _nj4;
19642  vinfos[5].jointtype = 1;
19643  vinfos[5].foffset = j5;
19644  vinfos[5].indices[0] = _ij5[0];
19645  vinfos[5].indices[1] = _ij5[1];
19646  vinfos[5].maxsolutions = _nj5;
19647  vinfos[6].jointtype = 1;
19648  vinfos[6].foffset = j6;
19649  vinfos[6].indices[0] = _ij6[0];
19650  vinfos[6].indices[1] = _ij6[1];
19651  vinfos[6].maxsolutions = _nj6;
19652  std::vector<int> vfree(0);
19653  solutions.AddSolution(vinfos, vfree);
19654  }
19655  }
19656  }
19657  }
19658  }
19659  }
19660  }
19661  }
19662  }
19663  }
19664  else
19665  {
19666  {
19667  IkReal j0array[1], cj0array[1], sj0array[1];
19668  bool j0valid[1] = { false };
19669  _nj0 = 1;
19670  CheckValue<IkReal> x922 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19671  if (!x922.valid)
19672  {
19673  continue;
19674  }
19675  CheckValue<IkReal> x923 = IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19676  if (!x923.valid)
19677  {
19678  continue;
19679  }
19680  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x922.value))) + (x923.value));
19681  sj0array[0] = IKsin(j0array[0]);
19682  cj0array[0] = IKcos(j0array[0]);
19683  if (j0array[0] > IKPI)
19684  {
19685  j0array[0] -= IK2PI;
19686  }
19687  else if (j0array[0] < -IKPI)
19688  {
19689  j0array[0] += IK2PI;
19690  }
19691  j0valid[0] = true;
19692  for (int ij0 = 0; ij0 < 1; ++ij0)
19693  {
19694  if (!j0valid[ij0])
19695  {
19696  continue;
19697  }
19698  _ij0[0] = ij0;
19699  _ij0[1] = -1;
19700  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19701  {
19702  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19703  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19704  {
19705  j0valid[iij0] = false;
19706  _ij0[1] = iij0;
19707  break;
19708  }
19709  }
19710  j0 = j0array[ij0];
19711  cj0 = cj0array[ij0];
19712  sj0 = sj0array[ij0];
19713  {
19714  IkReal evalcond[8];
19715  IkReal x924 = IKcos(j0);
19716  IkReal x925 = IKsin(j0);
19717  IkReal x926 = ((1.0) * cj1);
19718  IkReal x927 = ((1.0) * sj1);
19719  IkReal x928 = (new_r12 * x925);
19720  IkReal x929 = (new_r02 * x924);
19721  evalcond[0] = ((((-1.0) * x924 * x927)) + new_r02);
19722  evalcond[1] = ((((-1.0) * x925 * x927)) + new_r12);
19723  evalcond[2] = ((((-1.0) * new_r02 * x925)) + ((new_r12 * x924)));
19724  evalcond[3] = ((((-1.0) * x927)) + x928 + x929);
19725  evalcond[4] = ((((-1.0) * new_r22 * x927)) + ((cj1 * x929)) + ((cj1 * x928)));
19726  evalcond[5] = ((((-1.0) * new_r10 * x925 * x927)) + (((-1.0) * new_r00 * x924 * x927)) +
19727  (((-1.0) * new_r20 * x926)));
19728  evalcond[6] = ((((-1.0) * new_r21 * x926)) + (((-1.0) * new_r01 * x924 * x927)) +
19729  (((-1.0) * new_r11 * x925 * x927)));
19730  evalcond[7] =
19731  ((1.0) + (((-1.0) * x927 * x928)) + (((-1.0) * x927 * x929)) + (((-1.0) * new_r22 * x926)));
19732  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19733  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19734  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19735  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19736  {
19737  continue;
19738  }
19739  }
19740 
19741  {
19742  IkReal j2eval[3];
19743  j2eval[0] = sj1;
19744  j2eval[1] = IKsign(sj1);
19745  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
19746  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19747  IKabs(j2eval[2]) < 0.0000010000000000)
19748  {
19749  {
19750  IkReal j2eval[2];
19751  j2eval[0] = sj1;
19752  j2eval[1] = cj0;
19753  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
19754  {
19755  {
19756  IkReal j2eval[3];
19757  j2eval[0] = sj1;
19758  j2eval[1] = cj1;
19759  j2eval[2] = sj0;
19760  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19761  IKabs(j2eval[2]) < 0.0000010000000000)
19762  {
19763  {
19764  IkReal evalcond[5];
19765  bool bgotonextstatement = true;
19766  do
19767  {
19768  evalcond[0] = ((-3.14159265358979) +
19769  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
19770  evalcond[1] = new_r21;
19771  evalcond[2] = new_r02;
19772  evalcond[3] = new_r12;
19773  evalcond[4] = new_r20;
19774  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19775  IKabs(evalcond[1]) < 0.0000050000000000 &&
19776  IKabs(evalcond[2]) < 0.0000050000000000 &&
19777  IKabs(evalcond[3]) < 0.0000050000000000 &&
19778  IKabs(evalcond[4]) < 0.0000050000000000)
19779  {
19780  bgotonextstatement = false;
19781  {
19782  IkReal j2array[1], cj2array[1], sj2array[1];
19783  bool j2valid[1] = { false };
19784  _nj2 = 1;
19785  IkReal x930 = ((1.0) * cj0);
19786  if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) <
19787  IKFAST_ATAN2_MAGTHRESH &&
19788  IKabs(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) <
19789  IKFAST_ATAN2_MAGTHRESH &&
19790  IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) +
19791  IKsqr(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) -
19792  1) <= IKFAST_SINCOS_THRESH)
19793  continue;
19794  j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930))),
19795  ((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0))));
19796  sj2array[0] = IKsin(j2array[0]);
19797  cj2array[0] = IKcos(j2array[0]);
19798  if (j2array[0] > IKPI)
19799  {
19800  j2array[0] -= IK2PI;
19801  }
19802  else if (j2array[0] < -IKPI)
19803  {
19804  j2array[0] += IK2PI;
19805  }
19806  j2valid[0] = true;
19807  for (int ij2 = 0; ij2 < 1; ++ij2)
19808  {
19809  if (!j2valid[ij2])
19810  {
19811  continue;
19812  }
19813  _ij2[0] = ij2;
19814  _ij2[1] = -1;
19815  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19816  {
19817  if (j2valid[iij2] &&
19818  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19819  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19820  {
19821  j2valid[iij2] = false;
19822  _ij2[1] = iij2;
19823  break;
19824  }
19825  }
19826  j2 = j2array[ij2];
19827  cj2 = cj2array[ij2];
19828  sj2 = sj2array[ij2];
19829  {
19830  IkReal evalcond[8];
19831  IkReal x931 = IKcos(j2);
19832  IkReal x932 = IKsin(j2);
19833  IkReal x933 = ((1.0) * sj0);
19834  IkReal x934 = (cj0 * x931);
19835  IkReal x935 = (cj0 * x932);
19836  IkReal x936 = (x932 * x933);
19837  evalcond[0] = (((new_r10 * sj0)) + ((cj0 * new_r00)) + x931);
19838  evalcond[1] = ((((-1.0) * new_r00 * x933)) + ((cj0 * new_r10)) + x932);
19839  evalcond[2] = ((((-1.0) * new_r01 * x933)) + ((cj0 * new_r11)) + x931);
19840  evalcond[3] = (((sj0 * x931)) + new_r10 + x935);
19841  evalcond[4] = (((new_r11 * sj0)) + (((-1.0) * x932)) + ((cj0 * new_r01)));
19842  evalcond[5] = (new_r00 + x934 + (((-1.0) * x936)));
19843  evalcond[6] = (new_r11 + x934 + (((-1.0) * x936)));
19844  evalcond[7] = ((((-1.0) * x931 * x933)) + (((-1.0) * x935)) + new_r01);
19845  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19846  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19847  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19848  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19849  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19850  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19851  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19852  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19853  {
19854  continue;
19855  }
19856  }
19857 
19858  {
19859  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19860  vinfos[0].jointtype = 1;
19861  vinfos[0].foffset = j0;
19862  vinfos[0].indices[0] = _ij0[0];
19863  vinfos[0].indices[1] = _ij0[1];
19864  vinfos[0].maxsolutions = _nj0;
19865  vinfos[1].jointtype = 1;
19866  vinfos[1].foffset = j1;
19867  vinfos[1].indices[0] = _ij1[0];
19868  vinfos[1].indices[1] = _ij1[1];
19869  vinfos[1].maxsolutions = _nj1;
19870  vinfos[2].jointtype = 1;
19871  vinfos[2].foffset = j2;
19872  vinfos[2].indices[0] = _ij2[0];
19873  vinfos[2].indices[1] = _ij2[1];
19874  vinfos[2].maxsolutions = _nj2;
19875  vinfos[3].jointtype = 1;
19876  vinfos[3].foffset = j3;
19877  vinfos[3].indices[0] = _ij3[0];
19878  vinfos[3].indices[1] = _ij3[1];
19879  vinfos[3].maxsolutions = _nj3;
19880  vinfos[4].jointtype = 1;
19881  vinfos[4].foffset = j4;
19882  vinfos[4].indices[0] = _ij4[0];
19883  vinfos[4].indices[1] = _ij4[1];
19884  vinfos[4].maxsolutions = _nj4;
19885  vinfos[5].jointtype = 1;
19886  vinfos[5].foffset = j5;
19887  vinfos[5].indices[0] = _ij5[0];
19888  vinfos[5].indices[1] = _ij5[1];
19889  vinfos[5].maxsolutions = _nj5;
19890  vinfos[6].jointtype = 1;
19891  vinfos[6].foffset = j6;
19892  vinfos[6].indices[0] = _ij6[0];
19893  vinfos[6].indices[1] = _ij6[1];
19894  vinfos[6].maxsolutions = _nj6;
19895  std::vector<int> vfree(0);
19896  solutions.AddSolution(vinfos, vfree);
19897  }
19898  }
19899  }
19900  }
19901  } while (0);
19902  if (bgotonextstatement)
19903  {
19904  bool bgotonextstatement = true;
19905  do
19906  {
19907  evalcond[0] = ((-3.14159265358979) +
19908  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
19909  6.28318530717959)));
19910  evalcond[1] = new_r21;
19911  evalcond[2] = new_r02;
19912  evalcond[3] = new_r12;
19913  evalcond[4] = new_r20;
19914  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19915  IKabs(evalcond[1]) < 0.0000050000000000 &&
19916  IKabs(evalcond[2]) < 0.0000050000000000 &&
19917  IKabs(evalcond[3]) < 0.0000050000000000 &&
19918  IKabs(evalcond[4]) < 0.0000050000000000)
19919  {
19920  bgotonextstatement = false;
19921  {
19922  IkReal j2array[1], cj2array[1], sj2array[1];
19923  bool j2valid[1] = { false };
19924  _nj2 = 1;
19925  IkReal x937 = ((1.0) * cj0);
19926  if (IKabs(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) <
19927  IKFAST_ATAN2_MAGTHRESH &&
19928  IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) <
19929  IKFAST_ATAN2_MAGTHRESH &&
19930  IKabs(IKsqr(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) +
19931  IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) - 1) <=
19932  IKFAST_SINCOS_THRESH)
19933  continue;
19934  j2array[0] = IKatan2(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0))),
19935  (((new_r10 * sj0)) + (((-1.0) * new_r11 * x937))));
19936  sj2array[0] = IKsin(j2array[0]);
19937  cj2array[0] = IKcos(j2array[0]);
19938  if (j2array[0] > IKPI)
19939  {
19940  j2array[0] -= IK2PI;
19941  }
19942  else if (j2array[0] < -IKPI)
19943  {
19944  j2array[0] += IK2PI;
19945  }
19946  j2valid[0] = true;
19947  for (int ij2 = 0; ij2 < 1; ++ij2)
19948  {
19949  if (!j2valid[ij2])
19950  {
19951  continue;
19952  }
19953  _ij2[0] = ij2;
19954  _ij2[1] = -1;
19955  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19956  {
19957  if (j2valid[iij2] &&
19958  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19959  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19960  {
19961  j2valid[iij2] = false;
19962  _ij2[1] = iij2;
19963  break;
19964  }
19965  }
19966  j2 = j2array[ij2];
19967  cj2 = cj2array[ij2];
19968  sj2 = sj2array[ij2];
19969  {
19970  IkReal evalcond[8];
19971  IkReal x938 = IKsin(j2);
19972  IkReal x939 = IKcos(j2);
19973  IkReal x940 = ((1.0) * sj0);
19974  IkReal x941 = (cj0 * x938);
19975  IkReal x942 = ((1.0) * x939);
19976  IkReal x943 = (x939 * x940);
19977  evalcond[0] = (((new_r11 * sj0)) + ((cj0 * new_r01)) + x938);
19978  evalcond[1] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x940)) + x938);
19979  evalcond[2] = ((((-1.0) * new_r01 * x940)) + ((cj0 * new_r11)) + x939);
19980  evalcond[3] = ((((-1.0) * x942)) + ((new_r10 * sj0)) + ((cj0 * new_r00)));
19981  evalcond[4] = (((sj0 * x938)) + ((cj0 * x939)) + new_r11);
19982  evalcond[5] = ((((-1.0) * x943)) + new_r10 + x941);
19983  evalcond[6] = ((((-1.0) * x943)) + new_r01 + x941);
19984  evalcond[7] =
19985  ((((-1.0) * x938 * x940)) + (((-1.0) * cj0 * x942)) + new_r00);
19986  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19987  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19988  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19989  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19990  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19991  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19992  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19993  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19994  {
19995  continue;
19996  }
19997  }
19998 
19999  {
20000  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20001  vinfos[0].jointtype = 1;
20002  vinfos[0].foffset = j0;
20003  vinfos[0].indices[0] = _ij0[0];
20004  vinfos[0].indices[1] = _ij0[1];
20005  vinfos[0].maxsolutions = _nj0;
20006  vinfos[1].jointtype = 1;
20007  vinfos[1].foffset = j1;
20008  vinfos[1].indices[0] = _ij1[0];
20009  vinfos[1].indices[1] = _ij1[1];
20010  vinfos[1].maxsolutions = _nj1;
20011  vinfos[2].jointtype = 1;
20012  vinfos[2].foffset = j2;
20013  vinfos[2].indices[0] = _ij2[0];
20014  vinfos[2].indices[1] = _ij2[1];
20015  vinfos[2].maxsolutions = _nj2;
20016  vinfos[3].jointtype = 1;
20017  vinfos[3].foffset = j3;
20018  vinfos[3].indices[0] = _ij3[0];
20019  vinfos[3].indices[1] = _ij3[1];
20020  vinfos[3].maxsolutions = _nj3;
20021  vinfos[4].jointtype = 1;
20022  vinfos[4].foffset = j4;
20023  vinfos[4].indices[0] = _ij4[0];
20024  vinfos[4].indices[1] = _ij4[1];
20025  vinfos[4].maxsolutions = _nj4;
20026  vinfos[5].jointtype = 1;
20027  vinfos[5].foffset = j5;
20028  vinfos[5].indices[0] = _ij5[0];
20029  vinfos[5].indices[1] = _ij5[1];
20030  vinfos[5].maxsolutions = _nj5;
20031  vinfos[6].jointtype = 1;
20032  vinfos[6].foffset = j6;
20033  vinfos[6].indices[0] = _ij6[0];
20034  vinfos[6].indices[1] = _ij6[1];
20035  vinfos[6].maxsolutions = _nj6;
20036  std::vector<int> vfree(0);
20037  solutions.AddSolution(vinfos, vfree);
20038  }
20039  }
20040  }
20041  }
20042  } while (0);
20043  if (bgotonextstatement)
20044  {
20045  bool bgotonextstatement = true;
20046  do
20047  {
20048  evalcond[0] = ((-3.14159265358979) +
20049  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
20050  6.28318530717959)));
20051  evalcond[1] = new_r22;
20052  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20053  IKabs(evalcond[1]) < 0.0000050000000000)
20054  {
20055  bgotonextstatement = false;
20056  {
20057  IkReal j2array[1], cj2array[1], sj2array[1];
20058  bool j2valid[1] = { false };
20059  _nj2 = 1;
20060  if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
20061  IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
20062  IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
20063  IKFAST_SINCOS_THRESH)
20064  continue;
20065  j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
20066  sj2array[0] = IKsin(j2array[0]);
20067  cj2array[0] = IKcos(j2array[0]);
20068  if (j2array[0] > IKPI)
20069  {
20070  j2array[0] -= IK2PI;
20071  }
20072  else if (j2array[0] < -IKPI)
20073  {
20074  j2array[0] += IK2PI;
20075  }
20076  j2valid[0] = true;
20077  for (int ij2 = 0; ij2 < 1; ++ij2)
20078  {
20079  if (!j2valid[ij2])
20080  {
20081  continue;
20082  }
20083  _ij2[0] = ij2;
20084  _ij2[1] = -1;
20085  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20086  {
20087  if (j2valid[iij2] &&
20088  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20089  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20090  {
20091  j2valid[iij2] = false;
20092  _ij2[1] = iij2;
20093  break;
20094  }
20095  }
20096  j2 = j2array[ij2];
20097  cj2 = cj2array[ij2];
20098  sj2 = sj2array[ij2];
20099  {
20100  IkReal evalcond[8];
20101  IkReal x944 = IKsin(j2);
20102  IkReal x945 = IKcos(j2);
20103  IkReal x946 = ((1.0) * sj0);
20104  evalcond[0] = (new_r21 + x944);
20105  evalcond[1] = ((((-1.0) * x945)) + new_r20);
20106  evalcond[2] = (((new_r02 * x944)) + new_r10);
20107  evalcond[3] = (((cj0 * x945)) + new_r11);
20108  evalcond[4] = ((((-1.0) * x944 * x946)) + new_r00);
20109  evalcond[5] = ((((-1.0) * x945 * x946)) + new_r01);
20110  evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x946)) + x944);
20111  evalcond[7] = ((((-1.0) * new_r01 * x946)) + ((cj0 * new_r11)) + x945);
20112  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20113  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20114  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20115  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20116  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20117  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20118  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20119  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20120  {
20121  continue;
20122  }
20123  }
20124 
20125  {
20126  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20127  vinfos[0].jointtype = 1;
20128  vinfos[0].foffset = j0;
20129  vinfos[0].indices[0] = _ij0[0];
20130  vinfos[0].indices[1] = _ij0[1];
20131  vinfos[0].maxsolutions = _nj0;
20132  vinfos[1].jointtype = 1;
20133  vinfos[1].foffset = j1;
20134  vinfos[1].indices[0] = _ij1[0];
20135  vinfos[1].indices[1] = _ij1[1];
20136  vinfos[1].maxsolutions = _nj1;
20137  vinfos[2].jointtype = 1;
20138  vinfos[2].foffset = j2;
20139  vinfos[2].indices[0] = _ij2[0];
20140  vinfos[2].indices[1] = _ij2[1];
20141  vinfos[2].maxsolutions = _nj2;
20142  vinfos[3].jointtype = 1;
20143  vinfos[3].foffset = j3;
20144  vinfos[3].indices[0] = _ij3[0];
20145  vinfos[3].indices[1] = _ij3[1];
20146  vinfos[3].maxsolutions = _nj3;
20147  vinfos[4].jointtype = 1;
20148  vinfos[4].foffset = j4;
20149  vinfos[4].indices[0] = _ij4[0];
20150  vinfos[4].indices[1] = _ij4[1];
20151  vinfos[4].maxsolutions = _nj4;
20152  vinfos[5].jointtype = 1;
20153  vinfos[5].foffset = j5;
20154  vinfos[5].indices[0] = _ij5[0];
20155  vinfos[5].indices[1] = _ij5[1];
20156  vinfos[5].maxsolutions = _nj5;
20157  vinfos[6].jointtype = 1;
20158  vinfos[6].foffset = j6;
20159  vinfos[6].indices[0] = _ij6[0];
20160  vinfos[6].indices[1] = _ij6[1];
20161  vinfos[6].maxsolutions = _nj6;
20162  std::vector<int> vfree(0);
20163  solutions.AddSolution(vinfos, vfree);
20164  }
20165  }
20166  }
20167  }
20168  } while (0);
20169  if (bgotonextstatement)
20170  {
20171  bool bgotonextstatement = true;
20172  do
20173  {
20174  evalcond[0] = ((-3.14159265358979) +
20175  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
20176  6.28318530717959)));
20177  evalcond[1] = new_r22;
20178  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20179  IKabs(evalcond[1]) < 0.0000050000000000)
20180  {
20181  bgotonextstatement = false;
20182  {
20183  IkReal j2array[1], cj2array[1], sj2array[1];
20184  bool j2valid[1] = { false };
20185  _nj2 = 1;
20186  if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
20187  IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
20188  IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
20189  IKFAST_SINCOS_THRESH)
20190  continue;
20191  j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
20192  sj2array[0] = IKsin(j2array[0]);
20193  cj2array[0] = IKcos(j2array[0]);
20194  if (j2array[0] > IKPI)
20195  {
20196  j2array[0] -= IK2PI;
20197  }
20198  else if (j2array[0] < -IKPI)
20199  {
20200  j2array[0] += IK2PI;
20201  }
20202  j2valid[0] = true;
20203  for (int ij2 = 0; ij2 < 1; ++ij2)
20204  {
20205  if (!j2valid[ij2])
20206  {
20207  continue;
20208  }
20209  _ij2[0] = ij2;
20210  _ij2[1] = -1;
20211  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20212  {
20213  if (j2valid[iij2] &&
20214  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20215  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20216  {
20217  j2valid[iij2] = false;
20218  _ij2[1] = iij2;
20219  break;
20220  }
20221  }
20222  j2 = j2array[ij2];
20223  cj2 = cj2array[ij2];
20224  sj2 = sj2array[ij2];
20225  {
20226  IkReal evalcond[8];
20227  IkReal x947 = IKcos(j2);
20228  IkReal x948 = IKsin(j2);
20229  IkReal x949 = ((1.0) * sj0);
20230  IkReal x950 = ((1.0) * x948);
20231  evalcond[0] = (new_r20 + x947);
20232  evalcond[1] = (new_r21 + (((-1.0) * x950)));
20233  evalcond[2] = (((cj0 * x947)) + new_r11);
20234  evalcond[3] = (new_r10 + (((-1.0) * new_r02 * x950)));
20235  evalcond[4] = ((((-1.0) * x948 * x949)) + new_r00);
20236  evalcond[5] = ((((-1.0) * x947 * x949)) + new_r01);
20237  evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x949)) + x948);
20238  evalcond[7] = ((((-1.0) * new_r01 * x949)) + ((cj0 * new_r11)) + x947);
20239  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20240  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20241  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20242  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20243  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20244  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20245  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20246  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20247  {
20248  continue;
20249  }
20250  }
20251 
20252  {
20253  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20254  vinfos[0].jointtype = 1;
20255  vinfos[0].foffset = j0;
20256  vinfos[0].indices[0] = _ij0[0];
20257  vinfos[0].indices[1] = _ij0[1];
20258  vinfos[0].maxsolutions = _nj0;
20259  vinfos[1].jointtype = 1;
20260  vinfos[1].foffset = j1;
20261  vinfos[1].indices[0] = _ij1[0];
20262  vinfos[1].indices[1] = _ij1[1];
20263  vinfos[1].maxsolutions = _nj1;
20264  vinfos[2].jointtype = 1;
20265  vinfos[2].foffset = j2;
20266  vinfos[2].indices[0] = _ij2[0];
20267  vinfos[2].indices[1] = _ij2[1];
20268  vinfos[2].maxsolutions = _nj2;
20269  vinfos[3].jointtype = 1;
20270  vinfos[3].foffset = j3;
20271  vinfos[3].indices[0] = _ij3[0];
20272  vinfos[3].indices[1] = _ij3[1];
20273  vinfos[3].maxsolutions = _nj3;
20274  vinfos[4].jointtype = 1;
20275  vinfos[4].foffset = j4;
20276  vinfos[4].indices[0] = _ij4[0];
20277  vinfos[4].indices[1] = _ij4[1];
20278  vinfos[4].maxsolutions = _nj4;
20279  vinfos[5].jointtype = 1;
20280  vinfos[5].foffset = j5;
20281  vinfos[5].indices[0] = _ij5[0];
20282  vinfos[5].indices[1] = _ij5[1];
20283  vinfos[5].maxsolutions = _nj5;
20284  vinfos[6].jointtype = 1;
20285  vinfos[6].foffset = j6;
20286  vinfos[6].indices[0] = _ij6[0];
20287  vinfos[6].indices[1] = _ij6[1];
20288  vinfos[6].maxsolutions = _nj6;
20289  std::vector<int> vfree(0);
20290  solutions.AddSolution(vinfos, vfree);
20291  }
20292  }
20293  }
20294  }
20295  } while (0);
20296  if (bgotonextstatement)
20297  {
20298  bool bgotonextstatement = true;
20299  do
20300  {
20301  evalcond[0] =
20302  ((-3.14159265358979) +
20303  (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
20304  evalcond[1] = new_r12;
20305  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20306  IKabs(evalcond[1]) < 0.0000050000000000)
20307  {
20308  bgotonextstatement = false;
20309  {
20310  IkReal j2array[1], cj2array[1], sj2array[1];
20311  bool j2valid[1] = { false };
20312  _nj2 = 1;
20313  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
20314  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
20315  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) - 1) <=
20316  IKFAST_SINCOS_THRESH)
20317  continue;
20318  j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
20319  sj2array[0] = IKsin(j2array[0]);
20320  cj2array[0] = IKcos(j2array[0]);
20321  if (j2array[0] > IKPI)
20322  {
20323  j2array[0] -= IK2PI;
20324  }
20325  else if (j2array[0] < -IKPI)
20326  {
20327  j2array[0] += IK2PI;
20328  }
20329  j2valid[0] = true;
20330  for (int ij2 = 0; ij2 < 1; ++ij2)
20331  {
20332  if (!j2valid[ij2])
20333  {
20334  continue;
20335  }
20336  _ij2[0] = ij2;
20337  _ij2[1] = -1;
20338  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20339  {
20340  if (j2valid[iij2] &&
20341  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20342  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20343  {
20344  j2valid[iij2] = false;
20345  _ij2[1] = iij2;
20346  break;
20347  }
20348  }
20349  j2 = j2array[ij2];
20350  cj2 = cj2array[ij2];
20351  sj2 = sj2array[ij2];
20352  {
20353  IkReal evalcond[8];
20354  IkReal x951 = IKsin(j2);
20355  IkReal x952 = IKcos(j2);
20356  IkReal x953 = ((1.0) * sj1);
20357  IkReal x954 = ((1.0) * x951);
20358  evalcond[0] = (new_r10 + x951);
20359  evalcond[1] = (new_r11 + x952);
20360  evalcond[2] = (((sj1 * x951)) + new_r21);
20361  evalcond[3] = (((cj1 * x952)) + new_r00);
20362  evalcond[4] = (new_r20 + (((-1.0) * x952 * x953)));
20363  evalcond[5] = ((((-1.0) * cj1 * x954)) + new_r01);
20364  evalcond[6] =
20365  (((cj1 * new_r00)) + x952 + (((-1.0) * new_r20 * x953)));
20366  evalcond[7] = ((((-1.0) * new_r21 * x953)) + ((cj1 * new_r01)) +
20367  (((-1.0) * x954)));
20368  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20369  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20370  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20371  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20372  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20373  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20374  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20375  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20376  {
20377  continue;
20378  }
20379  }
20380 
20381  {
20382  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20383  vinfos[0].jointtype = 1;
20384  vinfos[0].foffset = j0;
20385  vinfos[0].indices[0] = _ij0[0];
20386  vinfos[0].indices[1] = _ij0[1];
20387  vinfos[0].maxsolutions = _nj0;
20388  vinfos[1].jointtype = 1;
20389  vinfos[1].foffset = j1;
20390  vinfos[1].indices[0] = _ij1[0];
20391  vinfos[1].indices[1] = _ij1[1];
20392  vinfos[1].maxsolutions = _nj1;
20393  vinfos[2].jointtype = 1;
20394  vinfos[2].foffset = j2;
20395  vinfos[2].indices[0] = _ij2[0];
20396  vinfos[2].indices[1] = _ij2[1];
20397  vinfos[2].maxsolutions = _nj2;
20398  vinfos[3].jointtype = 1;
20399  vinfos[3].foffset = j3;
20400  vinfos[3].indices[0] = _ij3[0];
20401  vinfos[3].indices[1] = _ij3[1];
20402  vinfos[3].maxsolutions = _nj3;
20403  vinfos[4].jointtype = 1;
20404  vinfos[4].foffset = j4;
20405  vinfos[4].indices[0] = _ij4[0];
20406  vinfos[4].indices[1] = _ij4[1];
20407  vinfos[4].maxsolutions = _nj4;
20408  vinfos[5].jointtype = 1;
20409  vinfos[5].foffset = j5;
20410  vinfos[5].indices[0] = _ij5[0];
20411  vinfos[5].indices[1] = _ij5[1];
20412  vinfos[5].maxsolutions = _nj5;
20413  vinfos[6].jointtype = 1;
20414  vinfos[6].foffset = j6;
20415  vinfos[6].indices[0] = _ij6[0];
20416  vinfos[6].indices[1] = _ij6[1];
20417  vinfos[6].maxsolutions = _nj6;
20418  std::vector<int> vfree(0);
20419  solutions.AddSolution(vinfos, vfree);
20420  }
20421  }
20422  }
20423  }
20424  } while (0);
20425  if (bgotonextstatement)
20426  {
20427  bool bgotonextstatement = true;
20428  do
20429  {
20430  evalcond[0] =
20431  ((-3.14159265358979) +
20432  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j0)))),
20433  6.28318530717959)));
20434  evalcond[1] = new_r12;
20435  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20436  IKabs(evalcond[1]) < 0.0000050000000000)
20437  {
20438  bgotonextstatement = false;
20439  {
20440  IkReal j2array[1], cj2array[1], sj2array[1];
20441  bool j2valid[1] = { false };
20442  _nj2 = 1;
20443  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
20444  IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
20445  IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <= IKFAST_SINCOS_THRESH)
20446  continue;
20447  j2array[0] = IKatan2(new_r10, new_r11);
20448  sj2array[0] = IKsin(j2array[0]);
20449  cj2array[0] = IKcos(j2array[0]);
20450  if (j2array[0] > IKPI)
20451  {
20452  j2array[0] -= IK2PI;
20453  }
20454  else if (j2array[0] < -IKPI)
20455  {
20456  j2array[0] += IK2PI;
20457  }
20458  j2valid[0] = true;
20459  for (int ij2 = 0; ij2 < 1; ++ij2)
20460  {
20461  if (!j2valid[ij2])
20462  {
20463  continue;
20464  }
20465  _ij2[0] = ij2;
20466  _ij2[1] = -1;
20467  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20468  {
20469  if (j2valid[iij2] &&
20470  IKabs(cj2array[ij2] - cj2array[iij2]) <
20471  IKFAST_SOLUTION_THRESH &&
20472  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20473  {
20474  j2valid[iij2] = false;
20475  _ij2[1] = iij2;
20476  break;
20477  }
20478  }
20479  j2 = j2array[ij2];
20480  cj2 = cj2array[ij2];
20481  sj2 = sj2array[ij2];
20482  {
20483  IkReal evalcond[8];
20484  IkReal x955 = IKsin(j2);
20485  IkReal x956 = IKcos(j2);
20486  IkReal x957 = ((1.0) * sj1);
20487  IkReal x958 = ((1.0) * new_r00);
20488  IkReal x959 = ((1.0) * new_r01);
20489  IkReal x960 = ((1.0) * x955);
20490  evalcond[0] = (((sj1 * x955)) + new_r21);
20491  evalcond[1] = ((((-1.0) * new_r10)) + x955);
20492  evalcond[2] = ((((-1.0) * new_r11)) + x956);
20493  evalcond[3] = ((((-1.0) * x956 * x957)) + new_r20);
20494  evalcond[4] = (((cj1 * x956)) + (((-1.0) * x958)));
20495  evalcond[5] = ((((-1.0) * cj1 * x960)) + (((-1.0) * x959)));
20496  evalcond[6] =
20497  ((((-1.0) * cj1 * x958)) + x956 + (((-1.0) * new_r20 * x957)));
20498  evalcond[7] = ((((-1.0) * new_r21 * x957)) +
20499  (((-1.0) * cj1 * x959)) + (((-1.0) * x960)));
20500  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20501  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20502  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20503  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20504  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20505  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20506  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20507  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20508  {
20509  continue;
20510  }
20511  }
20512 
20513  {
20514  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20515  vinfos[0].jointtype = 1;
20516  vinfos[0].foffset = j0;
20517  vinfos[0].indices[0] = _ij0[0];
20518  vinfos[0].indices[1] = _ij0[1];
20519  vinfos[0].maxsolutions = _nj0;
20520  vinfos[1].jointtype = 1;
20521  vinfos[1].foffset = j1;
20522  vinfos[1].indices[0] = _ij1[0];
20523  vinfos[1].indices[1] = _ij1[1];
20524  vinfos[1].maxsolutions = _nj1;
20525  vinfos[2].jointtype = 1;
20526  vinfos[2].foffset = j2;
20527  vinfos[2].indices[0] = _ij2[0];
20528  vinfos[2].indices[1] = _ij2[1];
20529  vinfos[2].maxsolutions = _nj2;
20530  vinfos[3].jointtype = 1;
20531  vinfos[3].foffset = j3;
20532  vinfos[3].indices[0] = _ij3[0];
20533  vinfos[3].indices[1] = _ij3[1];
20534  vinfos[3].maxsolutions = _nj3;
20535  vinfos[4].jointtype = 1;
20536  vinfos[4].foffset = j4;
20537  vinfos[4].indices[0] = _ij4[0];
20538  vinfos[4].indices[1] = _ij4[1];
20539  vinfos[4].maxsolutions = _nj4;
20540  vinfos[5].jointtype = 1;
20541  vinfos[5].foffset = j5;
20542  vinfos[5].indices[0] = _ij5[0];
20543  vinfos[5].indices[1] = _ij5[1];
20544  vinfos[5].maxsolutions = _nj5;
20545  vinfos[6].jointtype = 1;
20546  vinfos[6].foffset = j6;
20547  vinfos[6].indices[0] = _ij6[0];
20548  vinfos[6].indices[1] = _ij6[1];
20549  vinfos[6].maxsolutions = _nj6;
20550  std::vector<int> vfree(0);
20551  solutions.AddSolution(vinfos, vfree);
20552  }
20553  }
20554  }
20555  }
20556  } while (0);
20557  if (bgotonextstatement)
20558  {
20559  bool bgotonextstatement = true;
20560  do
20561  {
20562  evalcond[0] =
20563  ((-3.14159265358979) +
20564  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j0)))),
20565  6.28318530717959)));
20566  evalcond[1] = new_r02;
20567  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20568  IKabs(evalcond[1]) < 0.0000050000000000)
20569  {
20570  bgotonextstatement = false;
20571  {
20572  IkReal j2array[1], cj2array[1], sj2array[1];
20573  bool j2valid[1] = { false };
20574  _nj2 = 1;
20575  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
20576  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
20577  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
20578  IKFAST_SINCOS_THRESH)
20579  continue;
20580  j2array[0] = IKatan2(new_r00, new_r01);
20581  sj2array[0] = IKsin(j2array[0]);
20582  cj2array[0] = IKcos(j2array[0]);
20583  if (j2array[0] > IKPI)
20584  {
20585  j2array[0] -= IK2PI;
20586  }
20587  else if (j2array[0] < -IKPI)
20588  {
20589  j2array[0] += IK2PI;
20590  }
20591  j2valid[0] = true;
20592  for (int ij2 = 0; ij2 < 1; ++ij2)
20593  {
20594  if (!j2valid[ij2])
20595  {
20596  continue;
20597  }
20598  _ij2[0] = ij2;
20599  _ij2[1] = -1;
20600  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20601  {
20602  if (j2valid[iij2] &&
20603  IKabs(cj2array[ij2] - cj2array[iij2]) <
20604  IKFAST_SOLUTION_THRESH &&
20605  IKabs(sj2array[ij2] - sj2array[iij2]) <
20606  IKFAST_SOLUTION_THRESH)
20607  {
20608  j2valid[iij2] = false;
20609  _ij2[1] = iij2;
20610  break;
20611  }
20612  }
20613  j2 = j2array[ij2];
20614  cj2 = cj2array[ij2];
20615  sj2 = sj2array[ij2];
20616  {
20617  IkReal evalcond[8];
20618  IkReal x961 = IKsin(j2);
20619  IkReal x962 = IKcos(j2);
20620  IkReal x963 = ((1.0) * sj1);
20621  IkReal x964 = ((1.0) * x961);
20622  evalcond[0] = (((sj1 * x961)) + new_r21);
20623  evalcond[1] = ((((-1.0) * new_r00)) + x961);
20624  evalcond[2] = ((((-1.0) * new_r01)) + x962);
20625  evalcond[3] = (((cj1 * x962)) + new_r10);
20626  evalcond[4] = ((((-1.0) * x962 * x963)) + new_r20);
20627  evalcond[5] = ((((-1.0) * cj1 * x964)) + new_r11);
20628  evalcond[6] =
20629  (((cj1 * new_r10)) + x962 + (((-1.0) * new_r20 * x963)));
20630  evalcond[7] = ((((-1.0) * new_r21 * x963)) + ((cj1 * new_r11)) +
20631  (((-1.0) * x964)));
20632  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20633  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20634  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20635  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20636  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20637  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20638  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20639  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20640  {
20641  continue;
20642  }
20643  }
20644 
20645  {
20646  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20647  vinfos[0].jointtype = 1;
20648  vinfos[0].foffset = j0;
20649  vinfos[0].indices[0] = _ij0[0];
20650  vinfos[0].indices[1] = _ij0[1];
20651  vinfos[0].maxsolutions = _nj0;
20652  vinfos[1].jointtype = 1;
20653  vinfos[1].foffset = j1;
20654  vinfos[1].indices[0] = _ij1[0];
20655  vinfos[1].indices[1] = _ij1[1];
20656  vinfos[1].maxsolutions = _nj1;
20657  vinfos[2].jointtype = 1;
20658  vinfos[2].foffset = j2;
20659  vinfos[2].indices[0] = _ij2[0];
20660  vinfos[2].indices[1] = _ij2[1];
20661  vinfos[2].maxsolutions = _nj2;
20662  vinfos[3].jointtype = 1;
20663  vinfos[3].foffset = j3;
20664  vinfos[3].indices[0] = _ij3[0];
20665  vinfos[3].indices[1] = _ij3[1];
20666  vinfos[3].maxsolutions = _nj3;
20667  vinfos[4].jointtype = 1;
20668  vinfos[4].foffset = j4;
20669  vinfos[4].indices[0] = _ij4[0];
20670  vinfos[4].indices[1] = _ij4[1];
20671  vinfos[4].maxsolutions = _nj4;
20672  vinfos[5].jointtype = 1;
20673  vinfos[5].foffset = j5;
20674  vinfos[5].indices[0] = _ij5[0];
20675  vinfos[5].indices[1] = _ij5[1];
20676  vinfos[5].maxsolutions = _nj5;
20677  vinfos[6].jointtype = 1;
20678  vinfos[6].foffset = j6;
20679  vinfos[6].indices[0] = _ij6[0];
20680  vinfos[6].indices[1] = _ij6[1];
20681  vinfos[6].maxsolutions = _nj6;
20682  std::vector<int> vfree(0);
20683  solutions.AddSolution(vinfos, vfree);
20684  }
20685  }
20686  }
20687  }
20688  } while (0);
20689  if (bgotonextstatement)
20690  {
20691  bool bgotonextstatement = true;
20692  do
20693  {
20694  evalcond[0] =
20695  ((-3.14159265358979) +
20696  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j0)))),
20697  6.28318530717959)));
20698  evalcond[1] = new_r02;
20699  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20700  IKabs(evalcond[1]) < 0.0000050000000000)
20701  {
20702  bgotonextstatement = false;
20703  {
20704  IkReal j2array[1], cj2array[1], sj2array[1];
20705  bool j2valid[1] = { false };
20706  _nj2 = 1;
20707  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
20708  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
20709  IKabs(IKsqr(((-1.0) * new_r00)) + IKsqr(((-1.0) * new_r01)) -
20710  1) <= IKFAST_SINCOS_THRESH)
20711  continue;
20712  j2array[0] = IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
20713  sj2array[0] = IKsin(j2array[0]);
20714  cj2array[0] = IKcos(j2array[0]);
20715  if (j2array[0] > IKPI)
20716  {
20717  j2array[0] -= IK2PI;
20718  }
20719  else if (j2array[0] < -IKPI)
20720  {
20721  j2array[0] += IK2PI;
20722  }
20723  j2valid[0] = true;
20724  for (int ij2 = 0; ij2 < 1; ++ij2)
20725  {
20726  if (!j2valid[ij2])
20727  {
20728  continue;
20729  }
20730  _ij2[0] = ij2;
20731  _ij2[1] = -1;
20732  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20733  {
20734  if (j2valid[iij2] &&
20735  IKabs(cj2array[ij2] - cj2array[iij2]) <
20736  IKFAST_SOLUTION_THRESH &&
20737  IKabs(sj2array[ij2] - sj2array[iij2]) <
20738  IKFAST_SOLUTION_THRESH)
20739  {
20740  j2valid[iij2] = false;
20741  _ij2[1] = iij2;
20742  break;
20743  }
20744  }
20745  j2 = j2array[ij2];
20746  cj2 = cj2array[ij2];
20747  sj2 = sj2array[ij2];
20748  {
20749  IkReal evalcond[8];
20750  IkReal x965 = IKsin(j2);
20751  IkReal x966 = IKcos(j2);
20752  IkReal x967 = ((1.0) * new_r11);
20753  IkReal x968 = ((1.0) * sj1);
20754  IkReal x969 = ((1.0) * new_r10);
20755  IkReal x970 = ((1.0) * x965);
20756  evalcond[0] = (new_r00 + x965);
20757  evalcond[1] = (new_r01 + x966);
20758  evalcond[2] = (((sj1 * x965)) + new_r21);
20759  evalcond[3] = ((((-1.0) * x966 * x968)) + new_r20);
20760  evalcond[4] = (((cj1 * x966)) + (((-1.0) * x969)));
20761  evalcond[5] = ((((-1.0) * cj1 * x970)) + (((-1.0) * x967)));
20762  evalcond[6] = ((((-1.0) * cj1 * x969)) + x966 +
20763  (((-1.0) * new_r20 * x968)));
20764  evalcond[7] = ((((-1.0) * cj1 * x967)) +
20765  (((-1.0) * new_r21 * x968)) + (((-1.0) * x970)));
20766  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20767  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20768  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20769  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20770  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20771  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20772  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20773  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20774  {
20775  continue;
20776  }
20777  }
20778 
20779  {
20780  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20781  vinfos[0].jointtype = 1;
20782  vinfos[0].foffset = j0;
20783  vinfos[0].indices[0] = _ij0[0];
20784  vinfos[0].indices[1] = _ij0[1];
20785  vinfos[0].maxsolutions = _nj0;
20786  vinfos[1].jointtype = 1;
20787  vinfos[1].foffset = j1;
20788  vinfos[1].indices[0] = _ij1[0];
20789  vinfos[1].indices[1] = _ij1[1];
20790  vinfos[1].maxsolutions = _nj1;
20791  vinfos[2].jointtype = 1;
20792  vinfos[2].foffset = j2;
20793  vinfos[2].indices[0] = _ij2[0];
20794  vinfos[2].indices[1] = _ij2[1];
20795  vinfos[2].maxsolutions = _nj2;
20796  vinfos[3].jointtype = 1;
20797  vinfos[3].foffset = j3;
20798  vinfos[3].indices[0] = _ij3[0];
20799  vinfos[3].indices[1] = _ij3[1];
20800  vinfos[3].maxsolutions = _nj3;
20801  vinfos[4].jointtype = 1;
20802  vinfos[4].foffset = j4;
20803  vinfos[4].indices[0] = _ij4[0];
20804  vinfos[4].indices[1] = _ij4[1];
20805  vinfos[4].maxsolutions = _nj4;
20806  vinfos[5].jointtype = 1;
20807  vinfos[5].foffset = j5;
20808  vinfos[5].indices[0] = _ij5[0];
20809  vinfos[5].indices[1] = _ij5[1];
20810  vinfos[5].maxsolutions = _nj5;
20811  vinfos[6].jointtype = 1;
20812  vinfos[6].foffset = j6;
20813  vinfos[6].indices[0] = _ij6[0];
20814  vinfos[6].indices[1] = _ij6[1];
20815  vinfos[6].maxsolutions = _nj6;
20816  std::vector<int> vfree(0);
20817  solutions.AddSolution(vinfos, vfree);
20818  }
20819  }
20820  }
20821  }
20822  } while (0);
20823  if (bgotonextstatement)
20824  {
20825  bool bgotonextstatement = true;
20826  do
20827  {
20828  evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
20829  if (IKabs(evalcond[0]) < 0.0000050000000000)
20830  {
20831  bgotonextstatement = false;
20832  {
20833  IkReal j2eval[1];
20834  new_r21 = 0;
20835  new_r20 = 0;
20836  new_r02 = 0;
20837  new_r12 = 0;
20838  j2eval[0] = IKabs(new_r22);
20839  if (IKabs(j2eval[0]) < 0.0000000100000000)
20840  {
20841  continue; // no branches [j2]
20842  }
20843  else
20844  {
20845  IkReal op[2 + 1], zeror[2];
20846  int numroots;
20847  op[0] = ((-1.0) * new_r22);
20848  op[1] = 0;
20849  op[2] = new_r22;
20850  polyroots2(op, zeror, numroots);
20851  IkReal j2array[2], cj2array[2], sj2array[2], tempj2array[1];
20852  int numsolutions = 0;
20853  for (int ij2 = 0; ij2 < numroots; ++ij2)
20854  {
20855  IkReal htj2 = zeror[ij2];
20856  tempj2array[0] = ((2.0) * (atan(htj2)));
20857  for (int kj2 = 0; kj2 < 1; ++kj2)
20858  {
20859  j2array[numsolutions] = tempj2array[kj2];
20860  if (j2array[numsolutions] > IKPI)
20861  {
20862  j2array[numsolutions] -= IK2PI;
20863  }
20864  else if (j2array[numsolutions] < -IKPI)
20865  {
20866  j2array[numsolutions] += IK2PI;
20867  }
20868  sj2array[numsolutions] = IKsin(j2array[numsolutions]);
20869  cj2array[numsolutions] = IKcos(j2array[numsolutions]);
20870  numsolutions++;
20871  }
20872  }
20873  bool j2valid[2] = { true, true };
20874  _nj2 = 2;
20875  for (int ij2 = 0; ij2 < numsolutions; ++ij2)
20876  {
20877  if (!j2valid[ij2])
20878  {
20879  continue;
20880  }
20881  j2 = j2array[ij2];
20882  cj2 = cj2array[ij2];
20883  sj2 = sj2array[ij2];
20884  htj2 = IKtan(j2 / 2);
20885 
20886  _ij2[0] = ij2;
20887  _ij2[1] = -1;
20888  for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
20889  {
20890  if (j2valid[iij2] &&
20891  IKabs(cj2array[ij2] - cj2array[iij2]) <
20892  IKFAST_SOLUTION_THRESH &&
20893  IKabs(sj2array[ij2] - sj2array[iij2]) <
20894  IKFAST_SOLUTION_THRESH)
20895  {
20896  j2valid[iij2] = false;
20897  _ij2[1] = iij2;
20898  break;
20899  }
20900  }
20901  {
20902  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20903  vinfos[0].jointtype = 1;
20904  vinfos[0].foffset = j0;
20905  vinfos[0].indices[0] = _ij0[0];
20906  vinfos[0].indices[1] = _ij0[1];
20907  vinfos[0].maxsolutions = _nj0;
20908  vinfos[1].jointtype = 1;
20909  vinfos[1].foffset = j1;
20910  vinfos[1].indices[0] = _ij1[0];
20911  vinfos[1].indices[1] = _ij1[1];
20912  vinfos[1].maxsolutions = _nj1;
20913  vinfos[2].jointtype = 1;
20914  vinfos[2].foffset = j2;
20915  vinfos[2].indices[0] = _ij2[0];
20916  vinfos[2].indices[1] = _ij2[1];
20917  vinfos[2].maxsolutions = _nj2;
20918  vinfos[3].jointtype = 1;
20919  vinfos[3].foffset = j3;
20920  vinfos[3].indices[0] = _ij3[0];
20921  vinfos[3].indices[1] = _ij3[1];
20922  vinfos[3].maxsolutions = _nj3;
20923  vinfos[4].jointtype = 1;
20924  vinfos[4].foffset = j4;
20925  vinfos[4].indices[0] = _ij4[0];
20926  vinfos[4].indices[1] = _ij4[1];
20927  vinfos[4].maxsolutions = _nj4;
20928  vinfos[5].jointtype = 1;
20929  vinfos[5].foffset = j5;
20930  vinfos[5].indices[0] = _ij5[0];
20931  vinfos[5].indices[1] = _ij5[1];
20932  vinfos[5].maxsolutions = _nj5;
20933  vinfos[6].jointtype = 1;
20934  vinfos[6].foffset = j6;
20935  vinfos[6].indices[0] = _ij6[0];
20936  vinfos[6].indices[1] = _ij6[1];
20937  vinfos[6].maxsolutions = _nj6;
20938  std::vector<int> vfree(0);
20939  solutions.AddSolution(vinfos, vfree);
20940  }
20941  }
20942  }
20943  }
20944  }
20945  } while (0);
20946  if (bgotonextstatement)
20947  {
20948  bool bgotonextstatement = true;
20949  do
20950  {
20951  if (1)
20952  {
20953  bgotonextstatement = false;
20954  continue; // branch miss [j2]
20955  }
20956  } while (0);
20957  if (bgotonextstatement)
20958  {
20959  }
20960  }
20961  }
20962  }
20963  }
20964  }
20965  }
20966  }
20967  }
20968  }
20969  }
20970  }
20971  else
20972  {
20973  {
20974  IkReal j2array[1], cj2array[1], sj2array[1];
20975  bool j2valid[1] = { false };
20976  _nj2 = 1;
20977  CheckValue<IkReal> x972 = IKPowWithIntegerCheck(sj1, -1);
20978  if (!x972.valid)
20979  {
20980  continue;
20981  }
20982  IkReal x971 = x972.value;
20983  CheckValue<IkReal> x973 = IKPowWithIntegerCheck(cj1, -1);
20984  if (!x973.valid)
20985  {
20986  continue;
20987  }
20988  CheckValue<IkReal> x974 = IKPowWithIntegerCheck(sj0, -1);
20989  if (!x974.valid)
20990  {
20991  continue;
20992  }
20993  if (IKabs(((-1.0) * new_r21 * x971)) < IKFAST_ATAN2_MAGTHRESH &&
20994  IKabs((x971 * (x973.value) * (x974.value) *
20995  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
20996  IKFAST_ATAN2_MAGTHRESH &&
20997  IKabs(IKsqr(((-1.0) * new_r21 * x971)) +
20998  IKsqr((x971 * (x973.value) * (x974.value) *
20999  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
21000  1) <= IKFAST_SINCOS_THRESH)
21001  continue;
21002  j2array[0] = IKatan2(((-1.0) * new_r21 * x971),
21003  (x971 * (x973.value) * (x974.value) *
21004  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
21005  sj2array[0] = IKsin(j2array[0]);
21006  cj2array[0] = IKcos(j2array[0]);
21007  if (j2array[0] > IKPI)
21008  {
21009  j2array[0] -= IK2PI;
21010  }
21011  else if (j2array[0] < -IKPI)
21012  {
21013  j2array[0] += IK2PI;
21014  }
21015  j2valid[0] = true;
21016  for (int ij2 = 0; ij2 < 1; ++ij2)
21017  {
21018  if (!j2valid[ij2])
21019  {
21020  continue;
21021  }
21022  _ij2[0] = ij2;
21023  _ij2[1] = -1;
21024  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21025  {
21026  if (j2valid[iij2] &&
21027  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21028  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21029  {
21030  j2valid[iij2] = false;
21031  _ij2[1] = iij2;
21032  break;
21033  }
21034  }
21035  j2 = j2array[ij2];
21036  cj2 = cj2array[ij2];
21037  sj2 = sj2array[ij2];
21038  {
21039  IkReal evalcond[12];
21040  IkReal x975 = IKsin(j2);
21041  IkReal x976 = IKcos(j2);
21042  IkReal x977 = ((1.0) * sj0);
21043  IkReal x978 = ((1.0) * sj1);
21044  IkReal x979 = (cj0 * new_r00);
21045  IkReal x980 = (cj1 * sj0);
21046  IkReal x981 = (cj0 * new_r01);
21047  IkReal x982 = (cj1 * x976);
21048  IkReal x983 = (cj1 * x975);
21049  evalcond[0] = (((sj1 * x975)) + new_r21);
21050  evalcond[1] = ((((-1.0) * x976 * x978)) + new_r20);
21051  evalcond[2] = ((((-1.0) * new_r00 * x977)) + ((cj0 * new_r10)) + x975);
21052  evalcond[3] = ((((-1.0) * new_r01 * x977)) + ((cj0 * new_r11)) + x976);
21053  evalcond[4] = (((new_r10 * sj0)) + x982 + x979);
21054  evalcond[5] = (((x976 * x980)) + new_r10 + ((cj0 * x975)));
21055  evalcond[6] = ((((-1.0) * x983)) + ((new_r11 * sj0)) + x981);
21056  evalcond[7] = (new_r00 + ((cj0 * x982)) + (((-1.0) * x975 * x977)));
21057  evalcond[8] = ((((-1.0) * x977 * x983)) + new_r11 + ((cj0 * x976)));
21058  evalcond[9] = ((((-1.0) * x976 * x977)) + new_r01 + (((-1.0) * cj0 * x983)));
21059  evalcond[10] =
21060  ((((-1.0) * new_r20 * x978)) + ((cj1 * x979)) + ((new_r10 * x980)) + x976);
21061  evalcond[11] = (((cj1 * x981)) + (((-1.0) * x975)) + (((-1.0) * new_r21 * x978)) +
21062  ((new_r11 * x980)));
21063  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21064  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21065  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21066  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21067  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21068  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21069  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21070  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21071  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21072  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21073  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21074  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21075  {
21076  continue;
21077  }
21078  }
21079 
21080  {
21081  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21082  vinfos[0].jointtype = 1;
21083  vinfos[0].foffset = j0;
21084  vinfos[0].indices[0] = _ij0[0];
21085  vinfos[0].indices[1] = _ij0[1];
21086  vinfos[0].maxsolutions = _nj0;
21087  vinfos[1].jointtype = 1;
21088  vinfos[1].foffset = j1;
21089  vinfos[1].indices[0] = _ij1[0];
21090  vinfos[1].indices[1] = _ij1[1];
21091  vinfos[1].maxsolutions = _nj1;
21092  vinfos[2].jointtype = 1;
21093  vinfos[2].foffset = j2;
21094  vinfos[2].indices[0] = _ij2[0];
21095  vinfos[2].indices[1] = _ij2[1];
21096  vinfos[2].maxsolutions = _nj2;
21097  vinfos[3].jointtype = 1;
21098  vinfos[3].foffset = j3;
21099  vinfos[3].indices[0] = _ij3[0];
21100  vinfos[3].indices[1] = _ij3[1];
21101  vinfos[3].maxsolutions = _nj3;
21102  vinfos[4].jointtype = 1;
21103  vinfos[4].foffset = j4;
21104  vinfos[4].indices[0] = _ij4[0];
21105  vinfos[4].indices[1] = _ij4[1];
21106  vinfos[4].maxsolutions = _nj4;
21107  vinfos[5].jointtype = 1;
21108  vinfos[5].foffset = j5;
21109  vinfos[5].indices[0] = _ij5[0];
21110  vinfos[5].indices[1] = _ij5[1];
21111  vinfos[5].maxsolutions = _nj5;
21112  vinfos[6].jointtype = 1;
21113  vinfos[6].foffset = j6;
21114  vinfos[6].indices[0] = _ij6[0];
21115  vinfos[6].indices[1] = _ij6[1];
21116  vinfos[6].maxsolutions = _nj6;
21117  std::vector<int> vfree(0);
21118  solutions.AddSolution(vinfos, vfree);
21119  }
21120  }
21121  }
21122  }
21123  }
21124  }
21125  else
21126  {
21127  {
21128  IkReal j2array[1], cj2array[1], sj2array[1];
21129  bool j2valid[1] = { false };
21130  _nj2 = 1;
21131  CheckValue<IkReal> x985 = IKPowWithIntegerCheck(sj1, -1);
21132  if (!x985.valid)
21133  {
21134  continue;
21135  }
21136  IkReal x984 = x985.value;
21137  CheckValue<IkReal> x986 = IKPowWithIntegerCheck(cj0, -1);
21138  if (!x986.valid)
21139  {
21140  continue;
21141  }
21142  if (IKabs(((-1.0) * new_r21 * x984)) < IKFAST_ATAN2_MAGTHRESH &&
21143  IKabs((x984 * (x986.value) *
21144  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
21145  IKFAST_ATAN2_MAGTHRESH &&
21146  IKabs(IKsqr(((-1.0) * new_r21 * x984)) +
21147  IKsqr((x984 * (x986.value) *
21148  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
21149  1) <= IKFAST_SINCOS_THRESH)
21150  continue;
21151  j2array[0] = IKatan2(((-1.0) * new_r21 * x984),
21152  (x984 * (x986.value) *
21153  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
21154  sj2array[0] = IKsin(j2array[0]);
21155  cj2array[0] = IKcos(j2array[0]);
21156  if (j2array[0] > IKPI)
21157  {
21158  j2array[0] -= IK2PI;
21159  }
21160  else if (j2array[0] < -IKPI)
21161  {
21162  j2array[0] += IK2PI;
21163  }
21164  j2valid[0] = true;
21165  for (int ij2 = 0; ij2 < 1; ++ij2)
21166  {
21167  if (!j2valid[ij2])
21168  {
21169  continue;
21170  }
21171  _ij2[0] = ij2;
21172  _ij2[1] = -1;
21173  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21174  {
21175  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21176  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21177  {
21178  j2valid[iij2] = false;
21179  _ij2[1] = iij2;
21180  break;
21181  }
21182  }
21183  j2 = j2array[ij2];
21184  cj2 = cj2array[ij2];
21185  sj2 = sj2array[ij2];
21186  {
21187  IkReal evalcond[12];
21188  IkReal x987 = IKsin(j2);
21189  IkReal x988 = IKcos(j2);
21190  IkReal x989 = ((1.0) * sj0);
21191  IkReal x990 = ((1.0) * sj1);
21192  IkReal x991 = (cj0 * new_r00);
21193  IkReal x992 = (cj1 * sj0);
21194  IkReal x993 = (cj0 * new_r01);
21195  IkReal x994 = (cj1 * x988);
21196  IkReal x995 = (cj1 * x987);
21197  evalcond[0] = (((sj1 * x987)) + new_r21);
21198  evalcond[1] = (new_r20 + (((-1.0) * x988 * x990)));
21199  evalcond[2] = ((((-1.0) * new_r00 * x989)) + ((cj0 * new_r10)) + x987);
21200  evalcond[3] = ((((-1.0) * new_r01 * x989)) + ((cj0 * new_r11)) + x988);
21201  evalcond[4] = (((new_r10 * sj0)) + x991 + x994);
21202  evalcond[5] = (((x988 * x992)) + new_r10 + ((cj0 * x987)));
21203  evalcond[6] = ((((-1.0) * x995)) + ((new_r11 * sj0)) + x993);
21204  evalcond[7] = (((cj0 * x994)) + (((-1.0) * x987 * x989)) + new_r00);
21205  evalcond[8] = (new_r11 + (((-1.0) * x989 * x995)) + ((cj0 * x988)));
21206  evalcond[9] = ((((-1.0) * x988 * x989)) + (((-1.0) * cj0 * x995)) + new_r01);
21207  evalcond[10] =
21208  (((cj1 * x991)) + ((new_r10 * x992)) + (((-1.0) * new_r20 * x990)) + x988);
21209  evalcond[11] = (((cj1 * x993)) + (((-1.0) * x987)) + ((new_r11 * x992)) +
21210  (((-1.0) * new_r21 * x990)));
21211  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21212  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21213  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21214  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21215  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21216  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21217  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21218  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21219  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21220  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21221  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21222  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21223  {
21224  continue;
21225  }
21226  }
21227 
21228  {
21229  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21230  vinfos[0].jointtype = 1;
21231  vinfos[0].foffset = j0;
21232  vinfos[0].indices[0] = _ij0[0];
21233  vinfos[0].indices[1] = _ij0[1];
21234  vinfos[0].maxsolutions = _nj0;
21235  vinfos[1].jointtype = 1;
21236  vinfos[1].foffset = j1;
21237  vinfos[1].indices[0] = _ij1[0];
21238  vinfos[1].indices[1] = _ij1[1];
21239  vinfos[1].maxsolutions = _nj1;
21240  vinfos[2].jointtype = 1;
21241  vinfos[2].foffset = j2;
21242  vinfos[2].indices[0] = _ij2[0];
21243  vinfos[2].indices[1] = _ij2[1];
21244  vinfos[2].maxsolutions = _nj2;
21245  vinfos[3].jointtype = 1;
21246  vinfos[3].foffset = j3;
21247  vinfos[3].indices[0] = _ij3[0];
21248  vinfos[3].indices[1] = _ij3[1];
21249  vinfos[3].maxsolutions = _nj3;
21250  vinfos[4].jointtype = 1;
21251  vinfos[4].foffset = j4;
21252  vinfos[4].indices[0] = _ij4[0];
21253  vinfos[4].indices[1] = _ij4[1];
21254  vinfos[4].maxsolutions = _nj4;
21255  vinfos[5].jointtype = 1;
21256  vinfos[5].foffset = j5;
21257  vinfos[5].indices[0] = _ij5[0];
21258  vinfos[5].indices[1] = _ij5[1];
21259  vinfos[5].maxsolutions = _nj5;
21260  vinfos[6].jointtype = 1;
21261  vinfos[6].foffset = j6;
21262  vinfos[6].indices[0] = _ij6[0];
21263  vinfos[6].indices[1] = _ij6[1];
21264  vinfos[6].maxsolutions = _nj6;
21265  std::vector<int> vfree(0);
21266  solutions.AddSolution(vinfos, vfree);
21267  }
21268  }
21269  }
21270  }
21271  }
21272  }
21273  else
21274  {
21275  {
21276  IkReal j2array[1], cj2array[1], sj2array[1];
21277  bool j2valid[1] = { false };
21278  _nj2 = 1;
21279  CheckValue<IkReal> x996 =
21280  IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
21281  if (!x996.valid)
21282  {
21283  continue;
21284  }
21285  CheckValue<IkReal> x997 = IKPowWithIntegerCheck(IKsign(sj1), -1);
21286  if (!x997.valid)
21287  {
21288  continue;
21289  }
21290  j2array[0] = ((-1.5707963267949) + (x996.value) + (((1.5707963267949) * (x997.value))));
21291  sj2array[0] = IKsin(j2array[0]);
21292  cj2array[0] = IKcos(j2array[0]);
21293  if (j2array[0] > IKPI)
21294  {
21295  j2array[0] -= IK2PI;
21296  }
21297  else if (j2array[0] < -IKPI)
21298  {
21299  j2array[0] += IK2PI;
21300  }
21301  j2valid[0] = true;
21302  for (int ij2 = 0; ij2 < 1; ++ij2)
21303  {
21304  if (!j2valid[ij2])
21305  {
21306  continue;
21307  }
21308  _ij2[0] = ij2;
21309  _ij2[1] = -1;
21310  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21311  {
21312  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21313  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21314  {
21315  j2valid[iij2] = false;
21316  _ij2[1] = iij2;
21317  break;
21318  }
21319  }
21320  j2 = j2array[ij2];
21321  cj2 = cj2array[ij2];
21322  sj2 = sj2array[ij2];
21323  {
21324  IkReal evalcond[12];
21325  IkReal x998 = IKsin(j2);
21326  IkReal x999 = IKcos(j2);
21327  IkReal x1000 = ((1.0) * sj0);
21328  IkReal x1001 = ((1.0) * sj1);
21329  IkReal x1002 = (cj0 * new_r00);
21330  IkReal x1003 = (cj1 * sj0);
21331  IkReal x1004 = (cj0 * new_r01);
21332  IkReal x1005 = (cj1 * x999);
21333  IkReal x1006 = (cj1 * x998);
21334  evalcond[0] = (new_r21 + ((sj1 * x998)));
21335  evalcond[1] = ((((-1.0) * x1001 * x999)) + new_r20);
21336  evalcond[2] = ((((-1.0) * new_r00 * x1000)) + ((cj0 * new_r10)) + x998);
21337  evalcond[3] = ((((-1.0) * new_r01 * x1000)) + ((cj0 * new_r11)) + x999);
21338  evalcond[4] = (x1005 + x1002 + ((new_r10 * sj0)));
21339  evalcond[5] = (((x1003 * x999)) + ((cj0 * x998)) + new_r10);
21340  evalcond[6] = ((((-1.0) * x1006)) + x1004 + ((new_r11 * sj0)));
21341  evalcond[7] = (((cj0 * x1005)) + (((-1.0) * x1000 * x998)) + new_r00);
21342  evalcond[8] = (((cj0 * x999)) + new_r11 + (((-1.0) * x1000 * x1006)));
21343  evalcond[9] = ((((-1.0) * cj0 * x1006)) + (((-1.0) * x1000 * x999)) + new_r01);
21344  evalcond[10] =
21345  (((new_r10 * x1003)) + (((-1.0) * new_r20 * x1001)) + ((cj1 * x1002)) + x999);
21346  evalcond[11] = ((((-1.0) * x998)) + ((new_r11 * x1003)) + (((-1.0) * new_r21 * x1001)) +
21347  ((cj1 * x1004)));
21348  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21349  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21350  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21351  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21352  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21353  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21354  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21355  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21356  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21357  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21358  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21359  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21360  {
21361  continue;
21362  }
21363  }
21364 
21365  {
21366  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21367  vinfos[0].jointtype = 1;
21368  vinfos[0].foffset = j0;
21369  vinfos[0].indices[0] = _ij0[0];
21370  vinfos[0].indices[1] = _ij0[1];
21371  vinfos[0].maxsolutions = _nj0;
21372  vinfos[1].jointtype = 1;
21373  vinfos[1].foffset = j1;
21374  vinfos[1].indices[0] = _ij1[0];
21375  vinfos[1].indices[1] = _ij1[1];
21376  vinfos[1].maxsolutions = _nj1;
21377  vinfos[2].jointtype = 1;
21378  vinfos[2].foffset = j2;
21379  vinfos[2].indices[0] = _ij2[0];
21380  vinfos[2].indices[1] = _ij2[1];
21381  vinfos[2].maxsolutions = _nj2;
21382  vinfos[3].jointtype = 1;
21383  vinfos[3].foffset = j3;
21384  vinfos[3].indices[0] = _ij3[0];
21385  vinfos[3].indices[1] = _ij3[1];
21386  vinfos[3].maxsolutions = _nj3;
21387  vinfos[4].jointtype = 1;
21388  vinfos[4].foffset = j4;
21389  vinfos[4].indices[0] = _ij4[0];
21390  vinfos[4].indices[1] = _ij4[1];
21391  vinfos[4].maxsolutions = _nj4;
21392  vinfos[5].jointtype = 1;
21393  vinfos[5].foffset = j5;
21394  vinfos[5].indices[0] = _ij5[0];
21395  vinfos[5].indices[1] = _ij5[1];
21396  vinfos[5].maxsolutions = _nj5;
21397  vinfos[6].jointtype = 1;
21398  vinfos[6].foffset = j6;
21399  vinfos[6].indices[0] = _ij6[0];
21400  vinfos[6].indices[1] = _ij6[1];
21401  vinfos[6].maxsolutions = _nj6;
21402  std::vector<int> vfree(0);
21403  solutions.AddSolution(vinfos, vfree);
21404  }
21405  }
21406  }
21407  }
21408  }
21409  }
21410  }
21411  }
21412  }
21413  }
21414  }
21415  }
21416  }
21417  static inline void polyroots3(IkReal rawcoeffs[3 + 1], IkReal rawroots[3], int& numroots)
21418  {
21419  using std::complex;
21420  if (rawcoeffs[0] == 0)
21421  {
21422  // solve with one reduced degree
21423  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
21424  return;
21425  }
21426  IKFAST_ASSERT(rawcoeffs[0] != 0);
21427  const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21428  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21429  complex<IkReal> coeffs[3];
21430  const int maxsteps = 110;
21431  for (int i = 0; i < 3; ++i)
21432  {
21433  coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21434  }
21435  complex<IkReal> roots[3];
21436  IkReal err[3];
21437  roots[0] = complex<IkReal>(1, 0);
21438  roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21439  err[0] = 1.0;
21440  err[1] = 1.0;
21441  for (int i = 2; i < 3; ++i)
21442  {
21443  roots[i] = roots[i - 1] * roots[1];
21444  err[i] = 1.0;
21445  }
21446  for (int step = 0; step < maxsteps; ++step)
21447  {
21448  bool changed = false;
21449  for (int i = 0; i < 3; ++i)
21450  {
21451  if (err[i] >= tol)
21452  {
21453  changed = true;
21454  // evaluate
21455  complex<IkReal> x = roots[i] + coeffs[0];
21456  for (int j = 1; j < 3; ++j)
21457  {
21458  x = roots[i] * x + coeffs[j];
21459  }
21460  for (int j = 0; j < 3; ++j)
21461  {
21462  if (i != j)
21463  {
21464  if (roots[i] != roots[j])
21465  {
21466  x /= (roots[i] - roots[j]);
21467  }
21468  }
21469  }
21470  roots[i] -= x;
21471  err[i] = abs(x);
21472  }
21473  }
21474  if (!changed)
21475  {
21476  break;
21477  }
21478  }
21479 
21480  numroots = 0;
21481  bool visited[3] = { false };
21482  for (int i = 0; i < 3; ++i)
21483  {
21484  if (!visited[i])
21485  {
21486  // might be a multiple root, in which case it will have more error than the other roots
21487  // find any neighboring roots, and take the average
21488  complex<IkReal> newroot = roots[i];
21489  int n = 1;
21490  for (int j = i + 1; j < 3; ++j)
21491  {
21492  // care about error in real much more than imaginary
21493  if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21494  {
21495  newroot += roots[j];
21496  n += 1;
21497  visited[j] = true;
21498  }
21499  }
21500  if (n > 1)
21501  {
21502  newroot /= n;
21503  }
21504  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21505  // used, need to use the sqrt
21506  if (IKabs(imag(newroot)) < tolsqrt)
21507  {
21508  rawroots[numroots++] = real(newroot);
21509  }
21510  }
21511  }
21512  }
21513  static inline void polyroots2(IkReal rawcoeffs[2 + 1], IkReal rawroots[2], int& numroots)
21514  {
21515  IkReal det = rawcoeffs[1] * rawcoeffs[1] - 4 * rawcoeffs[0] * rawcoeffs[2];
21516  if (det < 0)
21517  {
21518  numroots = 0;
21519  }
21520  else if (det == 0)
21521  {
21522  rawroots[0] = -0.5 * rawcoeffs[1] / rawcoeffs[0];
21523  numroots = 1;
21524  }
21525  else
21526  {
21527  det = IKsqrt(det);
21528  rawroots[0] = (-rawcoeffs[1] + det) / (2 * rawcoeffs[0]);
21529  rawroots[1] = (-rawcoeffs[1] - det) / (2 * rawcoeffs[0]); // rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
21530  numroots = 2;
21531  }
21532  }
21533  static inline void polyroots4(IkReal rawcoeffs[4 + 1], IkReal rawroots[4], int& numroots)
21534  {
21535  using std::complex;
21536  if (rawcoeffs[0] == 0)
21537  {
21538  // solve with one reduced degree
21539  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
21540  return;
21541  }
21542  IKFAST_ASSERT(rawcoeffs[0] != 0);
21543  const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21544  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21545  complex<IkReal> coeffs[4];
21546  const int maxsteps = 110;
21547  for (int i = 0; i < 4; ++i)
21548  {
21549  coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21550  }
21551  complex<IkReal> roots[4];
21552  IkReal err[4];
21553  roots[0] = complex<IkReal>(1, 0);
21554  roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21555  err[0] = 1.0;
21556  err[1] = 1.0;
21557  for (int i = 2; i < 4; ++i)
21558  {
21559  roots[i] = roots[i - 1] * roots[1];
21560  err[i] = 1.0;
21561  }
21562  for (int step = 0; step < maxsteps; ++step)
21563  {
21564  bool changed = false;
21565  for (int i = 0; i < 4; ++i)
21566  {
21567  if (err[i] >= tol)
21568  {
21569  changed = true;
21570  // evaluate
21571  complex<IkReal> x = roots[i] + coeffs[0];
21572  for (int j = 1; j < 4; ++j)
21573  {
21574  x = roots[i] * x + coeffs[j];
21575  }
21576  for (int j = 0; j < 4; ++j)
21577  {
21578  if (i != j)
21579  {
21580  if (roots[i] != roots[j])
21581  {
21582  x /= (roots[i] - roots[j]);
21583  }
21584  }
21585  }
21586  roots[i] -= x;
21587  err[i] = abs(x);
21588  }
21589  }
21590  if (!changed)
21591  {
21592  break;
21593  }
21594  }
21595 
21596  numroots = 0;
21597  bool visited[4] = { false };
21598  for (int i = 0; i < 4; ++i)
21599  {
21600  if (!visited[i])
21601  {
21602  // might be a multiple root, in which case it will have more error than the other roots
21603  // find any neighboring roots, and take the average
21604  complex<IkReal> newroot = roots[i];
21605  int n = 1;
21606  for (int j = i + 1; j < 4; ++j)
21607  {
21608  // care about error in real much more than imaginary
21609  if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21610  {
21611  newroot += roots[j];
21612  n += 1;
21613  visited[j] = true;
21614  }
21615  }
21616  if (n > 1)
21617  {
21618  newroot /= n;
21619  }
21620  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21621  // used, need to use the sqrt
21622  if (IKabs(imag(newroot)) < tolsqrt)
21623  {
21624  rawroots[numroots++] = real(newroot);
21625  }
21626  }
21627  }
21628  }
21629 };
21630 
21633 IKFAST_API bool
21634 ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
21635 {
21636  IKSolver solver;
21637  return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21638 }
21639 
21640 IKFAST_API bool ComputeIk2(const IkReal* eetrans,
21641  const IkReal* eerot,
21642  const IkReal* pfree,
21643  IkSolutionListBase<IkReal>& solutions,
21644  void* pOpenRAVEManip)
21645 {
21646  IKSolver solver;
21647  return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21648 }
21649 
21650 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - iiwa7 (ad1a4bdb54d4d6b54bc346ee8087a8e6)>"; }
21651 
21652 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
21653 
21654 #ifdef IKFAST_NAMESPACE
21655 } // end namespace
21656 #endif
21657 
21658 #ifndef IKFAST_NO_MAIN
21659 #include <stdio.h>
21660 #include <stdlib.h>
21661 #ifdef IKFAST_NAMESPACE
21662 using namespace IKFAST_NAMESPACE;
21663 #endif
21664 int main(int argc, char** argv)
21665 {
21666  if (argc != 12 + GetNumFreeParameters() + 1)
21667  {
21668  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
21669  "Returns the ik solutions given the transformation of the end effector specified by\n"
21670  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
21671  "There are %d free parameters that have to be specified.\n\n",
21672  GetNumFreeParameters());
21673  return 1;
21674  }
21675 
21676  IkSolutionList<IkReal> solutions;
21677  std::vector<IkReal> vfree(GetNumFreeParameters());
21678  IkReal eerot[9], eetrans[3];
21679  eerot[0] = atof(argv[1]);
21680  eerot[1] = atof(argv[2]);
21681  eerot[2] = atof(argv[3]);
21682  eetrans[0] = atof(argv[4]);
21683  eerot[3] = atof(argv[5]);
21684  eerot[4] = atof(argv[6]);
21685  eerot[5] = atof(argv[7]);
21686  eetrans[1] = atof(argv[8]);
21687  eerot[6] = atof(argv[9]);
21688  eerot[7] = atof(argv[10]);
21689  eerot[8] = atof(argv[11]);
21690  eetrans[2] = atof(argv[12]);
21691  for (std::size_t i = 0; i < vfree.size(); ++i)
21692  vfree[i] = atof(argv[13 + i]);
21693  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
21694 
21695  if (!bSuccess)
21696  {
21697  fprintf(stderr, "Failed to get ik solution\n");
21698  return -1;
21699  }
21700 
21701  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
21702  std::vector<IkReal> solvalues(GetNumJoints());
21703  for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
21704  {
21705  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
21706  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
21707  std::vector<IkReal> vsolfree(sol.GetFree().size());
21708  sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
21709  for (std::size_t j = 0; j < solvalues.size(); ++j)
21710  printf("%.15f, ", solvalues[j]);
21711  printf("\n");
21712  }
21713  return 0;
21714 }
21715 
21716 #endif
21717 TESSERACT_COMMON_IGNORE_WARNINGS_POP
21718 // clang-format on
CheckValue::value
T value
Definition: iiwa7_ikfast_solver.hpp:271
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:258
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: iiwa7_ikfast_solver.hpp:319
IKacos
float IKacos(float f)
Definition: iiwa7_ikfast_solver.hpp:201
IKtan
float IKtan(float f)
Definition: iiwa7_ikfast_solver.hpp:225
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: iiwa7_ikfast_solver.hpp:276
IKabs
float IKabs(float f)
Definition: iiwa7_ikfast_solver.hpp:129
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:21634
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: iiwa7_ikfast_solver.hpp:157
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: iiwa7_ikfast_solver.hpp:239
IK2PI
#define IK2PI
Definition: iiwa7_ikfast_solver.hpp:75
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
dgetrs_
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
ikfast
GetKinematicsHash
const IKFAST_API char * GetKinematicsHash()
Definition: iiwa7_ikfast_solver.hpp:21650
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: iiwa7_ikfast_solver.hpp:21640
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: iiwa7_ikfast_solver.hpp:151
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:102
main
int main(int argc, char **argv)
Definition: ikfast_kinematics_7dof_unit.cpp:88
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
IKSolver
Definition: abb_irb2400_ikfast_solver.hpp:386
IKfmod
float IKfmod(float x, float y)
Definition: iiwa7_ikfast_solver.hpp:182
IKlog
float IKlog(float f)
Definition: iiwa7_ikfast_solver.hpp:135
CheckValue
Definition: iiwa7_ikfast_solver.hpp:269
IKPI
#define IKPI
Definition: iiwa7_ikfast_solver.hpp:76
ikfast.h
ikfast::IkSingleDOFSolutionBase
holds the solution for a single dof
Definition: ikfast.h:51
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKsign
float IKsign(float f)
Definition: iiwa7_ikfast_solver.hpp:292
IKsin
float IKsin(float f)
Definition: iiwa7_ikfast_solver.hpp:221
IKcos
float IKcos(float f)
Definition: iiwa7_ikfast_solver.hpp:223
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
GetIkType
IKFAST_API int GetIkType()
Definition: iiwa7_ikfast_solver.hpp:484
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: iiwa7_ikfast_solver.hpp:480
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: iiwa7_ikfast_solver.hpp:380
IKsqrt
float IKsqrt(float f)
Definition: iiwa7_ikfast_solver.hpp:227
GetIkFastVersion
const IKFAST_API char * GetIkFastVersion()
Definition: iiwa7_ikfast_solver.hpp:21652
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: iiwa7_ikfast_solver.hpp:475
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:71
CheckValue::valid
bool valid
Definition: iiwa7_ikfast_solver.hpp:272
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: iiwa7_ikfast_solver.hpp:474
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: iiwa7_ikfast_solver.hpp:482
IKPI_2
#define IKPI_2
Definition: iiwa7_ikfast_solver.hpp:77
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: iiwa7_ikfast_solver.hpp:32
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: iiwa7_ikfast_solver.hpp:56
IKatan2
float IKatan2(float fy, float fx)
Definition: iiwa7_ikfast_solver.hpp:240
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: iiwa7_ikfast_solver.hpp:140
IKsqr
float IKsqr(float f)
Definition: iiwa7_ikfast_solver.hpp:132
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const=0
macros.h
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: iiwa7_ikfast_solver.hpp:146
IKasin
float IKasin(float f)
Definition: iiwa7_ikfast_solver.hpp:160
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)

tesseract_kinematics
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:14