abb_irb2400_ikfast_solver.hpp
Go to the documentation of this file.
1 // clang-format off
2 
22 #include <tesseract_common/macros.h>
23 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
24 #ifndef IKFAST_HAS_LIBRARY
25 #define IKFAST_NO_MAIN
26 #endif
27 #include <tesseract_kinematics/ikfast/external/ikfast.h> // found inside share/openrave-X.Y/python/ikfast.h
28 using namespace ikfast;
29 
30 // check if the included ikfast version matches what this file was compiled with
31 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)(x)]
32 IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61);
33 
34 #include <cmath>
35 #include <vector>
36 #include <limits>
37 #include <complex>
38 
39 #define IKFAST_STRINGIZE2(s) #s
40 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
41 
42 #ifndef IKFAST_ASSERT
43 #include <stdexcept>
44 #include <iostream>
45 #include <cstddef> // for NULL
46 #include <memory> // for allocator_traits<>::value_type
47 
48 #ifdef _MSC_VER
49 #ifndef __PRETTY_FUNCTION__
50 #define __PRETTY_FUNCTION__ __FUNCDNAME__
51 #endif
52 #endif
53 
54 #ifndef __PRETTY_FUNCTION__
55 #define __PRETTY_FUNCTION__ __func__
56 #endif
57 
58 #define IKFAST_ASSERT(b) \
59  { \
60  if (!(b)) \
61  { \
62  std::stringstream ss; \
63  ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
64  << #b << "' failed"; \
65  throw std::runtime_error(ss.str()); \
66  } \
67  }
68 
69 #endif
70 
71 #if defined(_MSC_VER)
72 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
73 #else
74 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
75 #endif
76 
77 #define IK2PI ((IkReal)6.28318530717959)
78 #define IKPI ((IkReal)3.14159265358979)
79 #define IKPI_2 ((IkReal)1.57079632679490)
80 
81 #ifdef _MSC_VER
82 #ifndef isnan
83 #define isnan _isnan
84 #endif
85 #endif // _MSC_VER
86 
87 // lapack routines
88 extern "C" {
89 void dgetrf_(const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
90 void zgetrf_(const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
91 void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
92 void dgesv_(const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
93 void dgetrs_(const char* trans,
94  const int* n,
95  const int* nrhs,
96  double* a,
97  const int* lda,
98  int* ipiv,
99  double* b,
100  const int* ldb,
101  int* info);
102 void dgeev_(const char* jobvl,
103  const char* jobvr,
104  const int* n,
105  double* a,
106  const int* lda,
107  double* wr,
108  double* wi,
109  double* vl,
110  const int* ldvl,
111  double* vr,
112  const int* ldvr,
113  double* work,
114  const int* lwork,
115  int* info);
116 }
117 
118 using namespace std; // necessary to get std math routines
119 
120 #ifdef IKFAST_NAMESPACE
121 namespace IKFAST_NAMESPACE
122 {
123 #endif
124 
125 inline float IKabs(float f) { return fabsf(f); }
126 inline double IKabs(double f) { return fabs(f); }
127 
128 inline float IKsqr(float f) { return f * f; }
129 inline double IKsqr(double f) { return f * f; }
130 
131 inline float IKlog(float f) { return logf(f); }
132 inline double IKlog(double f) { return log(f); }
133 
134 // allows asin and acos to exceed 1
135 #ifndef IKFAST_SINCOS_THRESH
136 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
137 #endif
138 
139 // used to check input to atan2 for degenerate cases
140 #ifndef IKFAST_ATAN2_MAGTHRESH
141 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
142 #endif
143 
144 // minimum distance of separate solutions
145 #ifndef IKFAST_SOLUTION_THRESH
146 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
147 #endif
148 
149 inline float IKasin(float f)
150 {
151  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
152  // wrong with the solver
153  if (f <= -1)
154  return float(-IKPI_2);
155  else if (f >= 1)
156  return float(IKPI_2);
157  return asinf(f);
158 }
159 inline double IKasin(double f)
160 {
161  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
162  // wrong with the solver
163  if (f <= -1)
164  return -IKPI_2;
165  else if (f >= 1)
166  return IKPI_2;
167  return asin(f);
168 }
169 
170 // return positive value in [0,y)
171 inline float IKfmod(float x, float y)
172 {
173  while (x < 0)
174  {
175  x += y;
176  }
177  return fmodf(x, y);
178 }
179 
180 // return positive value in [0,y)
181 inline double IKfmod(double x, double y)
182 {
183  while (x < 0)
184  {
185  x += y;
186  }
187  return fmod(x, y);
188 }
189 
190 inline float IKacos(float f)
191 {
192  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
193  // wrong with the solver
194  if (f <= -1)
195  return float(IKPI);
196  else if (f >= 1)
197  return float(0);
198  return acosf(f);
199 }
200 inline double IKacos(double f)
201 {
202  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
203  // wrong with the solver
204  if (f <= -1)
205  return IKPI;
206  else if (f >= 1)
207  return 0;
208  return acos(f);
209 }
210 inline float IKsin(float f) { return sinf(f); }
211 inline double IKsin(double f) { return sin(f); }
212 inline float IKcos(float f) { return cosf(f); }
213 inline double IKcos(double f) { return cos(f); }
214 inline float IKtan(float f) { return tanf(f); }
215 inline double IKtan(double f) { return tan(f); }
216 inline float IKsqrt(float f)
217 {
218  if (f <= 0.0f)
219  return 0.0f;
220  return sqrtf(f);
221 }
222 inline double IKsqrt(double f)
223 {
224  if (f <= 0.0)
225  return 0.0;
226  return sqrt(f);
227 }
228 inline float IKatan2(float fy, float fx)
229 {
230  if (isnan(fy))
231  {
232  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
233  return float(IKPI_2);
234  }
235  else if (isnan(fx))
236  {
237  return 0;
238  }
239  return atan2f(fy, fx);
240 }
241 inline double IKatan2(double fy, double fx)
242 {
243  if (isnan(fy))
244  {
245  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
246  return IKPI_2;
247  }
248  else if (isnan(fx))
249  {
250  return 0;
251  }
252  return atan2(fy, fx);
253 }
254 
255 inline float IKsign(float f)
256 {
257  if (f > 0)
258  {
259  return float(1);
260  }
261  else if (f < 0)
262  {
263  return float(-1);
264  }
265  return 0;
266 }
267 
268 inline double IKsign(double f)
269 {
270  if (f > 0)
271  {
272  return 1.0;
273  }
274  else if (f < 0)
275  {
276  return -1.0;
277  }
278  return 0;
279 }
280 
283 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
284 {
285  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,
286  x24, x25, x26, x27, x28, x29, x30, x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45, x46,
287  x47, x48, x49;
288  x0 = IKcos(j[0]);
289  x1 = IKcos(j[3]);
290  x2 = IKsin(j[0]);
291  x3 = IKsin(j[3]);
292  x4 = IKsin(j[1]);
293  x5 = IKsin(j[2]);
294  x6 = IKcos(j[1]);
295  x7 = IKcos(j[2]);
296  x8 = IKsin(j[5]);
297  x9 = IKsin(j[4]);
298  x10 = IKcos(j[4]);
299  x11 = IKcos(j[5]);
300  x12 = ((IkReal(0.0850000000000000)) * (x1));
301  x13 = ((IkReal(0.135000000000000)) * (x7));
302  x14 = ((IkReal(0.0850000000000000)) * (x3));
303  x15 = ((IkReal(1.00000000000000)) * (x7));
304  x16 = ((IkReal(1.00000000000000)) * (x2));
305  x17 = ((IkReal(0.0850000000000000)) * (x5));
306  x18 = ((IkReal(1.00000000000000)) * (x9));
307  x19 = ((IkReal(1.00000000000000)) * (x10));
308  x20 = ((IkReal(1.00000000000000)) * (x0));
309  x21 = ((IkReal(0.755000000000000)) * (x5));
310  x22 = ((IkReal(1.00000000000000)) * (x5));
311  x23 = ((x0) * (x4));
312  x24 = ((x6) * (x7));
313  x25 = ((x5) * (x6));
314  x26 = ((x2) * (x4));
315  x27 = ((x4) * (x5));
316  x28 = ((x1) * (x9));
317  x29 = ((x4) * (x7));
318  x30 = ((x15) * (x6));
319  x31 = ((x20) * (x27));
320  x32 = ((x16) * (x27));
321  x33 = ((x27) + (((IkReal(-1.00000000000000)) * (x30))));
322  x34 = ((x30) + (((IkReal(-1.00000000000000)) * (x22) * (x4))));
323  x35 = ((((x22) * (x6))) + (((x15) * (x4))));
324  x36 = ((x1) * (x33));
325  x37 = ((x3) * (x34));
326  x38 = ((x10) * (x36));
327  x39 = ((((IkReal(-1.00000000000000)) * (x0) * (x30))) + (x31));
328  x40 = ((((IkReal(-1.00000000000000)) * (x2) * (x30))) + (x32));
329  x41 = ((((x15) * (x23))) + (((x20) * (x25))));
330  x42 = ((IkReal(-1.00000000000000)) * (x41));
331  x43 = ((((x15) * (x26))) + (((x16) * (x25))));
332  x44 = ((IkReal(-1.00000000000000)) * (x43));
333  x45 = ((((x1) * (x42))) + (((IkReal(-1.00000000000000)) * (x16) * (x3))));
334  x46 = ((((x3) * (x41))) + (((IkReal(-1.00000000000000)) * (x1) * (x16))));
335  x47 = ((((x3) * (x43))) + (((x0) * (x1))));
336  x48 = ((((x0) * (x3))) + (((x1) * (x44))));
337  x49 = ((x10) * (x45));
338  eerot[0] = ((((x46) * (x8))) + (((x11) * (((x49) + (((x39) * (x9))))))));
339  eerot[1] =
340  ((((x8) *
341  (((((IkReal(-1.00000000000000)) * (x18) * (x39))) + (((IkReal(-1.00000000000000)) * (x19) * (x45))))))) +
342  (((x11) * (x46))));
343  eerot[2] = ((((x45) * (x9))) + (((x10) * (((((IkReal(-1.00000000000000)) * (x31))) + (((x0) * (x24))))))));
344  IkReal x50 = ((x0) * (x24));
345  IkReal x51 = ((IkReal(1.00000000000000)) * (x23));
346  eetrans[0] =
347  ((((IkReal(0.135000000000000)) * (x0) * (x25))) + (((IkReal(0.755000000000000)) * (x50))) +
348  (((IkReal(-1.00000000000000)) * (x21) * (x51))) +
349  (((x10) * (((((IkReal(0.0850000000000000)) * (x50))) + (((IkReal(-1.00000000000000)) * (x17) * (x51))))))) +
350  (((x13) * (x23))) + (((IkReal(0.100000000000000)) * (x0))) +
351  (((x9) * (((((x12) * (x42))) + (((IkReal(-1.00000000000000)) * (x14) * (x2))))))) +
352  (((IkReal(0.705000000000000)) * (x23))));
353  eerot[3] = ((((x47) * (x8))) + (((x11) * (((((x40) * (x9))) + (((x10) * (x48))))))));
354  eerot[4] = ((((x11) * (x47))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x40))) +
355  (((IkReal(-1.00000000000000)) * (x19) * (x48))))))));
356  eerot[5] = ((((x48) * (x9))) + (((x10) * (((((IkReal(-1.00000000000000)) * (x32))) + (((x2) * (x24))))))));
357  IkReal x52 = ((x2) * (x24));
358  IkReal x53 = ((IkReal(1.00000000000000)) * (x26));
359  eetrans[1] =
360  ((((IkReal(0.755000000000000)) * (x52))) + (((x13) * (x26))) +
361  (((x10) * (((((IkReal(0.0850000000000000)) * (x52))) + (((IkReal(-1.00000000000000)) * (x17) * (x53))))))) +
362  (((x9) * (((((x0) * (x14))) + (((x12) * (x44))))))) + (((IkReal(0.100000000000000)) * (x2))) +
363  (((IkReal(-1.00000000000000)) * (x21) * (x53))) + (((IkReal(0.705000000000000)) * (x26))) +
364  (((IkReal(0.135000000000000)) * (x2) * (x25))));
365  eerot[6] = ((((x11) * (((((x35) * (x9))) + (x38))))) + (((x37) * (x8))));
366  eerot[7] = ((((x11) * (x37))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x35))) +
367  (((IkReal(-1.00000000000000)) * (x19) * (x36))))))));
368  eerot[8] = ((((IkReal(-1.00000000000000)) * (x10) * (x35))) + (((x28) * (x33))));
369  IkReal x54 = ((IkReal(1.00000000000000)) * (x6));
370  eetrans[2] =
371  ((IkReal(0.615000000000000)) +
372  (((x10) * (((((IkReal(-0.0850000000000000)) * (x29))) + (((IkReal(-1.00000000000000)) * (x17) * (x54))))))) +
373  (((x13) * (x6))) + (((x28) * (((((IkReal(-0.0850000000000000)) * (x24))) + (((x17) * (x4))))))) +
374  (((IkReal(-1.00000000000000)) * (x21) * (x54))) + (((IkReal(0.705000000000000)) * (x6))) +
375  (((IkReal(-0.755000000000000)) * (x29))) + (((IkReal(-0.135000000000000)) * (x27))));
376 }
377 
378 IKFAST_API int GetNumFreeParameters() { return 0; }
379 IKFAST_API int* GetFreeParameters() { return NULL; }
380 IKFAST_API int GetNumJoints() { return 6; }
381 
382 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
383 
384 IKFAST_API int GetIkType() { return 0x67000001; }
385 
386 class IKSolver
387 {
388 public:
389  IkReal j0, cj0, sj0, htj0, j1, cj1, sj1, htj1, j2, cj2, sj2, htj2, j3, cj3, sj3, htj3, j4, cj4, sj4, htj4, j5, cj5,
390  sj5, htj5, new_r00, r00, rxp0_0, new_r01, r01, rxp0_1, new_r02, r02, rxp0_2, new_r10, r10, rxp1_0, new_r11, r11,
391  rxp1_1, new_r12, r12, rxp1_2, new_r20, r20, rxp2_0, new_r21, r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx,
392  new_py, py, npy, new_pz, pz, npz, pp;
393  unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij4[2], _nj4, _ij5[2], _nj5;
394 
395  bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
396  {
397  j0 = numeric_limits<IkReal>::quiet_NaN();
398  _ij0[0] = -1;
399  _ij0[1] = -1;
400  _nj0 = -1;
401  j1 = numeric_limits<IkReal>::quiet_NaN();
402  _ij1[0] = -1;
403  _ij1[1] = -1;
404  _nj1 = -1;
405  j2 = numeric_limits<IkReal>::quiet_NaN();
406  _ij2[0] = -1;
407  _ij2[1] = -1;
408  _nj2 = -1;
409  j3 = numeric_limits<IkReal>::quiet_NaN();
410  _ij3[0] = -1;
411  _ij3[1] = -1;
412  _nj3 = -1;
413  j4 = numeric_limits<IkReal>::quiet_NaN();
414  _ij4[0] = -1;
415  _ij4[1] = -1;
416  _nj4 = -1;
417  j5 = numeric_limits<IkReal>::quiet_NaN();
418  _ij5[0] = -1;
419  _ij5[1] = -1;
420  _nj5 = -1;
421  for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
422  {
423  solutions.Clear();
424  r00 = eerot[0 * 3 + 0];
425  r01 = eerot[0 * 3 + 1];
426  r02 = eerot[0 * 3 + 2];
427  r10 = eerot[1 * 3 + 0];
428  r11 = eerot[1 * 3 + 1];
429  r12 = eerot[1 * 3 + 2];
430  r20 = eerot[2 * 3 + 0];
431  r21 = eerot[2 * 3 + 1];
432  r22 = eerot[2 * 3 + 2];
433  px = eetrans[0];
434  py = eetrans[1];
435  pz = eetrans[2];
436 
437  new_r00 = r00;
438  new_r01 = r01;
439  new_r02 = r02;
440  new_px = ((((IkReal(-0.0850000000000000)) * (r02))) + (px));
441  new_r10 = r10;
442  new_r11 = r11;
443  new_r12 = r12;
444  new_py = ((((IkReal(-0.0850000000000000)) * (r12))) + (py));
445  new_r20 = r20;
446  new_r21 = r21;
447  new_r22 = r22;
448  new_pz = ((IkReal(-0.615000000000000)) + (((IkReal(-0.0850000000000000)) * (r22))) + (pz));
449  r00 = new_r00;
450  r01 = new_r01;
451  r02 = new_r02;
452  r10 = new_r10;
453  r11 = new_r11;
454  r12 = new_r12;
455  r20 = new_r20;
456  r21 = new_r21;
457  r22 = new_r22;
458  px = new_px;
459  py = new_py;
460  pz = new_pz;
461  pp = (((px) * (px)) + ((pz) * (pz)) + ((py) * (py)));
462  npx = ((((py) * (r10))) + (((pz) * (r20))) + (((px) * (r00))));
463  npy = ((((px) * (r01))) + (((pz) * (r21))) + (((py) * (r11))));
464  npz = ((((py) * (r12))) + (((pz) * (r22))) + (((px) * (r02))));
465  rxp0_0 = ((((IkReal(-1.00000000000000)) * (py) * (r20))) + (((pz) * (r10))));
466  rxp0_1 = ((((px) * (r20))) + (((IkReal(-1.00000000000000)) * (pz) * (r00))));
467  rxp0_2 = ((((py) * (r00))) + (((IkReal(-1.00000000000000)) * (px) * (r10))));
468  rxp1_0 = ((((pz) * (r11))) + (((IkReal(-1.00000000000000)) * (py) * (r21))));
469  rxp1_1 = ((((IkReal(-1.00000000000000)) * (pz) * (r01))) + (((px) * (r21))));
470  rxp1_2 = ((((py) * (r01))) + (((IkReal(-1.00000000000000)) * (px) * (r11))));
471  rxp2_0 = ((((IkReal(-1.00000000000000)) * (py) * (r22))) + (((pz) * (r12))));
472  rxp2_1 = ((((px) * (r22))) + (((IkReal(-1.00000000000000)) * (pz) * (r02))));
473  rxp2_2 = ((((py) * (r02))) + (((IkReal(-1.00000000000000)) * (px) * (r12))));
474  {
475  IkReal j0array[2], cj0array[2], sj0array[2];
476  bool j0valid[2] = { false };
477  _nj0 = 2;
478  if (IKabs(((IkReal(-1.00000000000000)) * (py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH)
479  continue;
480  IkReal x55 = IKatan2(((IkReal(-1.00000000000000)) * (py)), px);
481  j0array[0] = ((IkReal(-1.00000000000000)) * (x55));
482  sj0array[0] = IKsin(j0array[0]);
483  cj0array[0] = IKcos(j0array[0]);
484  j0array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x55))));
485  sj0array[1] = IKsin(j0array[1]);
486  cj0array[1] = IKcos(j0array[1]);
487  if (j0array[0] > IKPI)
488  {
489  j0array[0] -= IK2PI;
490  }
491  else if (j0array[0] < -IKPI)
492  {
493  j0array[0] += IK2PI;
494  }
495  j0valid[0] = true;
496  if (j0array[1] > IKPI)
497  {
498  j0array[1] -= IK2PI;
499  }
500  else if (j0array[1] < -IKPI)
501  {
502  j0array[1] += IK2PI;
503  }
504  j0valid[1] = true;
505  for (int ij0 = 0; ij0 < 2; ++ij0)
506  {
507  if (!j0valid[ij0])
508  {
509  continue;
510  }
511  _ij0[0] = ij0;
512  _ij0[1] = -1;
513  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
514  {
515  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
516  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
517  {
518  j0valid[iij0] = false;
519  _ij0[1] = iij0;
520  break;
521  }
522  }
523  j0 = j0array[ij0];
524  cj0 = cj0array[ij0];
525  sj0 = sj0array[ij0];
526 
527  {
528  IkReal j2array[2], cj2array[2], sj2array[2];
529  bool j2valid[2] = { false };
530  _nj2 = 2;
531  if ((((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
532  (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) <
533  -1 - IKFAST_SINCOS_THRESH ||
534  (((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
535  (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) >
536  1 + IKFAST_SINCOS_THRESH)
537  continue;
538  IkReal x56 =
539  IKasin(((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
540  (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp)))));
541  j2array[0] = ((IkReal(-2.96465459743209)) + (((IkReal(-1.00000000000000)) * (x56))));
542  sj2array[0] = IKsin(j2array[0]);
543  cj2array[0] = IKcos(j2array[0]);
544  j2array[1] = ((IkReal(0.176938056157703)) + (x56));
545  sj2array[1] = IKsin(j2array[1]);
546  cj2array[1] = IKcos(j2array[1]);
547  if (j2array[0] > IKPI)
548  {
549  j2array[0] -= IK2PI;
550  }
551  else if (j2array[0] < -IKPI)
552  {
553  j2array[0] += IK2PI;
554  }
555  j2valid[0] = true;
556  if (j2array[1] > IKPI)
557  {
558  j2array[1] -= IK2PI;
559  }
560  else if (j2array[1] < -IKPI)
561  {
562  j2array[1] += IK2PI;
563  }
564  j2valid[1] = true;
565  for (int ij2 = 0; ij2 < 2; ++ij2)
566  {
567  if (!j2valid[ij2])
568  {
569  continue;
570  }
571  _ij2[0] = ij2;
572  _ij2[1] = -1;
573  for (int iij2 = ij2 + 1; iij2 < 2; ++iij2)
574  {
575  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
576  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
577  {
578  j2valid[iij2] = false;
579  _ij2[1] = iij2;
580  break;
581  }
582  }
583  j2 = j2array[ij2];
584  cj2 = cj2array[ij2];
585  sj2 = sj2array[ij2];
586 
587  {
588  IkReal dummyeval[1];
589  IkReal gconst1;
590  IkReal x57 = ((IkReal(0.755000000000000)) * (cj2));
591  IkReal x58 = ((py) * (sj0));
592  IkReal x59 = ((cj0) * (px));
593  IkReal x60 = ((IkReal(0.135000000000000)) * (sj2));
594  gconst1 = IKsign(
595  ((((IkReal(0.705000000000000)) * (pz))) + (((IkReal(-1.00000000000000)) * (x58) * (x60))) +
596  (((IkReal(0.0755000000000000)) * (cj2))) + (((IkReal(-0.755000000000000)) * (pz) * (sj2))) +
597  (((IkReal(-1.00000000000000)) * (x57) * (x58))) + (((IkReal(-1.00000000000000)) * (x59) * (x60))) +
598  (((IkReal(-1.00000000000000)) * (x57) * (x59))) + (((IkReal(0.0135000000000000)) * (sj2))) +
599  (((IkReal(0.135000000000000)) * (cj2) * (pz)))));
600  IkReal x61 = ((IkReal(10.0000000000000)) * (sj2));
601  IkReal x62 = ((cj0) * (px));
602  IkReal x63 = ((py) * (sj0));
603  IkReal x64 = ((IkReal(55.9259259259259)) * (cj2));
604  dummyeval[0] =
605  ((((IkReal(52.2222222222222)) * (pz))) + (((IkReal(-1.00000000000000)) * (x62) * (x64))) + (sj2) +
606  (((IkReal(10.0000000000000)) * (cj2) * (pz))) + (((IkReal(-1.00000000000000)) * (x63) * (x64))) +
607  (((IkReal(-1.00000000000000)) * (x61) * (x62))) + (((IkReal(-1.00000000000000)) * (x61) * (x63))) +
608  (((IkReal(5.59259259259259)) * (cj2))) + (((IkReal(-55.9259259259259)) * (pz) * (sj2))));
609  if (IKabs(dummyeval[0]) < 0.0000010000000000)
610  {
611  {
612  IkReal dummyeval[1];
613  IkReal gconst0;
614  IkReal x65 = ((IkReal(0.755000000000000)) * (sj2));
615  IkReal x66 = ((cj0) * (px));
616  IkReal x67 = ((py) * (sj0));
617  IkReal x68 = ((IkReal(0.135000000000000)) * (cj2));
618  gconst0 = IKsign(
619  ((IkReal(0.0705000000000000)) + (((x65) * (x66))) + (((IkReal(-0.705000000000000)) * (x67))) +
620  (((x65) * (x67))) + (((IkReal(-0.755000000000000)) * (cj2) * (pz))) +
621  (((IkReal(-0.705000000000000)) * (x66))) + (((IkReal(-1.00000000000000)) * (x66) * (x68))) +
622  (((IkReal(-1.00000000000000)) * (x67) * (x68))) + (((IkReal(0.0135000000000000)) * (cj2))) +
623  (((IkReal(-0.135000000000000)) * (pz) * (sj2))) + (((IkReal(-0.0755000000000000)) * (sj2)))));
624  IkReal x69 = ((cj0) * (px));
625  IkReal x70 = ((IkReal(10.0000000000000)) * (cj2));
626  IkReal x71 = ((IkReal(55.9259259259259)) * (sj2));
627  IkReal x72 = ((py) * (sj0));
628  dummyeval[0] =
629  ((IkReal(5.22222222222222)) + (((IkReal(-1.00000000000000)) * (x70) * (x72))) +
630  (((x71) * (x72))) + (((IkReal(-52.2222222222222)) * (x72))) +
631  (((IkReal(-10.0000000000000)) * (pz) * (sj2))) + (cj2) +
632  (((IkReal(-52.2222222222222)) * (x69))) + (((IkReal(-1.00000000000000)) * (x69) * (x70))) +
633  (((IkReal(-5.59259259259259)) * (sj2))) + (((IkReal(-55.9259259259259)) * (cj2) * (pz))) +
634  (((x69) * (x71))));
635  if (IKabs(dummyeval[0]) < 0.0000010000000000)
636  {
637  continue;
638  }
639  else
640  {
641  {
642  IkReal j1array[1], cj1array[1], sj1array[1];
643  bool j1valid[1] = { false };
644  _nj1 = 1;
645  IkReal x73 = (sj2) * (sj2);
646  IkReal x74 = (cj2) * (cj2);
647  IkReal x75 = ((cj2) * (sj2));
648  IkReal x76 = ((IkReal(1.00000000000000)) * (pz));
649  if (IKabs(
650  ((gconst0) *
651  (((IkReal(-0.497025000000000)) + (((IkReal(-0.190350000000000)) * (cj2))) +
652  ((pz) * (pz)) + (((IkReal(1.06455000000000)) * (sj2))) +
653  (((IkReal(0.203850000000000)) * (x75))) + (((IkReal(-0.570025000000000)) * (x73))) +
654  (((IkReal(-0.0182250000000000)) * (x74))))))) < IKFAST_ATAN2_MAGTHRESH &&
655  IKabs((
656  (gconst0) *
657  (((((IkReal(0.101925000000000)) * (x73))) + (((IkReal(-0.101925000000000)) * (x74))) +
658  (((IkReal(0.100000000000000)) * (pz))) + (((IkReal(0.551800000000000)) * (x75))) +
659  (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) +
660  (((IkReal(-0.532275000000000)) * (cj2))) + (((IkReal(-0.0951750000000000)) * (sj2))) +
661  (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76))))))) < IKFAST_ATAN2_MAGTHRESH)
662  continue;
663  j1array[0] = IKatan2(
664  ((gconst0) *
665  (((IkReal(-0.497025000000000)) + (((IkReal(-0.190350000000000)) * (cj2))) + ((pz) * (pz)) +
666  (((IkReal(1.06455000000000)) * (sj2))) + (((IkReal(0.203850000000000)) * (x75))) +
667  (((IkReal(-0.570025000000000)) * (x73))) + (((IkReal(-0.0182250000000000)) * (x74)))))),
668  ((gconst0) *
669  (((((IkReal(0.101925000000000)) * (x73))) + (((IkReal(-0.101925000000000)) * (x74))) +
670  (((IkReal(0.100000000000000)) * (pz))) + (((IkReal(0.551800000000000)) * (x75))) +
671  (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) +
672  (((IkReal(-0.532275000000000)) * (cj2))) + (((IkReal(-0.0951750000000000)) * (sj2))) +
673  (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76)))))));
674  sj1array[0] = IKsin(j1array[0]);
675  cj1array[0] = IKcos(j1array[0]);
676  if (j1array[0] > IKPI)
677  {
678  j1array[0] -= IK2PI;
679  }
680  else if (j1array[0] < -IKPI)
681  {
682  j1array[0] += IK2PI;
683  }
684  j1valid[0] = true;
685  for (int ij1 = 0; ij1 < 1; ++ij1)
686  {
687  if (!j1valid[ij1])
688  {
689  continue;
690  }
691  _ij1[0] = ij1;
692  _ij1[1] = -1;
693  for (int iij1 = ij1 + 1; iij1 < 1; ++iij1)
694  {
695  if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
696  IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
697  {
698  j1valid[iij1] = false;
699  _ij1[1] = iij1;
700  break;
701  }
702  }
703  j1 = j1array[ij1];
704  cj1 = cj1array[ij1];
705  sj1 = sj1array[ij1];
706  {
707  IkReal evalcond[5];
708  IkReal x77 = IKsin(j1);
709  IkReal x78 = IKcos(j1);
710  IkReal x79 = ((IkReal(0.135000000000000)) * (sj2));
711  IkReal x80 = ((cj0) * (px));
712  IkReal x81 = ((IkReal(0.755000000000000)) * (sj2));
713  IkReal x82 = ((py) * (sj0));
714  IkReal x83 = ((IkReal(0.755000000000000)) * (cj2));
715  IkReal x84 = ((IkReal(0.135000000000000)) * (cj2));
716  IkReal x85 = ((IkReal(0.135000000000000)) * (x78));
717  IkReal x86 = ((IkReal(1.00000000000000)) * (x78));
718  IkReal x87 = ((IkReal(1.41000000000000)) * (x77));
719  IkReal x88 = ((pz) * (x78));
720  evalcond[0] = ((IkReal(-0.705000000000000)) + (((x77) * (x80))) + (((x77) * (x82))) +
721  (((IkReal(-0.100000000000000)) * (x77))) +
722  (((IkReal(-1.00000000000000)) * (x84))) + (x88) + (x81));
723  evalcond[1] = ((((IkReal(0.100000000000000)) * (x78))) +
724  (((IkReal(-1.00000000000000)) * (x80) * (x86))) + (x79) +
725  (((IkReal(-1.00000000000000)) * (x82) * (x86))) + (x83) + (((pz) * (x77))));
726  evalcond[2] = ((((IkReal(-0.705000000000000)) * (x78))) +
727  (((IkReal(-1.00000000000000)) * (x78) * (x84))) + (((x77) * (x79))) + (pz) +
728  (((x78) * (x81))) + (((x77) * (x83))));
729  evalcond[3] =
730  ((IkReal(0.0812250000000000)) + (((IkReal(-0.141000000000000)) * (x77))) +
731  (((IkReal(-1.00000000000000)) * (pp))) + (((x82) * (x87))) +
732  (((IkReal(0.200000000000000)) * (x82))) + (((IkReal(1.41000000000000)) * (x88))) +
733  (((x80) * (x87))) + (((IkReal(0.200000000000000)) * (x80))));
734  evalcond[4] =
735  ((IkReal(0.100000000000000)) + (((IkReal(-1.00000000000000)) * (x82))) +
736  (((x77) * (x84))) + (((IkReal(-1.00000000000000)) * (x77) * (x81))) +
737  (((x78) * (x83))) + (((x78) * (x79))) + (((IkReal(0.705000000000000)) * (x77))) +
738  (((IkReal(-1.00000000000000)) * (x80))));
739  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
740  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
741  IKabs(evalcond[4]) > 0.000001)
742  {
743  continue;
744  }
745  }
746 
747  rotationfunction0(solutions);
748  }
749  }
750  }
751  }
752  }
753  else
754  {
755  {
756  IkReal j1array[1], cj1array[1], sj1array[1];
757  bool j1valid[1] = { false };
758  _nj1 = 1;
759  IkReal x228 = (sj2) * (sj2);
760  IkReal x229 = (cj2) * (cj2);
761  IkReal x230 = ((cj2) * (sj2));
762  if (IKabs(((gconst1) *
763  (((((IkReal(0.551800000000000)) * (x230))) + (((IkReal(0.101925000000000)) * (x228))) +
764  (((IkReal(-0.100000000000000)) * (pz))) + (((IkReal(-0.101925000000000)) * (x229))) +
765  (((cj0) * (px) * (pz))) + (((IkReal(-0.532275000000000)) * (cj2))) +
766  (((IkReal(-0.0951750000000000)) * (sj2))) + (((py) * (pz) * (sj0))))))) <
767  IKFAST_ATAN2_MAGTHRESH &&
768  IKabs(((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) +
769  (((IkReal(-0.203850000000000)) * (x230))) +
770  (((IkReal(-0.570025000000000)) * (x229))))))) < IKFAST_ATAN2_MAGTHRESH)
771  continue;
772  j1array[0] =
773  IKatan2(((gconst1) *
774  (((((IkReal(0.551800000000000)) * (x230))) + (((IkReal(0.101925000000000)) * (x228))) +
775  (((IkReal(-0.100000000000000)) * (pz))) + (((IkReal(-0.101925000000000)) * (x229))) +
776  (((cj0) * (px) * (pz))) + (((IkReal(-0.532275000000000)) * (cj2))) +
777  (((IkReal(-0.0951750000000000)) * (sj2))) + (((py) * (pz) * (sj0)))))),
778  ((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) +
779  (((IkReal(-0.203850000000000)) * (x230))) +
780  (((IkReal(-0.570025000000000)) * (x229)))))));
781  sj1array[0] = IKsin(j1array[0]);
782  cj1array[0] = IKcos(j1array[0]);
783  if (j1array[0] > IKPI)
784  {
785  j1array[0] -= IK2PI;
786  }
787  else if (j1array[0] < -IKPI)
788  {
789  j1array[0] += IK2PI;
790  }
791  j1valid[0] = true;
792  for (int ij1 = 0; ij1 < 1; ++ij1)
793  {
794  if (!j1valid[ij1])
795  {
796  continue;
797  }
798  _ij1[0] = ij1;
799  _ij1[1] = -1;
800  for (int iij1 = ij1 + 1; iij1 < 1; ++iij1)
801  {
802  if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
803  IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
804  {
805  j1valid[iij1] = false;
806  _ij1[1] = iij1;
807  break;
808  }
809  }
810  j1 = j1array[ij1];
811  cj1 = cj1array[ij1];
812  sj1 = sj1array[ij1];
813  {
814  IkReal evalcond[5];
815  IkReal x231 = IKsin(j1);
816  IkReal x232 = IKcos(j1);
817  IkReal x233 = ((IkReal(0.135000000000000)) * (sj2));
818  IkReal x234 = ((cj0) * (px));
819  IkReal x235 = ((IkReal(0.755000000000000)) * (sj2));
820  IkReal x236 = ((py) * (sj0));
821  IkReal x237 = ((IkReal(0.755000000000000)) * (cj2));
822  IkReal x238 = ((IkReal(0.135000000000000)) * (cj2));
823  IkReal x239 = ((IkReal(0.135000000000000)) * (x232));
824  IkReal x240 = ((IkReal(1.00000000000000)) * (x232));
825  IkReal x241 = ((IkReal(1.41000000000000)) * (x231));
826  IkReal x242 = ((pz) * (x232));
827  evalcond[0] = ((IkReal(-0.705000000000000)) + (((IkReal(-1.00000000000000)) * (x238))) +
828  (x235) + (((x231) * (x236))) + (x242) +
829  (((IkReal(-0.100000000000000)) * (x231))) + (((x231) * (x234))));
830  evalcond[1] = ((((IkReal(0.100000000000000)) * (x232))) + (((pz) * (x231))) + (x233) + (x237) +
831  (((IkReal(-1.00000000000000)) * (x234) * (x240))) +
832  (((IkReal(-1.00000000000000)) * (x236) * (x240))));
833  evalcond[2] =
834  ((((IkReal(-0.705000000000000)) * (x232))) + (((x231) * (x237))) + (((x231) * (x233))) +
835  (pz) + (((x232) * (x235))) + (((IkReal(-1.00000000000000)) * (x232) * (x238))));
836  evalcond[3] =
837  ((IkReal(0.0812250000000000)) + (((x236) * (x241))) +
838  (((IkReal(0.200000000000000)) * (x236))) + (((IkReal(0.200000000000000)) * (x234))) +
839  (((IkReal(-1.00000000000000)) * (pp))) + (((x234) * (x241))) +
840  (((IkReal(1.41000000000000)) * (x242))) + (((IkReal(-0.141000000000000)) * (x231))));
841  evalcond[4] =
842  ((IkReal(0.100000000000000)) + (((IkReal(-1.00000000000000)) * (x231) * (x235))) +
843  (((IkReal(0.705000000000000)) * (x231))) + (((IkReal(-1.00000000000000)) * (x236))) +
844  (((x231) * (x238))) + (((x232) * (x233))) + (((x232) * (x237))) +
845  (((IkReal(-1.00000000000000)) * (x234))));
846  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
847  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
848  IKabs(evalcond[4]) > 0.000001)
849  {
850  continue;
851  }
852  }
853 
854  rotationfunction0(solutions);
855  }
856  }
857  }
858  }
859  }
860  }
861  }
862  }
863  }
864  return solutions.GetNumSolutions() > 0;
865  }
866  inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
867  {
868  for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
869  {
870  IkReal x89 = ((cj0) * (r00));
871  IkReal x90 = ((cj0) * (r01));
872  IkReal x91 = ((sj1) * (sj2));
873  IkReal x92 = ((IkReal(1.00000000000000)) * (sj0));
874  IkReal x93 = ((r10) * (sj0));
875  IkReal x94 = ((IkReal(1.00000000000000)) * (cj2));
876  IkReal x95 = ((r12) * (sj0));
877  IkReal x96 = ((cj0) * (r02));
878  IkReal x97 = ((r11) * (sj0));
879  IkReal x98 = ((((IkReal(-1.00000000000000)) * (cj1) * (x94))) + (x91));
880  IkReal x99 = ((((IkReal(-1.00000000000000)) * (x91))) + (((cj1) * (cj2))));
881  IkReal x100 = ((cj0) * (x99));
882  IkReal x101 = ((((IkReal(-1.00000000000000)) * (sj1) * (x94))) + (((IkReal(-1.00000000000000)) * (cj1) * (sj2))));
883  IkReal x102 = ((sj0) * (x101));
884  new_r00 = ((((r20) * (x98))) + (((x101) * (x89))) + (((x101) * (x93))));
885  new_r01 = ((((x101) * (x90))) + (((x101) * (x97))) + (((r21) * (x98))));
886  new_r02 = ((((x101) * (x95))) + (((r22) * (x98))) + (((x101) * (x96))));
887  new_r10 = ((((IkReal(-1.00000000000000)) * (r00) * (x92))) + (((cj0) * (r10))));
888  new_r11 = ((((IkReal(-1.00000000000000)) * (r01) * (x92))) + (((cj0) * (r11))));
889  new_r12 = ((((IkReal(-1.00000000000000)) * (r02) * (x92))) + (((cj0) * (r12))));
890  new_r20 = ((((x93) * (x99))) + (((x89) * (x99))) + (((r20) * (x101))));
891  new_r21 = ((((r21) * (x101))) + (((x97) * (x99))) + (((x90) * (x99))));
892  new_r22 = ((((x96) * (x99))) + (((r22) * (x101))) + (((x95) * (x99))));
893  {
894  IkReal j4array[2], cj4array[2], sj4array[2];
895  bool j4valid[2] = { false };
896  _nj4 = 2;
897  cj4array[0] = new_r22;
898  if (cj4array[0] >= -1 - IKFAST_SINCOS_THRESH && cj4array[0] <= 1 + IKFAST_SINCOS_THRESH)
899  {
900  j4valid[0] = j4valid[1] = true;
901  j4array[0] = IKacos(cj4array[0]);
902  sj4array[0] = IKsin(j4array[0]);
903  cj4array[1] = cj4array[0];
904  j4array[1] = -j4array[0];
905  sj4array[1] = -sj4array[0];
906  }
907  else if (isnan(cj4array[0]))
908  {
909  // probably any value will work
910  j4valid[0] = true;
911  cj4array[0] = 1;
912  sj4array[0] = 0;
913  j4array[0] = 0;
914  }
915  for (int ij4 = 0; ij4 < 2; ++ij4)
916  {
917  if (!j4valid[ij4])
918  {
919  continue;
920  }
921  _ij4[0] = ij4;
922  _ij4[1] = -1;
923  for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
924  {
925  if (j4valid[iij4] && IKabs(cj4array[ij4] - cj4array[iij4]) < IKFAST_SOLUTION_THRESH &&
926  IKabs(sj4array[ij4] - sj4array[iij4]) < IKFAST_SOLUTION_THRESH)
927  {
928  j4valid[iij4] = false;
929  _ij4[1] = iij4;
930  break;
931  }
932  }
933  j4 = j4array[ij4];
934  cj4 = cj4array[ij4];
935  sj4 = sj4array[ij4];
936 
937  {
938  IkReal dummyeval[1];
939  IkReal gconst4;
940  gconst4 = IKsign(sj4);
941  dummyeval[0] = sj4;
942  if (IKabs(dummyeval[0]) < 0.0000010000000000)
943  {
944  {
945  IkReal dummyeval[1];
946  IkReal gconst2;
947  gconst2 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02))));
948  dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02)));
949  if (IKabs(dummyeval[0]) < 0.0000010000000000)
950  {
951  {
952  IkReal dummyeval[1];
953  IkReal gconst3;
954  gconst3 = IKsign(((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4)))));
955  dummyeval[0] = ((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4))));
956  if (IKabs(dummyeval[0]) < 0.0000010000000000)
957  {
958  {
959  IkReal evalcond[7];
960  IkReal x103 = ((IkReal(-1.00000000000000)) + (new_r22));
961  evalcond[0] = ((IkReal(-3.14159265358979)) +
962  (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
963  evalcond[1] = x103;
964  evalcond[2] = new_r20;
965  evalcond[3] = new_r21;
966  evalcond[4] = new_r20;
967  evalcond[5] = new_r21;
968  evalcond[6] = x103;
969  if (IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 &&
970  IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 &&
971  IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 &&
972  IKabs(evalcond[6]) < 0.0000010000000000)
973  {
974  {
975  IkReal j3array[2], cj3array[2], sj3array[2];
976  bool j3valid[2] = { false };
977  _nj3 = 2;
978  if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH)
979  continue;
980  IkReal x104 = IKatan2(new_r02, new_r12);
981  j3array[0] = ((IkReal(-1.00000000000000)) * (x104));
982  sj3array[0] = IKsin(j3array[0]);
983  cj3array[0] = IKcos(j3array[0]);
984  j3array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x104))));
985  sj3array[1] = IKsin(j3array[1]);
986  cj3array[1] = IKcos(j3array[1]);
987  if (j3array[0] > IKPI)
988  {
989  j3array[0] -= IK2PI;
990  }
991  else if (j3array[0] < -IKPI)
992  {
993  j3array[0] += IK2PI;
994  }
995  j3valid[0] = true;
996  if (j3array[1] > IKPI)
997  {
998  j3array[1] -= IK2PI;
999  }
1000  else if (j3array[1] < -IKPI)
1001  {
1002  j3array[1] += IK2PI;
1003  }
1004  j3valid[1] = true;
1005  for (int ij3 = 0; ij3 < 2; ++ij3)
1006  {
1007  if (!j3valid[ij3])
1008  {
1009  continue;
1010  }
1011  _ij3[0] = ij3;
1012  _ij3[1] = -1;
1013  for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1014  {
1015  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1016  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1017  {
1018  j3valid[iij3] = false;
1019  _ij3[1] = iij3;
1020  break;
1021  }
1022  }
1023  j3 = j3array[ij3];
1024  cj3 = cj3array[ij3];
1025  sj3 = sj3array[ij3];
1026  {
1027  IkReal evalcond[1];
1028  evalcond[0] = ((((new_r12) * (IKcos(j3)))) +
1029  (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3)))));
1030  if (IKabs(evalcond[0]) > 0.000001)
1031  {
1032  continue;
1033  }
1034  }
1035 
1036  {
1037  IkReal j5array[1], cj5array[1], sj5array[1];
1038  bool j5valid[1] = { false };
1039  _nj5 = 1;
1040  if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1041  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1042  IKFAST_ATAN2_MAGTHRESH &&
1043  IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) < IKFAST_ATAN2_MAGTHRESH &&
1044  IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1045  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
1046  IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
1047  IKFAST_SINCOS_THRESH)
1048  continue;
1049  j5array[0] = IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1050  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
1051  ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
1052  sj5array[0] = IKsin(j5array[0]);
1053  cj5array[0] = IKcos(j5array[0]);
1054  if (j5array[0] > IKPI)
1055  {
1056  j5array[0] -= IK2PI;
1057  }
1058  else if (j5array[0] < -IKPI)
1059  {
1060  j5array[0] += IK2PI;
1061  }
1062  j5valid[0] = true;
1063  for (int ij5 = 0; ij5 < 1; ++ij5)
1064  {
1065  if (!j5valid[ij5])
1066  {
1067  continue;
1068  }
1069  _ij5[0] = ij5;
1070  _ij5[1] = -1;
1071  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1072  {
1073  if (j5valid[iij5] &&
1074  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1075  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1076  {
1077  j5valid[iij5] = false;
1078  _ij5[1] = iij5;
1079  break;
1080  }
1081  }
1082  j5 = j5array[ij5];
1083  cj5 = cj5array[ij5];
1084  sj5 = sj5array[ij5];
1085  {
1086  IkReal evalcond[4];
1087  IkReal x105 = IKsin(j5);
1088  IkReal x106 = ((IkReal(1.00000000000000)) * (sj3));
1089  IkReal x107 = ((IkReal(1.00000000000000)) * (IKcos(j5)));
1090  evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x106))) +
1091  (((cj3) * (new_r10))) + (((IkReal(-1.00000000000000)) * (x105))));
1092  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x107))) + (((cj3) * (new_r11))) +
1093  (((IkReal(-1.00000000000000)) * (new_r01) * (x106))));
1094  evalcond[2] = ((((new_r11) * (sj3))) + (x105) + (((cj3) * (new_r01))));
1095  evalcond[3] = ((((IkReal(-1.00000000000000)) * (x107))) + (((new_r10) * (sj3))) +
1096  (((cj3) * (new_r00))));
1097  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1098  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
1099  {
1100  continue;
1101  }
1102  }
1103 
1104  {
1105  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1106  vinfos[0].jointtype = 1;
1107  vinfos[0].foffset = j0;
1108  vinfos[0].indices[0] = _ij0[0];
1109  vinfos[0].indices[1] = _ij0[1];
1110  vinfos[0].maxsolutions = _nj0;
1111  vinfos[1].jointtype = 1;
1112  vinfos[1].foffset = j1;
1113  vinfos[1].indices[0] = _ij1[0];
1114  vinfos[1].indices[1] = _ij1[1];
1115  vinfos[1].maxsolutions = _nj1;
1116  vinfos[2].jointtype = 1;
1117  vinfos[2].foffset = j2;
1118  vinfos[2].indices[0] = _ij2[0];
1119  vinfos[2].indices[1] = _ij2[1];
1120  vinfos[2].maxsolutions = _nj2;
1121  vinfos[3].jointtype = 1;
1122  vinfos[3].foffset = j3;
1123  vinfos[3].indices[0] = _ij3[0];
1124  vinfos[3].indices[1] = _ij3[1];
1125  vinfos[3].maxsolutions = _nj3;
1126  vinfos[4].jointtype = 1;
1127  vinfos[4].foffset = j4;
1128  vinfos[4].indices[0] = _ij4[0];
1129  vinfos[4].indices[1] = _ij4[1];
1130  vinfos[4].maxsolutions = _nj4;
1131  vinfos[5].jointtype = 1;
1132  vinfos[5].foffset = j5;
1133  vinfos[5].indices[0] = _ij5[0];
1134  vinfos[5].indices[1] = _ij5[1];
1135  vinfos[5].maxsolutions = _nj5;
1136  std::vector<int> vfree(0);
1137  solutions.AddSolution(vinfos, vfree);
1138  }
1139  }
1140  }
1141  }
1142  }
1143  }
1144  else
1145  {
1146  evalcond[0] = ((IkReal(-3.14159265358979)) + (IKfmod(j4, IkReal(6.28318530717959))));
1147  evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
1148  evalcond[2] = new_r20;
1149  evalcond[3] = new_r21;
1150  evalcond[4] = ((IkReal(-1.00000000000000)) * (new_r20));
1151  evalcond[5] = ((IkReal(-1.00000000000000)) * (new_r21));
1152  evalcond[6] = ((IkReal(-1.00000000000000)) + (((IkReal(-1.00000000000000)) * (new_r22))));
1153  if (IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 &&
1154  IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 &&
1155  IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 &&
1156  IKabs(evalcond[6]) < 0.0000010000000000)
1157  {
1158  {
1159  IkReal j3array[2], cj3array[2], sj3array[2];
1160  bool j3valid[2] = { false };
1161  _nj3 = 2;
1162  if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH)
1163  continue;
1164  IkReal x108 = IKatan2(new_r02, new_r12);
1165  j3array[0] = ((IkReal(-1.00000000000000)) * (x108));
1166  sj3array[0] = IKsin(j3array[0]);
1167  cj3array[0] = IKcos(j3array[0]);
1168  j3array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x108))));
1169  sj3array[1] = IKsin(j3array[1]);
1170  cj3array[1] = IKcos(j3array[1]);
1171  if (j3array[0] > IKPI)
1172  {
1173  j3array[0] -= IK2PI;
1174  }
1175  else if (j3array[0] < -IKPI)
1176  {
1177  j3array[0] += IK2PI;
1178  }
1179  j3valid[0] = true;
1180  if (j3array[1] > IKPI)
1181  {
1182  j3array[1] -= IK2PI;
1183  }
1184  else if (j3array[1] < -IKPI)
1185  {
1186  j3array[1] += IK2PI;
1187  }
1188  j3valid[1] = true;
1189  for (int ij3 = 0; ij3 < 2; ++ij3)
1190  {
1191  if (!j3valid[ij3])
1192  {
1193  continue;
1194  }
1195  _ij3[0] = ij3;
1196  _ij3[1] = -1;
1197  for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1198  {
1199  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1200  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1201  {
1202  j3valid[iij3] = false;
1203  _ij3[1] = iij3;
1204  break;
1205  }
1206  }
1207  j3 = j3array[ij3];
1208  cj3 = cj3array[ij3];
1209  sj3 = sj3array[ij3];
1210  {
1211  IkReal evalcond[1];
1212  evalcond[0] = ((((new_r12) * (IKcos(j3)))) +
1213  (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3)))));
1214  if (IKabs(evalcond[0]) > 0.000001)
1215  {
1216  continue;
1217  }
1218  }
1219 
1220  {
1221  IkReal j5array[1], cj5array[1], sj5array[1];
1222  bool j5valid[1] = { false };
1223  _nj5 = 1;
1224  if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) < IKFAST_ATAN2_MAGTHRESH &&
1225  IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1226  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1227  IKFAST_ATAN2_MAGTHRESH &&
1228  IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1229  IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1230  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1231  1) <= IKFAST_SINCOS_THRESH)
1232  continue;
1233  j5array[0] = IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
1234  ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1235  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
1236  sj5array[0] = IKsin(j5array[0]);
1237  cj5array[0] = IKcos(j5array[0]);
1238  if (j5array[0] > IKPI)
1239  {
1240  j5array[0] -= IK2PI;
1241  }
1242  else if (j5array[0] < -IKPI)
1243  {
1244  j5array[0] += IK2PI;
1245  }
1246  j5valid[0] = true;
1247  for (int ij5 = 0; ij5 < 1; ++ij5)
1248  {
1249  if (!j5valid[ij5])
1250  {
1251  continue;
1252  }
1253  _ij5[0] = ij5;
1254  _ij5[1] = -1;
1255  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1256  {
1257  if (j5valid[iij5] &&
1258  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1259  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1260  {
1261  j5valid[iij5] = false;
1262  _ij5[1] = iij5;
1263  break;
1264  }
1265  }
1266  j5 = j5array[ij5];
1267  cj5 = cj5array[ij5];
1268  sj5 = sj5array[ij5];
1269  {
1270  IkReal evalcond[4];
1271  IkReal x109 = IKcos(j5);
1272  IkReal x110 = ((IkReal(1.00000000000000)) * (sj3));
1273  IkReal x111 = ((IkReal(1.00000000000000)) * (IKsin(j5)));
1274  evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x110))) +
1275  (((IkReal(-1.00000000000000)) * (x111))) + (((cj3) * (new_r10))));
1276  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x109))) +
1277  (((IkReal(-1.00000000000000)) * (new_r01) * (x110))) +
1278  (((cj3) * (new_r11))));
1279  evalcond[2] = ((((new_r11) * (sj3))) + (((IkReal(-1.00000000000000)) * (x111))) +
1280  (((cj3) * (new_r01))));
1281  evalcond[3] = ((((new_r10) * (sj3))) + (x109) + (((cj3) * (new_r00))));
1282  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1283  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
1284  {
1285  continue;
1286  }
1287  }
1288 
1289  {
1290  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1291  vinfos[0].jointtype = 1;
1292  vinfos[0].foffset = j0;
1293  vinfos[0].indices[0] = _ij0[0];
1294  vinfos[0].indices[1] = _ij0[1];
1295  vinfos[0].maxsolutions = _nj0;
1296  vinfos[1].jointtype = 1;
1297  vinfos[1].foffset = j1;
1298  vinfos[1].indices[0] = _ij1[0];
1299  vinfos[1].indices[1] = _ij1[1];
1300  vinfos[1].maxsolutions = _nj1;
1301  vinfos[2].jointtype = 1;
1302  vinfos[2].foffset = j2;
1303  vinfos[2].indices[0] = _ij2[0];
1304  vinfos[2].indices[1] = _ij2[1];
1305  vinfos[2].maxsolutions = _nj2;
1306  vinfos[3].jointtype = 1;
1307  vinfos[3].foffset = j3;
1308  vinfos[3].indices[0] = _ij3[0];
1309  vinfos[3].indices[1] = _ij3[1];
1310  vinfos[3].maxsolutions = _nj3;
1311  vinfos[4].jointtype = 1;
1312  vinfos[4].foffset = j4;
1313  vinfos[4].indices[0] = _ij4[0];
1314  vinfos[4].indices[1] = _ij4[1];
1315  vinfos[4].maxsolutions = _nj4;
1316  vinfos[5].jointtype = 1;
1317  vinfos[5].foffset = j5;
1318  vinfos[5].indices[0] = _ij5[0];
1319  vinfos[5].indices[1] = _ij5[1];
1320  vinfos[5].maxsolutions = _nj5;
1321  std::vector<int> vfree(0);
1322  solutions.AddSolution(vinfos, vfree);
1323  }
1324  }
1325  }
1326  }
1327  }
1328  }
1329  else
1330  {
1331  if (1)
1332  {
1333  continue;
1334  }
1335  else
1336  {
1337  }
1338  }
1339  }
1340  }
1341  }
1342  else
1343  {
1344  {
1345  IkReal j3array[1], cj3array[1], sj3array[1];
1346  bool j3valid[1] = { false };
1347  _nj3 = 1;
1348  IkReal x112 = ((IkReal(-1.00000000000000)) * (cj4) * (gconst3) * (new_r20));
1349  if (IKabs(((new_r12) * (x112))) < IKFAST_ATAN2_MAGTHRESH &&
1350  IKabs(((new_r02) * (x112))) < IKFAST_ATAN2_MAGTHRESH)
1351  continue;
1352  j3array[0] = IKatan2(((new_r12) * (x112)), ((new_r02) * (x112)));
1353  sj3array[0] = IKsin(j3array[0]);
1354  cj3array[0] = IKcos(j3array[0]);
1355  if (j3array[0] > IKPI)
1356  {
1357  j3array[0] -= IK2PI;
1358  }
1359  else if (j3array[0] < -IKPI)
1360  {
1361  j3array[0] += IK2PI;
1362  }
1363  j3valid[0] = true;
1364  for (int ij3 = 0; ij3 < 1; ++ij3)
1365  {
1366  if (!j3valid[ij3])
1367  {
1368  continue;
1369  }
1370  _ij3[0] = ij3;
1371  _ij3[1] = -1;
1372  for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
1373  {
1374  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1375  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1376  {
1377  j3valid[iij3] = false;
1378  _ij3[1] = iij3;
1379  break;
1380  }
1381  }
1382  j3 = j3array[ij3];
1383  cj3 = cj3array[ij3];
1384  sj3 = sj3array[ij3];
1385  {
1386  IkReal evalcond[6];
1387  IkReal x113 = IKsin(j3);
1388  IkReal x114 = IKcos(j3);
1389  IkReal x115 = ((IkReal(1.00000000000000)) * (sj4));
1390  IkReal x116 = ((sj4) * (x113));
1391  IkReal x117 = ((sj4) * (x114));
1392  IkReal x118 = ((new_r02) * (x114));
1393  IkReal x119 = ((new_r12) * (x113));
1394  evalcond[0] =
1395  ((((IkReal(-1.00000000000000)) * (new_r02) * (x113))) + (((new_r12) * (x114))));
1396  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x115))) + (x119) + (x118));
1397  evalcond[2] = ((((new_r00) * (x117))) + (((cj4) * (new_r20))) + (((new_r10) * (x116))));
1398  evalcond[3] = ((((new_r11) * (x116))) + (((new_r01) * (x117))) + (((cj4) * (new_r21))));
1399  evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x117))) +
1400  (((new_r12) * (x116))) + (((cj4) * (new_r22))));
1401  evalcond[5] = ((((cj4) * (x118))) + (((cj4) * (x119))) +
1402  (((IkReal(-1.00000000000000)) * (new_r22) * (x115))));
1403  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1404  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
1405  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001)
1406  {
1407  continue;
1408  }
1409  }
1410 
1411  {
1412  IkReal dummyeval[1];
1413  IkReal gconst5;
1414  gconst5 = IKsign(sj4);
1415  dummyeval[0] = sj4;
1416  if (IKabs(dummyeval[0]) < 0.0000010000000000)
1417  {
1418  {
1419  IkReal dummyeval[1];
1420  dummyeval[0] = sj4;
1421  if (IKabs(dummyeval[0]) < 0.0000010000000000)
1422  {
1423  {
1424  IkReal dummyeval[1];
1425  dummyeval[0] = sj4;
1426  if (IKabs(dummyeval[0]) < 0.0000010000000000)
1427  {
1428  {
1429  IkReal evalcond[11];
1430  IkReal x120 = ((IkReal(-1.00000000000000)) + (new_r22));
1431  IkReal x121 = ((((cj3) * (new_r12))) +
1432  (((IkReal(-1.00000000000000)) * (new_r02) * (sj3))));
1433  IkReal x122 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02))));
1434  evalcond[0] =
1435  ((IkReal(-3.14159265358979)) +
1436  (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
1437  evalcond[1] = x120;
1438  evalcond[2] = new_r20;
1439  evalcond[3] = new_r21;
1440  evalcond[4] = x121;
1441  evalcond[5] = x121;
1442  evalcond[6] = x122;
1443  evalcond[7] = new_r20;
1444  evalcond[8] = new_r21;
1445  evalcond[9] = x120;
1446  evalcond[10] = x122;
1447  if (IKabs(evalcond[0]) < 0.0000010000000000 &&
1448  IKabs(evalcond[1]) < 0.0000010000000000 &&
1449  IKabs(evalcond[2]) < 0.0000010000000000 &&
1450  IKabs(evalcond[3]) < 0.0000010000000000 &&
1451  IKabs(evalcond[4]) < 0.0000010000000000 &&
1452  IKabs(evalcond[5]) < 0.0000010000000000 &&
1453  IKabs(evalcond[6]) < 0.0000010000000000 &&
1454  IKabs(evalcond[7]) < 0.0000010000000000 &&
1455  IKabs(evalcond[8]) < 0.0000010000000000 &&
1456  IKabs(evalcond[9]) < 0.0000010000000000 &&
1457  IKabs(evalcond[10]) < 0.0000010000000000)
1458  {
1459  {
1460  IkReal j5array[1], cj5array[1], sj5array[1];
1461  bool j5valid[1] = { false };
1462  _nj5 = 1;
1463  if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1464  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1465  IKFAST_ATAN2_MAGTHRESH &&
1466  IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
1467  IKFAST_ATAN2_MAGTHRESH &&
1468  IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1469  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
1470  IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
1471  IKFAST_SINCOS_THRESH)
1472  continue;
1473  j5array[0] = IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1474  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
1475  ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
1476  sj5array[0] = IKsin(j5array[0]);
1477  cj5array[0] = IKcos(j5array[0]);
1478  if (j5array[0] > IKPI)
1479  {
1480  j5array[0] -= IK2PI;
1481  }
1482  else if (j5array[0] < -IKPI)
1483  {
1484  j5array[0] += IK2PI;
1485  }
1486  j5valid[0] = true;
1487  for (int ij5 = 0; ij5 < 1; ++ij5)
1488  {
1489  if (!j5valid[ij5])
1490  {
1491  continue;
1492  }
1493  _ij5[0] = ij5;
1494  _ij5[1] = -1;
1495  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1496  {
1497  if (j5valid[iij5] &&
1498  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1499  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1500  {
1501  j5valid[iij5] = false;
1502  _ij5[1] = iij5;
1503  break;
1504  }
1505  }
1506  j5 = j5array[ij5];
1507  cj5 = cj5array[ij5];
1508  sj5 = sj5array[ij5];
1509  {
1510  IkReal evalcond[4];
1511  IkReal x123 = IKsin(j5);
1512  IkReal x124 = ((IkReal(1.00000000000000)) * (sj3));
1513  IkReal x125 = ((IkReal(1.00000000000000)) * (IKcos(j5)));
1514  evalcond[0] = ((((IkReal(-1.00000000000000)) * (x123))) +
1515  (((IkReal(-1.00000000000000)) * (new_r00) * (x124))) +
1516  (((cj3) * (new_r10))));
1517  evalcond[1] =
1518  ((((IkReal(-1.00000000000000)) * (new_r01) * (x124))) +
1519  (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x125))));
1520  evalcond[2] = ((((new_r11) * (sj3))) + (x123) + (((cj3) * (new_r01))));
1521  evalcond[3] =
1522  ((((new_r10) * (sj3))) + (((IkReal(-1.00000000000000)) * (x125))) +
1523  (((cj3) * (new_r00))));
1524  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1525  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
1526  {
1527  continue;
1528  }
1529  }
1530 
1531  {
1532  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1533  vinfos[0].jointtype = 1;
1534  vinfos[0].foffset = j0;
1535  vinfos[0].indices[0] = _ij0[0];
1536  vinfos[0].indices[1] = _ij0[1];
1537  vinfos[0].maxsolutions = _nj0;
1538  vinfos[1].jointtype = 1;
1539  vinfos[1].foffset = j1;
1540  vinfos[1].indices[0] = _ij1[0];
1541  vinfos[1].indices[1] = _ij1[1];
1542  vinfos[1].maxsolutions = _nj1;
1543  vinfos[2].jointtype = 1;
1544  vinfos[2].foffset = j2;
1545  vinfos[2].indices[0] = _ij2[0];
1546  vinfos[2].indices[1] = _ij2[1];
1547  vinfos[2].maxsolutions = _nj2;
1548  vinfos[3].jointtype = 1;
1549  vinfos[3].foffset = j3;
1550  vinfos[3].indices[0] = _ij3[0];
1551  vinfos[3].indices[1] = _ij3[1];
1552  vinfos[3].maxsolutions = _nj3;
1553  vinfos[4].jointtype = 1;
1554  vinfos[4].foffset = j4;
1555  vinfos[4].indices[0] = _ij4[0];
1556  vinfos[4].indices[1] = _ij4[1];
1557  vinfos[4].maxsolutions = _nj4;
1558  vinfos[5].jointtype = 1;
1559  vinfos[5].foffset = j5;
1560  vinfos[5].indices[0] = _ij5[0];
1561  vinfos[5].indices[1] = _ij5[1];
1562  vinfos[5].maxsolutions = _nj5;
1563  std::vector<int> vfree(0);
1564  solutions.AddSolution(vinfos, vfree);
1565  }
1566  }
1567  }
1568  }
1569  else
1570  {
1571  IkReal x126 = ((new_r12) * (sj3));
1572  IkReal x127 = ((IkReal(1.00000000000000)) * (new_r02));
1573  IkReal x128 = ((((IkReal(-1.00000000000000)) * (sj3) * (x127))) +
1574  (((cj3) * (new_r12))));
1575  evalcond[0] =
1576  ((IkReal(-3.14159265358979)) + (IKfmod(j4, IkReal(6.28318530717959))));
1577  evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
1578  evalcond[2] = new_r20;
1579  evalcond[3] = new_r21;
1580  evalcond[4] = x128;
1581  evalcond[5] = x128;
1582  evalcond[6] = ((x126) + (((cj3) * (new_r02))));
1583  evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20));
1584  evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21));
1585  evalcond[9] = ((IkReal(-1.00000000000000)) +
1586  (((IkReal(-1.00000000000000)) * (new_r22))));
1587  evalcond[10] = ((((IkReal(-1.00000000000000)) * (cj3) * (x127))) +
1588  (((IkReal(-1.00000000000000)) * (x126))));
1589  if (IKabs(evalcond[0]) < 0.0000010000000000 &&
1590  IKabs(evalcond[1]) < 0.0000010000000000 &&
1591  IKabs(evalcond[2]) < 0.0000010000000000 &&
1592  IKabs(evalcond[3]) < 0.0000010000000000 &&
1593  IKabs(evalcond[4]) < 0.0000010000000000 &&
1594  IKabs(evalcond[5]) < 0.0000010000000000 &&
1595  IKabs(evalcond[6]) < 0.0000010000000000 &&
1596  IKabs(evalcond[7]) < 0.0000010000000000 &&
1597  IKabs(evalcond[8]) < 0.0000010000000000 &&
1598  IKabs(evalcond[9]) < 0.0000010000000000 &&
1599  IKabs(evalcond[10]) < 0.0000010000000000)
1600  {
1601  {
1602  IkReal j5array[1], cj5array[1], sj5array[1];
1603  bool j5valid[1] = { false };
1604  _nj5 = 1;
1605  if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
1606  IKFAST_ATAN2_MAGTHRESH &&
1607  IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1608  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1609  IKFAST_ATAN2_MAGTHRESH &&
1610  IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1611  IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1612  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1613  1) <= IKFAST_SINCOS_THRESH)
1614  continue;
1615  j5array[0] =
1616  IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
1617  ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1618  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
1619  sj5array[0] = IKsin(j5array[0]);
1620  cj5array[0] = IKcos(j5array[0]);
1621  if (j5array[0] > IKPI)
1622  {
1623  j5array[0] -= IK2PI;
1624  }
1625  else if (j5array[0] < -IKPI)
1626  {
1627  j5array[0] += IK2PI;
1628  }
1629  j5valid[0] = true;
1630  for (int ij5 = 0; ij5 < 1; ++ij5)
1631  {
1632  if (!j5valid[ij5])
1633  {
1634  continue;
1635  }
1636  _ij5[0] = ij5;
1637  _ij5[1] = -1;
1638  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1639  {
1640  if (j5valid[iij5] &&
1641  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1642  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1643  {
1644  j5valid[iij5] = false;
1645  _ij5[1] = iij5;
1646  break;
1647  }
1648  }
1649  j5 = j5array[ij5];
1650  cj5 = cj5array[ij5];
1651  sj5 = sj5array[ij5];
1652  {
1653  IkReal evalcond[4];
1654  IkReal x129 = IKcos(j5);
1655  IkReal x130 = ((IkReal(1.00000000000000)) * (sj3));
1656  IkReal x131 = ((IkReal(1.00000000000000)) * (IKsin(j5)));
1657  evalcond[0] = ((((IkReal(-1.00000000000000)) * (x131))) +
1658  (((IkReal(-1.00000000000000)) * (new_r00) * (x130))) +
1659  (((cj3) * (new_r10))));
1660  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x129))) +
1661  (((cj3) * (new_r11))) +
1662  (((IkReal(-1.00000000000000)) * (new_r01) * (x130))));
1663  evalcond[2] = ((((IkReal(-1.00000000000000)) * (x131))) +
1664  (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
1665  evalcond[3] =
1666  ((x129) + (((new_r10) * (sj3))) + (((cj3) * (new_r00))));
1667  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1668  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
1669  {
1670  continue;
1671  }
1672  }
1673 
1674  {
1675  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1676  vinfos[0].jointtype = 1;
1677  vinfos[0].foffset = j0;
1678  vinfos[0].indices[0] = _ij0[0];
1679  vinfos[0].indices[1] = _ij0[1];
1680  vinfos[0].maxsolutions = _nj0;
1681  vinfos[1].jointtype = 1;
1682  vinfos[1].foffset = j1;
1683  vinfos[1].indices[0] = _ij1[0];
1684  vinfos[1].indices[1] = _ij1[1];
1685  vinfos[1].maxsolutions = _nj1;
1686  vinfos[2].jointtype = 1;
1687  vinfos[2].foffset = j2;
1688  vinfos[2].indices[0] = _ij2[0];
1689  vinfos[2].indices[1] = _ij2[1];
1690  vinfos[2].maxsolutions = _nj2;
1691  vinfos[3].jointtype = 1;
1692  vinfos[3].foffset = j3;
1693  vinfos[3].indices[0] = _ij3[0];
1694  vinfos[3].indices[1] = _ij3[1];
1695  vinfos[3].maxsolutions = _nj3;
1696  vinfos[4].jointtype = 1;
1697  vinfos[4].foffset = j4;
1698  vinfos[4].indices[0] = _ij4[0];
1699  vinfos[4].indices[1] = _ij4[1];
1700  vinfos[4].maxsolutions = _nj4;
1701  vinfos[5].jointtype = 1;
1702  vinfos[5].foffset = j5;
1703  vinfos[5].indices[0] = _ij5[0];
1704  vinfos[5].indices[1] = _ij5[1];
1705  vinfos[5].maxsolutions = _nj5;
1706  std::vector<int> vfree(0);
1707  solutions.AddSolution(vinfos, vfree);
1708  }
1709  }
1710  }
1711  }
1712  else
1713  {
1714  if (1)
1715  {
1716  continue;
1717  }
1718  else
1719  {
1720  }
1721  }
1722  }
1723  }
1724  }
1725  else
1726  {
1727  {
1728  IkReal j5array[1], cj5array[1], sj5array[1];
1729  bool j5valid[1] = { false };
1730  _nj5 = 1;
1731  if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1732  (((cj3) * (new_r10))))) < IKFAST_ATAN2_MAGTHRESH &&
1733  IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
1734  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1735  IKFAST_ATAN2_MAGTHRESH &&
1736  IKabs(
1737  IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1738  (((cj3) * (new_r10))))) +
1739  IKsqr(((IkReal(-1.00000000000000)) * (new_r20) *
1740  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) -
1741  1) <= IKFAST_SINCOS_THRESH)
1742  continue;
1743  j5array[0] =
1744  IKatan2(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1745  (((cj3) * (new_r10)))),
1746  ((IkReal(-1.00000000000000)) * (new_r20) *
1747  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))));
1748  sj5array[0] = IKsin(j5array[0]);
1749  cj5array[0] = IKcos(j5array[0]);
1750  if (j5array[0] > IKPI)
1751  {
1752  j5array[0] -= IK2PI;
1753  }
1754  else if (j5array[0] < -IKPI)
1755  {
1756  j5array[0] += IK2PI;
1757  }
1758  j5valid[0] = true;
1759  for (int ij5 = 0; ij5 < 1; ++ij5)
1760  {
1761  if (!j5valid[ij5])
1762  {
1763  continue;
1764  }
1765  _ij5[0] = ij5;
1766  _ij5[1] = -1;
1767  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1768  {
1769  if (j5valid[iij5] &&
1770  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1771  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1772  {
1773  j5valid[iij5] = false;
1774  _ij5[1] = iij5;
1775  break;
1776  }
1777  }
1778  j5 = j5array[ij5];
1779  cj5 = cj5array[ij5];
1780  sj5 = sj5array[ij5];
1781  {
1782  IkReal evalcond[8];
1783  IkReal x132 = IKsin(j5);
1784  IkReal x133 = IKcos(j5);
1785  IkReal x134 = ((IkReal(1.00000000000000)) * (sj3));
1786  IkReal x135 = ((new_r11) * (sj3));
1787  IkReal x136 = ((new_r10) * (sj3));
1788  IkReal x137 = ((cj3) * (cj4));
1789  IkReal x138 = ((IkReal(1.00000000000000)) * (sj4));
1790  IkReal x139 = ((IkReal(1.00000000000000)) * (x133));
1791  IkReal x140 = ((IkReal(1.00000000000000)) * (x132));
1792  evalcond[0] = ((new_r20) + (((sj4) * (x133))));
1793  evalcond[1] =
1794  ((((IkReal(-1.00000000000000)) * (x132) * (x138))) + (new_r21));
1795  evalcond[2] =
1796  ((((IkReal(-1.00000000000000)) * (x140))) + (((cj3) * (new_r10))) +
1797  (((IkReal(-1.00000000000000)) * (new_r00) * (x134))));
1798  evalcond[3] =
1799  ((((IkReal(-1.00000000000000)) * (x139))) + (((cj3) * (new_r11))) +
1800  (((IkReal(-1.00000000000000)) * (new_r01) * (x134))));
1801  evalcond[4] = ((((cj4) * (x132))) + (x135) + (((cj3) * (new_r01))));
1802  evalcond[5] = ((x136) + (((IkReal(-1.00000000000000)) * (cj4) * (x139))) +
1803  (((cj3) * (new_r00))));
1804  evalcond[6] = ((((cj4) * (x135))) + (((new_r01) * (x137))) + (x132) +
1805  (((IkReal(-1.00000000000000)) * (new_r21) * (x138))));
1806  evalcond[7] = ((((IkReal(-1.00000000000000)) * (x139))) +
1807  (((IkReal(-1.00000000000000)) * (new_r20) * (x138))) +
1808  (((cj4) * (x136))) + (((new_r00) * (x137))));
1809  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1810  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
1811  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
1812  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
1813  {
1814  continue;
1815  }
1816  }
1817 
1818  {
1819  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1820  vinfos[0].jointtype = 1;
1821  vinfos[0].foffset = j0;
1822  vinfos[0].indices[0] = _ij0[0];
1823  vinfos[0].indices[1] = _ij0[1];
1824  vinfos[0].maxsolutions = _nj0;
1825  vinfos[1].jointtype = 1;
1826  vinfos[1].foffset = j1;
1827  vinfos[1].indices[0] = _ij1[0];
1828  vinfos[1].indices[1] = _ij1[1];
1829  vinfos[1].maxsolutions = _nj1;
1830  vinfos[2].jointtype = 1;
1831  vinfos[2].foffset = j2;
1832  vinfos[2].indices[0] = _ij2[0];
1833  vinfos[2].indices[1] = _ij2[1];
1834  vinfos[2].maxsolutions = _nj2;
1835  vinfos[3].jointtype = 1;
1836  vinfos[3].foffset = j3;
1837  vinfos[3].indices[0] = _ij3[0];
1838  vinfos[3].indices[1] = _ij3[1];
1839  vinfos[3].maxsolutions = _nj3;
1840  vinfos[4].jointtype = 1;
1841  vinfos[4].foffset = j4;
1842  vinfos[4].indices[0] = _ij4[0];
1843  vinfos[4].indices[1] = _ij4[1];
1844  vinfos[4].maxsolutions = _nj4;
1845  vinfos[5].jointtype = 1;
1846  vinfos[5].foffset = j5;
1847  vinfos[5].indices[0] = _ij5[0];
1848  vinfos[5].indices[1] = _ij5[1];
1849  vinfos[5].maxsolutions = _nj5;
1850  std::vector<int> vfree(0);
1851  solutions.AddSolution(vinfos, vfree);
1852  }
1853  }
1854  }
1855  }
1856  }
1857  }
1858  else
1859  {
1860  {
1861  IkReal j5array[1], cj5array[1], sj5array[1];
1862  bool j5valid[1] = { false };
1863  _nj5 = 1;
1864  if (IKabs(((new_r21) *
1865  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1866  IKFAST_ATAN2_MAGTHRESH &&
1867  IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1868  (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH &&
1869  IKabs(IKsqr(((new_r21) *
1870  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
1871  IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1872  (((cj3) * (new_r11))))) -
1873  1) <= IKFAST_SINCOS_THRESH)
1874  continue;
1875  j5array[0] = IKatan2(
1876  ((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))),
1877  ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + (((cj3) * (new_r11)))));
1878  sj5array[0] = IKsin(j5array[0]);
1879  cj5array[0] = IKcos(j5array[0]);
1880  if (j5array[0] > IKPI)
1881  {
1882  j5array[0] -= IK2PI;
1883  }
1884  else if (j5array[0] < -IKPI)
1885  {
1886  j5array[0] += IK2PI;
1887  }
1888  j5valid[0] = true;
1889  for (int ij5 = 0; ij5 < 1; ++ij5)
1890  {
1891  if (!j5valid[ij5])
1892  {
1893  continue;
1894  }
1895  _ij5[0] = ij5;
1896  _ij5[1] = -1;
1897  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1898  {
1899  if (j5valid[iij5] &&
1900  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1901  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1902  {
1903  j5valid[iij5] = false;
1904  _ij5[1] = iij5;
1905  break;
1906  }
1907  }
1908  j5 = j5array[ij5];
1909  cj5 = cj5array[ij5];
1910  sj5 = sj5array[ij5];
1911  {
1912  IkReal evalcond[8];
1913  IkReal x141 = IKsin(j5);
1914  IkReal x142 = IKcos(j5);
1915  IkReal x143 = ((IkReal(1.00000000000000)) * (sj3));
1916  IkReal x144 = ((new_r11) * (sj3));
1917  IkReal x145 = ((new_r10) * (sj3));
1918  IkReal x146 = ((cj3) * (cj4));
1919  IkReal x147 = ((IkReal(1.00000000000000)) * (sj4));
1920  IkReal x148 = ((IkReal(1.00000000000000)) * (x142));
1921  IkReal x149 = ((IkReal(1.00000000000000)) * (x141));
1922  evalcond[0] = ((((sj4) * (x142))) + (new_r20));
1923  evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x141) * (x147))));
1924  evalcond[2] =
1925  ((((IkReal(-1.00000000000000)) * (new_r00) * (x143))) +
1926  (((cj3) * (new_r10))) + (((IkReal(-1.00000000000000)) * (x149))));
1927  evalcond[3] = ((((IkReal(-1.00000000000000)) * (x148))) +
1928  (((IkReal(-1.00000000000000)) * (new_r01) * (x143))) +
1929  (((cj3) * (new_r11))));
1930  evalcond[4] = ((((cj4) * (x141))) + (x144) + (((cj3) * (new_r01))));
1931  evalcond[5] = ((((IkReal(-1.00000000000000)) * (cj4) * (x148))) + (x145) +
1932  (((cj3) * (new_r00))));
1933  evalcond[6] = ((((cj4) * (x144))) + (((new_r01) * (x146))) + (x141) +
1934  (((IkReal(-1.00000000000000)) * (new_r21) * (x147))));
1935  evalcond[7] = ((((cj4) * (x145))) + (((IkReal(-1.00000000000000)) * (x148))) +
1936  (((IkReal(-1.00000000000000)) * (new_r20) * (x147))) +
1937  (((new_r00) * (x146))));
1938  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
1939  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
1940  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
1941  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
1942  {
1943  continue;
1944  }
1945  }
1946 
1947  {
1948  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1949  vinfos[0].jointtype = 1;
1950  vinfos[0].foffset = j0;
1951  vinfos[0].indices[0] = _ij0[0];
1952  vinfos[0].indices[1] = _ij0[1];
1953  vinfos[0].maxsolutions = _nj0;
1954  vinfos[1].jointtype = 1;
1955  vinfos[1].foffset = j1;
1956  vinfos[1].indices[0] = _ij1[0];
1957  vinfos[1].indices[1] = _ij1[1];
1958  vinfos[1].maxsolutions = _nj1;
1959  vinfos[2].jointtype = 1;
1960  vinfos[2].foffset = j2;
1961  vinfos[2].indices[0] = _ij2[0];
1962  vinfos[2].indices[1] = _ij2[1];
1963  vinfos[2].maxsolutions = _nj2;
1964  vinfos[3].jointtype = 1;
1965  vinfos[3].foffset = j3;
1966  vinfos[3].indices[0] = _ij3[0];
1967  vinfos[3].indices[1] = _ij3[1];
1968  vinfos[3].maxsolutions = _nj3;
1969  vinfos[4].jointtype = 1;
1970  vinfos[4].foffset = j4;
1971  vinfos[4].indices[0] = _ij4[0];
1972  vinfos[4].indices[1] = _ij4[1];
1973  vinfos[4].maxsolutions = _nj4;
1974  vinfos[5].jointtype = 1;
1975  vinfos[5].foffset = j5;
1976  vinfos[5].indices[0] = _ij5[0];
1977  vinfos[5].indices[1] = _ij5[1];
1978  vinfos[5].maxsolutions = _nj5;
1979  std::vector<int> vfree(0);
1980  solutions.AddSolution(vinfos, vfree);
1981  }
1982  }
1983  }
1984  }
1985  }
1986  }
1987  else
1988  {
1989  {
1990  IkReal j5array[1], cj5array[1], sj5array[1];
1991  bool j5valid[1] = { false };
1992  _nj5 = 1;
1993  if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
1994  IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) <
1995  IKFAST_ATAN2_MAGTHRESH)
1996  continue;
1997  j5array[0] = IKatan2(((gconst5) * (new_r21)),
1998  ((IkReal(-1.00000000000000)) * (gconst5) * (new_r20)));
1999  sj5array[0] = IKsin(j5array[0]);
2000  cj5array[0] = IKcos(j5array[0]);
2001  if (j5array[0] > IKPI)
2002  {
2003  j5array[0] -= IK2PI;
2004  }
2005  else if (j5array[0] < -IKPI)
2006  {
2007  j5array[0] += IK2PI;
2008  }
2009  j5valid[0] = true;
2010  for (int ij5 = 0; ij5 < 1; ++ij5)
2011  {
2012  if (!j5valid[ij5])
2013  {
2014  continue;
2015  }
2016  _ij5[0] = ij5;
2017  _ij5[1] = -1;
2018  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2019  {
2020  if (j5valid[iij5] &&
2021  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2022  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2023  {
2024  j5valid[iij5] = false;
2025  _ij5[1] = iij5;
2026  break;
2027  }
2028  }
2029  j5 = j5array[ij5];
2030  cj5 = cj5array[ij5];
2031  sj5 = sj5array[ij5];
2032  {
2033  IkReal evalcond[8];
2034  IkReal x150 = IKsin(j5);
2035  IkReal x151 = IKcos(j5);
2036  IkReal x152 = ((IkReal(1.00000000000000)) * (sj3));
2037  IkReal x153 = ((new_r11) * (sj3));
2038  IkReal x154 = ((new_r10) * (sj3));
2039  IkReal x155 = ((cj3) * (cj4));
2040  IkReal x156 = ((IkReal(1.00000000000000)) * (sj4));
2041  IkReal x157 = ((IkReal(1.00000000000000)) * (x151));
2042  IkReal x158 = ((IkReal(1.00000000000000)) * (x150));
2043  evalcond[0] = ((((sj4) * (x151))) + (new_r20));
2044  evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x150) * (x156))));
2045  evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x152))) +
2046  (((IkReal(-1.00000000000000)) * (x158))) + (((cj3) * (new_r10))));
2047  evalcond[3] = ((((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x157))) +
2048  (((IkReal(-1.00000000000000)) * (new_r01) * (x152))));
2049  evalcond[4] = ((x153) + (((cj4) * (x150))) + (((cj3) * (new_r01))));
2050  evalcond[5] = ((x154) + (((IkReal(-1.00000000000000)) * (cj4) * (x157))) +
2051  (((cj3) * (new_r00))));
2052  evalcond[6] = ((((IkReal(-1.00000000000000)) * (new_r21) * (x156))) + (x150) +
2053  (((new_r01) * (x155))) + (((cj4) * (x153))));
2054  evalcond[7] = ((((IkReal(-1.00000000000000)) * (x157))) +
2055  (((IkReal(-1.00000000000000)) * (new_r20) * (x156))) +
2056  (((cj4) * (x154))) + (((new_r00) * (x155))));
2057  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2058  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
2059  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
2060  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
2061  {
2062  continue;
2063  }
2064  }
2065 
2066  {
2067  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2068  vinfos[0].jointtype = 1;
2069  vinfos[0].foffset = j0;
2070  vinfos[0].indices[0] = _ij0[0];
2071  vinfos[0].indices[1] = _ij0[1];
2072  vinfos[0].maxsolutions = _nj0;
2073  vinfos[1].jointtype = 1;
2074  vinfos[1].foffset = j1;
2075  vinfos[1].indices[0] = _ij1[0];
2076  vinfos[1].indices[1] = _ij1[1];
2077  vinfos[1].maxsolutions = _nj1;
2078  vinfos[2].jointtype = 1;
2079  vinfos[2].foffset = j2;
2080  vinfos[2].indices[0] = _ij2[0];
2081  vinfos[2].indices[1] = _ij2[1];
2082  vinfos[2].maxsolutions = _nj2;
2083  vinfos[3].jointtype = 1;
2084  vinfos[3].foffset = j3;
2085  vinfos[3].indices[0] = _ij3[0];
2086  vinfos[3].indices[1] = _ij3[1];
2087  vinfos[3].maxsolutions = _nj3;
2088  vinfos[4].jointtype = 1;
2089  vinfos[4].foffset = j4;
2090  vinfos[4].indices[0] = _ij4[0];
2091  vinfos[4].indices[1] = _ij4[1];
2092  vinfos[4].maxsolutions = _nj4;
2093  vinfos[5].jointtype = 1;
2094  vinfos[5].foffset = j5;
2095  vinfos[5].indices[0] = _ij5[0];
2096  vinfos[5].indices[1] = _ij5[1];
2097  vinfos[5].maxsolutions = _nj5;
2098  std::vector<int> vfree(0);
2099  solutions.AddSolution(vinfos, vfree);
2100  }
2101  }
2102  }
2103  }
2104  }
2105  }
2106  }
2107  }
2108  }
2109  }
2110  else
2111  {
2112  {
2113  IkReal j3array[1], cj3array[1], sj3array[1];
2114  bool j3valid[1] = { false };
2115  _nj3 = 1;
2116  IkReal x159 = ((gconst2) * (sj4));
2117  if (IKabs(((new_r12) * (x159))) < IKFAST_ATAN2_MAGTHRESH &&
2118  IKabs(((new_r02) * (x159))) < IKFAST_ATAN2_MAGTHRESH)
2119  continue;
2120  j3array[0] = IKatan2(((new_r12) * (x159)), ((new_r02) * (x159)));
2121  sj3array[0] = IKsin(j3array[0]);
2122  cj3array[0] = IKcos(j3array[0]);
2123  if (j3array[0] > IKPI)
2124  {
2125  j3array[0] -= IK2PI;
2126  }
2127  else if (j3array[0] < -IKPI)
2128  {
2129  j3array[0] += IK2PI;
2130  }
2131  j3valid[0] = true;
2132  for (int ij3 = 0; ij3 < 1; ++ij3)
2133  {
2134  if (!j3valid[ij3])
2135  {
2136  continue;
2137  }
2138  _ij3[0] = ij3;
2139  _ij3[1] = -1;
2140  for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2141  {
2142  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
2143  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
2144  {
2145  j3valid[iij3] = false;
2146  _ij3[1] = iij3;
2147  break;
2148  }
2149  }
2150  j3 = j3array[ij3];
2151  cj3 = cj3array[ij3];
2152  sj3 = sj3array[ij3];
2153  {
2154  IkReal evalcond[6];
2155  IkReal x160 = IKsin(j3);
2156  IkReal x161 = IKcos(j3);
2157  IkReal x162 = ((IkReal(1.00000000000000)) * (sj4));
2158  IkReal x163 = ((sj4) * (x160));
2159  IkReal x164 = ((sj4) * (x161));
2160  IkReal x165 = ((new_r02) * (x161));
2161  IkReal x166 = ((new_r12) * (x160));
2162  evalcond[0] = ((((new_r12) * (x161))) + (((IkReal(-1.00000000000000)) * (new_r02) * (x160))));
2163  evalcond[1] = ((x166) + (x165) + (((IkReal(-1.00000000000000)) * (x162))));
2164  evalcond[2] = ((((new_r00) * (x164))) + (((new_r10) * (x163))) + (((cj4) * (new_r20))));
2165  evalcond[3] = ((((new_r01) * (x164))) + (((new_r11) * (x163))) + (((cj4) * (new_r21))));
2166  evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x164))) + (((new_r12) * (x163))) +
2167  (((cj4) * (new_r22))));
2168  evalcond[5] = ((((cj4) * (x165))) + (((IkReal(-1.00000000000000)) * (new_r22) * (x162))) +
2169  (((cj4) * (x166))));
2170  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2171  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
2172  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001)
2173  {
2174  continue;
2175  }
2176  }
2177 
2178  {
2179  IkReal dummyeval[1];
2180  IkReal gconst5;
2181  gconst5 = IKsign(sj4);
2182  dummyeval[0] = sj4;
2183  if (IKabs(dummyeval[0]) < 0.0000010000000000)
2184  {
2185  {
2186  IkReal dummyeval[1];
2187  dummyeval[0] = sj4;
2188  if (IKabs(dummyeval[0]) < 0.0000010000000000)
2189  {
2190  {
2191  IkReal dummyeval[1];
2192  dummyeval[0] = sj4;
2193  if (IKabs(dummyeval[0]) < 0.0000010000000000)
2194  {
2195  {
2196  IkReal evalcond[11];
2197  IkReal x167 = ((IkReal(-1.00000000000000)) + (new_r22));
2198  IkReal x168 =
2199  ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (new_r02) * (sj3))));
2200  IkReal x169 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02))));
2201  evalcond[0] =
2202  ((IkReal(-3.14159265358979)) +
2203  (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
2204  evalcond[1] = x167;
2205  evalcond[2] = new_r20;
2206  evalcond[3] = new_r21;
2207  evalcond[4] = x168;
2208  evalcond[5] = x168;
2209  evalcond[6] = x169;
2210  evalcond[7] = new_r20;
2211  evalcond[8] = new_r21;
2212  evalcond[9] = x167;
2213  evalcond[10] = x169;
2214  if (IKabs(evalcond[0]) < 0.0000010000000000 &&
2215  IKabs(evalcond[1]) < 0.0000010000000000 &&
2216  IKabs(evalcond[2]) < 0.0000010000000000 &&
2217  IKabs(evalcond[3]) < 0.0000010000000000 &&
2218  IKabs(evalcond[4]) < 0.0000010000000000 &&
2219  IKabs(evalcond[5]) < 0.0000010000000000 &&
2220  IKabs(evalcond[6]) < 0.0000010000000000 &&
2221  IKabs(evalcond[7]) < 0.0000010000000000 &&
2222  IKabs(evalcond[8]) < 0.0000010000000000 &&
2223  IKabs(evalcond[9]) < 0.0000010000000000 &&
2224  IKabs(evalcond[10]) < 0.0000010000000000)
2225  {
2226  {
2227  IkReal j5array[1], cj5array[1], sj5array[1];
2228  bool j5valid[1] = { false };
2229  _nj5 = 1;
2230  if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2231  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
2232  IKFAST_ATAN2_MAGTHRESH &&
2233  IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
2234  IKFAST_ATAN2_MAGTHRESH &&
2235  IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2236  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
2237  IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
2238  IKFAST_SINCOS_THRESH)
2239  continue;
2240  j5array[0] = IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2241  (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
2242  ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
2243  sj5array[0] = IKsin(j5array[0]);
2244  cj5array[0] = IKcos(j5array[0]);
2245  if (j5array[0] > IKPI)
2246  {
2247  j5array[0] -= IK2PI;
2248  }
2249  else if (j5array[0] < -IKPI)
2250  {
2251  j5array[0] += IK2PI;
2252  }
2253  j5valid[0] = true;
2254  for (int ij5 = 0; ij5 < 1; ++ij5)
2255  {
2256  if (!j5valid[ij5])
2257  {
2258  continue;
2259  }
2260  _ij5[0] = ij5;
2261  _ij5[1] = -1;
2262  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2263  {
2264  if (j5valid[iij5] &&
2265  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2266  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2267  {
2268  j5valid[iij5] = false;
2269  _ij5[1] = iij5;
2270  break;
2271  }
2272  }
2273  j5 = j5array[ij5];
2274  cj5 = cj5array[ij5];
2275  sj5 = sj5array[ij5];
2276  {
2277  IkReal evalcond[4];
2278  IkReal x170 = IKsin(j5);
2279  IkReal x171 = ((IkReal(1.00000000000000)) * (sj3));
2280  IkReal x172 = ((IkReal(1.00000000000000)) * (IKcos(j5)));
2281  evalcond[0] =
2282  ((((IkReal(-1.00000000000000)) * (x170))) + (((cj3) * (new_r10))) +
2283  (((IkReal(-1.00000000000000)) * (new_r00) * (x171))));
2284  evalcond[1] =
2285  ((((IkReal(-1.00000000000000)) * (new_r01) * (x171))) +
2286  (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x172))));
2287  evalcond[2] = ((x170) + (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
2288  evalcond[3] =
2289  ((((new_r10) * (sj3))) + (((IkReal(-1.00000000000000)) * (x172))) +
2290  (((cj3) * (new_r00))));
2291  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2292  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
2293  {
2294  continue;
2295  }
2296  }
2297 
2298  {
2299  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2300  vinfos[0].jointtype = 1;
2301  vinfos[0].foffset = j0;
2302  vinfos[0].indices[0] = _ij0[0];
2303  vinfos[0].indices[1] = _ij0[1];
2304  vinfos[0].maxsolutions = _nj0;
2305  vinfos[1].jointtype = 1;
2306  vinfos[1].foffset = j1;
2307  vinfos[1].indices[0] = _ij1[0];
2308  vinfos[1].indices[1] = _ij1[1];
2309  vinfos[1].maxsolutions = _nj1;
2310  vinfos[2].jointtype = 1;
2311  vinfos[2].foffset = j2;
2312  vinfos[2].indices[0] = _ij2[0];
2313  vinfos[2].indices[1] = _ij2[1];
2314  vinfos[2].maxsolutions = _nj2;
2315  vinfos[3].jointtype = 1;
2316  vinfos[3].foffset = j3;
2317  vinfos[3].indices[0] = _ij3[0];
2318  vinfos[3].indices[1] = _ij3[1];
2319  vinfos[3].maxsolutions = _nj3;
2320  vinfos[4].jointtype = 1;
2321  vinfos[4].foffset = j4;
2322  vinfos[4].indices[0] = _ij4[0];
2323  vinfos[4].indices[1] = _ij4[1];
2324  vinfos[4].maxsolutions = _nj4;
2325  vinfos[5].jointtype = 1;
2326  vinfos[5].foffset = j5;
2327  vinfos[5].indices[0] = _ij5[0];
2328  vinfos[5].indices[1] = _ij5[1];
2329  vinfos[5].maxsolutions = _nj5;
2330  std::vector<int> vfree(0);
2331  solutions.AddSolution(vinfos, vfree);
2332  }
2333  }
2334  }
2335  }
2336  else
2337  {
2338  IkReal x173 = ((new_r12) * (sj3));
2339  IkReal x174 = ((IkReal(1.00000000000000)) * (new_r02));
2340  IkReal x175 =
2341  ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (sj3) * (x174))));
2342  evalcond[0] =
2343  ((IkReal(-3.14159265358979)) + (IKfmod(j4, IkReal(6.28318530717959))));
2344  evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
2345  evalcond[2] = new_r20;
2346  evalcond[3] = new_r21;
2347  evalcond[4] = x175;
2348  evalcond[5] = x175;
2349  evalcond[6] = ((x173) + (((cj3) * (new_r02))));
2350  evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20));
2351  evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21));
2352  evalcond[9] =
2353  ((IkReal(-1.00000000000000)) + (((IkReal(-1.00000000000000)) * (new_r22))));
2354  evalcond[10] = ((((IkReal(-1.00000000000000)) * (x173))) +
2355  (((IkReal(-1.00000000000000)) * (cj3) * (x174))));
2356  if (IKabs(evalcond[0]) < 0.0000010000000000 &&
2357  IKabs(evalcond[1]) < 0.0000010000000000 &&
2358  IKabs(evalcond[2]) < 0.0000010000000000 &&
2359  IKabs(evalcond[3]) < 0.0000010000000000 &&
2360  IKabs(evalcond[4]) < 0.0000010000000000 &&
2361  IKabs(evalcond[5]) < 0.0000010000000000 &&
2362  IKabs(evalcond[6]) < 0.0000010000000000 &&
2363  IKabs(evalcond[7]) < 0.0000010000000000 &&
2364  IKabs(evalcond[8]) < 0.0000010000000000 &&
2365  IKabs(evalcond[9]) < 0.0000010000000000 &&
2366  IKabs(evalcond[10]) < 0.0000010000000000)
2367  {
2368  {
2369  IkReal j5array[1], cj5array[1], sj5array[1];
2370  bool j5valid[1] = { false };
2371  _nj5 = 1;
2372  if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
2373  IKFAST_ATAN2_MAGTHRESH &&
2374  IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2375  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
2376  IKFAST_ATAN2_MAGTHRESH &&
2377  IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
2378  IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2379  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
2380  1) <= IKFAST_SINCOS_THRESH)
2381  continue;
2382  j5array[0] = IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
2383  ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2384  (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
2385  sj5array[0] = IKsin(j5array[0]);
2386  cj5array[0] = IKcos(j5array[0]);
2387  if (j5array[0] > IKPI)
2388  {
2389  j5array[0] -= IK2PI;
2390  }
2391  else if (j5array[0] < -IKPI)
2392  {
2393  j5array[0] += IK2PI;
2394  }
2395  j5valid[0] = true;
2396  for (int ij5 = 0; ij5 < 1; ++ij5)
2397  {
2398  if (!j5valid[ij5])
2399  {
2400  continue;
2401  }
2402  _ij5[0] = ij5;
2403  _ij5[1] = -1;
2404  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2405  {
2406  if (j5valid[iij5] &&
2407  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2408  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2409  {
2410  j5valid[iij5] = false;
2411  _ij5[1] = iij5;
2412  break;
2413  }
2414  }
2415  j5 = j5array[ij5];
2416  cj5 = cj5array[ij5];
2417  sj5 = sj5array[ij5];
2418  {
2419  IkReal evalcond[4];
2420  IkReal x176 = IKcos(j5);
2421  IkReal x177 = ((IkReal(1.00000000000000)) * (sj3));
2422  IkReal x178 = ((IkReal(1.00000000000000)) * (IKsin(j5)));
2423  evalcond[0] =
2424  ((((IkReal(-1.00000000000000)) * (new_r00) * (x177))) +
2425  (((IkReal(-1.00000000000000)) * (x178))) + (((cj3) * (new_r10))));
2426  evalcond[1] = ((((cj3) * (new_r11))) +
2427  (((IkReal(-1.00000000000000)) * (new_r01) * (x177))) +
2428  (((IkReal(-1.00000000000000)) * (x176))));
2429  evalcond[2] = ((((IkReal(-1.00000000000000)) * (x178))) +
2430  (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
2431  evalcond[3] = ((x176) + (((new_r10) * (sj3))) + (((cj3) * (new_r00))));
2432  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2433  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001)
2434  {
2435  continue;
2436  }
2437  }
2438 
2439  {
2440  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2441  vinfos[0].jointtype = 1;
2442  vinfos[0].foffset = j0;
2443  vinfos[0].indices[0] = _ij0[0];
2444  vinfos[0].indices[1] = _ij0[1];
2445  vinfos[0].maxsolutions = _nj0;
2446  vinfos[1].jointtype = 1;
2447  vinfos[1].foffset = j1;
2448  vinfos[1].indices[0] = _ij1[0];
2449  vinfos[1].indices[1] = _ij1[1];
2450  vinfos[1].maxsolutions = _nj1;
2451  vinfos[2].jointtype = 1;
2452  vinfos[2].foffset = j2;
2453  vinfos[2].indices[0] = _ij2[0];
2454  vinfos[2].indices[1] = _ij2[1];
2455  vinfos[2].maxsolutions = _nj2;
2456  vinfos[3].jointtype = 1;
2457  vinfos[3].foffset = j3;
2458  vinfos[3].indices[0] = _ij3[0];
2459  vinfos[3].indices[1] = _ij3[1];
2460  vinfos[3].maxsolutions = _nj3;
2461  vinfos[4].jointtype = 1;
2462  vinfos[4].foffset = j4;
2463  vinfos[4].indices[0] = _ij4[0];
2464  vinfos[4].indices[1] = _ij4[1];
2465  vinfos[4].maxsolutions = _nj4;
2466  vinfos[5].jointtype = 1;
2467  vinfos[5].foffset = j5;
2468  vinfos[5].indices[0] = _ij5[0];
2469  vinfos[5].indices[1] = _ij5[1];
2470  vinfos[5].maxsolutions = _nj5;
2471  std::vector<int> vfree(0);
2472  solutions.AddSolution(vinfos, vfree);
2473  }
2474  }
2475  }
2476  }
2477  else
2478  {
2479  if (1)
2480  {
2481  continue;
2482  }
2483  else
2484  {
2485  }
2486  }
2487  }
2488  }
2489  }
2490  else
2491  {
2492  {
2493  IkReal j5array[1], cj5array[1], sj5array[1];
2494  bool j5valid[1] = { false };
2495  _nj5 = 1;
2496  if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
2497  (((cj3) * (new_r10))))) < IKFAST_ATAN2_MAGTHRESH &&
2498  IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
2499  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2500  IKFAST_ATAN2_MAGTHRESH &&
2501  IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
2502  (((cj3) * (new_r10))))) +
2503  IKsqr(((IkReal(-1.00000000000000)) * (new_r20) *
2504  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) -
2505  1) <= IKFAST_SINCOS_THRESH)
2506  continue;
2507  j5array[0] = IKatan2(
2508  ((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) + (((cj3) * (new_r10)))),
2509  ((IkReal(-1.00000000000000)) * (new_r20) *
2510  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))));
2511  sj5array[0] = IKsin(j5array[0]);
2512  cj5array[0] = IKcos(j5array[0]);
2513  if (j5array[0] > IKPI)
2514  {
2515  j5array[0] -= IK2PI;
2516  }
2517  else if (j5array[0] < -IKPI)
2518  {
2519  j5array[0] += IK2PI;
2520  }
2521  j5valid[0] = true;
2522  for (int ij5 = 0; ij5 < 1; ++ij5)
2523  {
2524  if (!j5valid[ij5])
2525  {
2526  continue;
2527  }
2528  _ij5[0] = ij5;
2529  _ij5[1] = -1;
2530  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2531  {
2532  if (j5valid[iij5] &&
2533  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2534  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2535  {
2536  j5valid[iij5] = false;
2537  _ij5[1] = iij5;
2538  break;
2539  }
2540  }
2541  j5 = j5array[ij5];
2542  cj5 = cj5array[ij5];
2543  sj5 = sj5array[ij5];
2544  {
2545  IkReal evalcond[8];
2546  IkReal x179 = IKsin(j5);
2547  IkReal x180 = IKcos(j5);
2548  IkReal x181 = ((IkReal(1.00000000000000)) * (sj3));
2549  IkReal x182 = ((new_r11) * (sj3));
2550  IkReal x183 = ((new_r10) * (sj3));
2551  IkReal x184 = ((cj3) * (cj4));
2552  IkReal x185 = ((IkReal(1.00000000000000)) * (sj4));
2553  IkReal x186 = ((IkReal(1.00000000000000)) * (x180));
2554  IkReal x187 = ((IkReal(1.00000000000000)) * (x179));
2555  evalcond[0] = ((((sj4) * (x180))) + (new_r20));
2556  evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x179) * (x185))));
2557  evalcond[2] =
2558  ((((IkReal(-1.00000000000000)) * (new_r00) * (x181))) +
2559  (((IkReal(-1.00000000000000)) * (x187))) + (((cj3) * (new_r10))));
2560  evalcond[3] =
2561  ((((IkReal(-1.00000000000000)) * (new_r01) * (x181))) +
2562  (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x186))));
2563  evalcond[4] = ((x182) + (((cj3) * (new_r01))) + (((cj4) * (x179))));
2564  evalcond[5] = ((x183) + (((IkReal(-1.00000000000000)) * (cj4) * (x186))) +
2565  (((cj3) * (new_r00))));
2566  evalcond[6] = ((x179) + (((cj4) * (x182))) + (((new_r01) * (x184))) +
2567  (((IkReal(-1.00000000000000)) * (new_r21) * (x185))));
2568  evalcond[7] = ((((new_r00) * (x184))) + (((cj4) * (x183))) +
2569  (((IkReal(-1.00000000000000)) * (new_r20) * (x185))) +
2570  (((IkReal(-1.00000000000000)) * (x186))));
2571  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2572  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
2573  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
2574  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
2575  {
2576  continue;
2577  }
2578  }
2579 
2580  {
2581  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2582  vinfos[0].jointtype = 1;
2583  vinfos[0].foffset = j0;
2584  vinfos[0].indices[0] = _ij0[0];
2585  vinfos[0].indices[1] = _ij0[1];
2586  vinfos[0].maxsolutions = _nj0;
2587  vinfos[1].jointtype = 1;
2588  vinfos[1].foffset = j1;
2589  vinfos[1].indices[0] = _ij1[0];
2590  vinfos[1].indices[1] = _ij1[1];
2591  vinfos[1].maxsolutions = _nj1;
2592  vinfos[2].jointtype = 1;
2593  vinfos[2].foffset = j2;
2594  vinfos[2].indices[0] = _ij2[0];
2595  vinfos[2].indices[1] = _ij2[1];
2596  vinfos[2].maxsolutions = _nj2;
2597  vinfos[3].jointtype = 1;
2598  vinfos[3].foffset = j3;
2599  vinfos[3].indices[0] = _ij3[0];
2600  vinfos[3].indices[1] = _ij3[1];
2601  vinfos[3].maxsolutions = _nj3;
2602  vinfos[4].jointtype = 1;
2603  vinfos[4].foffset = j4;
2604  vinfos[4].indices[0] = _ij4[0];
2605  vinfos[4].indices[1] = _ij4[1];
2606  vinfos[4].maxsolutions = _nj4;
2607  vinfos[5].jointtype = 1;
2608  vinfos[5].foffset = j5;
2609  vinfos[5].indices[0] = _ij5[0];
2610  vinfos[5].indices[1] = _ij5[1];
2611  vinfos[5].maxsolutions = _nj5;
2612  std::vector<int> vfree(0);
2613  solutions.AddSolution(vinfos, vfree);
2614  }
2615  }
2616  }
2617  }
2618  }
2619  }
2620  else
2621  {
2622  {
2623  IkReal j5array[1], cj5array[1], sj5array[1];
2624  bool j5valid[1] = { false };
2625  _nj5 = 1;
2626  if (IKabs(((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2627  IKFAST_ATAN2_MAGTHRESH &&
2628  IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2629  (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH &&
2630  IKabs(IKsqr(((new_r21) *
2631  (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
2632  IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2633  (((cj3) * (new_r11))))) -
2634  1) <= IKFAST_SINCOS_THRESH)
2635  continue;
2636  j5array[0] = IKatan2(
2637  ((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))),
2638  ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + (((cj3) * (new_r11)))));
2639  sj5array[0] = IKsin(j5array[0]);
2640  cj5array[0] = IKcos(j5array[0]);
2641  if (j5array[0] > IKPI)
2642  {
2643  j5array[0] -= IK2PI;
2644  }
2645  else if (j5array[0] < -IKPI)
2646  {
2647  j5array[0] += IK2PI;
2648  }
2649  j5valid[0] = true;
2650  for (int ij5 = 0; ij5 < 1; ++ij5)
2651  {
2652  if (!j5valid[ij5])
2653  {
2654  continue;
2655  }
2656  _ij5[0] = ij5;
2657  _ij5[1] = -1;
2658  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2659  {
2660  if (j5valid[iij5] &&
2661  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2662  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2663  {
2664  j5valid[iij5] = false;
2665  _ij5[1] = iij5;
2666  break;
2667  }
2668  }
2669  j5 = j5array[ij5];
2670  cj5 = cj5array[ij5];
2671  sj5 = sj5array[ij5];
2672  {
2673  IkReal evalcond[8];
2674  IkReal x188 = IKsin(j5);
2675  IkReal x189 = IKcos(j5);
2676  IkReal x190 = ((IkReal(1.00000000000000)) * (sj3));
2677  IkReal x191 = ((new_r11) * (sj3));
2678  IkReal x192 = ((new_r10) * (sj3));
2679  IkReal x193 = ((cj3) * (cj4));
2680  IkReal x194 = ((IkReal(1.00000000000000)) * (sj4));
2681  IkReal x195 = ((IkReal(1.00000000000000)) * (x189));
2682  IkReal x196 = ((IkReal(1.00000000000000)) * (x188));
2683  evalcond[0] = ((new_r20) + (((sj4) * (x189))));
2684  evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x188) * (x194))));
2685  evalcond[2] =
2686  ((((IkReal(-1.00000000000000)) * (x196))) +
2687  (((IkReal(-1.00000000000000)) * (new_r00) * (x190))) + (((cj3) * (new_r10))));
2688  evalcond[3] =
2689  ((((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (new_r01) * (x190))) +
2690  (((IkReal(-1.00000000000000)) * (x195))));
2691  evalcond[4] = ((((cj4) * (x188))) + (x191) + (((cj3) * (new_r01))));
2692  evalcond[5] = ((((IkReal(-1.00000000000000)) * (cj4) * (x195))) + (x192) +
2693  (((cj3) * (new_r00))));
2694  evalcond[6] = ((((IkReal(-1.00000000000000)) * (new_r21) * (x194))) +
2695  (((cj4) * (x191))) + (x188) + (((new_r01) * (x193))));
2696  evalcond[7] =
2697  ((((new_r00) * (x193))) + (((IkReal(-1.00000000000000)) * (x195))) +
2698  (((cj4) * (x192))) + (((IkReal(-1.00000000000000)) * (new_r20) * (x194))));
2699  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2700  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
2701  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
2702  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
2703  {
2704  continue;
2705  }
2706  }
2707 
2708  {
2709  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2710  vinfos[0].jointtype = 1;
2711  vinfos[0].foffset = j0;
2712  vinfos[0].indices[0] = _ij0[0];
2713  vinfos[0].indices[1] = _ij0[1];
2714  vinfos[0].maxsolutions = _nj0;
2715  vinfos[1].jointtype = 1;
2716  vinfos[1].foffset = j1;
2717  vinfos[1].indices[0] = _ij1[0];
2718  vinfos[1].indices[1] = _ij1[1];
2719  vinfos[1].maxsolutions = _nj1;
2720  vinfos[2].jointtype = 1;
2721  vinfos[2].foffset = j2;
2722  vinfos[2].indices[0] = _ij2[0];
2723  vinfos[2].indices[1] = _ij2[1];
2724  vinfos[2].maxsolutions = _nj2;
2725  vinfos[3].jointtype = 1;
2726  vinfos[3].foffset = j3;
2727  vinfos[3].indices[0] = _ij3[0];
2728  vinfos[3].indices[1] = _ij3[1];
2729  vinfos[3].maxsolutions = _nj3;
2730  vinfos[4].jointtype = 1;
2731  vinfos[4].foffset = j4;
2732  vinfos[4].indices[0] = _ij4[0];
2733  vinfos[4].indices[1] = _ij4[1];
2734  vinfos[4].maxsolutions = _nj4;
2735  vinfos[5].jointtype = 1;
2736  vinfos[5].foffset = j5;
2737  vinfos[5].indices[0] = _ij5[0];
2738  vinfos[5].indices[1] = _ij5[1];
2739  vinfos[5].maxsolutions = _nj5;
2740  std::vector<int> vfree(0);
2741  solutions.AddSolution(vinfos, vfree);
2742  }
2743  }
2744  }
2745  }
2746  }
2747  }
2748  else
2749  {
2750  {
2751  IkReal j5array[1], cj5array[1], sj5array[1];
2752  bool j5valid[1] = { false };
2753  _nj5 = 1;
2754  if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
2755  IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH)
2756  continue;
2757  j5array[0] =
2758  IKatan2(((gconst5) * (new_r21)), ((IkReal(-1.00000000000000)) * (gconst5) * (new_r20)));
2759  sj5array[0] = IKsin(j5array[0]);
2760  cj5array[0] = IKcos(j5array[0]);
2761  if (j5array[0] > IKPI)
2762  {
2763  j5array[0] -= IK2PI;
2764  }
2765  else if (j5array[0] < -IKPI)
2766  {
2767  j5array[0] += IK2PI;
2768  }
2769  j5valid[0] = true;
2770  for (int ij5 = 0; ij5 < 1; ++ij5)
2771  {
2772  if (!j5valid[ij5])
2773  {
2774  continue;
2775  }
2776  _ij5[0] = ij5;
2777  _ij5[1] = -1;
2778  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2779  {
2780  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2781  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2782  {
2783  j5valid[iij5] = false;
2784  _ij5[1] = iij5;
2785  break;
2786  }
2787  }
2788  j5 = j5array[ij5];
2789  cj5 = cj5array[ij5];
2790  sj5 = sj5array[ij5];
2791  {
2792  IkReal evalcond[8];
2793  IkReal x197 = IKsin(j5);
2794  IkReal x198 = IKcos(j5);
2795  IkReal x199 = ((IkReal(1.00000000000000)) * (sj3));
2796  IkReal x200 = ((new_r11) * (sj3));
2797  IkReal x201 = ((new_r10) * (sj3));
2798  IkReal x202 = ((cj3) * (cj4));
2799  IkReal x203 = ((IkReal(1.00000000000000)) * (sj4));
2800  IkReal x204 = ((IkReal(1.00000000000000)) * (x198));
2801  IkReal x205 = ((IkReal(1.00000000000000)) * (x197));
2802  evalcond[0] = ((new_r20) + (((sj4) * (x198))));
2803  evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x197) * (x203))));
2804  evalcond[2] = ((((IkReal(-1.00000000000000)) * (x205))) + (((cj3) * (new_r10))) +
2805  (((IkReal(-1.00000000000000)) * (new_r00) * (x199))));
2806  evalcond[3] = ((((IkReal(-1.00000000000000)) * (x204))) + (((cj3) * (new_r11))) +
2807  (((IkReal(-1.00000000000000)) * (new_r01) * (x199))));
2808  evalcond[4] = ((((cj4) * (x197))) + (x200) + (((cj3) * (new_r01))));
2809  evalcond[5] =
2810  ((x201) + (((IkReal(-1.00000000000000)) * (cj4) * (x204))) + (((cj3) * (new_r00))));
2811  evalcond[6] = ((((new_r01) * (x202))) + (((cj4) * (x200))) + (x197) +
2812  (((IkReal(-1.00000000000000)) * (new_r21) * (x203))));
2813  evalcond[7] = ((((cj4) * (x201))) + (((new_r00) * (x202))) +
2814  (((IkReal(-1.00000000000000)) * (x204))) +
2815  (((IkReal(-1.00000000000000)) * (new_r20) * (x203))));
2816  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
2817  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
2818  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
2819  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001)
2820  {
2821  continue;
2822  }
2823  }
2824 
2825  {
2826  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2827  vinfos[0].jointtype = 1;
2828  vinfos[0].foffset = j0;
2829  vinfos[0].indices[0] = _ij0[0];
2830  vinfos[0].indices[1] = _ij0[1];
2831  vinfos[0].maxsolutions = _nj0;
2832  vinfos[1].jointtype = 1;
2833  vinfos[1].foffset = j1;
2834  vinfos[1].indices[0] = _ij1[0];
2835  vinfos[1].indices[1] = _ij1[1];
2836  vinfos[1].maxsolutions = _nj1;
2837  vinfos[2].jointtype = 1;
2838  vinfos[2].foffset = j2;
2839  vinfos[2].indices[0] = _ij2[0];
2840  vinfos[2].indices[1] = _ij2[1];
2841  vinfos[2].maxsolutions = _nj2;
2842  vinfos[3].jointtype = 1;
2843  vinfos[3].foffset = j3;
2844  vinfos[3].indices[0] = _ij3[0];
2845  vinfos[3].indices[1] = _ij3[1];
2846  vinfos[3].maxsolutions = _nj3;
2847  vinfos[4].jointtype = 1;
2848  vinfos[4].foffset = j4;
2849  vinfos[4].indices[0] = _ij4[0];
2850  vinfos[4].indices[1] = _ij4[1];
2851  vinfos[4].maxsolutions = _nj4;
2852  vinfos[5].jointtype = 1;
2853  vinfos[5].foffset = j5;
2854  vinfos[5].indices[0] = _ij5[0];
2855  vinfos[5].indices[1] = _ij5[1];
2856  vinfos[5].maxsolutions = _nj5;
2857  std::vector<int> vfree(0);
2858  solutions.AddSolution(vinfos, vfree);
2859  }
2860  }
2861  }
2862  }
2863  }
2864  }
2865  }
2866  }
2867  }
2868  }
2869  else
2870  {
2871  {
2872  IkReal j5array[1], cj5array[1], sj5array[1];
2873  bool j5valid[1] = { false };
2874  _nj5 = 1;
2875  if (IKabs(((gconst4) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
2876  IKabs(((IkReal(-1.00000000000000)) * (gconst4) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH)
2877  continue;
2878  j5array[0] = IKatan2(((gconst4) * (new_r21)), ((IkReal(-1.00000000000000)) * (gconst4) * (new_r20)));
2879  sj5array[0] = IKsin(j5array[0]);
2880  cj5array[0] = IKcos(j5array[0]);
2881  if (j5array[0] > IKPI)
2882  {
2883  j5array[0] -= IK2PI;
2884  }
2885  else if (j5array[0] < -IKPI)
2886  {
2887  j5array[0] += IK2PI;
2888  }
2889  j5valid[0] = true;
2890  for (int ij5 = 0; ij5 < 1; ++ij5)
2891  {
2892  if (!j5valid[ij5])
2893  {
2894  continue;
2895  }
2896  _ij5[0] = ij5;
2897  _ij5[1] = -1;
2898  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2899  {
2900  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2901  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2902  {
2903  j5valid[iij5] = false;
2904  _ij5[1] = iij5;
2905  break;
2906  }
2907  }
2908  j5 = j5array[ij5];
2909  cj5 = cj5array[ij5];
2910  sj5 = sj5array[ij5];
2911  {
2912  IkReal evalcond[2];
2913  evalcond[0] = ((new_r20) + (((sj4) * (IKcos(j5)))));
2914  evalcond[1] = ((((IkReal(-1.00000000000000)) * (sj4) * (IKsin(j5)))) + (new_r21));
2915  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001)
2916  {
2917  continue;
2918  }
2919  }
2920 
2921  {
2922  IkReal dummyeval[1];
2923  IkReal gconst6;
2924  gconst6 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02))));
2925  dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02)));
2926  if (IKabs(dummyeval[0]) < 0.0000010000000000)
2927  {
2928  {
2929  IkReal dummyeval[1];
2930  IkReal gconst7;
2931  gconst7 = IKsign(((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) +
2932  (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12)))));
2933  dummyeval[0] = ((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) +
2934  (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12))));
2935  if (IKabs(dummyeval[0]) < 0.0000010000000000)
2936  {
2937  continue;
2938  }
2939  else
2940  {
2941  {
2942  IkReal j3array[1], cj3array[1], sj3array[1];
2943  bool j3valid[1] = { false };
2944  _nj3 = 1;
2945  IkReal x206 = ((cj4) * (gconst7) * (sj5));
2946  if (IKabs(((new_r12) * (x206))) < IKFAST_ATAN2_MAGTHRESH &&
2947  IKabs(((new_r02) * (x206))) < IKFAST_ATAN2_MAGTHRESH)
2948  continue;
2949  j3array[0] = IKatan2(((new_r12) * (x206)), ((new_r02) * (x206)));
2950  sj3array[0] = IKsin(j3array[0]);
2951  cj3array[0] = IKcos(j3array[0]);
2952  if (j3array[0] > IKPI)
2953  {
2954  j3array[0] -= IK2PI;
2955  }
2956  else if (j3array[0] < -IKPI)
2957  {
2958  j3array[0] += IK2PI;
2959  }
2960  j3valid[0] = true;
2961  for (int ij3 = 0; ij3 < 1; ++ij3)
2962  {
2963  if (!j3valid[ij3])
2964  {
2965  continue;
2966  }
2967  _ij3[0] = ij3;
2968  _ij3[1] = -1;
2969  for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2970  {
2971  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
2972  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
2973  {
2974  j3valid[iij3] = false;
2975  _ij3[1] = iij3;
2976  break;
2977  }
2978  }
2979  j3 = j3array[ij3];
2980  cj3 = cj3array[ij3];
2981  sj3 = sj3array[ij3];
2982  {
2983  IkReal evalcond[12];
2984  IkReal x207 = IKsin(j3);
2985  IkReal x208 = IKcos(j3);
2986  IkReal x209 = ((IkReal(1.00000000000000)) * (cj5));
2987  IkReal x210 = ((IkReal(1.00000000000000)) * (sj4));
2988  IkReal x211 = ((cj4) * (x208));
2989  IkReal x212 = ((sj4) * (x208));
2990  IkReal x213 = ((cj4) * (x207));
2991  IkReal x214 = ((new_r11) * (x207));
2992  IkReal x215 = ((sj4) * (x207));
2993  IkReal x216 = ((IkReal(1.00000000000000)) * (x207));
2994  evalcond[0] =
2995  ((((IkReal(-1.00000000000000)) * (new_r02) * (x216))) + (((new_r12) * (x208))));
2996  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x210))) + (((new_r12) * (x207))) +
2997  (((new_r02) * (x208))));
2998  evalcond[2] =
2999  ((((new_r10) * (x208))) + (((IkReal(-1.00000000000000)) * (new_r00) * (x216))) +
3000  (((IkReal(-1.00000000000000)) * (sj5))));
3001  evalcond[3] = ((((IkReal(-1.00000000000000)) * (new_r01) * (x216))) +
3002  (((IkReal(-1.00000000000000)) * (x209))) + (((new_r11) * (x208))));
3003  evalcond[4] = ((((new_r01) * (x208))) + (x214) + (((cj4) * (sj5))));
3004  evalcond[5] = ((((new_r10) * (x207))) + (((new_r00) * (x208))) +
3005  (((IkReal(-1.00000000000000)) * (cj4) * (x209))));
3006  evalcond[6] = ((((new_r00) * (x212))) + (((new_r10) * (x215))) + (((cj4) * (new_r20))));
3007  evalcond[7] = ((((cj4) * (new_r21))) + (((new_r01) * (x212))) + (((sj4) * (x214))));
3008  evalcond[8] = ((IkReal(-1.00000000000000)) + (((cj4) * (new_r22))) +
3009  (((new_r02) * (x212))) + (((new_r12) * (x215))));
3010  evalcond[9] = ((((new_r12) * (x213))) + (((new_r02) * (x211))) +
3011  (((IkReal(-1.00000000000000)) * (new_r22) * (x210))));
3012  evalcond[10] =
3013  ((((new_r11) * (x213))) + (((IkReal(-1.00000000000000)) * (new_r21) * (x210))) +
3014  (sj5) + (((new_r01) * (x211))));
3015  evalcond[11] =
3016  ((((IkReal(-1.00000000000000)) * (x209))) + (((new_r00) * (x211))) +
3017  (((IkReal(-1.00000000000000)) * (new_r20) * (x210))) + (((new_r10) * (x213))));
3018  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
3019  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
3020  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
3021  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ||
3022  IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 ||
3023  IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001)
3024  {
3025  continue;
3026  }
3027  }
3028 
3029  {
3030  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3031  vinfos[0].jointtype = 1;
3032  vinfos[0].foffset = j0;
3033  vinfos[0].indices[0] = _ij0[0];
3034  vinfos[0].indices[1] = _ij0[1];
3035  vinfos[0].maxsolutions = _nj0;
3036  vinfos[1].jointtype = 1;
3037  vinfos[1].foffset = j1;
3038  vinfos[1].indices[0] = _ij1[0];
3039  vinfos[1].indices[1] = _ij1[1];
3040  vinfos[1].maxsolutions = _nj1;
3041  vinfos[2].jointtype = 1;
3042  vinfos[2].foffset = j2;
3043  vinfos[2].indices[0] = _ij2[0];
3044  vinfos[2].indices[1] = _ij2[1];
3045  vinfos[2].maxsolutions = _nj2;
3046  vinfos[3].jointtype = 1;
3047  vinfos[3].foffset = j3;
3048  vinfos[3].indices[0] = _ij3[0];
3049  vinfos[3].indices[1] = _ij3[1];
3050  vinfos[3].maxsolutions = _nj3;
3051  vinfos[4].jointtype = 1;
3052  vinfos[4].foffset = j4;
3053  vinfos[4].indices[0] = _ij4[0];
3054  vinfos[4].indices[1] = _ij4[1];
3055  vinfos[4].maxsolutions = _nj4;
3056  vinfos[5].jointtype = 1;
3057  vinfos[5].foffset = j5;
3058  vinfos[5].indices[0] = _ij5[0];
3059  vinfos[5].indices[1] = _ij5[1];
3060  vinfos[5].maxsolutions = _nj5;
3061  std::vector<int> vfree(0);
3062  solutions.AddSolution(vinfos, vfree);
3063  }
3064  }
3065  }
3066  }
3067  }
3068  }
3069  else
3070  {
3071  {
3072  IkReal j3array[1], cj3array[1], sj3array[1];
3073  bool j3valid[1] = { false };
3074  _nj3 = 1;
3075  IkReal x217 = ((gconst6) * (sj4));
3076  if (IKabs(((new_r12) * (x217))) < IKFAST_ATAN2_MAGTHRESH &&
3077  IKabs(((new_r02) * (x217))) < IKFAST_ATAN2_MAGTHRESH)
3078  continue;
3079  j3array[0] = IKatan2(((new_r12) * (x217)), ((new_r02) * (x217)));
3080  sj3array[0] = IKsin(j3array[0]);
3081  cj3array[0] = IKcos(j3array[0]);
3082  if (j3array[0] > IKPI)
3083  {
3084  j3array[0] -= IK2PI;
3085  }
3086  else if (j3array[0] < -IKPI)
3087  {
3088  j3array[0] += IK2PI;
3089  }
3090  j3valid[0] = true;
3091  for (int ij3 = 0; ij3 < 1; ++ij3)
3092  {
3093  if (!j3valid[ij3])
3094  {
3095  continue;
3096  }
3097  _ij3[0] = ij3;
3098  _ij3[1] = -1;
3099  for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
3100  {
3101  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
3102  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
3103  {
3104  j3valid[iij3] = false;
3105  _ij3[1] = iij3;
3106  break;
3107  }
3108  }
3109  j3 = j3array[ij3];
3110  cj3 = cj3array[ij3];
3111  sj3 = sj3array[ij3];
3112  {
3113  IkReal evalcond[12];
3114  IkReal x218 = IKsin(j3);
3115  IkReal x219 = IKcos(j3);
3116  IkReal x220 = ((IkReal(1.00000000000000)) * (cj5));
3117  IkReal x221 = ((IkReal(1.00000000000000)) * (sj4));
3118  IkReal x222 = ((cj4) * (x219));
3119  IkReal x223 = ((sj4) * (x219));
3120  IkReal x224 = ((cj4) * (x218));
3121  IkReal x225 = ((new_r11) * (x218));
3122  IkReal x226 = ((sj4) * (x218));
3123  IkReal x227 = ((IkReal(1.00000000000000)) * (x218));
3124  evalcond[0] =
3125  ((((new_r12) * (x219))) + (((IkReal(-1.00000000000000)) * (new_r02) * (x227))));
3126  evalcond[1] = ((((IkReal(-1.00000000000000)) * (x221))) + (((new_r12) * (x218))) +
3127  (((new_r02) * (x219))));
3128  evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x227))) +
3129  (((new_r10) * (x219))) + (((IkReal(-1.00000000000000)) * (sj5))));
3130  evalcond[3] = ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r11) * (x219))) +
3131  (((IkReal(-1.00000000000000)) * (new_r01) * (x227))));
3132  evalcond[4] = ((((new_r01) * (x219))) + (x225) + (((cj4) * (sj5))));
3133  evalcond[5] = ((((new_r00) * (x219))) + (((new_r10) * (x218))) +
3134  (((IkReal(-1.00000000000000)) * (cj4) * (x220))));
3135  evalcond[6] = ((((new_r00) * (x223))) + (((cj4) * (new_r20))) + (((new_r10) * (x226))));
3136  evalcond[7] = ((((sj4) * (x225))) + (((cj4) * (new_r21))) + (((new_r01) * (x223))));
3137  evalcond[8] = ((IkReal(-1.00000000000000)) + (((new_r12) * (x226))) +
3138  (((new_r02) * (x223))) + (((cj4) * (new_r22))));
3139  evalcond[9] = ((((IkReal(-1.00000000000000)) * (new_r22) * (x221))) +
3140  (((new_r12) * (x224))) + (((new_r02) * (x222))));
3141  evalcond[10] = ((((new_r11) * (x224))) + (((new_r01) * (x222))) + (sj5) +
3142  (((IkReal(-1.00000000000000)) * (new_r21) * (x221))));
3143  evalcond[11] =
3144  ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r10) * (x224))) +
3145  (((IkReal(-1.00000000000000)) * (new_r20) * (x221))) + (((new_r00) * (x222))));
3146  if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ||
3147  IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ||
3148  IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ||
3149  IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ||
3150  IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 ||
3151  IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001)
3152  {
3153  continue;
3154  }
3155  }
3156 
3157  {
3158  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3159  vinfos[0].jointtype = 1;
3160  vinfos[0].foffset = j0;
3161  vinfos[0].indices[0] = _ij0[0];
3162  vinfos[0].indices[1] = _ij0[1];
3163  vinfos[0].maxsolutions = _nj0;
3164  vinfos[1].jointtype = 1;
3165  vinfos[1].foffset = j1;
3166  vinfos[1].indices[0] = _ij1[0];
3167  vinfos[1].indices[1] = _ij1[1];
3168  vinfos[1].maxsolutions = _nj1;
3169  vinfos[2].jointtype = 1;
3170  vinfos[2].foffset = j2;
3171  vinfos[2].indices[0] = _ij2[0];
3172  vinfos[2].indices[1] = _ij2[1];
3173  vinfos[2].maxsolutions = _nj2;
3174  vinfos[3].jointtype = 1;
3175  vinfos[3].foffset = j3;
3176  vinfos[3].indices[0] = _ij3[0];
3177  vinfos[3].indices[1] = _ij3[1];
3178  vinfos[3].maxsolutions = _nj3;
3179  vinfos[4].jointtype = 1;
3180  vinfos[4].foffset = j4;
3181  vinfos[4].indices[0] = _ij4[0];
3182  vinfos[4].indices[1] = _ij4[1];
3183  vinfos[4].maxsolutions = _nj4;
3184  vinfos[5].jointtype = 1;
3185  vinfos[5].foffset = j5;
3186  vinfos[5].indices[0] = _ij5[0];
3187  vinfos[5].indices[1] = _ij5[1];
3188  vinfos[5].maxsolutions = _nj5;
3189  std::vector<int> vfree(0);
3190  solutions.AddSolution(vinfos, vfree);
3191  }
3192  }
3193  }
3194  }
3195  }
3196  }
3197  }
3198  }
3199  }
3200  }
3201  }
3202  }
3203  }
3204 };
3205 
3208 IKFAST_API bool
3209 ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
3210 {
3211  IKSolver solver;
3212  return solver.ComputeIk(eetrans, eerot, pfree, solutions);
3213 }
3214 
3215 IKFAST_API const char* GetKinematicsHash()
3216 {
3217  return "<robot:genericrobot - abb_irb2400 (1f04c8a90b29778d31a8f2cb88b4a166)>";
3218 }
3219 
3220 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
3221 
3222 #ifdef IKFAST_NAMESPACE
3223 } // end namespace
3224 #endif
3225 
3226 #ifndef IKFAST_NO_MAIN
3227 #include <stdio.h>
3228 #include <stdlib.h>
3229 #ifdef IKFAST_NAMESPACE
3230 using namespace IKFAST_NAMESPACE;
3231 #endif
3232 int main(int argc, char** argv)
3233 {
3234  if (argc != 12 + GetNumFreeParameters() + 1)
3235  {
3236  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
3237  "Returns the ik solutions given the transformation of the end effector specified by\n"
3238  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
3239  "There are %d free parameters that have to be specified.\n\n",
3240  GetNumFreeParameters());
3241  return 1;
3242  }
3243 
3244  IkSolutionList<IkReal> solutions;
3245  std::vector<IkReal> vfree(GetNumFreeParameters());
3246  IkReal eerot[9], eetrans[3];
3247  eerot[0] = atof(argv[1]);
3248  eerot[1] = atof(argv[2]);
3249  eerot[2] = atof(argv[3]);
3250  eetrans[0] = atof(argv[4]);
3251  eerot[3] = atof(argv[5]);
3252  eerot[4] = atof(argv[6]);
3253  eerot[5] = atof(argv[7]);
3254  eetrans[1] = atof(argv[8]);
3255  eerot[6] = atof(argv[9]);
3256  eerot[7] = atof(argv[10]);
3257  eerot[8] = atof(argv[11]);
3258  eetrans[2] = atof(argv[12]);
3259  for (std::size_t i = 0; i < vfree.size(); ++i)
3260  vfree[i] = atof(argv[13 + i]);
3261  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
3262 
3263  if (!bSuccess)
3264  {
3265  fprintf(stderr, "Failed to get ik solution\n");
3266  return -1;
3267  }
3268 
3269  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
3270  std::vector<IkReal> solvalues(GetNumJoints());
3271  for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
3272  {
3273  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
3274  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
3275  std::vector<IkReal> vsolfree(sol.GetFree().size());
3276  sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
3277  for (std::size_t j = 0; j < solvalues.size(); ++j)
3278  printf("%.15f, ", solvalues[j]);
3279  printf("\n");
3280  }
3281 
3282  IkReal rot[9], trans[3];
3283  IkReal sol[6] = { 0.46365, 0.93285, 1.75595 - M_PI / 2.0, 6.28319, -2.68880, -0.46365 };
3284  ComputeFk(sol, trans, rot);
3285  printf("FK: %f %f %f\n", trans[0], trans[1], trans[2]);
3286 
3287  return 0;
3288 }
3289 
3290 #endif
3291 TESSERACT_COMMON_IGNORE_WARNINGS_POP
3292 // clang-format on
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: abb_irb2400_ikfast_solver.hpp:380
IKfmod
float IKfmod(float x, float y)
Definition: abb_irb2400_ikfast_solver.hpp:171
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:258
IKasin
float IKasin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:149
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: abb_irb2400_ikfast_solver.hpp:31
IKsqrt
float IKsqrt(float f)
Definition: abb_irb2400_ikfast_solver.hpp:216
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: abb_irb2400_ikfast_solver.hpp:58
IK2PI
#define IK2PI
Definition: abb_irb2400_ikfast_solver.hpp:77
IKPI_2
#define IKPI_2
Definition: abb_irb2400_ikfast_solver.hpp:79
GetIkFastVersion
const IKFAST_API char * GetIkFastVersion()
Definition: abb_irb2400_ikfast_solver.hpp:3220
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
ikfast
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: abb_irb2400_ikfast_solver.hpp:382
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: abb_irb2400_ikfast_solver.hpp:141
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:102
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
main
int main(int argc, char **argv)
Definition: ikfast_kinematics_7dof_unit.cpp:88
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
IKsign
float IKsign(float f)
Definition: abb_irb2400_ikfast_solver.hpp:255
IKSolver
Definition: abb_irb2400_ikfast_solver.hpp:386
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const=0
GetIkType
IKFAST_API int GetIkType()
Definition: abb_irb2400_ikfast_solver.hpp:384
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:136
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:379
ikfast.h
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:146
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
IKlog
float IKlog(float f)
Definition: abb_irb2400_ikfast_solver.hpp:131
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
IKabs
float IKabs(float f)
Definition: abb_irb2400_ikfast_solver.hpp:125
IKcos
float IKcos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:212
IKsqr
float IKsqr(float f)
Definition: abb_irb2400_ikfast_solver.hpp:128
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
IKacos
float IKacos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:190
IKPI
#define IKPI
Definition: abb_irb2400_ikfast_solver.hpp:78
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: abb_irb2400_ikfast_solver.hpp:283
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: abb_irb2400_ikfast_solver.hpp:40
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:71
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:378
IKatan2
float IKatan2(float fy, float fx)
Definition: abb_irb2400_ikfast_solver.hpp:228
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
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)
GetKinematicsHash
const IKFAST_API char * GetKinematicsHash()
Definition: abb_irb2400_ikfast_solver.hpp:3215
IKtan
float IKtan(float f)
Definition: abb_irb2400_ikfast_solver.hpp:214
IKsin
float IKsin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:210
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const=0
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)
macros.h
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:3209
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const

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