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

opw_kinematics
Author(s): Jon Meyer , Jeroen De Maeyer
autogenerated on Thu Jan 16 2025 03:40:37